v1.0.0a4

2025-06-26 13:34:31 -06:00 · 2025-06-18 08:58:24 -06:00 · 2025-06-16 08:13:55 -06:00 · 2025-06-08 14:15:28 -06:00 · 2025-06-06 13:35:51 -06:00 · 2025-06-06 13:33:50 -06:00
1471 changed files with 119418 additions and 20165 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -1,13 +1,11 @@
-Firmware/.vscode
-Firmware/RP2040/build
-Firmware/RP2040/.ignore
-Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid_xremote_rom.h
-Firmware/RP2040/src/BLEServer/att_delayed_response.h
-Firmware/ESP32/main/BLEServer/att_delayed_response.h
-Firmware/ESP32/.ignore
-Firmware/ESP32/build
-Firmware/ESP32/components/btstack
-Firmware/ESP32/sdkconfig.old
-Firmware/external/pico-sdk
-Firmware/external/picotool
-Tools/dvd-dongle-rom.bin
+/Firmware/pico/build/
+/Firmware/pico/debug/
+/Firmware/pico/.ignore/
+/Firmware/pico/src/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid_xremote_rom.h
+/Firmware/pico/src/BLEServer/att_delayed_response.h
+/Firmware/ESP32/main/BLEServer/att_delayed_response.h
+/Firmware/ESP32/.ignore/
+/Firmware/ESP32/build/
+/Firmware/ESP32/components/btstack/
+/Firmware/ESP32/sdkconfig.old
+/scripts/xremote/dvd-dongle-rom.bin
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,21 +1,42 @@
-[submodule "Firmware/external/bluepad32"]
-	path = Firmware/external/bluepad32
-	url = https://github.com/ricardoquesada/bluepad32.git
-[submodule "Firmware/external/tinyusb"]
-	path = Firmware/external/tinyusb
-	url = https://github.com/hathach/tinyusb.git
-[submodule "Tools/dump-dvd-kit"]
-	path = Tools/dump-dvd-kit
-	url = https://github.com/XboxDev/dump-dvd-kit.git
-[submodule "Firmware/external/Pico-PIO-USB"]
-	path = Firmware/external/Pico-PIO-USB
-	url = https://github.com/wiredopposite/Pico-PIO-USB.git
 [submodule "WebApp"]
 	path = WebApp
 	url = https://github.com/wiredopposite/OGX-Mini-WebApp.git
-[submodule "Firmware/external/libfixmath"]
-	path = Firmware/external/libfixmath
-	url = https://github.com/PetteriAimonen/libfixmath.git
-[submodule "Firmware/ESP32_Blueretro"]
-	path = Firmware/ESP32_Blueretro
+[submodule "Firmware/BlueRetro"]
+	path = Firmware/BlueRetro
 	url = https://github.com/wiredopposite/BlueRetro.git
+[submodule "scripts/xremote/dump-dvd-kit"]
+	path = scripts/xremote/dump-dvd-kit
+	url = https://github.com/XboxDev/dump-dvd-kit.git
+[submodule "Firmware/pico/external/pico_usb"]
+	path = Firmware/pico/external/pico_usb
+	url = https://github.com/wiredopposite/pico_usb.git
+[submodule "Firmware/pico/external/g726/g726"]
+	path = Firmware/pico/external/g726/g726
+	url = https://github.com/fredrikhederstierna/g726.git
+[submodule "Firmware/pico/external/libxsm3/libxsm3"]
+	path = Firmware/pico/external/libxsm3/libxsm3
+	url = https://github.com/InvoxiPlayGames/libxsm3.git
+[submodule "Firmware/pico/external/libfixmath"]
+	path = Firmware/pico/external/libfixmath
+	url = https://github.com/PetteriAimonen/libfixmath.git
+[submodule "Firmware/pico/external/tinyusb"]
+	path = Firmware/pico/external/tinyusb
+	url = https://github.com/hathach/tinyusb.git
+[submodule "Firmware/pico/external/Pico-PIO-USB"]
+	path = Firmware/pico/external/Pico-PIO-USB
+	url = https://github.com/sekigon-gonnoc/Pico-PIO-USB.git
+[submodule "Firmware/pico/external/bluepad32/bluepad32"]
+	path = Firmware/pico/external/bluepad32/bluepad32
+	url = https://github.com/ricardoquesada/bluepad32.git
+[submodule "Firmware/pico/external/bluepad32"]
+	path = Firmware/pico/external/bluepad32
+	url = https://github.com/ricardoquesada/bluepad32.git
+[submodule "Firmware/ESP32/components/bluepad32"]
+	path = Firmware/ESP32/components/bluepad32
+	url = https://github.com/ricardoquesada/bluepad32.git
+[submodule "Firmware/ESP32/components/libfixmath/libfixmath"]
+	path = Firmware/ESP32/components/libfixmath/libfixmath
+	url = https://github.com/PetteriAimonen/libfixmath.git
+[submodule "Firmware/pico/external/lwip"]
+	path = Firmware/pico/external/lwip
+	url = https://github.com/lwip-tcpip/lwip.git
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -1,20 +0,0 @@
-{
-    "configurations": [
-        {
-            "name": "Pico",
-            "includePath": [
-                "${workspaceFolder}/Firmware/external/pico-sdk/**"
-            ],
-            "forcedInclude": [
-                "${workspaceFolder}/Firmware/external/pico-sdk/src/common/pico_base_headers/include/pico.h",
-                "${workspaceFolder}/Firmware/RP2040/build/generated/pico_base/pico/config_autogen.h"
-            ],
-            "defines": [],
-            "compileCommands": "${workspaceFolder}/Firmware/RP2040/build/compile_commands.json",
-            "cStandard": "c17",
-            "cppStandard": "c++14",
-            "intelliSenseMode": "linux-gcc-arm"
-        }
-    ],
-    "version": 4
-}
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,64 +0,0 @@
-{
-    "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
-    "cmake.sourceDirectory": "${workspaceFolder}/Firmware/RP2040",
-    "cmake.buildDirectory": "${workspaceFolder}/Firmware/RP2040/build",
-    "cmake.configureArgs": [
-        "-DOGXM_BOARD=EXTERNAL_4CH_I2C",
-        "-DMAX_GAMEPADS=4"
-    ],
-    "files.associations": {
-        "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",
-        "cstring": "cpp",
-        "ctime": "cpp",
-        "cwchar": "cpp",
-        "cwctype": "cpp",
-        "deque": "cpp",
-        "list": "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",
-        "iostream": "cpp",
-        "istream": "cpp",
-        "limits": "cpp",
-        "new": "cpp",
-        "numbers": "cpp",
-        "ostream": "cpp",
-        "sstream": "cpp",
-        "stdexcept": "cpp",
-        "streambuf": "cpp",
-        "cinttypes": "cpp",
-        "typeinfo": "cpp",
-        "xmemory": "cpp",
-        "xtr1common": "cpp",
-        "xutility": "cpp"
-    },
-}
--- a/Firmware/ESP32_Blueretro
+++ b/Firmware/ESP32_Blueretro
--- a/Firmware/ESP32/.vscode/c_cpp_properties.json
+++ b/Firmware/ESP32/.vscode/c_cpp_properties.json
@@ -1,23 +1,25 @@
 {
    "configurations": [
-      {
-        "name": "ESP-IDF",
-        "compilerPath": "${default}",
-        "compileCommands": "${config:idf.buildPath}/compile_commands.json",
-        "includePath": [
-          "${config:idf.espIdfPath}/components/**",
-          "${config:idf.espIdfPathWin}/components/**",
-          "${workspaceFolder}/main/**"
-        ],
-        "browse": {
-          "path": [
-            "${config:idf.espIdfPath}/components",
-            "${config:idf.espIdfPathWin}/components",
-            "${workspaceFolder}/main"
-          ],
-          "limitSymbolsToIncludedHeaders": true
+        {
+            "name": "ESP-IDF",
+            "compilerPath": "${default}",
+            "compileCommands": "${config:idf.buildPath}/compile_commands.json",
+            "includePath": [
+            "${config:idf.espIdfPath}/components/**",
+            "${config:idf.espIdfPathWin}/components/**",
+            "${workspaceFolder}/main/**",
+            "${workspaceFolder}/components/**"
+            ],
+            "browse": {
+            "path": [
+                "${config:idf.espIdfPath}/components",
+                "${config:idf.espIdfPathWin}/components",
+                "${workspaceFolder}/main",
+                "${workspaceFolder}/components"
+            ],
+            "limitSymbolsToIncludedHeaders": true
+            }
        }
-      }
    ],
    "version": 4
-  }
+}
--- a/Firmware/ESP32/.vscode/settings.json
+++ b/Firmware/ESP32/.vscode/settings.json
@@ -1,106 +1,44 @@
 {
    "idf.adapterTargetName": "esp32",
    "cmake.configureOnOpen": false,
-    "files.associations": {
-        "freertos.h": "c",
-        "stdint.h": "c",
-        "stdlib.h": "c",
-        "atomic": "cpp",
-        "array": "cpp",
-        "task.h": "c",
-        "bit": "cpp",
-        "*.tcc": "cpp",
-        "bitset": "cpp",
-        "cctype": "cpp",
-        "chrono": "cpp",
-        "clocale": "cpp",
-        "cmath": "cpp",
-        "compare": "cpp",
-        "complex": "cpp",
-        "concepts": "cpp",
-        "condition_variable": "cpp",
-        "cstdarg": "cpp",
-        "cstddef": "cpp",
-        "cstdint": "cpp",
-        "cstdio": "cpp",
-        "cstdlib": "cpp",
-        "cstring": "cpp",
-        "ctime": "cpp",
-        "cwchar": "cpp",
-        "cwctype": "cpp",
-        "deque": "cpp",
-        "list": "cpp",
-        "map": "cpp",
-        "set": "cpp",
-        "string": "cpp",
-        "unordered_map": "cpp",
-        "vector": "cpp",
-        "exception": "cpp",
-        "algorithm": "cpp",
-        "functional": "cpp",
-        "iterator": "cpp",
-        "memory": "cpp",
-        "memory_resource": "cpp",
-        "netfwd": "cpp",
-        "numeric": "cpp",
-        "random": "cpp",
-        "ratio": "cpp",
-        "regex": "cpp",
-        "string_view": "cpp",
-        "system_error": "cpp",
-        "tuple": "cpp",
-        "type_traits": "cpp",
-        "utility": "cpp",
-        "fstream": "cpp",
-        "future": "cpp",
-        "initializer_list": "cpp",
-        "iosfwd": "cpp",
-        "iostream": "cpp",
-        "istream": "cpp",
-        "limits": "cpp",
-        "mutex": "cpp",
-        "new": "cpp",
-        "numbers": "cpp",
-        "ostream": "cpp",
-        "semaphore": "cpp",
-        "sstream": "cpp",
-        "stdexcept": "cpp",
-        "stop_token": "cpp",
-        "streambuf": "cpp",
-        "thread": "cpp",
-        "cinttypes": "cpp",
-        "typeinfo": "cpp",
-        "c_wrapper.h": "c",
-        "queue": "cpp",
-        "charconv": "cpp",
-        "format": "cpp",
-        "forward_list": "cpp",
-        "iomanip": "cpp",
-        "ios": "cpp",
-        "locale": "cpp",
-        "optional": "cpp",
-        "ranges": "cpp",
-        "span": "cpp",
-        "stack": "cpp",
-        "xfacet": "cpp",
-        "xhash": "cpp",
-        "xiosbase": "cpp",
-        "xlocale": "cpp",
-        "xlocbuf": "cpp",
-        "xlocinfo": "cpp",
-        "xlocmes": "cpp",
-        "xlocmon": "cpp",
-        "xlocnum": "cpp",
-        "xloctime": "cpp",
-        "xmemory": "cpp",
-        "xstring": "cpp",
-        "xtr1common": "cpp",
-        "xtree": "cpp",
-        "xutility": "cpp"
-    },
    "idf.portWin": "COM26",
    "idf.flashType": "UART",
    "C_Cpp.intelliSenseEngine": "default",
-    "idf.espIdfPathWin": "C:\\Programming\\esp\\v5.1\\esp-idf",
-    "idf.toolsPathWin": "C:\\Programming\\.espressif"
+    "idf.pythonInstallPath": "/usr/bin/python3",
+    "files.associations": {
+        "wired.h": "c",
+        "stdarg.h": "c",
+        "semphr.h": "c",
+        "spi_slave.h": "c",
+        "regex": "c",
+        "chrono": "c",
+        "variant": "c",
+        "atomic": "cpp",
+        "spi_hal.h": "c",
+        "spi_ll.h": "c",
+        "spi_struct.h": "c",
+        "wired_private.h": "c",
+        "gpio_ll.h": "c",
+        "gpio_sig_map.h": "c",
+        "gpio_hal.h": "c",
+        "spi_master.h": "c",
+        "spi_common.h": "c",
+        "clk_tree_defs.h": "c",
+        "io_mux_reg.h": "c",
+        "pcnt_ll.h": "c",
+        "gpio.h": "c",
+        "freertos.h": "c",
+        "bitset": "c",
+        "string_view": "c",
+        "i2c_ll.h": "c",
+        "i2c_hal.h": "c",
+        "stdbool.h": "c",
+        "i2c_master.h": "c",
+        "cstdint": "cpp",
+        "array": "cpp",
+        "string": "cpp",
+        "span": "cpp"
+    },
+    "idf.port": "/dev/ttyUSB0",
+    "idf.flashBaudRate": "57600"
 }
--- a/Firmware/ESP32/CMakeLists.txt
+++ b/Firmware/ESP32/CMakeLists.txt
@@ -3,38 +3,50 @@ cmake_minimum_required(VERSION 3.5)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_CXX_STANDARD 17)

-set(EXTERNAL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../external)
-set(EXTERNAL_CMAKE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../cmake)
-set(BTSTACK_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/components/btstack)
+include(${CMAKE_CURRENT_SOURCE_DIR}/FWDefines.cmake)
+set(COMPONENTS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/components)
+set(BLUEPAD32_ROOT ${COMPONENTS_PATH}/bluepad32/src/components/bluepad32)
+set(BTSTACK_REPO_PATH ${COMPONENTS_PATH}/bluepad32/external/btstack)
+set(BTSTACK_SCRIPT_PATH ${COMPONENTS_PATH}/integrate_btstack.py)

-include(${CMAKE_CURRENT_SOURCE_DIR}/../FWDefines.cmake)
-include(${EXTERNAL_CMAKE_DIR}/init_submodules.cmake)
-include(${EXTERNAL_CMAKE_DIR}/patch_libs.cmake)
-include(${EXTERNAL_CMAKE_DIR}/generate_gatt_header.cmake)
-include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/integrate_btstack.cmake)
-
-init_git_submodules(${EXTERNAL_DIR}
-    ${EXTERNAL_DIR}/bluepad32
-    ${EXTERNAL_DIR}/libfixmath
+execute_process(
+    COMMAND git apply --ignore-whitespace ${COMPONENTS_PATH}/btstack_l2cap.diff
+    WORKING_DIRECTORY ${BTSTACK_REPO_PATH}
+    # RESULT_VARIABLE result
+    # ERROR_VARIABLE error
 )
-apply_lib_patches(${EXTERNAL_DIR})
-integrate_btstack(${EXTERNAL_DIR})
-generate_gatt_header(
-    ${BTSTACK_ROOT} 
-    ${CMAKE_CURRENT_SOURCE_DIR}/../RP2040/src/BLEServer/att_delayed_response.gatt
-    ${CMAKE_CURRENT_SOURCE_DIR}/main/BLEServer/att_delayed_response.h
+
+if(NOT EXISTS ${COMPONENTS_PATH}/btstack)
+    find_package(Python3 COMPONENTS Interpreter REQUIRED)
+
+    if(Python3_Interpreter_FOUND)
+        message(STATUS "Python3 found: ${Python3_EXECUTABLE}")
+        message(STATUS "Python3 version: ${Python3_VERSION}")
+    else()
+        message(FATAL_ERROR "Python3 not found! Python is required to integrate BTStack with ESP-IDF")
+    endif()
+
+    message(STATUS "Integrating BTStack with project")
+    execute_process(
+        COMMAND ${Python3_EXECUTABLE} ${BTSTACK_SCRIPT_PATH} ${BTSTACK_REPO_PATH} ${COMPONENTS_PATH}
+        WORKING_DIRECTORY ${COMPONENTS_PATH}
+        RESULT_VARIABLE PYTHON_RESULT
+        OUTPUT_VARIABLE PYTHON_OUTPUT
+        ERROR_VARIABLE PYTHON_ERROR
    )

-set(BLUEPAD32_ROOT ${EXTERNAL_DIR}/bluepad32/src/components/bluepad32)
-
-if(NOT EXISTS ${BLUEPAD32_ROOT})
-    message(FATAL_ERROR "External directory not found: ${BLUEPAD32_ROOT}")
+    if(PYTHON_RESULT EQUAL 0)
+        message(STATUS "BTStack integration successful:\n${PYTHON_OUTPUT}")
+    else()
+        message(FATAL_ERROR "BTStack integration failed:\n${PYTHON_ERROR}")
+    endif()
 endif()

 set(EXTRA_COMPONENT_DIRS 
    ${BLUEPAD32_ROOT}/..
-    ${CMAKE_CURRENT_SOURCE_DIR}/components
+    ${COMPONENTS_PATH}
 )

 include($ENV{IDF_PATH}/tools/cmake/project.cmake)
-project("${FW_NAME}-${FW_VERSION}-ESP32")
+project("${FW_NAME}-${FW_VERSION}-ESP32")
+
--- a/Firmware/ESP32/FWDefines.cmake
+++ b/Firmware/ESP32/FWDefines.cmake
@@ -0,0 +1,2 @@
+set(FW_NAME "OGX-Mini")
+set(FW_VERSION "1.0.0a4")
--- a/Firmware/ESP32/cmake/integrate_btstack.cmake
+++ b/Firmware/ESP32/cmake/integrate_btstack.cmake
@@ -1,50 +0,0 @@
-function(integrate_btstack EXTERNAL_DIR)
-    if(NOT EXISTS ${EXTERNAL_DIR})
-        message(FATAL_ERROR "External directory not found: ${EXTERNAL_DIR}")
-    else()
-        message(STATUS "External directory found: ${EXTERNAL_DIR}")
-    endif()
-
-    set(PROJECT_COMPONENTS ${CMAKE_CURRENT_LIST_DIR}/components)
-    set(BTSTACK_DIR ${EXTERNAL_DIR}/bluepad32/external/btstack)
-    set(PROJECT_BTSTACK ${PROJECT_COMPONENTS}/btstack)
-    
-    if (NOT EXISTS ${PROJECT_COMPONENTS})
-        message(STATUS "Creating components folder at ${PROJECT_COMPONENTS}")
-        file(MAKE_DIRECTORY ${PROJECT_COMPONENTS})
-    endif()
-    
-    if (NOT EXISTS ${BTSTACK_DIR})
-        message(FATAL_ERROR "Error: No BTstack folder at ${BTSTACK_DIR}")
-    endif()
-    
-    if (NOT EXISTS ${PROJECT_BTSTACK})
-        find_package(Python3 COMPONENTS Interpreter REQUIRED)
-
-        if(Python3_Interpreter_FOUND)
-            message(STATUS "Python3 found: ${Python3_EXECUTABLE}")
-            message(STATUS "Python3 version: ${Python3_VERSION}")
-        else()
-            message(FATAL_ERROR "Python3 not found! Python is required to integrate BTStack with ESP-IDF")
-        endif()
-
-        set(BTSTACK_SCRIPT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/components/integrate_btstack.py)
-
-        message(STATUS "Integrating BTStack with project")
-        execute_process(
-            COMMAND ${Python3_EXECUTABLE} ${BTSTACK_SCRIPT_PATH} ${BTSTACK_DIR} ${PROJECT_COMPONENTS}
-            WORKING_DIRECTORY ${BTSTACK_SCRIPT_PATH}/..
-            RESULT_VARIABLE PYTHON_RESULT
-            OUTPUT_VARIABLE PYTHON_OUTPUT
-            ERROR_VARIABLE PYTHON_ERROR
-        )
-
-        if(PYTHON_RESULT EQUAL 0)
-            message(STATUS "BTStack integration successful:\n${PYTHON_OUTPUT}")
-        else()
-            message(FATAL_ERROR "BTStack integration failed:\n${PYTHON_ERROR}")
-        endif()
-    endif()
-    
-    message(STATUS "BTstack setup complete")
-endfunction()
--- a/Firmware/ESP32/components/bluepad32
+++ b/Firmware/ESP32/components/bluepad32
--- a/Firmware/ESP32/components/btstack_l2cap.diff
+++ b/Firmware/ESP32/components/btstack_l2cap.diff
--- a/Firmware/ESP32/components/integrate_btstack.py
+++ b/Firmware/ESP32/components/integrate_btstack.py
@@ -1,4 +1,4 @@
-# This script copies BTstack ESP32 port files to this project's components folder, ESP-IDF freaks out otherwise
+# This script copies BTstack ESP32 port files to this project's components folder

 import os
 import sys
--- a/Firmware/ESP32/components/libfixmath/CMakeLists.txt
+++ b/Firmware/ESP32/components/libfixmath/CMakeLists.txt
@@ -1,11 +1,9 @@
 cmake_minimum_required(VERSION 3.5)

-set(LIBFIXMATH_ROOT ${CMAKE_CURRENT_LIST_DIR}/../../../external/libfixmath) 
+set(LIBFIXMATH_ROOT ${CMAKE_CURRENT_LIST_DIR}/libfixmath) 

-if (NOT EXISTS ${LIBFIXMATH_ROOT})
-    message(FATAL_ERROR "External directory not found: ${LIBFIXMATH_ROOT}")
-else()
-    message(STATUS "Found libfixmath at ${LIBFIXMATH_ROOT}")
+if(NOT EXISTS ${LIBFIXMATH_ROOT}/libfixmath)
+    message(FATAL_ERROR "libfixmath directory not found at ${LIBFIXMATH_ROOT}. Please ensure the library is present.")
 endif()

 file(GLOB SRCS ${LIBFIXMATH_ROOT}/libfixmath/*.c)
--- a/Firmware/ESP32/components/libfixmath/libfixmath
+++ b/Firmware/ESP32/components/libfixmath/libfixmath
--- a/Firmware/ESP32/dependencies.lock
+++ b/Firmware/ESP32/dependencies.lock
@@ -2,7 +2,7 @@ dependencies:
  idf:
    source:
      type: idf
-    version: 5.1.5
+    version: 5.4.1
 direct_dependencies:
 - idf
 manifest_hash: ed38c883314ee552977d073f5c9d0e2ab9acc3248eefbeb203f3439de3e31b6e
--- a/Firmware/ESP32/esp-idf-v5.1.4
+++ b/Firmware/ESP32/esp-idf-v5.1.4
--- a/Firmware/ESP32/main/BLEServer/BLEServer.cpp
+++ b/Firmware/ESP32/main/BLEServer/BLEServer.cpp
@@ -1,334 +0,0 @@
-#include <cstring>
-#include <string>
-#include <esp_ota_ops.h>
-#include <esp_system.h>
-
-#include "att_server.h"
-#include "btstack.h"
-
-#include "BTManager/BTManager.h" 
-#include "Gamepad/Gamepad.h"
-#include "BLEServer/BLEServer.h"
-#include "BLEServer/att_delayed_response.h"
-#include "UserSettings/UserProfile.h"
-#include "UserSettings/UserSettings.h"
-
-namespace BLEServer {
-
-constexpr uint16_t PACKET_LEN_MAX = 20;
-constexpr size_t GAMEPAD_LEN = 23;
-
-namespace Handle
-{
-    constexpr uint16_t FW_VERSION    = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789020_01_VALUE_HANDLE;
-    constexpr uint16_t FW_NAME       = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789021_01_VALUE_HANDLE;
-
-    constexpr uint16_t SETUP_READ  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
-    constexpr uint16_t SETUP_WRITE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
-    constexpr uint16_t GET_SETUP   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789032_01_VALUE_HANDLE;
-
-    constexpr uint16_t PROFILE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
-
-    constexpr uint16_t GAMEPAD  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789050_01_VALUE_HANDLE;
-}
-
-namespace ADV
-{
-    // Flags general discoverable, BR/EDR not supported
-    static const uint8_t FLAGS[]    = { 0x02, 0x01, 0x06 };
-    static const uint8_t NAME_TYPE  = 0x09;
-
-    #pragma pack(push, 1)
-    struct Data
-    {
-        uint8_t flags[sizeof(FLAGS)];
-        uint8_t name_len;
-        uint8_t name_type;
-        uint8_t name[sizeof(FIRMWARE_NAME) - 1];
-
-        Data()
-        {
-            std::memcpy(flags, FLAGS, sizeof(flags));
-            name_len = sizeof(FIRMWARE_NAME);
-            name_type = NAME_TYPE;
-            std::string fw_name = FIRMWARE_NAME;
-            std::memcpy(name, fw_name.c_str(), std::min(sizeof(name), fw_name.size()));
-        }
-    };
-    static_assert(sizeof(Data) == 5 + sizeof(FIRMWARE_NAME) - 1, "BLEServer::ADV::Data struct size mismatch");
-    #pragma pack(pop)
-}
-
-#pragma pack(push, 1)
-struct SetupPacket
-{
-    DeviceDriverType device_type{DeviceDriverType::NONE};
-    uint8_t max_gamepads{MAX_GAMEPADS};
-    uint8_t player_idx{0};
-    uint8_t profile_id{0};
-};
-static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
-#pragma pack(pop)
-
-class ProfileReader
-{
-public:
-    ProfileReader() = default;
-    ~ProfileReader() = default;
-
-    void set_setup_packet(const SetupPacket& setup_packet)
-    {
-        setup_packet_ = setup_packet;
-        current_offset_ = 0;
-    }
-    const SetupPacket& get_setup_packet() const
-    {
-        return setup_packet_;
-    }
-    uint16_t get_xfer_len()
-    {
-        return static_cast<uint16_t>(std::min(static_cast<size_t>(PACKET_LEN_MAX), sizeof(UserProfile) - current_offset_));
-    }
-    uint16_t get_profile_data(uint8_t* buffer, uint16_t buffer_len)
-    {
-        size_t copy_len = get_xfer_len();
-        if (!buffer || buffer_len < copy_len)
-        {
-            return 0;
-        }
-
-        if (current_offset_ == 0 && !set_profile())
-        {
-            return 0;
-        }
-
-        std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile_) + current_offset_, copy_len);
-
-        current_offset_ += copy_len;
-        if (current_offset_ >= sizeof(UserProfile))
-        {
-            current_offset_ = 0;
-            OGXM_LOG("ProfileReader: Read complete for profile ID: %i\n", profile_.id);
-        }
-        return copy_len;
-    }
-
-private:
-    SetupPacket setup_packet_;
-    UserProfile profile_;
-    size_t current_offset_ = 0;
-
-    bool set_profile()
-    {
-        if (setup_packet_.profile_id == 0xFF)
-        {
-            if (setup_packet_.player_idx >= UserSettings::MAX_PROFILES)
-            {
-                return false;
-            }
-            profile_ = UserSettings::get_instance().get_profile_by_index(setup_packet_.player_idx);
-            OGXM_LOG("ProfileReader: Reading profile for player %d\n", setup_packet_.player_idx);
-        }
-        else
-        {
-            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
-            {
-                return false;
-            }
-            profile_ = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
-            OGXM_LOG("ProfileReader: Reading profile with ID %d\n", setup_packet_.profile_id);
-        }
-        return true;
-    }
-};
-
-class ProfileWriter
-{
-public:
-    ProfileWriter() = default;
-    ~ProfileWriter() = default;
-
-    void set_setup_packet(const SetupPacket& setup_packet)
-    {
-        setup_packet_ = setup_packet;
-        current_offset_ = 0;
-    }
-    const SetupPacket& get_setup_packet() const
-    {
-        return setup_packet_;
-    }
-    uint16_t get_xfer_len()
-    {
-        return static_cast<uint16_t>(std::min(static_cast<size_t>(PACKET_LEN_MAX), sizeof(UserProfile) - current_offset_));
-    }
-    size_t set_profile_data(const uint8_t* buffer, uint16_t buffer_len)
-    {
-        size_t copy_len = get_xfer_len();
-        if (!buffer || buffer_len < copy_len)
-        {
-            return 0;
-        }
-
-        std::memcpy(reinterpret_cast<uint8_t*>(&profile_) + current_offset_, buffer, copy_len);
-
-        current_offset_ += copy_len;
-        size_t ret = current_offset_;
-
-        if (current_offset_ >= sizeof(UserProfile))
-        {
-            current_offset_ = 0;
-        }
-        return ret;
-    }
-    bool commit_profile()
-    {
-        if (setup_packet_.device_type != DeviceDriverType::NONE)
-        {
-            UserSettings::get_instance().store_profile_and_driver_type(setup_packet_.device_type, setup_packet_.player_idx, profile_);
-        }
-        else
-        {
-            UserSettings::get_instance().store_profile(setup_packet_.player_idx, profile_);
-        }
-        return true;
-    }
-
-private:
-    SetupPacket setup_packet_;
-    UserProfile profile_;
-    size_t current_offset_ = 0;
-};
-
-ProfileReader profile_reader_;
-ProfileWriter profile_writer_;
-
-static int verify_write(const uint16_t buffer_size, const uint16_t expected_size)
-{
-    if (buffer_size != expected_size)
-    {
-        return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
-    }
-    return 0;
-}
-
-static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
-                                    uint16_t att_handle,
-                                    uint16_t offset,
-                                    uint8_t *buffer,
-                                    uint16_t buffer_size)
-{
-    std::string fw_version;
-    std::string fw_name;
-
-    switch (att_handle)
-    {
-        case Handle::FW_VERSION:
-            fw_version = FIRMWARE_VERSION;
-            if (buffer)
-            {
-                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_version.c_str()), fw_version.size());
-            }
-            return static_cast<uint16_t>(fw_version.size());
-
-        case Handle::FW_NAME:
-            fw_name = FIRMWARE_NAME;
-            if (buffer)
-            {
-                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_name.c_str()), fw_name.size());;
-            }
-            return static_cast<uint16_t>(fw_name.size());
-
-        case Handle::GET_SETUP:
-            if (buffer)
-            {
-                buffer[0] = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
-                buffer[1] = MAX_GAMEPADS;
-                buffer[2] = 0;
-                buffer[3] = UserSettings::get_instance().get_active_profile_id(0);
-            }
-            return static_cast<uint16_t>(sizeof(SetupPacket));
-
-        case Handle::PROFILE:
-            if (buffer)
-            {
-                return profile_reader_.get_profile_data(buffer, buffer_size);
-            }
-            return profile_reader_.get_xfer_len();
-
-        case Handle::GAMEPAD:
-            if (buffer)
-            {
-                I2CDriver::PacketIn packet_in = BTManager::get_instance().get_packet_in(0);
-                std::memcpy(buffer, &packet_in.dpad, 13);
-            }
-            return static_cast<uint16_t>(13);
-
-        default:
-            break;
-    }
-    return 0;
-}
-
-static int att_write_callback(  hci_con_handle_t connection_handle,
-                                uint16_t att_handle,
-                                uint16_t transaction_mode,
-                                uint16_t offset,
-                                uint8_t *buffer,
-                                uint16_t buffer_size)
-{
-    int ret = 0;
-
-    switch (att_handle)
-    {
-        case Handle::SETUP_READ:
-            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
-            {
-                break;
-            }
-            profile_reader_.set_setup_packet(*reinterpret_cast<SetupPacket*>(buffer));
-            break;
-
-        case Handle::SETUP_WRITE:
-            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
-            {
-                break;
-            }
-            profile_writer_.set_setup_packet(*reinterpret_cast<SetupPacket*>(buffer));
-            break;
-
-        case Handle::PROFILE:
-            if ((ret = verify_write(buffer_size, profile_writer_.get_xfer_len())) != 0)
-            {
-                break;
-            }
-            if (profile_writer_.set_profile_data(buffer, buffer_size) == sizeof(UserProfile))
-            {
-                profile_writer_.commit_profile();
-            }
-            break;
-
-        default:
-            break;
-    }
-    return ret;
-}
-
-void init_server()
-{
-    att_server_init(profile_data, att_read_callback, att_write_callback);
-
-    uint16_t adv_int_min = 0x0030;
-    uint16_t adv_int_max = 0x0030;
-    uint8_t adv_type = 0;
-
-    bd_addr_t null_addr;
-    std::memset(null_addr, 0, sizeof(null_addr));
-
-    static ADV::Data adv_data = ADV::Data();
-
-    gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, 0, null_addr, 0x07, 0x00);
-    gap_advertisements_set_data(static_cast<uint8_t>(sizeof(adv_data)), reinterpret_cast<uint8_t*>(&adv_data));
-    gap_advertisements_enable(1);
-}
-
-} // namespace BLEServer
--- a/Firmware/ESP32/main/BLEServer/BLEServer.h
+++ b/Firmware/ESP32/main/BLEServer/BLEServer.h
@@ -1,13 +0,0 @@
-#ifndef BLE_SERVER_H
-#define BLE_SERVER_H
-
-#include <cstdint>
-
-#include "Gamepad/Gamepad.h"
-
-namespace BLEServer 
-{
-    void init_server();
-}
-
-#endif // BLE_SERVER_H
--- a/Firmware/ESP32/main/BTManager/BTManager.cpp
+++ b/Firmware/ESP32/main/BTManager/BTManager.cpp
@@ -1,257 +0,0 @@
-#include "btstack_port_esp32.h"
-#include "btstack_run_loop.h"
-#include "btstack_stdio_esp32.h"
-#include "uni.h"
-
-#include "sdkconfig.h"
-#include "Board/ogxm_log.h"
-#include "Board/board_api.h"
-#include "BTManager/BTManager.h"
-#include "BLEServer/BLEServer.h"
-
-void BTManager::run_task()
-{
-    board_api::init_board(); 
-    UserSettings::get_instance().initialize_flash();
-
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        UserProfile profile = UserSettings::get_instance().get_profile_by_index(i);
-        devices_[i].mapper.set_profile(profile);
-    }
-
-    i2c_driver_.initialize_i2c(
-        static_cast<i2c_port_t>(CONFIG_I2C_PORT), 
-        static_cast<gpio_num_t>(CONFIG_I2C_SDA_PIN), 
-        static_cast<gpio_num_t>(CONFIG_I2C_SCL_PIN),  
-        CONFIG_I2C_BAUDRATE
-    );
-
-    xTaskCreatePinnedToCore(
-        [](void* parameter)
-        { 
-            get_instance().i2c_driver_.run_tasks(); 
-        },
-        "i2c",
-        2048 * 2,
-        nullptr,
-        configMAX_PRIORITIES-8,
-        nullptr,
-        1 
-    );
-
-    btstack_init();
-
-    uni_platform_set_custom(get_bp32_driver());
-    uni_init(0, nullptr);
-
-    btstack_timer_source_t led_timer;
-    led_timer.process = check_led_cb;
-    led_timer.context = nullptr;
-    btstack_run_loop_set_timer(&led_timer, LED_TIME_MS);
-    btstack_run_loop_add_timer(&led_timer);
-
-    btstack_timer_source_t driver_update_timer;
-    driver_update_timer.process = driver_update_timer_cb;
-    driver_update_timer.context = nullptr;
-    btstack_run_loop_set_timer(&driver_update_timer, UserSettings::GP_CHECK_DELAY_MS);
-    btstack_run_loop_add_timer(&driver_update_timer);
-
-    BLEServer::init_server();
-
-    //Doesn't return
-    btstack_run_loop_execute();
-}
-
-bool BTManager::is_connected(uint8_t index)
-{
-    if (index >= devices_.size())
-    {
-        return false;
-    }
-    return devices_[index].connected.load();
-}
-
-bool BTManager::any_connected()
-{
-    for (auto& device : devices_)
-    {
-        if (device.connected.load())
-        {
-            return true;
-        }
-    }
-    return false;
-}
-
-void BTManager::check_led_cb(btstack_timer_source *ts)
-{
-    static bool led_state = false;
-    led_state = !led_state;
-
-    if constexpr (board_api::NUM_LEDS == 1)
-    {
-        board_api::set_led(get_instance().any_connected() ? true : (led_state ? true : false));
-    }
-    else
-    {
-        for (uint8_t i = 0; i < board_api::NUM_LEDS; ++i)
-        {
-            board_api::set_led(i, get_instance().is_connected(i) ? true : (led_state ? true : false));
-        }
-    }
-
-    btstack_run_loop_set_timer(ts, LED_TIME_MS);
-    btstack_run_loop_add_timer(ts);
-}
-
-void BTManager::send_driver_type(DeviceDriverType driver_type)
-{
-    if constexpr (I2CDriver::MULTI_SLAVE)
-    {
-        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-        {
-            I2CDriver::PacketIn packet_in = devices_[i].packet_in;
-            packet_in.packet_id = I2CDriver::PacketID::SET_DRIVER;
-            packet_in.index = i;
-            packet_in.device_driver = driver_type;
-            i2c_driver_.write_packet(i + 1, packet_in);
-        }
-    }
-    else
-    {
-        I2CDriver::PacketIn packet_in = devices_[0].packet_in;
-        packet_in.packet_id = I2CDriver::PacketID::SET_DRIVER;
-        packet_in.index = 0;
-        packet_in.device_driver = driver_type;
-        i2c_driver_.write_packet(0x01, packet_in);
-    }
-}
-
-uni_hid_device_t* BTManager::get_connected_bp32_device(uint8_t index)
-{
-    uni_hid_device_t* bp_device = nullptr;
-    if (!(bp_device = uni_hid_device_get_instance_for_idx(index)) || 
-        !uni_bt_conn_is_connected(&bp_device->conn) ||
-        !bp_device->report_parser.play_dual_rumble)
-    {
-        return nullptr;
-    }
-    return bp_device;
-}
-
-void BTManager::driver_update_timer_cb(btstack_timer_source *ts)
-{
-    BTManager& bt_manager = get_instance();
-    I2CDriver::PacketIn& packet_in = bt_manager.devices_.front().packet_in;
-
-    if (get_connected_bp32_device(0) &&
-        UserSettings::get_instance().check_for_driver_change(packet_in))
-    {
-        OGXM_LOG("BP32: Driver change detected\n");
-        UserSettings::get_instance().store_driver_type(UserSettings::get_instance().get_current_driver());
-    }
-
-    //Notify pico of current driver type regardless of change
-    bt_manager.send_driver_type(UserSettings::get_instance().get_current_driver());
-
-    btstack_run_loop_set_timer(ts, UserSettings::GP_CHECK_DELAY_MS);
-    btstack_run_loop_add_timer(ts);
-}
-
-void BTManager::send_feedback_cb(void* context)
-{
-    FBContext* fb_context = reinterpret_cast<FBContext*>(context);
-    uni_hid_device_t* bp_device = nullptr;
-
-    if (!(bp_device = get_connected_bp32_device(fb_context->index)))
-    {
-        return;
-    }
-
-    I2CDriver::PacketOut packet_out = fb_context->packet_out->load();
-
-    if (packet_out.rumble_l || packet_out.rumble_r)
-    {
-        bp_device->report_parser.play_dual_rumble(
-            bp_device, 
-            0, 
-            FEEDBACK_TIME_MS, 
-            packet_out.rumble_l, 
-            packet_out.rumble_r
-            );
-    }
-}
-
-//This will have to be changed once full support for multiple devices is added
-void BTManager::feedback_timer_cb(btstack_timer_source *ts)
-{
-    static FBContext fb_contexts[MAX_GAMEPADS];
-    BTManager& bt_manager = get_instance();
-
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        if (!get_connected_bp32_device(i))
-        {
-            continue;
-        }
-
-        FBContext& fb_context = fb_contexts[i];
-        fb_context.index = i;
-        fb_context.packet_out = &bt_manager.devices_[i].packet_out;
-        fb_context.cb_reg.callback = send_feedback_cb;
-        fb_context.cb_reg.context = reinterpret_cast<void*>(&fb_context);
-
-        //Register a read on i2c thread, with callback to send feedback on btstack thread
-        bt_manager.i2c_driver_.read_packet(I2CDriver::MULTI_SLAVE ? i + 1 : 0x01,
-            [&fb_context](const I2CDriver::PacketOut& packet_out)
-            {
-                fb_context.packet_out->store(packet_out);
-                btstack_run_loop_execute_on_main_thread(&fb_context.cb_reg);
-            });
-    }
-
-    btstack_run_loop_set_timer(ts, FEEDBACK_TIME_MS);
-    btstack_run_loop_add_timer(ts);
-}
-
-void BTManager::manage_connection(uint8_t index, bool connected)
-{
-    devices_[index].connected.store(connected);
-    if (connected)
-    {
-        if (!fb_timer_running_)
-        {
-            fb_timer_running_ = true;
-            fb_timer_.process = feedback_timer_cb;
-            fb_timer_.context = nullptr;
-            btstack_run_loop_set_timer(&fb_timer_, FEEDBACK_TIME_MS);
-            btstack_run_loop_add_timer(&fb_timer_);
-        }
-    }
-    else
-    {
-        if (!any_connected())
-        {
-            if (fb_timer_running_)
-            {
-                fb_timer_running_ = false;
-                btstack_run_loop_remove_timer(&fb_timer_);
-            }
-        }
-
-        I2CDriver::PacketIn packet_in = I2CDriver::PacketIn();
-        packet_in.packet_id = I2CDriver::PacketID::SET_PAD;
-        packet_in.index = index;
-        i2c_driver_.write_packet(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
-    }
-}
-
-I2CDriver::PacketIn BTManager::get_packet_in(uint8_t index)
-{
-    if (index >= devices_.size())
-    {
-        return I2CDriver::PacketIn();
-    }
-    return devices_[index].packet_in;
-}
--- a/Firmware/ESP32/main/BTManager/BTManager.h
+++ b/Firmware/ESP32/main/BTManager/BTManager.h
@@ -1,83 +0,0 @@
-#ifndef _BT_MANAGER_H_ 
-#define _BT_MANAGER_H_
-
-#include <cstdint>
-#include <array>
-#include <atomic>
-#include <string>
-#include <freertos/FreeRTOS.h>
-#include <freertos/task.h>
-
-#include "I2CDriver/I2CDriver.h"
-#include "Gamepad/Gamepad.h"
-
-class BTManager
-{
-public:
-    static BTManager& get_instance()
-    {
-        static BTManager instance;
-        return instance;
-    }
-
-    void run_task();
-    bool any_connected();
-    bool is_connected(uint8_t index);
-    I2CDriver::PacketIn get_packet_in(uint8_t index);
-
-private:
-    BTManager() = default;
-    ~BTManager() = default;
-    BTManager(const BTManager&) = delete;
-    BTManager& operator=(const BTManager&) = delete;
-
-    static constexpr uint32_t FEEDBACK_TIME_MS = 200;
-    static constexpr uint32_t LED_TIME_MS = 500;
-
-    struct Device
-    {
-        std::atomic<bool> connected{false};
-        GamepadMapper mapper;
-        I2CDriver::PacketIn packet_in;
-        std::atomic<I2CDriver::PacketOut> packet_out; //Can be updated from i2c thread
-    };
-
-    struct FBContext
-    {
-        uint8_t index;
-        std::atomic<I2CDriver::PacketOut>* packet_out;
-        btstack_context_callback_registration_t cb_reg;
-    };
-
-    std::array<Device, MAX_GAMEPADS> devices_;
-    I2CDriver i2c_driver_;
-
-    btstack_timer_source_t fb_timer_;
-    bool fb_timer_running_ = false;
-
-    void send_driver_type(DeviceDriverType driver_type);
-    void manage_connection(uint8_t index, bool connected);
-    
-    static uni_hid_device_t* get_connected_bp32_device(uint8_t index);
-    static void check_led_cb(btstack_timer_source *ts);
-    static void send_feedback_cb(void* context);
-    static void feedback_timer_cb(btstack_timer_source *ts);
-    static void driver_update_timer_cb(btstack_timer_source *ts);
-
-    //Bluepad32 driver
-
-    void init(int argc, const char** arg_V);
-    void init_complete_cb(void);
-    uni_error_t device_discovered_cb(bd_addr_t addr, const char* name, uint16_t cod, uint8_t rssi);
-    void device_connected_cb(uni_hid_device_t* device);
-    void device_disconnected_cb(uni_hid_device_t* device);
-    uni_error_t device_ready_cb(uni_hid_device_t* device);
-    void controller_data_cb(uni_hid_device_t* bp_device, uni_controller_t* controller);
-    const uni_property_t* get_property_cb(uni_property_idx_t idx);
-    void oob_event_cb(uni_platform_oob_event_t event, void* data);
-
-    static uni_platform* get_bp32_driver();
-
-}; // class BTManager
-
-#endif // _BT_MANAGER_H_
--- a/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp
+++ b/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp
@@ -1,194 +0,0 @@
-#include "btstack_port_esp32.h"
-#include "btstack_run_loop.h"
-#include "btstack_stdio_esp32.h"
-#include "uni.h"
-
-#include "Board/ogxm_log.h"
-#include "BTManager/BTManager.h"
-#include "BLEServer/BLEServer.h"
-
-void BTManager::init(int argc, const char** arg_V) 
-{
-    OGXM_LOG("BP32: Initializing Bluepad32\n");
-}
-
-void BTManager::init_complete_cb(void) 
-{
-    uni_bt_enable_new_connections_unsafe(true);
-    // uni_bt_del_keys_unsafe();
-    uni_property_dump_all();
-}
-
-uni_error_t BTManager::device_discovered_cb(bd_addr_t addr, const char* name, uint16_t cod, uint8_t rssi) 
-{
-    if (!((cod & UNI_BT_COD_MINOR_MASK) & UNI_BT_COD_MINOR_GAMEPAD))
-    {
-        return UNI_ERROR_IGNORE_DEVICE;
-    }
-    return UNI_ERROR_SUCCESS;
-}
-
-void BTManager::device_connected_cb(uni_hid_device_t* device) 
-{
-    OGXM_LOG("BP32: Device connected, addr:  %p, index: %i\n", device, uni_hid_device_get_idx_for_instance(device));
-}
-
-void BTManager::device_disconnected_cb(uni_hid_device_t* device) 
-{
-    int idx = uni_hid_device_get_idx_for_instance(device);
-    if (idx >= MAX_GAMEPADS || idx < 0)
-    {
-        return;
-    }
-    manage_connection(static_cast<uint8_t>(idx), false);
-}
-
-uni_error_t BTManager::device_ready_cb(uni_hid_device_t* device) 
-{
-    int idx = uni_hid_device_get_idx_for_instance(device);
-    if (idx >= MAX_GAMEPADS || idx < 0)
-    {
-        return UNI_ERROR_IGNORE_DEVICE;
-    }
-    manage_connection(static_cast<uint8_t>(idx), true);
-    return UNI_ERROR_SUCCESS;
-}
-
-void BTManager::controller_data_cb(uni_hid_device_t* bp_device, uni_controller_t* controller) 
-{
-    static uni_gamepad_t prev_uni_gps[MAX_GAMEPADS] = {};
-
-    if (controller->klass != UNI_CONTROLLER_CLASS_GAMEPAD)
-    {
-        return;
-    }
-
-    int idx = uni_hid_device_get_idx_for_instance(bp_device);
-    uni_gamepad_t *uni_gp = &controller->gamepad;
-
-    if (idx < 0 || idx >= MAX_GAMEPADS || std::memcmp(uni_gp, &prev_uni_gps[idx], sizeof(uni_gamepad_t)) == 0)
-    {
-        return;
-    }
-
-    I2CDriver::PacketIn& packet_in =  devices_[idx].packet_in;
-    GamepadMapper& mapper =  devices_[idx].mapper;
-
-    packet_in = I2CDriver::PacketIn();
-    packet_in.packet_id = I2CDriver::PacketID::SET_PAD;
-    packet_in.index = static_cast<uint8_t>(idx);
-
-    switch (uni_gp->dpad) 
-    {
-        case DPAD_UP:
-            packet_in.dpad = mapper.DPAD_UP;
-            break;
-        case DPAD_DOWN:
-            packet_in.dpad = mapper.DPAD_DOWN;
-            break;
-        case DPAD_LEFT:
-            packet_in.dpad = mapper.DPAD_LEFT;
-            break;
-        case DPAD_RIGHT:
-            packet_in.dpad = mapper.DPAD_RIGHT;
-            break;
-        case (DPAD_UP | DPAD_RIGHT):
-            packet_in.dpad = mapper.DPAD_UP_RIGHT;
-            break;
-        case (DPAD_DOWN | DPAD_RIGHT):
-            packet_in.dpad = mapper.DPAD_DOWN_RIGHT;
-            break;
-        case (DPAD_DOWN | DPAD_LEFT):
-            packet_in.dpad = mapper.DPAD_DOWN_LEFT;
-            break;
-        case (DPAD_UP | DPAD_LEFT):
-            packet_in.dpad = mapper.DPAD_UP_LEFT;
-            break;
-        default:
-            break;
-    }
-
-    if (uni_gp->buttons & BUTTON_A) packet_in.buttons |= mapper.BUTTON_A;
-    if (uni_gp->buttons & BUTTON_B) packet_in.buttons |= mapper.BUTTON_B;
-    if (uni_gp->buttons & BUTTON_X) packet_in.buttons |= mapper.BUTTON_X;
-    if (uni_gp->buttons & BUTTON_Y) packet_in.buttons |= mapper.BUTTON_Y;
-    if (uni_gp->buttons & BUTTON_SHOULDER_L) packet_in.buttons |= mapper.BUTTON_LB;
-    if (uni_gp->buttons & BUTTON_SHOULDER_R) packet_in.buttons |= mapper.BUTTON_RB;
-    if (uni_gp->buttons & BUTTON_THUMB_L)    packet_in.buttons |= mapper.BUTTON_L3;
-    if (uni_gp->buttons & BUTTON_THUMB_R)    packet_in.buttons |= mapper.BUTTON_R3;
-    if (uni_gp->misc_buttons & MISC_BUTTON_BACK)    packet_in.buttons |= mapper.BUTTON_BACK;
-    if (uni_gp->misc_buttons & MISC_BUTTON_START)   packet_in.buttons |= mapper.BUTTON_START;
-    if (uni_gp->misc_buttons & MISC_BUTTON_SYSTEM)  packet_in.buttons |= mapper.BUTTON_SYS;
-    if (uni_gp->misc_buttons & MISC_BUTTON_CAPTURE) packet_in.buttons |= mapper.BUTTON_MISC;
-
-    packet_in.trigger_l = mapper.scale_trigger_l<10>(static_cast<uint16_t>(uni_gp->brake));
-    packet_in.trigger_r = mapper.scale_trigger_r<10>(static_cast<uint16_t>(uni_gp->throttle));
-
-    // auto joy_l = mapper.scale_joystick_l<10>(uni_gp->axis_x, uni_gp->axis_y);
-    // auto joy_r = mapper.scale_joystick_r<10>(uni_gp->axis_rx, uni_gp->axis_ry);
-
-    // packet_in.joystick_lx = joy_l.first;
-    // packet_in.joystick_ly = joy_l.second;
-    // packet_in.joystick_rx = joy_r.first;
-    // packet_in.joystick_ry = joy_r.second;
-
-    std::tie(packet_in.joystick_lx, packet_in.joystick_ly) = mapper.scale_joystick_l<10>(uni_gp->axis_x, uni_gp->axis_y);
-    std::tie(packet_in.joystick_rx, packet_in.joystick_ry) = mapper.scale_joystick_r<10>(uni_gp->axis_rx, uni_gp->axis_ry);
-
-    i2c_driver_.write_packet(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
-
-    std::memcpy(&prev_uni_gps[idx], uni_gp, sizeof(uni_gamepad_t));
-}
-
-const uni_property_t* BTManager::get_property_cb(uni_property_idx_t idx) 
-{
-    return nullptr;
-}
-
-void BTManager::oob_event_cb(uni_platform_oob_event_t event, void* data) 
-{
-    return;
-}
-
-uni_platform* BTManager::get_bp32_driver() 
-{
-    static uni_platform driver = {
-        .name = "OGXMiniW",
-        .init = 
-            [](int argc, const char** argv)
-            { get_instance().init(argc, argv); },
-
-        .on_init_complete = 
-            [](void)
-            { get_instance().init_complete_cb(); },
-
-        .on_device_discovered = 
-            [](bd_addr_t addr, const char* name, uint16_t cod, uint8_t rssi) 
-            { return get_instance().device_discovered_cb(addr, name, cod, rssi); },
-
-        .on_device_connected = 
-            [](uni_hid_device_t* device) 
-            { get_instance().device_connected_cb(device); },
-
-        .on_device_disconnected = 
-            [](uni_hid_device_t* device) 
-            { get_instance().device_disconnected_cb(device); },
-
-        .on_device_ready = 
-            [](uni_hid_device_t* device) 
-            { return get_instance().device_ready_cb(device); }, 
-
-        .on_controller_data = 
-            [](uni_hid_device_t* device, uni_controller_t* controller) 
-            { get_instance().controller_data_cb(device, controller); },
-
-        .get_property = 
-            [](uni_property_idx_t idx) 
-            {  return get_instance().get_property_cb(idx); },
-
-        .on_oob_event = 
-            [](uni_platform_oob_event_t event, void* data) 
-            { get_instance().oob_event_cb(event, data); },
-    };
-    return &driver;
-}
--- a/Firmware/ESP32/main/Board/board_api.cpp
+++ b/Firmware/ESP32/main/Board/board_api.cpp
@@ -1,76 +0,0 @@
-#include <driver/gpio.h>
-
-#include "freertos/FreeRTOS.h"
-#include "freertos/semphr.h"
-
-#include "sdkconfig.h"
-#include "board/board_api.h"
-
-namespace board_api {
-
-SemaphoreHandle_t leds_mutex_ = nullptr;
-// SemaphoreHandle_t reset_mutex_ = nullptr;
-
-void init_board()
-{
-    if (leds_mutex_ == nullptr)
-    {
-        leds_mutex_ = xSemaphoreCreateMutex();
-    }
-    // if (reset_mutex_ == nullptr)
-    // {
-    //     reset_mutex_ = xSemaphoreCreateMutex();
-    // }
-
-    if (xSemaphoreTake(leds_mutex_, portMAX_DELAY))
-    {
-        for (const auto& LED_PIN : LED_PINS)
-        {
-            gpio_reset_pin(LED_PIN);
-            gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);
-            gpio_set_level(LED_PIN, 0);
-        }
-        xSemaphoreGive(leds_mutex_);
-    }
-    // if (xSemaphoreTake(reset_mutex_, portMAX_DELAY))
-    // {
-    //     gpio_reset_pin(RESET_PIN);
-    //     gpio_set_direction(RESET_PIN, GPIO_MODE_INPUT);
-    //     gpio_set_level(RESET_PIN, 1);
-    //     xSemaphoreGive(reset_mutex_);
-    // }
-}
-
-//Set LED by index
-void set_led(uint8_t index, bool state)
-{
-    if constexpr (NUM_LEDS < 1)
-    {
-        return;
-    }
-    if (index >= NUM_LEDS)
-    {
-        return;
-    }
-    if (xSemaphoreTake(leds_mutex_, portMAX_DELAY))
-    {
-        gpio_set_level(LED_PINS[index], state ? 1 : 0);
-        xSemaphoreGive(leds_mutex_);
-    }
-}
-
-//Set first LED
-void set_led(bool state)
-{
-    if constexpr (NUM_LEDS < 1)
-    {
-        return;
-    }
-    if (xSemaphoreTake(leds_mutex_, portMAX_DELAY))
-    {
-        gpio_set_level(LED_PINS[0], state ? 1 : 0);
-        xSemaphoreGive(leds_mutex_);
-    }
-}
-
-} // namespace board_api
--- a/Firmware/ESP32/main/Board/board_api.h
+++ b/Firmware/ESP32/main/Board/board_api.h
@@ -1,43 +0,0 @@
-#ifndef _BOARD_API_H_
-#define _BOARD_API_H_
-
-#include <cstdint>
-#include <driver/gpio.h>
-
-#include "sdkconfig.h"
-
-#define MAX_GPIO_NUM 40
-
-namespace board_api
-{
-    static constexpr gpio_num_t RESET_PIN = static_cast<gpio_num_t>(CONFIG_RESET_PIN);
-    static_assert(RESET_PIN < MAX_GPIO_NUM, "Invalid RESET_PIN");
-
-    static constexpr gpio_num_t LED_PINS[] =
-    {
-    #if defined(CONFIG_ENABLE_LED_1) && (CONFIG_LED_PIN_1 < MAX_GPIO_NUM) && (CONFIG_BLUEPAD32_MAX_DEVICES > 0)
-        static_cast<gpio_num_t>(CONFIG_LED_PIN_1),
-
-    #if defined(CONFIG_ENABLE_LED_2) && (CONFIG_LED_PIN_2 < MAX_GPIO_NUM) && (CONFIG_BLUEPAD32_MAX_DEVICES > 1)
-        static_cast<gpio_num_t>(CONFIG_LED_PIN_2),
-
-    #if defined(CONFIG_ENABLE_LED_3) && (CONFIG_LED_PIN_3 < MAX_GPIO_NUM) && (CONFIG_BLUEPAD32_MAX_DEVICES > 2)
-        static_cast<gpio_num_t>(CONFIG_LED_PIN_3),
-        
-    #if defined(CONFIG_ENABLE_LED_4) && (CONFIG_LED_PIN_4 < MAX_GPIO_NUM) && (CONFIG_BLUEPAD32_MAX_DEVICES > 3)
-        static_cast<gpio_num_t>(CONFIG_LED_PIN_4),
-
-    #endif // CONFIG_ENABLE_LED_4
-    #endif // CONFIG_ENABLE_LED_3
-    #endif // CONFIG_ENABLE_LED_2
-    #endif // CONFIG_ENABLE_LED_1
-    };
-
-    static constexpr uint8_t NUM_LEDS = sizeof(LED_PINS) / sizeof(LED_PINS[0]);
-
-    void init_board();
-    void set_led(uint8_t index, bool state);
-    void set_led(bool state);
-}
-
-#endif // _BOARD_API_H_
--- a/Firmware/ESP32/main/Board/ogxm_log.h
+++ b/Firmware/ESP32/main/Board/ogxm_log.h
@@ -1,67 +0,0 @@
-#ifndef _OGXM_LOG_H_
-#define _OGXM_LOG_H_
-
-#include <esp_log.h>
-#if ESP_LOG_LEVEL >= ESP_LOG_INFO
-
-#include <cstdint>
-#include <string>
-#include <sstream>
-#include <iomanip>
-#include <stdarg.h>
-
-#include "uni.h"
-
-namespace OGXM 
-{
-    static inline void log(const std::string& message)
-    {
-        logi(message.c_str());
-    }
-
-    static inline void log(const char* fmt, ...)
-    {
-        va_list args;
-        va_start(args, fmt);
-        uni_logv(fmt, args);
-        va_end(args);
-    }
-
-    static inline void log_hex(const std::string& message, const uint8_t* data, const size_t len)
-    {
-        log(message);
-
-        std::ostringstream hex_stream;
-        hex_stream << std::hex << std::setfill('0');
-        int char_num = 0;
-        for (uint16_t i = 0; i < len; ++i)
-        {
-            hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
-            char_num++;
-            if (char_num == 16)
-            {
-                hex_stream << "\n";
-                char_num = 0;
-            }
-        }
-        if (char_num != 0)
-        {
-            hex_stream << "\n";
-        }
-
-        log(hex_stream.str());
-    }
-
-} // namespace OGXM
-
-#define OGXM_LOG OGXM::log
-#define OGXM_LOG_HEX OGXM::log_hex
-
-#else // ESP_LOG_LEVEL >= ESP_LOG_INFO
-
-#define OGXM_LOG(...)
-#define OGXM_LOG_HEX(...)
-
-#endif // ESP_LOG_LEVEL >= ESP_LOG_INFO
-
-#endif // _OGXM_LOG_H_
--- a/Firmware/ESP32/main/CMakeLists.txt
+++ b/Firmware/ESP32/main/CMakeLists.txt
@@ -1,29 +1,28 @@
-include(${CMAKE_CURRENT_SOURCE_DIR}/../../FWDefines.cmake)
+include(${CMAKE_CURRENT_SOURCE_DIR}/../FWDefines.cmake)

 idf_component_register( 
    SRCS    
        "main.c" 
-        "main.cpp"
-        "Board/board_api.cpp"
-        "BTManager/BTManager.cpp"
-        "BTManager/BTManager_BP32.cpp"
-        "BLEServer/BLEServer.cpp"
-        "I2CDriver/I2CDriver.cpp"
-        "UserSettings/UserSettings.cpp"
-        "UserSettings/UserProfile.cpp"
-        "UserSettings/TriggerSettings.cpp"
-        "UserSettings/JoystickSettings.cpp"
+        "log/log.c"
+        "wired/wired_i2c.c"
+        "wired/wired_spi.c"
+        "led/led.c"
+        "settings/nvs.c"
+        "settings/settings.cpp"
+        "settings/button_combo.c"
+        "bluetooth/bluetooth.c"
+        "periph/i2c_master.c"
    INCLUDE_DIRS 
        "."
    REQUIRES 
        bluepad32 
        btstack 
-        driver 
        nvs_flash 
-        libfixmath 
+        libfixmath
 )

-target_compile_definitions(${COMPONENT_LIB} PRIVATE
+target_compile_definitions(${COMPONENT_LIB} 
+    PRIVATE
        FIRMWARE_NAME=\"${FW_NAME}\"
        FIRMWARE_VERSION=\"${FW_VERSION}\"
 )
--- a/Firmware/ESP32/main/Gamepad/Gamepad.h
+++ b/Firmware/ESP32/main/Gamepad/Gamepad.h
@@ -1,450 +0,0 @@
-#ifndef GAMEPAD_H
-#define GAMEPAD_H
-
-#include <cstdint>
-
-#include "sdkconfig.h"
-#include "Gamepad/Range.h"
-#include "Gamepad/fix16ext.h"
-#include "UserSettings/UserProfile.h"
-#include "UserSettings/UserSettings.h"
-#include "Board/ogxm_log.h"
-
-#define MAX_GAMEPADS CONFIG_BLUEPAD32_MAX_DEVICES
-static_assert(  MAX_GAMEPADS > 0 && 
-                MAX_GAMEPADS <= 4, 
-                "MAX_GAMEPADS must be between 1 and 4");
-
-namespace Gamepad
-{
-    static constexpr uint8_t DPAD_UP         = 0x01;
-    static constexpr uint8_t DPAD_DOWN       = 0x02;
-    static constexpr uint8_t DPAD_LEFT       = 0x04;
-    static constexpr uint8_t DPAD_RIGHT      = 0x08;
-    static constexpr uint8_t DPAD_UP_LEFT    = DPAD_UP | DPAD_LEFT;
-    static constexpr uint8_t DPAD_UP_RIGHT   = DPAD_UP | DPAD_RIGHT;
-    static constexpr uint8_t DPAD_DOWN_LEFT  = DPAD_DOWN | DPAD_LEFT;
-    static constexpr uint8_t DPAD_DOWN_RIGHT = DPAD_DOWN | DPAD_RIGHT;
-    static constexpr uint8_t DPAD_NONE       = 0x00;
-
-    static constexpr uint16_t BUTTON_A     = 0x0001;
-    static constexpr uint16_t BUTTON_B     = 0x0002;
-    static constexpr uint16_t BUTTON_X     = 0x0004;
-    static constexpr uint16_t BUTTON_Y     = 0x0008;
-    static constexpr uint16_t BUTTON_L3    = 0x0010;
-    static constexpr uint16_t BUTTON_R3    = 0x0020;
-    static constexpr uint16_t BUTTON_BACK  = 0x0040;
-    static constexpr uint16_t BUTTON_START = 0x0080;
-    static constexpr uint16_t BUTTON_LB    = 0x0100;
-    static constexpr uint16_t BUTTON_RB    = 0x0200;
-    static constexpr uint16_t BUTTON_SYS   = 0x0400;
-    static constexpr uint16_t BUTTON_MISC  = 0x0800;
-
-    static constexpr uint8_t ANALOG_OFF_UP    = 0;
-    static constexpr uint8_t ANALOG_OFF_DOWN  = 1;
-    static constexpr uint8_t ANALOG_OFF_LEFT  = 2;
-    static constexpr uint8_t ANALOG_OFF_RIGHT = 3;
-    static constexpr uint8_t ANALOG_OFF_A     = 4;
-    static constexpr uint8_t ANALOG_OFF_B     = 5;
-    static constexpr uint8_t ANALOG_OFF_X     = 6;
-    static constexpr uint8_t ANALOG_OFF_Y     = 7;
-    static constexpr uint8_t ANALOG_OFF_LB    = 8;
-    static constexpr uint8_t ANALOG_OFF_RB    = 9;
-}
-
-class GamepadMapper
-{
-public:
-    uint8_t DPAD_UP         = Gamepad::DPAD_UP        ;
-    uint8_t DPAD_DOWN       = Gamepad::DPAD_DOWN      ;
-    uint8_t DPAD_LEFT       = Gamepad::DPAD_LEFT      ;
-    uint8_t DPAD_RIGHT      = Gamepad::DPAD_RIGHT     ;
-    uint8_t DPAD_UP_LEFT    = Gamepad::DPAD_UP_LEFT   ;
-    uint8_t DPAD_UP_RIGHT   = Gamepad::DPAD_UP_RIGHT  ;
-    uint8_t DPAD_DOWN_LEFT  = Gamepad::DPAD_DOWN_LEFT ;
-    uint8_t DPAD_DOWN_RIGHT = Gamepad::DPAD_DOWN_RIGHT;
-    uint8_t DPAD_NONE       = Gamepad::DPAD_NONE      ;
-
-    uint16_t BUTTON_A     = Gamepad::BUTTON_A    ;
-    uint16_t BUTTON_B     = Gamepad::BUTTON_B    ;
-    uint16_t BUTTON_X     = Gamepad::BUTTON_X    ;
-    uint16_t BUTTON_Y     = Gamepad::BUTTON_Y    ;
-    uint16_t BUTTON_L3    = Gamepad::BUTTON_L3   ;
-    uint16_t BUTTON_R3    = Gamepad::BUTTON_R3   ;
-    uint16_t BUTTON_BACK  = Gamepad::BUTTON_BACK ;
-    uint16_t BUTTON_START = Gamepad::BUTTON_START;
-    uint16_t BUTTON_LB    = Gamepad::BUTTON_LB   ;
-    uint16_t BUTTON_RB    = Gamepad::BUTTON_RB   ;
-    uint16_t BUTTON_SYS   = Gamepad::BUTTON_SYS  ;
-    uint16_t BUTTON_MISC  = Gamepad::BUTTON_MISC ;
-
-    GamepadMapper() = default;
-    ~GamepadMapper() = default;
-
-    void set_profile(const UserProfile& profile)
-    {
-        set_profile_settings(profile);
-        set_profile_mappings(profile);
-    }
-
-    template <uint8_t bits = 0, typename T>
-    inline std::pair<int16_t, int16_t> scale_joystick_r(T x, T y, bool invert_y = false) const
-    {
-        int16_t joy_x = 0;
-        int16_t joy_y = 0;
-        if constexpr (bits > 0)
-        {
-            joy_x = Range::scale_from_bits<int16_t, bits>(x);
-            joy_y = Range::scale_from_bits<int16_t, bits>(y);
-        }
-        else if constexpr (!std::is_same_v<T, int16_t>)
-        {
-            joy_x = Range::scale<int16_t>(x);
-            joy_y = Range::scale<int16_t>(y);
-        }
-        else
-        {
-            joy_x = x;
-            joy_y = y;
-        }
-
-        return  joy_settings_r_en_ 
-                    ? apply_joystick_settings(joy_x, joy_y, joy_settings_r_, invert_y) 
-                    : std::make_pair(joy_x, (invert_y ? Range::invert(joy_y) : joy_y));
-    }
-
-    template <uint8_t bits = 0, typename T>
-    inline std::pair<int16_t, int16_t> scale_joystick_l(T x, T y, bool invert_y = false) const
-    {
-        int16_t joy_x = 0;
-        int16_t joy_y = 0;
-        if constexpr (bits > 0)
-        {
-            joy_x = Range::scale_from_bits<int16_t, bits>(x);
-            joy_y = Range::scale_from_bits<int16_t, bits>(y);
-        }
-        else if constexpr (!std::is_same_v<T, int16_t>)
-        {
-            joy_x = Range::scale<int16_t>(x);
-            joy_y = Range::scale<int16_t>(y);
-        }
-        else
-        {
-            joy_x = x;
-            joy_y = y;
-        }
-
-        return  joy_settings_l_en_ 
-                    ? apply_joystick_settings(joy_x, joy_y, joy_settings_l_, invert_y) 
-                    : std::make_pair(joy_x, (invert_y ? Range::invert(joy_y) : joy_y));
-    }
-
-    template <uint8_t bits = 0, typename T>
-    inline uint8_t scale_trigger_l(T value) const
-    {
-        uint8_t trigger_value = 0;
-        if constexpr (bits > 0)
-        {
-            trigger_value = Range::scale_from_bits<uint8_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, uint8_t>)
-        {
-            trigger_value = Range::scale<uint8_t>(value);
-        }
-        else
-        {
-            trigger_value = value;
-        }
-        return  trig_settings_l_en_ 
-                    ? apply_trigger_settings(trigger_value, trig_settings_l_) 
-                    : trigger_value;
-    }
-
-    template <uint8_t bits = 0, typename T>
-    inline uint8_t scale_trigger_r(T value) const
-    {
-        uint8_t trigger_value = 0;
-        if constexpr (bits > 0)
-        {
-            trigger_value = Range::scale_from_bits<uint8_t, bits>(value);
-        }
-        else if constexpr (!std::is_same_v<T, uint8_t>)
-        {
-            trigger_value = Range::scale<uint8_t>(value);
-        }
-        else
-        {
-            trigger_value = value;
-        }
-        return  trig_settings_r_en_ 
-                    ? apply_trigger_settings(trigger_value, trig_settings_r_) 
-                    : trigger_value;
-    }
-
-private:
-    JoystickSettings joy_settings_l_;
-    JoystickSettings joy_settings_r_;
-    TriggerSettings trig_settings_l_;
-    TriggerSettings trig_settings_r_;
-
-    bool joy_settings_l_en_{false};
-    bool joy_settings_r_en_{false};
-    bool trig_settings_l_en_{false};
-    bool trig_settings_r_en_{false};
-
-    void set_profile_settings(const UserProfile& profile)
-    {
-        if ((joy_settings_l_en_ = !joy_settings_l_.is_same(profile.joystick_settings_l)))
-        {
-            joy_settings_l_.set_from_raw(profile.joystick_settings_l);
-            //This needs to be addressed in the webapp, just multiply here for now
-            joy_settings_l_.axis_restrict *= static_cast<int16_t>(100);
-            joy_settings_l_.angle_restrict *= static_cast<int16_t>(100);
-            joy_settings_l_.anti_dz_angular *= static_cast<int16_t>(100);
-        }
-        if ((joy_settings_r_en_ = !joy_settings_r_.is_same(profile.joystick_settings_r)))
-        {
-            joy_settings_r_.set_from_raw(profile.joystick_settings_r);
-            //This needs to be addressed in the webapp, just multiply here for now
-            joy_settings_r_.axis_restrict *= static_cast<int16_t>(100);
-            joy_settings_r_.angle_restrict *= static_cast<int16_t>(100);
-            joy_settings_r_.anti_dz_angular *= static_cast<int16_t>(100);
-        }
-        if ((trig_settings_l_en_ = !trig_settings_l_.is_same(profile.trigger_settings_l)))
-        {
-            trig_settings_l_.set_from_raw(profile.trigger_settings_l);
-        }
-        if ((trig_settings_r_en_ = !trig_settings_r_.is_same(profile.trigger_settings_r)))
-        {
-            trig_settings_r_.set_from_raw(profile.trigger_settings_r);
-        }
-
-        OGXM_LOG("GamepadMapper: JoyL: %s, JoyR: %s, TrigL: %s, TrigR: %s\n",
-            joy_settings_l_en_ ? "Enabled" : "Disabled",
-            joy_settings_r_en_ ? "Enabled" : "Disabled",
-            trig_settings_l_en_ ? "Enabled" : "Disabled",
-            trig_settings_r_en_ ? "Enabled" : "Disabled");
-    }
-
-    void set_profile_mappings(const UserProfile& profile)
-    {
-        DPAD_UP         = profile.dpad_up;
-        DPAD_DOWN       = profile.dpad_down;
-        DPAD_LEFT       = profile.dpad_left;
-        DPAD_RIGHT      = profile.dpad_right;
-        DPAD_UP_LEFT    = profile.dpad_up | profile.dpad_left;
-        DPAD_UP_RIGHT   = profile.dpad_up | profile.dpad_right;
-        DPAD_DOWN_LEFT  = profile.dpad_down | profile.dpad_left;
-        DPAD_DOWN_RIGHT = profile.dpad_down | profile.dpad_right;
-        DPAD_NONE       = 0;
-
-        BUTTON_A     = profile.button_a;
-        BUTTON_B     = profile.button_b;
-        BUTTON_X     = profile.button_x;
-        BUTTON_Y     = profile.button_y;
-        BUTTON_L3    = profile.button_l3;
-        BUTTON_R3    = profile.button_r3;
-        BUTTON_BACK  = profile.button_back;
-        BUTTON_START = profile.button_start;
-        BUTTON_LB    = profile.button_lb;
-        BUTTON_RB    = profile.button_rb;
-        BUTTON_SYS   = profile.button_sys;
-        BUTTON_MISC  = profile.button_misc;
-    }
-
-    static inline std::pair<int16_t, int16_t> apply_joystick_settings(
-        int16_t gp_joy_x, 
-        int16_t gp_joy_y, 
-        const JoystickSettings& set,
-        bool invert_y)
-    {
-        static const Fix16 
-            FIX_0(0.0f),
-            FIX_1(1.0f),
-            FIX_2(2.0f),
-            FIX_45(45.0f),
-            FIX_90(90.0f),
-            FIX_100(100.0f),
-            FIX_180(180.0f),
-            FIX_EPSILON(0.0001f),
-            FIX_EPSILON2(0.001f),
-            FIX_ELLIPSE_DEF(1.570796f),
-            FIX_DIAG_DIVISOR(0.29289f);
-
-        Fix16 x = (set.invert_x ? Fix16(Range::invert(gp_joy_x)) : Fix16(gp_joy_x)) / Range::MAX<int16_t>;
-        Fix16 y = ((set.invert_y ^ invert_y) ? Fix16(Range::invert(gp_joy_y)) : Fix16(gp_joy_y)) / Range::MAX<int16_t>;
-
-        const Fix16 abs_x = fix16::abs(x);
-        const Fix16 abs_y = fix16::abs(y);
-        const Fix16 inv_axis_restrict = FIX_1 / (FIX_1 - set.axis_restrict);
-
-        Fix16 rAngle = (abs_x < FIX_EPSILON) 
-            ? FIX_90 
-            : fix16::rad2deg(fix16::abs(fix16::atan(y / x)));
-
-        Fix16 axial_x = (abs_x <= set.axis_restrict && rAngle > FIX_45) 
-            ? FIX_0 
-            : ((abs_x - set.axis_restrict) * inv_axis_restrict);
-                
-        Fix16 axial_y = (abs_y <= set.axis_restrict && rAngle <= FIX_45) 
-            ? FIX_0 
-            : ((abs_y - set.axis_restrict) * inv_axis_restrict);
-
-        Fix16 in_magnitude = fix16::sqrt(fix16::sq(axial_x) + fix16::sq(axial_y));
-
-        if (in_magnitude < set.dz_inner)
-        {
-            return { 0, 0 };
-        }
-
-        Fix16 angle = 
-            fix16::abs(axial_x) < FIX_EPSILON 
-                ? FIX_90 
-                : fix16::rad2deg(fix16::abs(fix16::atan(axial_y / axial_x)));
-
-        Fix16 anti_r_scale = (set.anti_dz_square_y_scale == FIX_0) ? set.anti_dz_square : set.anti_dz_square_y_scale;
-        Fix16 anti_dz_c = set.anti_dz_circle;
-
-        if (anti_r_scale > FIX_0 && anti_dz_c > FIX_0)
-        {
-            Fix16 anti_ellip_scale = anti_ellip_scale / anti_dz_c;
-            Fix16 ellipse_angle = fix16::atan((FIX_1 / anti_ellip_scale) * fix16::tan(fix16::rad2deg(rAngle)));
-            ellipse_angle = (ellipse_angle < FIX_0) ? FIX_ELLIPSE_DEF : ellipse_angle;
-
-            Fix16 ellipse_x = fix16::cos(ellipse_angle);
-            Fix16 ellipse_y = fix16::sqrt(fix16::sq(anti_ellip_scale) * (FIX_1 - fix16::sq(ellipse_x)));
-            anti_dz_c *= fix16::sqrt(fix16::sq(ellipse_x) + fix16::sq(ellipse_y));
-        }
-
-        if (anti_dz_c > FIX_0)
-        {
-            anti_dz_c = anti_dz_c / ((anti_dz_c * (FIX_1 - set.anti_dz_circle / set.dz_outer)) / (anti_dz_c * (FIX_1 - set.anti_dz_square)));
-        }
-
-        if (abs_x > set.axis_restrict && abs_y > set.axis_restrict)
-        {
-            const Fix16 FIX_ANGLE_MAX = set.angle_restrict / 2.0f;
-
-            if (angle > FIX_0 && angle < FIX_ANGLE_MAX)
-            {
-                angle = FIX_0;
-            }
-            if (angle > (FIX_90 - FIX_ANGLE_MAX))
-            {
-                angle = FIX_90;
-            }
-            if (angle > FIX_ANGLE_MAX && angle < (FIX_90 - FIX_ANGLE_MAX))
-            {
-                angle = ((angle - FIX_ANGLE_MAX) * FIX_90) / ((FIX_90 - FIX_ANGLE_MAX) - FIX_ANGLE_MAX);
-            }
-        }
-
-        Fix16 ref_angle = (angle < FIX_EPSILON2) ? FIX_0 : angle;
-        Fix16 diagonal = (angle > FIX_45) ? (((angle - FIX_45) * (-FIX_45)) / FIX_45) + FIX_45 : angle;
-
-        const Fix16 angle_comp = set.angle_restrict / FIX_2;
-
-        if (angle < FIX_90 && angle > FIX_0)
-        {
-            angle = ((angle * ((FIX_90 - angle_comp) - angle_comp)) / FIX_90) + angle_comp;
-        }
-
-        if (axial_x < FIX_0 && axial_y > FIX_0)
-        {
-            angle = -angle;
-        }
-        if (axial_x > FIX_0 && axial_y < FIX_0)
-        {
-            angle = angle - FIX_180;
-        }
-        if (axial_x < FIX_0 && axial_y < FIX_0)
-        {
-            angle = angle + FIX_180;
-        }
-
-        //Deadzone Warp
-        Fix16 out_magnitude = (in_magnitude - set.dz_inner) / (set.anti_dz_outer - set.dz_inner);
-        out_magnitude = fix16::pow(out_magnitude, (FIX_1 / set.curve)) * (set.dz_outer - anti_dz_c) + anti_dz_c;
-        out_magnitude = (out_magnitude > set.dz_outer && !set.uncap_radius) ? set.dz_outer : out_magnitude;
-
-		Fix16 d_scale = (((out_magnitude - anti_dz_c) * (set.diag_scale_max - set.diag_scale_min)) / (set.dz_outer - anti_dz_c)) + set.diag_scale_min;		
-		Fix16 c_scale = (diagonal * (FIX_1 / fix16::sqrt(FIX_2))) / FIX_45; //Both these lines scale the intensity of the warping
-		c_scale       = FIX_1 - fix16::sqrt(FIX_1 - c_scale * c_scale);     //based on a circular curve to the perfect diagonal
-		d_scale       = (c_scale * (d_scale - FIX_1)) / FIX_DIAG_DIVISOR + FIX_1;
-
-		out_magnitude = out_magnitude * d_scale;
-
-		//Scaling values for square antideadzone
-		Fix16 new_x = fix16::cos(fix16::deg2rad(angle)) * out_magnitude;
-		Fix16 new_y = fix16::sin(fix16::deg2rad(angle)) * out_magnitude;
-		
-		//Magic angle wobble fix by user ME.
-		// if (angle > 45.0 && angle < 225.0) {
-		// 	newX = inv(Math.sin(deg2rad(angle - 90.0)))*outputMagnitude;
-		// 	newY = inv(Math.cos(deg2rad(angle - 270.0)))*outputMagnitude;
-		// }
-		
-		//Square antideadzone scaling
-		Fix16 output_x = fix16::abs(new_x) * (FIX_1 - set.anti_dz_square / set.dz_outer) + set.anti_dz_square;
-		if (x < FIX_0)
-        {
-            output_x = -output_x;
-        }
-		if (ref_angle == FIX_90)
-        {
-            output_x = FIX_0;
-        }
-		
-		Fix16 output_y = fix16::abs(new_y) * (FIX_1 - anti_r_scale / set.dz_outer) + anti_r_scale;
-		if (y < FIX_0)
-        {
-            output_y = -output_y;
-        }
-		if (ref_angle == FIX_0)
-        {
-            output_y = FIX_0;
-        }
-
-        output_x = fix16::clamp(output_x, -FIX_1, FIX_1) * Range::MAX<int16_t>;
-        output_y = fix16::clamp(output_y, -FIX_1, FIX_1) * Range::MAX<int16_t>;
-
-        return { static_cast<int16_t>(fix16_to_int(output_x)), static_cast<int16_t>(fix16_to_int(output_y)) };
-    }
-
-    static inline uint8_t apply_trigger_settings(uint8_t value, const TriggerSettings& set)
-    {
-        Fix16 abs_value = fix16::abs(Fix16(static_cast<int16_t>(value)) / static_cast<int16_t>(Range::MAX<uint8_t>));
-
-        if (abs_value < set.dz_inner)
-        {
-            return 0;
-        }
-
-        static const Fix16 
-            FIX_0(0.0f),
-            FIX_1(1.0f),
-            FIX_2(2.0f);
-
-        Fix16 value_out = (abs_value - set.dz_inner) / (set.anti_dz_outer - set.dz_inner);
-        value_out = fix16::clamp(value_out, FIX_0, FIX_1);
-
-        if (set.anti_dz_inner > FIX_0)
-        {
-            value_out = set.anti_dz_inner + (FIX_1 - set.anti_dz_inner) * value_out;
-        }
-        if (set.curve != FIX_1)
-        {
-            value_out = fix16::pow(value_out, FIX_1 / set.curve);
-        }
-        if (set.anti_dz_outer < FIX_1)
-        {
-            value_out = fix16::clamp(value_out * (FIX_1 / (FIX_1 - set.anti_dz_outer)), FIX_0, FIX_1);
-        }
-
-        value_out *= set.dz_outer;
-        return static_cast<uint8_t>(fix16_to_int(value_out * static_cast<int16_t>(Range::MAX<uint8_t>)));
-    }
-
-}; // class GamepadMapper
-
-#endif // GAMEPAD_H
--- a/Firmware/ESP32/main/Gamepad/Range.h
+++ b/Firmware/ESP32/main/Gamepad/Range.h
@@ -1,204 +0,0 @@
-#ifndef _RANGE_H_
-#define _RANGE_H_
-
-#include <cstdint>
-#include <limits>
-#include <type_traits>
-#include "Board/ogxm_log.h"
-
-namespace Range {
-
-    template <typename T>
-    requires std::is_integral_v<T>
-    constexpr T MAX = std::numeric_limits<T>::max();
-
-    template <typename T>
-    requires std::is_integral_v<T>
-    constexpr T MIN = std::numeric_limits<T>::min();
-
-    template <typename T>
-    requires std::is_integral_v<T>
-    constexpr T MID = 
-        [] {
-            if constexpr (std::is_unsigned_v<T>) 
-            {
-                return MAX<T> / 2 + 1;
-            } 
-            return 0;
-        }();
-    static_assert(MID<uint8_t> == 128, "MID<uint8_t> != 128");
-    static_assert(MID<int8_t> == 0, "MID<int8_t> != 0");
-
-    template <typename T>
-    requires std::is_integral_v<T>
-    constexpr uint16_t NUM_BITS = sizeof(T) * 8;
-
-    //Maximum value for a given number of bits, result will be signed/unsigned depending on type
-    template<typename T, uint8_t bits>
-    requires std::is_integral_v<T>
-    constexpr T BITS_MAX() 
-    {
-        static_assert(bits <= NUM_BITS<T>, "BITS_MAX: Return type cannot represet maximum bit value");
-        static_assert(bits <= 64, "BITS_MAX: Bits exceed 64");
-        if constexpr (std::is_unsigned_v<T>) 
-        {
-            return (1ULL << bits) - 1;
-        }
-        return (1LL << (bits - 1)) - 1;
-    }
-    static_assert(BITS_MAX<int16_t, 10>() == 511, "BITS_MAX<int16_t>(10) != 511");
-    static_assert(BITS_MAX<uint16_t, 10>() == 1023, "BITS_MAX<uint16_t>(10) != 1023");
-
-    //Minimum value for a given number of bits, result will be signed/unsigned depending on type
-    template<typename T, uint8_t bits>
-    requires std::is_integral_v<T>
-    constexpr T BITS_MIN() 
-    {
-        static_assert(bits <= NUM_BITS<T>, "BITS_MIN: Return type cannot represet minimum bit value");
-        static_assert(bits <= 64, "BITS_MIN: Bits exceed 64");
-        if constexpr (std::is_unsigned_v<T>) 
-        {
-            return 0;
-        }
-        return static_cast<T>(-(1LL << (bits - 1)));
-    }
-    static_assert(BITS_MIN<int16_t, 10>() == -512, "BITS_MIN<int16_t>(10) != -512");
-    static_assert(BITS_MIN<uint16_t, 10>() == 0, "BITS_MIN<uint16_t>(10) != 0");
-
-    template <typename T>
-    static inline T invert(T value) 
-    {   
-        if constexpr (std::is_unsigned_v<T>) 
-        {
-            return Range::MAX<T> - value;
-        } 
-        return (value == Range::MIN<T>) ? Range::MAX<T> : -value; 
-    }
-
-    template <typename To, typename From>
-    requires std::is_integral_v<To> && std::is_integral_v<From>
-    static inline To clamp(From value) 
-    {
-        return static_cast<To>((value < Range::MIN<To>) 
-                                ? Range::MIN<To> 
-                                : (value > Range::MAX<To>) 
-                                    ? Range::MAX<To> 
-                                    : value);
-    }
-
-    template <typename T>
-    static inline T clamp(T value, T min, T max) 
-    {
-        return (value < min) ? min : (value > max) ? max : value;
-    }
-
-    template <typename To, typename From> 
-    static inline To clamp(From value, To min_to, To max_to) 
-    {
-        return (value < min_to) ? min_to : (value > max_to) ? max_to : static_cast<To>(value);
-    }
-
-    template <typename To, typename From>
-    requires std::is_integral_v<To> && std::is_integral_v<From>
-    static constexpr To scale(From value, From min_from, From max_from, To min_to, To max_to) 
-    {
-        return static_cast<To>(
-            (static_cast<int64_t>(value - min_from) * (max_to - min_to) / (max_from - min_from)) + min_to);
-    }
-
-    template <typename To, typename From>
-    static inline To scale(From value) 
-    {
-        return scale<To>(value, Range::MIN<From>, Range::MAX<From>, Range::MIN<To>, Range::MAX<To>);
-    }
-
-    template <typename To, typename From>
-    static inline To scale(From value, To min_to, To max_to) 
-    {
-        return scale<To>(value, Range::MIN<From>, Range::MAX<From>, min_to, max_to);
-    }
-
-    //Cast value to signed/unsigned for accurate scaling
-    template <typename To, uint8_t bits, typename From>
-    static inline To scale_from_bits(From value) 
-    {
-        return scale<To>(
-            Range::clamp(value, BITS_MIN<From, bits>(), BITS_MAX<From, bits>()),
-            BITS_MIN<From, bits>(), 
-            BITS_MAX<From, bits>(), 
-            Range::MIN<To>, 
-            Range::MAX<To>);
-    }
-
-    //Cast value to signed/unsigned for accurate scaling
-    template <typename To, uint8_t bits, typename From>
-    static inline To scale_to_bits(From value) 
-    {
-        return scale<To>(
-            Range::clamp(value, BITS_MIN<From, bits>(), BITS_MAX<From, bits>()), 
-            Range::MIN<From>, 
-            Range::MAX<From>, 
-            BITS_MIN<To, bits>(), 
-            BITS_MAX<To, bits>());
-    }
-
-} // namespace Range
-
-namespace Scale //Scale and invert values
-{
-    static inline uint8_t int16_to_uint8(int16_t value)
-    {
-        uint16_t shifted_value = static_cast<uint16_t>(value + Range::MID<uint16_t>);
-        return static_cast<uint8_t>(shifted_value >> 8);
-    }
-    static inline uint16_t int16_to_uint16(int16_t value)
-    {
-        return static_cast<uint16_t>(value + Range::MID<uint16_t>);
-    }
-    static inline int8_t int16_to_int8(int16_t value)
-    {
-        return static_cast<int8_t>((value + Range::MID<uint16_t>) >> 8);
-    }
-
-    static inline uint8_t uint16_to_uint8(uint16_t value)
-    {
-        return static_cast<uint8_t>(value >> 8);
-    }
-    static inline int16_t uint16_to_int16(uint16_t value)
-    {
-        return static_cast<int16_t>(value - Range::MID<uint16_t>);
-    }
-    static inline int8_t uint16_to_int8(uint16_t value)
-    {
-        return static_cast<int8_t>((value >> 8) - Range::MID<uint8_t>);
-    }
-
-    static inline int16_t uint8_to_int16(uint8_t value)
-    {
-        return static_cast<int16_t>((static_cast<int32_t>(value) << 8) - Range::MID<uint16_t>);
-    }
-    static inline uint16_t uint8_to_uint16(uint8_t value)
-    {
-        return static_cast<uint16_t>(value) << 8;
-    }
-    static inline int8_t uint8_to_int8(uint8_t value)
-    {
-        return static_cast<int8_t>(value - Range::MID<uint8_t>);
-    }
-
-    static inline int16_t int8_to_int16(int8_t value)
-    {
-        return static_cast<int16_t>(value) << 8;
-    }
-    static inline uint16_t int8_to_uint16(int8_t value)
-    {
-        return static_cast<uint16_t>((value + Range::MID<uint8_t>) << 8);
-    }
-    static inline uint8_t int8_to_uint8(int8_t value)
-    {
-        return static_cast<uint8_t>(value + Range::MID<uint8_t>);
-    }
-
-} // namespace Scale
-
-#endif // _RANGE_H_
--- a/Firmware/ESP32/main/Gamepad/fix16ext.h
+++ b/Firmware/ESP32/main/Gamepad/fix16ext.h
@@ -1,106 +0,0 @@
-#ifndef FIX16_EXT_H
-#define FIX16_EXT_H
-
-#include <cstdint>
-
-#include "libfixmath/fix16.hpp"
-
-namespace fix16 {
-
-inline Fix16 abs(Fix16 x)
-{
-    return Fix16(fix16_abs(x.value));
-}
-
-inline Fix16 rad2deg(Fix16 rad)
-{
-    return Fix16(fix16_rad_to_deg(rad.value));
-}
-
-inline Fix16 deg2rad(Fix16 deg)
-{
-    return Fix16(fix16_deg_to_rad(deg.value));
-}
-
-inline Fix16 atan(Fix16 x)
-{
-    return Fix16(fix16_atan(x.value));
-}
-
-inline Fix16 atan2(Fix16 y, Fix16 x)
-{
-    return Fix16(fix16_atan2(y.value, x.value));
-}
-
-inline Fix16 tan(Fix16 x)
-{
-    return Fix16(fix16_tan(x.value));
-}
-
-inline Fix16 cos(Fix16 x)
-{
-    return Fix16(fix16_cos(x.value));
-}
-
-inline Fix16 sin(Fix16 x)
-{
-    return Fix16(fix16_sin(x.value));
-}
-
-inline Fix16 sqrt(Fix16 x)
-{
-    return Fix16(fix16_sqrt(x.value));
-}
-
-inline Fix16 sq(Fix16 x)
-{
-    return Fix16(fix16_sq(x.value));
-}
-
-inline Fix16 clamp(Fix16 x, Fix16 min, Fix16 max)
-{
-    return Fix16(fix16_clamp(x.value, min.value, max.value));
-}
-
-inline Fix16 pow(Fix16 x, Fix16 y)
-{
-    fix16_t& base = x.value;
-    fix16_t& exponent = y.value;
-
-    if (exponent == F16(0.0)) 
-        return Fix16(fix16_from_int(1));
-    if (base == F16(0.0)) 
-        return Fix16(fix16_from_int(0));
-
-    int32_t int_exp = fix16_to_int(exponent);
-
-    if (fix16_from_int(int_exp) == exponent) 
-    {
-        fix16_t result = F16(1.0);
-        fix16_t current_base = base;
-
-        if (int_exp < 0) 
-        {
-            current_base = fix16_div(F16(1.0), base);
-            int_exp = -int_exp;
-        }
-
-        while (int_exp) 
-        {
-            if (int_exp & 1) 
-            {
-                result = fix16_mul(result, current_base);
-            }
-            current_base = fix16_mul(current_base, current_base);
-            int_exp >>= 1;
-        }
-
-        return Fix16(result);
-    }
-
-    return Fix16(fix16_exp(fix16_mul(exponent, fix16_log(base))));
-}
-
-} // namespace fix16
-
-#endif // FIX16_EXT_H
--- a/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
+++ b/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
@@ -1,72 +0,0 @@
-#include <freertos/FreeRTOS.h>
-#include <freertos/task.h>
-#include <freertos/timers.h>
-#include <driver/gpio.h>
-#include <esp_log.h>
-
-#include "I2CDriver/I2CDriver.h"
-
-I2CDriver::~I2CDriver()
-{
-    i2c_driver_delete(i2c_port_);
-}
-
-void I2CDriver::initialize_i2c(i2c_port_t i2c_port, gpio_num_t sda, gpio_num_t scl, uint32_t clk_speed)
-{
-    if (initialized_)
-    {
-        return;
-    }
-
-    i2c_port_ = i2c_port;
-
-    i2c_config_t conf;
-    std::memset(&conf, 0, sizeof(i2c_config_t));
-
-    conf.mode = I2C_MODE_MASTER;
-    conf.sda_io_num = sda;
-    conf.scl_io_num = scl;
-    conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
-    conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
-    conf.master.clk_speed = clk_speed;
-
-    i2c_param_config(i2c_port_, &conf);
-    i2c_driver_install(i2c_port_, conf.mode, 0, 0, 0);
-
-    initialized_ = true;
-}
-
-void I2CDriver::run_tasks()
-{
-    std::function<void()> task;
-
-    while (true)
-    {   
-        while (task_queue_.pop(task))
-        {
-            task();
-        }
-
-        vTaskDelay(1);
-    }
-}
-
-void I2CDriver::write_packet(uint8_t address, const PacketIn& data_in) 
-{
-    task_queue_.push([this, address, data_in]() 
-    {
-        i2c_write_blocking(address, reinterpret_cast<const uint8_t*>(&data_in), sizeof(PacketIn));
-    });
-}
-
-void I2CDriver::read_packet(uint8_t address, std::function<void(const PacketOut&)> callback) 
-{
-    task_queue_.push([this, address, callback]() 
-    {
-        PacketOut data_out;
-        if (i2c_read_blocking(address, reinterpret_cast<uint8_t*>(&data_out), sizeof(PacketOut)) == ESP_OK)
-        {
-            callback(data_out);
-        }
-    });
-}
--- a/Firmware/ESP32/main/I2CDriver/I2CDriver.h
+++ b/Firmware/ESP32/main/I2CDriver/I2CDriver.h
@@ -1,108 +0,0 @@
-#ifndef _I2C_DRIVER_H_
-#define _I2C_DRIVER_H_
-
-#include <cstdint>
-#include <cstring>
-#include <functional>
-#include <driver/i2c.h>
-
-#include "sdkconfig.h"
-#include "RingBuffer.h"
-#include "UserSettings/DeviceDriverTypes.h"
-
-class I2CDriver 
-{
-public:
-    static constexpr bool MULTI_SLAVE = 
-#if CONFIG_MULTI_SLAVE_MODE == 0
-        false;
-#else
-        true;
-#endif
-
-    enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD, SET_DRIVER };
-    enum class PacketResp : uint8_t { OK = 1, ERROR };
-
-    #pragma pack(push, 1)
-    struct PacketIn
-    {
-        uint8_t packet_len{sizeof(PacketIn)};
-        PacketID packet_id{static_cast<uint8_t>(PacketID::SET_PAD)};
-        uint8_t index{0};
-        DeviceDriverType device_driver{DeviceDriverType::NONE};
-        uint8_t dpad{0};
-        uint16_t buttons{0};
-        uint8_t trigger_l{0};
-        uint8_t trigger_r{0};
-        int16_t joystick_lx{0};
-        int16_t joystick_ly{0};
-        int16_t joystick_rx{0};
-        int16_t joystick_ry{0};
-        std::array<uint8_t, 15> reserved1{0};
-    };
-    static_assert(sizeof(PacketIn) == 32, "PacketIn is misaligned");
-
-    struct PacketOut
-    {
-        uint8_t packet_len{0};
-        PacketID packet_id{0};
-        uint8_t index{0};
-        uint8_t rumble_l{0};
-        uint8_t rumble_r{0};
-        std::array<uint8_t, 3> reserved{0};
-    };
-    static_assert(sizeof(PacketOut) == 8, "PacketOut is misaligned");
-    #pragma pack(pop)
-
-    I2CDriver() = default;
-    ~I2CDriver();
-
-    void initialize_i2c(i2c_port_t i2c_port, gpio_num_t sda, gpio_num_t scl, uint32_t clk_speed);
-
-    //Does not return
-    void run_tasks();
-
-    void write_packet(uint8_t address, const PacketIn& data_in);
-    void read_packet(uint8_t address, std::function<void(const PacketOut&)> callback);
-
-private:
-    using TaskQueue = RingBuffer<std::function<void()>, CONFIG_I2C_RING_BUFFER_SIZE>;
-    
-    TaskQueue task_queue_;
-    i2c_port_t i2c_port_ = I2C_NUM_0;
-    bool initialized_ = false;
-
-    static inline esp_err_t i2c_write_blocking(uint8_t address, const uint8_t* buffer, size_t len) 
-    {
-        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-        i2c_master_start(cmd);
-        i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, true);
-        i2c_master_write(cmd, buffer, len, true);
-        i2c_master_stop(cmd);
-
-        esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(2));
-        i2c_cmd_link_delete(cmd);
-        return ret;
-    }
-
-    static inline esp_err_t i2c_read_blocking(uint8_t address, uint8_t* buffer, size_t len) 
-    {
-        i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-        i2c_master_start(cmd);
-        i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_READ, true);
-        
-        if (len > 1) 
-        {
-            i2c_master_read(cmd, buffer, len - 1, I2C_MASTER_ACK);
-        }
-
-        i2c_master_read_byte(cmd, buffer + len - 1, I2C_MASTER_NACK);
-        i2c_master_stop(cmd);
-        
-        esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, pdMS_TO_TICKS(2));
-        i2c_cmd_link_delete(cmd);
-        return ret;
-    }
-}; // class I2CDriver
-
-#endif // _I2C_DRIVER_H_
--- a/Firmware/ESP32/main/Kconfig
+++ b/Firmware/ESP32/main/Kconfig
@@ -1,32 +1,66 @@
 menu "OGXMini Options"

-    config I2C_RING_BUFFER_SIZE
-        int "Set I2C ring buffer size"
-        default 6
+    config LOG_ENABLED
+        bool "Enable logging"
+        default n
+        help
+            Enable logging to the console. This is useful for debugging and monitoring the system.
+    
+    config LOG_LEVEL
+        int "Set log level"
+        default 2
+        range 0 3
+        depends on LOG_ENABLED
+
+    choice
+        prompt "Communication Interface"
+        default I2C_ENABLED
+
+        config I2C_ENABLED
+            bool "Enable I2C support"
+            
+        config SPI_ENABLED
+            bool "Enable SPI support"
+    endchoice

    config I2C_PORT
        int "Set I2C port"
        default 0
+        depends on I2C_ENABLED

    config I2C_SDA_PIN
        int "Set I2C SDA pin"
        default 21
+        depends on I2C_ENABLED

    config I2C_SCL_PIN
        int "Set I2C SCL pin"
        default 22
+        depends on I2C_ENABLED
+
+    config I2C_IRQ_PIN
+        int "Set I2C IRQ pin"
+        default 9
+        depends on I2C_ENABLED

    config I2C_BAUDRATE
        int "Set I2C baudrate"
        default 1000000
+        depends on I2C_ENABLED

-    config RESET_PIN
-        int "Set reset pin"
-        default 9
-
-    config MULTI_SLAVE_MODE
+    config I2C_MULTI_SLAVE_MODE
        bool "Enable multiple slave devices"
        default n
+        depends on I2C_ENABLED
+
+    config WIRED_HEADSET_ENABLED
+        bool "Enable wired headset support"
+        default n
+        depends on SPI_ENABLED
+
+    # config RESET_PIN
+    #     int "Set reset pin"
+    #     default 9

    config ENABLE_LED_1
        bool "Enable LED 1"
@@ -44,7 +78,7 @@ menu "OGXMini Options"

    config LED_PIN_2
        int "Set LED 2 pin"
-        default 255
+        default 15
        depends on ENABLE_LED_2

    config ENABLE_LED_3
@@ -54,7 +88,7 @@ menu "OGXMini Options"

    config LED_PIN_3
        int "Set LED 3 pin"
-        default 255
+        default 15
        depends on ENABLE_LED_3

    config ENABLE_LED_4
@@ -64,7 +98,7 @@ menu "OGXMini Options"

    config LED_PIN_4
        int "Set LED 4 pin"
-        default 255
+        default 15
        depends on ENABLE_LED_4
    
 endmenu
--- a/Firmware/ESP32/main/RingBuffer.h
+++ b/Firmware/ESP32/main/RingBuffer.h
@@ -1,55 +0,0 @@
-#ifndef _RING_BUFFER_H_
-#define _RING_BUFFER_H_
-
-#include <cstdint>
-#include <atomic>
-#include <array>
-
-template<typename Type, size_t SIZE>
-class RingBuffer
-{
-public:
-    RingBuffer() : head_(0), tail_(0) {}
-
-    RingBuffer(const RingBuffer&) = delete;
-    RingBuffer& operator=(const RingBuffer&) = delete;
-
-    RingBuffer(RingBuffer&&) = default;
-    RingBuffer& operator=(RingBuffer&&) = default;
-
-    bool push(const Type& item)
-    {
-        size_t head = head_.load(std::memory_order_relaxed);
-        size_t next_head = (head + 1) % SIZE;
-
-        if (next_head == tail_.load(std::memory_order_acquire))
-        {
-            tail_.store((tail_.load(std::memory_order_relaxed) + 1) % SIZE, std::memory_order_release);
-        }
-
-        buffer_[head] = item;
-        head_.store(next_head, std::memory_order_release);
-        return true;
-    }
-
-    bool pop(Type& item)
-    {
-        size_t tail = tail_.load(std::memory_order_relaxed);
-
-        if (tail == head_.load(std::memory_order_acquire))
-        {
-            return false;
-        }
-
-        item = buffer_[tail];
-        tail_.store((tail + 1) % SIZE, std::memory_order_release);
-        return true;
-    }
-
-private:
-    std::array<Type, SIZE> buffer_;
-    std::atomic<size_t> head_;
-    std::atomic<size_t> tail_;
-};
-
-#endif // _RING_BUFFER_H_
--- a/Firmware/ESP32/main/UserSettings/DeviceDriverTypes.h
+++ b/Firmware/ESP32/main/UserSettings/DeviceDriverTypes.h
@@ -1,39 +0,0 @@
-#ifndef _DEVICE_DRIVER_TYPES_H_
-#define _DEVICE_DRIVER_TYPES_H_
-
-#include <cstdint>
-
-enum class DeviceDriverType : uint8_t
-{
-    NONE = 0,
-    XBOXOG,
-    XBOXOG_SB,
-    XBOXOG_XR,
-    XINPUT,
-    PS3,
-    DINPUT,
-    PSCLASSIC,
-    SWITCH,
-    WEBAPP = 100,
-    UART_BRIDGE
-};
-
-static inline std::string DRIVER_NAME(DeviceDriverType driver)
-{
-    switch (driver)
-    {
-        case DeviceDriverType::XBOXOG: return "XBOX OG";
-        case DeviceDriverType::XBOXOG_SB: return "XBOX OG SB";
-        case DeviceDriverType::XBOXOG_XR: return "XBOX OG XR";
-        case DeviceDriverType::XINPUT: return "XINPUT";
-        case DeviceDriverType::PS3: return "PS3";
-        case DeviceDriverType::DINPUT: return "DINPUT";
-        case DeviceDriverType::PSCLASSIC: return "PS CLASSIC";
-        case DeviceDriverType::SWITCH: return "SWITCH";
-        case DeviceDriverType::WEBAPP: return "WEBAPP";
-        case DeviceDriverType::UART_BRIDGE: return "UART BRIDGE";
-        default: return "UNKNOWN";
-    }
-}
-
-#endif // _DEVICE_DRIVER_TYPES_H_
--- a/Firmware/ESP32/main/UserSettings/JoystickSettings.cpp
+++ b/Firmware/ESP32/main/UserSettings/JoystickSettings.cpp
@@ -1,63 +0,0 @@
-#include "Board/ogxm_log.h"
-#include "UserSettings/JoystickSettings.h"
-
-bool JoystickSettings::is_same(const JoystickSettingsRaw& raw) const
-{
-    return  dz_inner == Fix16(raw.dz_inner) &&
-            dz_outer == Fix16(raw.dz_outer) &&
-            anti_dz_circle == Fix16(raw.anti_dz_circle) &&
-            anti_dz_circle_y_scale == Fix16(raw.anti_dz_circle_y_scale) &&
-            anti_dz_square == Fix16(raw.anti_dz_square) &&
-            anti_dz_square_y_scale == Fix16(raw.anti_dz_square_y_scale) &&
-            anti_dz_angular == Fix16(raw.anti_dz_angular) &&
-            anti_dz_outer == Fix16(raw.anti_dz_outer) &&
-            axis_restrict == Fix16(raw.axis_restrict) &&
-            angle_restrict == Fix16(raw.angle_restrict) &&
-            diag_scale_min == Fix16(raw.diag_scale_min) &&
-            diag_scale_max == Fix16(raw.diag_scale_max) &&
-            curve == Fix16(raw.curve) &&
-            uncap_radius == raw.uncap_radius &&
-            invert_y == raw.invert_y &&
-            invert_x == raw.invert_x;
-}
-
-void JoystickSettings::set_from_raw(const JoystickSettingsRaw& raw)
-{
-    dz_inner = Fix16(raw.dz_inner);
-    dz_outer = Fix16(raw.dz_outer);
-    anti_dz_circle = Fix16(raw.anti_dz_circle);
-    anti_dz_circle_y_scale = Fix16(raw.anti_dz_circle_y_scale);
-    anti_dz_square = Fix16(raw.anti_dz_square);
-    anti_dz_square_y_scale = Fix16(raw.anti_dz_square_y_scale);
-    anti_dz_angular = Fix16(raw.anti_dz_angular);
-    anti_dz_outer = Fix16(raw.anti_dz_outer);
-    axis_restrict = Fix16(raw.axis_restrict);
-    angle_restrict = Fix16(raw.angle_restrict);
-    diag_scale_min = Fix16(raw.diag_scale_min);
-    diag_scale_max = Fix16(raw.diag_scale_max);
-    curve = Fix16(raw.curve);
-    uncap_radius = raw.uncap_radius;
-    invert_y = raw.invert_y;
-    invert_x = raw.invert_x;
-}
-
-void JoystickSettingsRaw::log_values()
-{
-    OGXM_LOG("dz_inner: %f\n", fix16_to_float(dz_inner));
-    OGXM_LOG_HEX("dz_inner: ", reinterpret_cast<uint8_t*>(&dz_inner), sizeof(dz_inner));
-    OGXM_LOG("dz_outer: %f\n", fix16_to_float(dz_outer));
-    OGXM_LOG("anti_dz_circle: %f\n", fix16_to_float(anti_dz_circle));
-    OGXM_LOG("anti_dz_circle_y_scale: %f\n", fix16_to_float(anti_dz_circle_y_scale));
-    OGXM_LOG("anti_dz_square: %f\n", fix16_to_float(anti_dz_square));
-    OGXM_LOG("anti_dz_square_y_scale: %f\n", fix16_to_float(anti_dz_square_y_scale));
-    OGXM_LOG("anti_dz_angular: %f\n", fix16_to_float(anti_dz_angular));
-    OGXM_LOG("anti_dz_outer: %f\n", fix16_to_float(anti_dz_outer));
-    OGXM_LOG("axis_restrict: %f\n", fix16_to_float(axis_restrict));
-    OGXM_LOG("angle_restrict: %f\n", fix16_to_float(angle_restrict));
-    OGXM_LOG("diag_scale_min: %f\n", fix16_to_float(diag_scale_min));
-    OGXM_LOG("diag_scale_max: %f\n", fix16_to_float(diag_scale_max));
-    OGXM_LOG("curve: %f\n", fix16_to_float(curve));
-    OGXM_LOG("uncap_radius: %d\n", uncap_radius);
-    OGXM_LOG("invert_y: %d\n", invert_y);
-    OGXM_LOG("invert_x: %d\n", invert_x);
-}
--- a/Firmware/ESP32/main/UserSettings/JoystickSettings.h
+++ b/Firmware/ESP32/main/UserSettings/JoystickSettings.h
@@ -1,68 +0,0 @@
-#ifndef _JOYSTICK_SETTINGS_H_
-#define _JOYSTICK_SETTINGS_H_
-
-#include <cstdint>
-
-#include "libfixmath/fix16.hpp"
-
-struct JoystickSettingsRaw;
-
-struct JoystickSettings
-{
-    Fix16 dz_inner{Fix16(0.0f)};
-    Fix16 dz_outer{Fix16(1.0f)};
-
-    Fix16 anti_dz_circle{Fix16(0.0f)};
-    Fix16 anti_dz_circle_y_scale{Fix16(0.0f)};
-    Fix16 anti_dz_square{Fix16(0.0f)};
-    Fix16 anti_dz_square_y_scale{Fix16(0.0f)};
-    Fix16 anti_dz_angular{Fix16(0.0f)};
-    Fix16 anti_dz_outer{Fix16(1.0f)};
-    
-    Fix16 axis_restrict{Fix16(0.0f)};
-    Fix16 angle_restrict{Fix16(0.0f)};
-    
-    Fix16 diag_scale_min{Fix16(1.0f)};
-    Fix16 diag_scale_max{Fix16(1.0f)};
-
-    Fix16 curve{Fix16(1.0f)};
-
-    bool uncap_radius{true};
-    bool invert_y{false};
-    bool invert_x{false};
-
-    bool is_same(const JoystickSettingsRaw& raw) const;
-    void set_from_raw(const JoystickSettingsRaw& raw);
-};
-
-#pragma pack(push, 1)
-struct JoystickSettingsRaw
-{
-    fix16_t dz_inner{fix16_from_int(0)};
-    fix16_t dz_outer{fix16_from_int(1)};
-
-    fix16_t anti_dz_circle{fix16_from_int(0)};
-    fix16_t anti_dz_circle_y_scale{fix16_from_int(0)};
-    fix16_t anti_dz_square{fix16_from_int(0)};
-    fix16_t anti_dz_square_y_scale{fix16_from_int(0)};
-    fix16_t anti_dz_angular{fix16_from_int(0)};
-    fix16_t anti_dz_outer{fix16_from_int(1)};
-    
-    fix16_t axis_restrict{fix16_from_int(0)};
-    fix16_t angle_restrict{fix16_from_int(0)};
-    
-    fix16_t diag_scale_min{fix16_from_int(1)};
-    fix16_t diag_scale_max{fix16_from_int(1)};
-
-    fix16_t curve{fix16_from_int(1)};
-
-    uint8_t uncap_radius{true};
-    uint8_t invert_y{false};
-    uint8_t invert_x{false};
-
-    void log_values();
-};
-static_assert(sizeof(JoystickSettingsRaw) == 55, "JoystickSettingsRaw is an unexpected size");
-#pragma pack(pop)
-
-#endif // _JOYSTICK_SETTINGS_H_
--- a/Firmware/ESP32/main/UserSettings/NVSHelper.h
+++ b/Firmware/ESP32/main/UserSettings/NVSHelper.h
@@ -1,113 +0,0 @@
-#ifndef _NVS_HELPER_H_
-#define _NVS_HELPER_H_
-
-#include <cstdint>
-#include <array>
-#include <string>
-#include <freertos/FreeRTOS.h>
-#include <freertos/semphr.h>
-#include <nvs_flash.h>
-#include <nvs.h>
-#include <esp_err.h>
-
-class NVSHelper
-{
-public:
-    static NVSHelper& get_instance()
-    {
-        static NVSHelper instance;
-        return instance;
-    }
-
-    esp_err_t write(const std::string& key, const void* value, size_t len)
-    {
-        esp_err_t err;
-        nvs_handle_t handle;
-
-        xSemaphoreTake(nvs_mutex_, portMAX_DELAY);
-
-        if ((err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle)) != ESP_OK) 
-        {
-            xSemaphoreGive(nvs_mutex_);
-            return err;
-        }
-
-        if ((err = nvs_set_blob(handle, key.c_str(), value, len)) != ESP_OK) 
-        {
-            nvs_close(handle);
-            xSemaphoreGive(nvs_mutex_);
-            return err;
-        } 
-
-        err = nvs_commit(handle);
-        nvs_close(handle);
-
-        xSemaphoreGive(nvs_mutex_);
-        return err;
-    }
-
-    esp_err_t read(const std::string& key, void* value, size_t len)
-    {
-        esp_err_t err;
-        nvs_handle_t handle;
-
-        xSemaphoreTake(nvs_mutex_, portMAX_DELAY);
-
-        if ((err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle)) != ESP_OK) 
-        {
-            xSemaphoreGive(nvs_mutex_);
-            return err;
-        }
-
-        err = nvs_get_blob(handle, key.c_str(), value, &len);
-        nvs_close(handle);
-
-        xSemaphoreGive(nvs_mutex_);
-        return err;
-    }
-
-    esp_err_t erase_all()
-    {
-        esp_err_t err;
-        nvs_handle_t handle;
-
-        xSemaphoreTake(nvs_mutex_, portMAX_DELAY);
-
-        if ((err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle)) != ESP_OK) 
-        {
-            xSemaphoreGive(nvs_mutex_);
-            return err;
-        }
-
-        err = nvs_erase_all(handle);
-        nvs_close(handle);
-
-        xSemaphoreGive(nvs_mutex_);
-        return err;
-    }
-
-private:
-    NVSHelper()
-    {
-        nvs_mutex_ = xSemaphoreCreateMutex();
-        xSemaphoreTake(nvs_mutex_, portMAX_DELAY);
-
-        if (nvs_flash_init() != ESP_OK) 
-        {
-            ESP_ERROR_CHECK(nvs_flash_erase());
-            ESP_ERROR_CHECK(nvs_flash_init());
-        }
-
-        xSemaphoreGive(nvs_mutex_);
-    }
-    ~NVSHelper() = default;
-    NVSHelper(const NVSHelper&) = delete;
-    NVSHelper& operator=(const NVSHelper&) = delete; 
-
-    SemaphoreHandle_t nvs_mutex_;
-
-    static constexpr char NVS_NAMESPACE[] = "user_data";
-    
-}; // class NVSHelper
-
-#endif // _NVS_HELPER_H_
--- a/Firmware/ESP32/main/UserSettings/TriggerSettings.cpp
+++ b/Firmware/ESP32/main/UserSettings/TriggerSettings.cpp
@@ -1,29 +0,0 @@
-#include "Board/ogxm_log.h"
-#include "UserSettings/TriggerSettings.h"
-
-bool TriggerSettings::is_same(const TriggerSettingsRaw& raw) const
-{
-    return  dz_inner == Fix16(raw.dz_inner) &&
-            dz_outer == Fix16(raw.dz_outer) &&
-            anti_dz_inner == Fix16(raw.anti_dz_inner) &&
-            anti_dz_outer == Fix16(raw.anti_dz_outer) &&
-            curve == Fix16(raw.curve);
-}
-
-void TriggerSettings::set_from_raw(const TriggerSettingsRaw& raw)
-{
-    dz_inner = Fix16(raw.dz_inner);
-    dz_outer = Fix16(raw.dz_outer);
-    anti_dz_inner = Fix16(raw.anti_dz_inner);
-    anti_dz_outer = Fix16(raw.anti_dz_outer);
-    curve = Fix16(raw.curve);
-}
-
-void TriggerSettingsRaw::log_values()
-{
-    OGXM_LOG("dz_inner: %f\n", fix16_to_float(dz_inner));
-    OGXM_LOG("dz_outer: %f\n", fix16_to_float(dz_outer));
-    OGXM_LOG("anti_dz_inner: %f\n", fix16_to_float(anti_dz_inner));
-    OGXM_LOG("anti_dz_outer: %f\n", fix16_to_float(anti_dz_outer));
-    OGXM_LOG("curve: %f\n", fix16_to_float(curve));
-}
--- a/Firmware/ESP32/main/UserSettings/TriggerSettings.h
+++ b/Firmware/ESP32/main/UserSettings/TriggerSettings.h
@@ -1,40 +0,0 @@
-#ifndef TRIGGER_SETTINGS_H
-#define TRIGGER_SETTINGS_H
-
-#include <cstdint>
-
-#include "libfixmath/fix16.hpp"
-
-struct TriggerSettingsRaw;
-
-struct TriggerSettings
-{
-    Fix16 dz_inner{Fix16(0.0f)};
-    Fix16 dz_outer{Fix16(1.0f)};
-
-    Fix16 anti_dz_inner{Fix16(0.0f)};
-    Fix16 anti_dz_outer{Fix16(1.0f)};
-
-    Fix16 curve{Fix16(1.0f)};
-
-    bool is_same(const TriggerSettingsRaw& raw) const;
-    void set_from_raw(const TriggerSettingsRaw& raw);
-};
-
-#pragma pack(push, 1)
-struct TriggerSettingsRaw
-{
-    fix16_t dz_inner{fix16_from_int(0)};
-    fix16_t dz_outer{fix16_from_int(1)};
-
-    fix16_t anti_dz_inner{fix16_from_int(0)};
-    fix16_t anti_dz_outer{fix16_from_int(1)};
-    
-    fix16_t curve{fix16_from_int(1)};
-
-    void log_values();
-};
-static_assert(sizeof(TriggerSettingsRaw) == 20, "TriggerSettingsRaw is an unexpected size");
-#pragma pack(pop)
-
-#endif // TRIGGER_SETTINGS_H
--- a/Firmware/ESP32/main/UserSettings/UserProfile.cpp
+++ b/Firmware/ESP32/main/UserSettings/UserProfile.cpp
@@ -1,40 +0,0 @@
-#include <cstring>
-
-#include "Gamepad/Gamepad.h"
-#include "UserSettings/UserProfile.h"
-
-UserProfile::UserProfile()
-{
-    id = 1;
-
-    dpad_up = Gamepad::DPAD_UP;
-    dpad_down = Gamepad::DPAD_DOWN;
-    dpad_left = Gamepad::DPAD_LEFT;
-    dpad_right = Gamepad::DPAD_RIGHT;
-
-    button_a = Gamepad::BUTTON_A;
-    button_b = Gamepad::BUTTON_B;
-    button_x = Gamepad::BUTTON_X;
-    button_y = Gamepad::BUTTON_Y;
-    button_l3 = Gamepad::BUTTON_L3;
-    button_r3 = Gamepad::BUTTON_R3;
-    button_back = Gamepad::BUTTON_BACK;
-    button_start = Gamepad::BUTTON_START;
-    button_lb = Gamepad::BUTTON_LB;
-    button_rb = Gamepad::BUTTON_RB;
-    button_sys = Gamepad::BUTTON_SYS;
-    button_misc = Gamepad::BUTTON_MISC;
-
-    analog_enabled = 0;
-
-    analog_off_up = Gamepad::ANALOG_OFF_UP;
-    analog_off_down = Gamepad::ANALOG_OFF_DOWN;
-    analog_off_left = Gamepad::ANALOG_OFF_LEFT;
-    analog_off_right = Gamepad::ANALOG_OFF_RIGHT;
-    analog_off_a = Gamepad::ANALOG_OFF_A;
-    analog_off_b = Gamepad::ANALOG_OFF_B;
-    analog_off_x = Gamepad::ANALOG_OFF_X;
-    analog_off_y = Gamepad::ANALOG_OFF_Y;
-    analog_off_lb = Gamepad::ANALOG_OFF_LB;
-    analog_off_rb = Gamepad::ANALOG_OFF_RB;
-}
--- a/Firmware/ESP32/main/UserSettings/UserProfile.h
+++ b/Firmware/ESP32/main/UserSettings/UserProfile.h
@@ -1,55 +0,0 @@
-#ifndef _USER_PROFILE_H_
-#define _USER_PROFILE_H_
-
-#include <cstdint>
-
-#include "UserSettings/JoystickSettings.h"
-#include "UserSettings/TriggerSettings.h"   
-
-#pragma pack(push, 1)
-struct UserProfile
-{
-    uint8_t id;
-
-    JoystickSettingsRaw joystick_settings_l;
-    JoystickSettingsRaw joystick_settings_r;
-    TriggerSettingsRaw trigger_settings_l;
-    TriggerSettingsRaw trigger_settings_r;
-
-    uint8_t dpad_up;
-    uint8_t dpad_down;
-    uint8_t dpad_left;
-    uint8_t dpad_right;
-
-    uint16_t button_a;
-    uint16_t button_b;
-    uint16_t button_x;
-    uint16_t button_y;
-    uint16_t button_l3;
-    uint16_t button_r3;
-    uint16_t button_back;
-    uint16_t button_start;
-    uint16_t button_lb;
-    uint16_t button_rb;
-    uint16_t button_sys;
-    uint16_t button_misc;
-
-    uint8_t analog_enabled;
-
-    uint8_t analog_off_up;
-    uint8_t analog_off_down;
-    uint8_t analog_off_left;
-    uint8_t analog_off_right;
-    uint8_t analog_off_a;
-    uint8_t analog_off_b;
-    uint8_t analog_off_x;
-    uint8_t analog_off_y;
-    uint8_t analog_off_lb;
-    uint8_t analog_off_rb;
-
-    UserProfile();
-};
-static_assert(sizeof(UserProfile) == 190, "UserProfile struct size mismatch");
-#pragma pack(pop)
-
-#endif // _USER_PROFILE_H_
--- a/Firmware/ESP32/main/UserSettings/UserSettings.cpp
+++ b/Firmware/ESP32/main/UserSettings/UserSettings.cpp
@@ -1,273 +0,0 @@
-#include <cstring>
-#include <array>
-#include <string>
-#include <nvs_flash.h>
-#include <nvs.h>
-#include <esp_err.h>
-#include <esp_log.h>
-
-#include "Gamepad/Gamepad.h"
-#include "UserSettings/NVSHelper.h"
-
-static constexpr uint32_t BUTTON_COMBO(const uint16_t& buttons, const uint8_t& dpad = 0)
-{
-    return (static_cast<uint32_t>(buttons) << 16) | static_cast<uint32_t>(dpad);
-}
-
-namespace ButtonCombo 
-{
-    static constexpr uint32_t PS3       = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_LEFT);
-    static constexpr uint32_t DINPUT    = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_RB, Gamepad::DPAD_LEFT);
-    static constexpr uint32_t XINPUT    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_UP);
-    static constexpr uint32_t SWITCH    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_DOWN);
-    static constexpr uint32_t XBOXOG    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_RIGHT);
-    static constexpr uint32_t XBOXOG_SB = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_RB, Gamepad::DPAD_RIGHT);
-    static constexpr uint32_t XBOXOG_XR = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_LB, Gamepad::DPAD_RIGHT);
-    static constexpr uint32_t PSCLASSIC = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_A);
-    static constexpr uint32_t WEBAPP    = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_LB | Gamepad::BUTTON_RB);
-};
-
-static constexpr DeviceDriverType VALID_DRIVER_TYPES[] =
-{
-#if MAX_GAMEPADS > 1
-    DeviceDriverType::DINPUT, 
-    DeviceDriverType::SWITCH, 
-
-#else // MAX_GAMEPADS == 1
-    DeviceDriverType::XBOXOG, 
-    // DeviceDriverType::XBOXOG_SB, 
-    DeviceDriverType::XBOXOG_XR,
-    DeviceDriverType::DINPUT, 
-    DeviceDriverType::SWITCH, 
-    DeviceDriverType::PS3,
-    DeviceDriverType::PSCLASSIC, 
-    DeviceDriverType::XINPUT,
-
-#endif
-};
-
-struct ComboMap { uint32_t combo; DeviceDriverType driver; };
-static constexpr std::array<ComboMap, 9> BUTTON_COMBO_MAP = 
-{{
-    { ButtonCombo::XBOXOG,    DeviceDriverType::XBOXOG    },
-    { ButtonCombo::XBOXOG_SB, DeviceDriverType::XBOXOG_SB },
-    { ButtonCombo::XBOXOG_XR, DeviceDriverType::XBOXOG_XR },
-    { ButtonCombo::WEBAPP,    DeviceDriverType::WEBAPP    },
-    { ButtonCombo::DINPUT,    DeviceDriverType::DINPUT    },
-    { ButtonCombo::SWITCH,    DeviceDriverType::SWITCH    },
-    { ButtonCombo::XINPUT,    DeviceDriverType::XINPUT    },
-    { ButtonCombo::PS3,       DeviceDriverType::PS3       },
-    { ButtonCombo::PSCLASSIC, DeviceDriverType::PSCLASSIC }
-}};
-
-const std::string UserSettings::INIT_FLAG_KEY()
-{
-    return std::string("init_flag");
-}
-
-const std::string UserSettings::PROFILE_KEY(const uint8_t profile_id)
-{
-    return std::string("profile_") + std::to_string(profile_id);
-}
-
-const std::string UserSettings::ACTIVE_PROFILE_KEY(const uint8_t index)
-{
-    return std::string("active_id_") + std::to_string(index);
-}
-
-const std::string UserSettings::DRIVER_TYPE_KEY()
-{
-    return std::string("driver_type");
-}
-
-const std::string UserSettings::FIRMWARE_VER_KEY()
-{
-    return std::string("firmware_ver");
-}
-
-void UserSettings::initialize_flash()
-{
-    ESP_LOGD("UserSettings", "Checking for UserSettings init flag");
-
-    uint8_t init_flag = 0;
-
-    ESP_ERROR_CHECK(nvs_helper_.read(INIT_FLAG_KEY(), &init_flag, sizeof(init_flag)));
-
-    if (init_flag == INIT_FLAG)
-    {
-        ESP_LOGD("UserSettings", "UserSettings already initialized");
-        return;
-    }
-
-    ESP_ERROR_CHECK(nvs_helper_.erase_all());
-    OGXM_LOG("Initializing UserSettings\n");
-
-    current_driver_ = DEFAULT_DRIVER();
-    uint8_t driver_type = static_cast<uint8_t>(current_driver_);
-    ESP_ERROR_CHECK(nvs_helper_.write(DRIVER_TYPE_KEY(), &driver_type, sizeof(driver_type)));
-
-    uint8_t active_id = 1;
-    for (uint8_t i = 0; i < MAX_GAMEPADS; i++)
-    {
-        ESP_ERROR_CHECK(nvs_helper_.write(ACTIVE_PROFILE_KEY(i), &active_id, sizeof(active_id)));
-    }
-
-    UserProfile profile = UserProfile();
-    for (uint8_t i = 0; i < MAX_PROFILES; i++)
-    {
-        profile.id = i + 1;
-        ESP_ERROR_CHECK(nvs_helper_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile)));
-    }
-
-    init_flag = INIT_FLAG;
-    ESP_ERROR_CHECK(nvs_helper_.write(INIT_FLAG_KEY(), &init_flag, sizeof(init_flag)));
-}
-
-DeviceDriverType UserSettings::get_current_driver()
-{
-    if (current_driver_ != DeviceDriverType::NONE)
-    {
-        return current_driver_;
-    }
-
-    uint8_t stored_type = 0;
-    nvs_helper_.read(DRIVER_TYPE_KEY(), &stored_type, sizeof(stored_type));
-
-    if (is_valid_driver(static_cast<DeviceDriverType>(stored_type)))
-    {
-        current_driver_ = static_cast<DeviceDriverType>(stored_type);
-        return current_driver_;
-    }
-
-    current_driver_ = DEFAULT_DRIVER();
-    return current_driver_;
-}
-
-//Checks if button combo has been held for 3 seconds, returns true if mode has been changed
-bool UserSettings::check_for_driver_change(const I2CDriver::PacketIn& packet_in)
-{
-    static uint32_t last_button_combo = BUTTON_COMBO(packet_in.buttons, packet_in.dpad);
-    static uint8_t call_count = 0;
-
-    uint32_t current_button_combo = BUTTON_COMBO(packet_in.buttons, packet_in.dpad);
-
-    if (!(current_button_combo & (static_cast<uint32_t>(Gamepad::BUTTON_START) << 16)) || 
-        last_button_combo != current_button_combo)
-    {
-        last_button_combo = current_button_combo;
-        call_count = 0;
-        return false;
-    }
-
-    ++call_count;
-
-    if (call_count < GP_CHECK_COUNT)
-    {
-        return false;
-    }
-
-    call_count = 0;
-
-    DeviceDriverType new_driver = DeviceDriverType::NONE;
-
-    for (const auto& combo : BUTTON_COMBO_MAP)
-    {
-        if (combo.combo == current_button_combo &&
-            is_valid_driver(combo.driver))
-        {
-            new_driver = combo.driver;
-            break;
-        }
-    }
-
-    if (new_driver == DeviceDriverType::NONE || new_driver == current_driver_)
-    {
-        return false;
-    }
-
-    current_driver_ = new_driver;
-    return true;
-}
-
-void UserSettings::store_driver_type(DeviceDriverType new_driver_type)
-{
-    if (!is_valid_driver(new_driver_type))
-    {
-        return;
-    }
-
-    uint8_t new_driver = static_cast<uint8_t>(new_driver_type);
-    nvs_helper_.write(DRIVER_TYPE_KEY(), &new_driver, sizeof(new_driver));
-}
-
-void UserSettings::store_profile(const uint8_t index, UserProfile& profile)
-{
-    if (index > MAX_GAMEPADS || profile.id < 1 || profile.id > MAX_PROFILES)
-    {
-        return;
-    }
-
-    OGXM_LOG("Storing profile %d for gamepad %d\n", profile.id, index);
-
-    if (nvs_helper_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile)) == ESP_OK)
-    {
-        OGXM_LOG("Profile %d stored successfully\n", profile.id);
-
-        nvs_helper_.write(ACTIVE_PROFILE_KEY(index), &profile.id, sizeof(profile.id));
-    }
-}
-
-void UserSettings::store_profile_and_driver_type(DeviceDriverType new_driver_type, const uint8_t index, UserProfile& profile)
-{
-    store_driver_type(new_driver_type);
-    store_profile(index, profile);
-}
-
-uint8_t UserSettings::get_active_profile_id(const uint8_t index)
-{
-    uint8_t read_profile_id = 0;
-
-    if (nvs_helper_.read(ACTIVE_PROFILE_KEY(index), &read_profile_id, sizeof(read_profile_id)) != ESP_OK ||
-        read_profile_id < 1 || 
-        read_profile_id > MAX_PROFILES)
-    {
-        return 0x01;
-    }
-    return read_profile_id;
-}
-
-UserProfile UserSettings::get_profile_by_index(const uint8_t index)
-{
-    return get_profile_by_id(get_active_profile_id(index));
-}
-
-UserProfile UserSettings::get_profile_by_id(const uint8_t profile_id)
-{
-    UserProfile profile;
-
-    if (profile_id < 1 || 
-        profile_id > MAX_PROFILES ||
-        nvs_helper_.read(PROFILE_KEY(profile_id), &profile, sizeof(UserProfile)) != ESP_OK)
-    {
-        return UserProfile();
-    }
-
-    return profile;
-}
-
-DeviceDriverType UserSettings::DEFAULT_DRIVER()
-{
-    return VALID_DRIVER_TYPES[0];
-}
-
-bool UserSettings::is_valid_driver(DeviceDriverType mode)
-{
-    for (const auto& driver : VALID_DRIVER_TYPES)
-    {
-        if (mode == driver)
-        {
-            return true;
-        }
-    }
-    return false;
-}
--- a/Firmware/ESP32/main/UserSettings/UserSettings.h
+++ b/Firmware/ESP32/main/UserSettings/UserSettings.h
@@ -1,60 +0,0 @@
-#ifndef _USER_SETTINGS_H_
-#define _USER_SETTINGS_H_
-
-#include <cstdint>
-#include <atomic>
-
-#include "sdkconfig.h"
-#include "I2CDriver/I2CDriver.h"
-#include "UserSettings/UserProfile.h"
-#include "UserSettings/DeviceDriverTypes.h"
-#include "UserSettings/NVSHelper.h"
-
-class UserSettings
-{
-public:
-    static constexpr uint8_t MAX_PROFILES = 8;
-    static constexpr uint32_t GP_CHECK_DELAY_MS = 1000;
-
-    static UserSettings& get_instance()
-    {
-        static UserSettings instance;
-        return instance;
-    }
-
-    void initialize_flash();
-
-    DeviceDriverType get_current_driver();
-    bool check_for_driver_change(const I2CDriver::PacketIn& packet_in);
-    
-    UserProfile get_profile_by_index(const uint8_t index);
-    UserProfile get_profile_by_id(const uint8_t profile_id);
-    uint8_t get_active_profile_id(const uint8_t index);
-
-    void store_driver_type(DeviceDriverType new_driver_type);
-    void store_profile(uint8_t index, UserProfile& profile);
-    void store_profile_and_driver_type(DeviceDriverType new_driver_type, uint8_t index, UserProfile& profile);
-
-private:
-    UserSettings() = default;
-    ~UserSettings() = default;
-    UserSettings(const UserSettings&) = delete;
-    UserSettings& operator=(const UserSettings&) = delete;
-
-    static constexpr uint8_t GP_CHECK_COUNT = 3000 / GP_CHECK_DELAY_MS;
-    static constexpr uint8_t INIT_FLAG = 0x12;
-
-    NVSHelper& nvs_helper_{NVSHelper::get_instance()};
-    DeviceDriverType current_driver_{DeviceDriverType::NONE};
-
-    bool is_valid_driver(DeviceDriverType mode);
-
-    DeviceDriverType DEFAULT_DRIVER();
-    const std::string INIT_FLAG_KEY();
-    const std::string PROFILE_KEY(const uint8_t profile_id);
-    const std::string ACTIVE_PROFILE_KEY(const uint8_t index);
-    const std::string DRIVER_TYPE_KEY();
-    const std::string FIRMWARE_VER_KEY();
-};
-
-#endif // _USER_SETTINGS_H_
--- a/Firmware/ESP32/main/bluetooth/ble_server.c
+++ b/Firmware/ESP32/main/bluetooth/ble_server.c
--- a/Firmware/ESP32/main/bluetooth/ble_server.h
+++ b/Firmware/ESP32/main/bluetooth/ble_server.h
--- a/Firmware/ESP32/main/bluetooth/bluetooth.c
+++ b/Firmware/ESP32/main/bluetooth/bluetooth.c
@@ -0,0 +1,330 @@
+#include <stdlib.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+#include "btstack_port_esp32.h"
+#include "btstack_run_loop.h"
+#include "btstack_stdio_esp32.h"
+#include "uni.h"
+#include "sdkconfig.h"
+#include "bluetooth/bluetooth.h"
+#include "bluetooth/ring_rumble.h"
+#include "led/led.h"
+#include "log/log.h"
+#include "gamepad/range.h"
+#include "settings/settings.h"
+
+#define LED_INTERVAL_MS         500U
+#define JOY_DIF_THRESHOLD       0x50
+#define TRIGGER_DIF_THRESHOLD   0x50
+
+typedef struct {
+    volatile bool connected;
+    gamepad_pad_t report;
+    uni_gamepad_t prev_uni;
+    struct {
+        bool joy_l;
+        bool joy_r;
+        bool trigger_l;
+        bool trigger_r;
+    } map;
+} gp_context_t;
+
+typedef struct {
+    bool                    led_state;
+    bool                    led_timer_running;
+    btstack_timer_source_t  led_timer;
+    gp_context_t            gp_ctx[GAMEPADS_MAX];
+    bt_connect_cb_t         connect_cb;
+    bt_gamepad_cb_t         gamepad_cb;
+    ring_rumble_t           rumble_ring;
+    btstack_context_callback_registration_t rumble_cb_reg;
+} bt_context_t;
+
+static bt_context_t bt_context = {0};
+
+static inline bool any_connected(void) {
+    for (int i = 0; i < GAMEPADS_MAX; i++) {
+        if (bt_context.gp_ctx[i].connected) {
+            return true;
+        }
+    }
+    return false;
+}
+
+static void led_blink_cb(btstack_timer_source_t *ts) {
+    bt_context.led_state = !bt_context.led_state;
+    led_set(0, bt_context.led_state);
+    btstack_run_loop_set_timer(ts, LED_INTERVAL_MS);
+    btstack_run_loop_add_timer(ts);
+}
+
+static void led_blink_set_enabled(bool enable) {
+    if (enable && !bt_context.led_timer_running) {
+        bt_context.led_timer_running = true;
+        bt_context.led_timer.process = led_blink_cb;
+        btstack_run_loop_set_timer(&bt_context.led_timer, LED_INTERVAL_MS);
+        btstack_run_loop_add_timer(&bt_context.led_timer);
+    } else if (!enable && bt_context.led_timer_running) {
+        bt_context.led_timer_running = false;
+        btstack_run_loop_remove_timer(&bt_context.led_timer);
+    }
+}
+
+static void set_rumble_cb(void* ctx) {
+    uint8_t index = 0;
+    gamepad_rumble_t rumble = {0};
+    if (!ring_rumble_pop(&bt_context.rumble_ring, &index, &rumble)) {
+        return;
+    }
+    uni_hid_device_t* bp_dev = uni_hid_device_get_instance_for_idx(index);
+    if ((bp_dev == NULL) || !bt_context.gp_ctx[index].connected || 
+        (bp_dev->report_parser.play_dual_rumble == NULL)) {
+        return;
+    }
+    uint8_t duration = 100;
+    if ((rumble.l_duration > 0) || (rumble.r_duration > 0)) {
+        duration = MAX(rumble.l_duration, rumble.r_duration);
+    } 
+    else if (bp_dev->controller_type == CONTROLLER_TYPE_XBoxOneController) {
+        duration += 10;
+    }
+    bp_dev->report_parser.play_dual_rumble(bp_dev, 0, duration, rumble.l, rumble.r);
+
+    if (!ring_rumble_empty(&bt_context.rumble_ring)) {
+        bt_context.rumble_cb_reg.callback = set_rumble_cb;
+        bt_context.rumble_cb_reg.context = NULL;
+        btstack_run_loop_execute_on_main_thread(&bt_context.rumble_cb_reg);
+    }
+}
+
+/* ---- Bluepad32 driver ---- */
+
+static void bp32_init_cb(int argc, const char** arg_V) {
+    (void)argc;
+    (void)arg_V;
+}
+
+static void bp32_init_complete_cb(void) {
+    // uni_bt_enable_new_connections_unsafe(true);
+    uni_bt_start_scanning_and_autoconnect_unsafe();
+    // uni_bt_del_keys_unsafe();
+    uni_property_dump_all();
+}
+
+static uni_error_t bp32_device_discovered_cb(bd_addr_t addr, const char* name, 
+                                             uint16_t cod, uint8_t rssi) {
+    (void)rssi;
+    (void)name;
+    (void)addr;
+
+    if (((cod & UNI_BT_COD_MINOR_MASK) & UNI_BT_COD_MINOR_GAMEPAD) == 0) {
+        return UNI_ERROR_IGNORE_DEVICE;
+    }
+    return UNI_ERROR_SUCCESS;
+}
+
+static void bp32_device_connected_cb(uni_hid_device_t* device) {
+    (void)device;
+}
+
+static uni_error_t bp32_device_ready_cb(uni_hid_device_t* device) {
+    int index = uni_hid_device_get_idx_for_instance(device);
+    if (index < 0 || index >= GAMEPADS_MAX) {
+        return UNI_ERROR_INVALID_CONTROLLER;
+    }
+    bt_context.gp_ctx[index].connected = true;
+    led_blink_set_enabled(false);
+    led_set(index, true);
+    if (bt_context.connect_cb) {
+        bt_context.connect_cb(index, true);
+    }
+    return UNI_ERROR_SUCCESS;
+}
+
+static void bp32_device_disconnect_cb(uni_hid_device_t* device) {
+    int index = uni_hid_device_get_idx_for_instance(device);
+    if (index < 0 || index >= GAMEPADS_MAX) {
+        return;
+    }
+    bt_context.gp_ctx[index].connected = false;
+    if (!any_connected()) {
+        led_blink_set_enabled(true);
+    }
+    if (bt_context.connect_cb) {
+        bt_context.connect_cb(index, false);
+    }
+}
+
+static void bp32_oob_event_cb(uni_platform_oob_event_t event, void* data) {
+    (void)data;
+    (void)event;
+}
+
+// Prevent I2C/SPI bus flooding
+static inline bool gp_state_change(const uni_gamepad_t* uni_a, const uni_gamepad_t* uni_b) {
+    if (memcmp(uni_a, uni_b, sizeof(uni_gamepad_t)) == 0) {
+        return false;
+    }
+    if ((uni_a->dpad != uni_b->dpad) || 
+        (uni_a->buttons != uni_b->buttons) ||
+        (uni_a->misc_buttons != uni_b->misc_buttons)) {
+        return true;
+    }
+    if ((abs(uni_a->brake - uni_b->brake) > TRIGGER_DIF_THRESHOLD)  ||
+        (uni_a->brake <= 0) || (uni_a->brake >= R_UINT10_MAX)) {
+        return true;
+    }
+    if ((abs(uni_a->throttle - uni_b->throttle) > TRIGGER_DIF_THRESHOLD) ||
+        (uni_a->throttle <= 0) || (uni_a->throttle >= R_UINT10_MAX)) {
+        return true;
+    }
+    if ((abs(uni_a->axis_x - uni_b->axis_x) > JOY_DIF_THRESHOLD) ||
+        (uni_a->axis_x <= R_INT10_MIN) || (uni_a->axis_x >= R_INT10_MAX)) {
+        return true;
+    }
+    if ((abs(uni_a->axis_y - uni_b->axis_y) > JOY_DIF_THRESHOLD) ||
+        (uni_a->axis_y <= R_INT10_MIN) || (uni_a->axis_y >= R_INT10_MAX)) {
+        return true;
+    }
+    if ((abs(uni_a->axis_rx - uni_b->axis_rx) > JOY_DIF_THRESHOLD) ||
+        (uni_a->axis_rx <= R_INT10_MIN) || (uni_a->axis_rx >= R_INT10_MAX)) {   
+        return true;
+    }
+    if ((abs(uni_a->axis_ry - uni_b->axis_ry) > JOY_DIF_THRESHOLD) ||
+        (uni_a->axis_ry <= R_INT10_MIN) || (uni_a->axis_ry >= R_INT10_MAX)) {   
+        return true;
+    }
+    return false;
+}
+
+static void bp32_controller_data_cb(uni_hid_device_t* device, uni_controller_t* controller) {
+    if (controller->klass != UNI_CONTROLLER_CLASS_GAMEPAD){
+        return;
+    }
+
+    const uni_gamepad_t* uni = &controller->gamepad;
+    int index = uni_hid_device_get_idx_for_instance(device);
+
+    if ((index < 0) || (index >= GAMEPADS_MAX) ||
+        !gp_state_change(uni, &bt_context.gp_ctx[index].prev_uni)) {
+        return;
+    }
+
+    gamepad_pad_t* pad = &bt_context.gp_ctx[index].report;
+    memset(pad, 0, sizeof(gamepad_pad_t));
+
+    switch (uni->dpad) {
+        case DPAD_UP:
+            pad->dpad |= GAMEPAD_D_UP;
+            break;
+        case DPAD_DOWN:
+            pad->dpad |= GAMEPAD_D_DOWN;
+            break;
+        case DPAD_LEFT:
+            pad->dpad |= GAMEPAD_D_LEFT;
+            break;
+        case DPAD_RIGHT:
+            pad->dpad |= GAMEPAD_D_RIGHT;
+            break;
+        case DPAD_UP | DPAD_RIGHT:
+            pad->dpad |= (GAMEPAD_D_UP | GAMEPAD_D_RIGHT);
+            break;
+        case DPAD_DOWN | DPAD_RIGHT:
+            pad->dpad |= (GAMEPAD_D_DOWN | GAMEPAD_D_RIGHT);
+            break;
+        case DPAD_DOWN | DPAD_LEFT:
+            pad->dpad |= (GAMEPAD_D_DOWN | GAMEPAD_D_LEFT);
+            break;
+        case DPAD_UP | DPAD_LEFT:
+            pad->dpad |= (GAMEPAD_D_UP | GAMEPAD_D_LEFT);
+            break;
+        default:
+            break;
+    }
+
+    if (uni->buttons & BUTTON_A)                { pad->buttons |= GAMEPAD_BTN_A; }
+    if (uni->buttons & BUTTON_B)                { pad->buttons |= GAMEPAD_BTN_B; }
+    if (uni->buttons & BUTTON_X)                { pad->buttons |= GAMEPAD_BTN_X; }
+    if (uni->buttons & BUTTON_Y)                { pad->buttons |= GAMEPAD_BTN_Y; }
+    if (uni->buttons & BUTTON_SHOULDER_L)       { pad->buttons |= GAMEPAD_BTN_LB; }
+    if (uni->buttons & BUTTON_SHOULDER_R)       { pad->buttons |= GAMEPAD_BTN_RB; }
+    if (uni->buttons & BUTTON_THUMB_L)          { pad->buttons |= GAMEPAD_BTN_L3; }
+    if (uni->buttons & BUTTON_THUMB_R)          { pad->buttons |= GAMEPAD_BTN_R3; }
+    if (uni->misc_buttons & MISC_BUTTON_BACK)   { pad->buttons |= GAMEPAD_BTN_BACK; }
+    if (uni->misc_buttons & MISC_BUTTON_START)  { pad->buttons |= GAMEPAD_BTN_START; }
+    if (uni->misc_buttons & MISC_BUTTON_SYSTEM) { pad->buttons |= GAMEPAD_BTN_SYS; }
+
+    pad->trigger_l = range_uint10_to_uint8(uni->brake);
+    pad->trigger_r = range_uint10_to_uint8(uni->throttle);
+
+    pad->joystick_lx = range_int10_to_int16(uni->axis_x);
+    pad->joystick_ly = range_invert_int16(range_int10_to_int16(uni->axis_y));
+    pad->joystick_rx = range_int10_to_int16(uni->axis_rx);
+    pad->joystick_ry = range_invert_int16(range_int10_to_int16(uni->axis_ry));
+
+    if (bt_context.gamepad_cb != NULL) {
+        bt_context.gamepad_cb(index, pad, GAMEPAD_FLAG_IN_PAD);
+    }
+    memcpy(&bt_context.gp_ctx[index].prev_uni, uni, sizeof(uni_gamepad_t));
+}
+
+static const uni_property_t* bp32_get_property_cb(uni_property_idx_t idx) {
+    (void)idx;
+    return NULL;
+}
+
+static struct uni_platform BP32_DRIVER = {
+    .name = "OGX-Mini",
+    .init = bp32_init_cb,
+    .on_init_complete = bp32_init_complete_cb,
+    .on_device_discovered = bp32_device_discovered_cb,
+    .on_device_connected = bp32_device_connected_cb,
+    .on_device_disconnected = bp32_device_disconnect_cb,
+    .on_device_ready = bp32_device_ready_cb,
+    .on_controller_data = bp32_controller_data_cb,
+    .get_property = bp32_get_property_cb,
+    .on_oob_event = bp32_oob_event_cb,
+};
+
+/* ---- Public ---- */
+
+void bluetooth_set_rumble(uint8_t index, const gamepad_rumble_t* rumble) {
+    if ((index >= GAMEPADS_MAX) || 
+        !bt_context.gp_ctx[index].connected || 
+        (rumble == NULL)) {
+        return;
+    }
+    ring_rumble_push(&bt_context.rumble_ring, index, rumble);
+    bt_context.rumble_cb_reg.callback = set_rumble_cb;
+    bt_context.rumble_cb_reg.context = NULL;
+    btstack_run_loop_execute_on_main_thread(&bt_context.rumble_cb_reg);
+}
+
+void bluetooth_set_audio(uint8_t index, const gamepad_pcm_in_t* pcm) {
+    (void)index;
+    (void)pcm;
+    // Bluetooth audio handling is not implemented in this version
+}
+
+void bluetooth_config(bt_connect_cb_t connect_cb, bt_gamepad_cb_t gamepad_cb, bt_audio_cb_t audio_cb) {
+    (void)audio_cb;
+    memset(&bt_context, 0, sizeof(bt_context_t));
+    bt_context.connect_cb = connect_cb;
+    bt_context.gamepad_cb = gamepad_cb;
+}
+
+void bluetooth_task(void* args) {
+    (void)args;
+
+    btstack_init();
+
+    // ble_server_init();
+
+    uni_platform_set_custom(&BP32_DRIVER);
+    uni_init(0, NULL);
+
+    led_init();
+    led_blink_set_enabled(true);
+
+    btstack_run_loop_execute();
+}
--- a/Firmware/ESP32/main/bluetooth/bluetooth.h
+++ b/Firmware/ESP32/main/bluetooth/bluetooth.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "gamepad/gamepad.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef void (*bt_connect_cb_t)(uint8_t index, bool connected);
+typedef void (*bt_gamepad_cb_t)(uint8_t index, const gamepad_pad_t* pad, uint32_t flags);
+typedef void (*bt_audio_cb_t)(uint8_t index, const gamepad_pcm_out_t* pcm);
+
+void bluetooth_config(bt_connect_cb_t connect_cb, bt_gamepad_cb_t gamepad_cb, bt_audio_cb_t audio_cb);
+void bluetooth_task(void* args);
+void bluetooth_set_rumble(uint8_t index, const gamepad_rumble_t* rumble);
+void bluetooth_set_audio(uint8_t index, const gamepad_pcm_in_t* pcm);
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/bluetooth/ring_rumble.h
+++ b/Firmware/ESP32/main/bluetooth/ring_rumble.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <stdatomic.h>
+#include "gamepad/gamepad.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define RUBMLE_RING_SIZE 8U
+
+typedef struct {
+    gamepad_rumble_t buf[RUBMLE_RING_SIZE] __attribute__((aligned(4)));
+    uint8_t index[RUBMLE_RING_SIZE];
+    atomic_uint head;
+    atomic_uint tail;
+} ring_rumble_t;
+
+static inline bool ring_rumble_push(ring_rumble_t* ring, uint8_t index, const gamepad_rumble_t* rumble) {
+    uint32_t head = atomic_load_explicit(&ring->head, memory_order_relaxed);
+    uint32_t next_head = (head + 1) % RUBMLE_RING_SIZE;
+    uint32_t tail = atomic_load_explicit(&ring->tail, memory_order_acquire);
+
+    if (next_head == tail) {
+        atomic_store_explicit(&ring->tail, (tail + 1) % RUBMLE_RING_SIZE, memory_order_release);
+    }
+    ring->index[head] = index;
+    memcpy(&ring->buf[head], rumble, sizeof(gamepad_rumble_t));
+    atomic_store_explicit(&ring->head, next_head, memory_order_release);
+    return true;
+}
+
+static inline bool ring_rumble_pop(ring_rumble_t* ring, uint8_t* index, gamepad_rumble_t* rumble) {
+    uint32_t tail = atomic_load_explicit(&ring->tail, memory_order_relaxed);
+    uint32_t head = atomic_load_explicit(&ring->head, memory_order_acquire);
+    if (tail == head) {
+        return false;
+    }
+    *index = ring->index[tail];
+    memcpy(rumble, &ring->buf[tail], sizeof(gamepad_rumble_t));
+    atomic_store_explicit(&ring->tail, (tail + 1) % RUBMLE_RING_SIZE, memory_order_release);
+    return true;
+}
+
+static inline bool ring_rumble_empty(const ring_rumble_t* ring) {
+    return (atomic_load_explicit(&ring->head, memory_order_relaxed) ==
+            atomic_load_explicit(&ring->tail, memory_order_acquire));
+}
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/gamepad/gamepad.h
+++ b/Firmware/ESP32/main/gamepad/gamepad.h
@@ -0,0 +1,125 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "sdkconfig.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef GAMEPADS_MAX
+#define GAMEPADS_MAX CONFIG_BLUEPAD32_MAX_DEVICES
+#endif
+
+#define GAMPAD_PCM_FRAME_SIZE       16U
+
+typedef enum {
+    GAMEPAD_DPAD_BIT_UP = 0,
+    GAMEPAD_DPAD_BIT_DOWN,
+    GAMEPAD_DPAD_BIT_LEFT,
+    GAMEPAD_DPAD_BIT_RIGHT,
+    GAMEPAD_DPAD_BIT_COUNT,
+    GAMEPAD_DPAD_BIT_NONE = GAMEPAD_DPAD_BIT_COUNT,
+} gamepad_dpad_bit_t;
+
+typedef enum {
+    GAMEPAD_BTN_BIT_A = 0,
+    GAMEPAD_BTN_BIT_B,
+    GAMEPAD_BTN_BIT_X,
+    GAMEPAD_BTN_BIT_Y,
+    GAMEPAD_BTN_BIT_L3,
+    GAMEPAD_BTN_BIT_R3,
+    GAMEPAD_BTN_BIT_LB,
+    GAMEPAD_BTN_BIT_RB,
+    GAMEPAD_BTN_BIT_LT,
+    GAMEPAD_BTN_BIT_RT,
+    GAMEPAD_BTN_BIT_BACK,
+    GAMEPAD_BTN_BIT_START,
+    GAMEPAD_BTN_BIT_SYS,
+    GAMEPAD_BTN_BIT_MISC,
+    GAMEPAD_BTN_BIT_COUNT,
+    GAMEPAD_BTN_BIT_NONE = GAMEPAD_BTN_BIT_COUNT,
+} gamepad_btn_bit_t;
+
+typedef enum {
+    GAMEPAD_ANALOG_UP = 0,
+    GAMEPAD_ANALOG_DOWN,
+    GAMEPAD_ANALOG_LEFT,
+    GAMEPAD_ANALOG_RIGHT,
+    GAMEPAD_ANALOG_A,
+    GAMEPAD_ANALOG_B,
+    GAMEPAD_ANALOG_X,
+    GAMEPAD_ANALOG_Y,
+    GAMEPAD_ANALOG_LB,
+    GAMEPAD_ANALOG_RB,
+    GAMEPAD_ANALOG_COUNT
+} gampead_analog_t;
+
+#define GP_BIT8(bit)           ((uint8_t)1U << (bit))
+#define GP_BIT16(bit)          ((uint16_t)1U << (bit))
+
+#define GAMEPAD_D_UP        GP_BIT8(GAMEPAD_DPAD_BIT_UP)
+#define GAMEPAD_D_DOWN      GP_BIT8(GAMEPAD_DPAD_BIT_DOWN)
+#define GAMEPAD_D_LEFT      GP_BIT8(GAMEPAD_DPAD_BIT_LEFT)
+#define GAMEPAD_D_RIGHT     GP_BIT8(GAMEPAD_DPAD_BIT_RIGHT)
+
+#define GAMEPAD_BTN_A       GP_BIT16(GAMEPAD_BTN_BIT_A)
+#define GAMEPAD_BTN_B       GP_BIT16(GAMEPAD_BTN_BIT_B)
+#define GAMEPAD_BTN_X       GP_BIT16(GAMEPAD_BTN_BIT_X)
+#define GAMEPAD_BTN_Y       GP_BIT16(GAMEPAD_BTN_BIT_Y)
+#define GAMEPAD_BTN_L3      GP_BIT16(GAMEPAD_BTN_BIT_L3)
+#define GAMEPAD_BTN_R3      GP_BIT16(GAMEPAD_BTN_BIT_R3)
+#define GAMEPAD_BTN_LB      GP_BIT16(GAMEPAD_BTN_BIT_LB)
+#define GAMEPAD_BTN_RB      GP_BIT16(GAMEPAD_BTN_BIT_RB)
+#define GAMEPAD_BTN_LT      GP_BIT16(GAMEPAD_BTN_BIT_LT)
+#define GAMEPAD_BTN_RT      GP_BIT16(GAMEPAD_BTN_BIT_RT)
+#define GAMEPAD_BTN_BACK    GP_BIT16(GAMEPAD_BTN_BIT_BACK)
+#define GAMEPAD_BTN_START   GP_BIT16(GAMEPAD_BTN_BIT_START)
+#define GAMEPAD_BTN_SYS     GP_BIT16(GAMEPAD_BTN_BIT_SYS)
+#define GAMEPAD_BTN_MISC    GP_BIT16(GAMEPAD_BTN_BIT_MISC)
+
+typedef struct __attribute__((packed, aligned(4))) {
+    uint8_t  dpad;
+    uint8_t  reserved0;
+    uint16_t buttons;
+    uint8_t  trigger_l;
+    uint8_t  trigger_r;
+    int16_t  joystick_lx;
+    int16_t  joystick_ly;
+    int16_t  joystick_rx;
+    int16_t  joystick_ry;
+    uint8_t  analog[GAMEPAD_ANALOG_COUNT];
+    // uint8_t  chatpad[3];
+    // uint8_t  reserved1;
+} gamepad_pad_t;
+_Static_assert(sizeof(gamepad_pad_t) == 24, "Gamepad pad size mismatch");
+
+typedef struct __attribute__((packed, aligned(4))) {
+    uint8_t l;
+    uint8_t r;
+    uint8_t l_duration;
+    uint8_t r_duration;
+} gamepad_rumble_t;
+_Static_assert(sizeof(gamepad_rumble_t) == 4, "Gamepad rumble size mismatch");
+
+typedef struct __attribute__((packed, aligned(4))) {
+    int16_t  data[GAMPAD_PCM_FRAME_SIZE];
+    uint16_t samples;
+    uint16_t reserved;
+} gamepad_pcm_t, gamepad_pcm_in_t, gamepad_pcm_out_t;
+_Static_assert(sizeof(gamepad_pcm_t) == 36, "Gamepad PCM size mismatch");
+
+/* ---- IN/OUT Data Flags ---- */
+
+#define GAMEPAD_FLAG_IN_PAD            ((uint32_t)1 << 0) /* New Device->Host pad data */
+#define GAMEPAD_FLAG_IN_PAD_ANALOG     ((uint32_t)1 << 1) /* New Device->Host pad data has analog data */
+#define GAMEPAD_FLAG_IN_CHATPAD        ((uint32_t)1 << 2) /* New Device->Host chatpad data */
+#define GAMEPAD_FLAG_IN_PCM            ((uint32_t)1 << 3) /* New Device->Host PCM data */
+#define GAMEPAD_FLAG_OUT_PCM           ((uint32_t)1 << 4) /* New Host->Device PCM data */
+#define GAMEPAD_FLAG_OUT_RUMBLE        ((uint32_t)1 << 5) /* New Host->Device rumble data */
+#define GAMEPAD_FLAG_OUT_RUMBLE_DUR    ((uint32_t)1 << 6) /* New Host->Device rumble has duration info */
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/gamepad/range.h
+++ b/Firmware/ESP32/main/gamepad/range.h
@@ -0,0 +1,98 @@
+#pragma once
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef MIN
+#define MIN(a,b)    (((a) < (b)) ? (a) : (b))
+#endif
+
+#ifndef MAX
+#define MAX(a,b)    (((a) > (b)) ? (a) : (b))
+#endif
+
+#define R_INT16_MAX   ((int16_t)32767)
+#define R_INT16_MID   ((int16_t)0)
+#define R_INT16_MIN   ((int16_t)-32768)
+
+#define R_UINT8_MAX   ((uint8_t)255)
+#define R_UINT8_MID   ((uint8_t)128)
+#define R_UINT8_MIN   ((uint8_t)0)
+
+#define R_UINT16_MAX  ((uint16_t)65535)
+#define R_UINT16_MID  ((uint16_t)32768)
+#define R_UINT16_MIN  ((uint16_t)0)
+
+#define R_UINT10_MAX  ((uint16_t)1023)
+#define R_UINT10_MID  ((uint16_t)512)
+#define R_UINT10_MIN  ((uint16_t)0)
+
+#define R_INT10_MAX   ((int16_t)511)
+#define R_INT10_MID   ((int16_t)0)
+#define R_INT10_MIN   ((int16_t)-512)
+
+static inline int16_t range_uint8_to_int16(uint8_t value) {
+    return (int16_t)(((int32_t)value << 8) - R_UINT16_MID);
+}
+
+static inline uint16_t range_uint8_to_uint16(uint8_t value) {
+    return (uint16_t)value << 8;
+}
+
+static inline uint8_t range_int16_to_uint8(int16_t value) {
+    return (uint8_t)(((int32_t)value + R_UINT16_MID) >> 8);
+}
+
+static inline uint8_t range_uint16_to_uint8(uint16_t value) {
+    return (uint8_t)(value >> 8);
+}
+
+static inline int16_t range_uint10_to_int16(uint16_t value) {
+    return (int16_t)(((int32_t)MIN(value, R_UINT10_MAX) << 6) - R_UINT16_MID);
+}
+
+static inline uint8_t range_uint10_to_uint8(uint16_t value) {
+    return (uint8_t)(MIN(value, R_UINT10_MAX) >> 2);
+}
+
+static inline int16_t range_int10_to_int16(int16_t value) {
+    if (value > R_INT10_MAX) {
+        value = R_INT10_MAX;
+    } else if (value < R_INT10_MIN) {
+        value = R_INT10_MIN;
+    }
+    return value << 6;
+}
+
+static inline int16_t range_invert_int16(int16_t value) {
+    return (int16_t)(-((int32_t)value) - 1);
+}
+
+static inline int16_t range_free_scale_int16(int32_t value, int32_t min, int32_t max) {
+    if (max <= min) {
+        return 0;
+    } else if (value < min) {
+        return R_INT16_MIN;
+    } else if (value > max) {
+        return R_INT16_MAX;
+    }
+    return (int16_t)(((value - min) * (int64_t)(R_INT16_MAX - R_INT16_MIN)) / (max - min) + R_INT16_MIN);
+}
+
+static inline uint8_t range_free_scale_uint8(int32_t value, int32_t min, int32_t max) {
+    if (max <= min) {
+        return 0;
+    } else if (value < min) {
+        return R_UINT8_MIN;
+    } else if (value > max) {
+        return R_UINT8_MAX;
+    }
+    return (uint8_t)(((value - min) * (int64_t)(R_UINT8_MAX - R_UINT8_MIN)) / (max - min) + R_UINT8_MIN);
+}
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/headset/headset.c
+++ b/Firmware/ESP32/main/headset/headset.c
--- a/Firmware/ESP32/main/headset/headset.h
+++ b/Firmware/ESP32/main/headset/headset.h
--- a/Firmware/ESP32/main/headset/max.c
+++ b/Firmware/ESP32/main/headset/max.c
--- a/Firmware/ESP32/main/headset/max.h
+++ b/Firmware/ESP32/main/headset/max.h
--- a/Firmware/ESP32/main/led/led.c
+++ b/Firmware/ESP32/main/led/led.c
@@ -0,0 +1,42 @@
+#include <hal/gpio_ll.h>
+#include "sdkconfig.h"
+#include "led/led.h"
+#include "gamepad/gamepad.h"
+
+static const int LED_PINS[] = {
+#if defined(CONFIG_ENABLE_LED_1) && (GAMEPADS_MAX > 0)
+    CONFIG_LED_PIN_1,
+
+#if defined(CONFIG_ENABLE_LED_2) && (GAMEPADS_MAX > 1)
+    CONFIG_LED_PIN_2,
+
+#if defined(CONFIG_ENABLE_LED_3) && (GAMEPADS_MAX > 2)
+    CONFIG_LED_PIN_3,
+    
+#if defined(CONFIG_ENABLE_LED_4) && (GAMEPADS_MAX > 3)
+    CONFIG_LED_PIN_4,
+
+#endif // CONFIG_ENABLE_LED_4
+#endif // CONFIG_ENABLE_LED_3
+#endif // CONFIG_ENABLE_LED_2
+#endif // CONFIG_ENABLE_LED_1
+};
+
+#define NUM_LEDS (sizeof(LED_PINS) / sizeof(LED_PINS[0]))
+
+void led_init(void) {
+    for (uint8_t i = 0; i < NUM_LEDS; ++i) {
+        gpio_ll_output_enable(&GPIO, LED_PINS[i]);
+        gpio_ll_set_level(&GPIO, LED_PINS[i], 0);
+    }
+}
+
+void led_set(uint8_t index, bool state) {
+    if (NUM_LEDS == 0) {
+        return;
+    }
+    if (index >= NUM_LEDS) {
+        index = NUM_LEDS - 1;
+    }
+    gpio_ll_set_level(&GPIO, LED_PINS[index], state ? 1 : 0);
+}
--- a/Firmware/ESP32/main/led/led.h
+++ b/Firmware/ESP32/main/led/led.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void led_init(void);
+void led_set(uint8_t index, bool state);
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/log/log.c
+++ b/Firmware/ESP32/main/log/log.c
@@ -0,0 +1,100 @@
+#include "log/log.h"
+#if CONFIG_LOG_ENABLED
+
+#include <stdio.h>
+#include <string.h>
+#include <stdarg.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+
+static SemaphoreHandle_t log_mutex;
+static bool multithread = false;
+
+void ogxm_log_init(bool multithreaded) {
+    multithread = multithreaded;
+    if (multithread) {
+        log_mutex = xSemaphoreCreateMutex();
+    }
+}
+
+void ogxm_log_level(uint8_t level, const char* fmt, ...) {
+    if (level > CONFIG_LOG_LEVEL) {
+        return;
+    }
+    if (multithread) {
+        xSemaphoreTake(log_mutex, portMAX_DELAY);
+    }
+
+    va_list args;
+    va_start(args, fmt);
+    
+    switch (level) {
+        case 0: // Error
+            fprintf(stderr, "OGXM ERROR: ");
+            vfprintf(stderr, fmt, args);
+            break;
+        case 1: // Info
+            printf("OGXM INFO: ");
+            vprintf(fmt, args);
+            break;
+        case 2: // Debug
+            printf("OGXM DEBUG: ");
+            vprintf(fmt, args);
+            break;
+        case 3: // Verbose
+            printf("OGXM VERBOSE: ");
+            vprintf(fmt, args);
+            break;
+    }
+    va_end(args);
+    if (multithread) {
+        xSemaphoreGive(log_mutex);
+    }
+}
+
+void ogxm_log_hex_level(uint8_t level, const void* data, uint16_t len, const char* fmt, ...) {
+    if (level > CONFIG_LOG_LEVEL) {
+        return;
+    }
+    if (multithread) {
+        xSemaphoreTake(log_mutex, portMAX_DELAY);
+    }
+
+    va_list args;
+    va_start(args, fmt);
+    
+    switch (level) {
+        case 0: // Error
+            fprintf(stderr, "OGXM ERROR: ");
+            vfprintf(stderr, fmt, args);
+            break;
+        case 1: // Info
+            printf("OGXM INFO: ");
+            vprintf(fmt, args);
+            break;
+        case 2: // Debug
+            printf("OGXM DEBUG: ");
+            vprintf(fmt, args);
+            break;
+        case 3: // Verbose
+            printf("OGXM VERBOSE: ");
+            vprintf(fmt, args);
+            break;
+    }
+    va_end(args);
+    printf("\n");
+
+    for (uint16_t i = 0; i < len; i++) {
+        if (i % 8 == 0 && i != 0) {
+            printf("\n");
+        }
+        printf(" %02X", ((uint8_t*)data)[i]);
+    }
+    printf("\n");
+
+    if (multithread) {
+        xSemaphoreGive(log_mutex);
+    }
+}
+
+#endif // CONFIG_LOG_ENABLED
--- a/Firmware/ESP32/main/log/log.h
+++ b/Firmware/ESP32/main/log/log.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include "sdkconfig.h"
+#if CONFIG_LOG_ENABLED
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void ogxm_log_init(bool multithreaded);
+void ogxm_log_level(uint8_t level, const char* fmt, ...);
+void ogxm_log_hex_level(uint8_t level, const void* data, uint16_t len, const char* fmt, ...);
+
+#define ogxm_loge(fmt, ...) ogxm_log_level(0, fmt, ##__VA_ARGS__)
+#define ogxm_logi(fmt, ...) ogxm_log_level(1, fmt, ##__VA_ARGS__)
+#define ogxm_logd(fmt, ...) ogxm_log_level(2, fmt, ##__VA_ARGS__)
+#define ogxm_logv(fmt, ...) ogxm_log_level(3, fmt, ##__VA_ARGS__)
+#define ogxm_loge_hex(data, len, fmt, ...) ogxm_log_hex_level(0, data, len, fmt, ##__VA_ARGS__)
+#define ogxm_logi_hex(data, len, fmt, ...) ogxm_log_hex_level(1, data, len, fmt, ##__VA_ARGS__)
+#define ogxm_logd_hex(data, len, fmt, ...) ogxm_log_hex_level(2, data, len, fmt, ##__VA_ARGS__)
+#define ogxm_logv_hex(data, len, fmt, ...) ogxm_log_hex_level(3, data, len, fmt, ##__VA_ARGS__)
+
+#ifdef __cplusplus
+}
+#endif
+
+#else 
+
+#define ogxm_log_init(...)      ((void)0)
+#define ogxm_loge(fmt, ...)     ((void)0)
+#define ogxm_logi(fmt, ...)     ((void)0)
+#define ogxm_logd(fmt, ...)     ((void)0)
+#define ogxm_logv(fmt, ...)     ((void)0)
+#define ogxm_loge_hex(fmt, ...) ((void)0)
+#define ogxm_logi_hex(fmt, ...) ((void)0)
+#define ogxm_logd_hex(fmt, ...) ((void)0)
+#define ogxm_logv_hex(fmt, ...) ((void)0)
+
+#endif // CONFIG_LOG_ENABLED
--- a/Firmware/ESP32/main/main.c
+++ b/Firmware/ESP32/main/main.c
@@ -1,9 +1,32 @@
-#include <stdlib.h>
 #include <stdint.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+#include "log/log.h"
+#include "bluetooth/bluetooth.h"
+#include "wired/wired.h"

-#include "main.h"
+void app_main(void) {
+    ogxm_log_init(true);
+    bluetooth_config(wired_set_connected, wired_set_pad, wired_set_audio);
+    wired_config(bluetooth_set_rumble, bluetooth_set_audio);

-void app_main(void) 
-{
-    cpp_main();
-}   
+    xTaskCreatePinnedToCore(
+        wired_task,
+        "wired",
+        2048U * 2U,
+        NULL,
+        configMAX_PRIORITIES - 6,
+        NULL,
+        1
+    );
+
+    xTaskCreatePinnedToCore(
+        bluetooth_task,
+        "bluetooth",
+        2048U * 4U,
+        NULL,
+        configMAX_PRIORITIES - 4,
+        NULL,
+        0
+    );
+}
--- a/Firmware/ESP32/main/main.cpp
+++ b/Firmware/ESP32/main/main.cpp
@@ -1,22 +0,0 @@
-#include <functional>
-#include <freertos/FreeRTOS.h>
-#include <freertos/task.h>
-
-#include "main.h"
-#include "sdkconfig.h"
-#include "BTManager/BTManager.h"
-
-void cpp_main()
-{
-    xTaskCreatePinnedToCore(
-        [](void* parameter)
-        { 
-            BTManager::get_instance().run_task(); 
-        },
-        "bp32",
-        2048 * 4,
-        NULL,
-        configMAX_PRIORITIES-4,
-        NULL,
-        0 );
-}
--- a/Firmware/ESP32/main/main.h
+++ b/Firmware/ESP32/main/main.h
@@ -1,16 +0,0 @@
-#ifndef _C_WRAPPER_H_
-#define _C_WRAPPER_H_
-
-#include <stdint.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void cpp_main();
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // _C_WRAPPER_H_
--- a/Firmware/ESP32/main/periph/i2c_master.c
+++ b/Firmware/ESP32/main/periph/i2c_master.c
@@ -0,0 +1,463 @@
+#include <string.h>
+#include <esp_private/periph_ctrl.h>
+#include <esp_rom_gpio.h>
+#include <esp_clk_tree.h>
+#include <esp_intr_alloc.h>
+#include <soc/clk_tree_defs.h>
+#include <rom/lldesc.h>
+#include <hal/gpio_ll.h>
+#include <hal/i2c_ll.h>
+#include <hal/i2c_hal.h>
+#include <esp_rom_sys.h>
+#include <esp_timer.h>
+
+#include "log/log.h"
+#include "periph/i2c_master.h"
+
+typedef struct i2c_master_handle_ {
+    bool                alloc;
+    int                 port;
+    int                 freq_hz;
+    int                 pin_sda;
+    int                 pin_scl;
+    bool                lsb_first;
+    bool                pullup_en;
+    int                 glitch_ignore_cnt;
+    i2c_dev_t*          dev;
+    uint32_t            clk_src_hz;
+    volatile bool       xfer_done;
+    i2c_intr_event_t    xfer_result; 
+} i2c_master_handle_t;
+
+static const uint32_t IO_MUX_REGS[40] = {
+    IO_MUX_GPIO0_REG, IO_MUX_GPIO1_REG, IO_MUX_GPIO2_REG, IO_MUX_GPIO3_REG,
+    IO_MUX_GPIO4_REG, IO_MUX_GPIO5_REG, IO_MUX_GPIO6_REG, IO_MUX_GPIO7_REG,
+    IO_MUX_GPIO8_REG, IO_MUX_GPIO9_REG, IO_MUX_GPIO10_REG, IO_MUX_GPIO11_REG,
+    IO_MUX_GPIO12_REG, IO_MUX_GPIO13_REG, IO_MUX_GPIO14_REG, IO_MUX_GPIO15_REG,
+    IO_MUX_GPIO16_REG, IO_MUX_GPIO17_REG, IO_MUX_GPIO18_REG, IO_MUX_GPIO19_REG,
+    IO_MUX_GPIO20_REG, IO_MUX_GPIO21_REG, IO_MUX_GPIO22_REG, IO_MUX_GPIO23_REG,
+    IO_MUX_GPIO24_REG, IO_MUX_GPIO25_REG, IO_MUX_GPIO26_REG, IO_MUX_GPIO27_REG,
+    0, 0, 0, 0,
+    IO_MUX_GPIO32_REG, IO_MUX_GPIO33_REG, IO_MUX_GPIO34_REG, IO_MUX_GPIO35_REG,
+    IO_MUX_GPIO36_REG, IO_MUX_GPIO37_REG, IO_MUX_GPIO38_REG, IO_MUX_GPIO39_REG,
+};
+
+static i2c_master_handle_t i2c_handles[2] = {0};
+
+static void i2c_master_config_gpio(i2c_master_handle_t* handle);
+static void i2c_master_config_bus(i2c_master_handle_t* handle);
+static void i2c_master_isr_init(i2c_master_handle_t* handle);
+void i2c_master_isr_enable(i2c_dev_t* dev, bool enable);
+void i2c_master_isr_handler(void* arg);
+static void i2c_master_clear_bus(i2c_master_handle_t* handle);
+static inline void i2c_master_cmd_start(i2c_dev_t* dev, int* cmd_idx);
+static inline void i2c_master_cmd_stop(i2c_dev_t* dev, int* cmd_idx);
+static inline void i2c_master_cmd_end(i2c_dev_t* dev, int* cmd_idx);
+static inline void i2c_master_cmd_write(i2c_dev_t* dev, int* cmd_idx, uint8_t* data, uint8_t len);
+static inline void i2c_master_cmd_read(i2c_dev_t* dev, int* cmd_idx, uint8_t len, bool last);
+static inline i2c_intr_event_t i2c_master_xfer_blocking(i2c_master_handle_t* handle, int cmd_idx, uint32_t timeout_us);
+static inline bool i2c_master_err_check(i2c_master_handle_t* handle, i2c_intr_event_t event);
+
+i2c_master_handle_t* i2c_master_init(const i2c_master_cfg* cfg) {
+    if (cfg == NULL) {
+        ogxm_loge("I2C: Invalid config\n");
+        return NULL;
+    }
+    if (cfg->port_num >= 2) {
+        ogxm_loge("I2C: Invalid port number %d\n", 
+            cfg->port_num);
+        return NULL;
+    }
+    if (i2c_handles[cfg->port_num].alloc) {
+        ogxm_loge("I2C: Port %d already initialized\n", 
+            cfg->port_num);
+        return NULL;
+    }
+    if ((cfg->pin_sda >= 40) || 
+        (cfg->pin_scl >= 40) ||
+        (IO_MUX_REGS[cfg->pin_sda] == 0) || 
+        (IO_MUX_REGS[cfg->pin_scl] == 0)) {
+        ogxm_loge("I2C: Invalid pin number sda=%d scl=%d\n", 
+            cfg->pin_sda, cfg->pin_scl);
+        return NULL;
+    }
+
+    i2c_master_handle_t* handle = &i2c_handles[cfg->port_num];
+    memset(handle, 0, sizeof(i2c_master_handle_t));
+    handle->alloc = true;
+    handle->port = cfg->port_num;
+    handle->dev = I2C_LL_GET_HW(handle->port);
+    handle->freq_hz = cfg->freq_hz;
+    handle->pin_sda = cfg->pin_sda;
+    handle->pin_scl = cfg->pin_scl;
+    handle->lsb_first = cfg->lsb_first;
+    handle->pullup_en = cfg->pullup_en;
+    handle->glitch_ignore_cnt = cfg->glitch_ignore_cnt;
+
+    i2c_master_config_gpio(handle);
+    i2c_master_config_bus(handle);
+    i2c_master_isr_init(handle);
+    return handle;
+}
+
+bool i2c_master_write_blocking(i2c_master_handle_t* handle, uint8_t addr, 
+                               uint8_t* data, size_t len, uint32_t timeout_us) {
+    if ((handle == NULL) || ((data == NULL) && (len > 0)) || !handle->alloc) {
+        ogxm_loge("I2C: Invalid write params\n");
+        return false;
+    }
+    i2c_ll_txfifo_rst(handle->dev);
+
+    uint8_t addr7 = (addr << 1) | 0;
+    int cmd_idx = 0;
+    i2c_master_cmd_start(handle->dev, &cmd_idx);
+    i2c_master_cmd_write(handle->dev, &cmd_idx, &addr7, 1);
+
+    uint32_t offset = 0;
+    uint32_t txfifo_free = 0;
+    i2c_ll_get_txfifo_len(handle->dev, &txfifo_free);
+
+    while (len > txfifo_free) {
+        i2c_master_cmd_write(handle->dev, &cmd_idx, 
+                             data + offset, txfifo_free);
+        i2c_master_cmd_end(handle->dev, &cmd_idx);
+        i2c_intr_event_t result = i2c_master_xfer_blocking(handle, cmd_idx, timeout_us);
+        if (!i2c_master_err_check(handle, result)) {
+            return false;
+        }
+
+        offset += txfifo_free;
+        len -= txfifo_free;
+
+        i2c_ll_txfifo_rst(handle->dev);
+        i2c_ll_get_txfifo_len(handle->dev, &txfifo_free);
+    }
+
+    if (len) {
+        i2c_master_cmd_write(handle->dev, &cmd_idx, data + offset, len);
+    }
+    i2c_master_cmd_stop(handle->dev, &cmd_idx);
+    i2c_intr_event_t result = i2c_master_xfer_blocking(handle, cmd_idx, timeout_us);
+    return i2c_master_err_check(handle, result);
+}
+
+bool i2c_master_read_blocking(i2c_master_handle_t* handle, uint8_t addr, 
+                              uint8_t* data, size_t len, uint32_t timeout_us) {
+    if ((handle == NULL) || ((data == NULL) && (len > 0)) || !handle->alloc) {
+        ogxm_loge("I2C: Invalid read params\n");
+        return false;
+    }
+    i2c_ll_txfifo_rst(handle->dev);
+    i2c_ll_rxfifo_rst(handle->dev);
+
+    uint8_t addr7 = (addr << 1) | 1;
+    int cmd_idx = 0;
+
+    i2c_master_cmd_start(handle->dev, &cmd_idx);
+    i2c_master_cmd_write(handle->dev, &cmd_idx, &addr7, 1);
+
+    uint32_t offset = 0;
+
+    while (len > SOC_I2C_FIFO_LEN) {
+        i2c_master_cmd_read(handle->dev, &cmd_idx, 
+                            SOC_I2C_FIFO_LEN, false);
+        i2c_master_cmd_end(handle->dev, &cmd_idx);
+        i2c_intr_event_t result = i2c_master_xfer_blocking(handle, cmd_idx, timeout_us);
+        if (!i2c_master_err_check(handle, result)) {
+            return false;
+        }
+        i2c_ll_read_rxfifo(handle->dev, data + offset, len);
+        i2c_ll_rxfifo_rst(handle->dev);
+
+        offset += SOC_I2C_FIFO_LEN;
+        len -= SOC_I2C_FIFO_LEN;
+    }
+
+    if (len) {
+        i2c_master_cmd_read(handle->dev, &cmd_idx, len, true);
+    }
+
+    i2c_master_cmd_stop(handle->dev, &cmd_idx);
+    i2c_intr_event_t result = i2c_master_xfer_blocking(handle, cmd_idx, timeout_us);
+    if (!i2c_master_err_check(handle, result)) {
+        return false;
+    }
+    i2c_ll_read_rxfifo(handle->dev, data + offset, len);
+    return true;
+}
+
+void i2c_master_deinit(i2c_master_handle_t* handle) {
+    if ((handle == NULL) || !handle->alloc) {
+        ogxm_loge("I2C: Invalid handle\n");
+        return;
+    }
+    i2c_master_isr_enable(handle->dev, false);
+    i2c_ll_enable_controller_clock(handle->dev, false);
+
+    PERIPH_RCC_ATOMIC() {
+        i2c_ll_reset_register(handle->port);
+        i2c_ll_enable_bus_clock(handle->port, false);
+    }
+    i2c_master_clear_bus(handle);
+    memset(handle, 0, sizeof(i2c_master_handle_t));
+}
+
+static void i2c_master_config_gpio(i2c_master_handle_t* handle) {
+    gpio_ll_input_enable(&GPIO, handle->pin_sda);
+    gpio_ll_input_enable(&GPIO, handle->pin_scl);
+    gpio_ll_od_enable(&GPIO, handle->pin_sda);
+    gpio_ll_od_enable(&GPIO, handle->pin_scl);
+
+    if (handle->pullup_en) {
+        gpio_ll_pullup_en(&GPIO, handle->pin_sda);
+        gpio_ll_pullup_en(&GPIO, handle->pin_scl);
+    } else {
+        gpio_ll_pullup_dis(&GPIO, handle->pin_sda);
+        gpio_ll_pullup_dis(&GPIO, handle->pin_scl);
+    }
+
+    gpio_ll_iomux_func_sel(IO_MUX_REGS[handle->pin_scl], PIN_FUNC_GPIO);
+    gpio_ll_iomux_func_sel(IO_MUX_REGS[handle->pin_sda], PIN_FUNC_GPIO);
+
+    int ext_scl_out_idx = (handle->port == 0) ? I2CEXT0_SCL_OUT_IDX 
+                                              : I2CEXT1_SCL_OUT_IDX;
+    int ext_sda_out_idx = (handle->port == 0) ? I2CEXT0_SDA_OUT_IDX 
+                                              : I2CEXT1_SDA_OUT_IDX;
+    int ext_scl_in_idx = (handle->port == 0) ? I2CEXT0_SCL_IN_IDX 
+                                             : I2CEXT1_SCL_IN_IDX;
+    int ext_sda_in_idx = (handle->port == 0) ? I2CEXT0_SDA_IN_IDX 
+                                             : I2CEXT1_SDA_IN_IDX;
+
+    esp_rom_gpio_connect_out_signal(handle->pin_scl, ext_scl_out_idx, false, false);
+    esp_rom_gpio_connect_in_signal(handle->pin_scl, ext_scl_in_idx, false);
+    esp_rom_gpio_connect_out_signal(handle->pin_sda, ext_sda_out_idx, false, false);
+    esp_rom_gpio_connect_in_signal(handle->pin_sda, ext_sda_in_idx, false);
+}
+
+static void i2c_master_config_bus(i2c_master_handle_t* handle) {
+    i2c_ll_enable_controller_clock(handle->dev, true);
+
+    PERIPH_RCC_ATOMIC() {
+        i2c_ll_enable_bus_clock(handle->port, true);
+        i2c_ll_reset_register(handle->port);
+    }
+
+    i2c_ll_master_init(handle->dev);
+    i2c_ll_set_data_mode(handle->dev, 
+                         handle->lsb_first 
+                            ? I2C_DATA_MODE_LSB_FIRST 
+                            : I2C_DATA_MODE_MSB_FIRST, 
+                         handle->lsb_first 
+                            ? I2C_DATA_MODE_LSB_FIRST 
+                            : I2C_DATA_MODE_MSB_FIRST);
+    i2c_ll_txfifo_rst(handle->dev);
+    i2c_ll_rxfifo_rst(handle->dev);
+
+    soc_module_clk_t clk_src = (soc_module_clk_t)I2C_CLK_SRC_DEFAULT;
+    esp_clk_tree_src_get_freq_hz(clk_src, 
+                                 ESP_CLK_TREE_SRC_FREQ_PRECISION_APPROX, 
+                                 &handle->clk_src_hz);
+
+    i2c_hal_clk_config_t clk_cfg;
+    i2c_ll_master_cal_bus_clk(handle->clk_src_hz, handle->freq_hz, &clk_cfg);
+    i2c_ll_master_set_bus_timing(handle->dev, &clk_cfg);
+    i2c_ll_master_set_filter(handle->dev, handle->glitch_ignore_cnt);
+    i2c_ll_set_source_clk(handle->dev, (soc_periph_i2c_clk_src_t)clk_src);
+    i2c_ll_update(handle->dev);
+    i2c_ll_clear_intr_mask(handle->dev, I2C_LL_MASTER_EVENT_INTR);
+}
+
+static void i2c_master_isr_init(i2c_master_handle_t* handle) {
+    i2c_master_isr_enable(handle->dev, false);
+    i2c_ll_clear_intr_mask(handle->dev, I2C_LL_MASTER_EVENT_INTR);
+    esp_intr_alloc(ETS_I2C_EXT0_INTR_SOURCE, 
+                   ESP_INTR_FLAG_IRAM | ESP_INTR_FLAG_LEVEL1, 
+                   i2c_master_isr_handler, (void*)handle, NULL);
+}
+
+void IRAM_ATTR i2c_master_isr_enable(i2c_dev_t* dev, bool enable) {
+    if (enable) {
+        i2c_ll_enable_intr_mask(dev, I2C_LL_MASTER_EVENT_INTR);
+    } else {
+        i2c_ll_disable_intr_mask(dev, I2C_LL_MASTER_EVENT_INTR);
+    }
+}
+
+void IRAM_ATTR i2c_master_isr_handler(void* arg) {
+    i2c_master_handle_t* handle = (i2c_master_handle_t*)arg;
+    i2c_intr_event_t event;
+    i2c_ll_master_get_event(handle->dev, &event);
+    i2c_ll_clear_intr_mask(handle->dev, I2C_LL_MASTER_EVENT_INTR);
+    if (!handle->xfer_done && (event != I2C_INTR_EVENT_ERR)) {
+        i2c_master_isr_enable(handle->dev, false);
+        handle->xfer_result = event;
+        handle->xfer_done = true;
+    }
+}
+
+static void i2c_master_clear_bus(i2c_master_handle_t* handle) {
+    i2c_ll_master_fsm_rst(handle->dev);
+    i2c_ll_txfifo_rst(handle->dev);
+    i2c_ll_rxfifo_rst(handle->dev);
+    i2c_ll_update(handle->dev);
+
+    gpio_ll_iomux_func_sel(IO_MUX_REGS[handle->pin_sda], PIN_FUNC_GPIO);
+    gpio_ll_iomux_func_sel(IO_MUX_REGS[handle->pin_scl], PIN_FUNC_GPIO);
+    gpio_ll_od_enable(&GPIO, handle->pin_sda);
+    gpio_ll_od_enable(&GPIO, handle->pin_scl);
+    gpio_ll_pullup_en(&GPIO, handle->pin_sda);
+    gpio_ll_pullup_en(&GPIO, handle->pin_scl);
+
+    /*
+     * If SDA is low it is driven by the slave, wait until SDA goes high, at
+     * maximum 9 clock cycles in standard mode at 100 kHz including the ACK bit.
+     */
+    uint32_t half_cycle = 5;
+    int count = 9;
+
+    while (!gpio_ll_get_level(&GPIO, handle->pin_sda) && count--) {
+        // Drive SCL low
+        gpio_ll_output_enable(&GPIO, handle->pin_scl);
+        gpio_ll_set_level(&GPIO, handle->pin_scl, 0);
+        esp_rom_delay_us(half_cycle);
+        // Drive SCL high
+        gpio_ll_set_level(&GPIO, handle->pin_scl, 1);
+        esp_rom_delay_us(half_cycle);
+    }
+
+    // Generate STOP condition
+    // SCL low, SDA low
+    gpio_ll_set_level(&GPIO, handle->pin_scl, 0);
+    esp_rom_delay_us(half_cycle);
+    gpio_ll_set_level(&GPIO, handle->pin_sda, 0);
+    esp_rom_delay_us(half_cycle);
+    // SCL high
+    gpio_ll_set_level(&GPIO, handle->pin_scl, 1);
+    esp_rom_delay_us(half_cycle);
+    // SDA high (STOP)
+    gpio_ll_set_level(&GPIO, handle->pin_sda, 1);
+    esp_rom_delay_us(half_cycle);
+}
+
+static inline const char* i2c_master_evt_to_str(i2c_intr_event_t event) {
+    switch (event) {
+    case I2C_INTR_EVENT_ERR:
+        return "I2C_INTR_EVENT_ERR";
+    case I2C_INTR_EVENT_ARBIT_LOST:
+        return "I2C_INTR_EVENT_ARBIT_LOST";
+    case I2C_INTR_EVENT_NACK:
+        return "I2C_INTR_EVENT_NACK";
+    case I2C_INTR_EVENT_TOUT:
+        return "I2C_INTR_EVENT_TOUT";
+    case I2C_INTR_EVENT_END_DET:
+        return "I2C_INTR_EVENT_END_DET";
+    case I2C_INTR_EVENT_TRANS_DONE:
+        return "I2C_INTR_EVENT_TRANS_DONE";
+    case I2C_INTR_EVENT_RXFIFO_FULL:
+        return "I2C_INTR_EVENT_RXFIFO_FULL";
+    case I2C_INTR_EVENT_TXFIFO_EMPTY:
+        return "I2C_INTR_EVENT_TXFIFO_EMPTY";
+    default:
+        return "Unknown event";
+    }
+}
+
+static inline bool i2c_master_err_check(i2c_master_handle_t* handle, 
+                                        i2c_intr_event_t event) {
+    if ((event != I2C_INTR_EVENT_TRANS_DONE) &&
+        (event != I2C_INTR_EVENT_END_DET)) {
+        ogxm_loge("I2C: Xfer failed: %s\n", 
+            i2c_master_evt_to_str(event));
+        i2c_master_clear_bus(handle);
+        i2c_master_config_gpio(handle);
+        i2c_master_config_bus(handle);
+        return false;
+    }
+    return true;
+}
+
+static inline void i2c_master_cmd_start(i2c_dev_t* dev, int* cmd_idx) {
+    i2c_ll_hw_cmd_t cmd = {0};
+    cmd.op_code = I2C_LL_CMD_RESTART;
+    i2c_ll_master_write_cmd_reg(dev, cmd, *cmd_idx);
+    (*cmd_idx)++;
+}
+
+static inline void i2c_master_cmd_stop(i2c_dev_t* dev, int* cmd_idx) {
+    i2c_ll_hw_cmd_t cmd = {0};
+    cmd.op_code = I2C_LL_CMD_STOP;
+    i2c_ll_master_write_cmd_reg(dev, cmd, *cmd_idx);
+    (*cmd_idx)++;
+}
+
+static inline void i2c_master_cmd_end(i2c_dev_t* dev, int* cmd_idx) {
+    i2c_ll_hw_cmd_t cmd = {0};
+    cmd.op_code = I2C_LL_CMD_END;
+    i2c_ll_master_write_cmd_reg(dev, cmd, *cmd_idx);
+    (*cmd_idx)++;
+}
+
+static inline void i2c_master_cmd_write(i2c_dev_t* dev, int* cmd_idx, 
+                                        uint8_t* data, uint8_t len) {
+    i2c_ll_write_txfifo(dev, data, len);
+    i2c_ll_hw_cmd_t cmd = { 
+        .op_code = I2C_LL_CMD_WRITE,
+        .byte_num = len,
+        .ack_en = 1,
+        .ack_exp = 0,
+        .ack_val = 0 
+    };
+    i2c_ll_master_write_cmd_reg(dev, cmd, *cmd_idx);
+    (*cmd_idx)++;
+}
+
+static inline void i2c_master_cmd_read(i2c_dev_t* dev, int* cmd_idx, uint8_t len, bool last) {
+    if ((len == 0) || (len > SOC_I2C_FIFO_LEN)) {
+        ogxm_loge("I2C: Invalid read length: %d\n", len);
+        return;
+    }
+    if (len > 1) {
+        i2c_ll_hw_cmd_t cmd = { 
+            .op_code = I2C_LL_CMD_READ,
+            .byte_num = len - 1,
+            .ack_en = 0,
+            .ack_exp = 0,
+            .ack_val = 0 
+        };
+        i2c_ll_master_write_cmd_reg(dev, cmd, *cmd_idx);
+        (*cmd_idx)++;
+    }
+    i2c_ll_hw_cmd_t cmd = { 
+        .op_code = I2C_LL_CMD_READ,
+        .byte_num = 1,
+        .ack_en = 1,
+        .ack_exp = 0,
+        .ack_val = last ? 1 : 0
+    };
+    i2c_ll_master_write_cmd_reg(dev, cmd, *cmd_idx);
+    (*cmd_idx)++;
+}
+
+static inline i2c_intr_event_t i2c_master_xfer_blocking(i2c_master_handle_t* handle, 
+                                                        int cmd_idx, uint32_t timeout_us) {
+    uint32_t reg_val = 
+        i2c_ll_calculate_timeout_us_to_reg_val(handle->clk_src_hz, 
+                                               timeout_us);
+    i2c_ll_set_tout(handle->dev, reg_val);
+    i2c_ll_update(handle->dev);
+    handle->xfer_done = false;
+    i2c_master_isr_enable(handle->dev, true);
+    i2c_ll_start_trans(handle->dev);
+
+    uint32_t start = esp_timer_get_time();
+    while (!handle->xfer_done) {
+        // A timeout is written to the register, but this can get stuck anyway
+        if ((esp_timer_get_time() - start) > (timeout_us + 100)) {
+            i2c_master_isr_enable(handle->dev, false);
+            handle->xfer_result = I2C_INTR_EVENT_TOUT;
+            break;
+        }
+    }
+    ogxm_logv("I2C: xfer done: %s\n", i2c_master_evt_to_str(handle->xfer_result));
+    return handle->xfer_result;
+}
--- a/Firmware/ESP32/main/periph/i2c_master.h
+++ b/Firmware/ESP32/main/periph/i2c_master.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct i2c_master_handle_ i2c_master_handle_t;
+
+typedef struct {
+    uint8_t     port_num;
+    uint32_t    freq_hz;
+    uint8_t     pin_sda;
+    uint8_t     pin_scl;
+    bool        lsb_first;
+    bool        pullup_en;
+    int         glitch_ignore_cnt;
+} i2c_master_cfg;
+
+i2c_master_handle_t* i2c_master_init(const i2c_master_cfg* cfg);
+bool i2c_master_write_blocking(i2c_master_handle_t* handle, uint8_t addr, uint8_t* data, size_t len, uint32_t timeout_us);
+bool i2c_master_read_blocking(i2c_master_handle_t* handle, uint8_t addr, uint8_t* data, size_t len, uint32_t timeout_us);
+void i2c_master_deinit(i2c_master_handle_t* handle);
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/settings/button_combo.c
+++ b/Firmware/ESP32/main/settings/button_combo.c
@@ -0,0 +1,42 @@
+#include <stddef.h>
+#include "esp_timer.h"
+#include "settings/button_combo.h"
+
+#define BUTTON_COMBO(dpad, buttons) (((uint32_t)(dpad) << 16) | (buttons))
+#define BUTTON_COMBO_NONE           ((uint32_t)0xFFFFFFFFU)
+
+static const uint32_t BUTTON_COMBOS[USBD_TYPE_COUNT] = {
+    [USBD_TYPE_XBOXOG_GP]   = BUTTON_COMBO(GAMEPAD_D_RIGHT, GAMEPAD_BTN_START),
+    [USBD_TYPE_XBOXOG_SB]   = BUTTON_COMBO(GAMEPAD_D_RIGHT, GAMEPAD_BTN_START | GAMEPAD_BTN_RB),
+    [USBD_TYPE_XBOXOG_XR]   = BUTTON_COMBO(GAMEPAD_D_RIGHT, GAMEPAD_BTN_START | GAMEPAD_BTN_LB),
+    [USBD_TYPE_XINPUT]      = BUTTON_COMBO(GAMEPAD_D_UP,    GAMEPAD_BTN_START),
+    [USBD_TYPE_PS3]         = BUTTON_COMBO(GAMEPAD_D_LEFT,  GAMEPAD_BTN_START),
+    [USBD_TYPE_PSCLASSIC]   = BUTTON_COMBO(0,               GAMEPAD_BTN_START | GAMEPAD_BTN_A),
+    [USBD_TYPE_SWITCH]      = BUTTON_COMBO(GAMEPAD_D_DOWN,  GAMEPAD_BTN_START),
+    // [USBD_TYPE_WEBAPP]      = BUTTON_COMBO(0,               GAMEPAD_BTN_START | GAMEPAD_BTN_LB | GAMEPAD_BTN_RB),
+    // [USBD_TYPE_UART_BRIDGE] = BUTTON_COMBO_NONE,
+};
+
+static uint32_t check_time[GAMEPADS_MAX] = { 0 };
+static uint32_t last_combo[GAMEPADS_MAX] = { BUTTON_COMBO_NONE };
+
+usbd_type_t check_button_combo(uint8_t index, const gamepad_pad_t* pad) {
+    if ((index >= GAMEPADS_MAX) || (pad == NULL)) {
+        return USBD_TYPE_COUNT;
+    }
+    const uint32_t now = (uint32_t)esp_timer_get_time();
+    if (BUTTON_COMBO(pad->dpad, pad->buttons) != last_combo[index]) {
+        check_time[index] = now;
+        last_combo[index] = BUTTON_COMBO(pad->dpad, pad->buttons);
+        return USBD_TYPE_COUNT;
+    }
+    if ((now - check_time[index]) >= COMBO_HOLD_TIME_US) {
+        check_time[index] = now;
+        for (uint8_t type = 0; type < USBD_TYPE_COUNT; type++) {
+            if ((BUTTON_COMBOS[type] == last_combo[index])) {
+                return (usbd_type_t)type;
+            }
+        }
+    }
+    return USBD_TYPE_COUNT;
+}
--- a/Firmware/ESP32/main/settings/button_combo.h
+++ b/Firmware/ESP32/main/settings/button_combo.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <stdint.h>
+#include "gamepad/gamepad.h"
+#include "settings/device_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define COMBO_CHECK_INTERVAL_MS     ((uint32_t)600U)
+#define COMBO_CHECK_INTERVAL_US     (COMBO_CHECK_INTERVAL_MS * 1000U)
+#define COMBO_HOLD_TIME_US          ((uint32_t)(3U * 1000U * 1000U))
+
+usbd_type_t check_button_combo(uint8_t index, const gamepad_pad_t* pad);
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/settings/device_types.h
+++ b/Firmware/ESP32/main/settings/device_types.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+    USBD_TYPE_XBOXOG_GP = 0,
+    USBD_TYPE_XBOXOG_SB,
+    USBD_TYPE_XBOXOG_XR,
+    USBD_TYPE_XINPUT,
+    USBD_TYPE_PS3,
+    USBD_TYPE_PSCLASSIC,
+    USBD_TYPE_SWITCH,
+    // USBD_TYPE_WEBAPP,
+    // USBD_TYPE_UART_BRIDGE,
+    USBD_TYPE_COUNT
+} usbd_type_t;
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/settings/nvs.c
+++ b/Firmware/ESP32/main/settings/nvs.c
@@ -0,0 +1,86 @@
+#include <string.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+#include <nvs_flash.h>
+#include <nvs.h>
+#include <esp_err.h>
+#include "log/log.h"
+#include "settings/nvs.h"
+
+static const char NVS_NAMESPACE[] = "user_data";
+static SemaphoreHandle_t nvs_mutex;
+static volatile bool nvs_inited = false;
+
+void nvs_init(void) {
+    nvs_mutex = xSemaphoreCreateMutex();
+    xSemaphoreTake(nvs_mutex, portMAX_DELAY);
+
+    if (nvs_flash_init() != ESP_OK) {
+        ESP_ERROR_CHECK(nvs_flash_erase());
+        ESP_ERROR_CHECK(nvs_flash_init());
+    }
+    nvs_inited = true;
+    xSemaphoreGive(nvs_mutex);
+}
+
+void nvs_erase(void) {
+    if (!nvs_inited) {
+        return;
+    }
+    nvs_handle_t handle;
+    xSemaphoreTake(nvs_mutex, portMAX_DELAY);
+
+    if (nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle) != ESP_OK) {
+        xSemaphoreGive(nvs_mutex);
+        return;
+    }
+    nvs_erase_all(handle);
+    nvs_close(handle);
+    xSemaphoreGive(nvs_mutex);
+}
+
+bool nvs_read(const char* key, void* value, size_t len) {
+    if (!nvs_inited) {
+        return false;
+    }
+    esp_err_t err;
+    nvs_handle_t handle;
+
+    xSemaphoreTake(nvs_mutex, portMAX_DELAY);
+
+    if ((err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle)) != ESP_OK) {
+        xSemaphoreGive(nvs_mutex);
+        return (err == ESP_OK);
+    }
+
+    err = nvs_get_blob(handle, key, value, &len);
+    nvs_close(handle);
+
+    xSemaphoreGive(nvs_mutex);
+    return (err == ESP_OK);
+}
+
+bool nvs_write(const char* key, const void* value, size_t len) {
+    if (!nvs_inited) {
+        return false;
+    }
+    esp_err_t err;
+    nvs_handle_t handle;
+
+    xSemaphoreTake(nvs_mutex, portMAX_DELAY);
+
+    if ((err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle)) != ESP_OK) {
+        xSemaphoreGive(nvs_mutex);
+        return (err == ESP_OK);
+    }
+    if ((err = nvs_set_blob(handle, key, value, len)) != ESP_OK) {
+        nvs_close(handle);
+        xSemaphoreGive(nvs_mutex);
+        return (err == ESP_OK);
+    } 
+    err = nvs_commit(handle);
+    nvs_close(handle);
+
+    xSemaphoreGive(nvs_mutex);
+    return (err == ESP_OK);
+}
--- a/Firmware/ESP32/main/settings/nvs.h
+++ b/Firmware/ESP32/main/settings/nvs.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void nvs_init(void);
+bool nvs_read(const char* key, void* value, size_t len);
+bool nvs_write(const char* key, const void* value, size_t len);
+void nvs_erase(void);
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/settings/settings.cpp
+++ b/Firmware/ESP32/main/settings/settings.cpp
@@ -0,0 +1,181 @@
+#include <string>
+#include <cstring>
+#include "log/log.h"
+#include "gamepad/gamepad.h"
+#include "settings/nvs.h"
+#include "settings/settings.h"
+
+#define INIT_FLAG           ((uint16_t)0xFFA3)
+
+typedef struct __attribute__((aligned(4))) {
+    volatile uint16_t    init_flag;
+    volatile usbd_type_t device_type;
+    volatile uint32_t    addons;
+    volatile uint8_t     active_profile_id[GAMEPADS_MAX];
+} settings_t;
+
+static settings_t settings = {};
+
+static inline const std::string SETTINGS_KEY() {
+    return std::string("settings");
+}
+
+static inline const std::string PROFILE_KEY(uint8_t id) {
+    return std::string("profile_") + std::to_string(id);
+}
+
+void settings_init(void) {
+    if (settings.init_flag == INIT_FLAG) {
+        ogxm_logd("Settings already initialized\n");
+        return;
+    }
+    ogxm_logd("Initializing settings\n");
+
+    nvs_init();
+    nvs_read(SETTINGS_KEY().c_str(), &settings, sizeof(settings));
+    if (settings.init_flag == INIT_FLAG) {
+        ogxm_logd("Settings already initialized with flag: %04X\n", settings.init_flag);
+        return;
+    }
+
+    ogxm_logd("Settings not initialized, setting default values\n");
+
+    settings.init_flag = INIT_FLAG;
+    settings.device_type = USBD_TYPE_XBOXOG_GP;
+    settings.addons = 0;
+    for (uint8_t i = 0; i < GAMEPADS_MAX; i++) {
+        settings.active_profile_id[i] = 1;
+    }
+    nvs_erase();
+    
+    user_profile_t profile = {};
+    settings_get_default_profile(&profile); 
+
+    for (uint8_t i = 0; i < USER_PROFILES_MAX; i++) {
+        profile.id = i + 1;
+        nvs_write(PROFILE_KEY(i + 1).c_str(), &profile, sizeof(profile));
+    }
+
+    nvs_write(SETTINGS_KEY().c_str(), &settings, sizeof(settings));
+
+    ogxm_logd("Settings initialized successfully\n");
+}
+
+usbd_type_t settings_get_device_type(void) {
+    return settings.device_type;
+}
+
+void settings_get_profile_by_index(uint8_t index, user_profile_t* profile) {
+    if ((settings.init_flag != INIT_FLAG) || (index > GAMEPADS_MAX)) {
+        ogxm_logd("returning default profile\n");
+        settings_get_default_profile(profile);
+        return;
+    }
+    ogxm_logd("Getting profile by index: %d\n", index);
+    nvs_read(PROFILE_KEY(settings.active_profile_id[index]).c_str(), 
+             profile, sizeof(user_profile_t));
+}
+
+void settings_get_profile_by_id(uint8_t id, user_profile_t* profile) {
+    if ((settings.init_flag != INIT_FLAG) || 
+        (id > USER_PROFILES_MAX) || (id == 0)) {
+        settings_get_default_profile(profile);
+        return;
+    }
+    ogxm_logd("Getting profile by ID: %d\n", id);
+    nvs_read(PROFILE_KEY(id).c_str(), profile, sizeof(user_profile_t));
+}
+
+uint8_t settings_get_active_profile_id(uint8_t index) {
+    if ((settings.init_flag != INIT_FLAG) || (index > GAMEPADS_MAX)) {
+        return 1;
+    }
+    return settings.active_profile_id[index];
+}
+
+void settings_store_device_type(usbd_type_t type) {
+    if ((settings.init_flag != INIT_FLAG) ||
+        (type >= USBD_TYPE_COUNT)) {
+        return;
+    }
+    settings.device_type = type;
+    ogxm_logd("Storing device type: %d\n", type);
+    nvs_write(SETTINGS_KEY().c_str(), &settings, sizeof(settings));
+}
+
+void settings_store_profile(uint8_t index, const user_profile_t* profile) {
+    if ((settings.init_flag != INIT_FLAG) || (index > USER_PROFILES_MAX)) {
+        return;
+    }
+    ogxm_logd("Storing profile, index: %d", index);
+    nvs_write(PROFILE_KEY(settings.active_profile_id[index]).c_str(), 
+              profile, sizeof(user_profile_t));
+}
+
+void settings_get_default_joystick(joystick_settings_t* joy_set) {
+    joy_set->dz_inner = fix16_from_int(0);
+    joy_set->dz_outer = fix16_from_int(1);
+    joy_set->anti_dz_circle = fix16_from_int(0);
+    joy_set->anti_dz_circle_y_scale = fix16_from_int(0);
+    joy_set->anti_dz_square = fix16_from_int(0);
+    joy_set->anti_dz_square_y_scale = fix16_from_int(0);
+    joy_set->anti_dz_angular = fix16_from_int(0);
+    joy_set->anti_dz_outer = fix16_from_int(1);
+    joy_set->axis_restrict = fix16_from_int(0);
+    joy_set->angle_restrict = fix16_from_int(0);
+    joy_set->diag_scale_min = fix16_from_int(1);
+    joy_set->diag_scale_max = fix16_from_int(1);
+    joy_set->curve = fix16_from_int(1);
+    joy_set->uncap_radius = 1;
+    joy_set->invert_y = 0;
+    joy_set->invert_x = 0;
+}
+
+void settings_get_default_trigger(trigger_settings_t* trig_set) {
+    trig_set->dz_inner = fix16_from_int(0);
+    trig_set->dz_outer = fix16_from_int(1);
+    trig_set->anti_dz_inner = fix16_from_int(0);
+    trig_set->anti_dz_outer = fix16_from_int(1);
+    trig_set->curve = fix16_from_int(1);
+}
+
+void settings_get_default_profile(user_profile_t* profile) {
+    if (profile == NULL) {
+        return;
+    }
+    memset(profile, 0, sizeof(user_profile_t));
+    settings_get_default_joystick(&profile->joystick_l);
+    memcpy(&profile->joystick_r, &profile->joystick_l, sizeof(joystick_settings_t));
+    settings_get_default_trigger(&profile->trigger_l);
+    memcpy(&profile->trigger_r, &profile->trigger_l, sizeof(trigger_settings_t));
+
+    for (uint8_t i = 0; i < GAMEPAD_BTN_BIT_COUNT; i++) {
+        profile->btns[i] = i;
+    }
+    for (uint8_t i = 0; i < GAMEPAD_DPAD_BIT_COUNT; i++) {
+        profile->dpad[i] = i;
+    }
+    for (uint8_t i = 0; i < GAMEPAD_ANALOG_COUNT; i++) {
+        profile->analog[i] = i;
+    }
+    profile->analog_en  = 1;
+    profile->id = 1;
+}
+
+bool settings_is_default_joystick(const joystick_settings_t* joy_set) {
+    if (joy_set == NULL) {
+        return false;
+    }
+    joystick_settings_t default_joy = {};
+    settings_get_default_joystick(&default_joy);
+    return (memcmp(joy_set, &default_joy, sizeof(joystick_settings_t)) == 0);
+}
+
+bool settings_is_default_trigger(const trigger_settings_t* trig_set) {
+    if (trig_set == NULL) {
+        return false;
+    }
+    trigger_settings_t default_trig = {};
+    settings_get_default_trigger(&default_trig);
+    return (memcmp(trig_set, &default_trig, sizeof(trigger_settings_t)) == 0);
+}
--- a/Firmware/ESP32/main/settings/settings.h
+++ b/Firmware/ESP32/main/settings/settings.h
@@ -0,0 +1,388 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "libfixmath/fix16.h"
+#include "settings/device_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define USER_PROFILES_MAX   8U
+
+typedef struct __attribute__((packed, aligned(4))) {
+    fix16_t dz_inner;
+    fix16_t dz_outer;
+
+    fix16_t anti_dz_inner;
+    fix16_t anti_dz_outer;
+
+    fix16_t curve;
+} trigger_settings_t;
+_Static_assert(sizeof(trigger_settings_t) == 20, "Trigger settings size mismatch");
+_Static_assert((sizeof(trigger_settings_t) % 4) == 0, "Trigger settings size mismatch");
+
+typedef struct __attribute__((packed, aligned(4))) {
+    fix16_t dz_inner;
+    fix16_t dz_outer;
+
+    fix16_t anti_dz_circle;
+    fix16_t anti_dz_circle_y_scale;
+    fix16_t anti_dz_square;
+    fix16_t anti_dz_square_y_scale;
+    fix16_t anti_dz_angular;
+    fix16_t anti_dz_outer;
+    
+    fix16_t axis_restrict;
+    fix16_t angle_restrict;
+    
+    fix16_t diag_scale_min;
+    fix16_t diag_scale_max;
+
+    fix16_t curve;
+
+    uint8_t uncap_radius;
+    uint8_t invert_y;
+    uint8_t invert_x;
+
+    uint8_t reserved;
+} joystick_settings_t;
+_Static_assert(sizeof(joystick_settings_t) == 56, "Joystick settings size mismatch");
+_Static_assert((sizeof(joystick_settings_t) % 4) == 0, "Joystick settings size mismatch");
+
+typedef struct __attribute__((packed, aligned(4))) {
+    uint8_t id; /* Profile ID */
+
+    union {
+        struct {
+            uint8_t d_up; /* Uint8 bit offset */
+            uint8_t d_down; /* Uint8 bit offset */
+            uint8_t d_left; /* Uint8 bit offset */
+            uint8_t d_right; /* Uint8 bit offset */
+        };
+        uint8_t dpad[GAMEPAD_DPAD_BIT_COUNT]; /* Raw array of uint8 bit offsets */
+    };
+
+    union {
+        struct {
+            uint8_t btn_a; /* Uint16 bit offset */
+            uint8_t btn_b; /* Uint16 bit offset */
+            uint8_t btn_x; /* Uint16 bit offset */
+            uint8_t btn_y; /* Uint16 bit offset */
+            uint8_t btn_l3; /* Uint16 bit offset */
+            uint8_t btn_r3; /* Uint16 bit offset */
+            uint8_t btn_lb; /* Uint16 bit offset */
+            uint8_t btn_rb; /* Uint16 bit offset */
+            uint8_t btn_lt; /* Uint16 bit offset */
+            uint8_t btn_rt; /* Uint16 bit offset */
+            uint8_t btn_back; /* Uint16 bit offset */
+            uint8_t btn_start; /* Uint16 bit offset */
+            uint8_t btn_sys; /* Uint16 bit offset */
+            uint8_t btn_misc; /* Uint16 bit offset */
+        };
+        uint8_t btns[GAMEPAD_BTN_BIT_COUNT]; /* Raw array of uint16 bit offsets */
+    };
+
+    uint8_t analog_en; /* Analog buttons enabled */
+
+    union {
+        struct {
+            uint8_t a_up; /* Analog button array byte offset */
+            uint8_t a_down; /* Analog button array byte offset */
+            uint8_t a_left; /* Analog button array byte offset */
+            uint8_t a_right; /* Analog button array byte offset */
+            uint8_t a_a; /* Analog button array byte offset */
+            uint8_t a_b; /* Analog button array byte offset */
+            uint8_t a_x; /* Analog button array byte offset */
+            uint8_t a_y; /* Analog button array byte offset */
+            uint8_t a_lb; /* Analog button array byte offset */
+            uint8_t a_rb; /* Analog button array byte offset */
+        };
+        uint8_t analog[GAMEPAD_ANALOG_COUNT]; /* Raw array of analog button array byte offsets */
+    };
+
+    uint8_t reserved[2];
+
+    joystick_settings_t joystick_l;
+    joystick_settings_t joystick_r;
+    trigger_settings_t trigger_l;
+    trigger_settings_t trigger_r;
+} user_profile_t;
+_Static_assert(sizeof(user_profile_t) == 184, "User profile size mismatch");
+_Static_assert((sizeof(user_profile_t) % 4) == 0, "User profile size mismatch");
+
+typedef struct __attribute__((packed, aligned(4))) {
+    float dz_inner;
+    float dz_outer;
+
+    float anti_dz_inner;
+    float anti_dz_outer;
+
+    float curve;
+} trigger_settings_f_t;
+// _Static_assert(sizeof(trigger_settings_t) == 20, "Trigger settings size mismatch");
+// _Static_assert((sizeof(trigger_settings_t) % 4) == 0, "Trigger settings size mismatch");
+
+typedef struct __attribute__((packed, aligned(4))) {
+    float dz_inner;
+    float dz_outer;
+
+    float anti_dz_circle;
+    float anti_dz_circle_y_scale;
+    float anti_dz_square;
+    float anti_dz_square_y_scale;
+    float anti_dz_angular;
+    float anti_dz_outer;
+    
+    float axis_restrict;
+    float angle_restrict;
+    
+    float diag_scale_min;
+    float diag_scale_max;
+
+    float curve;
+
+    uint8_t uncap_radius;
+    uint8_t invert_y;
+    uint8_t invert_x;
+
+    uint8_t reserved;
+} joystick_settings_f_t;
+// _Static_assert(sizeof(joystick_settings_t) == 56, "Joystick settings size mismatch");
+// _Static_assert((sizeof(joystick_settings_t) % 4) == 0, "Joystick settings size mismatch");
+
+typedef struct __attribute__((packed, aligned(4))) {
+    uint8_t id; /* Profile ID */
+
+    union {
+        struct {
+            uint8_t d_up; /* Uint8 bit offset */
+            uint8_t d_down; /* Uint8 bit offset */
+            uint8_t d_left; /* Uint8 bit offset */
+            uint8_t d_right; /* Uint8 bit offset */
+        };
+        uint8_t dpad[GAMEPAD_DPAD_BIT_COUNT]; /* Raw array of uint8 bit offsets */
+    };
+
+    union {
+        struct {
+            uint8_t btn_a; /* Uint16 bit offset */
+            uint8_t btn_b; /* Uint16 bit offset */
+            uint8_t btn_x; /* Uint16 bit offset */
+            uint8_t btn_y; /* Uint16 bit offset */
+            uint8_t btn_l3; /* Uint16 bit offset */
+            uint8_t btn_r3; /* Uint16 bit offset */
+            uint8_t btn_lb; /* Uint16 bit offset */
+            uint8_t btn_rb; /* Uint16 bit offset */
+            uint8_t btn_lt; /* Uint16 bit offset */
+            uint8_t btn_rt; /* Uint16 bit offset */
+            uint8_t btn_back; /* Uint16 bit offset */
+            uint8_t btn_start; /* Uint16 bit offset */
+            uint8_t btn_sys; /* Uint16 bit offset */
+            uint8_t btn_misc; /* Uint16 bit offset */
+        };
+        uint8_t btns[GAMEPAD_BTN_BIT_COUNT]; /* Raw array of uint16 bit offsets */
+    };
+
+    uint8_t analog_en; /* Analog buttons enabled */
+
+    union {
+        struct {
+            uint8_t a_up; /* Analog button array byte offset */
+            uint8_t a_down; /* Analog button array byte offset */
+            uint8_t a_left; /* Analog button array byte offset */
+            uint8_t a_right; /* Analog button array byte offset */
+            uint8_t a_a; /* Analog button array byte offset */
+            uint8_t a_b; /* Analog button array byte offset */
+            uint8_t a_x; /* Analog button array byte offset */
+            uint8_t a_y; /* Analog button array byte offset */
+            uint8_t a_lb; /* Analog button array byte offset */
+            uint8_t a_rb; /* Analog button array byte offset */
+        };
+        uint8_t analog[GAMEPAD_ANALOG_COUNT]; /* Raw array of analog button array byte offsets */
+    };
+
+    uint8_t reserved[2];
+
+    joystick_settings_f_t joystick_l;
+    joystick_settings_f_t joystick_r;
+    trigger_settings_f_t trigger_l;
+    trigger_settings_f_t trigger_r;
+} user_profile_f_t;
+// _Static_assert(sizeof(user_profile_f_t) == 184, "User profile size mismatch");
+// _Static_assert((sizeof(user_profile_f_t) % 4) == 0, "User profile size mismatch");
+
+void settings_init(void);
+usbd_type_t settings_get_device_type(void);
+void settings_get_profile_by_index(uint8_t index, user_profile_t* profile);
+void settings_get_profile_by_id(uint8_t id, user_profile_t* profile);
+uint8_t settings_get_active_profile_id(uint8_t index);
+void settings_store_device_type(usbd_type_t type);
+void settings_store_profile(uint8_t index, const user_profile_t* profile);
+
+void settings_get_default_profile(user_profile_t* profile);
+bool settings_is_default_joystick(const joystick_settings_t* joy_set);
+bool settings_is_default_trigger(const trigger_settings_t* trig_set);
+
+static inline void settings_scale_trigger(const trigger_settings_t* set, uint8_t* trigger) {
+
+}
+
+static inline void settings_scale_joysticks(const joystick_settings_t* set, int16_t* joy_x, int16_t* joy_y) {
+    // #define fix16_i(x) fix16_from_int(x)
+    // #define fix16_f(x) fix16_from_float(x)
+
+    // #define FIX_90 fix16_f(90.0f)
+    // #define FIX_100 fix16_f(100.0f)
+    // #define FIX_180 fix16_f(180.0f)
+    // #define FIX_EPSILON fix16_f(0.0001f)
+    // #define FIX_EPSILON2 fix16_f(0.001f)
+    // #define FIX_ELLIPSE_DEF fix16_f(1.570796f)
+    // #define FIX_DIAG_DIVISOR fix16_f(0.29289f)
+
+    // fix16_t x = fix16_div(  set->invert_x 
+    //                             ? fix16_i(range_invert_int16(*joy_x)) 
+    //                             : fix16_i(*joy_x), 
+    //                         fix16_i(R_INT16_MAX));
+    // fix16_t y = fix16_div(  (set->invert_y ^ invert_y) 
+    //                             ? fix16_i(range_invert_int16(*joy_y)) 
+    //                             : fix16_i(*joy_y), 
+    //                         fix16_i(R_INT16_MAX));
+
+    // const fix16_t abs_x = fix16_abs(x);
+    // const fix16_t abs_y = fix16_abs(y);
+    // const fix16_t inv_axis_restrict = fix16_f(1.0f) / (fix16_f(1.0f) - set->axis_restrict);
+
+    // fix16_t rAngle = (abs_x < FIX_EPSILON) 
+    //     ? FIX_90 
+    //     : fix16_rad2deg(fix16_abs(fix16_atan(fix16_div(y, x))));
+
+    // fix16_t axial_x = ((abs_x <= set->axis_restrict) && (rAngle > fix16_f(45.0f))) 
+    //     ? fix16_f(0.0f) 
+    //     : fix16_mul((abs_x - set->axis_restrict), inv_axis_restrict);
+            
+    // fix16_t axial_y = ((abs_y <= set->axis_restrict) && (rAngle <= fix16_f(45.0f))) 
+    //     ? fix16_f(0.0f) 
+    //     : fix16_mul((abs_y - set->axis_restrict), inv_axis_restrict);
+
+    // fix16_t in_magnitude = fix16_sqrt(fix16_sq(axial_x) + fix16_sq(axial_y));
+
+    // if (in_magnitude < set->dz_inner) {
+    //     *joy_x = 0;
+    //     *joy_y = 0;
+    //     return;
+    // }
+
+    // fix16_t angle = 
+    //     fix16_abs(axial_x) < FIX_EPSILON 
+    //         ? FIX_90 
+    //         : fix16_rad2deg(fix16_abs(fix16_atan(axial_y / axial_x)));
+
+    // fix16_t anti_r_scale =  (set->anti_dz_square_y_scale == fix16_f(0.0f)) 
+    //                         ? set->anti_dz_square : set->anti_dz_square_y_scale;
+    // fix16_t anti_dz_c = set->anti_dz_circle;
+
+    // if ((anti_r_scale > fix16_f(0.0f)) && (anti_dz_c > fix16_f(0.0f))) {
+    //     fix16_t anti_ellip_scale = fix16_div(anti_ellip_scale, anti_dz_c);
+    //     fix16_t ellipse_angle = fix16_atan(fix16_mul(fix16_div(fix16_f(1.0f), anti_ellip_scale), fix16_tan(fix16_rad2deg(rAngle))));
+    //     ellipse_angle = (ellipse_angle < fix16_f(0.0f)) ? FIX_ELLIPSE_DEF : ellipse_angle;
+
+    //     fix16_t ellipse_x = fix16_cos(ellipse_angle);
+    //     fix16_t ellipse_y = fix16_sqrt(fix16_mul(fix16_sq(anti_ellip_scale), (fix16_f(1.0f) - fix16_sq(ellipse_x))));
+    //     anti_dz_c = fix16_mul(fix16_sqrt(fix16_sq(ellipse_x) + fix16_sq(ellipse_y)), anti_dz_c);
+    // }
+
+    // if (anti_dz_c > fix16_f(0.0f)) {
+    //     fix16_t a = fix16_mul(anti_dz_c, (fix16_f(1.0f) - fix16_div(set->anti_dz_circle, set->dz_outer)));
+    //     fix16_t b = fix16_mul(anti_dz_c, (fix16_f(1.0f) - set->anti_dz_square));
+    //     anti_dz_c = fix16_div(anti_dz_c, fix16_div(a, b));
+    //     // anti_dz_c = anti_dz_c / ((anti_dz_c * (fix16_f(1.0f) - set->anti_dz_circle / set->dz_outer)) / (anti_dz_c * (fix16_f(1.0f) - set->anti_dz_square)));
+    // }
+
+    // if ((abs_x > set->axis_restrict) && (abs_y > set->axis_restrict)) {
+    //     const fix16_t FIX_ANGLE_MAX = fix16_div(set->angle_restrict, fix16_f(2.0f));
+
+    //     if ((angle > fix16_f(0.0f)) && (angle < FIX_ANGLE_MAX)) {
+    //         angle = fix16_f(0.0f);
+    //     }
+    //     if (angle > (FIX_90 - FIX_ANGLE_MAX)) {
+    //         angle = FIX_90;
+    //     }
+    //     if (angle > FIX_ANGLE_MAX && angle < (FIX_90 - FIX_ANGLE_MAX)) {
+    //         angle = fix16_div(fix16_mul((angle - FIX_ANGLE_MAX), FIX_90), ((FIX_90 - FIX_ANGLE_MAX) - FIX_ANGLE_MAX));
+    //     }
+    // }
+
+    // fix16_t ref_angle = (angle < FIX_EPSILON2) ? fix16_f(0.0f) : angle;
+    // fix16_t diagonal =  (angle > fix16_f(45.0f)) 
+    //                     ? fix16_div(fix16_mul(angle - fix16_f(45.0f), (-fix16_f(45.0f))), fix16_f(45.0f)) + fix16_f(45.0f) 
+    //                     : angle;
+
+    // const fix16_t angle_comp = set->angle_restrict / fix16_f(2.0f);
+
+    // if (angle < FIX_90 && angle > fix16_f(0.0f)) {
+    //     angle = ((angle * ((FIX_90 - angle_comp) - angle_comp)) / FIX_90) + angle_comp;
+    // }
+
+    // if ((axial_x < fix16_f(0.0f)) && (axial_y > fix16_f(0.0f))) {
+    //     angle = -angle;
+    // }
+    // if ((axial_x > fix16_f(0.0f)) && (axial_y < fix16_f(0.0f))) {
+    //     angle = angle - FIX_180;
+    // }
+    // if ((axial_x < fix16_f(0.0f)) && (axial_y < fix16_f(0.0f))) {
+    //     angle = angle + FIX_180;
+    // }
+
+    // //Deadzone Warp
+    // fix16_t out_magnitude = (in_magnitude - set->dz_inner) / (set->anti_dz_outer - set->dz_inner);
+    // out_magnitude = fix16_pow(out_magnitude, (fix16_f(1.0f) / set->curve)) * (set->dz_outer - anti_dz_c) + anti_dz_c;
+    // out_magnitude = (out_magnitude > set->dz_outer && !set->uncap_radius) ? set->dz_outer : out_magnitude;
+
+    // fix16_t d_scale = (((out_magnitude - anti_dz_c) * (set->diag_scale_max - set->diag_scale_min)) / (set->dz_outer - anti_dz_c)) + set->diag_scale_min;		
+    // fix16_t c_scale = (diagonal * (fix16_f(1.0f) / fix16_sqrt(fix16_f(2.0f)))) / fix16_f(45.0f); //Both these lines scale the intensity of the warping
+    // c_scale       = fix16_f(1.0f) - fix16_sqrt(fix16_f(1.0f) - c_scale * c_scale);     //based on a circular curve to the perfect diagonal
+    // d_scale       = (c_scale * (d_scale - fix16_f(1.0f))) / FIX_DIAG_DIVISOR + fix16_f(1.0f);
+
+    // out_magnitude = out_magnitude * d_scale;
+
+    // //Scaling values for square antideadzone
+    // fix16_t new_x = fix16_cos(fix16_deg2rad(angle)) * out_magnitude;
+    // fix16_t new_y = fix16_sin(fix16_deg2rad(angle)) * out_magnitude;
+    
+    // //Magic angle wobble fix by user ME.
+    // // if (angle > 45.0 && angle < 225.0) {
+    // // 	newX = inv(Math.sin(deg2rad(angle - 90.0)))*outputMagnitude;
+    // // 	newY = inv(Math.cos(deg2rad(angle - 270.0)))*outputMagnitude;
+    // // }
+    
+    // //Square antideadzone scaling
+    // fix16_t output_x = fix16_abs(new_x) * (fix16_f(1.0f) - fix16_div(set->anti_dz_square, set->dz_outer)) + set->anti_dz_square;
+    // if (x < fix16_f(0.0f)) {
+    //     output_x = -output_x;
+    // }
+    // if (ref_angle == FIX_90) {
+    //     output_x = fix16_f(0.0f);
+    // }
+    
+    // fix16_t output_y = fix16_abs(new_y) * (fix16_f(1.0f) - anti_r_scale / set->dz_outer) + anti_r_scale;
+    // if (y < fix16_f(0.0f)) {
+    //     output_y = -output_y;
+    // }
+    // if (ref_angle == fix16_f(0.0f)) {
+    //     output_y = fix16_f(0.0f);
+    // }
+
+    // output_x = fix16_clamp(output_x, -fix16_f(1.0f), fix16_f(1.0f)) * fix16_i(R_INT16_MAX);
+    // output_y = fix16_clamp(output_y, -fix16_f(1.0f), fix16_f(1.0f)) * fix16_i(R_INT16_MAX);
+
+    // *joy_x = (int16_t)fix16_to_int(output_x);
+    // *joy_y = (int16_t)fix16_to_int(output_y);
+
+    // // return { static_cast<int16_t>(fix16_to_int(output_x)), static_cast<int16_t>(fix16_to_int(output_y)) };
+}
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/wired/wired.h
+++ b/Firmware/ESP32/main/wired/wired.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "gamepad/gamepad.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef void (*wired_begin_cb_t)(void);
+typedef void (*wired_rumble_cb_t)(uint8_t index, const gamepad_rumble_t* rumble);
+typedef void (*wired_audio_cb_t)(uint8_t index, const gamepad_pcm_out_t* pcm);
+
+void wired_config(wired_rumble_cb_t rumble_cb, wired_audio_cb_t audio_cb);
+void wired_task(void* args);
+void wired_set_connected(uint8_t index, bool connected);
+void wired_set_pad(uint8_t index, const gamepad_pad_t* pad, uint32_t flags);
+void wired_set_audio(uint8_t index, const gamepad_pcm_in_t* pcm) __attribute__((weak));
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/wired/wired_i2c.c
+++ b/Firmware/ESP32/main/wired/wired_i2c.c
@@ -0,0 +1,144 @@
+#include "sdkconfig.h"
+#if defined(CONFIG_I2C_ENABLED)
+
+#include <freertos/FreeRTOS.h>
+#include <esp_private/periph_ctrl.h>
+#include <esp_rom_gpio.h>
+#include <esp_clk_tree.h>
+#include <soc/clk_tree_defs.h>
+#include <rom/lldesc.h>
+#include <hal/gpio_ll.h>
+#include <hal/i2c_ll.h>
+#include <hal/i2c_hal.h>
+#include <esp_timer.h>
+
+#include "log/log.h"
+#include "periph/i2c_master.h"
+#include "wired/wired.h"
+#include "wired/wired_ring.h"
+
+#define I2C_PORT                0
+#define I2C_HW                  I2C_LL_GET_HW(I2C_PORT)
+#define I2C_PIN_SDA             21
+#define I2C_PIN_SCL             22
+#define I2C_PIN_IRQ             3
+#define I2C_BAUD                (1*1000*1000)
+#define I2C_TIMEOUT_US          (10U*1000)
+#define I2C_GLITCH_IGNORE_CNT   7
+
+#define SLAVE_ADDR ((uint8_t)0x40)
+
+typedef struct {
+    i2c_master_handle_t*    handle;
+    ring_wired_t            ring_i2c;
+    uint8_t                 bt_buf_tx[WIRED_MAX_SIZE] __attribute__((aligned(4))); /* Belongs to bt thread */
+    uint8_t                 i2c_buf_tx[WIRED_MAX_SIZE] __attribute__((aligned(4))); /* Belongs to i2c thread */
+    uint8_t                 i2c_buf_rx[WIRED_MAX_SIZE] __attribute__((aligned(4))); /* Belongs to i2c thread */
+    wired_rumble_cb_t       rumble_cb;
+} i2c_state_t;
+
+static i2c_state_t i2c_state = {0};
+
+static void parse_rx(void) {
+    const wired_packet_t* packet = 
+        (const wired_packet_t*)i2c_state.i2c_buf_rx;
+    switch (packet->report_id) {
+    case WIRED_REPORT_ID_RUMBLE:
+        if (i2c_state.rumble_cb != NULL) {
+            i2c_state.rumble_cb(packet->index, 
+                                (gamepad_rumble_t*)packet->payload);
+        }
+        ogxm_logd("I2C: Gamepad %d rumble set\n", packet->index);
+        break;
+    default:
+        ogxm_logd_hex(packet, WIRED_MAX_SIZE,
+                      "I2C: Received unknown report %02X:", 
+                      packet->report_id);
+        break;
+    }
+    memset(i2c_state.i2c_buf_rx, 0, WIRED_MAX_SIZE);
+}
+
+void wired_task(void* args) {
+    (void)args;
+
+    i2c_master_cfg cfg = {
+        .port_num = I2C_PORT,
+        .freq_hz = I2C_BAUD,
+        .pin_sda = I2C_PIN_SDA,
+        .pin_scl = I2C_PIN_SCL,
+        .lsb_first = false,
+        .pullup_en = true,
+        .glitch_ignore_cnt = I2C_GLITCH_IGNORE_CNT,
+    };
+    
+    i2c_master_handle_t* handle = i2c_master_init(&cfg);
+    if (handle == NULL) {
+        ogxm_loge("I2C: Failed to initialize I2C\n");
+        while (1) {
+            vTaskDelay(pdMS_TO_TICKS(1000));
+        }
+    }
+
+    gpio_ll_func_sel(&GPIO, I2C_PIN_IRQ, FUNC_GPIO5_GPIO5);
+    gpio_ll_input_enable(&GPIO, I2C_PIN_IRQ);
+    gpio_ll_pullup_en(&GPIO, I2C_PIN_IRQ);
+
+    vTaskDelay(pdMS_TO_TICKS(1000));
+
+    while (true) {
+        if ((gpio_ll_get_level(&GPIO, I2C_PIN_IRQ) == 0)) {
+            if (i2c_master_read_blocking(handle, SLAVE_ADDR, i2c_state.i2c_buf_rx, 
+                                         WIRED_MAX_SIZE, I2C_TIMEOUT_US)) {
+                parse_rx();
+            } else {
+                ogxm_loge("I2C: Failed to read from slave\n");
+            }
+        }
+        if (ring_wired_pop(&i2c_state.ring_i2c, i2c_state.i2c_buf_tx)) {
+            wired_packet_t* packet = (wired_packet_t*)i2c_state.i2c_buf_tx;
+            if (packet->report_id == WIRED_REPORT_ID_CONNECTED) {
+                ogxm_logd("I2C: Gamepad %d connect event, connected=%d\n",
+                          packet->index, packet->payload[0]);
+            }
+            i2c_master_write_blocking(handle, SLAVE_ADDR, i2c_state.i2c_buf_tx, 
+                                      WIRED_MAX_SIZE, I2C_TIMEOUT_US);
+        }
+        vTaskDelay(1);
+    }
+}
+
+void wired_config(wired_rumble_cb_t rumble_cb, wired_audio_cb_t audio_cb) {
+    (void)audio_cb;
+    memset(&i2c_state, 0, sizeof(i2c_state_t));
+    i2c_state.rumble_cb = rumble_cb;
+}
+
+void wired_set_connected(uint8_t index, bool connected) {
+    ogxm_logd("I2C: Set gamepad %d connected=%d\n", index, connected);
+    wired_packet_t* packet = (wired_packet_t*)i2c_state.bt_buf_tx;
+    packet->report_id = WIRED_REPORT_ID_CONNECTED;
+    packet->index = index;
+    packet->reserved = 0U;
+    packet->payload_len = 2U;
+    packet->payload[0] = connected ? 1U : 0U;
+    packet->payload[1] = 0U; // Reserved
+    ring_wired_push(&i2c_state.ring_i2c, i2c_state.bt_buf_tx, true);
+}
+
+void wired_set_pad(uint8_t index, const gamepad_pad_t* pad, uint32_t flags) {
+    wired_packet_t* packet = (wired_packet_t*)i2c_state.bt_buf_tx;
+    packet->report_id = WIRED_REPORT_ID_PAD;
+    packet->index = index;
+    packet->reserved = 0U;
+    packet->payload_len = sizeof(gamepad_pad_t);
+    memcpy(packet->payload, pad, sizeof(gamepad_pad_t));
+    ring_wired_push(&i2c_state.ring_i2c, i2c_state.bt_buf_tx, false);
+}
+
+void wired_set_audio(uint8_t index, const gamepad_pcm_in_t* pcm) {
+    (void)index;
+    (void)pcm;
+}
+
+#endif // CONFIG_I2C_ENABLED
--- a/Firmware/ESP32/main/wired/wired_ring.h
+++ b/Firmware/ESP32/main/wired/wired_ring.h
@@ -0,0 +1,84 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <stdatomic.h>
+#include "gamepad/gamepad.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef MAX
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+#endif
+
+#define WIRED_RING_SIZE 16U
+#if defined(CONFIG_I2C_ENABLED)
+#define WIRED_MAX_SIZE (sizeof(gamepad_pad_t) + sizeof(wired_packet_t))
+#else
+#define WIRED_MAX_SIZE (MAX(sizeof(gamepad_pad_t), sizeof(gamepad_pcm_in_t)) + sizeof(wired_packet_t))
+#endif
+
+typedef enum {
+    WIRED_REPORT_ID_NONE = 0,
+    WIRED_REPORT_ID_CONNECTED,
+    WIRED_REPORT_ID_PAD,
+    WIRED_REPORT_ID_RUMBLE,
+    WIRED_REPORT_ID_PCM,
+} wired_report_id_t;
+
+typedef struct __attribute__((packed, aligned(4))) {
+    uint8_t report_id;
+    uint8_t index;
+    uint8_t reserved;
+    uint8_t payload_len;
+    uint8_t payload[];
+} wired_packet_t;
+_Static_assert(sizeof(wired_packet_t) == 4, "Wired packet size mismatch");
+
+typedef struct {
+    uint8_t buf[WIRED_RING_SIZE][WIRED_MAX_SIZE] __attribute__((aligned(4)));
+    bool no_overwrite[WIRED_RING_SIZE];
+    atomic_uint head;
+    atomic_uint tail;
+} ring_wired_t;
+
+static inline bool ring_wired_push(ring_wired_t* ring, const uint8_t* data, bool no_overwrite) {
+    uint32_t head = atomic_load_explicit(&ring->head, memory_order_relaxed);
+    uint32_t next_head = (head + 1) % WIRED_RING_SIZE;
+    uint32_t tail = atomic_load_explicit(&ring->tail, memory_order_acquire);
+
+    if (next_head == tail) {
+        if (ring->no_overwrite[tail]) {
+            ogxm_loge("SPI ring buffer full, cannot push data\n");
+            return false;
+        }
+        atomic_store_explicit(&ring->tail, (tail + 1) % WIRED_RING_SIZE, memory_order_release);
+    }
+    memcpy(ring->buf[head], data, WIRED_MAX_SIZE);
+    ring->no_overwrite[head] = no_overwrite;
+    atomic_store_explicit(&ring->head, next_head, memory_order_release);
+    return true;
+}
+
+static inline bool ring_wired_pop(ring_wired_t* ring, uint8_t* data) {
+    uint32_t tail = atomic_load_explicit(&ring->tail, memory_order_relaxed);
+    uint32_t head = atomic_load_explicit(&ring->head, memory_order_acquire);
+    if (tail == head) {
+        return false;
+    }
+    memcpy(data, ring->buf[tail], WIRED_MAX_SIZE);
+    atomic_store_explicit(&ring->tail, (tail + 1) % WIRED_RING_SIZE, memory_order_release);
+    return true;
+}
+
+static inline bool ring_wired_empty(const ring_wired_t* ring) {
+    return (atomic_load_explicit(&ring->head, memory_order_relaxed) ==
+           atomic_load_explicit(&ring->tail, memory_order_acquire));
+}
+
+#ifdef __cplusplus
+}
+#endif
--- a/Firmware/ESP32/main/wired/wired_spi.c
+++ b/Firmware/ESP32/main/wired/wired_spi.c
@@ -0,0 +1,258 @@
+#include "sdkconfig.h"
+#if defined(CONFIG_SPI_ENABLED)
+
+#include <freertos/FreeRTOS.h>
+#include <esp_private/periph_ctrl.h>
+#include <rom/lldesc.h>
+#include <hal/gpio_ll.h>
+#include <hal/spi_ll.h>
+#include <hal/spi_hal.h>
+
+#include "log/log.h"
+#include "wired/wired.h"
+#include "wired/wired_ring.h"
+
+#define SPI_DEVICE      SPI2_HOST
+#define SPI_PIN_CS      15
+#define SPI_PIN_SCK     14
+#define SPI_PIN_MOSI    13 
+#define SPI_PIN_MISO    12 
+#define SPI_PIN_IRQ     5 
+
+typedef struct {
+    spi_hal_context_t       hal;
+    spi_hal_dev_config_t    hal_dev_cfg;
+    int                     dma_channel;
+    ring_wired_t            ring_spi;
+    uint8_t                 bt_buf_tx[WIRED_MAX_SIZE] __attribute__((aligned(4))); /* Belongs to bt thread */
+    uint8_t*                dma_buf_tx; /* Belongs to spi thread */
+    uint8_t*                dma_buf_rx; /* Belongs to spi thread */
+    lldesc_t*               dma_desc_tx;
+    lldesc_t*               dma_desc_rx;
+    wired_rumble_cb_t       rumble_cb;
+    wired_audio_cb_t        audio_cb;
+} spi_state_t;
+
+static spi_state_t spi_state = {0};
+// static const size_t spi_state_size = sizeof(spi_state_t);
+
+static void spi_init(void) {
+    spi_state.dma_channel = 1;
+    PERIPH_RCC_ATOMIC() {
+        spi_ll_enable_bus_clock(SPI_DEVICE, true);
+        spi_ll_reset_register(SPI_DEVICE);
+        spi_dma_ll_enable_bus_clock(spi_state.dma_channel, true);
+        spi_dma_ll_reset_register(spi_state.dma_channel);
+        /* Link DMA */
+        DPORT_SET_PERI_REG_BITS(DPORT_SPI_DMA_CHAN_SEL_REG, 3, spi_state.dma_channel, (SPI_DEVICE * 2));
+    }
+
+    gpio_ll_iomux_func_sel(IO_MUX_GPIO14_REG, FUNC_MTMS_HSPICLK);
+    gpio_ll_iomux_func_sel(IO_MUX_GPIO13_REG, FUNC_MTCK_HSPID);  
+    gpio_ll_iomux_func_sel(IO_MUX_GPIO12_REG, FUNC_MTDI_HSPIQ);  
+    gpio_ll_iomux_func_sel(IO_MUX_GPIO15_REG, FUNC_MTDO_HSPICS0);
+
+    gpio_ll_output_enable(&GPIO, SPI_PIN_SCK); 
+    gpio_ll_output_enable(&GPIO, SPI_PIN_MOSI);
+    gpio_ll_input_enable(&GPIO, SPI_PIN_MISO); 
+    gpio_ll_output_enable(&GPIO, SPI_PIN_CS);
+
+    spi_hal_timing_param_t timing = {
+        .clk_src_hz = APB_CLK_FREQ,
+        .half_duplex = false,
+        .no_compensate = false,
+        .expected_freq = 1*1000*1000,
+        .duty_cycle = 128,
+        .input_delay_ns = 0,
+        .use_gpio = true,
+    };
+
+    spi_state.hal_dev_cfg.mode = 3;
+    spi_state.hal_dev_cfg.cs_setup = 1;
+    spi_state.hal_dev_cfg.cs_hold = 1;
+    spi_state.hal_dev_cfg.cs_pin_id = -1;
+    ESP_ERROR_CHECK(spi_hal_cal_clock_conf(&timing, &spi_state.hal_dev_cfg.timing_conf));
+    spi_state.hal_dev_cfg.sio = 0;
+    spi_state.hal_dev_cfg.half_duplex = 0;
+    spi_state.hal_dev_cfg.tx_lsbfirst = 0;
+    spi_state.hal_dev_cfg.rx_lsbfirst = 0;
+    spi_state.hal_dev_cfg.no_compensate = 0;
+    spi_state.hal_dev_cfg.as_cs = 0;
+    spi_state.hal_dev_cfg.positive_cs = 0;
+
+    spi_hal_init(&spi_state.hal, SPI_DEVICE);
+    spi_hal_setup_device(&spi_state.hal, &spi_state.hal_dev_cfg);
+    spi_hal_enable_data_line(spi_state.hal.hw, true, true);
+
+    gpio_ll_output_enable(&GPIO, SPI_PIN_CS);
+    gpio_ll_set_level(&GPIO, SPI_PIN_CS, 1);
+
+    gpio_ll_input_enable(&GPIO, SPI_PIN_IRQ);
+    gpio_ll_pullup_en(&GPIO, SPI_PIN_IRQ);
+
+    spi_state.dma_buf_tx = heap_caps_malloc(WIRED_MAX_SIZE, MALLOC_CAP_DMA);
+    spi_state.dma_buf_rx = heap_caps_malloc(WIRED_MAX_SIZE, MALLOC_CAP_DMA);
+    spi_state.dma_desc_tx = heap_caps_malloc(sizeof(lldesc_t), MALLOC_CAP_DMA);
+    spi_state.dma_desc_rx = heap_caps_malloc(sizeof(lldesc_t), MALLOC_CAP_DMA);
+    if ((spi_state.dma_buf_tx == NULL) || 
+        (spi_state.dma_buf_rx == NULL) ||
+        (spi_state.dma_desc_rx == NULL) || 
+        (spi_state.dma_desc_tx == NULL)) {
+        ogxm_loge("SPI: Failed to allocate DMA buffers\n");
+        abort();
+    }
+    memset(spi_state.dma_buf_tx, 0, WIRED_MAX_SIZE);
+    memset(spi_state.dma_buf_rx, 0, WIRED_MAX_SIZE);
+    memset(spi_state.dma_desc_tx, 0, sizeof(lldesc_t));
+    memset(spi_state.dma_desc_rx, 0, sizeof(lldesc_t));
+
+    spi_state.dma_desc_rx->size = WIRED_MAX_SIZE;
+    spi_state.dma_desc_rx->length = WIRED_MAX_SIZE;
+    spi_state.dma_desc_rx->eof = 1;
+    spi_state.dma_desc_rx->owner = DMA_DESCRIPTOR_BUFFER_OWNER_DMA;
+    spi_state.dma_desc_rx->buf = spi_state.dma_buf_rx;
+    spi_state.dma_desc_rx->empty = 0;
+
+    spi_state.dma_desc_tx->size = WIRED_MAX_SIZE;
+    spi_state.dma_desc_tx->length = WIRED_MAX_SIZE;
+    spi_state.dma_desc_tx->eof = 1;
+    spi_state.dma_desc_tx->owner = DMA_DESCRIPTOR_BUFFER_OWNER_DMA;
+    spi_state.dma_desc_tx->buf = spi_state.dma_buf_tx;
+    spi_state.dma_desc_tx->empty = 0;
+}
+
+static bool sync_slave(void) {
+    return true;
+}
+
+static void transmit_blocking(void) {
+    spi_hal_trans_config_t xfer_cfg = {
+        .cmd = 0,
+        .cmd_bits = 0,
+        .addr = 0,
+        .addr_bits = 0,
+        .dummy_bits = 0,
+        .tx_bitlen = WIRED_MAX_SIZE * 8U,
+        .rx_bitlen = WIRED_MAX_SIZE * 8U,
+        .send_buffer = spi_state.dma_buf_tx,
+        .rcv_buffer = spi_state.dma_buf_rx,
+        .line_mode = {1,1,1},
+        .cs_keep_active = false,
+    };
+
+    ogxm_logd_hex(spi_state.dma_buf_tx, WIRED_MAX_SIZE, "SPI TX Buffer before:");
+
+    gpio_ll_set_level(&GPIO, SPI_PIN_CS, 0);
+
+    spi_hal_setup_trans(&spi_state.hal, &spi_state.hal_dev_cfg, &xfer_cfg);
+    spi_hal_hw_prepare_tx(spi_state.hal.hw);
+    spi_hal_hw_prepare_rx(spi_state.hal.hw);
+    spi_dma_ll_rx_start(spi_state.hal.hw, spi_state.dma_channel, spi_state.dma_desc_rx);
+    spi_dma_ll_tx_start(spi_state.hal.hw, spi_state.dma_channel, spi_state.dma_desc_tx);
+
+    spi_ll_user_start(spi_state.hal.hw);
+    while (!spi_ll_usr_is_done(spi_state.hal.hw)) {
+        // Wait
+    }
+
+    gpio_ll_set_level(&GPIO, SPI_PIN_CS, 1);
+
+    ogxm_logd_hex(spi_state.dma_buf_tx, WIRED_MAX_SIZE, "SPI TX Buffer after:");
+}
+
+static void parse_rx(void) {
+    const wired_packet_t* packet = 
+        (const wired_packet_t*)spi_state.dma_buf_rx;
+    switch (packet->report_id) {
+    case WIRED_REPORT_ID_PCM:
+        if (spi_state.audio_cb != NULL) {
+            spi_state.audio_cb(packet->index, 
+                            (gamepad_pcm_out_t*)packet->payload);
+        }
+        ogxm_logd("SPI: Gamepad %d PCM data received\n", packet->index);
+        break;
+    case WIRED_REPORT_ID_RUMBLE:
+        if (spi_state.rumble_cb != NULL) {
+            spi_state.rumble_cb(packet->index, 
+                                (gamepad_rumble_t*)packet->payload);
+        }
+        ogxm_logd("SPI: Gamepad %d rumble set\n", packet->index);
+        break;
+    default:
+        ogxm_logd_hex(packet, WIRED_MAX_SIZE,
+                      "SPI: Received unknown report %02X:", 
+                      packet->report_id);
+        break;
+    }
+    memset(spi_state.dma_buf_rx, 0, WIRED_MAX_SIZE);
+}
+
+void wired_task(void* args) {
+    (void)args;
+
+    spi_init();
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    transmit_blocking();
+
+    while (true) {
+        if (gpio_ll_get_level(&GPIO, SPI_PIN_IRQ) == 1) {
+            vTaskDelay(pdMS_TO_TICKS(1));
+            continue;
+        }
+        if (ring_wired_pop(&spi_state.ring_spi, spi_state.dma_buf_tx)) {
+            wired_packet_t* packet = (wired_packet_t*)spi_state.dma_buf_tx;
+            if (packet->report_id == WIRED_REPORT_ID_CONNECTED) {
+                ogxm_logd("SPI: Gamepad %d connect event, connected=%d\n",
+                          packet->index, packet->payload[0]);
+            }
+            transmit_blocking();
+            parse_rx();
+        } 
+        else {
+            vTaskDelay(pdMS_TO_TICKS(1));
+            continue;
+        } 
+        // spi_transmit_blocking();
+        vTaskDelay(1);
+    }
+}
+
+void wired_config(wired_rumble_cb_t rumble_cb, wired_audio_cb_t audio_cb) {
+    memset(&spi_state, 0, sizeof(spi_state_t));
+    spi_state.rumble_cb = rumble_cb;
+    spi_state.audio_cb = audio_cb;
+}
+
+void wired_set_connected(uint8_t index, bool connected) {
+    ogxm_logd("SPI: Set gamepad %d connected=%d\n", index, connected);
+    wired_packet_t* packet = (wired_packet_t*)spi_state.bt_buf_tx;
+    packet->report_id = WIRED_REPORT_ID_CONNECTED;
+    packet->index = index;
+    packet->reserved = 0U;
+    packet->payload_len = 2U;
+    packet->payload[0] = connected ? 1U : 0U;
+    packet->payload[1] = 0U; // Reserved
+    ring_wired_push(&spi_state.ring_spi, spi_state.bt_buf_tx, true);
+}
+
+void wired_set_pad(uint8_t index, const gamepad_pad_t* pad, uint32_t flags) {
+    wired_packet_t* packet = (wired_packet_t*)spi_state.bt_buf_tx;
+    packet->report_id = WIRED_REPORT_ID_PAD;
+    packet->index = index;
+    packet->reserved = 0U;
+    packet->payload_len = sizeof(gamepad_pad_t);
+    memcpy(packet->payload, pad, sizeof(gamepad_pad_t));
+    ring_wired_push(&spi_state.ring_spi, spi_state.bt_buf_tx, false);
+}
+
+void wired_set_audio(uint8_t index, const gamepad_pcm_in_t* pcm) {
+    wired_packet_t* packet = (wired_packet_t*)spi_state.bt_buf_tx;
+    packet->report_id = WIRED_REPORT_ID_PCM;
+    packet->index = index;
+    packet->reserved = 0U;
+    packet->payload_len = sizeof(gamepad_pcm_in_t);
+    memcpy(packet->payload, pcm, sizeof(gamepad_pcm_in_t));
+    ring_wired_push(&spi_state.ring_spi, spi_state.bt_buf_tx, false);
+}
+
+#endif // CONFIG_SPI_ENABLED
--- a/Firmware/ESP32/sdkconfig
+++ b/Firmware/ESP32/sdkconfig
--- a/Firmware/FWDefines.cmake
+++ b/Firmware/FWDefines.cmake
@@ -1,2 +0,0 @@
-set(FW_NAME "OGX-Mini")
-set(FW_VERSION "v1.0.0a3")
--- a/Firmware/RP2040/.vscode/settings.json
+++ b/Firmware/RP2040/.vscode/settings.json
@@ -1,13 +0,0 @@
-{
-    "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
-
-    "cmake.configureArgs": [
-        "-DOGXM_BOARD=ESP32_BLUERETRO_I2C",
-        // "-DOGXM_RETAIL=TRUE",
-        "-DMAX_GAMEPADS=1"
-    ],
-    
-    "files.associations": {
-
-    }
-}
--- a/Firmware/RP2040/CMakeLists.txt
+++ b/Firmware/RP2040/CMakeLists.txt
@@ -1,417 +0,0 @@
-cmake_minimum_required(VERSION 3.13)
-
-include(${CMAKE_CURRENT_LIST_DIR}/../FWDefines.cmake)
-
-set(CMAKE_C_COMPILER arm-none-eabi-gcc)
-set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
-
-set(CMAKE_C_STANDARD 11)
-set(CMAKE_CXX_STANDARD 20)
-set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
-
-set(SRC ${CMAKE_CURRENT_LIST_DIR}/src)
-set(EXTERNAL_DIR ${CMAKE_CURRENT_LIST_DIR}/../external)
-set(PICOSDK_VERSION_TAG "2.1.0")
-
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/init_submodules.cmake)
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/patch_libs.cmake)
-include(${CMAKE_CURRENT_LIST_DIR}/../cmake/generate_gatt_header.cmake)
-include(${CMAKE_CURRENT_LIST_DIR}/cmake/get_pico_sdk.cmake)
-
-set(PICO_PIO_USB_PATH ${EXTERNAL_DIR}/Pico-PIO-USB)
-set(PICO_TINYUSB_PATH ${EXTERNAL_DIR}/tinyusb)
-set(BLUEPAD32_ROOT ${EXTERNAL_DIR}/bluepad32)
-set(BTSTACK_ROOT ${BLUEPAD32_ROOT}/external/btstack)
-set(PICO_BTSTACK_PATH ${BTSTACK_ROOT})
-set(LIBFIXMATH_PATH ${EXTERNAL_DIR}/libfixmath)
-
-get_pico_sdk(${EXTERNAL_DIR} ${PICOSDK_VERSION_TAG})
-init_git_submodules(${EXTERNAL_DIR}
-    ${BLUEPAD32_ROOT}
-    ${PICO_TINYUSB_PATH}
-)
-apply_lib_patches(${EXTERNAL_DIR})
-
-set(SOURCES_BOARD 
-    ${SRC}/main.cpp
-
-    ${SRC}/OGXMini/OGXMini.cpp
-    ${SRC}/OGXMini/Board/Standard.cpp
-    ${SRC}/OGXMini/Board/PicoW.cpp
-    ${SRC}/OGXMini/Board/Four_Channel_I2C.cpp
-    ${SRC}/OGXMini/Board/ESP32_Bluepad32_I2C.cpp
-    ${SRC}/OGXMini/Board/ESP32_Blueretro_I2C.cpp
-
-    ${SRC}/TaskQueue/TaskQueue.cpp
-
-    ${SRC}/Board/ogxm_log.cpp
-    ${SRC}/Board/esp32_api.cpp
-    ${SRC}/Board/board_api.cpp
-    ${SRC}/Board/board_api_private/board_api_led.cpp
-    ${SRC}/Board/board_api_private/board_api_rgb.cpp
-    ${SRC}/Board/board_api_private/board_api_bt.cpp
-    ${SRC}/Board/board_api_private/board_api_usbh.cpp
-    
-    ${SRC}/UserSettings/UserSettings.cpp
-    ${SRC}/UserSettings/UserProfile.cpp
-    ${SRC}/UserSettings/JoystickSettings.cpp
-    ${SRC}/UserSettings/TriggerSettings.cpp
-
-    ${SRC}/USBDevice/tud_callbacks.cpp
-    ${SRC}/USBDevice/DeviceManager.cpp
-    ${SRC}/USBDevice/DeviceDriver/DeviceDriver.cpp
-    ${SRC}/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp
-    ${SRC}/USBDevice/DeviceDriver/PS3/PS3.cpp
-    ${SRC}/USBDevice/DeviceDriver/Switch/Switch.cpp
-    ${SRC}/USBDevice/DeviceDriver/XInput/XInput.cpp
-    ${SRC}/USBDevice/DeviceDriver/XboxOG/XboxOG_GP.cpp
-    ${SRC}/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp
-    ${SRC}/USBDevice/DeviceDriver/XboxOG/XboxOG_XR.cpp
-    ${SRC}/USBDevice/DeviceDriver/DInput/DInput.cpp
-    ${SRC}/USBDevice/DeviceDriver/WebApp/WebApp.cpp
-    ${SRC}/USBDevice/DeviceDriver/XInput/tud_xinput/tud_xinput.cpp
-    ${SRC}/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid.cpp
-)
-set(LIBS_BOARD
-    # Base
-    pico_stdlib
-    pico_multicore
-    pico_rand
-    hardware_timer
-    hardware_clocks
-    hardware_flash
-    tinyusb_device
-    tinyusb_board
-    # UART
-    hardware_uart
-    hardware_irq
-    #fix16
-    libfixmath
-)
-set(INC_DIRS_BOARD
-)
-
-# Config options
-
-set(MAX_GAMEPADS 1 CACHE STRING "Set number of gamepads, 1 to 4")
-if (MAX_GAMEPADS GREATER 4 OR MAX_GAMEPADS LESS 1)
-    message(FATAL_ERROR "MAX_GAMEPADS must be between 1 and 4")
-endif()
-add_definitions(-DMAX_GAMEPADS=${MAX_GAMEPADS})
-
-set(OGXM_BOARD "PI_PICO" CACHE STRING "Set board type, options can be found in src/board_config.h")
-set(FLASH_SIZE_MB 2)
-set(PICO_BOARD none)
-
-if (OGXM_BOARD STREQUAL "PI_PICO")
-    add_compile_definitions(CONFIG_OGXM_BOARD_PI_PICO=1)
-    set(EN_USB_HOST TRUE)
-
-elseif (OGXM_BOARD STREQUAL "PI_PICO2")
-    add_compile_definitions(CONFIG_OGXM_BOARD_PI_PICO2=1)
-    set(EN_USB_HOST TRUE)
-    set(PICO_PLATFORM rp2350)
-    set(FLASH_SIZE_MB 4)
-
-elseif(OGXM_BOARD STREQUAL "PI_PICOW")
-    add_compile_definitions(CONFIG_OGXM_BOARD_PI_PICOW=1)
-    set(EN_BLUETOOTH TRUE)
-    set(PICO_BOARD pico_w)
-
-elseif(OGXM_BOARD STREQUAL "PI_PICO2W")
-    add_compile_definitions(CONFIG_OGXM_BOARD_PI_PICO2W=1)
-    set(EN_BLUETOOTH TRUE)
-    set(PICO_BOARD pico2_w)
-    set(PICO_PLATFORM rp2350)
-    set(FLASH_SIZE_MB 4)
-
-elseif(OGXM_BOARD STREQUAL "ADAFRUIT_FEATHER")
-    add_compile_definitions(CONFIG_OGXM_BOARD_ADAFRUIT_FEATHER=1)
-    set(EN_USB_HOST TRUE)
-    set(EN_RGB TRUE)
-    set(FLASH_SIZE_MB 8)
-
-elseif(OGXM_BOARD STREQUAL "RP2040_ZERO")
-    add_compile_definitions(CONFIG_OGXM_BOARD_RP2040_ZERO=1)
-    set(EN_USB_HOST TRUE)
-    set(EN_RGB TRUE)
-
-# elseif(OGXM_BOARD STREQUAL "INTERNAL_4CH")
-#     set(EN_USB_HOST TRUE)
-#     set(EN_4CH TRUE)
-
-elseif(OGXM_BOARD STREQUAL "EXTERNAL_4CH_I2C")
-    add_compile_definitions(CONFIG_OGXM_BOARD_EXTERNAL_4CH=1)
-    set(EN_USB_HOST TRUE)
-    set(EN_4CH TRUE)
-    set(EN_RGB TRUE)
-
-elseif(OGXM_BOARD STREQUAL "ESP32_BLUEPAD32_I2C")
-    add_compile_definitions(CONFIG_OGXM_BOARD_ESP32_BLUEPAD32_I2C=1)
-    set(EN_ESP32 TRUE)
-    set(EN_UART_BRIDGE TRUE)
-
-    if(OGXM_RETAIL STREQUAL "TRUE")
-        message(STATUS "Retail mode enabled.")
-        add_compile_definitions(OGXM_RETAIL=1)
-    endif()
-
-elseif(OGXM_BOARD STREQUAL "ESP32_BLUERETRO_I2C")
-    add_compile_definitions(CONFIG_OGXM_BOARD_ESP32_BLUERETRO_I2C=1)
-    set(EN_ESP32 TRUE)
-    set(EN_BLUERETRO_I2C TRUE)
-    set(EN_UART_BRIDGE TRUE)
-
-    if(OGXM_RETAIL STREQUAL "TRUE")
-        message(STATUS "Retail mode enabled.")
-        add_compile_definitions(OGXM_RETAIL=1)
-    endif()
-
-else()
-    message(FATAL_ERROR "Invalid OGXM_BOARD value. See options in src/board_config.h")
-
-endif()
-
-add_definitions(-DOGXM_BOARD=${OGXM_BOARD})
-
-if(EN_USB_HOST)
-    message(STATUS "USB host enabled.")
-    add_compile_definitions(CONFIG_EN_USB_HOST=1)
-    list(APPEND SOURCES_BOARD
-        ${SRC}/USBHost/tuh_callbacks.cpp
-        # HID
-        ${SRC}/USBHost/HostDriver/DInput/DInput.cpp
-        ${SRC}/USBHost/HostDriver/PSClassic/PSClassic.cpp
-        ${SRC}/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
-        ${SRC}/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
-        ${SRC}/USBHost/HostDriver/PS5/PS5.cpp
-        ${SRC}/USBHost/HostDriver/PS4/PS4.cpp
-        ${SRC}/USBHost/HostDriver/PS3/PS3.cpp
-        ${SRC}/USBHost/HostDriver/N64/N64.cpp
-        ${SRC}/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
-
-        ${SRC}/USBHost/HIDParser/HIDJoystick.cpp
-        ${SRC}/USBHost/HIDParser/HIDReportDescriptor.cpp
-        ${SRC}/USBHost/HIDParser/HIDReportDescriptorElements.cpp
-        ${SRC}/USBHost/HIDParser/HIDReportDescriptorUsages.cpp
-        ${SRC}/USBHost/HIDParser/HIDUtils.cpp
-
-        # XInput
-        ${SRC}/USBHost/HostDriver/XInput/XboxOG.cpp
-        ${SRC}/USBHost/HostDriver/XInput/XboxOne.cpp
-        ${SRC}/USBHost/HostDriver/XInput/Xbox360.cpp
-        ${SRC}/USBHost/HostDriver/XInput/Xbox360W.cpp
-        ${SRC}/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.cpp
-    )
-    list(APPEND LIBS_BOARD
-        tinyusb_host
-        tinyusb_pico_pio_usb
-    )
-endif()
-
-if(EN_BLUETOOTH)
-    add_compile_definitions(CONFIG_EN_BLUETOOTH=1)
-    message(STATUS "Bluetooth enabled.")
-
-    generate_gatt_header(${BTSTACK_ROOT} ${SRC}/BLEServer/att_delayed_response.gatt ${SRC}/BLEServer/att_delayed_response.h)
-    
-    list(APPEND SOURCES_BOARD
-        ${SRC}/Bluepad32/Bluepad32.cpp
-        ${SRC}/BLEServer/BLEServer.cpp
-    )
-    list(APPEND INC_DIRS_BOARD
-        ${SRC}
-        ${BTSTACK_ROOT}/3rd-party/bluedroid/encoder/include
-        ${BTSTACK_ROOT}/3rd-party/bluedroid/decoder/include
-    )
-    list(APPEND LIBS_BOARD
-        pico_cyw43_arch_none
-        pico_btstack_classic
-        pico_btstack_cyw43
-        bluepad32
-    )
-endif()
-
-if(EN_RGB)
-    add_compile_definitions(CONFIG_EN_RGB=1)
-    message(STATUS "RGB enabled.")
-    list(APPEND SOURCES_BOARD
-        ${SRC}/Board/Pico_WS2812/WS2812.cpp
-    )
-    list(APPEND LIBS_BOARD
-        hardware_pio
-    )
-endif()
-
-if(EN_4CH)
-    add_compile_definitions(CONFIG_EN_4CH=1)
-    message(STATUS "4CH enabled.")
-    # list(APPEND SOURCES_BOARD
-    #     ${SRC}/I2CDriver/4Channel/I2CMaster.cpp
-    #     ${SRC}/I2CDriver/4Channel/I2CSlave.cpp
-    # )
-    list(APPEND LIBS_BOARD
-        hardware_i2c
-        pico_i2c_slave
-    )
-endif()
-
-if(EN_ESP32)
-    add_compile_definitions(CONFIG_EN_ESP32=1)
-    message(STATUS "ESP32 enabled.")
-    if (EN_BLUERETRO_I2C)
-        # Nothing
-    else()
-        list(APPEND LIBS_BOARD
-            pico_i2c_slave
-        )
-    endif()
-    list(APPEND LIBS_BOARD
-        hardware_i2c
-    )
-endif()
-
-if(EN_UART_BRIDGE)
-    add_compile_definitions(CONFIG_EN_UART_BRIDGE=1)
-    message(STATUS "UART bridge enabled.")
-    list(APPEND SOURCES_BOARD
-        ${SRC}/USBDevice/DeviceDriver/UARTBridge/UARTBridge.cpp
-        ${SRC}/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c
-    )
-endif()
-
-string(TIMESTAMP CURRENT_DATETIME "%Y-%m-%d %H:%M:%S")
-add_compile_definitions(BUILD_DATETIME="${CURRENT_DATETIME}")
-add_compile_definitions(FIRMWARE_NAME="${FW_NAME}")
-add_compile_definitions(FIRMWARE_VERSION="${FW_VERSION}")
-add_compile_definitions(PICO_FLASH_SIZE_BYTES=${FLASH_SIZE_MB}*1024*1024)
-add_compile_definitions(NVS_SECTORS=4)
-
-# Check for DVD dongle firmware
-if(EXISTS ${SRC}/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid_xremote_rom.h)
-    message(STATUS "XRemote ROM available.")
-    add_definitions(-DXREMOTE_ROM_AVAILABLE)
-endif()
-
-if(NOT EN_BLUETOOTH)
-    add_compile_definitions(PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H=1)
-    add_compile_definitions(PICO_XOSC_STARTUP_DELAY_MULTIPLIER=64)
-endif()
-
-include(${PICO_SDK_PATH}/pico_sdk_init.cmake)
-
-message("PICO_SDK_VERSION_STRING: ${PICO_SDK_VERSION_STRING}")
-if(PICO_SDK_VERSION_STRING VERSION_LESS "${PICOSDK_VERSION_TAG}")
-    message(FATAL_ERROR 
-        "Raspberry Pi Pico SDK version ${PICOSDK_VERSION_TAG} (or later) required. 
-        Your version is ${PICO_SDK_VERSION_STRING}"
-    )
-endif()
-
-project(${FW_NAME} C CXX ASM)
-
-pico_sdk_init()
-
-add_executable(${FW_NAME} ${SOURCES_BOARD})
-
-set(BUILD_STR "")
-
-if(CMAKE_BUILD_TYPE STREQUAL "Debug")
-    if(OGXM_BOARD STREQUAL "PI_PICOW")
-        message(FATAL_ERROR "Debug build will not work with the Pico W currently")
-    endif()
-
-    message(STATUS "Debug build enabled.")
-    set(BUILD_STR "-Debug")
-
-    set(TX_PIN 0)
-    set(RX_PIN 1)
-    set(UART_PORT)
-
-    include(${CMAKE_CURRENT_LIST_DIR}/cmake/get_pico_uart_port.cmake)
-    get_pico_uart_port(${TX_PIN} UART_PORT)
-
-    message(STATUS "UART port: ${UART_PORT}, TX: ${TX_PIN}, RX: ${RX_PIN}")
-
-    pico_enable_stdio_uart(${FW_NAME} 1)
-    target_compile_definitions(${FW_NAME} PRIVATE
-        PICO_DEFAULT_UART=${UART_PORT}
-        PICO_DEFAULT_UART_TX_PIN=${TX_PIN}
-        PICO_DEFAULT_UART_RX_PIN=${RX_PIN}
-    )
- 
-    add_compile_definitions(CFG_TUSB_DEBUG=1)
-    add_compile_definitions(OGXM_DEBUG=1)
-
-    target_compile_options(${FW_NAME} PRIVATE
-        -Wall                # Enable most warnings
-        -Wextra              # Enable extra warnings
-        -Wconversion         # Warn on type conversion issues
-        -Wsign-conversion    # Warn on sign conversion issues
-        -Wno-unused-parameter # Disable warnings for unused parameters
-        # -Werror              # Treat warnings as errors
-    )
-
-elseif(CMAKE_BUILD_TYPE STREQUAL "Release")
-    pico_enable_stdio_uart(${FW_NAME} 0)
-    add_compile_definitions(CFG_TUSB_DEBUG=0)
-    add_compile_options(
-        -O3                     # Optimize for speed
-        # -mcpu=cortex-m0plus     # Target ARM Cortex-M0+
-        -mthumb                 # Use Thumb instruction set
-        -ffunction-sections     # Place each function in its own section
-        -fdata-sections         # Place each data item in its own section
-        -fomit-frame-pointer    # Omit frame pointers
-        -finline-functions      # Enable inlining
-    )
-
-    add_link_options(
-        -Wl,--gc-sections       # Remove unused sections
-        -flto                   # Link-Time Optimization
-    )
-endif()
-
-pico_set_program_name(${FW_NAME} ${FW_NAME})
-pico_set_program_version(${FW_NAME} ${FW_VERSION})
-
-target_include_directories(${FW_NAME} PRIVATE 
-    ${SRC}
-)
-
-if(EN_RGB)
-    pico_generate_pio_header(${FW_NAME} ${SRC}/Board/Pico_WS2812/WS2812.pio)
-endif()
-
-include_directories(${INC_DIRS_BOARD})
-
-if(EN_USB_HOST)
-    add_subdirectory(${PICO_PIO_USB_PATH} ${CMAKE_BINARY_DIR}/Pico-PIO-USB)
-endif()
-
-if(EN_BLUETOOTH)
-    add_subdirectory(${BLUEPAD32_ROOT}/src/components/bluepad32 libbluepad32)
-endif()
-
-add_subdirectory(${LIBFIXMATH_PATH} libfixmath)
-
-target_link_libraries(${FW_NAME} PRIVATE ${LIBS_BOARD})
-
-target_compile_definitions(libfixmath PRIVATE
-    FIXMATH_FAST_SIN
-    FIXMATH_NO_64BIT
-    FIXMATH_NO_CACHE
-    FIXMATH_NO_HARD_DIVISION
-    FIXMATH_NO_OVERFLOW
-    # FIXMATH_NO_ROUNDING
-    # FIXMATH_OPTIMIZE_8BIT
-)
-
-if(OGXM_RETAIL STREQUAL "TRUE")
-    set(EXE_FILENAME "${FW_NAME}-${FW_VERSION}-${OGXM_BOARD}${BUILD_STR}-Retail")
-else()
-    set(EXE_FILENAME "${FW_NAME}-${FW_VERSION}-${OGXM_BOARD}${BUILD_STR}")
-endif()
-
-set_target_properties(${FW_NAME} PROPERTIES OUTPUT_NAME ${EXE_FILENAME})
-
-pico_add_extra_outputs(${FW_NAME})
--- a/Firmware/RP2040/cmake/get_pico_sdk.cmake
+++ b/Firmware/RP2040/cmake/get_pico_sdk.cmake
@@ -1,37 +0,0 @@
-function(get_pico_sdk EXTERNAL_DIR VERSION_TAG)
-    if(NOT DEFINED ENV{PICO_SDK_PATH})
-        message("PICO_SDK_PATH not set")
-        set(PICO_SDK_PATH ${EXTERNAL_DIR}/pico-sdk PARENT_SCOPE)
-
-        if(NOT EXISTS ${PICO_SDK_PATH})
-            message("Cloning pico-sdk to ${PICO_SDK_PATH}")
-            execute_process(
-                COMMAND git clone --recursive https://github.com/raspberrypi/pico-sdk.git
-                WORKING_DIRECTORY ${EXTERNAL_DIR}
-            )
-
-        endif()
-
-        execute_process(
-            COMMAND git fetch --tags
-            WORKING_DIRECTORY ENV{PICO_SDK_PATH}
-            )
-    
-        execute_process(
-            COMMAND git checkout tags/${VERSION_TAG}
-            WORKING_DIRECTORY ENV{PICO_SDK_PATH}
-            )
-    
-        execute_process(
-            COMMAND git submodule update --init --recursive 
-            WORKING_DIRECTORY ENV{PICO_SDK_PATH}
-            )
-
-    else()
-        message("Using PICO_SDK_PATH from environment: $ENV{PICO_SDK_PATH}")
-        set(PICO_SDK_PATH ENV{PICO_SDK_PATH} PARENT_SCOPE)
-
-    endif()
-
-    set(PICOTOOL_FETCH_FROM_GIT_PATH ${EXTERNAL_DIR}/picotool PARENT_SCOPE)
-endfunction()
--- a/Firmware/RP2040/cmake/get_pico_uart_port.cmake
+++ b/Firmware/RP2040/cmake/get_pico_uart_port.cmake
@@ -1,9 +0,0 @@
-function(get_pico_uart_port TX_PIN_IN PICO_UART_PORT_OUT)
-    if(TX_PIN_IN EQUAL 0 OR TX_PIN_IN EQUAL 12 OR TX_PIN_IN EQUAL 16)
-        set(${PICO_UART_PORT_OUT} 0 PARENT_SCOPE)
-    elseif(TX_PIN_IN EQUAL 4 OR TX_PIN_IN EQUAL 8)
-        set(${PICO_UART_PORT_OUT} 1 PARENT_SCOPE)
-    else()
-        message(FATAL_ERROR "Invalid TX pin for Pico UART")
-    endif()
-endfunction()
--- a/Firmware/RP2040/src/BLEServer/BLEServer.cpp
+++ b/Firmware/RP2040/src/BLEServer/BLEServer.cpp
@@ -1,324 +0,0 @@
-#include <cstring>
-#include <string>
-#include <algorithm>
-
-#include "att_delayed_response.h"
-#include "btstack.h"
-
-#include "BLEServer/BLEServer.h"
-#include "UserSettings/UserProfile.h"
-#include "UserSettings/UserSettings.h"
-#include "TaskQueue/TaskQueue.h"
-
-namespace BLEServer {
-
-static constexpr uint16_t PACKET_LEN_MAX = 20;
-
-namespace Handle {
-    static constexpr uint16_t FW_VERSION    = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789020_01_VALUE_HANDLE;
-    static constexpr uint16_t FW_NAME       = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789021_01_VALUE_HANDLE;
-
-    static constexpr uint16_t SETUP_READ  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
-    static constexpr uint16_t SETUP_WRITE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
-    static constexpr uint16_t GET_SETUP   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789032_01_VALUE_HANDLE;
-
-    static constexpr uint16_t PROFILE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
-
-    static constexpr uint16_t GAMEPAD  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789050_01_VALUE_HANDLE;
-}
-
-namespace ADV {
-    // Flags general discoverable, BR/EDR not supported
-    static const uint8_t FLAGS[]    = { 0x02, 0x01, 0x06 };
-    static const uint8_t NAME_TYPE  = 0x09;
-
-    #pragma pack(push, 1)
-    struct Data {
-        uint8_t flags[sizeof(FLAGS)];
-        uint8_t name_len;
-        uint8_t name_type;
-        uint8_t name[sizeof(FIRMWARE_NAME) - 1];
-
-        Data() {
-            std::memcpy(flags, FLAGS, sizeof(flags));
-            name_len = sizeof(FIRMWARE_NAME);
-            name_type = NAME_TYPE;
-            std::string fw_name = FIRMWARE_NAME;
-            std::memcpy(name, fw_name.c_str(), std::min(sizeof(name), fw_name.size()));
-        }
-    };
-    static_assert(sizeof(Data) == 5 + sizeof(FIRMWARE_NAME) - 1, "BLEServer::ADV::Data struct size mismatch");
-    #pragma pack(pop)
-}
-
-#pragma pack(push, 1)
-struct SetupPacket {
-    DeviceDriverType device_type{DeviceDriverType::NONE};
-    uint8_t max_gamepads{MAX_GAMEPADS};
-    uint8_t player_idx{0};
-    uint8_t profile_id{0};
-};
-static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
-#pragma pack(pop)
-
-class ProfileReader {
-public:
-    ProfileReader() = default;
-    ~ProfileReader() = default;
-
-    void set_setup_packet(const SetupPacket& setup_packet) {
-        setup_packet_ = setup_packet;
-        current_offset_ = 0;
-    }
-
-    const SetupPacket& get_setup_packet() const {
-        return setup_packet_;
-    }
-
-    uint16_t get_xfer_len() {
-        return static_cast<uint16_t>(std::min(static_cast<size_t>(PACKET_LEN_MAX), sizeof(UserProfile) - current_offset_));
-    }
-
-    uint16_t get_profile_data(uint8_t* buffer, uint16_t buffer_len) {
-        size_t copy_len = get_xfer_len();
-        if (!buffer || buffer_len < copy_len) {
-            return 0;
-        }
-        if (current_offset_ == 0 && !set_profile()) {
-            return 0;
-        }
-        std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile_) + current_offset_, copy_len);
-        current_offset_ += copy_len;
-        if (current_offset_ >= sizeof(UserProfile)) {
-            current_offset_ = 0;
-        }
-        return copy_len;
-    }
-
-private:
-    SetupPacket setup_packet_;
-    UserProfile profile_;
-    size_t current_offset_ = 0;
-
-    bool set_profile() {
-        if (setup_packet_.profile_id == 0xFF) {
-            if (setup_packet_.player_idx >= UserSettings::MAX_PROFILES) {
-                return false;
-            }
-            profile_ = UserSettings::get_instance().get_profile_by_index(setup_packet_.player_idx);
-        } else {
-            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES) {
-                return false;
-            }
-            profile_ = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
-        }
-        return true;
-    }
-};
-
-class ProfileWriter
-{
-public:
-    ProfileWriter() = default;
-    ~ProfileWriter() = default;
-
-    void set_setup_packet(const SetupPacket& setup_packet) {
-        setup_packet_ = setup_packet;
-        current_offset_ = 0;
-    }
-
-    const SetupPacket& get_setup_packet() const {
-        return setup_packet_;
-    }
-
-    uint16_t get_xfer_len() {
-        return static_cast<uint16_t>(std::min(static_cast<size_t>(PACKET_LEN_MAX), sizeof(UserProfile) - current_offset_));
-    }
-
-    size_t set_profile_data(const uint8_t* buffer, uint16_t buffer_len) {
-        size_t copy_len = get_xfer_len();
-        if (!buffer || buffer_len < copy_len) {
-            return 0;
-        }
-
-        std::memcpy(reinterpret_cast<uint8_t*>(&profile_) + current_offset_, buffer, copy_len);
-
-        current_offset_ += copy_len;
-        size_t ret = current_offset_;
-
-        if (current_offset_ >= sizeof(UserProfile)) {
-            current_offset_ = 0;
-        }
-        return ret;
-    }
-
-    bool commit_profile() {
-        bool success = false;
-        if (setup_packet_.device_type != DeviceDriverType::NONE) {
-            success = TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
-                [driver_type = setup_packet_.device_type, profile = profile_, index = setup_packet_.player_idx]
-                {
-                    UserSettings::get_instance().store_profile_and_driver_type(driver_type, index, profile);
-                });
-        } else {
-            success = TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
-                [index = setup_packet_.player_idx, profile = profile_]
-                {
-                    UserSettings::get_instance().store_profile(index, profile);
-                });
-        }
-        return success;
-    }
-
-private:
-    SetupPacket setup_packet_;
-    UserProfile profile_;
-    size_t current_offset_ = 0;
-};
-
-std::array<Gamepad*, MAX_GAMEPADS> gamepads_;
-ProfileReader profile_reader_;
-ProfileWriter profile_writer_;
-
-static int verify_write(const uint16_t buffer_size, const uint16_t expected_size) {
-    if (buffer_size != expected_size) {
-        return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
-    }
-    return 0;
-}
-
-static void disconnect_client_cb(btstack_timer_source_t *ts) {
-    hci_con_handle_t connection_handle = *static_cast<hci_con_handle_t*>(ts->context);
-    hci_send_cmd(&hci_disconnect, connection_handle);
-    delete static_cast<hci_con_handle_t*>(ts->context);
-}
-
-static void queue_disconnect(hci_con_handle_t connection_handle, uint32_t dealy_ms) {
-    static btstack_timer_source_t disconnect_timer;
-
-    hci_con_handle_t* connection_handle_ptr = new hci_con_handle_t(connection_handle);
-
-    disconnect_timer.process = disconnect_client_cb;
-    disconnect_timer.context = connection_handle_ptr;
-
-    btstack_run_loop_set_timer(&disconnect_timer, dealy_ms);
-    btstack_run_loop_add_timer(&disconnect_timer);
-}
-
-static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
-                                    uint16_t att_handle,
-                                    uint16_t offset,
-                                    uint8_t *buffer,
-                                    uint16_t buffer_size) {
-    std::string fw_version;
-    std::string fw_name;
-    Gamepad::PadIn pad_in;
-
-    switch (att_handle) {
-        case Handle::FW_VERSION:
-            fw_version = FIRMWARE_VERSION;
-            if (buffer)  {
-                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_version.c_str()), fw_version.size());
-            }
-            return static_cast<uint16_t>(fw_version.size());
-
-        case Handle::FW_NAME:
-            fw_name = FIRMWARE_NAME;
-            if (buffer) {
-                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_name.c_str()), fw_name.size());;
-            }
-            return static_cast<uint16_t>(fw_name.size());
-
-        case Handle::GET_SETUP:
-            if (buffer) {
-                buffer[0] = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
-                buffer[1] = MAX_GAMEPADS;
-                buffer[2] = 0;
-                buffer[3] = UserSettings::get_instance().get_active_profile_id(0);
-            }
-            return static_cast<uint16_t>(sizeof(SetupPacket));
-
-        case Handle::PROFILE:
-            if (buffer) {
-                return profile_reader_.get_profile_data(buffer, buffer_size);
-            }
-            return profile_reader_.get_xfer_len();
-
-        case Handle::GAMEPAD:
-            if (buffer) {
-                pad_in = gamepads_.front()->get_pad_in();
-                std::memcpy(buffer, &pad_in, sizeof(Gamepad::PadIn));
-            }
-            return static_cast<uint16_t>(sizeof(Gamepad::PadIn));
-
-        default:
-            break;
-    }
-    return 0;
-}
-
-static int att_write_callback(  hci_con_handle_t connection_handle,
-                                uint16_t att_handle,
-                                uint16_t transaction_mode,
-                                uint16_t offset,
-                                uint8_t *buffer,
-                                uint16_t buffer_size) {
-    int ret = 0;
-
-    switch (att_handle) {
-        case Handle::SETUP_READ:
-            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0) {
-                break;
-            }
-            profile_reader_.set_setup_packet(*reinterpret_cast<SetupPacket*>(buffer));
-            break;
-
-        case Handle::SETUP_WRITE:
-            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0) {
-                break;
-            }
-            profile_writer_.set_setup_packet(*reinterpret_cast<SetupPacket*>(buffer));
-            break;
-
-        case Handle::PROFILE:
-            if ((ret = verify_write(buffer_size, profile_writer_.get_xfer_len())) != 0) {
-                break;
-            }
-            if (profile_writer_.set_profile_data(buffer, buffer_size) == sizeof(UserProfile)) {
-                queue_disconnect(connection_handle, 500);
-                profile_writer_.commit_profile();
-            }
-            break;
-
-        default:
-            break;
-    }
-    return ret;
-}
-
-void init_server(Gamepad(&gamepads)[MAX_GAMEPADS]) {
-    for (uint8_t i = 0; i < MAX_GAMEPADS; i++) {
-        gamepads_[i] = &gamepads[i];
-    }
-
-    UserSettings::get_instance().initialize_flash();
-
-    // setup ATT server
-    att_server_init(profile_data, att_read_callback, att_write_callback);
-
-    // setup advertisements
-    uint16_t adv_int_min = 0x0030;
-    uint16_t adv_int_max = 0x0030;
-    uint8_t adv_type = 0;
-
-    bd_addr_t null_addr;
-    std::memset(null_addr, 0, sizeof(null_addr));
-
-    static ADV::Data adv_data = ADV::Data();
-
-    gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, 0, null_addr, 0x07, 0x00);
-    gap_advertisements_set_data(static_cast<uint8_t>(sizeof(adv_data)), reinterpret_cast<uint8_t*>(&adv_data));
-    gap_advertisements_enable(1);
-}
-
-} // namespace BLEServer
--- a/Firmware/RP2040/src/BLEServer/BLEServer.h
+++ b/Firmware/RP2040/src/BLEServer/BLEServer.h
@@ -1,9 +0,0 @@
-#pragma once
-
-#include <cstdint>
-
-#include "Gamepad/Gamepad.h"
-
-namespace BLEServer {
-    void init_server(Gamepad(&gamepads)[MAX_GAMEPADS]);
-}
--- a/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt
+++ b/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt
@@ -1,29 +0,0 @@
-PRIMARY_SERVICE, GAP_SERVICE
-CHARACTERISTIC, GAP_DEVICE_NAME, READ, "XPad"
-
-PRIMARY_SERVICE, GATT_SERVICE
-CHARACTERISTIC, GATT_DATABASE_HASH, READ,
-
-// Handle::PRIMARY_SERVICE
-PRIMARY_SERVICE, 12345678-1234-1234-1234-123456789012
-
-// Handle::FW_VERSION
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789020, READ | DYNAMIC,
-
-// Handle::FW_NAME
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789021, READ | DYNAMIC,
-
-// Handle::SETUP_READ
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789030, WRITE | DYNAMIC,
-
-// Handle::SETUP_WRITE
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789031, WRITE | DYNAMIC,
-
-// Handle::GET_SETUP
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789032, READ | DYNAMIC,
-
-// Handle::PROFILE
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789040, READ | WRITE | DYNAMIC,
-
-// Handle::GAMEPAD
-CHARACTERISTIC,  12345678-1234-1234-1234-123456789050, READ | WRITE | DYNAMIC,
--- a/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
+++ b/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
@@ -1,301 +0,0 @@
-#include <atomic>
-#include <cstring>
-#include <functional>
-#include <pico/mutex.h>
-#include <pico/cyw43_arch.h>
-
-#include "btstack_run_loop.h"
-#include "uni.h"
-
-#include "sdkconfig.h"
-#include "Bluepad32/Bluepad32.h"
-#include "Board/board_api.h"
-#include "Board/ogxm_log.h"
-
-#ifndef CONFIG_BLUEPAD32_PLATFORM_CUSTOM
-    #error "Pico W must use BLUEPAD32_PLATFORM_CUSTOM"
-#endif
-
-static_assert((CONFIG_BLUEPAD32_MAX_DEVICES == MAX_GAMEPADS), "Mismatch between BP32 and Gamepad max devices");
-
-namespace bluepad32 {
-
-static constexpr uint32_t FEEDBACK_TIME_MS = 250;
-static constexpr uint32_t LED_CHECK_TIME_MS = 500;
-
-struct BTDevice {
-    bool connected{false};
-    Gamepad* gamepad{nullptr};
-};
-
-BTDevice bt_devices_[MAX_GAMEPADS];
-btstack_timer_source_t feedback_timer_;
-btstack_timer_source_t led_timer_;
-bool led_timer_set_{false};
-bool feedback_timer_set_{false};
-
-bool any_connected()
-{
-    for (auto& device : bt_devices_)
-    {
-        if (device.connected)
-        {
-            return true;
-        }
-    }
-    return false;
-}
-
-//This solves a function pointer/crash issue with bluepad32
-void set_rumble(uni_hid_device_t* bp_device, uint16_t length, uint8_t rumble_l, uint8_t rumble_r)
-{
-    switch (bp_device->controller_type)
-    {
-        case CONTROLLER_TYPE_XBoxOneController:
-            uni_hid_parser_xboxone_play_dual_rumble(bp_device, 0, length + 10, rumble_l, rumble_r);
-            break;
-        case CONTROLLER_TYPE_AndroidController:
-            if (bp_device->vendor_id == UNI_HID_PARSER_STADIA_VID && bp_device->product_id == UNI_HID_PARSER_STADIA_PID) 
-            {
-                uni_hid_parser_stadia_play_dual_rumble(bp_device, 0, length, rumble_l, rumble_r);
-            }
-            break;
-        case CONTROLLER_TYPE_PSMoveController:
-            uni_hid_parser_psmove_play_dual_rumble(bp_device, 0, length, rumble_l, rumble_r);
-            break;
-        case CONTROLLER_TYPE_PS3Controller:
-            uni_hid_parser_ds3_play_dual_rumble(bp_device, 0, length, rumble_l, rumble_r);
-            break;
-        case CONTROLLER_TYPE_PS4Controller:
-            uni_hid_parser_ds4_play_dual_rumble(bp_device, 0, length, rumble_l, rumble_r);
-            break;
-        case CONTROLLER_TYPE_PS5Controller:
-            uni_hid_parser_ds5_play_dual_rumble(bp_device, 0, length, rumble_l, rumble_r);
-            break;
-        case CONTROLLER_TYPE_WiiController:
-            uni_hid_parser_wii_play_dual_rumble(bp_device, 0, length, rumble_l, rumble_r);
-            break;
-        case CONTROLLER_TYPE_SwitchProController:
-        case CONTROLLER_TYPE_SwitchJoyConRight:
-        case CONTROLLER_TYPE_SwitchJoyConLeft:
-            uni_hid_parser_switch_play_dual_rumble(bp_device, 0, length, rumble_l, rumble_r);
-            break;
-        default:
-            break;
-    }
-}
-
-static void send_feedback_cb(btstack_timer_source *ts)
-{
-    uni_hid_device_t* bp_device = nullptr;
-
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        if (!bt_devices_[i].connected || 
-            !(bp_device = uni_hid_device_get_instance_for_idx(i)))
-        {
-            continue;
-        }
-
-        Gamepad::PadOut gp_out = bt_devices_[i].gamepad->get_pad_out();
-        if (gp_out.rumble_l > 0 || gp_out.rumble_r > 0)
-        {
-            set_rumble(bp_device, static_cast<uint16_t>(FEEDBACK_TIME_MS), gp_out.rumble_l, gp_out.rumble_r);
-        }
-    }
-
-    btstack_run_loop_set_timer(ts, FEEDBACK_TIME_MS);
-    btstack_run_loop_add_timer(ts);
-}
-
-static void check_led_cb(btstack_timer_source *ts)
-{
-    static bool led_state = false;
-
-    led_state = !led_state;
-
-    board_api::set_led(any_connected() ? true : led_state);
-
-    btstack_run_loop_set_timer(ts, LED_CHECK_TIME_MS);
-    btstack_run_loop_add_timer(ts);
-}
-
-//BT Driver
-
-static void init(int argc, const char** arg_V) {
-}
-
-static void init_complete_cb(void) {
-    uni_bt_enable_new_connections_unsafe(true);
-    // uni_bt_del_keys_unsafe();
-    uni_property_dump_all();
-}
-
-static uni_error_t device_discovered_cb(bd_addr_t addr, const char* name, uint16_t cod, uint8_t rssi) {
-    if (!((cod & UNI_BT_COD_MINOR_MASK) & UNI_BT_COD_MINOR_GAMEPAD)) {
-        return UNI_ERROR_IGNORE_DEVICE;
-    }
-    return UNI_ERROR_SUCCESS;
-}
-
-static void device_connected_cb(uni_hid_device_t* device) {
-}
-
-static void device_disconnected_cb(uni_hid_device_t* device) {
-    int idx = uni_hid_device_get_idx_for_instance(device);
-    if (idx >= MAX_GAMEPADS || idx < 0) {
-        return;
-    }
-
-    bt_devices_[idx].connected = false;
-    bt_devices_[idx].gamepad->reset_pad_in();
-
-    if (!led_timer_set_ && !any_connected()) {
-        led_timer_set_ = true;
-        led_timer_.process = check_led_cb;
-        led_timer_.context = nullptr;
-        btstack_run_loop_set_timer(&led_timer_, LED_CHECK_TIME_MS);
-        btstack_run_loop_add_timer(&led_timer_);
-    }
-    if (feedback_timer_set_ && !any_connected()) {
-        feedback_timer_set_ = false;
-        btstack_run_loop_remove_timer(&feedback_timer_);
-    }
-}
-
-static uni_error_t device_ready_cb(uni_hid_device_t* device) {    
-    int idx = uni_hid_device_get_idx_for_instance(device);
-    if (idx >= MAX_GAMEPADS || idx < 0) {
-        return UNI_ERROR_SUCCESS;
-    }
-
-    bt_devices_[idx].connected = true;
-
-    if (led_timer_set_) {
-        led_timer_set_ = false;
-        btstack_run_loop_remove_timer(&led_timer_);
-        board_api::set_led(true);
-    }
-    if (!feedback_timer_set_) {
-        feedback_timer_set_ = true;
-        feedback_timer_.process = send_feedback_cb;
-        feedback_timer_.context = nullptr;
-        btstack_run_loop_set_timer(&feedback_timer_, FEEDBACK_TIME_MS);
-        btstack_run_loop_add_timer(&feedback_timer_);
-    }
-    return UNI_ERROR_SUCCESS;
-}
-
-static void oob_event_cb(uni_platform_oob_event_t event, void* data) {
-	return;
-}
-
-static void controller_data_cb(uni_hid_device_t* device, uni_controller_t* controller) {
-    static uni_gamepad_t prev_uni_gp[MAX_GAMEPADS] = {};
-
-    if (controller->klass != UNI_CONTROLLER_CLASS_GAMEPAD){
-        return;
-    }
-
-    uni_gamepad_t *uni_gp = &controller->gamepad;
-    int idx = uni_hid_device_get_idx_for_instance(device);
-
-    Gamepad* gamepad = bt_devices_[idx].gamepad;
-    Gamepad::PadIn gp_in;
-
-    switch (uni_gp->dpad) 
-    {
-        case DPAD_UP:
-            gp_in.dpad = gamepad->MAP_DPAD_UP;
-            break;
-        case DPAD_DOWN:
-            gp_in.dpad = gamepad->MAP_DPAD_DOWN;
-            break;
-        case DPAD_LEFT:
-            gp_in.dpad = gamepad->MAP_DPAD_LEFT;
-            break;
-        case DPAD_RIGHT:
-            gp_in.dpad = gamepad->MAP_DPAD_RIGHT;
-            break;
-        case DPAD_UP | DPAD_RIGHT:
-            gp_in.dpad = gamepad->MAP_DPAD_UP_RIGHT;
-            break;
-        case DPAD_DOWN | DPAD_RIGHT:
-            gp_in.dpad = gamepad->MAP_DPAD_DOWN_RIGHT;
-            break;
-        case DPAD_DOWN | DPAD_LEFT:
-            gp_in.dpad = gamepad->MAP_DPAD_DOWN_LEFT;
-            break;
-        case DPAD_UP | DPAD_LEFT:
-            gp_in.dpad = gamepad->MAP_DPAD_UP_LEFT;
-            break;
-        default:
-            break;
-    }
-
-    if (uni_gp->buttons & BUTTON_A) gp_in.buttons |= gamepad->MAP_BUTTON_A;
-    if (uni_gp->buttons & BUTTON_B) gp_in.buttons |= gamepad->MAP_BUTTON_B;
-    if (uni_gp->buttons & BUTTON_X) gp_in.buttons |= gamepad->MAP_BUTTON_X;
-    if (uni_gp->buttons & BUTTON_Y) gp_in.buttons |= gamepad->MAP_BUTTON_Y;
-    if (uni_gp->buttons & BUTTON_SHOULDER_L) gp_in.buttons |= gamepad->MAP_BUTTON_LB;
-    if (uni_gp->buttons & BUTTON_SHOULDER_R) gp_in.buttons |= gamepad->MAP_BUTTON_RB;
-    if (uni_gp->buttons & BUTTON_THUMB_L)    gp_in.buttons |= gamepad->MAP_BUTTON_L3;  
-    if (uni_gp->buttons & BUTTON_THUMB_R)    gp_in.buttons |= gamepad->MAP_BUTTON_R3;
-    if (uni_gp->misc_buttons & MISC_BUTTON_BACK)    gp_in.buttons |= gamepad->MAP_BUTTON_BACK;
-    if (uni_gp->misc_buttons & MISC_BUTTON_START)   gp_in.buttons |= gamepad->MAP_BUTTON_START;
-    if (uni_gp->misc_buttons & MISC_BUTTON_SYSTEM)  gp_in.buttons |= gamepad->MAP_BUTTON_SYS;
-
-    gp_in.trigger_l = gamepad->scale_trigger_l<10>(static_cast<uint16_t>(uni_gp->brake));
-    gp_in.trigger_r = gamepad->scale_trigger_r<10>(static_cast<uint16_t>(uni_gp->throttle));
-    
-    std::tie(gp_in.joystick_lx, gp_in.joystick_ly) = gamepad->scale_joystick_l<10>(uni_gp->axis_x, uni_gp->axis_y);
-    std::tie(gp_in.joystick_rx, gp_in.joystick_ry) = gamepad->scale_joystick_r<10>(uni_gp->axis_rx, uni_gp->axis_ry);
-
-    gamepad->set_pad_in(gp_in);
-}
-
-const uni_property_t* get_property_cb(uni_property_idx_t idx) 
-{
-    return nullptr;
-}
-
-uni_platform* get_driver() 
-{
-    static uni_platform driver = 
-    {
-        .name = "OGXMiniW",
-        .init = init,
-        .on_init_complete = init_complete_cb,
-        .on_device_discovered = device_discovered_cb,
-        .on_device_connected = device_connected_cb,
-        .on_device_disconnected = device_disconnected_cb,
-        .on_device_ready = device_ready_cb,
-        .on_controller_data = controller_data_cb,
-        .get_property = get_property_cb,
-        .on_oob_event = oob_event_cb,
-    };
-    return &driver;
-}
-
-//Public API
-
-void run_task(Gamepad(&gamepads)[MAX_GAMEPADS])
-{
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        bt_devices_[i].gamepad = &gamepads[i];
-    }
-
-    uni_platform_set_custom(get_driver());
-    uni_init(0, nullptr);
-
-    led_timer_set_ = true;
-    led_timer_.process = check_led_cb;
-    led_timer_.context = nullptr;
-    btstack_run_loop_set_timer(&led_timer_, LED_CHECK_TIME_MS);
-    btstack_run_loop_add_timer(&led_timer_);
-
-    btstack_run_loop_execute();
-}
-
-} // namespace bluepad32 
--- a/Firmware/RP2040/src/Bluepad32/Bluepad32.h
+++ b/Firmware/RP2040/src/Bluepad32/Bluepad32.h
@@ -1,14 +0,0 @@
-#pragma once
-
-#include <cstdint>
-#include <array>
-
-#include "Gamepad/Gamepad.h"
-#include "Board/Config.h"
-
-/*  NOTE: Everything bluepad32/uni needs to be wrapped
-    and kept away from tinyusb due to naming conflicts */
-
-namespace bluepad32 {
-    void run_task(Gamepad(&gamepads)[MAX_GAMEPADS]);
-} 
--- a/Firmware/RP2040/src/Board/Config.h
+++ b/Firmware/RP2040/src/Board/Config.h
@@ -1,129 +0,0 @@
-#ifndef _BOARD_CONFIG_H_
-#define _BOARD_CONFIG_H_
-
-#define PI_PICO             0
-#define RP2040_ZERO         1
-#define ADAFRUIT_FEATHER    2
-#define PI_PICOW            3
-#define ESP32_BLUEPAD32_I2C 4
-#define ESP32_BLUERETRO_I2C 5
-#define EXTERNAL_4CH_I2C    6
-#define INTERNAL_4CH_I2C    7
-#define BOARDS_COUNT        8
-
-#define SYSCLOCK_KHZ 240000
-
-#ifndef MAX_GAMEPADS
-    #define MAX_GAMEPADS 1
-#endif
-
-#if defined(CONFIG_OGXM_BOARD_PI_PICO) || defined(CONFIG_OGXM_BOARD_PI_PICO2)
-    #define OGXM_BOARD          PI_PICO
-    #define PIO_USB_DP_PIN      0 // DM = 1
-    #define LED_INDICATOR_PIN   25
-
-#elif defined(CONFIG_OGXM_BOARD_PI_PICOW) || defined(CONFIG_OGXM_BOARD_PI_PICO2W)
-    #define OGXM_BOARD          PI_PICOW
-
-#elif defined(CONFIG_OGXM_BOARD_RP2040_ZERO)
-    #define OGXM_BOARD          RP2040_ZERO
-    #define RGB_PXL_PIN         16 
-    #define PIO_USB_DP_PIN      10 // DM = 11
-    #define LED_INDICATOR_PIN   14
-
-#elif defined(CONFIG_OGXM_BOARD_ADAFRUIT_FEATHER)
-    #define OGXM_BOARD          ADAFRUIT_FEATHER
-    #define RGB_PWR_PIN         20
-    #define RGB_PXL_PIN         21 
-
-    #define PIO_USB_DP_PIN      16 // DM = 17
-    #define LED_INDICATOR_PIN   13
-    #define VCC_EN_PIN          18
-
-#elif defined(CONFIG_OGXM_BOARD_INTERNAL_4CH)
-    #define OGXM_BOARD          INTERNAL_4CH_I2C
-    #define PIO_USB_DP_PIN      16 // DM = 17 
-    #define FOUR_CH_ENABLED     1
-    #define I2C_SDA_PIN         10
-    #define I2C_SCL_PIN         11
-    #define SLAVE_ADDR_PIN_1    20
-    #define SLAVE_ADDR_PIN_2    21
-
-#elif defined(CONFIG_OGXM_BOARD_EXTERNAL_4CH)
-    #define OGXM_BOARD          EXTERNAL_4CH_I2C
-    #define RGB_PXL_PIN         16 
-    #define FOUR_CH_ENABLED     1
-    #define PIO_USB_DP_PIN      10 // DM = 11
-    #define I2C_SDA_PIN         6
-    #define I2C_SCL_PIN         7
-    #define SLAVE_ADDR_PIN_1    13
-    #define SLAVE_ADDR_PIN_2    14
-
-#elif defined(CONFIG_OGXM_BOARD_ESP32_BLUEPAD32_I2C)
-    #define OGXM_BOARD          ESP32_BLUEPAD32_I2C
-    #define I2C_SDA_PIN         18
-    #define I2C_SCL_PIN         19
-    #define UART0_TX_PIN        16
-    #define UART0_RX_PIN        17
-    #define MODE_SEL_PIN        21
-    #define ESP_PROG_PIN        20 // ESP32 IO0
-    #define ESP_RST_PIN         8  // ESP32 EN
-
-    #if MAX_GAMEPADS > 1
-        #undef MAX_GAMEPADS
-        #define MAX_GAMEPADS 1
-    #endif
-
-#elif defined(CONFIG_OGXM_BOARD_ESP32_BLUERETRO_I2C)
-    #define OGXM_BOARD          ESP32_BLUERETRO_I2C
-    #define I2C_SDA_PIN         18
-    #define I2C_SCL_PIN         19
-    #define UART0_TX_PIN        16
-    #define UART0_RX_PIN        17
-    #define MODE_SEL_PIN        21
-    #define ESP_PROG_PIN        20 // ESP32 IO0
-    #define ESP_RST_PIN         8  // ESP32 EN
-
-    #if MAX_GAMEPADS > 1
-        #undef MAX_GAMEPADS
-        #define MAX_GAMEPADS 1
-    #endif
-
-#else
-    #error "Invalid OGXMini board selected"
-
-#endif
-
-#if defined(CONFIG_OGXM_DEBUG)
-    //Pins and port are defined in CMakeLists.txt
-    #define DEBUG_UART_PORT __CONCAT(uart,PICO_DEFAULT_UART)
-#endif // defined(CONFIG_OGXM_DEBUG)
-
-#if defined(I2C_SDA_PIN)
-    #define I2C_BAUDRATE 400 * 1000
-    #define I2C_PORT    ((I2C_SDA_PIN == 2 ) || \
-                         (I2C_SDA_PIN == 6 ) || \
-                         (I2C_SDA_PIN == 10) || \
-                         (I2C_SDA_PIN == 14) || \
-                         (I2C_SDA_PIN == 18) || \
-                         (I2C_SDA_PIN == 26)) ? i2c1 : i2c0
-#endif // defined(I2C_SDA_PIN)
-
-#if defined(PIO_USB_DP_PIN)
-    #define PIO_USB_CONFIG { \
-        PIO_USB_DP_PIN, \
-        PIO_USB_TX_DEFAULT, \
-        PIO_SM_USB_TX_DEFAULT, \
-        PIO_USB_DMA_TX_DEFAULT, \
-        PIO_USB_RX_DEFAULT, \
-        PIO_SM_USB_RX_DEFAULT, \
-        PIO_SM_USB_EOP_DEFAULT, \
-        NULL, \
-        PIO_USB_DEBUG_PIN_NONE, \
-        PIO_USB_DEBUG_PIN_NONE, \
-        false, \
-        PIO_USB_PINOUT_DPDM \
-    }
-#endif // defined(PIO_USB_DP_PIN)
-
-#endif // _BOARD_CONFIG_H_
--- a/Firmware/RP2040/src/Board/board_api.cpp
+++ b/Firmware/RP2040/src/Board/board_api.cpp
@@ -1,107 +0,0 @@
-#include <pico/stdlib.h>
-#include <pico/mutex.h>
-#include <pico/multicore.h>
-#include <hardware/clocks.h>
-
-#include "tusb.h"
-
-#include "Board/Config.h"
-#include "Board/board_api.h"
-#include "Board/ogxm_log.h"
-#include "Board/board_api_private/board_api_private.h"
-#include "TaskQueue/TaskQueue.h"
-
-namespace board_api {
-
-mutex_t gpio_mutex_;
-
-bool usb::host_connected() {
-    if (board_api_usbh::host_connected) {
-        return board_api_usbh::host_connected();
-    }
-    return false;
-}
-
-//Only call this from core0
-void usb::disconnect_all() {
-    OGXM_LOG("Disconnecting USB and resetting Core1\n");
-
-    TaskQueue::suspend_delayed_tasks();
-    multicore_reset_core1();
-    sleep_ms(500);
-    tud_disconnect();
-    sleep_ms(500);
-}
-
-// If using PicoW, only use this method from the core running btstack and after you've called init_bluetooth
-void set_led(bool state) {
-    mutex_enter_blocking(&gpio_mutex_);
-
-    if (board_api_led::set_led) {
-        board_api_led::set_led(state);
-    }
-    if (board_api_bt::set_led) {
-        board_api_bt::set_led(state);
-    }
-    if (board_api_rgb::set_led) {
-        board_api_rgb::set_led(state ? 0x00 : 0xFF, state ? 0xFF : 0x00, 0x00);
-    }
-
-    mutex_exit(&gpio_mutex_);
-}
-
-void reboot() {
-    #define AIRCR_REG (*((volatile uint32_t *)(0xE000ED0C)))
-    #define AIRCR_SYSRESETREQ (1 << 2)
-    #define AIRCR_VECTKEY (0x5FA << 16)
-
-    OGXM_LOG("Rebooting\n");
-
-    AIRCR_REG = AIRCR_VECTKEY | AIRCR_SYSRESETREQ;
-    while(1);
-}
-
-uint32_t ms_since_boot() {
-    return to_ms_since_boot(get_absolute_time());
-}
-
-//Call after board is initialized
-void init_bluetooth() {
-    if (board_api_bt::init) {
-        board_api_bt::init();
-    }
-}
-
-//Call on core0 before any other method
-void init_board() {
-    if (!set_sys_clock_khz(SYSCLOCK_KHZ, true)) {
-        if (!set_sys_clock_khz((SYSCLOCK_KHZ / 2), true)) {
-            panic("Failed to set sys clock");
-        }
-    }
-
-    stdio_init_all();
-
-    if (!mutex_is_initialized(&gpio_mutex_)) {
-        mutex_init(&gpio_mutex_); 
-        mutex_enter_blocking(&gpio_mutex_);
-
-        if (ogxm_log::init) {
-            ogxm_log::init();
-        }
-        if (board_api_led::init) {
-            board_api_led::init();
-        }
-        if (board_api_rgb::init) {
-            board_api_rgb::init();
-        }
-        if (board_api_usbh::init) {
-            board_api_usbh::init();
-        }
-
-        mutex_exit(&gpio_mutex_);
-    }
-    OGXM_LOG("Board initialized\n");
-}
-
-} // namespace board_api
--- a/Firmware/RP2040/src/Board/board_api.h
+++ b/Firmware/RP2040/src/Board/board_api.h
@@ -1,20 +0,0 @@
-#ifndef _OGXM_BOARD_API_H_
-#define _OGXM_BOARD_API_H_
-
-#include <cstdint>
-#include <string>
-
-namespace board_api {
-    void init_board();
-    void init_bluetooth();
-    void reboot();
-    void set_led(bool state);
-    uint32_t ms_since_boot();
-
-    namespace usb {
-        bool host_connected();
-        void disconnect_all();
-    }
-}
-
-#endif // _OGXM_BOARD_API_H_
--- a/Firmware/RP2040/src/Board/board_api_private/board_api_bt.cpp
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_bt.cpp
@@ -1,34 +0,0 @@
-#include "Board/Config.h"
-#if defined(CONFIG_EN_BLUETOOTH)
-
-#include <atomic>
-#include <pico/cyw43_arch.h>
-
-#include "Board/board_api_private/board_api_private.h"
-
-#if defined(CYW43_WL_GPIO_LED_PIN) && defined(LED_INDICATOR_PIN)
-static_assert(CYW43_WL_GPIO_LED_PIN != LED_INDICATOR_PIN, "CYW43_WL_GPIO_LED_PIN cannot be the same as LED_INDICATOR_PIN");
-#endif
-
-namespace board_api_bt {
-
-std::atomic<bool> inited{false};
-
-void init() {
-    if (cyw43_arch_init() != 0) {  
-        panic("CYW43 init failed");
-    } else {
-        inited.store(true);
-    }
-}
-
-void set_led(bool state) {
-    if (!inited.load()) {
-        return;
-    }
-    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, state ? 1 : 0);
-}
-
-} // namespace board_api_bt
-
-#endif // defined(CONFIG_EN_BLUETOOTH)
--- a/Firmware/RP2040/src/Board/board_api_private/board_api_led.cpp
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_led.cpp
@@ -1,20 +0,0 @@
-#include "Board/Config.h"
-#if defined(LED_INDICATOR_PIN)
-
-#include <hardware/gpio.h>
-
-namespace board_api_led {
-    
-void init() { 
-    gpio_init(LED_INDICATOR_PIN);
-    gpio_set_dir(LED_INDICATOR_PIN, GPIO_OUT);
-    gpio_put(LED_INDICATOR_PIN, 0);
-}
-
-void set_led(bool state) {
-    gpio_put(LED_INDICATOR_PIN, state ? 1 : 0);
-}
-
-} // namespace board_api
-
-#endif // LED_INDICATOR_PIN
--- a/Firmware/RP2040/src/Board/board_api_private/board_api_private.h
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_private.h
@@ -1,29 +0,0 @@
-#ifndef BOARD_API_PRIVATE_H
-#define BOARD_API_PRIVATE_H
-
-#include <cstdint>
-#include <string>
-#include <sstream>
-#include <iostream>
-
-namespace board_api_bt {
-    void init() __attribute__((weak));
-    void set_led(bool state) __attribute__((weak));
-}
-
-namespace board_api_led {
-    void init() __attribute__((weak));
-    void set_led(bool state) __attribute__((weak));
-}
-
-namespace board_api_rgb {
-    void init() __attribute__((weak));
-    void set_led(uint8_t r, uint8_t g, uint8_t b) __attribute__((weak));
-}
-
-namespace board_api_usbh {
-    void init() __attribute__((weak));
-    bool host_connected() __attribute__((weak));
-}
-
-#endif // BOARD_API_PRIVATE_H
--- a/Firmware/RP2040/src/Board/board_api_private/board_api_rgb.cpp
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_rgb.cpp
@@ -1,33 +0,0 @@
-#include "Board/Config.h"
-#if defined(CONFIG_EN_RGB)
-
-#include <hardware/gpio.h>
-
-#include "Board/Pico_WS2812/WS2812.hpp"
-#include "Board/board_api_private/board_api_private.h"
-
-namespace board_api_rgb {
-
-WS2812& get_ws2812() {
-    static WS2812 ws2812 = WS2812(RGB_PXL_PIN, 1, pio1, 0, WS2812::FORMAT_GRB);
-    return ws2812;
-}
-
-void init() {
-#if defined(RGB_PWR_PIN)
-    gpio_init(RGB_PWR_PIN);
-    gpio_set_dir(RGB_PWR_PIN, GPIO_OUT);
-    gpio_put(RGB_PWR_PIN, 1);
-#endif
-
-    set_led(0xFF, 0, 0);
-}
-
-void set_led(uint8_t r, uint8_t g, uint8_t b) {
-    get_ws2812().setPixelColor(0, WS2812::RGB(r, g, b));
-    get_ws2812().show();
-}
-
-} // namespace board_api_rgb
-
-#endif // defined(CONFIG_EN_RGB)
--- a/Firmware/RP2040/src/Board/board_api_private/board_api_usbh.cpp
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_usbh.cpp
@@ -1,60 +0,0 @@
-#include "Board/Config.h"
-#if defined(CONFIG_EN_USB_HOST)
-
-#include <atomic>
-#include <hardware/gpio.h>
-
-#include "Board/board_api_private/board_api_private.h"
-
-namespace board_api_usbh {
-
-std::atomic<bool> host_connected_ = false;
-
-void host_pin_isr(uint gpio, uint32_t events) {
-    gpio_set_irq_enabled(PIO_USB_DP_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, false);
-    gpio_set_irq_enabled(PIO_USB_DP_PIN + 1, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, false);
-
-    if (gpio == PIO_USB_DP_PIN || gpio == PIO_USB_DP_PIN + 1) {
-        uint32_t dp_state = gpio_get(PIO_USB_DP_PIN);
-        uint32_t dm_state = gpio_get(PIO_USB_DP_PIN + 1);
-
-        if (dp_state || dm_state) {
-            host_connected_.store(true);
-        } else {
-            host_connected_.store(false);
-            gpio_set_irq_enabled(PIO_USB_DP_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
-            gpio_set_irq_enabled(PIO_USB_DP_PIN + 1, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
-        }
-    }
-}
-
-bool host_connected() {
-    return host_connected_.load();
-}
-
-void init() {
-#if defined(VCC_EN_PIN)
-    gpio_init(VCC_EN_PIN);
-    gpio_set_dir(VCC_EN_PIN, GPIO_OUT);
-    gpio_put(VCC_EN_PIN, 1);
-#endif 
-
-    gpio_init(PIO_USB_DP_PIN);
-    gpio_set_dir(PIO_USB_DP_PIN, GPIO_IN);
-    gpio_pull_down(PIO_USB_DP_PIN);
-
-    gpio_init(PIO_USB_DP_PIN + 1);
-    gpio_set_dir(PIO_USB_DP_PIN + 1, GPIO_IN);
-    gpio_pull_down(PIO_USB_DP_PIN + 1);
-
-    if (gpio_get(PIO_USB_DP_PIN) || gpio_get(PIO_USB_DP_PIN + 1)) {
-        host_connected_.store(true);
-    } else {
-        gpio_set_irq_enabled_with_callback(PIO_USB_DP_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &host_pin_isr);
-        gpio_set_irq_enabled_with_callback(PIO_USB_DP_PIN + 1, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &host_pin_isr);
-    }
-}
-
-} // namespace board_api_usbh
-
-#endif // defined(CONFIG_EN_USB_HOST)
--- a/Firmware/RP2040/src/Board/esp32_api.cpp
+++ b/Firmware/RP2040/src/Board/esp32_api.cpp
@@ -1,50 +0,0 @@
-#include "Board/Config.h"
-#if defined(CONFIG_EN_ESP32)
-
-#include <pico/stdlib.h>
-#include <hardware/gpio.h>
-
-#include "Board/esp32_api.h"
-
-bool esp32_api::uart_bridge_mode()
-{
-    gpio_pull_up(MODE_SEL_PIN);
-    return (gpio_get(MODE_SEL_PIN) == 0);
-}  
-
-void esp32_api::reset() 
-{
-    gpio_put(ESP_RST_PIN, 0);
-    sleep_ms(500);
-    gpio_put(ESP_RST_PIN, 1);
-    sleep_ms(250);
-}
-
-void esp32_api::enter_programming_mode() 
-{
-    gpio_put(ESP_PROG_PIN, 1);
-    sleep_ms(250);
-    gpio_put(ESP_PROG_PIN, 0);
-	sleep_ms(250);
-
-    reset();
-
-	gpio_put(ESP_PROG_PIN, 1);
-}
-
-void esp32_api::init()
-{
-    gpio_init(ESP_PROG_PIN);
-    gpio_set_dir(ESP_PROG_PIN, GPIO_OUT);
-    gpio_put(ESP_PROG_PIN, 1);
-
-    gpio_init(ESP_RST_PIN);
-    gpio_set_dir(ESP_RST_PIN, GPIO_OUT);
-    gpio_put(ESP_RST_PIN, 1);
-
-    gpio_init(MODE_SEL_PIN);
-    gpio_set_dir(MODE_SEL_PIN, GPIO_IN);
-    gpio_pull_up(MODE_SEL_PIN);
-}
-
-#endif // defined(CONFIG_EN_ESP32)
--- a/Firmware/RP2040/src/Board/esp32_api.h
+++ b/Firmware/RP2040/src/Board/esp32_api.h
@@ -1,13 +0,0 @@
-#include "Board/Config.h"
-#if defined(CONFIG_EN_ESP32)
-
-#include <cstdint>
-
-namespace esp32_api {
-    bool uart_bridge_mode();
-    void reset();
-    void enter_programming_mode();
-    void init();
-} // namespace board_api_esp32
-
-#endif // defined(CONFIG_EN_ESP32)
--- a/Firmware/RP2040/src/Board/ogxm_log.cpp
+++ b/Firmware/RP2040/src/Board/ogxm_log.cpp
@@ -1,89 +0,0 @@
-#include "Board/Config.h"
-#if defined(CONFIG_OGXM_DEBUG)
-
-#include <cstdint>
-#include <string>
-#include <sstream>
-#include <iostream>
-#include <iomanip>
-#include <pico/mutex.h>
-#include <hardware/uart.h>
-#include <hardware/gpio.h>
-
-#include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
-#include "Board/ogxm_log.h"
-
-std::ostream& operator<<(std::ostream& os, DeviceDriverType type) {
-    switch (type) {
-        case DeviceDriverType::NONE:          os << "NONE"; break;
-        case DeviceDriverType::XBOXOG:        os << "XBOXOG"; break;
-        case DeviceDriverType::XBOXOG_SB:     os << "XBOXOG_SB"; break;
-        case DeviceDriverType::XBOXOG_XR:     os << "XBOXOG_XR"; break;
-        case DeviceDriverType::XINPUT:        os << "XINPUT"; break;
-        case DeviceDriverType::PS3:           os << "PS3"; break;
-        case DeviceDriverType::DINPUT:        os << "DINPUT"; break;
-        case DeviceDriverType::PSCLASSIC:     os << "PSCLASSIC"; break;
-        case DeviceDriverType::SWITCH:        os << "SWITCH"; break;
-        case DeviceDriverType::WEBAPP:        os << "WEBAPP"; break;
-        case DeviceDriverType::UART_BRIDGE:   os << "UART_BRIDGE"; break;
-        default:                              os << "UNKNOWN"; break;
-    }
-    return os;
-}
-
-namespace ogxm_log {
-
-void init() {
-    uart_init(DEBUG_UART_PORT, PICO_DEFAULT_UART_BAUD_RATE);
-    gpio_set_function(PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART);
-    gpio_set_function(PICO_DEFAULT_UART_RX_PIN, GPIO_FUNC_UART);
-}
-
-void log(const std::string& message) {
-    static mutex_t log_mutex;
-
-    if (!mutex_is_initialized(&log_mutex)) {
-        mutex_init(&log_mutex);
-    }
-
-    mutex_enter_blocking(&log_mutex);
-
-    std::string formatted_msg = "OGXM: " + message;
-
-    uart_puts(DEBUG_UART_PORT, formatted_msg.c_str());
-
-    mutex_exit(&log_mutex);
-}
-
-void log(const char* fmt, ...) {
-    va_list args;
-    va_start(args, fmt);
-
-    char buffer[256];
-    vsnprintf(buffer, sizeof(buffer), fmt, args);
-
-    std::string formatted_msg = std::string(buffer);
-
-    log(formatted_msg);
-
-    va_end(args);
-}
-
-void log_hex(const uint8_t* data, size_t size) {
-    std::ostringstream hex_stream;
-    hex_stream << std::hex << std::setfill('0');
-    int count = 0;
-    for (uint16_t i = 0; i < size; ++i) {
-        hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
-        if (++count == 16) {
-            hex_stream << "\n";
-            count = 0;
-        }
-    }
-    hex_stream << "\n";
-    log(hex_stream.str());
-}
-
-} // namespace ogxm_log
-
-#endif // defined(CONFIG_OGXM_DEBUG)
--- a/Firmware/RP2040/src/Board/ogxm_log.h
+++ b/Firmware/RP2040/src/Board/ogxm_log.h
@@ -1,55 +0,0 @@
-#ifndef BOARD_API_LOG_H
-#define BOARD_API_LOG_H
-
-#include <cstdint>
-
-#include "Board/Config.h"
-#if defined(CONFIG_OGXM_DEBUG)
-
-#include <string>
-#include <sstream>
-#include <iostream>
-#include <stdarg.h>
-
-#include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
-
-std::ostream& operator<<(std::ostream& os, DeviceDriverType type);
-
-namespace ogxm_log {
-    void init() __attribute__((weak));
-    //Don't use this directly, use the OGXM_LOG macro
-    void log(const std::string& message);
-    //Don't use this directly, use the OGXM_LOG macro
-    void log(const char* fmt, ...);
-    //Don't use this directly, use the OGXM_LOG_HEX macro
-    void log_hex(const uint8_t* data, size_t size);
-
-    template <typename T>
-    std::string to_string(const T& value) {
-        std::ostringstream stream;
-        stream << value;
-        return stream.str();
-    }
-}
-
-#define OGXM_LOG ogxm_log::log
-#define OGXM_LOG_HEX ogxm_log::log_hex
-#define OGXM_ASSERT(x) if (!(x)) { OGXM_LOG("Assertion failed: " #x); while(1); }
-#define OGXM_ASSERT_MSG(x, msg) if (!(x)) { OGXM_LOG("Assertion failed: " #x " " msg); while(1); }
-#define OGXM_TO_STRING ogxm_log::to_string
-
-#else // CONFIG_OGXM_DEBUG
-
-namespace ogxm_log {
-    void init() __attribute__((weak));
-}
-
-#define OGXM_LOG(...)
-#define OGXM_LOG_HEX(...)
-#define OGXM_ASSERT(x)
-#define OGXM_ASSERT_MSG(x, msg)
-#define OGXM_TO_STRING(x)
-    
-#endif // CONFIG_OGXM_DEBUG
-
-#endif // BOARD_API_LOG_H
--- a/Firmware/RP2040/src/Descriptors/CDCDev.h
+++ b/Firmware/RP2040/src/Descriptors/CDCDev.h
@@ -1,66 +0,0 @@
-#ifndef _CDC_DEV_DESCRIPTORS_H_
-#define _CDC_DEV_DESCRIPTORS_H_
-
-#include <cstdint>
-#include <cstring>
-
-#include "tusb.h"
-
-namespace CDCDesc 
-{
-    #define _PID_MAP(itf, n)  ( (CFG_TUD_##itf) << (n) )
-
-    static const uint16_t VID = 0xCafe;
-    static const uint16_t PID = (0x4000 | _PID_MAP(CDC, 0));
-    static const uint16_t USB_VER = 0x0200;
-
-    const tusb_desc_device_t DESC_DEVICE = 
-    {
-        .bLength            = sizeof(tusb_desc_device_t),
-        .bDescriptorType    = TUSB_DESC_DEVICE,
-        .bcdUSB             = USB_VER,
-        .bDeviceClass       = TUSB_CLASS_MISC,
-        .bDeviceSubClass    = MISC_SUBCLASS_COMMON,
-        .bDeviceProtocol    = MISC_PROTOCOL_IAD,
-        .bMaxPacketSize0    = CFG_TUD_ENDPOINT0_SIZE,
-        .idVendor           = VID,
-        .idProduct          = PID,
-        .bcdDevice          = 0x0100,
-        .iManufacturer      = 0x01,
-        .iProduct           = 0x02,
-        .iSerialNumber      = 0x03,
-        .bNumConfigurations = 0x01
-    };
-
-    enum Itf
-    {
-        CDC_0 = 0,
-        CDC_0_DATA,
-        ITF_TOTAL
-    };
-
-    static const int CONFIG_LEN = (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN);
-
-    static const uint8_t DESC_CONFIG[] =
-    {
-        TUD_CONFIG_DESCRIPTOR(1, Itf::ITF_TOTAL, 0, CONFIG_LEN, 0x00, 500),
-        TUD_CDC_DESCRIPTOR(Itf::CDC_0, 4, 0x80 | (Itf::CDC_0 + 1), 8, (Itf::CDC_0 + 2), 0x80 | (Itf::CDC_0 + 2), 64),
-    };
-
-    static const uint8_t STRING_DESC_LANGUAGE[] = { 0x09, 0x04 };
-    static const uint8_t STRING_MANUFACTURER[]  = "Wired Opposite";
-    static const uint8_t STRING_PRODUCT[]       = "OGX-Mini";
-    static const uint8_t STRING_INTERFACE[]     = "OGX-Mini CDC";
-
-    static const uint8_t *DESC_STRING[] =
-    {
-        STRING_DESC_LANGUAGE,
-        STRING_MANUFACTURER,
-        STRING_PRODUCT,
-        nullptr, //Serial
-        STRING_INTERFACE
-    };
-
-}; // namespace CDCDesc
-
-#endif // _CDC_DEV_DESCRIPTORS_H_
--- a/Firmware/RP2040/src/Descriptors/DInput.h
+++ b/Firmware/RP2040/src/Descriptors/DInput.h
@@ -1,298 +0,0 @@
-#pragma once
-
-#include <cstdint>
-#include <cstring>
-
-#include "tusb.h"
-
-#include "Board/Config.h"
-
-namespace DInput 
-{
-    static constexpr uint8_t DPAD_MASK = 0x0F;
-    static constexpr uint8_t AXIS_MIN  = 0x00;
-    static constexpr uint8_t AXIS_MID  = 0x80;
-    static constexpr uint8_t AXIS_MAX  = 0xFF;
-
-    namespace Buttons0 
-    {
-        static constexpr uint8_t SQUARE     = 0x01;
-        static constexpr uint8_t CROSS      = 0x02;
-        static constexpr uint8_t CIRCLE     = 0x04;
-        static constexpr uint8_t TRIANGLE   = 0x08;
-        static constexpr uint8_t L1         = 0x10;
-        static constexpr uint8_t R1         = 0x20;
-        static constexpr uint8_t L2         = 0x40;
-        static constexpr uint8_t R2         = 0x80;
-    };
-
-    namespace Buttons1 
-    {
-        static constexpr uint8_t SELECT = 0x01;
-        static constexpr uint8_t START  = 0x02;
-        static constexpr uint8_t L3     = 0x04;
-        static constexpr uint8_t R3     = 0x08;
-        static constexpr uint8_t SYS    = 0x10;
-        static constexpr uint8_t TP     = 0x20;
-    };
-
-    namespace DPad 
-    {
-        static constexpr uint8_t UP         = 0x00;
-        static constexpr uint8_t UP_RIGHT   = 0x01;
-        static constexpr uint8_t RIGHT      = 0x02;
-        static constexpr uint8_t DOWN_RIGHT = 0x03;
-        static constexpr uint8_t DOWN       = 0x04;
-        static constexpr uint8_t DOWN_LEFT  = 0x05;
-        static constexpr uint8_t LEFT       = 0x06;
-        static constexpr uint8_t UP_LEFT    = 0x07;
-        static constexpr uint8_t CENTER     = 0x08;
-    };
-
-    #pragma pack(push, 1)
-    struct InReport
-    {
-        uint8_t buttons[2];
-        uint8_t dpad;
-
-        uint8_t joystick_lx;
-        uint8_t joystick_ly;
-        uint8_t joystick_rx;
-        uint8_t joystick_ry;
-
-        uint8_t right_axis;
-        uint8_t left_axis;
-        uint8_t up_axis;
-        uint8_t down_axis;
-
-        uint8_t triangle_axis;
-        uint8_t circle_axis;
-        uint8_t cross_axis;
-        uint8_t square_axis;
-
-        uint8_t l1_axis;
-        uint8_t r1_axis;
-        uint8_t l2_axis;
-        uint8_t r2_axis;
-
-        InReport()
-        {
-            std::memset(this, 0, sizeof(InReport));
-        }
-    };
-    #pragma pack(pop)
-    static_assert(sizeof(InReport) == 19, "DInput::InReport is misaligned");
-
-    static const uint8_t STRING_DESC_LANGUAGE[] = { 0x09, 0x04 };
-    static const uint8_t STRING_MANUFACTURER[]  = "SHANWAN";
-    static const uint8_t STRING_PRODUCT[]       = "2In1 USB Joystick";
-    static const uint8_t STRING_VERSION[]       = "1.0";
-
-    static const uint8_t *STRING_DESCRIPTORS[] __attribute__((unused)) =
-    {
-        STRING_DESC_LANGUAGE,
-        STRING_MANUFACTURER,
-        STRING_PRODUCT,
-        STRING_VERSION
-    };
-
-    static const uint8_t DEVICE_DESCRIPTORS[] =
-    {
-        0x12,        // bLength
-        0x01,        // bDescriptorType (Device)
-        0x10, 0x01,  // bcdUSB 1.10
-        0x00,        // bDeviceClass (Use class information in the Interface Descriptors)
-        0x00,        // bDeviceSubClass 
-        0x00,        // bDeviceProtocol 
-        0x40,        // bMaxPacketSize0 64
-        0x63, 0x25,  // idVendor 0x2563
-        0x75, 0x05,  // idProduct 0x0575
-        0x00, 0x02,  // bcdDevice 4.00
-        0x01,        // iManufacturer (String Index)
-        0x02,        // iProduct (String Index)
-        0x00,        // iSerialNumber (String Index)
-        0x01,        // bNumConfigurations 1
-    };
-
-    static const uint8_t REPORT_DESCRIPTORS[] =
-    {
-        0x05, 0x01,        // Usage Page (Generic Desktop Ctrls)
-        0x09, 0x05,        // Usage (Game Pad)
-        0xA1, 0x01,        // Collection (Application)
-        0x15, 0x00,        //   Logical Minimum (0)
-        0x25, 0x01,        //   Logical Maximum (1)
-        0x35, 0x00,        //   Physical Minimum (0)
-        0x45, 0x01,        //   Physical Maximum (1)
-        0x75, 0x01,        //   Report Size (1)
-        0x95, 0x0D,        //   Report Count (13)
-        0x05, 0x09,        //   Usage Page (Button)
-        0x19, 0x01,        //   Usage Minimum (0x01)
-        0x29, 0x0D,        //   Usage Maximum (0x0D)
-        0x81, 0x02,        //   Input (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-        0x95, 0x03,        //   Report Count (3)
-        0x81, 0x01,        //   Input (Const,Array,Abs,No Wrap,Linear,Preferred State,No Null Position)
-        0x05, 0x01,        //   Usage Page (Generic Desktop Ctrls)
-        0x25, 0x07,        //   Logical Maximum (7)
-        0x46, 0x3B, 0x01,  //   Physical Maximum (315)
-        0x75, 0x04,        //   Report Size (4)
-        0x95, 0x01,        //   Report Count (1)
-        0x65, 0x14,        //   Unit (System: English Rotation, Length: Centimeter)
-        0x09, 0x39,        //   Usage (Hat switch)
-        0x81, 0x42,        //   Input (Data,Var,Abs,No Wrap,Linear,Preferred State,Null State)
-        0x65, 0x00,        //   Unit (None)
-        0x95, 0x01,        //   Report Count (1)
-        0x81, 0x01,        //   Input (Const,Array,Abs,No Wrap,Linear,Preferred State,No Null Position)
-        0x26, 0xFF, 0x00,  //   Logical Maximum (255)
-        0x46, 0xFF, 0x00,  //   Physical Maximum (255)
-        0x09, 0x30,        //   Usage (X)
-        0x09, 0x31,        //   Usage (Y)
-        0x09, 0x32,        //   Usage (Z)
-        0x09, 0x35,        //   Usage (Rz)
-        0x75, 0x08,        //   Report Size (8)
-        0x95, 0x04,        //   Report Count (4)
-        0x81, 0x02,        //   Input (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-        0x06, 0x00, 0xFF,  //   Usage Page (Vendor Defined 0xFF00)
-        0x09, 0x20,        //   Usage (0x20)
-        0x09, 0x21,        //   Usage (0x21)
-        0x09, 0x22,        //   Usage (0x22)
-        0x09, 0x23,        //   Usage (0x23)
-        0x09, 0x24,        //   Usage (0x24)
-        0x09, 0x25,        //   Usage (0x25)
-        0x09, 0x26,        //   Usage (0x26)
-        0x09, 0x27,        //   Usage (0x27)
-        0x09, 0x28,        //   Usage (0x28)
-        0x09, 0x29,        //   Usage (0x29)
-        0x09, 0x2A,        //   Usage (0x2A)
-        0x09, 0x2B,        //   Usage (0x2B)
-        0x95, 0x0C,        //   Report Count (12)
-        0x81, 0x02,        //   Input (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-        0x0A, 0x21, 0x26,  //   Usage (0x2621)
-        0x95, 0x08,        //   Report Count (8)
-        0xB1, 0x02,        //   Feature (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position,Non-volatile)
-        0x0A, 0x21, 0x26,  //   Usage (0x2621)
-        0x91, 0x02,        //   Output (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position,Non-volatile)
-        0x26, 0xFF, 0x03,  //   Logical Maximum (1023)
-        0x46, 0xFF, 0x03,  //   Physical Maximum (1023)
-        0x09, 0x2C,        //   Usage (0x2C)
-        0x09, 0x2D,        //   Usage (0x2D)
-        0x09, 0x2E,        //   Usage (0x2E)
-        0x09, 0x2F,        //   Usage (0x2F)
-        0x75, 0x10,        //   Report Size (16)
-        0x95, 0x04,        //   Report Count (4)
-        0x81, 0x02,        //   Input (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-        0xC0,              // End Collection
-    };
-
-    // uint8_t const CONFIGURATION_DESCRIPTORS[] =
-    // {
-    //     0x09,        // bLength
-    //     0x02,        // bDescriptorType (Configuration)
-    //     0x29, 0x00,  // wTotalLength 41
-    //     0x01,        // bNumInterfaces 1
-    //     0x01,        // bConfigurationValue
-    //     0x00,        // iConfiguration (String Index)
-    //     0x80,        // bmAttributes
-    //     0xFA,        // bMaxPower 500mA
-
-    //     0x09,        // bLength
-    //     0x04,        // bDescriptorType (Interface)
-    //     0x00,        // bInterfaceNumber 0
-    //     0x00,        // bAlternateSetting
-    //     0x02,        // bNumEndpoints 2
-    //     0x03,        // bInterfaceClass
-    //     0x00,        // bInterfaceSubClass
-    //     0x00,        // bInterfaceProtocol
-    //     0x00,        // iInterface (String Index)
-
-    //     0x09,        // bLength
-    //     0x21,        // bDescriptorType (HID)
-    //     0x10, 0x01,  // bcdHID 1.10
-    //     0x00,        // bCountryCode
-    //     0x01,        // bNumDescriptors
-    //     0x22,        // bDescriptorType[0] (HID)
-    //     0x89, 0x00,  // wDescriptorLength[0] 137
-
-    //     0x07,        // bLength
-    //     0x05,        // bDescriptorType (Endpoint)
-    //     0x02,        // bEndpointAddress (OUT/H2D)
-    //     0x03,        // bmAttributes (Interrupt)
-    //     0x20, 0x00,  // wMaxPacketSize 32
-    //     0x0A,        // bInterval 10 (unit depends on device speed)
-
-    //     0x07,        // bLength
-    //     0x05,        // bDescriptorType (Endpoint)
-    //     0x81,        // bEndpointAddress (IN/D2H)
-    //     0x03,        // bmAttributes (Interrupt)
-    //     0x20, 0x00,  // wMaxPacketSize 32
-    //     0x0A,        // bInterval 10 (unit depends on device speed)
-    // };
-
-    enum Itf { 
-        NUM_HID1 = 0, 
-#if MAX_GAMEPADS > 1
-        NUM_HID2,
-#endif
-#if MAX_GAMEPADS > 2 
-        NUM_HID3,
-#endif
-#if MAX_GAMEPADS > 3 
-        NUM_HID4,
-#endif 
-        NUM_TOTAL 
-    };
-
-    #define CONFIG_TOTAL_LEN (TUD_CONFIG_DESC_LEN + (TUD_HID_DESC_LEN * MAX_GAMEPADS))
-
-    // #define EPNUM_HID1 0x81
-    // #define EPNUM_HID2 0x82
-    // #define EPNUM_HID3 0x83
-    // #define EPNUM_HID4 0x84
-
-    uint8_t const CONFIGURATION_DESCRIPTORS[] = 
-    {
-        // Config number, interface count, string index, total length, attribute, power in mA
-        TUD_CONFIG_DESCRIPTOR(  1,
-                                Itf::NUM_TOTAL,
-                                0,
-                                CONFIG_TOTAL_LEN,
-                                TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP,
-                                500),
-
-        // Interface number, string index, protocol, report descriptor len, EP In address, size & polling interval
-        TUD_HID_DESCRIPTOR( Itf::NUM_HID1,
-                            0,
-                            HID_ITF_PROTOCOL_NONE,
-                            sizeof(REPORT_DESCRIPTORS),
-                            (0x80 | (Itf::NUM_HID1 + 1)),
-                            CFG_TUD_HID_EP_BUFSIZE,
-                            1),
-#if MAX_GAMEPADS > 1
-        TUD_HID_DESCRIPTOR( Itf::NUM_HID2,
-                            0,
-                            HID_ITF_PROTOCOL_NONE,
-                            sizeof(REPORT_DESCRIPTORS),
-                            (0x80 | (Itf::NUM_HID2 + 1)),
-                            CFG_TUD_HID_EP_BUFSIZE,
-                            1),
-#endif
-#if MAX_GAMEPADS > 2
-        TUD_HID_DESCRIPTOR( Itf::NUM_HID3,
-                            0,
-                            HID_ITF_PROTOCOL_NONE,
-                            sizeof(REPORT_DESCRIPTORS),
-                            (0x80 | (Itf::NUM_HID3 + 1)),
-                            CFG_TUD_HID_EP_BUFSIZE,
-                            1),
-#endif
-#if MAX_GAMEPADS > 3
-        TUD_HID_DESCRIPTOR( Itf::NUM_HID4,
-                            0,
-                            HID_ITF_PROTOCOL_NONE,
-                            sizeof(REPORT_DESCRIPTORS),
-                            (0x80 | (Itf::NUM_HID4 + 1)),
-                            CFG_TUD_HID_EP_BUFSIZE,
-                            1)
-#endif
-    };
-
-}; // namespace DInput
--- a/Firmware/RP2040/src/Descriptors/N64.h
+++ b/Firmware/RP2040/src/Descriptors/N64.h
@@ -1,50 +0,0 @@
-#ifndef _N64_DESCRIPTORS_H_
-#define _N64_DESCRIPTORS_H_
-
-#include <cstdint>
-
-namespace N64 
-{
-    static constexpr uint16_t DPAD_MASK = 0x0F;
-    static constexpr uint8_t JOY_MIN = 0x00;
-    static constexpr uint8_t JOY_MID = 0x80;
-    static constexpr uint8_t JOY_MAX = 0xFF;
-
-    namespace Buttons 
-    {
-        static constexpr uint16_t DPAD_UP         = 0x00;
-        static constexpr uint16_t DPAD_UP_RIGHT   = 0x01;
-        static constexpr uint16_t DPAD_RIGHT      = 0x02;
-        static constexpr uint16_t DPAD_RIGHT_DOWN = 0x03;
-        static constexpr uint16_t DPAD_DOWN       = 0x04;
-        static constexpr uint16_t DPAD_DOWN_LEFT  = 0x05;
-        static constexpr uint16_t DPAD_LEFT       = 0x06;
-        static constexpr uint16_t DPAD_LEFT_UP    = 0x07;
-        static constexpr uint16_t DPAD_NONE       = 0x08;
-
-        static constexpr uint16_t C_UP    = (1 << 4);
-        static constexpr uint16_t C_RIGHT = (1 << 5);
-        static constexpr uint16_t C_DOWN  = (1 << 6);
-        static constexpr uint16_t C_LEFT  = (1 << 7);
-        static constexpr uint16_t L       = (1 << 8);
-        static constexpr uint16_t R       = (1 << 9);
-        static constexpr uint16_t A       = (1 << 10);
-        static constexpr uint16_t Z       = (1 << 11);
-        static constexpr uint16_t B       = (1 << 12);
-        static constexpr uint16_t START   = (1 << 13);
-    };
-
-    #pragma pack(push, 1)
-    struct InReport
-    {
-        uint8_t joystick_x;
-        uint8_t joystick_y;
-        uint8_t padding[3];
-        uint16_t buttons;
-    };
-    static_assert(sizeof(InReport) == 7, "N64 InReport size is not correct");
-    #pragma pack(pop)
-
-}; // namespace N64
-
-#endif // _N64_DESCRIPTORS_H_
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
weebrobot	b2149e4454	v1.0.0a4	2025-06-26 13:34:31 -06:00
weebrobot	7ac52dde74	v1.0.0a4	2025-06-18 08:58:24 -06:00
weebrobot	7b63b6206c	v1.0.0a4	2025-06-16 08:13:55 -06:00
weebrobot	3c9b9f2648	v1.0.0a4	2025-06-08 14:15:28 -06:00
weebrobot	2e6b73cd88	v1.0.0a4	2025-06-06 13:35:51 -06:00
weebrobot	69fdb74cbc	v1.0.0a4	2025-06-06 13:33:50 -06:00
weebrobot	d6a8294cb5	v1.0.0a4	2025-06-06 13:31:09 -06:00
wiredopposite	ed52dff4b0	v1.0.0a4	2025-06-06 12:47:40 -06:00