Merged i2c-helper in Main

2025-01-12 00:16:00 +01:00
parent f4792de050
commit b5c7e5b4c1
396 changed files with 182143 additions and 14 deletions
--- a/i2c-hub/firmware/i2c-hub-firmware/.gitignore
+++ b/i2c-hub/firmware/i2c-hub-firmware/.gitignore
--- a/i2c-hub/firmware/i2c-hub-firmware/.vscode/c_cpp_properties.json
+++ b/i2c-hub/firmware/i2c-hub-firmware/.vscode/c_cpp_properties.json
@@ -0,0 +1,23 @@
+{
+    "configurations": [
+        {
+            "name": "Pico",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "${userHome}/.pico-sdk/sdk/2.1.0/**"
+            ],
+            "forcedInclude": [
+                "${userHome}/.pico-sdk/sdk/2.1.0/src/common/pico_base_headers/include/pico.h",
+                "${workspaceFolder}/build/generated/pico_base/pico/config_autogen.h"
+            ],
+            "defines": [],
+            "compilerPath": "${userHome}/.pico-sdk/toolchain/13_3_Rel1/bin/arm-none-eabi-gcc",
+            "compileCommands": "${workspaceFolder}/build/compile_commands.json",
+            "cStandard": "c17",
+            "cppStandard": "c++14",
+            "intelliSenseMode": "linux-gcc-arm",
+            "configurationProvider": "ms-vscode.cmake-tools"
+        }
+    ],
+    "version": 4
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/.vscode/cmake-kits.json
+++ b/i2c-hub/firmware/i2c-hub-firmware/.vscode/cmake-kits.json
@@ -0,0 +1,15 @@
+[
+    {
+        "name": "Pico",
+        "compilers": {
+            "C": "${command:raspberry-pi-pico.getCompilerPath}",
+            "CXX": "${command:raspberry-pi-pico.getCxxCompilerPath}"
+        },
+        "environmentVariables": {
+            "PATH": "${command:raspberry-pi-pico.getEnvPath};${env:PATH}"
+        },
+        "cmakeSettings": {
+            "Python3_EXECUTABLE": "${command:raspberry-pi-pico.getPythonPath}"
+        }
+    }
+]
--- a/i2c-hub/firmware/i2c-hub-firmware/.vscode/extensions.json
+++ b/i2c-hub/firmware/i2c-hub-firmware/.vscode/extensions.json
@@ -0,0 +1,9 @@
+{
+    "recommendations": [
+        "marus25.cortex-debug",
+        "ms-vscode.cpptools",
+        "ms-vscode.cpptools-extension-pack",
+        "ms-vscode.vscode-serial-monitor",
+        "raspberry-pi.raspberry-pi-pico"
+    ]
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/.vscode/launch.json
+++ b/i2c-hub/firmware/i2c-hub-firmware/.vscode/launch.json
@@ -0,0 +1,70 @@
+{
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Pico Debug (Cortex-Debug)",
+            "cwd": "${userHome}/.pico-sdk/openocd/0.12.0+dev/scripts",
+            "executable": "${command:raspberry-pi-pico.launchTargetPath}",
+            "request": "launch",
+            "type": "cortex-debug",
+            "servertype": "openocd",
+            "serverpath": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
+            "gdbPath": "${command:raspberry-pi-pico.getGDBPath}",
+            "device": "${command:raspberry-pi-pico.getChipUppercase}",
+            "configFiles": [
+                "interface/cmsis-dap.cfg",
+                "target/${command:raspberry-pi-pico.getTarget}.cfg"
+            ],
+            "svdFile": "${userHome}/.pico-sdk/sdk/2.1.0/src/${command:raspberry-pi-pico.getChip}/hardware_regs/${command:raspberry-pi-pico.getChipUppercase}.svd",
+            "runToEntryPoint": "main",
+            // Fix for no_flash binaries, where monitor reset halt doesn't do what is expected
+            // Also works fine for flash binaries
+            "overrideLaunchCommands": [
+                "monitor reset init",
+                "load \"${command:raspberry-pi-pico.launchTargetPath}\""
+            ],
+            "openOCDLaunchCommands": [
+                "adapter speed 5000"
+            ]
+        },
+        {
+            "name": "Pico Debug (Cortex-Debug with external OpenOCD)",
+            "cwd": "${workspaceRoot}",
+            "executable": "${command:raspberry-pi-pico.launchTargetPath}",
+            "request": "launch",
+            "type": "cortex-debug",
+            "servertype": "external",
+            "gdbTarget": "localhost:3333",
+            "gdbPath": "${command:raspberry-pi-pico.getGDBPath}",
+            "device": "${command:raspberry-pi-pico.getChipUppercase}",
+            "svdFile": "${userHome}/.pico-sdk/sdk/2.1.0/src/${command:raspberry-pi-pico.getChip}/hardware_regs/${command:raspberry-pi-pico.getChipUppercase}.svd",
+            "runToEntryPoint": "main",
+            // Fix for no_flash binaries, where monitor reset halt doesn't do what is expected
+            // Also works fine for flash binaries
+            "overrideLaunchCommands": [
+                "monitor reset init",
+                "load \"${command:raspberry-pi-pico.launchTargetPath}\""
+            ]
+        },
+        {
+            "name": "Pico Debug (C++ Debugger)",
+            "type": "cppdbg",
+            "request": "launch",
+            "cwd": "${workspaceRoot}",
+            "program": "${command:raspberry-pi-pico.launchTargetPath}",
+            "MIMode": "gdb",
+            "miDebuggerPath": "${command:raspberry-pi-pico.getGDBPath}",
+            "miDebuggerServerAddress": "localhost:3333",
+            "debugServerPath": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
+            "debugServerArgs": "-f interface/cmsis-dap.cfg -f target/${command:raspberry-pi-pico.getTarget}.cfg -c \"adapter speed 5000\"",
+            "serverStarted": "Listening on port .* for gdb connections",
+            "filterStderr": true,
+            "hardwareBreakpoints": {
+                "require": true,
+                "limit": 4
+            },
+            "preLaunchTask": "Flash",
+            "svdPath": "${userHome}/.pico-sdk/sdk/2.1.0/src/${command:raspberry-pi-pico.getChip}/hardware_regs/${command:raspberry-pi-pico.getChipUppercase}.svd"
+        },
+    ]
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/.vscode/settings.json
+++ b/i2c-hub/firmware/i2c-hub-firmware/.vscode/settings.json
@@ -0,0 +1,87 @@
+{
+    "cmake.options.statusBarVisibility": "hidden",
+    "cmake.options.advanced": {
+        "build": {
+            "statusBarVisibility": "hidden"
+        },
+        "launch": {
+            "statusBarVisibility": "hidden"
+        },
+        "debug": {
+            "statusBarVisibility": "hidden"
+        }
+    },
+    "cmake.configureOnEdit": true,
+    "cmake.automaticReconfigure": true,
+    "cmake.configureOnOpen": true,
+    "cmake.generator": "Ninja",
+    "cmake.cmakePath": "${userHome}/.pico-sdk/cmake/v3.29.9/bin/cmake",
+    "C_Cpp.debugShortcut": false,
+    "terminal.integrated.env.windows": {
+        "PICO_SDK_PATH": "${env:USERPROFILE}/.pico-sdk/sdk/2.1.0",
+        "PICO_TOOLCHAIN_PATH": "${env:USERPROFILE}/.pico-sdk/toolchain/13_3_Rel1",
+        "Path": "${env:USERPROFILE}/.pico-sdk/toolchain/13_3_Rel1/bin;${env:USERPROFILE}/.pico-sdk/picotool/2.1.0/picotool;${env:USERPROFILE}/.pico-sdk/cmake/v3.29.9/bin;${env:USERPROFILE}/.pico-sdk/ninja/v1.12.1;${env:PATH}"
+    },
+    "terminal.integrated.env.osx": {
+        "PICO_SDK_PATH": "${env:HOME}/.pico-sdk/sdk/2.1.0",
+        "PICO_TOOLCHAIN_PATH": "${env:HOME}/.pico-sdk/toolchain/13_3_Rel1",
+        "PATH": "${env:HOME}/.pico-sdk/toolchain/13_3_Rel1/bin:${env:HOME}/.pico-sdk/picotool/2.1.0/picotool:${env:HOME}/.pico-sdk/cmake/v3.29.9/bin:${env:HOME}/.pico-sdk/ninja/v1.12.1:${env:PATH}"
+    },
+    "terminal.integrated.env.linux": {
+        "PICO_SDK_PATH": "${env:HOME}/.pico-sdk/sdk/2.1.0",
+        "PICO_TOOLCHAIN_PATH": "${env:HOME}/.pico-sdk/toolchain/13_3_Rel1",
+        "PATH": "${env:HOME}/.pico-sdk/toolchain/13_3_Rel1/bin:${env:HOME}/.pico-sdk/picotool/2.1.0/picotool:${env:HOME}/.pico-sdk/cmake/v3.29.9/bin:${env:HOME}/.pico-sdk/ninja/v1.12.1:${env:PATH}"
+    },
+    "raspberry-pi-pico.cmakeAutoConfigure": false,
+    "raspberry-pi-pico.useCmakeTools": true,
+    "raspberry-pi-pico.cmakePath": "${HOME}/.pico-sdk/cmake/v3.29.9/bin/cmake",
+    "raspberry-pi-pico.ninjaPath": "${HOME}/.pico-sdk/ninja/v1.12.1/ninja",
+    "files.associations": {
+        "uart.h": "c",
+        "freertos.h": "c",
+        "array": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "compare": "cpp",
+        "concepts": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "deque": "cpp",
+        "map": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "initializer_list": "cpp",
+        "iosfwd": "cpp",
+        "limits": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "typeinfo": "cpp"
+    }
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/.vscode/tasks.json
+++ b/i2c-hub/firmware/i2c-hub-firmware/.vscode/tasks.json
@@ -0,0 +1,106 @@
+{
+    "version": "2.0.0",
+    "tasks": [
+        {
+            "label": "Compile Project",
+            "type": "process",
+            "isBuildCommand": true,
+            "command": "${userHome}/.pico-sdk/ninja/v1.12.1/ninja",
+            "args": ["-C", "${workspaceFolder}/build"],
+            "group": "build",
+            "presentation": {
+                "echo": true,
+                "reveal": "always",
+                "focus": true,
+                "panel": "dedicated",
+                "showReuseMessage": true,
+                "clear": true
+            },
+            "problemMatcher": "$gcc",
+            "windows": {
+                "command": "${env:USERPROFILE}/.pico-sdk/ninja/v1.12.1/ninja.exe"
+            }
+        },
+        {
+            "label": "Run Project",
+            "type": "process",
+            "command": "${env:HOME}/.pico-sdk/picotool/2.1.0/picotool/picotool",
+            "args": [
+                "load",
+                "${command:raspberry-pi-pico.launchTargetPath}",
+                "-fx"
+            ],
+            "presentation": {
+                "echo": true,
+                "reveal": "always",
+                "focus": true,
+                "panel": "shared",
+                "showReuseMessage": true,
+                "clear": true
+            },
+            "problemMatcher": [],
+            "windows": {
+                "command": "${env:USERPROFILE}/.pico-sdk/picotool/2.1.0/picotool/picotool.exe"
+            }
+        },
+        {
+            "label": "Flash",
+            "type": "process",
+            "command": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
+            "args": [
+                "-s",
+                "${userHome}/.pico-sdk/openocd/0.12.0+dev/scripts",
+                "-f",
+                "interface/cmsis-dap.cfg",
+                "-f",
+                "target/${command:raspberry-pi-pico.getTarget}.cfg",
+                "-c", "adapter speed 5000; program \"${command:raspberry-pi-pico.launchTargetPath}\" verify",
+                "-c", "reset halt",
+                "-c", "rp2040.core1 arp_reset assert 0",
+                "-c", "rp2040.core0 arp_reset assert 0",
+                "-c", "exit"
+            ],
+            "presentation": {
+                "echo": true,
+                "reveal": "always",
+                "focus": true,
+                "panel": "shared",
+                "showReuseMessage": true,
+                "clear": true
+            },
+            "dependsOn": ["Compile Project"],
+            "problemMatcher": [],
+            "windows": {
+                "command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
+            }
+        },
+        {
+            "label": "Reset",
+            "type": "process",
+            "command": "${userHome}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
+            "args": [
+                "-s",
+                "${userHome}/.pico-sdk/openocd/0.12.0+dev/scripts",
+                "-f",
+                "interface/cmsis-dap.cfg",
+                "-f", "target/${command:raspberry-pi-pico.getTarget}.cfg",
+                "-c", "adapter speed 5000; init; reset halt;",
+                "-c", "rp2040.core1 arp_reset assert 0",
+                "-c", "rp2040.core0 arp_reset assert 0",
+                "-c", "exit"
+            ],
+            "presentation": {
+                "echo": true,
+                "reveal": "always",
+                "focus": true,
+                "panel": "shared",
+                "showReuseMessage": true,
+                "clear": true
+            },
+            "problemMatcher": [],
+            "windows": {
+                "command": "${env:USERPROFILE}/.pico-sdk/openocd/0.12.0+dev/openocd.exe",
+            }
+        }
+    ]
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/CMakeLists.txt
+++ b/i2c-hub/firmware/i2c-hub-firmware/CMakeLists.txt
@@ -0,0 +1,75 @@
+# Generated Cmake Pico project file
+set(CMAKE_VERBOSE_MAKEFILE ON CACHE BOOL "ON")
+cmake_minimum_required(VERSION 3.13)
+
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# Initialise pico_sdk from installed location
+# (note this can come from environment, CMake cache etc)
+
+# == DO NOT EDIT THE FOLLOWING LINES for the Raspberry Pi Pico VS Code Extension to work ==
+if(WIN32)
+    set(USERHOME $ENV{USERPROFILE})
+else()
+    set(USERHOME $ENV{HOME})
+endif()
+set(sdkVersion 2.1.0)
+set(toolchainVersion 13_3_Rel1)
+set(picotoolVersion 2.1.0)
+set(picoVscode ${USERHOME}/.pico-sdk/cmake/pico-vscode.cmake)
+if (EXISTS ${picoVscode})
+    include(${picoVscode})
+endif()
+# ====================================================================================
+set(PICO_BOARD pico CACHE STRING "Board type")
+
+# Pull in Raspberry Pi Pico SDK (must be before project)
+include(pico_sdk_import.cmake)
+
+# Adding FreeRTOS
+set(FREERTOS_KERNEL_PATH ../lib/FreeRTOS-Kernel)
+include(cmake/FreeRTOS_Kernel_import.cmake)
+include(src/gobotrpc/cmake/CMakeLists.txt)
+
+project(i2c-hub-firmware C CXX ASM)
+
+# Initialise the Raspberry Pi Pico SDK
+pico_sdk_init()
+
+# Add executable. Default name is the project name, version 0.1
+
+add_executable(i2c-hub-firmware 
+    src/main_core0.cpp
+    src/main_core1.cpp
+    #src/freeRTOS_impl.c
+    )
+
+pico_set_program_name(i2c-hub-firmware "i2c-hub-firmware")
+pico_set_program_version(i2c-hub-firmware "0.1")
+
+# Modify the below lines to enable/disable output over UART/USB
+pico_enable_stdio_uart(i2c-hub-firmware 1)
+pico_enable_stdio_usb(i2c-hub-firmware 0)
+
+# Add the standard library to the build
+target_link_libraries(i2c-hub-firmware
+        FreeRTOS-Kernel-Heap4
+        GobotRPC
+        pico_stdlib
+    )
+    
+# Add the standard include files to the build
+target_include_directories(i2c-hub-firmware PRIVATE
+  ${CMAKE_CURRENT_LIST_DIR}
+  ${CMAKE_CURRENT_LIST_DIR}/include
+)
+
+# Add any user requested libraries
+target_link_libraries(i2c-hub-firmware 
+        hardware_i2c
+        )
+
+pico_add_extra_outputs(i2c-hub-firmware)
+
--- a/i2c-hub/firmware/i2c-hub-firmware/FreeRTOSConfig.h
+++ b/i2c-hub/firmware/i2c-hub-firmware/FreeRTOSConfig.h
@@ -0,0 +1,143 @@
+/*
+ * FreeRTOS V202111.00
+ * Copyright (C) 2020 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+#ifndef FREERTOS_CONFIG_H
+#define FREERTOS_CONFIG_H
+
+/*-----------------------------------------------------------
+ * Application specific definitions.
+ *
+ * These definitions should be adjusted for your particular hardware and
+ * application requirements.
+ *
+ * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
+ * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
+ *
+ * See http://www.freertos.org/a00110.html
+ *----------------------------------------------------------*/
+
+/* Scheduler Related */
+#define configUSE_PREEMPTION                    1
+#define configUSE_TICKLESS_IDLE                 0
+#define configUSE_IDLE_HOOK                     0
+#define configUSE_TICK_HOOK                     0
+#define configTICK_RATE_HZ                      ( ( TickType_t ) 1000 )
+#define configMAX_PRIORITIES                    32
+#define configMINIMAL_STACK_SIZE                ( configSTACK_DEPTH_TYPE ) 256
+#define configUSE_16_BIT_TICKS                  0
+
+#define configIDLE_SHOULD_YIELD                 1
+
+/* Synchronization Related */
+#define configUSE_MUTEXES                       1
+#define configUSE_RECURSIVE_MUTEXES             1
+#define configUSE_APPLICATION_TASK_TAG          0
+#define configUSE_COUNTING_SEMAPHORES           1
+#define configQUEUE_REGISTRY_SIZE               8
+#define configUSE_QUEUE_SETS                    1
+#define configUSE_TIME_SLICING                  1
+#define configUSE_NEWLIB_REENTRANT              0
+// todo need this for lwip FreeRTOS sys_arch to compile
+#define configENABLE_BACKWARD_COMPATIBILITY     1
+#define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5
+
+/* System */
+#define configSTACK_DEPTH_TYPE                  uint32_t
+#define configMESSAGE_BUFFER_LENGTH_TYPE        size_t
+
+/* Memory allocation related definitions. */
+#define configSUPPORT_STATIC_ALLOCATION         0
+#define configSUPPORT_DYNAMIC_ALLOCATION        1
+#define configTOTAL_HEAP_SIZE                   (1024 * 128)
+#define configAPPLICATION_ALLOCATED_HEAP        (1024 * 84)
+
+/* Hook function related definitions. */
+#define configCHECK_FOR_STACK_OVERFLOW          0
+#define configUSE_MALLOC_FAILED_HOOK            0
+#define configUSE_DAEMON_TASK_STARTUP_HOOK      0
+
+/* Run time and task stats gathering related definitions. */
+#define configGENERATE_RUN_TIME_STATS           0
+#define configUSE_TRACE_FACILITY                1
+#define configUSE_STATS_FORMATTING_FUNCTIONS    0
+
+/* Co-routine related definitions. */
+#define configUSE_CO_ROUTINES                   0
+#define configMAX_CO_ROUTINE_PRIORITIES         1
+
+/* Software timer related definitions. */
+#define configUSE_TIMERS                        1
+#define configTIMER_TASK_PRIORITY               ( configMAX_PRIORITIES - 1 )
+#define configTIMER_QUEUE_LENGTH                10
+#define configTIMER_TASK_STACK_DEPTH            1024
+
+/* Interrupt nesting behaviour configuration. */
+/*
+#define configKERNEL_INTERRUPT_PRIORITY         [dependent of processor]
+#define configMAX_SYSCALL_INTERRUPT_PRIORITY    [dependent on processor and application]
+#define configMAX_API_CALL_INTERRUPT_PRIORITY   [dependent on processor and application]
+*/
+
+#if FREE_RTOS_KERNEL_SMP // set by the RP2040 SMP port of FreeRTOS
+/* SMP port only */
+#define configNUMBER_OF_CORES                   2
+#define configTICK_CORE                         0
+#define configRUN_MULTIPLE_PRIORITIES           1
+#define configUSE_CORE_AFFINITY                 1
+#define configUSE_PASSIVE_IDLE_HOOK             0
+#endif
+
+/* RP2040 specific */
+#define configSUPPORT_PICO_SYNC_INTEROP         1
+#define configSUPPORT_PICO_TIME_INTEROP         1
+
+#include <assert.h>
+/* Define to trap errors during development. */
+#define configASSERT(x)                         assert(x)
+
+/* Set the following definitions to 1 to include the API function, or zero
+to exclude the API function. */
+#define INCLUDE_vTaskPrioritySet                1
+#define INCLUDE_uxTaskPriorityGet               1
+#define INCLUDE_vTaskDelete                     1
+#define INCLUDE_vTaskSuspend                    1
+#define INCLUDE_vTaskDelayUntil                 1
+#define INCLUDE_vTaskDelay                      1
+#define INCLUDE_xTaskGetSchedulerState          1
+#define INCLUDE_xTaskGetCurrentTaskHandle       1
+#define INCLUDE_uxTaskGetStackHighWaterMark     1
+#define INCLUDE_xTaskGetIdleTaskHandle          1
+#define INCLUDE_eTaskGetState                   1
+#define INCLUDE_xTimerPendFunctionCall          1
+#define INCLUDE_xTaskAbortDelay                 1
+#define INCLUDE_xTaskGetHandle                  1
+#define INCLUDE_xTaskResumeFromISR              1
+#define INCLUDE_xQueueGetMutexHolder            1
+
+/* A header file that defines trace macro can be included here. */
+
+#endif /* FREERTOS_CONFIG_H */
--- a/i2c-hub/firmware/i2c-hub-firmware/cmake/FreeRTOS_Kernel_import.cmake
+++ b/i2c-hub/firmware/i2c-hub-firmware/cmake/FreeRTOS_Kernel_import.cmake
@@ -0,0 +1,61 @@
+# This is a copy of <FREERTOS_KERNEL_PATH>/portable/ThirdParty/GCC/RP2040/FREERTOS_KERNEL_import.cmake
+
+# This can be dropped into an external project to help locate the FreeRTOS kernel
+# It should be include()ed prior to project(). Alternatively this file may
+# or the CMakeLists.txt in this directory may be included or added via add_subdirectory
+# respectively.
+
+if (DEFINED ENV{FREERTOS_KERNEL_PATH} AND (NOT FREERTOS_KERNEL_PATH))
+    set(FREERTOS_KERNEL_PATH $ENV{FREERTOS_KERNEL_PATH})
+    message("Using FREERTOS_KERNEL_PATH from environment ('${FREERTOS_KERNEL_PATH}')")
+endif ()
+
+set(FREERTOS_KERNEL_RP2040_RELATIVE_PATH "portable/ThirdParty/GCC/RP2040")
+# undo the above
+set(FREERTOS_KERNEL_RP2040_BACK_PATH "../../../..")
+
+if (NOT FREERTOS_KERNEL_PATH)
+    # check if we are inside the FreeRTOS kernel tree (i.e. this file has been included directly)
+    get_filename_component(_ACTUAL_PATH ${CMAKE_CURRENT_LIST_DIR} REALPATH)
+    get_filename_component(_POSSIBLE_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH} REALPATH)
+    if (_ACTUAL_PATH STREQUAL _POSSIBLE_PATH)
+        get_filename_component(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH} REALPATH)
+    endif()
+    if (_ACTUAL_PATH STREQUAL _POSSIBLE_PATH)
+        get_filename_component(FREERTOS_KERNEL_PATH ${CMAKE_CURRENT_LIST_DIR}/${FREERTOS_KERNEL_RP2040_BACK_PATH} REALPATH)
+        message("Setting FREERTOS_KERNEL_PATH to ${FREERTOS_KERNEL_PATH} based on location of FreeRTOS-Kernel-import.cmake")
+    elseif (PICO_SDK_PATH AND EXISTS "${PICO_SDK_PATH}/../FreeRTOS-Kernel")
+        set(FREERTOS_KERNEL_PATH ${PICO_SDK_PATH}/../FreeRTOS-Kernel)
+        message("Defaulting FREERTOS_KERNEL_PATH as sibling of PICO_SDK_PATH: ${FREERTOS_KERNEL_PATH}")
+    endif()
+endif ()
+
+if (NOT FREERTOS_KERNEL_PATH)
+    foreach(POSSIBLE_SUFFIX Source FreeRTOS-Kernel FreeRTOS/Source)
+        # check if FreeRTOS-Kernel exists under directory that included us
+        set(SEARCH_ROOT ${CMAKE_CURRENT_SOURCE_DIR})
+        get_filename_component(_POSSIBLE_PATH ${SEARCH_ROOT}/${POSSIBLE_SUFFIX} REALPATH)
+        if (EXISTS ${_POSSIBLE_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}/CMakeLists.txt)
+            get_filename_component(FREERTOS_KERNEL_PATH ${_POSSIBLE_PATH} REALPATH)
+            message("Setting FREERTOS_KERNEL_PATH to '${FREERTOS_KERNEL_PATH}' found relative to enclosing project")
+            break()
+        endif()
+    endforeach()
+endif()
+
+if (NOT FREERTOS_KERNEL_PATH)
+    message(FATAL_ERROR "FreeRTOS location was not specified. Please set FREERTOS_KERNEL_PATH.")
+endif()
+
+set(FREERTOS_KERNEL_PATH "${FREERTOS_KERNEL_PATH}" CACHE PATH "Path to the FreeRTOS Kernel")
+
+get_filename_component(FREERTOS_KERNEL_PATH "${FREERTOS_KERNEL_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${FREERTOS_KERNEL_PATH})
+    message(FATAL_ERROR "Directory '${FREERTOS_KERNEL_PATH}' not found")
+endif()
+if (NOT EXISTS ${FREERTOS_KERNEL_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}/CMakeLists.txt)
+    message(FATAL_ERROR "Directory '${FREERTOS_KERNEL_PATH}' does not contain an RP2040 port here: ${FREERTOS_KERNEL_RP2040_RELATIVE_PATH}")
+endif()
+set(FREERTOS_KERNEL_PATH ${FREERTOS_KERNEL_PATH} CACHE PATH "Path to the FreeRTOS_KERNEL" FORCE)
+
+add_subdirectory(${FREERTOS_KERNEL_PATH}/${FREERTOS_KERNEL_RP2040_RELATIVE_PATH} FREERTOS_KERNEL)
--- a/i2c-hub/firmware/i2c-hub-firmware/docs/.$NetworkStack.drawio.bkp
+++ b/i2c-hub/firmware/i2c-hub-firmware/docs/.$NetworkStack.drawio.bkp
--- a/i2c-hub/firmware/i2c-hub-firmware/docs/NetworkStack.drawio
+++ b/i2c-hub/firmware/i2c-hub-firmware/docs/NetworkStack.drawio
--- a/i2c-hub/firmware/i2c-hub-firmware/include/pinConfig.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/include/pinConfig.hpp
@@ -0,0 +1,23 @@
+#pragma once
+
+#define LED1_PIN 8
+#define LED2_PIN 9
+#define LED3_PIN 25
+
+#define GOBOTRPC_CI_UART_RX 0
+#define GOBOTRPC_CI_UART_TX 1
+
+#define GOBOTRPC_TI_I2C_SDA 4
+#define GOBOTRPC_TI_I2C_SCL 5
+#define GOBOTRPC_TI_CLOCK_SPEED 10000
+
+#define GOBOTRPC_TI_INT_PIN_START 10
+#define GOBOTRPC_TI_INT_NUM 4
+#define GOBOTRPC_TI_COMBINED_INT_PIN 18
+
+#define GOBOTRPC_TI_EXTERNAL_PULLUP 1
+
+#define GOBOTRPC_HEARTBEAT_INTERVAL 3000
+
+#define UART_CORE_MASK 0b01
+#define I2C_CORE_MASK  0b01
--- a/i2c-hub/firmware/i2c-hub-firmware/lib/FreeRTOS-Kernel
+++ b/i2c-hub/firmware/i2c-hub-firmware/lib/FreeRTOS-Kernel
--- a/i2c-hub/firmware/i2c-hub-firmware/pico_sdk_import.cmake
+++ b/i2c-hub/firmware/i2c-hub-firmware/pico_sdk_import.cmake
@@ -0,0 +1,84 @@
+# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
+
+# This can be dropped into an external project to help locate this SDK
+# It should be include()ed prior to project()
+
+if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
+    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
+    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
+    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
+    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
+    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
+    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_TAG} AND (NOT PICO_SDK_FETCH_FROM_GIT_TAG))
+    set(PICO_SDK_FETCH_FROM_GIT_TAG $ENV{PICO_SDK_FETCH_FROM_GIT_TAG})
+    message("Using PICO_SDK_FETCH_FROM_GIT_TAG from environment ('${PICO_SDK_FETCH_FROM_GIT_TAG}')")
+endif ()
+
+if (PICO_SDK_FETCH_FROM_GIT AND NOT PICO_SDK_FETCH_FROM_GIT_TAG)
+  set(PICO_SDK_FETCH_FROM_GIT_TAG "master")
+  message("Using master as default value for PICO_SDK_FETCH_FROM_GIT_TAG")
+endif()
+
+set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
+set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
+set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
+set(PICO_SDK_FETCH_FROM_GIT_TAG "${PICO_SDK_FETCH_FROM_GIT_TAG}" CACHE FILEPATH "release tag for SDK")
+
+if (NOT PICO_SDK_PATH)
+    if (PICO_SDK_FETCH_FROM_GIT)
+        include(FetchContent)
+        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
+        if (PICO_SDK_FETCH_FROM_GIT_PATH)
+            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
+        endif ()
+        # GIT_SUBMODULES_RECURSE was added in 3.17
+        if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
+            FetchContent_Declare(
+                    pico_sdk
+                    GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                    GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+                    GIT_SUBMODULES_RECURSE FALSE
+            )
+        else ()
+            FetchContent_Declare(
+                    pico_sdk
+                    GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                    GIT_TAG ${PICO_SDK_FETCH_FROM_GIT_TAG}
+            )
+        endif ()
+
+        if (NOT pico_sdk)
+            message("Downloading Raspberry Pi Pico SDK")
+            FetchContent_Populate(pico_sdk)
+            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
+        endif ()
+        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
+    else ()
+        message(FATAL_ERROR
+                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
+                )
+    endif ()
+endif ()
+
+get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${PICO_SDK_PATH})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
+endif ()
+
+set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
+if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
+endif ()
+
+set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
+
+include(${PICO_SDK_INIT_CMAKE_FILE})
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/cmake/CMakeLists.txt
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/cmake/CMakeLists.txt
@@ -0,0 +1,29 @@
+add_library(GobotRPC STATIC
+    ${CMAKE_CURRENT_LIST_DIR}/../ctrl_interface/base.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../ctrl_interface/ci_instruction.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../ctrl_interface/uart_rp2040/hardware_rp2040_uart.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../ctrl_interface/uart_rp2040/rx.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../crc16.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../protocol_base.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_init.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_tx.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_rx.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_int.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_ci_instructions.cpp
+)
+
+target_include_directories(GobotRPC PUBLIC 
+    ${CMAKE_CURRENT_LIST_DIR}/../include
+    ${CMAKE_CURRENT_LIST_DIR}/../include/util
+    ${CMAKE_CURRENT_LIST_DIR}/../include/ti
+    ${CMAKE_CURRENT_LIST_DIR}/../../..
+    ${CMAKE_CURRENT_LIST_DIR}/../../../include
+)
+
+target_link_libraries(GobotRPC
+    FreeRTOS-Kernel-Heap4
+    pico_stdlib
+    hardware_uart
+    hardware_irq
+    hardware_i2c
+)
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/cmake/gobotRPC_Node.cmake
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/cmake/gobotRPC_Node.cmake
@@ -0,0 +1,19 @@
+add_library(GobotRPC_Node_RP2040_I2C STATIC
+    ${CMAKE_CURRENT_LIST_DIR}/../node_interface/rp2040/i2c/init.cpp  
+    ${CMAKE_CURRENT_LIST_DIR}/../node_interface/rp2040/i2c/rx_tx.cpp  
+    ${CMAKE_CURRENT_LIST_DIR}/../crc16.cpp   
+    ${CMAKE_CURRENT_LIST_DIR}/../protocol_base.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../node_interface/init.cpp
+)
+target_include_directories(GobotRPC_Node_RP2040_I2C PUBLIC 
+    ${CMAKE_CURRENT_LIST_DIR}/../node_interface/include
+    ${CMAKE_CURRENT_LIST_DIR}/../include/util
+    ${CMAKE_CURRENT_LIST_DIR}/../../..
+)
+
+target_link_libraries(GobotRPC_Node_RP2040_I2C
+    FreeRTOS-Kernel-Heap4
+    pico_stdlib
+    hardware_i2c
+    hardware_irq
+)
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/crc16.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/crc16.cpp
@@ -0,0 +1,39 @@
+#include "crc16.hpp"
+
+#define POLY 0x8408
+/*
+//                                      16   12   5
+// this is the CCITT CRC 16 polynomial X  + X  + X  + 1.
+// This works out to be 0x1021, but the way the algorithm works
+// lets us use 0x8408 (the reverse of the bit pattern).  The high
+// bit is always assumed to be set, thus we only use 16 bits to
+// represent the 17 bit value.
+*/
+
+unsigned short crc16(char *data_p, unsigned short length)
+{
+      unsigned char i;
+      unsigned int data;
+      unsigned int crc = 0xffff;
+
+      if (length == 0)
+            return (~crc);
+
+      do
+      {
+            for (i=0, data=(unsigned int)0xff & *data_p++;
+                 i < 8; 
+                 i++, data >>= 1)
+            {
+                  if ((crc & 0x0001) ^ (data & 0x0001))
+                        crc = (crc >> 1) ^ POLY;
+                  else  crc >>= 1;
+            }
+      } while (--length);
+
+      crc = ~crc;
+      data = crc;
+      crc = (crc << 8) | (data >> 8 & 0xff);
+
+      return (crc);
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/base.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/base.cpp
@@ -0,0 +1,100 @@
+#include "ci/base.hpp"
+#include "pinConfig.hpp"
+
+#include "protocol.hpp"
+
+#include "pico/stdlib.h"
+#include "hardware/watchdog.h"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+
+GobotRPC_CI::GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue, QueueHandle_t ciInstructionReverseQueue) {
+    this->ciInstructionQueue = ciInstructionQueue;
+    this->ciInstructionReverseQueue = ciInstructionReverseQueue;
+    
+    this->hardware = hardware;
+    this->hardware->registerCB_RxData(GobotRPC_CI_rxData_cb, this);
+
+    this->cb_TxPacket = NULL;
+    this->cb_TxPacket_args = NULL;
+    this->cb_SetAddressMap = NULL;
+    this->cb_SetAddressMap_args = NULL;
+
+    xTaskCreateAffinitySet(GobotRPC_CI_heartBeatTaskFn, "Heartbeat Task", 2048, this, 2, core, &this->heartBeatTaskHandle);
+    xTaskCreateAffinitySet(GobotRPC_CI_txCIInstructionTaskFn, "Tx CI Instruction Task", 2048, this, 3, core, &this->txCIInstructionTaskHandle);
+}
+
+// Rx Side
+void GobotRPC_CI_rxData_cb(void * args, char *data, size_t len) {
+    GobotRPC_CI *gobotRPC = (GobotRPC_CI *)args;
+    gobotRPC->onRxData(data, len);
+}
+
+void GobotRPC_CI::onRxData(char *data, size_t len) {
+    GobotRPC_CI_CMD cmd = (GobotRPC_CI_CMD)data[0];
+
+    switch (cmd) {
+        case TX_CI_PACKET:
+            if(this->cb_TxPacket != NULL) {
+                uint32_t addr = (data[2] << 24) | (data[3] << 16) | (data[4] << 8) | data[5];
+                this->cb_TxPacket(this->cb_TxPacket_args, data+6, len-6, addr);
+            }
+            break;
+
+        case RESET_CI_PACKET:
+            //softwareReset();
+            break;
+
+        case SET_ADDR_PORT_MAP: {
+            if(this->cb_SetAddressMap != NULL) {
+                uint32_t addr = (data[2] << 24) | (data[3] << 16) | (data[4] << 8) | data[5];
+                uint8_t port = data[6];
+                this->cb_SetAddressMap(cb_SetAddressMap_args, addr, port);
+            }
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+void GobotRPC_CI::registerCB_TxPacket(callback_TxPacket cb, void *args) {
+    this->cb_TxPacket = cb;
+    this->cb_TxPacket_args = args;
+}
+
+void GobotRPC_CI::registerCB_SetAddress(callback_SetAddress cb, void *args) {
+    this->cb_SetAddressMap = cb;
+    this->cb_SetAddressMap_args = args;
+}
+
+void GobotRPC_CI::send_RxPacket(char *data, size_t len, uint32_t addr) {
+    data[0] = RX_CI_PACKET;
+    data[1] = len + CI_RX_PACKAGE_DATA_OFFSET;
+
+    data[2] = (addr >> 24) & 0xff;
+    data[3] = (addr >> 16) & 0xff;
+    data[4] = (addr >> 8) & 0xff;
+    data[5] = addr & 0xff;
+
+    this->hardware->send(data, len+6);
+}
+
+// Heartbeat Task
+
+void GobotRPC_CI_heartBeatTaskFn(void *args) {
+    GobotRPC_CI *gobotRPC = (GobotRPC_CI *)args;
+    gobotRPC->heartBeartTaskFn();
+}
+
+void GobotRPC_CI::heartBeartTaskFn() {
+    char heartBeatPacket[] = {HEARTBEAT, 0x02};
+    while(1) {
+        this->hardware->send(heartBeatPacket, 2);
+        vTaskDelay(GOBOTRPC_HEARTBEAT_INTERVAL / portTICK_PERIOD_MS);
+    }
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/ci_instruction.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/ci_instruction.cpp
@@ -0,0 +1,90 @@
+#include "ci/base.hpp"
+
+#include "ci/instructions.hpp"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+void GobotRPC_CI_txCIInstructionTaskFn(void *args) {
+    GobotRPC_CI *gobotRPC = (GobotRPC_CI *)args;
+    gobotRPC->txCIInstructionTask();
+}
+
+void GobotRPC_CI::txCIInstructionTask() {
+    while(1) {
+        CI_Instruction_Transport ciInstruction;
+        xQueueReceive(ciInstructionQueue, &ciInstruction, portMAX_DELAY);
+
+        switch (ciInstruction.type) {
+            case CI_INSTRUCTION_SEND_TRANMISSION_ERROR: {
+                uint32_t addr = ciInstruction.data[1] | (ciInstruction.data[2] << 8) | (ciInstruction.data[3] << 16) | (ciInstruction.data[4] << 24);
+                uint8_t rx = ciInstruction.data[0];
+                send_ErrorTransmission(rx, addr);
+                break;
+            }
+
+            case CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS: {
+                uint32_t addr = ciInstruction.data[0] \
+                    | (ciInstruction.data[1] << 8) \
+                    | (ciInstruction.data[2] << 16) \
+                    | (ciInstruction.data[3] << 24);
+                send_SuccessTransmission(addr);
+                break;
+            }
+
+            case CI_INSTRUCTION_SEND_INFO_RESET:
+                send_InfoReset();
+                break;
+        }
+    }
+}
+
+void GobotRPC_CI::send_ErrorTransmission(bool rx, uint64_t addr) {
+    char errorPacket[7];
+
+    errorPacket[0] = ERROR_TRANMISSION;
+    errorPacket[1] = 7;
+    errorPacket[2] = rx ? 0x01 : 0x00;
+    errorPacket[3] = (addr >> 24) & 0xff;
+    errorPacket[4] = (addr >> 16) & 0xff;
+    errorPacket[5] = (addr >> 8) & 0xff;
+    errorPacket[6] = addr & 0xff;
+
+    this->hardware->send(errorPacket, 7);
+}
+
+void GobotRPC_CI::send_SuccessTransmission(uint64_t addr) {
+    char successPacket[6];
+
+    successPacket[0] = SUCESS_TRANMISSION;
+    successPacket[1] = 6;
+    successPacket[2] = (addr >> 24) & 0xff;
+    successPacket[3] = (addr >> 16) & 0xff;
+    successPacket[4] = (addr >> 8) & 0xff;
+    successPacket[5] = addr & 0xff;
+
+    this->hardware->send(successPacket, 6);
+}
+
+void GobotRPC_CI::send_InfoReset() {
+    char resetPacket[2];
+
+    resetPacket[0] = RESET_INFO_CI_PACKET;
+    resetPacket[1] = 2;
+
+    this->hardware->send(resetPacket, 2);
+}
+
+// CI Reverse Instructions (CI -> TI)
+void GobotRPC_CI::send_rev_SetAddrMap(uint32_t addr, uint8_t port) {
+    CI_Instruction_Transport ciInstruction;
+    ciInstruction.type = CI_INSTRUCTION_REV_SEND_SET_ADDR_MAP;
+    ciInstruction.data[0] = addr & 0xff;
+    ciInstruction.data[1] = (addr >> 8) & 0xff;
+    ciInstruction.data[2] = (addr >> 16) & 0xff;
+    ciInstruction.data[3] = (addr >> 24) & 0xff;
+    ciInstruction.data[4] = port;
+
+    xQueueSend(ciInstructionReverseQueue, &ciInstruction, portMAX_DELAY);
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/uart_rp2040/hardware_rp2040_uart.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/uart_rp2040/hardware_rp2040_uart.cpp
@@ -0,0 +1,54 @@
+#include "ci/hardware.hpp"
+#include "pinConfig.hpp"
+
+#include "pico/stdlib.h"
+#include "hardware/uart.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+#include <string.h>
+
+// Initialize stuff
+GobotRPC_CI_Hardware_RP2040_UART * g_GobotRPC_CI_Hardware_RP2040_UART;
+
+GobotRPC_CI_Hardware_RP2040_UART::GobotRPC_CI_Hardware_RP2040_UART(uart_inst_t *uart, uint baudrate, UBaseType_t core) {
+    g_GobotRPC_CI_Hardware_RP2040_UART = this;
+    this->core = core;
+
+    initRxTaskAndQueues();
+
+    uartTxMutex = xSemaphoreCreateMutex();
+
+    this->uart = uart;
+    uart_init(this->uart, baudrate);
+    
+    gpio_set_function(GOBOTRPC_CI_UART_RX, UART_FUNCSEL_NUM(uart, GOBOTRPC_CI_UART_RX));
+    gpio_set_function(GOBOTRPC_CI_UART_TX, UART_FUNCSEL_NUM(uart, GOBOTRPC_CI_UART_TX));
+    
+    uart_set_hw_flow(uart, false, false);
+    uart_set_format(uart, 8, 1, UART_PARITY_NONE);
+    uart_set_baudrate(uart, baudrate);
+    uart_set_fifo_enabled(uart, true);
+
+    int UART_IRQ = uart == uart0 ? UART0_IRQ : UART1_IRQ;
+
+    irq_set_exclusive_handler(UART_IRQ, GobotRPC_CI_Hardware_RP2040_UART_isr);
+    irq_set_enabled(UART_IRQ, true);
+
+    uart_set_irq_enables(uart, true, false);
+}
+
+void GobotRPC_CI_Hardware_RP2040_UART::registerCB_RxData(callback_rxData cb, void *args) {
+    this->cb_rxData = cb;
+    this->cb_rxData_args = args;
+}
+
+
+// Sending data
+void GobotRPC_CI_Hardware_RP2040_UART::send(char *data, size_t len) {
+    xSemaphoreTake(uartTxMutex, portMAX_DELAY);
+    uart_write_blocking(this->uart, (uint8_t *)(data), len);
+    uart_tx_wait_blocking(this->uart);
+    xSemaphoreGive(uartTxMutex);
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/uart_rp2040/rx.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/uart_rp2040/rx.cpp
@@ -0,0 +1,122 @@
+#include "ci/hardware.hpp"
+#include "pico/stdlib.h"
+
+#include <string.h>
+
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+
+// External Functions
+void GobotRPC_CI_Hardware_RP2040_UART_isr() {
+    GobotRPC_CI_Hardware_RP2040_UART *uart = (GobotRPC_CI_Hardware_RP2040_UART *)g_GobotRPC_CI_Hardware_RP2040_UART;
+    uart->onRx_ISR();
+};
+
+void GobotRPC_CI_Hardware_RP2040_UART_Task_RXProcessing(void *args) {
+    GobotRPC_CI_Hardware_RP2040_UART *uart = (GobotRPC_CI_Hardware_RP2040_UART *)args;
+    uart->rxProcessingTaskFn();
+}
+
+// Init
+
+void GobotRPC_CI_Hardware_RP2040_UART::initRxTaskAndQueues() {
+    emptyInputBuffersQueue = xQueueCreate(NUM_INPUT_BUFFERS, sizeof(inputBuffers_t *));
+    filledInputBuffersQueue = xQueueCreate(NUM_INPUT_BUFFERS, sizeof(inputBuffers_t *));
+
+    for(int i = 0; i < NUM_INPUT_BUFFERS; i++) {
+        inputBuffers_t * p = &inputBufferPool[i];
+        memset(p->data, 0xff, 256);
+        p->len = 0;
+        p->state = NEW;
+        p->expected_length = 0;
+        xQueueSend(emptyInputBuffersQueue, &p, portMAX_DELAY);
+    }
+
+    rxSignalSemaphore = xSemaphoreCreateBinary();
+    
+    xTaskCreateAffinitySet(GobotRPC_CI_Hardware_RP2040_UART_Task_RXProcessing, "UART RX Processing", 1024, this, 3, core, &this->rxProcessingTaskHandle);
+}
+
+inputBufferRXState_t fillBuffer(inputBuffers_t * buffer, char data) {
+    switch(buffer->state) {
+        case NEW:
+            buffer->data[0] = data;
+            buffer->len = 1;
+            buffer->state = READ_SIZE;
+            break;
+        
+        case READ_SIZE:
+            buffer->data[1] = data;
+            buffer->expected_length = data;
+            buffer->len += 1;
+
+            if(buffer->len == buffer->expected_length) {
+                buffer->state = FULL;
+            } else {
+                buffer->state = READ_DATA;
+            }
+            break;
+
+        case READ_DATA:
+            buffer->data[buffer->len] = data;
+            buffer->len += 1;
+
+            if(buffer->len == buffer->expected_length) {
+                buffer->state = FULL;
+            } else {
+                buffer->state = READ_DATA;
+            }
+            break;
+
+        case FULL:
+            break;
+    }
+    return buffer->state;
+}
+
+// real Task Fns
+void GobotRPC_CI_Hardware_RP2040_UART::onRx_ISR() {
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+
+    static inputBuffers_t * inputBuffer = NULL;
+
+    while (uart_is_readable(this->uart)) {
+
+        if(inputBuffer == NULL) {
+            xQueueReceiveFromISR(emptyInputBuffersQueue, &inputBuffer, &xHigherPriorityTaskWoken);
+
+            if(inputBuffer == NULL) {
+                return; // No more buffers -> device overload
+            }
+
+            inputBuffer->state = NEW;
+            inputBuffer->len = 0;
+            inputBuffer->expected_length = 0;
+        }
+
+        inputBufferRXState_t state = fillBuffer(inputBuffer, uart_getc(this->uart));
+
+        if (state == FULL) {
+            xQueueSendFromISR(filledInputBuffersQueue, &inputBuffer, &xHigherPriorityTaskWoken);
+            inputBuffer = NULL;
+        }
+    }
+
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+}
+
+
+void GobotRPC_CI_Hardware_RP2040_UART::rxProcessingTaskFn() {
+    inputBuffers_t * inputBuffer = NULL;
+
+    while(1) {
+        xQueueReceive(filledInputBuffersQueue, &inputBuffer, portMAX_DELAY);
+
+        if(this->cb_rxData != NULL) {
+            this->cb_rxData(this->cb_rxData_args, inputBuffer->data, inputBuffer->len);
+        }
+
+        xQueueSend(emptyInputBuffersQueue, &inputBuffer, portMAX_DELAY);
+        vTaskDelay(1000);
+    }
+}  
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/base.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/base.hpp
@@ -0,0 +1,64 @@
+#pragma once
+#include "ci/hardware.hpp"
+#include <strings.h>
+
+enum GobotRPC_CI_CMD {
+    TX_CI_PACKET = 0x01,
+    RX_CI_PACKET = 0x02,
+    SET_ADDR_PORT_MAP = 0x03,
+
+    SUCESS_TRANMISSION = 0xfc,
+    ERROR_TRANMISSION = 0xfd,
+    HEARTBEAT = 0xff,
+
+    RESET_CI_PACKET = 0xcc,
+    RESET_INFO_CI_PACKET = 0xcd
+};
+
+#define CI_TX_PACKAGE_DATA_OFFSET 6
+#define CI_TX_PACKAGE_SIZE(data_len) (data_len + CI_TX_PACKAGE_SIZE)
+
+#define CI_RX_PACKAGE_DATA_OFFSET 6
+#define CI_RX_PACKAGE_SIZE(data_len) (data_len + CI_RX_PACKAGE_DATA_OFFSET)
+
+typedef void (*callback_TxPacket)(void * args, char *data, size_t len, uint32_t addr);
+typedef void (*callback_SetAddress)(void * args, uint32_t addr, uint8_t port);
+
+void GobotRPC_CI_rxData_cb(void * args, char *data, size_t len);
+void GobotRPC_CI_heartBeatTaskFn(void *args);
+void GobotRPC_CI_txCIInstructionTaskFn(void *args);
+
+class GobotRPC_CI {
+private:
+    I_GobotRPC_CI_Hardware *hardware;
+
+    callback_TxPacket cb_TxPacket;
+    void * cb_TxPacket_args;
+
+    callback_SetAddress cb_SetAddressMap;
+    void * cb_SetAddressMap_args;
+
+    TaskHandle_t heartBeatTaskHandle;
+    TaskHandle_t txCIInstructionTaskHandle;
+
+    QueueHandle_t ciInstructionQueue;
+    QueueHandle_t ciInstructionReverseQueue;
+public:
+    GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue, QueueHandle_t ciInstructionReverseQueue);
+
+    void registerCB_TxPacket(callback_TxPacket cb, void *args);
+    void registerCB_SetAddress(callback_SetAddress cb, void *args);
+
+    void send_RxPacket(char *data, size_t len, uint32_t addr);
+
+    void onRxData(char *data, size_t len);
+
+    void heartBeartTaskFn();
+
+    // CI Instruction Stuff
+    void send_rev_SetAddrMap(uint32_t addr, uint8_t port);
+    void txCIInstructionTask();
+    void send_ErrorTransmission(bool rx, uint64_t addr);
+    void send_SuccessTransmission(uint64_t addr);
+    void send_InfoReset();
+};
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/hardware.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/hardware.hpp
@@ -0,0 +1,73 @@
+#pragma once
+#include <strings.h>
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "semphr.h"
+#include "queue.h"
+
+typedef void (*callback_rxData)(void * args, char *data, size_t len);
+
+class I_GobotRPC_CI_Hardware {
+public:
+    virtual void send(char *data, size_t len) = 0;
+    virtual void registerCB_RxData(callback_rxData cb, void *args) = 0;
+};
+
+
+#include "hardware/uart.h"
+
+void GobotRPC_CI_Hardware_RP2040_UART_isr();
+void GobotRPC_CI_Hardware_RP2040_UART_Task_RXBuffering(void *args);
+void GobotRPC_CI_Hardware_RP2040_UART_Task_RXProcessing(void *args);
+
+#define NUM_INPUT_BUFFERS 16
+
+enum inputBufferRXState_t {
+    NEW,
+    READ_SIZE,
+    READ_DATA,
+    FULL
+};
+
+struct inputBuffers_t {
+    char data[256];
+    size_t expected_length;
+    size_t len;
+    inputBufferRXState_t state;
+};
+
+class GobotRPC_CI_Hardware_RP2040_UART : public I_GobotRPC_CI_Hardware {
+private:
+    uart_inst_t *uart;
+
+    inputBuffers_t inputBufferPool[NUM_INPUT_BUFFERS];
+
+    TaskHandle_t rxProcessingTaskHandle;
+
+    SemaphoreHandle_t rxSignalSemaphore;
+    QueueHandle_t emptyInputBuffersQueue;
+    QueueHandle_t filledInputBuffersQueue;
+
+    callback_rxData cb_rxData;
+    void * cb_rxData_args;
+
+    SemaphoreHandle_t uartTxMutex;
+
+    UBaseType_t core;
+
+public:
+    GobotRPC_CI_Hardware_RP2040_UART(uart_inst_t *uart, uint baudrate, UBaseType_t core);
+    void send(char *data, size_t len) override;
+    void registerCB_RxData(callback_rxData cb, void *args);
+    
+    void initRxTaskAndQueues();
+
+    void onRx_ISR();
+    void rxBufferingTaskFn();
+    void rxProcessingTaskFn();
+};
+
+extern GobotRPC_CI_Hardware_RP2040_UART * g_GobotRPC_CI_Hardware_RP2040_UART;
+
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/instructions.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/instructions.hpp
@@ -0,0 +1,17 @@
+enum CI_Instruction_Type {
+    CI_INSTRUCTION_SEND_TRANMISSION_ERROR,
+    CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS,
+    CI_INSTRUCTION_SEND_INFO_RESET,
+    CI_INSTRUCTION_REV_SEND_SET_ADDR_MAP,
+};
+
+struct CI_Instruction_Transport {
+    CI_Instruction_Type type;
+    uint32_t addr;
+    char data[16];
+};
+
+struct CI_Instruction_Data_SEND_TRANMISSION_ERROR {
+    bool rx;
+    uint32_t addr;
+};
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/main.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/main.hpp
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+struct AppData {
+    QueueHandle_t txQueue;
+    QueueHandle_t rxQueue;
+    QueueHandle_t ciInstructionQueue;
+    QueueHandle_t ciInstructionReverseQueue;
+};
+
+void main_core2(void * pvParameters);
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ti/transmission_interface.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ti/transmission_interface.hpp
@@ -0,0 +1,98 @@
+#pragma once
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "semphr.h"
+
+#include <stdint.h>
+#include <strings.h>
+
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+
+#include "pinConfig.hpp"
+
+#include "ci/instructions.hpp"
+
+struct GoRPCPackage_Transport {
+    uint32_t addr;
+    uint32_t len;
+    char data[256];
+};
+
+typedef void (*callback_pullPackage)(GoRPCPackage_Transport *src, void *args);
+typedef void (*callback_pushPackage)(GoRPCPackage_Transport *dest, void *args);
+typedef void (*callback_pushCIInstruction)(CI_Instruction_Transport *src, void *args);
+
+class GobotRPC_TI_Hardware {
+public:
+    virtual void registerPullPackageCB(callback_pullPackage cb, void *args) = 0;
+    virtual void registerPushPackageCB(callback_pushPackage cb, void *args) = 0;
+    virtual void registerPushCIInstructionCB(callback_pushCIInstruction cb, void *args) = 0;
+    virtual void setAddrMap(uint32_t addr, int intNum) = 0;
+
+    void raiseTranmissionError(bool rx, uint32_t addr);
+    void raiseTransmissionSuceess(uint32_t addr);
+    void raiseInfoReset();
+};
+
+void i2cRxTaskFn(void * args);
+void i2cTxTaskFn(void * args);
+
+void intPin_ISR(uint gpio, uint32_t events);
+
+class GobotRPC_TI_Hardware_RP2040_I2C : public GobotRPC_TI_Hardware {
+private:
+    callback_pullPackage pullPackageCB;
+    void *pullPackageCBArgs;
+
+    callback_pushPackage pushPackageCB;
+    void *pushPackageCBArgs;
+
+    callback_pushCIInstruction pushCIInstructionCB;
+    void *pushCIInstructionCBArgs;
+
+    SemaphoreHandle_t i2cMutex;
+    SemaphoreHandle_t i2cRXSemaphore;
+
+
+    TaskHandle_t i2cRxTaskHandle;
+    TaskHandle_t i2cTxTaskHandle;
+    TaskHandle_t i2cIntTaskHandle;
+    TaskHandle_t revCIInstructionTaskHandle;
+
+    QueueHandle_t revCIInstructionQueue;
+
+    UBaseType_t core;
+
+    i2c_inst_t * i2c;
+
+    uint32_t intAddressMap[GOBOTRPC_TI_INT_NUM];
+
+    void initTasks();
+    bool readI2C(GoRPCPackage_Transport * pkg, uint32_t addr);
+
+public:
+    GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c, QueueHandle_t revCIInstructionQueue);
+
+    void registerPullPackageCB(callback_pullPackage cb, void *args);
+    void registerPushPackageCB(callback_pushPackage cb, void *args);
+    void registerPushCIInstructionCB(callback_pushCIInstruction cb, void *args);
+    
+    void setAddrMap(uint32_t addr, int intNum);
+
+    void i2cRxTask();
+    void i2cTxTask();
+    void manualTriggerIntTask();
+    void intPinISR(BaseType_t * xHigherPriorityTaskWoken);
+    uint32_t readIntPins();
+
+    void raiseTranmissionError(bool rx, uint32_t addr);
+    void raiseTransmissionSuceess(uint32_t addr);
+    void raiseInfoReset();
+
+    void revCIInstructionTask();
+};
+
+
+extern GobotRPC_TI_Hardware_RP2040_I2C * g_GobotRPC_TI_Hardware_RP2040_I2C;
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/crc16.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/crc16.hpp
@@ -0,0 +1,2 @@
+#pragma once
+unsigned short crc16(char *data_p, unsigned short length);
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/protocol.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/protocol.hpp
@@ -0,0 +1,45 @@
+#pragma once
+#include <stdint.h>
+#include <strings.h>
+
+enum GobotRPCNumber {
+    HOME_XY = 0x1,
+    SET_PADDING = 0x2,
+    GOTO = 0x3,
+    RELEASE_MOTORS = 0x4,
+
+    DROP_STONE = 0x5,
+    MOVE_Z_AXIS = 0x7,
+
+    VACUM = 0x8,
+    RESET = 0xc,
+    GET_INFO = 0xd
+};
+
+enum GobotRPCTypes {
+    REQUEST  = 0b00,
+    RESPONSE = 0b01,
+    ERROR    = 0b10
+};
+
+struct GobotRPCPackage_Req_Vacum {
+    uint8_t enable : 8;
+};
+
+#define GobotRPC_Package_DATA_OFFSET 2
+#define CALC_SIZE_GobotRPC_PACKAGE(data_len) (data_len + GobotRPC_Package_DATA_OFFSET + 2)
+
+struct GobotRPCHeaderInfo {
+public:
+    GobotRPCNumber number;
+    GobotRPCTypes type;
+    size_t len;
+};
+
+void assembleGobotRPCHeader(char * buffer, GobotRPCNumber number, GobotRPCTypes data_size, size_t);
+void assembleCRC(char * buffer, size_t data_len);
+bool checkCRC(char * buffer, size_t data_len);
+
+GobotRPCHeaderInfo extractGobotRPCHeader(char * buffer);
+
+void softwareReset();
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface.hpp
@@ -0,0 +1,45 @@
+#pragma once
+#include "protocol.hpp"
+#include "node_interface_hardware.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+typedef void (*onPackageRxCallback)(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number);
+
+enum NODE_TYPE: uint8_t {
+    NODE_TYPE_VACUM = 0xa1,
+    NODE_TYPE_HEAD = 0xa2,
+    NODE_TYPE_COREXY = 0xa3,
+};
+
+struct InfoData {
+    uint32_t addr;
+    NODE_TYPE type;
+};
+
+class GobotRPC_NI {
+private:
+    onPackageRxCallback onPackageRx;
+    void *onPackageRxArgs;
+
+    QueueHandle_t txQueue;
+    QueueHandle_t rxQueue;
+    TaskHandle_t rxTaskHandle;
+    
+    InfoData info;
+
+    unsigned int core;
+
+public:
+    GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue, InfoData info);
+    
+    void registerOnPackageRxCallback(onPackageRxCallback callback, void *context);
+    void sendPackage(char *data, size_t len, GobotRPCTypes type, GobotRPCNumber number);
+    
+    InfoData getInfo();
+
+    void rxTask();
+};
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface_hardware.hpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/include/node_interface_hardware.hpp
@@ -0,0 +1,78 @@
+#pragma once
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "queue.h"
+
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#include "hardware/irq.h"
+
+struct GobotRPC_NI_Package_Transport {
+    char data[256];
+    uint length;
+    uint index;
+};
+
+enum I2C_WRITE_STATE {
+    I2C_WRITE_STATE_FIRST,
+    I2C_WRITE_STATE_LENGTH,
+    I2C_WRITE_STATE_DATA,
+};
+
+enum I2C_READ_STATE {
+    I2C_READ_STAGE_FIRST,
+    I2C_READ_STAGE_DONE,
+    I2C_READ_STAGE_WIP,
+    I2C_READ_STAGE_INVALID
+};
+
+enum I2C_SLAVE_EVENT {
+    I2C_SLAVE_RECEIVE,
+    I2C_SLAVE_REQUEST,
+    I2C_SLAVE_FINISH
+};
+
+class GobotRPC_NI_Hardware {
+
+};
+
+struct GobotRPC_NI_Hardware_RP2040_I2C_PreTxQueues {
+    QueueHandle_t isrPreTxQueue;
+    QueueHandle_t isrTXQueue;
+};
+
+class GobotRPC_NI_Hardware_RP2040_I2C : public GobotRPC_NI_Hardware {
+    uint core;
+
+    QueueHandle_t isrPreTxQueue;
+    QueueHandle_t isrTXQueue;
+    QueueHandle_t isrRXQueue;
+    TaskHandle_t preTxTaskHandle;
+
+    uint i2cSDA_PIN;
+    uint i2cSCL_PIN;
+    uint int_PIN;
+    uint8_t i2cAddress;
+
+    I2C_WRITE_STATE writeState = I2C_WRITE_STATE_FIRST;
+    I2C_READ_STATE readState = I2C_READ_STAGE_INVALID;
+
+    GobotRPC_NI_Package_Transport rxPackage;
+    GobotRPC_NI_Package_Transport txPackage;
+    
+public:
+    i2c_inst_t * i2c_inst;
+
+    GobotRPC_NI_Hardware_RP2040_I2C(
+        xQueueHandle TXQueue, xQueueHandle RXQueue, uint core,
+        i2c_inst_t * i2c_inst,
+        uint8_t i2cAddress,
+        uint i2cSDA_PIN, uint i2cSCL_PIN, uint int_PIN
+    );
+
+    void onI2CIRQ(I2C_SLAVE_EVENT event, BaseType_t * xHigherPriorityTaskWoken);
+    void preTxTask();
+};
+
+extern GobotRPC_NI_Hardware_RP2040_I2C * g_gobotrpc_ni_hardware_rp2040_i2c;
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/init.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/init.cpp
@@ -0,0 +1,73 @@
+#include "node_interface.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include <string.h>
+
+static void rxTaskFn(void *pvParameters) {
+    GobotRPC_NI *ni = (GobotRPC_NI *)pvParameters;
+    ni->rxTask();
+}
+
+GobotRPC_NI::GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue, InfoData info) {
+    this->core = core;
+    this->info = info;
+
+    this->txQueue = txQueue;
+    this->rxQueue = rxQueue;
+
+    this->onPackageRx = NULL;
+    this->onPackageRxArgs = NULL;
+
+    xTaskCreateAffinitySet(
+        rxTaskFn,
+        "GobotRPC_NI RX Task",
+        2048,
+        this,
+        4,
+        this->core,
+        &this->rxTaskHandle
+    );
+}
+
+void GobotRPC_NI::registerOnPackageRxCallback(onPackageRxCallback callback, void *context) {
+    this->onPackageRx = callback;
+    this->onPackageRxArgs = context;
+}
+
+void GobotRPC_NI::rxTask() {
+    while (true) {
+        GobotRPC_NI_Package_Transport package;
+        
+        if (xQueueReceive(this->rxQueue, &package, portMAX_DELAY) == pdTRUE) {    
+            GobotRPCHeaderInfo header = extractGobotRPCHeader(package.data);
+
+            if(this->onPackageRx != NULL) {
+                if (!checkCRC(package.data, header.len)) {
+                    volatile int a = 0;
+                    continue;
+                }
+
+                this->onPackageRx(
+                    onPackageRxArgs, 
+                    package.data + GobotRPC_Package_DATA_OFFSET, 
+                    header.len, header.type, header.number
+                );
+            }
+        }
+    }
+}
+
+void GobotRPC_NI::sendPackage(char *data, size_t len, GobotRPCTypes type, GobotRPCNumber number) {
+    GobotRPC_NI_Package_Transport package;
+    package.length = len + GobotRPC_Package_DATA_OFFSET + 2;
+    memcpy(package.data + GobotRPC_Package_DATA_OFFSET, data, len);
+    assembleGobotRPCHeader(package.data, number, type, len);
+    assembleCRC(package.data, len);
+    xQueueSend(this->txQueue, &package, portMAX_DELAY);
+}
+
+InfoData GobotRPC_NI::getInfo() {
+    return info;
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/init.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/init.cpp
@@ -0,0 +1,127 @@
+#include "node_interface_hardware.hpp"
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#include <string.h>
+
+GobotRPC_NI_Hardware_RP2040_I2C * g_gobotrpc_ni_hardware_rp2040_i2c;
+i2c_inst_t * g_i2c_inst;
+bool g_transfer_in_progress = false;
+
+static void finish_transfer(BaseType_t * xHigherPriorityTaskWoken) {
+    if(g_transfer_in_progress) {
+        GobotRPC_NI_Hardware_RP2040_I2C * gobotrpc_ni = g_gobotrpc_ni_hardware_rp2040_i2c;
+        g_transfer_in_progress = false;
+        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_FINISH, xHigherPriorityTaskWoken);
+    }
+}
+
+static void i2c_isr() {
+    GobotRPC_NI_Hardware_RP2040_I2C * gobotrpc_ni = g_gobotrpc_ni_hardware_rp2040_i2c;
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+
+    i2c_hw_t * hw = i2c_get_hw(g_i2c_inst);
+    uint32_t intr_stat = hw->intr_stat;
+
+    if (intr_stat == 0) {
+        return;
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) {
+        hw->clr_tx_abrt;
+        finish_transfer(&xHigherPriorityTaskWoken);
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) {
+        hw->clr_start_det;
+        finish_transfer(&xHigherPriorityTaskWoken);
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) {
+        hw->clr_stop_det;
+        finish_transfer(&xHigherPriorityTaskWoken);
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) {
+        g_transfer_in_progress = true;
+        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_RECEIVE, &xHigherPriorityTaskWoken);
+    }
+
+    if (intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) {
+        hw->clr_rd_req;
+        g_transfer_in_progress = true;
+        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_REQUEST, &xHigherPriorityTaskWoken);
+    }
+
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+}
+
+static void preTxTaskFn(void *pvParameters) {
+    GobotRPC_NI_Hardware_RP2040_I2C * hw = (GobotRPC_NI_Hardware_RP2040_I2C *)(pvParameters);
+    hw->preTxTask();
+};
+
+GobotRPC_NI_Hardware_RP2040_I2C::GobotRPC_NI_Hardware_RP2040_I2C(
+    xQueueHandle TXQueue, xQueueHandle RXQueue, uint core,
+    i2c_inst_t * i2c_inst,
+    uint8_t i2cAddress,
+    uint i2cSDA_PIN, uint i2cSCL_PIN, uint int_PIN
+) {
+    g_gobotrpc_ni_hardware_rp2040_i2c = this;
+    g_i2c_inst = i2c_inst;
+
+    this->core = core;
+
+    this->readState = I2C_READ_STAGE_FIRST;
+    this->writeState = I2C_WRITE_STATE_FIRST;
+
+    this->isrRXQueue = RXQueue;
+    this->isrPreTxQueue = TXQueue;
+    this->isrTXQueue = xQueueCreate(3, sizeof(GobotRPC_NI_Package_Transport));
+
+    xTaskCreateAffinitySet(
+        preTxTaskFn,
+        "GobotRPC_NI Hardware RP2040 I2C PreTx Task",
+        2048,
+        this,
+        4,
+        this->core,
+        &this->preTxTaskHandle
+    );
+
+    this->i2cSDA_PIN = i2cSDA_PIN;
+    this->i2cSCL_PIN = i2cSCL_PIN;
+    this->int_PIN = int_PIN;
+
+    this->i2cAddress = i2cAddress;
+    this->i2c_inst = i2c_inst;
+
+    memset(&rxPackage.data, 0, sizeof(GobotRPC_NI_Package_Transport::data));
+    memset(&txPackage.data, 0, sizeof(GobotRPC_NI_Package_Transport::data));
+
+    gpio_init(i2cSDA_PIN);
+    gpio_init(i2cSCL_PIN);
+    gpio_set_function(i2cSDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(i2cSCL_PIN, GPIO_FUNC_I2C);
+
+    gpio_init(int_PIN);
+    gpio_set_dir(int_PIN, GPIO_OUT);
+    gpio_put(int_PIN, 0);
+
+    // Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty.
+    // There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it
+    // disabled since the Rx FIFO should never fill up (unless slave->handler() is way too slow).
+    i2c_set_slave_mode(i2c_inst, true, i2cAddress);
+
+    i2c_hw_t *hw = i2c_get_hw(i2c_inst);
+    // unmask necessary interrupts
+    hw->intr_mask = I2C_IC_INTR_MASK_M_RX_FULL_BITS |\
+        I2C_IC_INTR_MASK_M_RD_REQ_BITS |\
+        I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS |\
+        I2C_IC_INTR_MASK_M_STOP_DET_BITS |\ 
+        I2C_IC_INTR_MASK_M_START_DET_BITS;
+
+    // enable interrupt for current core
+    unsigned int intNum = i2c_inst == i2c0 ? I2C0_IRQ : I2C1_IRQ;
+    irq_set_exclusive_handler(intNum, i2c_isr);
+    irq_set_enabled(intNum, true);
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/rx_tx.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/rp2040/i2c/rx_tx.cpp
@@ -0,0 +1,99 @@
+#include "node_interface_hardware.hpp"
+#include "hardware/irq.h"
+
+void GobotRPC_NI_Hardware_RP2040_I2C::onI2CIRQ(I2C_SLAVE_EVENT event, BaseType_t * xHigherPriorityTaskWoken) {
+
+    switch (event) {
+    case I2C_SLAVE_RECEIVE: { // I2C Write from Master
+        char data = i2c_read_byte_raw(i2c_inst);
+
+        switch (writeState) {
+        case I2C_WRITE_STATE_FIRST:
+            writeState = I2C_WRITE_STATE_LENGTH;
+            rxPackage.index = 0;
+            rxPackage.length = 0;
+            rxPackage.data[rxPackage.index++] = data;
+            break;
+
+        case I2C_WRITE_STATE_LENGTH:
+            rxPackage.data[rxPackage.index++] = data;
+            rxPackage.length = data;
+            writeState = I2C_WRITE_STATE_DATA;
+            break;
+        
+        case I2C_WRITE_STATE_DATA:
+            rxPackage.data[rxPackage.index++] = data;
+            
+            if(rxPackage.index == rxPackage.length) {
+                xQueueSendFromISR(isrRXQueue, &rxPackage, xHigherPriorityTaskWoken);
+                writeState = I2C_WRITE_STATE_FIRST;
+            } else {
+                writeState = I2C_WRITE_STATE_DATA;
+            }
+
+            break;
+
+        default:
+            break;
+        }
+
+        break;
+    }
+
+    case I2C_SLAVE_REQUEST: { 
+        // I2C Read from Master
+        //if(xQueueIsQueueEmptyFromISR(isrTXQueue) == pdTRUE) {
+        //    readState = I2C_READ_STAGE_INVALID;
+        //}
+
+        switch (readState) {
+        case I2C_READ_STAGE_INVALID:
+            i2c_write_byte_raw(i2c_inst, 0x01);
+            break;
+        
+        case I2C_READ_STAGE_FIRST:
+            xQueueReceiveFromISR(isrTXQueue, &txPackage, xHigherPriorityTaskWoken);
+            txPackage.index = 0;
+            i2c_write_byte_raw(i2c_inst, txPackage.data[txPackage.index++]);
+            readState = I2C_READ_STAGE_WIP;
+            break;
+
+        case I2C_READ_STAGE_WIP:
+            i2c_write_byte_raw(i2c_inst, txPackage.data[txPackage.index++]);
+            
+            if(txPackage.index == txPackage.length) {
+                readState = I2C_READ_STAGE_FIRST;
+                gpio_put(int_PIN, 0);
+            }
+
+            break;
+
+        default:
+            break;
+        }
+
+        break;
+    }
+
+    case I2C_SLAVE_FINISH:
+        writeState = I2C_WRITE_STATE_FIRST;
+        
+        rxPackage.index = 0;
+        txPackage.index = 0;
+        break;
+
+    default:
+        break;
+    }
+}
+
+void GobotRPC_NI_Hardware_RP2040_I2C::preTxTask() {
+    GobotRPC_NI_Package_Transport pkg;
+
+    while (1) {
+        xQueueReceive(isrPreTxQueue, &pkg, portMAX_DELAY);
+        xQueueSend(isrTXQueue, &pkg, portMAX_DELAY);
+        vTaskDelay(5 / portTICK_PERIOD_MS);
+        gpio_put(int_PIN, 1);
+    }
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/protocol_base.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/protocol_base.cpp
@@ -0,0 +1,37 @@
+#include "protocol.hpp"
+#include <stdint.h>
+
+#include "pico/stdlib.h"
+#include "hardware/watchdog.h"
+
+#include "crc16.hpp"
+
+void assembleGobotRPCHeader(char * buffer, GobotRPCNumber number, GobotRPCTypes type, size_t len) {
+    buffer[0] = ((number & 0b1111) << 4) | ((type & 0b11) << 2);
+    buffer[1] = (len + 4) & 0xff;
+}
+
+GobotRPCHeaderInfo extractGobotRPCHeader(char * buffer) {
+    GobotRPCHeaderInfo info;
+    info.number = (GobotRPCNumber)((buffer[0] >> 4) & 0b1111);
+    info.type = (GobotRPCTypes)((buffer[0] >> 2) & 0b11);
+    info.len = buffer[1];
+    return info;
+}
+
+void assembleCRC(char * buffer, size_t data_len) {
+    unsigned short crc = crc16(buffer, data_len + GobotRPC_Package_DATA_OFFSET);
+    buffer[GobotRPC_Package_DATA_OFFSET + data_len + 1] = crc & 0xff;
+    buffer[GobotRPC_Package_DATA_OFFSET + data_len + 0] = (crc >> 8) & 0xff;
+}
+
+bool checkCRC(char * buffer, size_t data_len) {
+    unsigned short crc = crc16(buffer, data_len - 2);
+    unsigned short crc_received = buffer[data_len - GobotRPC_Package_DATA_OFFSET + 1] | (buffer[data_len - GobotRPC_Package_DATA_OFFSET + 0] << 8);
+    return crc == crc_received;
+}
+
+void softwareReset() {
+    watchdog_enable(1, 1);
+    while(1);
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_ci_instructions.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_ci_instructions.cpp
@@ -0,0 +1,53 @@
+#include "transmission_interface.hpp"
+
+#include "FreeRTOS.h"
+#include "queue.h"
+
+void GobotRPC_TI_Hardware_RP2040_I2C::raiseTranmissionError(bool rx, uint32_t addr) {
+    CI_Instruction_Transport ciInstruction;
+    ciInstruction.type = CI_INSTRUCTION_SEND_TRANMISSION_ERROR;
+    ciInstruction.data[0] = rx;
+    ciInstruction.data[1] = addr & 0xff;
+    ciInstruction.data[2] = (addr >> 8) & 0xff;
+    ciInstruction.data[3] = (addr >> 16) & 0xff;
+    ciInstruction.data[4] = (addr >> 24) & 0xff;
+    
+    this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::raiseTransmissionSuceess(uint32_t addr) {
+    CI_Instruction_Transport ciInstruction;
+    ciInstruction.type = CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS;
+    ciInstruction.data[0] = addr & 0xff;
+    ciInstruction.data[1] = (addr >> 8) & 0xff;
+    ciInstruction.data[2] = (addr >> 16) & 0xff;
+    ciInstruction.data[3] = (addr >> 24) & 0xff;
+    
+    this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::raiseInfoReset() {
+    CI_Instruction_Transport ciInstruction;
+    ciInstruction.type = CI_INSTRUCTION_SEND_INFO_RESET;
+    
+    this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::revCIInstructionTask() {
+    CI_Instruction_Transport ciInstruction;
+    while(1) {
+        xQueueReceive(revCIInstructionQueue, &ciInstruction, portMAX_DELAY);
+        
+        switch (ciInstruction.type) {
+            case CI_INSTRUCTION_REV_SEND_SET_ADDR_MAP: {
+                uint32_t addr = ciInstruction.data[0] |\
+                    (ciInstruction.data[1] << 8) |\ 
+                    (ciInstruction.data[2] << 16) |\
+                    (ciInstruction.data[3] << 24);
+                uint8_t port = ciInstruction.data[4];
+                this->setAddrMap(addr, port);
+                break;
+            }
+        }
+    }
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_init.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_init.cpp
@@ -0,0 +1,87 @@
+#include "transmission_interface.hpp"
+#include "hardware/i2c.h"
+
+#include <string.h>
+
+#include "pinConfig.hpp"
+
+GobotRPC_TI_Hardware_RP2040_I2C * g_GobotRPC_TI_Hardware_RP2040_I2C;
+
+GobotRPC_TI_Hardware_RP2040_I2C::GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c, QueueHandle_t revCIInstructionQueue) {
+    this->core = core;
+    this->i2c = i2c0;
+    this->revCIInstructionQueue = revCIInstructionQueue;
+
+    g_GobotRPC_TI_Hardware_RP2040_I2C = this;
+
+    i2c_init(i2c, GOBOTRPC_TI_CLOCK_SPEED);
+    gpio_set_function(GOBOTRPC_TI_I2C_SDA, GPIO_FUNC_I2C);
+    gpio_set_function(GOBOTRPC_TI_I2C_SCL, GPIO_FUNC_I2C);
+
+    if(!GOBOTRPC_TI_EXTERNAL_PULLUP) {
+        gpio_pull_up(GOBOTRPC_TI_I2C_SDA);
+        gpio_pull_up(GOBOTRPC_TI_I2C_SCL);
+    }
+
+    for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
+        intAddressMap[i] = 0xffffffff;
+        gpio_init(GOBOTRPC_TI_INT_PIN_START + i);
+        gpio_set_dir(GOBOTRPC_TI_INT_PIN_START + i, GPIO_IN);
+        //gpio_pull_down(GOBOTRPC_TI_INT_PIN_START + i);
+        gpio_set_irq_enabled_with_callback(GOBOTRPC_TI_INT_PIN_START + i, GPIO_IRQ_EDGE_RISE, true, intPin_ISR);
+    }
+
+    initTasks();
+
+    //gpio_init(GOBOTRPC_TI_COMBINED_INT_PIN);
+    //gpio_set_dir(GOBOTRPC_TI_COMBINED_INT_PIN, GPIO_IN);
+    //gpio_pull_down(GOBOTRPC_TI_COMBINED_INT_PIN);
+
+}
+
+static void i2cIntTaskFn(void * pvParameters) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)pvParameters;
+    hw->manualTriggerIntTask();
+}
+
+static void revCIInstructionTaskFn(void * pvParameters) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)pvParameters;
+    hw->revCIInstructionTask();
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::initTasks() {
+    i2cMutex = xSemaphoreCreateMutex();
+    i2cRXSemaphore = xSemaphoreCreateBinary();
+
+    pushPackageCB = NULL;
+    pullPackageCB = NULL;
+    pushPackageCBArgs = NULL;
+    pullPackageCBArgs = NULL;
+
+    volatile int a = xPortGetFreeHeapSize();
+
+    xTaskCreateAffinitySet(i2cRxTaskFn, "i2c Rx Task", 4096, this, 3, core, &i2cRxTaskHandle);
+    xTaskCreateAffinitySet(i2cTxTaskFn, "i2c Tx Task", 4096, this, 3, core, &i2cTxTaskHandle);
+    xTaskCreateAffinitySet(i2cIntTaskFn, "i2c Int Task", 4096, this, 3, core, &i2cIntTaskHandle);
+    xTaskCreateAffinitySet(revCIInstructionTaskFn, "Rev CI Instruction Task", 4096, this, 3, core, &revCIInstructionTaskHandle);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::registerPullPackageCB(callback_pullPackage cb, void *args) {
+    pullPackageCB = cb;
+    pullPackageCBArgs = args;
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::registerPushPackageCB(callback_pushPackage cb, void *args) {
+    pushPackageCB = cb;
+    pushPackageCBArgs = args;
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::registerPushCIInstructionCB(callback_pushCIInstruction cb, void *args) {
+    pushCIInstructionCB = cb;
+    pushCIInstructionCBArgs = args;
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::setAddrMap(uint32_t addr, int intNum) {
+    intAddressMap[intNum] = addr;
+}
+
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_int.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_int.cpp
@@ -0,0 +1,49 @@
+#include "transmission_interface.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+uint32_t GobotRPC_TI_Hardware_RP2040_I2C::readIntPins() {
+    uint32_t intPinsStates = 0;
+
+    for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
+        intPinsStates |= gpio_get(GOBOTRPC_TI_INT_PIN_START + i) << i;
+    }
+    return intPinsStates;
+}
+
+void intPin_ISR(uint gpio, uint32_t events) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = g_GobotRPC_TI_Hardware_RP2040_I2C;
+
+    uint32_t intPinsStates = hw->readIntPins();
+
+    if(intPinsStates == 0) {
+        return;
+    }
+
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    hw->intPinISR(&xHigherPriorityTaskWoken);
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::intPinISR(BaseType_t * xHigherPriorityTaskWoken) {
+    xSemaphoreGiveFromISR(i2cRXSemaphore, xHigherPriorityTaskWoken);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::manualTriggerIntTask() {
+    while (1) {
+        uint32_t intPinsStates = readIntPins();
+
+        if(intPinsStates == 0) {
+            vTaskDelay(500 / portTICK_PERIOD_MS);
+            continue;
+        }
+
+        vTaskDelay(500 / portTICK_PERIOD_MS);
+
+        xSemaphoreGive(i2cRXSemaphore);
+    
+    }
+    
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_rx.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_rx.cpp
@@ -0,0 +1,69 @@
+#include "transmission_interface.hpp"
+#include "ci/base.hpp"
+
+void i2cRxTaskFn(void * args) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)args;
+    hw->i2cRxTask();
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::i2cRxTask() {
+    while (1) {
+        xSemaphoreTake(i2cRXSemaphore, portMAX_DELAY);
+
+        uint32_t done_mask = 0xffffffff;
+
+        uint32_t pinStates = readIntPins();
+
+        do {
+            for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
+                if((pinStates & done_mask) & (1 << i)) {
+                    done_mask &= ~(1 << i);
+            
+
+                    uint32_t addr = intAddressMap[i];
+
+                    if(addr > 0xff)
+                        continue;
+
+                    GoRPCPackage_Transport pkg;
+
+                    xSemaphoreTake(i2cMutex, portMAX_DELAY);
+                    bool read_res = readI2C(&pkg, addr);
+                    xSemaphoreGive(i2cMutex);
+
+                    if(!read_res)
+                        continue;
+
+                    pkg.addr |= (i) << 8;
+
+                    if(pushPackageCB != NULL)
+                        pushPackageCB(&pkg, pushPackageCBArgs);
+            
+                    break;
+                }
+            }
+            
+            pinStates = readIntPins();
+        } while(pinStates & done_mask);
+    }
+}
+
+bool GobotRPC_TI_Hardware_RP2040_I2C::readI2C(GoRPCPackage_Transport * pkg, uint32_t addr) {
+    unsigned int res;
+    //uint res = i2c_read_blocking(i2c, addr, (uint8_t *)pkg->data, 2, true);
+
+    i2c_read_blocking(i2c, addr, (uint8_t *)pkg->data + CI_RX_PACKAGE_DATA_OFFSET, 2, true);
+
+
+    size_t len = pkg->data[CI_RX_PACKAGE_DATA_OFFSET + 1];
+
+    if(len > 64) {
+        return false;
+    }
+
+    res = i2c_read_blocking(i2c, addr, ((uint8_t *)pkg->data) + CI_RX_PACKAGE_DATA_OFFSET, len, false);
+    pkg->len = len;
+    pkg->addr = addr;
+
+    return true;
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_tx.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_tx.cpp
@@ -0,0 +1,29 @@
+#include "transmission_interface.hpp"
+
+void i2cTxTaskFn(void * args) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)args;
+    hw->i2cTxTask();
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::i2cTxTask() {
+    while (1) {
+        GoRPCPackage_Transport pkg;
+
+        if(pullPackageCB == NULL) {
+            vTaskDelay(pdMS_TO_TICKS(100));
+            continue;
+        }
+        
+        pullPackageCB(&pkg, pullPackageCBArgs);
+
+        xSemaphoreTake(i2cMutex, portMAX_DELAY);
+        unsigned int res = i2c_write_blocking(i2c, pkg.addr & 0x7f, (uint8_t * )(pkg.data), pkg.len, false);        
+        xSemaphoreGive(i2cMutex);
+
+        if(res == PICO_ERROR_GENERIC) {
+            raiseTranmissionError(false, pkg.addr);
+        } else {
+            raiseTransmissionSuceess(pkg.addr);
+        }
+    }
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/main_core0.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/main_core0.cpp
@@ -0,0 +1,86 @@
+#include <stdio.h>
+#include <string.h>
+#include "pico/stdlib.h"
+
+#include "ci/base.hpp"
+#include "ci/hardware.hpp"
+#include "transmission_interface.hpp"
+
+#include "main.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "message_buffer.h"
+
+#include "protocol.hpp"
+
+#define LED_PIN 25
+
+AppData appData;
+
+void vTaskMain(void * pvParameters) {
+    while(1) {
+        gpio_put(LED_PIN, 1);
+        vTaskDelay(pdMS_TO_TICKS(500));
+        gpio_put(LED_PIN, 0);
+        vTaskDelay(pdMS_TO_TICKS(500));
+    }
+}
+
+void onTxPacket(void * args, char *data, size_t len, uint32_t addr) {
+    GobotRPC_CI * gobotRPC_ci = (GobotRPC_CI *)args;
+
+    GoRPCPackage_Transport pkg;
+
+    memcpy(pkg.data, data, len);
+    pkg.len = len;
+    pkg.addr = addr;
+    
+    xQueueSend(appData.txQueue, &pkg, portMAX_DELAY);
+
+    //gobotRPC_ci->send_RxPacket(pkg.data, CALC_SIZE_GobotRPC_PACKAGE(0), addr);
+}
+
+void onRXFromI2CTask(void * pvParameters) {
+    GobotRPC_CI * gobotRPC_ci = (GobotRPC_CI *)pvParameters;
+
+    GoRPCPackage_Transport pkg;
+    while (true) {
+        xQueueReceive(appData.rxQueue, &pkg, portMAX_DELAY);
+
+        gobotRPC_ci->send_RxPacket(pkg.data, pkg.len, pkg.addr);
+    }
+}
+
+static void onSetAddrMap(void * args, uint32_t addr, uint8_t port) {
+    GobotRPC_CI * gobotrpc_ci = (GobotRPC_CI *)args;
+    gobotrpc_ci->send_rev_SetAddrMap(addr, port);
+}
+
+int main() {
+    gpio_init(LED_PIN);
+    gpio_set_dir(LED_PIN, true);
+
+    appData.txQueue = xQueueCreate(5, sizeof(GoRPCPackage_Transport));
+    appData.rxQueue = xQueueCreate(5, sizeof(GoRPCPackage_Transport));
+    appData.ciInstructionQueue = xQueueCreate(3, sizeof(CI_Instruction_Transport));
+    appData.ciInstructionReverseQueue = xQueueCreate(3, sizeof(CI_Instruction_Transport));
+
+    GobotRPC_CI_Hardware_RP2040_UART gobotrpc_ci_hardware(uart0, 115200, UART_CORE_MASK);
+
+    GobotRPC_CI gobotRPC_ci(&gobotrpc_ci_hardware, UART_CORE_MASK, appData.ciInstructionQueue, appData.ciInstructionReverseQueue);
+    gobotRPC_ci.registerCB_SetAddress(onSetAddrMap, &gobotRPC_ci);
+    gobotRPC_ci.registerCB_TxPacket(onTxPacket, &gobotRPC_ci);
+
+    TaskHandle_t taskHandleCore0;
+    TaskHandle_t taskHandleCore1;
+    TaskHandle_t taskRXFromI2CTask;
+    xTaskCreateAffinitySet(vTaskMain, "Main Task Core 0", 2048, &gobotRPC_ci, 1, UART_CORE_MASK, &taskHandleCore0);
+    xTaskCreateAffinitySet(main_core2, "Main Task Core 1", 2048, &appData, 1, UART_CORE_MASK, &taskHandleCore1);
+    xTaskCreateAffinitySet(onRXFromI2CTask, "RX From I2C Task", 2048, &gobotRPC_ci, 3, UART_CORE_MASK, &taskRXFromI2CTask);
+
+    vTaskStartScheduler();
+
+    while(1) {}
+}
--- a/i2c-hub/firmware/i2c-hub-firmware/src/main_core1.cpp
+++ b/i2c-hub/firmware/i2c-hub-firmware/src/main_core1.cpp
@@ -0,0 +1,43 @@
+#include "main.hpp"
+#include "transmission_interface.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+#include "pinConfig.hpp"
+
+extern AppData appData;
+
+void pullPackageCB(GoRPCPackage_Transport * dest, void *args) {
+    QueueHandle_t queue = appData.txQueue;
+    xQueueReceive(queue, dest, portMAX_DELAY);
+}
+
+void pushPackageCB(GoRPCPackage_Transport * src, void *args) {
+    QueueHandle_t queue = appData.rxQueue;
+    xQueueSend(queue, src, portMAX_DELAY);
+}
+
+void pushCIInstruction(CI_Instruction_Transport * src, void *args) {
+    QueueHandle_t queue = appData.ciInstructionQueue;
+    xQueueSend(queue, src, portMAX_DELAY);
+}
+
+void main_core2(void * pvParameters) {
+    AppData appData1 = *(AppData *)pvParameters;
+
+    GobotRPC_TI_Hardware_RP2040_I2C gobotrpc_ti_hardware(UART_CORE_MASK, i2c0, appData1.ciInstructionReverseQueue);
+    //gobotrpc_ti_hardware.setAddrMap(0x21, 0);
+
+    gobotrpc_ti_hardware.registerPullPackageCB(pullPackageCB, appData.txQueue);
+    gobotrpc_ti_hardware.registerPushPackageCB(pushPackageCB, appData.rxQueue);
+    gobotrpc_ti_hardware.registerPushCIInstructionCB(pushCIInstruction, appData.ciInstructionQueue);
+
+    gobotrpc_ti_hardware.raiseInfoReset();
+
+    while(1) {
+        vTaskDelay(pdMS_TO_TICKS(10000));
+    }
+}