add blueretro support via i2c

.github/FUNDING.yml

@@ -0,0 +1,15 @@
+# These are supported funding model platforms
+
+github: # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2]
+patreon: # Replace with a single Patreon username
+open_collective: # Replace with a single Open Collective username
+ko_fi: wiredopposite
+tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel
+community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry
+liberapay: # Replace with a single Liberapay username
+issuehunt: # Replace with a single IssueHunt username
+lfx_crowdfunding: # Replace with a single LFX Crowdfunding project-name e.g., cloud-foundry
+polar: # Replace with a single Polar username
+buy_me_a_coffee: # Replace with a single Buy Me a Coffee username
+thanks_dev: # Replace with a single thanks.dev username
+custom: # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2']

.gitignore

@@ -2,6 +2,8 @@ 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

.gitmodules

@@ -10,3 +10,12 @@
 [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
+	url = https://github.com/wiredopposite/BlueRetro.git

.vscode/c_cpp_properties.json

@@ -0,0 +1,20 @@
+{
+    "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
+}

.vscode/settings.json

@@ -0,0 +1,64 @@
+{
+    "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"
+    },
+}

Firmware/ESP32/.vscode/settings.json

@@ -100,5 +100,7 @@
     },
     "idf.portWin": "COM26",
     "idf.flashType": "UART",
-    "C_Cpp.intelliSenseEngine": "default"
+    "C_Cpp.intelliSenseEngine": "default",
+    "idf.espIdfPathWin": "C:\\Programming\\esp\\v5.1\\esp-idf",
+    "idf.toolsPathWin": "C:\\Programming\\.espressif"
 }

Firmware/ESP32/CMakeLists.txt

@@ -1,14 +1,29 @@
 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(${EXTERNAL_DIR}/patch_libs.cmake)
+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
+)
 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
+    )
 
 set(BLUEPAD32_ROOT ${EXTERNAL_DIR}/bluepad32/src/components/bluepad32)
 
@@ -22,4 +37,4 @@ set(EXTRA_COMPONENT_DIRS
 )
 
 include($ENV{IDF_PATH}/tools/cmake/project.cmake)
-project(OGX-Mini-ESP32)
+project("${FW_NAME}-${FW_VERSION}-ESP32")

Firmware/ESP32/components/libfixmath/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.5)
+
+set(LIBFIXMATH_ROOT ${CMAKE_CURRENT_LIST_DIR}/../../../external/libfixmath) 
+
+if (NOT EXISTS ${LIBFIXMATH_ROOT})
+    message(FATAL_ERROR "External directory not found: ${LIBFIXMATH_ROOT}")
+else()
+    message(STATUS "Found libfixmath at ${LIBFIXMATH_ROOT}")
+endif()
+
+file(GLOB SRCS ${LIBFIXMATH_ROOT}/libfixmath/*.c)
+
+idf_component_register(
+    SRCS
+        ${SRCS}
+    INCLUDE_DIRS
+        ${LIBFIXMATH_ROOT}
+        ${LIBFIXMATH_ROOT}/libfixmath
+)
+
+target_compile_definitions(${COMPONENT_LIB} PRIVATE
+    FIXMATH_FAST_SIN
+    FIXMATH_NO_64BIT
+    FIXMATH_NO_CACHE
+    FIXMATH_NO_HARD_DIVISION
+    FIXMATH_NO_OVERFLOW
+)

Firmware/ESP32/main/BLEServer/BLEServer.cpp

@@ -0,0 +1,334 @@
+#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

Firmware/ESP32/main/BLEServer/BLEServer.h

@@ -0,0 +1,13 @@
+#ifndef BLE_SERVER_H
+#define BLE_SERVER_H
+
+#include <cstdint>
+
+#include "Gamepad/Gamepad.h"
+
+namespace BLEServer 
+{
+    void init_server();
+}
+
+#endif // BLE_SERVER_H

Firmware/ESP32/main/BTManager/BTManager.cpp

@@ -0,0 +1,257 @@
+#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;
+}

Firmware/ESP32/main/BTManager/BTManager.h

@@ -0,0 +1,83 @@
+#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_

Firmware/ESP32/main/BTManager/BTManager_BP32.cpp

@@ -0,0 +1,194 @@
+#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;
+}

Firmware/ESP32/main/Bluepad32/Bluepad32.cpp

@@ -1,348 +0,0 @@
-#include <functional>
-
-#include "btstack_port_esp32.h"
-#include "btstack_run_loop.h"
-#include "btstack_stdio_esp32.h"
-#include "uni.h"
-
-#include "sdkconfig.h"
-#include "I2CDriver/I2CDriver.h"
-#include "Board/board_api.h"
-#include "Bluepad32/Gamepad.h"
-#include "Bluepad32/Bluepad32.h"
-
-namespace BP32 {
-
-static constexpr uint8_t  MAX_DEVICES = CONFIG_BLUEPAD32_MAX_DEVICES;
-static constexpr uint32_t FEEDBACK_TIME_MS = 200;
-static constexpr uint32_t LED_TIME_MS = 500;
-
-I2CDriver i2c_driver_;
-btstack_timer_source_t feedback_timer_;
-std::atomic<bool> devs_conn_[MAX_DEVICES]{false};
-
-static inline void send_feedback_cb(void* context)
-{
-    I2CDriver::PacketOut packet_out = reinterpret_cast<std::atomic<I2CDriver::PacketOut>*>(context)->load();
-    uni_hid_device_t* bp_device = nullptr;
-
-    if (!(bp_device = uni_hid_device_get_instance_for_idx(packet_out.index)) || 
-        !uni_bt_conn_is_connected(&bp_device->conn) ||
-        !bp_device->report_parser.play_dual_rumble)
-    {
-        return;
-    }
-
-    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
-            );
-    }
-}
-
-static inline void feedback_timer_cb(btstack_timer_source *ts)
-{
-    static btstack_context_callback_registration_t cb_registration[MAX_DEVICES];
-    static std::atomic<I2CDriver::PacketOut> packets_out[MAX_DEVICES];
-
-    uni_hid_device_t* bp_device = nullptr;
-
-    for (uint8_t i = 0; i < MAX_DEVICES; ++i)
-    {
-        if (!(bp_device = uni_hid_device_get_instance_for_idx(i)) || 
-            !uni_bt_conn_is_connected(&bp_device->conn))
-        {
-            continue;
-        }
-
-        cb_registration[i].callback = send_feedback_cb;
-        cb_registration[i].context = reinterpret_cast<void*>(&packets_out[i]);
-        
-        //Get i2c rumble packet then register callback on BTStack thread
-        i2c_driver_.push_task([i]
-        {
-            I2CDriver::PacketOut packet_out;
-            if (i2c_driver_.i2c_read_blocking(  I2CDriver::MULTI_SLAVE ? i + 1 : 0x01, 
-                                                reinterpret_cast<uint8_t*>(&packet_out), 
-                                                sizeof(I2CDriver::PacketOut)) == ESP_OK)
-            {
-                packets_out[i].store(packet_out);
-                btstack_run_loop_execute_on_main_thread(&cb_registration[i]);
-            }
-        });
-    }
-
-    btstack_run_loop_set_timer(ts, FEEDBACK_TIME_MS);
-    btstack_run_loop_add_timer(ts);
-}
-
-inline void 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(any_connected() ? 1 : (led_state ? 1 : 0));
-    }
-    else
-    {
-        for (uint8_t i = 0; i < board_api::NUM_LEDS; ++i)
-        {
-            board_api::set_led(i, devs_conn_[i].load() ? 1 : (led_state ? 1 : 0));
-        }
-    }
-
-    btstack_run_loop_set_timer(ts, LED_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);
-
-    // Based on runtime condition, you can delete or list the stored BT keys.
-    if (1)
-    {
-        uni_bt_del_keys_unsafe();
-    }
-    else
-    {
-        uni_bt_list_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) 
-{
-#ifdef CONFIG_BLUEPAD32_USB_CONSOLE_ENABLE
-    logd("BP32", "Device connected, addr:  %p, index: %i\n", device, uni_hid_device_get_idx_for_instance(device));
-#endif
-}
-
-void device_disconnected_cb(uni_hid_device_t* device) 
-{
-#ifdef CONFIG_BLUEPAD32_USB_CONSOLE_ENABLE
-    logd("BP32", "Device disconnected, addr:  %p, index: %i\n", device, uni_hid_device_get_idx_for_instance(device));
-#endif
-
-    int idx = uni_hid_device_get_idx_for_instance(device);
-    if (idx >= MAX_DEVICES || idx < 0)
-    {
-        return;
-    }
-
-    devs_conn_[idx].store(false);
-    if (!any_connected())
-    {
-        btstack_run_loop_remove_timer(&feedback_timer_);
-    }
-
-    I2CDriver::PacketIn packet_in = I2CDriver::PacketIn();
-    packet_in.index = static_cast<uint8_t>(idx);
-    
-    i2c_driver_.push_task([packet_in]
-    {
-        i2c_driver_.i2c_write_blocking( I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, 
-                                        reinterpret_cast<const uint8_t*>(&packet_in), 
-                                        sizeof(I2CDriver::PacketIn));
-    });
-}
-
-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_DEVICES || idx < 0)
-    {
-        return UNI_ERROR_IGNORE_DEVICE;
-    }
-
-    devs_conn_[idx].store(true);
-
-    feedback_timer_.process = feedback_timer_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 inline void controller_data_cb(uni_hid_device_t* device, uni_controller_t* controller) 
-{
-    static uni_gamepad_t prev_uni_gps[MAX_DEVICES]{0};
-
-    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);
-
-    if (idx < 0 || std::memcmp(uni_gp, &prev_uni_gps[idx], sizeof(uni_gamepad_t)) == 0)
-    {
-        return;
-    }
-
-    I2CDriver::PacketIn packet_in;
-    packet_in.index = static_cast<uint8_t>(idx);
-
-    switch (uni_gp->dpad) 
-    {
-        case DPAD_UP:
-            packet_in.dpad = Gamepad::DPad::UP;
-            break;
-        case DPAD_DOWN:
-            packet_in.dpad = Gamepad::DPad::DOWN;
-            break;
-        case DPAD_LEFT:
-            packet_in.dpad = Gamepad::DPad::LEFT;
-            break;
-        case DPAD_RIGHT:
-            packet_in.dpad = Gamepad::DPad::RIGHT;
-            break;
-        case (DPAD_UP | DPAD_RIGHT):
-            packet_in.dpad = Gamepad::DPad::UP_RIGHT;
-            break;
-        case (DPAD_DOWN | DPAD_RIGHT):
-            packet_in.dpad = Gamepad::DPad::DOWN_RIGHT;
-            break;
-        case (DPAD_DOWN | DPAD_LEFT):
-            packet_in.dpad = Gamepad::DPad::DOWN_LEFT;
-            break;
-        case (DPAD_UP | DPAD_LEFT):
-            packet_in.dpad = Gamepad::DPad::UP_LEFT;
-            break;
-        default:
-            break;
-    }
-
-    if (uni_gp->buttons & BUTTON_A) packet_in.buttons |= Gamepad::Button::A;
-    if (uni_gp->buttons & BUTTON_B) packet_in.buttons |= Gamepad::Button::B;
-    if (uni_gp->buttons & BUTTON_X) packet_in.buttons |= Gamepad::Button::X;
-    if (uni_gp->buttons & BUTTON_Y) packet_in.buttons |= Gamepad::Button::Y;
-    if (uni_gp->buttons & BUTTON_SHOULDER_L) packet_in.buttons |= Gamepad::Button::LB;
-    if (uni_gp->buttons & BUTTON_SHOULDER_R) packet_in.buttons |= Gamepad::Button::RB;
-    if (uni_gp->buttons & BUTTON_THUMB_L)    packet_in.buttons |= Gamepad::Button::L3;
-    if (uni_gp->buttons & BUTTON_THUMB_R)    packet_in.buttons |= Gamepad::Button::R3;
-    if (uni_gp->misc_buttons & MISC_BUTTON_BACK)    packet_in.buttons |= Gamepad::Button::BACK;
-    if (uni_gp->misc_buttons & MISC_BUTTON_START)   packet_in.buttons |= Gamepad::Button::START;
-    if (uni_gp->misc_buttons & MISC_BUTTON_SYSTEM)  packet_in.buttons |= Gamepad::Button::SYS;
-    if (uni_gp->misc_buttons & MISC_BUTTON_CAPTURE) packet_in.buttons |= Gamepad::Button::MISC;
-
-    packet_in.trigger_l = Scale::uint10_to_uint8(uni_gp->brake);
-    packet_in.trigger_r = Scale::uint10_to_uint8(uni_gp->throttle);
-
-    packet_in.joystick_lx = Scale::int10_to_int16(uni_gp->axis_x);
-    packet_in.joystick_ly = Scale::int10_to_int16(uni_gp->axis_y);
-    packet_in.joystick_rx = Scale::int10_to_int16(uni_gp->axis_rx);
-    packet_in.joystick_ry = Scale::int10_to_int16(uni_gp->axis_ry);
-
-    i2c_driver_.push_task([packet_in]
-    {
-        i2c_driver_.i2c_write_blocking( I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, 
-                                        reinterpret_cast<const uint8_t*>(&packet_in), 
-                                        sizeof(I2CDriver::PacketIn));
-    });
-
-    std::memcpy(&prev_uni_gps[idx], uni_gp, sizeof(uni_gamepad_t));
-}
-
-static const uni_property_t* get_property_cb(uni_property_idx_t idx) 
-{
-    return nullptr;
-}
-
-static void oob_event_cb(uni_platform_oob_event_t event, void* data) 
-{
-    return;
-}
-
-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
-
-bool any_connected()
-{
-    for (auto& connected : devs_conn_)
-    {
-        if (connected.load())
-        {
-            return true;
-        }
-    }
-    return false;
-}
-
-bool connected(uint8_t index)
-{
-    return devs_conn_[index].load();
-}
-
-void run_task()
-{
-    board_api::init_pins(); 
-
-    i2c_driver_.initialize_i2c();
-
-    xTaskCreatePinnedToCore(
-        i2c_driver_.run_tasks,
-        "i2c",
-        2048 * 2,
-        nullptr,
-        configMAX_PRIORITIES-8,
-        nullptr,
-        1 );
-    
-    btstack_init();
-
-    uni_platform_set_custom(get_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_run_loop_execute();
-}
-
-} // namespace BP32

Firmware/ESP32/main/Bluepad32/Bluepad32.h

@@ -1,13 +0,0 @@
-#ifndef _BLUEPAD32_DRIVER_H_
-#define _BLUEPAD32_DRIVER_H_
-
-#include <cstdint>
-
-namespace BP32
-{
-    void run_task();
-    bool any_connected();
-    bool connected(uint8_t index);
-}
-
-#endif // _BLUEPAD32_DRIVER_H_

Firmware/ESP32/main/Bluepad32/Gamepad.h

@@ -1,100 +0,0 @@
-#ifndef GAMEPAD_H
-#define GAMEPAD_H
-
-#include <cstdint>
-
-namespace Gamepad 
-{
-    namespace DPad
-    {
-        static constexpr uint8_t UP    = 0x01;
-        static constexpr uint8_t DOWN  = 0x02;
-        static constexpr uint8_t LEFT  = 0x04;
-        static constexpr uint8_t RIGHT = 0x08;
-        static constexpr uint8_t UP_LEFT    = UP | LEFT;
-        static constexpr uint8_t UP_RIGHT   = UP | RIGHT;
-        static constexpr uint8_t DOWN_LEFT  = DOWN | LEFT;
-        static constexpr uint8_t DOWN_RIGHT = DOWN | RIGHT;
-        static constexpr uint8_t NONE = 0x00;
-    }
-    namespace Button 
-    {
-        static constexpr uint16_t A     = 0x0001;
-        static constexpr uint16_t B     = 0x0002;
-        static constexpr uint16_t X     = 0x0004;
-        static constexpr uint16_t Y     = 0x0008;
-        static constexpr uint16_t L3    = 0x0010;
-        static constexpr uint16_t R3    = 0x0020;
-        static constexpr uint16_t BACK  = 0x0040;
-        static constexpr uint16_t START = 0x0080;
-        static constexpr uint16_t LB    = 0x0100;
-        static constexpr uint16_t RB    = 0x0200;
-        static constexpr uint16_t SYS   = 0x0400;
-        static constexpr uint16_t MISC  = 0x0800;
-    }
-
-} // namespace Gamepad
-
-namespace Scale 
-{
-    namespace INT_16
-    {
-        static constexpr int16_t MIN = INT16_MIN;
-        static constexpr int16_t MID = 0;
-        static constexpr int16_t MAX = INT16_MAX;
-    }
-    namespace UINT_16
-    {
-        static constexpr uint16_t MIN = 0;
-        static constexpr uint16_t MID = 0x8000;
-        static constexpr uint16_t MAX = 0xFFFF;
-    }
-    namespace UINT_8
-    {
-        static constexpr uint8_t  MAX  = 0xFF;
-        static constexpr uint8_t  MID  = 0x80;
-        static constexpr uint8_t  MIN  = 0x00;
-    }
-    namespace INT_10
-    {
-        static constexpr int32_t  MIN  = -512;
-        static constexpr int32_t  MAX  = 511;
-    }
-    namespace UINT_10
-    {
-        static constexpr int32_t MAX = 1023;
-    }
-
-    static inline int16_t int10_to_int16(int32_t value)
-    {
-        constexpr int32_t scale_factor = INT_16::MAX - INT_16::MIN;
-        constexpr int32_t range = INT_10::MAX - INT_10::MIN;
-
-        if (value >= INT_10::MAX)
-        {
-            return INT_16::MAX;
-        }
-        else if (value <= INT_10::MIN)
-        {
-            return INT_16::MIN;
-        }
-
-        int32_t scaled_value = (value - INT_10::MIN) * scale_factor;
-        return static_cast<int16_t>(scaled_value / range + INT_16::MIN);
-    }
-    static inline uint8_t uint10_to_uint8(int32_t value)
-    {
-        if (value > UINT_10::MAX) 
-        {
-            value = UINT_10::MAX;
-        }
-        else if (value < 0) 
-        {
-            value = 0;
-        }
-        return static_cast<uint8_t>(value >> 2);
-    }
-    
-} // namespace Scale
-
-#endif // GAMEPAD_H

Firmware/ESP32/main/Board/board_api.cpp

@@ -9,18 +9,18 @@
 namespace board_api {
 
 SemaphoreHandle_t leds_mutex_ = nullptr;
-SemaphoreHandle_t reset_mutex_ = nullptr;
+// SemaphoreHandle_t reset_mutex_ = nullptr;
 
-void init_pins()
+void init_board()
 {
     if (leds_mutex_ == nullptr)
     {
         leds_mutex_ = xSemaphoreCreateMutex();
     }
-    if (reset_mutex_ == nullptr)
-    {
-        reset_mutex_ = xSemaphoreCreateMutex();
-    }
+    // if (reset_mutex_ == nullptr)
+    // {
+    //     reset_mutex_ = xSemaphoreCreateMutex();
+    // }
 
     if (xSemaphoreTake(leds_mutex_, portMAX_DELAY))
     {

Firmware/ESP32/main/Board/board_api.h

@@ -12,7 +12,7 @@ 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)
@@ -35,7 +35,7 @@ namespace board_api
 
     static constexpr uint8_t NUM_LEDS = sizeof(LED_PINS) / sizeof(LED_PINS[0]);
 
-    void init_pins();
+    void init_board();
     void set_led(uint8_t index, bool state);
     void set_led(bool state);
 }

Firmware/ESP32/main/Board/ogxm_log.h

@@ -0,0 +1,67 @@
+#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_

Firmware/ESP32/main/CMakeLists.txt

@@ -1,15 +1,29 @@
+include(${CMAKE_CURRENT_SOURCE_DIR}/../../FWDefines.cmake)
+
 idf_component_register( 
     SRCS    
         "main.c" 
-        "c_wrapper.cpp"
+        "main.cpp"
         "Board/board_api.cpp"
-        "Bluepad32/Bluepad32.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"
     INCLUDE_DIRS 
         "."
     REQUIRES 
         bluepad32 
         btstack 
         driver 
-        nvs_flash
+        nvs_flash 
+        libfixmath 
+)
+
+target_compile_definitions(${COMPONENT_LIB} PRIVATE
+        FIRMWARE_NAME=\"${FW_NAME}\"
+        FIRMWARE_VERSION=\"${FW_VERSION}\"
 )

Firmware/ESP32/main/Gamepad/Gamepad.h

@@ -0,0 +1,450 @@
+#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

Firmware/ESP32/main/Gamepad/Range.h

@@ -0,0 +1,204 @@
+#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_

Firmware/ESP32/main/Gamepad/fix16ext.h

@@ -0,0 +1,106 @@
+#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

Firmware/ESP32/main/I2CDriver/I2CDriver.cpp

@@ -5,32 +5,38 @@
 #include <esp_log.h>
 
 #include "I2CDriver/I2CDriver.h"
-#include "Bluepad32/Bluepad32.h"
-
-I2CDriver::TaskQueue I2CDriver::task_queue_;
 
 I2CDriver::~I2CDriver()
 {
-    i2c_driver_delete(I2C_NUM_0);
+    i2c_driver_delete(i2c_port_);
 }
 
-void I2CDriver::initialize_i2c()
+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 = GPIO_NUM_21;
-    conf.scl_io_num = GPIO_NUM_22;
+    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 = 1000 * 1000;
+    conf.master.clk_speed = clk_speed;
 
-    i2c_param_config(I2C_NUM_0, &conf);
-    i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0);
+    i2c_param_config(i2c_port_, &conf);
+    i2c_driver_install(i2c_port_, conf.mode, 0, 0, 0);
+
+    initialized_ = true;
 }
 
-void I2CDriver::run_tasks(void* parameter)
+void I2CDriver::run_tasks()
 {
     std::function<void()> task;
 
@@ -43,4 +49,24 @@ void I2CDriver::run_tasks(void* parameter)
 
         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);
+        }
+    });
 }

Firmware/ESP32/main/I2CDriver/I2CDriver.h

@@ -8,6 +8,7 @@
 
 #include "sdkconfig.h"
 #include "RingBuffer.h"
+#include "UserSettings/DeviceDriverTypes.h"
 
 class I2CDriver 
 {
@@ -19,47 +20,36 @@ public:
         true;
 #endif
 
-    enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
+    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;
-        uint8_t packet_id;
-        uint8_t index;
-        uint8_t dpad;
-        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;
-
-        PacketIn()
-        {
-            std::memset(this, 0, sizeof(PacketIn));
-            packet_len = sizeof(PacketIn);
-            packet_id = static_cast<uint8_t>(PacketID::SET_PAD);
-        }
+        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) == 16, "PacketIn is misaligned");
+    static_assert(sizeof(PacketIn) == 32, "PacketIn is misaligned");
 
     struct PacketOut
     {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        uint8_t index;
-        uint8_t rumble_l;
-        uint8_t rumble_r;
-        uint8_t reserved[3];
-
-        PacketOut()
-        {
-            std::memset(this, 0, sizeof(PacketOut));
-            packet_len = sizeof(PacketOut);
-            packet_id = static_cast<uint8_t>(PacketID::GET_PAD);
-        }
+        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)
@@ -67,19 +57,22 @@ public:
     I2CDriver() = default;
     ~I2CDriver();
 
-    void initialize_i2c();
+    void initialize_i2c(i2c_port_t i2c_port, gpio_num_t sda, gpio_num_t scl, uint32_t clk_speed);
 
     //Does not return
-    static void run_tasks(void* parameter);
+    void run_tasks();
 
-    //Thread safe
-    inline void push_task(std::function<void()> task)
-    { 
-        task_queue_.push(task); 
-    }
+    void write_packet(uint8_t address, const PacketIn& data_in);
+    void read_packet(uint8_t address, std::function<void(const PacketOut&)> callback);
 
-    //Don't call directly from another thread, use in push_task
-    inline esp_err_t i2c_write_blocking(uint8_t address, const uint8_t* buffer, size_t len) 
+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);
@@ -92,8 +85,7 @@ public:
         return ret;
     }
 
-    //Don't call directly from another thread, use in push_task
-    inline esp_err_t i2c_read_blocking(uint8_t address, uint8_t* buffer, size_t len) 
+    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);
@@ -111,10 +103,6 @@ public:
         i2c_cmd_link_delete(cmd);
         return ret;
     }
-
-private:
-    using TaskQueue = RingBuffer<std::function<void()>, 6>;
-    static TaskQueue task_queue_;
-};
+}; // class I2CDriver
 
 #endif // _I2C_DRIVER_H_

Firmware/ESP32/main/Kconfig

@@ -1,8 +1,28 @@
 menu "OGXMini Options"
 
+    config I2C_RING_BUFFER_SIZE
+        int "Set I2C ring buffer size"
+        default 6
+
+    config I2C_PORT
+        int "Set I2C port"
+        default 0
+
+    config I2C_SDA_PIN
+        int "Set I2C SDA pin"
+        default 21
+
+    config I2C_SCL_PIN
+        int "Set I2C SCL pin"
+        default 22
+
+    config I2C_BAUDRATE
+        int "Set I2C baudrate"
+        default 1000000
+
     config RESET_PIN
         int "Set reset pin"
-        default 15
+        default 9
 
     config MULTI_SLAVE_MODE
         bool "Enable multiple slave devices"

Firmware/ESP32/main/RingBuffer.h

@@ -5,7 +5,7 @@
 #include <atomic>
 #include <array>
 
-template<typename Type, size_t Size>
+template<typename Type, size_t SIZE>
 class RingBuffer
 {
 public:
@@ -47,9 +47,7 @@ public:
     }
 
 private:
-    const size_t SIZE = Size;
-
-    std::array<Type, Size> buffer_;
+    std::array<Type, SIZE> buffer_;
     std::atomic<size_t> head_;
     std::atomic<size_t> tail_;
 };

Firmware/ESP32/main/UserSettings/DeviceDriverTypes.h

@@ -0,0 +1,39 @@
+#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_

Firmware/ESP32/main/UserSettings/JoystickSettings.cpp

@@ -0,0 +1,63 @@
+#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);
+}

Firmware/ESP32/main/UserSettings/JoystickSettings.h

@@ -0,0 +1,68 @@
+#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_

Firmware/ESP32/main/UserSettings/NVSHelper.h

@@ -0,0 +1,113 @@
+#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_

Firmware/ESP32/main/UserSettings/TriggerSettings.cpp

@@ -0,0 +1,29 @@
+#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));
+}

Firmware/ESP32/main/UserSettings/TriggerSettings.h

@@ -0,0 +1,40 @@
+#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

Firmware/ESP32/main/UserSettings/UserProfile.cpp

@@ -0,0 +1,40 @@
+#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;
+}

Firmware/ESP32/main/UserSettings/UserProfile.h

@@ -0,0 +1,55 @@
+#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_

Firmware/ESP32/main/UserSettings/UserSettings.cpp

@@ -0,0 +1,273 @@
+#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;
+}

Firmware/ESP32/main/UserSettings/UserSettings.h

@@ -0,0 +1,60 @@
+#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_

Firmware/ESP32/main/btstack_config.h

@@ -0,0 +1,82 @@
+#ifndef _ESP_BTSTACK_CONFIG_H
+#define _ESP_BTSTACK_CONFIG_H
+
+// BTstack features that can be enabled
+// #define ENABLE_LE_PERIPHERAL
+// #define ENABLE_LE_CENTRAL
+// #define ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE
+// #define ENABLE_LOG_INFO
+// #define ENABLE_LOG_ERROR
+// #define ENABLE_PRINTF_HEXDUMP
+#define ENABLE_SCO_OVER_HCI
+
+// BTstack configuration. buffers, sizes, ...
+#define HCI_OUTGOING_PRE_BUFFER_SIZE 4
+#define HCI_ACL_PAYLOAD_SIZE (1691 + 4)
+#define HCI_ACL_CHUNK_SIZE_ALIGNMENT 4
+// #define MAX_NR_AVDTP_CONNECTIONS 1
+// #define MAX_NR_AVDTP_STREAM_ENDPOINTS 1
+// #define MAX_NR_AVRCP_CONNECTIONS 2
+// #define MAX_NR_BNEP_CHANNELS 1
+// #define MAX_NR_BNEP_SERVICES 1
+// #define MAX_NR_BTSTACK_LINK_KEY_DB_MEMORY_ENTRIES 2
+// #define MAX_NR_GATT_CLIENTS 4
+// #define MAX_NR_HCI_CONNECTIONS 4
+// #define MAX_NR_HID_HOST_CONNECTIONS 4
+// #define MAX_NR_HIDS_CLIENTS 4
+// #define MAX_NR_HFP_CONNECTIONS 1
+// #define MAX_NR_L2CAP_CHANNELS 6
+// #define MAX_NR_L2CAP_SERVICES 5
+// #define MAX_NR_RFCOMM_CHANNELS 1
+// #define MAX_NR_RFCOMM_MULTIPLEXERS 1
+// #define MAX_NR_RFCOMM_SERVICES 1
+// #define MAX_NR_SERVICE_RECORD_ITEMS 4
+// #define MAX_NR_SM_LOOKUP_ENTRIES 3
+// #define MAX_NR_WHITELIST_ENTRIES 16
+// #define MAX_NR_LE_DEVICE_DB_ENTRIES 16
+
+// Limit number of ACL/SCO Buffer to use by stack to avoid cyw43 shared bus overrun
+#define MAX_NR_CONTROLLER_ACL_BUFFERS 6
+#define MAX_NR_CONTROLLER_SCO_PACKETS 6
+
+// Enable and configure HCI Controller to Host Flow Control to avoid cyw43 shared bus overrun
+#define ENABLE_HCI_CONTROLLER_TO_HOST_FLOW_CONTROL
+#define HCI_HOST_ACL_PACKET_LEN 1024
+#define HCI_HOST_ACL_PACKET_NUM 6
+#define HCI_HOST_SCO_PACKET_LEN 120
+#define HCI_HOST_SCO_PACKET_NUM 6
+
+#ifndef HCI_INCOMING_PRE_BUFFER_SIZE
+#define HCI_INCOMING_PRE_BUFFER_SIZE 6
+#endif
+
+// Link Key DB and LE Device DB using TLV on top of Flash Sector interface
+// #define NVM_NUM_DEVICE_DB_ENTRIES 16
+// #define NVM_NUM_LINK_KEYS 16
+
+// We don't give btstack a malloc, so use a fixed-size ATT DB.
+#define MAX_ATT_DB_SIZE 512
+// #define HAVE_MALLOC
+
+// BTstack HAL configuration
+// #define HAVE_EMBEDDED_TIME_MS
+
+// // map btstack_assert onto Pico SDK assert()
+// #define HAVE_ASSERT
+
+// // Some USB dongles take longer to respond to HCI reset (e.g. BCM20702A).
+#define HCI_RESET_RESEND_TIMEOUT_MS 1000
+
+// #define ENABLE_SOFTWARE_AES128
+// #define ENABLE_MICRO_ECC_FOR_LE_SECURE_CONNECTIONS
+
+// #define HAVE_BTSTACK_STDIN
+
+// To get the audio demos working even with HCI dump at 115200, this truncates long ACL packetws
+#define HCI_DUMP_STDOUT_MAX_SIZE_ACL 100
+
+#ifdef ENABLE_CLASSIC
+#define ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
+#endif
+
+#endif  // _ESP_BTSTACK_CONFIG_H

Firmware/ESP32/main/main.c

@@ -1,7 +1,7 @@
 #include <stdlib.h>
 #include <stdint.h>
 
-#include "c_wrapper.h"
+#include "main.h"
 
 void app_main(void) 
 {

Firmware/ESP32/main/main.cpp

@@ -2,19 +2,17 @@
 #include <freertos/FreeRTOS.h>
 #include <freertos/task.h>
 
-#include "c_wrapper.h"
+#include "main.h"
 #include "sdkconfig.h"
-#include "Bluepad32/Bluepad32.h"
-
-void bp32_run_task(void* parameter)
-{
-    BP32::run_task();
-}
+#include "BTManager/BTManager.h"
 
 void cpp_main()
 {
     xTaskCreatePinnedToCore(
-        bp32_run_task,
+        [](void* parameter)
+        { 
+            BTManager::get_instance().run_task(); 
+        },
         "bp32",
         2048 * 4,
         NULL,

Firmware/ESP32/sdkconfig

@@ -1,6 +1,6 @@
 #
 # Automatically generated file. DO NOT EDIT.
-# Espressif IoT Development Framework (ESP-IDF)  Project Configuration
+# Espressif IoT Development Framework (ESP-IDF) 5.1.5 Project Configuration
 #
 CONFIG_SOC_BROWNOUT_RESET_SUPPORTED="Not determined"
 CONFIG_SOC_TWAI_BRP_DIV_SUPPORTED="Not determined"
@@ -1961,7 +1961,12 @@ CONFIG_WIFI_PROV_STA_ALL_CHANNEL_SCAN=y
 #
 # OGXMini Options
 #
-CONFIG_RESET_PIN=15
+CONFIG_I2C_RING_BUFFER_SIZE=6
+CONFIG_I2C_PORT=0
+CONFIG_I2C_SDA_PIN=21
+CONFIG_I2C_SCL_PIN=22
+CONFIG_I2C_BAUDRATE=1000000
+CONFIG_RESET_PIN=9
 # CONFIG_MULTI_SLAVE_MODE is not set
 CONFIG_ENABLE_LED_1=y
 CONFIG_LED_PIN_1=15

Firmware/FWDefines.cmake

@@ -0,0 +1,2 @@
+set(FW_NAME "OGX-Mini")
+set(FW_VERSION "v1.0.0a3")

Firmware/RP2040/.vscode/settings.json

@@ -1,87 +1,13 @@
 {
     "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
-    "cmake.buildEnvironment": {
-        "PICO_SDK_PATH": "C:/Programming/pico-sdk"
-    },
-    "cmake.environment": {
-        "PICO_SDK_PATH": "C:/Programming/pico-sdk"
-    },
+
     "cmake.configureArgs": [
-        "-DOGXM_BOARD=PI_PICO",
+        "-DOGXM_BOARD=ESP32_BLUERETRO_I2C",
+        // "-DOGXM_RETAIL=TRUE",
         "-DMAX_GAMEPADS=1"
     ],
+    
     "files.associations": {
-        "array": "cpp",
-        "atomic": "cpp",
-        "any": "cpp",
-        "bit": "cpp",
-        "*.tcc": "cpp",
-        "bitset": "cpp",
-        "cctype": "cpp",
-        "charconv": "cpp",
-        "chrono": "cpp",
-        "cinttypes": "cpp",
-        "clocale": "cpp",
-        "cmath": "cpp",
-        "codecvt": "cpp",
-        "compare": "cpp",
-        "concepts": "cpp",
-        "csignal": "cpp",
-        "cstdarg": "cpp",
-        "cstddef": "cpp",
-        "cstdint": "cpp",
-        "cstdio": "cpp",
-        "cstdlib": "cpp",
-        "cstring": "cpp",
-        "ctime": "cpp",
-        "cuchar": "cpp",
-        "cwchar": "cpp",
-        "cwctype": "cpp",
-        "deque": "cpp",
-        "forward_list": "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",
-        "numeric": "cpp",
-        "optional": "cpp",
-        "random": "cpp",
-        "ratio": "cpp",
-        "regex": "cpp",
-        "string_view": "cpp",
-        "system_error": "cpp",
-        "tuple": "cpp",
-        "type_traits": "cpp",
-        "utility": "cpp",
-        "format": "cpp",
-        "fstream": "cpp",
-        "initializer_list": "cpp",
-        "iomanip": "cpp",
-        "iosfwd": "cpp",
-        "iostream": "cpp",
-        "istream": "cpp",
-        "limits": "cpp",
-        "new": "cpp",
-        "numbers": "cpp",
-        "ostream": "cpp",
-        "ranges": "cpp",
-        "semaphore": "cpp",
-        "span": "cpp",
-        "sstream": "cpp",
-        "stdexcept": "cpp",
-        "stop_token": "cpp",
-        "streambuf": "cpp",
-        "thread": "cpp",
-        "typeinfo": "cpp",
-        "valarray": "cpp",
-        "variant": "cpp"
+
     }
 }

Firmware/RP2040/CMakeLists.txt

@@ -1,44 +1,61 @@
 cmake_minimum_required(VERSION 3.13)
 
-set(FW_NAME "OGX-Mini")
-set(FW_VERSION "v1.0.0a")
+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 17)
+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(${EXTERNAL_DIR}/patch_libs.cmake)
+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)
 
-apply_lib_patches(${EXTERNAL_DIR})
-get_pico_sdk(${EXTERNAL_DIR})
-
 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_Standard.cpp
-    ${SRC}/OGXMini/OGXMini_4Channel.cpp
-    ${SRC}/OGXMini/OGXMini_PicoW.cpp
-    ${SRC}/OGXMini/OGXMini_ESP32.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
@@ -68,59 +85,88 @@ set(LIBS_BOARD
     # UART
     hardware_uart
     hardware_irq
+    #fix16
+    libfixmath
 )
 set(INC_DIRS_BOARD
 )
 
 # Config options
 
-# Max gamepads
 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})
 
-# Board type
 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 "ADA_FEATHER")
+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 "RP_ZERO")
+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 "INTERNAL_4CH")
+#     set(EN_USB_HOST TRUE)
+#     set(EN_4CH TRUE)
 
-elseif(OGXM_BOARD STREQUAL "EXTERNAL_4CH")
+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 "PI_PICOW")
-    set(EN_BLUETOOTH TRUE)
-    set(PICO_BOARD pico_w)
-
-elseif(OGXM_BOARD STREQUAL "W_ESP32")
+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")
 
@@ -166,8 +212,12 @@ 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}
@@ -196,10 +246,10 @@ 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 SOURCES_BOARD
+    #     ${SRC}/I2CDriver/4Channel/I2CMaster.cpp
+    #     ${SRC}/I2CDriver/4Channel/I2CSlave.cpp
+    # )
     list(APPEND LIBS_BOARD
         hardware_i2c
         pico_i2c_slave
@@ -209,12 +259,15 @@ endif()
 if(EN_ESP32)
     add_compile_definitions(CONFIG_EN_ESP32=1)
     message(STATUS "ESP32 enabled.")
-    list(APPEND SOURCES_BOARD
-        ${SRC}/I2CDriver/ESP32/I2CDriver.cpp
-    )
+    if (EN_BLUERETRO_I2C)
+        # Nothing
+    else()
+        list(APPEND LIBS_BOARD
+            pico_i2c_slave
+        )
+    endif()
     list(APPEND LIBS_BOARD
         hardware_i2c
-        pico_i2c_slave
     )
 endif()
 
@@ -227,8 +280,12 @@ if(EN_UART_BRIDGE)
     )
 endif()
 
-add_compile_definitions(PICO_FLASH_SIZE_BYTES=${FLASH_SIZE_MB}*1024*1024)
+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)
@@ -237,14 +294,18 @@ if(EXISTS ${SRC}/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid_xremote_rom.h)
 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 "2.1.0")
-    message(FATAL_ERROR "Raspberry Pi Pico SDK version 2.1.0 (or later) required. Your version is ${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)
@@ -256,16 +317,32 @@ 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=1
-        PICO_DEFAULT_UART_TX_PIN=4
-        PICO_DEFAULT_UART_RX_PIN=5
+        PICO_DEFAULT_UART=${UART_PORT}
+        PICO_DEFAULT_UART_TX_PIN=${TX_PIN}
+        PICO_DEFAULT_UART_RX_PIN=${RX_PIN}
     )
-    add_compile_definitions(LOG=1)
+ 
     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
@@ -274,12 +351,13 @@ if(CMAKE_BUILD_TYPE STREQUAL "Debug")
         -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+
+        # -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
@@ -296,7 +374,9 @@ endif()
 pico_set_program_name(${FW_NAME} ${FW_NAME})
 pico_set_program_version(${FW_NAME} ${FW_VERSION})
 
-target_include_directories(${FW_NAME} PRIVATE ${SRC})
+target_include_directories(${FW_NAME} PRIVATE 
+    ${SRC}
+)
 
 if(EN_RGB)
     pico_generate_pio_header(${FW_NAME} ${SRC}/Board/Pico_WS2812/WS2812.pio)
@@ -312,9 +392,26 @@ 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})
 
-set(EXE_FILENAME "${FW_NAME}-${FW_VERSION}-${OGXM_BOARD}${BUILD_STR}")
+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})

Firmware/RP2040/cmake/get_pico_sdk.cmake

@@ -1,16 +1,36 @@
-function(get_pico_sdk EXTERNAL_DIR)
+function(get_pico_sdk EXTERNAL_DIR VERSION_TAG)
     if(NOT DEFINED ENV{PICO_SDK_PATH})
-        message("PICO_SDK_PATH not set, downloading Pico SDK...")
+        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)
+        set(PICO_SDK_PATH ENV{PICO_SDK_PATH} PARENT_SCOPE)
+
     endif()
 
     set(PICOTOOL_FETCH_FROM_GIT_PATH ${EXTERNAL_DIR}/picotool PARENT_SCOPE)

Firmware/RP2040/cmake/get_pico_uart_port.cmake

@@ -0,0 +1,9 @@
+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()

Firmware/RP2040/src/BLEServer/BLEServer.cpp

@@ -0,0 +1,324 @@
+#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

Firmware/RP2040/src/BLEServer/BLEServer.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <cstdint>
+
+#include "Gamepad/Gamepad.h"
+
+namespace BLEServer {
+    void init_server(Gamepad(&gamepads)[MAX_GAMEPADS]);
+}

Firmware/RP2040/src/BLEServer/att_delayed_response.gatt

@@ -0,0 +1,29 @@
+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,

Firmware/RP2040/src/Bluepad32/Bluepad32.cpp

@@ -10,9 +10,10 @@
 #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"
+    #error "Pico W must use BLUEPAD32_PLATFORM_CUSTOM"
 #endif
 
 static_assert((CONFIG_BLUEPAD32_MAX_DEVICES == MAX_GAMEPADS), "Mismatch between BP32 and Gamepad max devices");
@@ -22,8 +23,7 @@ namespace bluepad32 {
 static constexpr uint32_t FEEDBACK_TIME_MS = 250;
 static constexpr uint32_t LED_CHECK_TIME_MS = 500;
 
-struct BTDevice
-{
+struct BTDevice {
     bool connected{false};
     Gamepad* gamepad{nullptr};
 };
@@ -34,6 +34,18 @@ 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)
 {
@@ -110,73 +122,61 @@ static void check_led_cb(btstack_timer_source *ts)
 
 //BT Driver
 
-static void init(int argc, const char** arg_V)
-{
-
+static void init(int argc, const char** arg_V) {
 }
 
-static void init_complete_cb(void) 
-{
+static void init_complete_cb(void) {
     uni_bt_enable_new_connections_unsafe(true);
-
-    uni_bt_del_keys_unsafe();
-
+    // 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))
-    {
+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_connected_cb(uni_hid_device_t* device) {
 }
 
-static void device_disconnected_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)
-    {
+    if (idx >= MAX_GAMEPADS || idx < 0) {
         return;
     }
-    if (!led_timer_set_)
-    {
+
+    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())
-    {
+    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) 
-{    
+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)
-    {
+    if (idx >= MAX_GAMEPADS || idx < 0) {
         return UNI_ERROR_SUCCESS;
     }
 
     bt_devices_[idx].connected = true;
-    if (led_timer_set_)
-    {
+
+    if (led_timer_set_) {
         led_timer_set_ = false;
         btstack_run_loop_remove_timer(&led_timer_);
         board_api::set_led(true);
     }
-    if (!feedback_timer_set_)
-    {
+    if (!feedback_timer_set_) {
         feedback_timer_set_ = true;
         feedback_timer_.process = send_feedback_cb;
         feedback_timer_.context = nullptr;
@@ -186,17 +186,14 @@ static uni_error_t device_ready_cb(uni_hid_device_t* device)
     return UNI_ERROR_SUCCESS;
 }
 
-static void oob_event_cb(uni_platform_oob_event_t event, void* data) 
-{
+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 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)
-    {
+    if (controller->klass != UNI_CONTROLLER_CLASS_GAMEPAD){
         return;
     }
 
@@ -248,13 +245,11 @@ static void controller_data_cb(uni_hid_device_t* device, uni_controller_t* contr
     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 = Scale::uint10_to_uint8(uni_gp->brake);
-    gp_in.trigger_r = Scale::uint10_to_uint8(uni_gp->throttle);
-
-    gp_in.joystick_lx = Scale::int10_to_int16(uni_gp->axis_x);
-    gp_in.joystick_ly = Scale::int10_to_int16(uni_gp->axis_y);
-    gp_in.joystick_rx = Scale::int10_to_int16(uni_gp->axis_rx);
-    gp_in.joystick_ry = Scale::int10_to_int16(uni_gp->axis_ry);
+    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);
 }
@@ -284,7 +279,7 @@ uni_platform* get_driver()
 
 //Public API
 
-void run_task(Gamepad (&gamepads)[MAX_GAMEPADS])
+void run_task(Gamepad(&gamepads)[MAX_GAMEPADS])
 {
     for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
     {
@@ -303,16 +298,4 @@ void run_task(Gamepad (&gamepads)[MAX_GAMEPADS])
     btstack_run_loop_execute();
 }
 
-bool any_connected()
-{
-    for (auto& device : bt_devices_)
-    {
-        if (device.connected)
-        {
-            return true;
-        }
-    }
-    return false;
-}
-
 } // namespace bluepad32 

Firmware/RP2040/src/Bluepad32/Bluepad32.h

@@ -1,21 +1,14 @@
-#ifndef _BLUEPAD_32_H_
-#define _BLUEPAD_32_H_
+#pragma once
 
 #include <cstdint>
 #include <array>
 
-#include "Gamepad.h"
-#include "board_config.h"
-#include "UserSettings/UserProfile.h"
+#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]);
-    bool any_connected();
-
-} //namespace bluepad32
-
-#endif // _BLUEPAD_32_H_
+namespace bluepad32 {
+    void run_task(Gamepad(&gamepads)[MAX_GAMEPADS]);
+} 

Firmware/RP2040/src/Board/Config.h

@@ -0,0 +1,129 @@
+#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_

Firmware/RP2040/src/Board/board_api.cpp

@@ -1,225 +1,106 @@
 #include <pico/stdlib.h>
 #include <pico/mutex.h>
+#include <pico/multicore.h>
 #include <hardware/clocks.h>
-#include <hardware/gpio.h>
-#include <hardware/watchdog.h>
 
+#include "tusb.h"
+
+#include "Board/Config.h"
 #include "Board/board_api.h"
-#include "OGXMini/Debug.h"
-#include "board_config.h"
-
-#if defined(CONFIG_EN_BLUETOOTH)
-#include <pico/cyw43_arch.h>
-#endif // defined(CONFIG_EN_BLUETOOTH)
-
-#if defined(CONFIG_EN_RGB)
-#include "WS2812.pio.h"
-#include "Board/Pico_WS2812/WS2812.hpp"
-#endif // defined(CONFIG_EN_RGB)
+#include "Board/ogxm_log.h"
+#include "Board/board_api_private/board_api_private.h"
+#include "TaskQueue/TaskQueue.h"
 
 namespace board_api {
 
-bool inited_ = false;
 mutex_t gpio_mutex_;
 
-void init_vcc_en_pin_unsafe()
-{
-#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 
-}
-
-void init_rgb_unsafe()
-{
-#if defined(CONFIG_EN_RGB) && defined(RGB_PWR_PIN)
-        gpio_init(RGB_PWR_PIN);
-        gpio_set_dir(RGB_PWR_PIN, GPIO_OUT);
-        gpio_put(RGB_PWR_PIN, 1);
-#endif // defined(CONFIG_EN_RGB) && defined(RGB_PWR_PIN)
-}
-
-void init_led_indicator_unsafe()
-{
-#if defined(LED_INDICATOR_PIN) && !defined(CONFIG_EN_BLUETOOTH)   
-    gpio_init(LED_INDICATOR_PIN);
-    gpio_set_dir(LED_INDICATOR_PIN, GPIO_OUT);
-    gpio_put(LED_INDICATOR_PIN, 0);
-#endif // defined(LED_INDICATOR_PIN)
-}
-
-void init_esp32_io_unsafe()
-{
-#if defined(CONFIG_EN_ESP32)
-    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);
-
-#endif //defined(CONFIG_EN_ESP32)
-}
-
-void init_uart_bridge_io_unsafe()
-{
-#if defined(CONFIG_EN_UART_BRIDGE)
-    gpio_init(MODE_SEL_PIN);
-    gpio_set_dir(MODE_SEL_PIN, GPIO_IN);
-    gpio_pull_up(MODE_SEL_PIN);
-
-#endif // defined(CONFIG_EN_UART_BRIDGE)
-}
-
-void init_uart_debug_unsafe()
-{
-#if defined(OGXM_DEBUG)
-    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);
-#endif // defined(OGXM_DEBUG)
-}
-
-void set_led(bool state)
-{
-    mutex_enter_blocking(&gpio_mutex_);
-
-    if (!inited_)
-    {
-        mutex_exit(&gpio_mutex_);
-        return;
+bool usb::host_connected() {
+    if (board_api_usbh::host_connected) {
+        return board_api_usbh::host_connected();
     }
-
-#if defined(CONFIG_EN_RGB)
-    static WS2812 ws2812 = WS2812(RGB_PXL_PIN, 1, pio1, 0, WS2812::FORMAT_GRB);
-
-    ws2812.setPixelColor(0, state ? WS2812::RGB(0x00, 0xFF, 0x00) : WS2812::RGB(0xFF, 0, 0));
-    ws2812.show();
-
-#endif // defined(CONFIG_EN_RGB)
-
-#if defined(CONFIG_EN_BLUETOOTH)
-    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, state ? 1 : 0);
-
-#elif defined(LED_INDICATOR_PIN)  
-    gpio_put(LED_INDICATOR_PIN, state ? 1 : 0);
-
-#endif //defined(CONFIG_EN_RGB)
-
-    mutex_exit(&gpio_mutex_);
+    return false;
 }
 
-#if defined(CONFIG_EN_UART_BRIDGE)
-bool uart_bridge_mode()
-{
-    if (!inited_)
-    {
-        return false;
-    }
+//Only call this from core0
+void usb::disconnect_all() {
+    OGXM_LOG("Disconnecting USB and resetting Core1\n");
 
-    bool mode = false;
-    mutex_enter_blocking(&gpio_mutex_);
-
-    gpio_pull_up(MODE_SEL_PIN);
-    if (gpio_get(MODE_SEL_PIN) == 0) 
-    {
-        mode = true;
-    } 
-
-    mutex_exit(&gpio_mutex_);
-    return mode;
-}  
-#endif // defined(CONFIG_EN_UART_BRIDGE)
-
-#if defined(CONFIG_EN_ESP32)
-void reset_esp32_unsafe() 
-{
-    gpio_put(ESP_RST_PIN, 0);
+    TaskQueue::suspend_delayed_tasks();
+    multicore_reset_core1();
+    sleep_ms(500);
+    tud_disconnect();
     sleep_ms(500);
-    gpio_put(ESP_RST_PIN, 1);
-    sleep_ms(250);
 }
 
-void reset_esp32() 
-{
-    if (!inited_)
-    {
-        return;
-    }
+// 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_);
-    reset_esp32_unsafe();
+
+    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 enter_esp32_prog_mode() 
-{
-    if (!inited_)
-    {
-        return;
-    }
-    mutex_enter_blocking(&gpio_mutex_);
-
-    gpio_put(ESP_PROG_PIN, 1);
-    sleep_ms(250);
-    gpio_put(ESP_PROG_PIN, 0);
-	sleep_ms(250);
-
-    reset_esp32_unsafe();
-
-	gpio_put(ESP_PROG_PIN, 1);
-
-    mutex_exit(&gpio_mutex_);
-}
-#endif // defined(CONFIG_EN_ESP32)
-
-void reboot()
-{
+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);
 }
 
-void init_board()
-{
-    if (inited_)
-    {
-        return;
-    }
+uint32_t ms_since_boot() {
+    return to_ms_since_boot(get_absolute_time());
+}
 
-    if (!set_sys_clock_khz(SYSCLOCK_KHZ, true) && !set_sys_clock_khz(120000, true))
-    {
-        panic("Failed to set sys clock");
+//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_))
-    {
+    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_);
     }
-    
-    mutex_enter_blocking(&gpio_mutex_);
-
-    init_uart_debug_unsafe();
-    init_vcc_en_pin_unsafe();
-    init_rgb_unsafe();
-    init_led_indicator_unsafe();
-    init_esp32_io_unsafe();
-    init_uart_bridge_io_unsafe();
-
-    inited_ = true;
-
-    mutex_exit(&gpio_mutex_);
-
-#if (OGXM_BOARD != PI_PICOW)
-    set_led(false);
-#endif
-
     OGXM_LOG("Board initialized\n");
 }
 

Firmware/RP2040/src/Board/board_api.h

@@ -4,15 +4,17 @@
 #include <cstdint>
 #include <string>
 
-namespace board_api
-{
+namespace board_api {
     void init_board();
-    void set_led(bool state);
+    void init_bluetooth();
     void reboot();
+    void set_led(bool state);
+    uint32_t ms_since_boot();
 
-    bool uart_bridge_mode();
-    void reset_esp32();
-    void enter_esp32_prog_mode();
+    namespace usb {
+        bool host_connected();
+        void disconnect_all();
+    }
 }
 
 #endif // _OGXM_BOARD_API_H_

Firmware/RP2040/src/Board/board_api_private/board_api_bt.cpp

@@ -0,0 +1,34 @@
+#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)

Firmware/RP2040/src/Board/board_api_private/board_api_led.cpp

@@ -0,0 +1,20 @@
+#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

Firmware/RP2040/src/Board/board_api_private/board_api_private.h

@@ -0,0 +1,29 @@
+#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

Firmware/RP2040/src/Board/board_api_private/board_api_rgb.cpp

@@ -0,0 +1,33 @@
+#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)

Firmware/RP2040/src/Board/board_api_private/board_api_usbh.cpp

@@ -0,0 +1,60 @@
+#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)

Firmware/RP2040/src/Board/esp32_api.cpp

@@ -0,0 +1,50 @@
+#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)

Firmware/RP2040/src/Board/esp32_api.h

@@ -0,0 +1,13 @@
+#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)

Firmware/RP2040/src/Board/ogxm_log.cpp

@@ -0,0 +1,89 @@
+#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)

Firmware/RP2040/src/Board/ogxm_log.h

@@ -0,0 +1,55 @@
+#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

Firmware/RP2040/src/Descriptors/DInput.h

@@ -1,12 +1,11 @@
-#ifndef _DINPUT_DESCRIPTORS_H_
-#define _DINPUT_DESCRIPTORS_H_
+#pragma once
 
 #include <cstdint>
 #include <cstring>
 
 #include "tusb.h"
 
-#include "board_config.h"
+#include "Board/Config.h"
 
 namespace DInput 
 {
@@ -33,7 +32,7 @@ namespace DInput
         static constexpr uint8_t START  = 0x02;
         static constexpr uint8_t L3     = 0x04;
         static constexpr uint8_t R3     = 0x08;
-        static constexpr uint8_t PS     = 0x10;
+        static constexpr uint8_t SYS    = 0x10;
         static constexpr uint8_t TP     = 0x20;
     };
 
@@ -228,8 +227,7 @@ namespace DInput
     //     0x0A,        // bInterval 10 (unit depends on device speed)
     // };
 
-    enum Itf
-    { 
+    enum Itf { 
         NUM_HID1 = 0, 
 #if MAX_GAMEPADS > 1
         NUM_HID2,
@@ -297,6 +295,4 @@ namespace DInput
 #endif
     };
 
-}; // namespace DInput
-
-#endif // _DINPUT_DESCRIPTORS_H_
+}; // namespace DInput

Firmware/RP2040/src/Descriptors/PS3.h

@@ -3,9 +3,7 @@
 
 #include <stdint.h>
 #include <cstring>
-#include <pico/rand.h>
-
-#include "tusb.h"
+#include <random>
 
 namespace PS3 
 {
@@ -75,7 +73,7 @@ namespace PS3
 
 	namespace Buttons2
 	{
-		static constexpr uint8_t PS = 0x01;
+		static constexpr uint8_t SYS = 0x01;
 		static constexpr uint8_t TP = 0x02;
 	};
 
@@ -190,6 +188,7 @@ namespace PS3
 		0xFF, 0xFF,
 		0x00, 0x20, 0x40, 0xCE, 0x00, 0x00, 0x00,
 		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00
 	};
 
 	struct BTInfo
@@ -201,15 +200,18 @@ namespace PS3
 
 		BTInfo()
 		{
-			std::memset(this, 0, sizeof(BTInfo));
 			std::memcpy(device_address, DEFAULT_BT_INFO_HEADER, sizeof(DEFAULT_BT_INFO_HEADER));
-			for (uint8_t addr = 0; addr < 3; addr++) 
+
+            std::mt19937 gen(12345);
+            std::uniform_int_distribution<uint8_t> dist(0, 0xFF);
+
+			for (uint8_t i = 4; i < sizeof(device_address); i++) 
 			{
-				device_address[4 + addr] = static_cast<uint8_t>(get_rand_32() % 0xff);
+				device_address[i] = dist(gen);
 			}
-			for (uint8_t addr = 0; addr < 6; addr++) 
+			for (uint8_t i = 1; i < sizeof(host_address); i++) 
 			{
-				host_address[1 + addr] = static_cast<uint8_t>(get_rand_32() % 0xff);
+				host_address[i] = dist(gen);
 			}
 		}
 	};

Firmware/RP2040/src/Descriptors/XInput.h

@@ -108,15 +108,26 @@ namespace XInput
 		int16_t joystick_rx;
 		int16_t joystick_ry;
 		uint8_t reserved[6];
+
+		InReport()
+		{
+			std::memset(this, 0, sizeof(InReport));
+			report_size = sizeof(InReport);
+		}
 	};
-	static_assert(sizeof(InReport) == 20, "XInput::InReport is not the correct size");
+	static_assert(sizeof(InReport) == 20, "XInput::InReport is misaligned");
 
 	struct WiredChatpadReport
 	{
 		uint8_t report_id;
 		uint8_t chatpad[3];
+
+		WiredChatpadReport()
+		{
+			std::memset(this, 0, sizeof(WiredChatpadReport));
+		}
 	};
-	static_assert(sizeof(WiredChatpadReport) == 4, "XInput::WiredChatpadReport is not the correct size");
+	static_assert(sizeof(WiredChatpadReport) == 4, "XInput::WiredChatpadReport is misaligned");
 
 	struct InReportWireless
 	{
@@ -137,9 +148,10 @@ namespace XInput
 		InReportWireless()
 		{
 			std::memset(this, 0, sizeof(InReportWireless));
+			report_size = sizeof(InReportWireless);
 		}
 	};
-	static_assert(sizeof(InReportWireless) == 28, "XInput::InReportWireless is not the correct size");
+	static_assert(sizeof(InReportWireless) == 28, "XInput::InReportWireless is misaligned");
 
 	struct OutReport
 	{
@@ -149,8 +161,13 @@ namespace XInput
 		uint8_t rumble_l;
 		uint8_t rumble_r;
 		uint8_t reserved[3];
+
+		OutReport()
+		{
+			std::memset(this, 0, sizeof(OutReport));
+		}
 	};
-	static_assert(sizeof(OutReport) == 8, "XInput::OutReport is not the correct size");
+	static_assert(sizeof(OutReport) == 8, "XInput::OutReport is misaligned");
 	#pragma pack(pop)
 
 	static const uint8_t STRING_LANGUAGE[]     = { 0x09, 0x04 };
@@ -158,7 +175,7 @@ namespace XInput
 	static const uint8_t STRING_PRODUCT[]      = "XInput STANDARD GAMEPAD";
 	static const uint8_t STRING_VERSION[]      = "1.0";
 
-	static const uint8_t *STRING_DESCRIPTORS[] __attribute__((unused)) =
+	static const uint8_t *DESC_STRING[] __attribute__((unused)) =
 	{
 		STRING_LANGUAGE,
 		STRING_MANUFACTURER,
@@ -166,7 +183,7 @@ namespace XInput
 		STRING_VERSION
 	};
 
-	static const uint8_t DEVICE_DESCRIPTORS[] =
+	static const uint8_t DESC_DEVICE[] =
 	{
 		0x12,       // bLength
 		0x01,       // bDescriptorType (Device)
@@ -184,7 +201,7 @@ namespace XInput
 		0x01,       // bNumConfigurations 1
 	};
 
-	static const uint8_t CONFIGURATION_DESCRIPTORS[] =
+	static const uint8_t DESC_CONFIGURATION[] =
 	{
 		0x09,        // bLength
 		0x02,        // bDescriptorType (Configuration)

Firmware/RP2040/src/Descriptors/XboxOG.h

@@ -447,42 +447,6 @@ namespace XboxOG
 
         static constexpr uint16_t DRIVER_LEN = 9+7+9;
 
-        // enum xboxog_xremote_xid_interface
-        // {
-        //     ITF_NUM_XID_XREMOTE = 0,
-        //     ITF_NUM_XID_XREMOTE_ROM,
-        //     XID_REMOTE_ITF_NUM_TOTAL
-        // };
-
-        // #define TUD_XID_XREMOTE_DESC_LEN  (9+7+9)
-
-        // #define TUD_XID_XREMOTE_DESCRIPTOR(_itfnum, _epin) \
-        //     /* Interface 0 (HID DATA)*/\
-        //     9, TUSB_DESC_INTERFACE, _itfnum, 0, 1, XID_REMOTE_INTERFACE_CLASS, XID_REMOTE_INTERFACE_SUBCLASS, 0x00, 0x00,\
-        //     /* Endpoint In */\
-        //     7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(8), 16, \
-        //     /* Interface 1 (ROM DATA)*/\
-        //     9, TUSB_DESC_INTERFACE, _itfnum + 1, 0, 0, XID_XREMOTE_ROM_CLASS, 0x00, 0x00, 0x00
-
-
-        // #define XID_REMOTE_CONFIG_TOTAL_LEN \
-        //     (TUD_CONFIG_DESC_LEN) + \
-        //     (TUD_XID_XREMOTE_DESC_LEN)
-
-        // static const uint8_t CONFIGURATION_DESCRIPTORS[] =
-        // {
-        //     // Config number, interface count, string index, total length, attribute, power in mA
-        //     TUD_CONFIG_DESCRIPTOR(  1, 
-        //                             XID_REMOTE_ITF_NUM_TOTAL, 
-        //                             0, 
-        //                             XID_REMOTE_CONFIG_TOTAL_LEN, 
-        //                             TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, 
-        //                             500),
-
-        //     TUD_XID_XREMOTE_DESCRIPTOR( ITF_NUM_XID_XREMOTE, 
-        //                                 0x80 | (ITF_NUM_XID_XREMOTE + 1)),
-        // };
-
         static const uint8_t DEVICE_DESCRIPTORS[] =
         {
             0x12,        // bLength

Firmware/RP2040/src/Gamepad.h

@@ -1,338 +0,0 @@
-#ifndef _GAMEPAD_H_
-#define _GAMEPAD_H_
-
-#include <cstdint>
-#include <atomic>
-#include <limits>
-#include <cstring>
-#include <array>
-#include <pico/mutex.h>
-
-#include "Scale.h"
-#include "UserSettings/UserProfile.h"
-
-class Gamepad 
-{
-public:
-    //Defaults used by device to get buttons
-
-    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;
-
-    //Mappings used by host to set buttons
-
-    uint8_t MAP_DPAD_UP         = DPAD_UP        ;
-    uint8_t MAP_DPAD_DOWN       = DPAD_DOWN      ;
-    uint8_t MAP_DPAD_LEFT       = DPAD_LEFT      ;
-    uint8_t MAP_DPAD_RIGHT      = DPAD_RIGHT     ;
-    uint8_t MAP_DPAD_UP_LEFT    = DPAD_UP_LEFT   ;
-    uint8_t MAP_DPAD_UP_RIGHT   = DPAD_UP_RIGHT  ;
-    uint8_t MAP_DPAD_DOWN_LEFT  = DPAD_DOWN_LEFT ;
-    uint8_t MAP_DPAD_DOWN_RIGHT = DPAD_DOWN_RIGHT;
-    uint8_t MAP_DPAD_NONE       = DPAD_NONE      ;
-
-    uint16_t MAP_BUTTON_A     = BUTTON_A    ;
-    uint16_t MAP_BUTTON_B     = BUTTON_B    ;
-    uint16_t MAP_BUTTON_X     = BUTTON_X    ;
-    uint16_t MAP_BUTTON_Y     = BUTTON_Y    ;
-    uint16_t MAP_BUTTON_L3    = BUTTON_L3   ;
-    uint16_t MAP_BUTTON_R3    = BUTTON_R3   ;
-    uint16_t MAP_BUTTON_BACK  = BUTTON_BACK ;
-    uint16_t MAP_BUTTON_START = BUTTON_START;
-    uint16_t MAP_BUTTON_LB    = BUTTON_LB   ;
-    uint16_t MAP_BUTTON_RB    = BUTTON_RB   ;
-    uint16_t MAP_BUTTON_SYS   = BUTTON_SYS  ;
-    uint16_t MAP_BUTTON_MISC  = BUTTON_MISC ;
-
-    uint8_t MAP_ANALOG_OFF_UP    = ANALOG_OFF_UP   ;
-    uint8_t MAP_ANALOG_OFF_DOWN  = ANALOG_OFF_DOWN ;
-    uint8_t MAP_ANALOG_OFF_LEFT  = ANALOG_OFF_LEFT ;
-    uint8_t MAP_ANALOG_OFF_RIGHT = ANALOG_OFF_RIGHT;
-    uint8_t MAP_ANALOG_OFF_A     = ANALOG_OFF_A    ;
-    uint8_t MAP_ANALOG_OFF_B     = ANALOG_OFF_B    ;
-    uint8_t MAP_ANALOG_OFF_X     = ANALOG_OFF_X    ;
-    uint8_t MAP_ANALOG_OFF_Y     = ANALOG_OFF_Y    ;
-    uint8_t MAP_ANALOG_OFF_LB    = ANALOG_OFF_LB   ;
-    uint8_t MAP_ANALOG_OFF_RB    = ANALOG_OFF_RB   ;
-
-#pragma pack(push, 1)
-    struct PadIn
-    {
-        uint8_t dpad;
-        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 chatpad[3];
-        uint8_t analog[10];
-    
-        PadIn()
-        {
-            std::memset(this, 0, sizeof(PadIn));
-        }
-    };
-
-    struct PadOut
-    {
-        uint8_t rumble_l;
-        uint8_t rumble_r;
-
-        PadOut()
-        {
-            std::memset(this, 0, sizeof(PadOut));
-        }
-    };
-
-    using ChatpadIn = std::array<uint8_t, 3>;
-
-#pragma pack(pop)
-
-    Gamepad()
-    {
-        mutex_init(&pad_in_mutex_);
-        mutex_init(&pad_out_mutex_);
-        mutex_init(&chatpad_in_mutex_);
-        reset_pad_in();
-        reset_pad_out();
-        reset_chatpad_in();
-        setup_deadzones();
-    };
-
-    ~Gamepad() = default;
-
-    //Get
-    inline bool new_pad_in() const { return new_pad_in_.load(); }
-    inline bool new_pad_out() const { return new_pad_out_.load(); }
-
-    //True if both host and device have enabled analog
-    inline bool analog_enabled() const { return analog_enabled_.load(std::memory_order_relaxed); }
-
-    inline PadIn get_pad_in()
-    {
-        PadIn pad_in;
-
-        mutex_enter_blocking(&pad_in_mutex_);
-        pad_in = pad_in_;
-        new_pad_in_.store(false);
-        mutex_exit(&pad_in_mutex_);
-
-        return pad_in;
-    }
-
-    inline PadOut get_pad_out()
-    {
-        PadOut pad_out;
-
-        mutex_enter_blocking(&pad_out_mutex_);
-        pad_out = pad_out_;
-        new_pad_out_.store(false);
-        mutex_exit(&pad_out_mutex_);
-
-        return pad_out;
-    }
-
-    inline ChatpadIn get_chatpad_in()
-    {
-        ChatpadIn chatpad_in;
-
-        mutex_enter_blocking(&chatpad_in_mutex_);
-        chatpad_in = chatpad_in_;
-        mutex_exit(&chatpad_in_mutex_);
-
-        return chatpad_in;
-    }
-
-    //Set
-
-    void set_analog_device(bool value) 
-    { 
-        analog_device_.store(value); 
-        if (analog_host_.load() && analog_device_.load() && profile_.analog_enabled)
-        {
-            analog_enabled_.store(true);
-        }
-    }
-
-    void set_analog_host(bool value) 
-    { 
-        analog_host_.store(value); 
-        if (analog_host_.load() && analog_device_.load() && profile_.analog_enabled)
-        {
-            analog_enabled_.store(true);
-        }
-    }
-
-    void set_profile(const UserProfile& user_profile) 
-    { 
-        profile_ = user_profile; 
-        setup_mappings();
-        setup_deadzones();
-    }
-
-    inline void set_pad_in(PadIn pad_in)
-    {
-        pad_in.trigger_l = (pad_in.trigger_l > dz_.trigger_l) ? pad_in.trigger_l : UINT_8::MIN;
-        pad_in.trigger_r = (pad_in.trigger_r > dz_.trigger_r) ? pad_in.trigger_r : UINT_8::MIN;
-        pad_in.joystick_lx = (pad_in.joystick_lx < dz_.joystick_l_neg || pad_in.joystick_lx > dz_.joystick_l_pos) ? pad_in.joystick_lx : INT_16::MID;
-        pad_in.joystick_ly = (pad_in.joystick_ly < dz_.joystick_l_neg || pad_in.joystick_ly > dz_.joystick_l_pos) ? pad_in.joystick_ly : INT_16::MID;
-        pad_in.joystick_rx = (pad_in.joystick_rx < dz_.joystick_r_neg || pad_in.joystick_rx > dz_.joystick_r_pos) ? pad_in.joystick_rx : INT_16::MID;
-        pad_in.joystick_ry = (pad_in.joystick_ry < dz_.joystick_r_neg || pad_in.joystick_ry > dz_.joystick_r_pos) ? pad_in.joystick_ry : INT_16::MID;
-        pad_in.joystick_ly = profile_.invert_ly ? Scale::invert_joy(pad_in.joystick_ly) : pad_in.joystick_ly;
-        pad_in.joystick_ry = profile_.invert_ry ? Scale::invert_joy(pad_in.joystick_ry) : pad_in.joystick_ry;
-
-        mutex_enter_blocking(&pad_in_mutex_);
-        pad_in_ = pad_in;
-        new_pad_in_.store(true);
-        mutex_exit(&pad_in_mutex_);
-    }
-
-    inline void set_pad_out(const PadOut& pad_out)
-    {
-        mutex_enter_blocking(&pad_out_mutex_);
-        pad_out_ = pad_out;
-        new_pad_out_.store(true);
-        mutex_exit(&pad_out_mutex_);
-    }
-
-    inline void set_chatpad_in(const ChatpadIn& chatpad_in)
-    {
-        mutex_enter_blocking(&chatpad_in_mutex_);
-        chatpad_in_ = chatpad_in;
-        mutex_exit(&chatpad_in_mutex_);
-    }
-
-    inline void reset_pad_in() 
-	{ 
-        mutex_enter_blocking(&pad_in_mutex_);
-        std::memset(reinterpret_cast<void*>(&pad_in_), 0, sizeof(pad_in_));
-        mutex_exit(&pad_in_mutex_);
-        new_pad_in_.store(true);
-    }
-    
-    inline void reset_pad_out()
-    {
-        mutex_enter_blocking(&pad_out_mutex_);
-        std::memset(reinterpret_cast<void*>(&pad_out_), 0, sizeof(pad_out_));
-        new_pad_out_.store(true);
-        mutex_exit(&pad_out_mutex_);
-    }
-
-    inline void reset_chatpad_in()
-    {
-        mutex_enter_blocking(&chatpad_in_mutex_);
-        chatpad_in_.fill(0);
-        mutex_exit(&chatpad_in_mutex_);
-    }
-
-private:
-    mutex_t pad_in_mutex_;
-    mutex_t pad_out_mutex_;
-    mutex_t chatpad_in_mutex_;
-
-    PadOut pad_out_;
-    PadIn pad_in_;
-    ChatpadIn chatpad_in_;
-
-    std::atomic<bool> new_pad_in_{false};
-    std::atomic<bool> new_pad_out_{false};
-
-    std::atomic<bool> analog_enabled_{false};
-    std::atomic<bool> analog_host_{false};
-    std::atomic<bool> analog_device_{false};
-
-    UserProfile profile_;
-
-    struct Deadzones
-    {
-        uint8_t trigger_l{0};
-        uint8_t trigger_r{0};
-        int16_t joystick_l_neg{0};
-        int16_t joystick_l_pos{0};
-        int16_t joystick_r_neg{0};
-        int16_t joystick_r_pos{0};
-    } dz_;
-
-    void setup_mappings()
-    {
-        MAP_DPAD_UP         = profile_.dpad_up;
-        MAP_DPAD_DOWN       = profile_.dpad_down;
-        MAP_DPAD_LEFT       = profile_.dpad_left;
-        MAP_DPAD_RIGHT      = profile_.dpad_right;
-        MAP_DPAD_UP_LEFT    = profile_.dpad_up | profile_.dpad_left;
-        MAP_DPAD_UP_RIGHT   = profile_.dpad_up | profile_.dpad_right;
-        MAP_DPAD_DOWN_LEFT  = profile_.dpad_down | profile_.dpad_left;
-        MAP_DPAD_DOWN_RIGHT = profile_.dpad_down | profile_.dpad_right;
-        MAP_DPAD_NONE       = 0;
-
-        MAP_BUTTON_A     = profile_.button_a;
-        MAP_BUTTON_B     = profile_.button_b;
-        MAP_BUTTON_X     = profile_.button_x;
-        MAP_BUTTON_Y     = profile_.button_y;
-        MAP_BUTTON_L3    = profile_.button_l3;
-        MAP_BUTTON_R3    = profile_.button_r3;
-        MAP_BUTTON_BACK  = profile_.button_back;
-        MAP_BUTTON_START = profile_.button_start;
-        MAP_BUTTON_LB    = profile_.button_lb;
-        MAP_BUTTON_RB    = profile_.button_rb;
-        MAP_BUTTON_SYS   = profile_.button_sys;
-        MAP_BUTTON_MISC  = profile_.button_misc;
-
-        MAP_ANALOG_OFF_UP    = profile_.analog_off_up;
-        MAP_ANALOG_OFF_DOWN  = profile_.analog_off_down;
-        MAP_ANALOG_OFF_LEFT  = profile_.analog_off_left;
-        MAP_ANALOG_OFF_RIGHT = profile_.analog_off_right;
-        MAP_ANALOG_OFF_A     = profile_.analog_off_a;
-        MAP_ANALOG_OFF_B     = profile_.analog_off_b;
-        MAP_ANALOG_OFF_X     = profile_.analog_off_x;
-        MAP_ANALOG_OFF_Y     = profile_.analog_off_y;
-        MAP_ANALOG_OFF_LB    = profile_.analog_off_lb;
-        MAP_ANALOG_OFF_RB    = profile_.analog_off_rb;
-    }
-
-    void setup_deadzones() //Deadzones in the profile are 0-255 (0-100%)
-    {
-        dz_.trigger_l = profile_.dz_trigger_l;
-        dz_.trigger_r = profile_.dz_trigger_r;
-
-        dz_.joystick_l_pos = Scale::uint8_to_int16(profile_.dz_joystick_l / 2);
-        dz_.joystick_l_neg = Scale::invert_joy(dz_.joystick_l_pos);
-        dz_.joystick_r_pos = Scale::uint8_to_int16(profile_.dz_joystick_r / 2);
-        dz_.joystick_r_neg = Scale::invert_joy(dz_.joystick_r_pos);
-    }
-};
-
-#endif // _GAMEPAD_H_

Firmware/RP2040/src/Gamepad/Gamepad.h

@@ -0,0 +1,640 @@
+#ifndef _GAMEPAD_H_
+#define _GAMEPAD_H_
+
+#include <cstdint>
+#include <atomic>
+#include <limits>
+#include <cstring>
+#include <array>
+#include <cmath>
+#include <pico/mutex.h>
+
+#include "libfixmath/fix16.hpp"
+
+#include "Gamepad/Range.h"
+#include "Gamepad/fix16ext.h"
+#include "UserSettings/UserProfile.h"
+#include "UserSettings/JoystickSettings.h"
+#include "UserSettings/TriggerSettings.h"
+#include "Board/ogxm_log.h"
+
+class Gamepad 
+{
+public:
+    //Defaults used by device to get buttons
+
+    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;
+
+    //Mappings used by host to set buttons
+
+    uint8_t MAP_DPAD_UP         = DPAD_UP        ;
+    uint8_t MAP_DPAD_DOWN       = DPAD_DOWN      ;
+    uint8_t MAP_DPAD_LEFT       = DPAD_LEFT      ;
+    uint8_t MAP_DPAD_RIGHT      = DPAD_RIGHT     ;
+    uint8_t MAP_DPAD_UP_LEFT    = DPAD_UP_LEFT   ;
+    uint8_t MAP_DPAD_UP_RIGHT   = DPAD_UP_RIGHT  ;
+    uint8_t MAP_DPAD_DOWN_LEFT  = DPAD_DOWN_LEFT ;
+    uint8_t MAP_DPAD_DOWN_RIGHT = DPAD_DOWN_RIGHT;
+    uint8_t MAP_DPAD_NONE       = DPAD_NONE      ;
+
+    uint16_t MAP_BUTTON_A     = BUTTON_A    ;
+    uint16_t MAP_BUTTON_B     = BUTTON_B    ;
+    uint16_t MAP_BUTTON_X     = BUTTON_X    ;
+    uint16_t MAP_BUTTON_Y     = BUTTON_Y    ;
+    uint16_t MAP_BUTTON_L3    = BUTTON_L3   ;
+    uint16_t MAP_BUTTON_R3    = BUTTON_R3   ;
+    uint16_t MAP_BUTTON_BACK  = BUTTON_BACK ;
+    uint16_t MAP_BUTTON_START = BUTTON_START;
+    uint16_t MAP_BUTTON_LB    = BUTTON_LB   ;
+    uint16_t MAP_BUTTON_RB    = BUTTON_RB   ;
+    uint16_t MAP_BUTTON_SYS   = BUTTON_SYS  ;
+    uint16_t MAP_BUTTON_MISC  = BUTTON_MISC ;
+
+    uint8_t MAP_ANALOG_OFF_UP    = ANALOG_OFF_UP   ;
+    uint8_t MAP_ANALOG_OFF_DOWN  = ANALOG_OFF_DOWN ;
+    uint8_t MAP_ANALOG_OFF_LEFT  = ANALOG_OFF_LEFT ;
+    uint8_t MAP_ANALOG_OFF_RIGHT = ANALOG_OFF_RIGHT;
+    uint8_t MAP_ANALOG_OFF_A     = ANALOG_OFF_A    ;
+    uint8_t MAP_ANALOG_OFF_B     = ANALOG_OFF_B    ;
+    uint8_t MAP_ANALOG_OFF_X     = ANALOG_OFF_X    ;
+    uint8_t MAP_ANALOG_OFF_Y     = ANALOG_OFF_Y    ;
+    uint8_t MAP_ANALOG_OFF_LB    = ANALOG_OFF_LB   ;
+    uint8_t MAP_ANALOG_OFF_RB    = ANALOG_OFF_RB   ;
+
+#pragma pack(push, 1)
+    struct PadIn
+    {
+        uint8_t  dpad;
+        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[10];
+    
+        PadIn()
+        {
+            std::memset(this, 0, sizeof(PadIn));
+        }
+    };
+
+    struct PadOut
+    {
+        uint8_t rumble_l;
+        uint8_t rumble_r;
+
+        PadOut()
+        {
+            std::memset(this, 0, sizeof(PadOut));
+        }
+    };
+
+    using ChatpadIn = std::array<uint8_t, 3>;
+
+#pragma pack(pop)
+
+    Gamepad()
+    {
+        mutex_init(&pad_in_mutex_);
+        mutex_init(&pad_out_mutex_);
+        mutex_init(&chatpad_in_mutex_);
+        reset_pad_in();
+        reset_pad_out();
+        reset_chatpad_in();
+    };
+
+    ~Gamepad() = default;
+
+    //Get
+    inline bool new_pad_in() const { return new_pad_in_.load(); }
+    inline bool new_pad_out() const { return new_pad_out_.load(); }
+
+    //True if both host and device have enabled analog
+    inline bool analog_enabled() const { return analog_enabled_.load(std::memory_order_relaxed); }
+
+    inline PadIn get_pad_in()
+    {
+        mutex_enter_blocking(&pad_in_mutex_);
+        PadIn pad_in = pad_in_;
+        new_pad_in_.store(false);
+        mutex_exit(&pad_in_mutex_);
+
+        return pad_in;
+    }
+
+    inline PadOut get_pad_out()
+    {
+        mutex_enter_blocking(&pad_out_mutex_);
+        PadOut pad_out = pad_out_;
+        new_pad_out_.store(false);
+        mutex_exit(&pad_out_mutex_);
+
+        return pad_out;
+    }
+
+    inline ChatpadIn get_chatpad_in()
+    {
+        mutex_enter_blocking(&chatpad_in_mutex_);
+        ChatpadIn chatpad_in = chatpad_in_;
+        mutex_exit(&chatpad_in_mutex_);
+
+        return chatpad_in;
+    }
+
+    //Set
+
+    void set_analog_device(bool value) 
+    { 
+        analog_device_.store(value); 
+        if (analog_host_.load() && analog_device_.load() && profile_analog_enabled_)
+        {
+            analog_enabled_.store(true);
+        }
+    }
+
+    void set_analog_host(bool value) 
+    { 
+        analog_host_.store(value); 
+        if (analog_host_.load() && analog_device_.load() && profile_analog_enabled_)
+        {
+            analog_enabled_.store(true);
+        }
+    }
+
+    void set_profile(const UserProfile& user_profile) 
+    { 
+        set_profile_mappings(user_profile);
+        set_profile_settings(user_profile);
+    }
+
+    inline void set_pad_in(PadIn pad_in)
+    {
+        mutex_enter_blocking(&pad_in_mutex_);
+        pad_in_ = pad_in;
+        new_pad_in_.store(true);
+        mutex_exit(&pad_in_mutex_);
+    }
+
+    inline void set_pad_out(const PadOut& pad_out)
+    {
+        mutex_enter_blocking(&pad_out_mutex_);
+        pad_out_ = pad_out;
+        new_pad_out_.store(true);
+        mutex_exit(&pad_out_mutex_);
+    }
+
+    inline void set_chatpad_in(const ChatpadIn& chatpad_in)
+    {
+        mutex_enter_blocking(&chatpad_in_mutex_);
+        chatpad_in_ = chatpad_in;
+        mutex_exit(&chatpad_in_mutex_);
+    }
+
+    inline void reset_pad_in() 
+	{ 
+        mutex_enter_blocking(&pad_in_mutex_);
+        pad_in_ = PadIn();
+        mutex_exit(&pad_in_mutex_);
+        new_pad_in_.store(true);
+    }
+    
+    inline void reset_pad_out()
+    {
+        mutex_enter_blocking(&pad_out_mutex_);
+        pad_out_ = PadOut();
+        new_pad_out_.store(true);
+        mutex_exit(&pad_out_mutex_);
+    }
+
+    inline void reset_chatpad_in()
+    {
+        mutex_enter_blocking(&chatpad_in_mutex_);
+        chatpad_in_.fill(0);
+        mutex_exit(&chatpad_in_mutex_);
+    }
+
+    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:    
+    mutex_t pad_in_mutex_;
+    mutex_t pad_out_mutex_;
+    mutex_t chatpad_in_mutex_;
+
+    PadOut pad_out_;
+    PadIn pad_in_;
+    ChatpadIn chatpad_in_{0};
+
+    std::atomic<bool> new_pad_in_{false};
+    std::atomic<bool> new_pad_out_{false};
+
+    std::atomic<bool> analog_enabled_{false};
+    std::atomic<bool> analog_host_{false};
+    std::atomic<bool> analog_device_{false};
+
+    bool profile_analog_enabled_{false};
+
+    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)
+    {
+        profile_analog_enabled_ = profile.analog_enabled ? true : false;
+        OGXM_LOG("profile_analog_enabled_: %d\n", profile_analog_enabled_);
+
+        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)
+    {
+        MAP_DPAD_UP         = profile.dpad_up;
+        MAP_DPAD_DOWN       = profile.dpad_down;
+        MAP_DPAD_LEFT       = profile.dpad_left;
+        MAP_DPAD_RIGHT      = profile.dpad_right;
+        MAP_DPAD_UP_LEFT    = profile.dpad_up | profile.dpad_left;
+        MAP_DPAD_UP_RIGHT   = profile.dpad_up | profile.dpad_right;
+        MAP_DPAD_DOWN_LEFT  = profile.dpad_down | profile.dpad_left;
+        MAP_DPAD_DOWN_RIGHT = profile.dpad_down | profile.dpad_right;
+        MAP_DPAD_NONE       = 0;
+
+        MAP_BUTTON_A     = profile.button_a;
+        MAP_BUTTON_B     = profile.button_b;
+        MAP_BUTTON_X     = profile.button_x;
+        MAP_BUTTON_Y     = profile.button_y;
+        MAP_BUTTON_L3    = profile.button_l3;
+        MAP_BUTTON_R3    = profile.button_r3;
+        MAP_BUTTON_BACK  = profile.button_back;
+        MAP_BUTTON_START = profile.button_start;
+        MAP_BUTTON_LB    = profile.button_lb;
+        MAP_BUTTON_RB    = profile.button_rb;
+        MAP_BUTTON_SYS   = profile.button_sys;
+        MAP_BUTTON_MISC  = profile.button_misc;
+
+        MAP_ANALOG_OFF_UP    = profile.analog_off_up;
+        MAP_ANALOG_OFF_DOWN  = profile.analog_off_down;
+        MAP_ANALOG_OFF_LEFT  = profile.analog_off_left;
+        MAP_ANALOG_OFF_RIGHT = profile.analog_off_right;
+        MAP_ANALOG_OFF_A     = profile.analog_off_a;
+        MAP_ANALOG_OFF_B     = profile.analog_off_b;
+        MAP_ANALOG_OFF_X     = profile.analog_off_x;
+        MAP_ANALOG_OFF_Y     = profile.analog_off_y;
+        MAP_ANALOG_OFF_LB    = profile.analog_off_lb;
+        MAP_ANALOG_OFF_RB    = profile.analog_off_rb;
+    }
+
+    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)) };
+    }
+
+    uint8_t apply_trigger_settings(uint8_t value, const TriggerSettings& set) const
+    {
+        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>)));
+    }
+};
+
+#endif // _GAMEPAD_H_

Firmware/RP2040/src/Gamepad/Range.h

@@ -0,0 +1,204 @@
+#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_

Firmware/RP2040/src/Gamepad/fix16ext.h

@@ -0,0 +1,106 @@
+#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

Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h

@@ -1,77 +0,0 @@
-#ifndef I2C_DRIVER_4CH_H
-#define I2C_DRIVER_4CH_H
-
-#include <cstdint>
-#include <algorithm>
-
-#include "board_config.h"
-#include "Gamepad.h"
-
-#include "USBHost/HostDriver/HostDriver.h"
-
-class I2CDriver
-{
-public:
-    virtual ~I2CDriver() {};
-    virtual void initialize(uint8_t address) = 0;
-    virtual void process(Gamepad (&gamepads)[MAX_GAMEPADS]) = 0;
-    virtual void notify_tuh_mounted(HostDriver::Type host_type = HostDriver::Type::UNKNOWN) = 0;
-    virtual void notify_tuh_unmounted(HostDriver::Type host_type = HostDriver::Type::UNKNOWN) = 0;
-    virtual void notify_xbox360w_connected(uint8_t idx) {};
-    virtual void notify_xbox360w_disconnected(uint8_t idx) {};
-
-protected:
-    enum class PacketID : uint8_t { UNKNOWN = 0, PAD, STATUS, ENABLE, DISABLE, REBOOT };
-    enum class SlaveStatus : uint8_t { NC = 0, NOT_READY, READY, RESP_OK };
-
-#pragma pack(push, 1)
-    struct PacketIn
-    {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        Gamepad::PadIn pad_in;
-
-        PacketIn()
-        {
-            std::memset(this, 0, sizeof(PacketIn));
-            packet_len = sizeof(PacketIn);
-            packet_id = static_cast<uint8_t>(PacketID::PAD);
-        }
-    };
-    static_assert(sizeof(PacketIn) == 28, "I2CDriver::PacketIn is misaligned");
-
-    struct PacketOut
-    {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        Gamepad::PadOut pad_out;    
-
-        PacketOut()
-        {
-            std::memset(this, 0, sizeof(PacketOut));
-            packet_len = sizeof(PacketOut);
-            packet_id = static_cast<uint8_t>(PacketID::PAD);
-        }
-    };
-    static_assert(sizeof(PacketOut) == 4, "I2CDriver::PacketOut is misaligned");
-
-    struct PacketStatus
-    {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        uint8_t status;
-
-        PacketStatus()
-        {
-            packet_len = sizeof(PacketStatus);
-            packet_id = static_cast<uint8_t>(PacketID::STATUS);
-            status = static_cast<uint8_t>(SlaveStatus::NC);
-        }
-    };
-    static_assert(sizeof(PacketStatus) == 3, "I2CDriver::PacketStatus is misaligned");
-#pragma pack(pop)
-
-    static constexpr size_t MAX_PACKET_SIZE = std::max(sizeof(PacketStatus), std::max(sizeof(PacketIn), sizeof(PacketOut)));
-};
-
-#endif // I2C_DRIVER_4CH_H

Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h

@@ -1,91 +0,0 @@
-#ifndef I2C_4CH_MANAGER_H
-#define I2C_4CH_MANAGER_H
-
-#include <cstdint>
-#include <memory>
-#include <atomic>   
-
-#include "board_config.h"
-#include "Gamepad.h"
-#include "I2CDriver/4Channel/I2CMaster.h"
-#include "I2CDriver/4Channel/I2CSlave.h"
-#include "I2CDriver/4Channel/I2CDriver.h"
-
-class I2CManager
-{
-public:
-    I2CManager(const I2CManager&) = delete;
-    I2CManager& operator=(const I2CManager&) = delete;
-
-    static I2CManager& get_instance()
-    {
-        static I2CManager instance;
-        return instance;
-    }
-
-    bool initialize_driver()
-    {
-        uint8_t i2c_address = get_i2c_address();
-        if (i2c_address == 0xFF)
-        {
-            return false;
-        }
-        if (i2c_address < 1)
-        {
-            is_master_.store(true);
-            driver_ = std::make_unique<I2CMaster>();
-        }
-        else
-        {
-            driver_ = std::make_unique<I2CSlave>();
-        }
-        driver_->initialize(i2c_address);
-        return true;
-    }
-
-    I2CDriver* get_driver() 
-    { 
-        return driver_.get(); 
-    }
-
-    bool is_master()
-    {
-        return is_master_.load();
-    }
-
-private:
-    I2CManager() {};
-    ~I2CManager() {};
-
-    std::unique_ptr<I2CDriver> driver_{nullptr};
-    std::atomic<bool> is_master_{false};
-
-    uint8_t get_i2c_address()
-    {
-        gpio_init(SLAVE_ADDR_PIN_1);
-        gpio_init(SLAVE_ADDR_PIN_2);
-        gpio_pull_up(SLAVE_ADDR_PIN_1);
-        gpio_pull_up(SLAVE_ADDR_PIN_2);
-
-        if (gpio_get(SLAVE_ADDR_PIN_1) == 1 && gpio_get(SLAVE_ADDR_PIN_2) == 1)
-        {
-            return 0x00;
-        }
-        else if (gpio_get(SLAVE_ADDR_PIN_1) == 1 && gpio_get(SLAVE_ADDR_PIN_2) == 0)
-        {
-            return 0x01;
-        }
-        else if (gpio_get(SLAVE_ADDR_PIN_1) == 0 && gpio_get(SLAVE_ADDR_PIN_2) == 1)
-        {
-            return 0x02;
-        }
-        else if (gpio_get(SLAVE_ADDR_PIN_1) == 0 && gpio_get(SLAVE_ADDR_PIN_2) == 0)
-        {
-            return 0x03;
-        }
-
-        return 0xFF;
-    }
-};
-
-#endif // I2C_4CH_MANAGER_H

Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp

@@ -1,210 +0,0 @@
-#include <cstring>
-
-#include "TaskQueue/TaskQueue.h"
-#include "OGXMini/OGXMini.h"    
-#include "I2CDriver/4Channel/I2CMaster.h"
-
-I2CMaster::~I2CMaster()
-{
-    TaskQueue::Core0::cancel_delayed_task(tid_update_slave_);
-}
-
-void I2CMaster::initialize(uint8_t address) 
-{
-    i2c_init(I2C_PORT, I2C_BAUDRATE);
-
-    gpio_init(I2C_SDA_PIN);
-    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(I2C_SDA_PIN);
-
-    gpio_init(I2C_SCL_PIN);
-    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(I2C_SCL_PIN);
-
-    for (uint8_t i = 0; i < NUM_SLAVES; ++i)
-    {
-        slaves_[i].address = address + i + 1;
-    }
-
-    tid_update_slave_ = TaskQueue::Core0::get_new_task_id();
-
-    TaskQueue::Core0::queue_delayed_task(tid_update_slave_, 2000, true, [this]
-    {
-        for (auto& slave : slaves_)
-        {
-            check_slave_status(slave);
-            sleep_us(10);
-        }
-    });
-}
-
-void I2CMaster::process(Gamepad (&gamepads)[MAX_GAMEPADS])
-{
-    for (uint8_t i = 0; i < NUM_SLAVES; ++i)
-    {
-        Slave& slave = slaves_[i];
-
-        if (slave.status != SlaveStatus::READY || !slave_detected(slave.address))
-        {
-            continue;
-        }
-        if (send_packet_in(slave, gamepads[i + 1]))
-        {
-            get_packet_out(slave, gamepads[i + 1]);
-        }
-
-        sleep_us(100);
-    }
-}
-
-void I2CMaster::notify_tuh_mounted(HostDriver::Type host_type)
-{
-    if (host_type == HostDriver::Type::XBOX360W)
-    {
-        i2c_enabled_.store(true);
-    }
-}
-
-void I2CMaster::notify_tuh_unmounted(HostDriver::Type host_type)
-{
-    if (host_type == HostDriver::Type::XBOX360W)
-    {
-        i2c_enabled_.store(false);
-
-        TaskQueue::Core0::queue_task([this]()
-        {
-            for (auto& slave : slaves_)
-            {
-                notify_disable(slave);
-            }
-        });
-    }
-}
-
-void I2CMaster::notify_xbox360w_connected(uint8_t idx)
-{
-    if (idx < 1 || idx >= MAX_GAMEPADS)
-    {
-        return;
-    }
-    slaves_[idx - 1].enabled.store(true);
-
-    TaskQueue::Core0::queue_task([this, idx]()
-    {
-        notify_enable(slaves_[idx - 1].address);
-    });
-}
-
-void I2CMaster::notify_xbox360w_disconnected(uint8_t idx)
-{
-    if (idx < 1 || idx >= MAX_GAMEPADS)
-    {
-        return;
-    }
-    slaves_[idx - 1].enabled.store(false);
-
-    TaskQueue::Core0::queue_task([this, idx]()
-    {
-        notify_disable(slaves_[idx - 1]);
-    });
-}
-
-bool I2CMaster::slave_detected(uint8_t address)
-{
-    uint8_t dummy_data = 0;
-    int result = i2c_write_timeout_us(I2C_PORT, address, &dummy_data, 0, false, 1000);
-    return (result >= 0);
-}
-
-void I2CMaster::check_slave_status(Slave& slave)
-{
-    if (!slave_detected(slave.address))
-    {
-        slave.status = SlaveStatus::NC;
-        return;
-    }
-    if (slave.enabled.load())
-    {
-        slave.status = get_slave_status(slave.address);
-    }
-    slave.status = SlaveStatus::NOT_READY;
-}
-
-I2CMaster::SlaveStatus I2CMaster::get_slave_status(uint8_t address)
-{
-    PacketStatus status_packet;
-    status_packet.packet_id = static_cast<uint8_t>(PacketID::STATUS);
-
-    int count = i2c_write_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-    if (count == sizeof(PacketStatus))
-    {
-        count = i2c_read_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-        return static_cast<SlaveStatus>(status_packet.status);
-    }
-    return SlaveStatus::NC;
-}
-
-I2CMaster::SlaveStatus I2CMaster::notify_enable(uint8_t address)
-{
-    PacketStatus status_packet;
-    status_packet.packet_id = static_cast<uint8_t>(PacketID::ENABLE);
-
-    int count = i2c_write_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-    if (count == sizeof(PacketStatus))
-    {
-        count = i2c_read_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-        return static_cast<SlaveStatus>(status_packet.status);
-    }
-    return SlaveStatus::NC;
-}
-
-bool I2CMaster::notify_disable(Slave& slave)
-{
-    if (slave_detected(slave.address) && slave.enabled.load())
-    {
-        int retries = 6;
-        bool success = false;
-
-        while (!success && retries--)
-        {
-            PacketStatus status_packet;
-            status_packet.packet_id = static_cast<uint8_t>(PacketID::DISABLE);
-
-            int count = i2c_write_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-            if (count == sizeof(PacketStatus))
-            {
-                count = i2c_read_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-                success = (static_cast<SlaveStatus>(status_packet.status) == SlaveStatus::RESP_OK);
-            }
-
-            sleep_ms(1);
-        }
-        return success;
-    }
-    return false;
-}
-
-bool I2CMaster::send_packet_in(Slave& slave, Gamepad& gamepad)
-{
-    static PacketIn packet_in = PacketIn();
-
-    Gamepad::PadIn pad_in = gamepad.get_pad_in();
-    packet_in.pad_in = pad_in;
-
-    int count = i2c_write_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&packet_in), sizeof(packet_in), false);
-    return (count == sizeof(PacketIn));
-}
-
-bool I2CMaster::get_packet_out(Slave& slave, Gamepad& gamepad)
-{
-    static PacketOut packet_out = PacketOut();
-
-    int count = i2c_read_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&packet_out), sizeof(PacketOut), false);
-    if (count != sizeof(PacketOut))
-    {
-        return false;
-    }
-
-    gamepad.set_pad_out(packet_out.pad_out);
-    return true;
-}

Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h

@@ -1,56 +0,0 @@
-#ifndef I2C_MASTER_4CH_H
-#define I2C_MASTER_4CH_H
-
-#include <cstdint>
-#include <atomic>
-#include <array>
-
-#include <hardware/gpio.h>
-#include <hardware/i2c.h>
-
-#include "board_config.h"
-#include "Gamepad.h"
-#include "I2CDriver/4Channel/I2CDriver.h"
-
-class I2CMaster : public I2CDriver
-{
-public:
-    ~I2CMaster() override;
-    void initialize(uint8_t address) override;
-    void process(Gamepad (&gamepads)[MAX_GAMEPADS]) override;
-    void notify_tuh_mounted(HostDriver::Type host_type) override;
-    void notify_tuh_unmounted(HostDriver::Type host_type) override;
-    void notify_xbox360w_connected(uint8_t idx) override;
-    void notify_xbox360w_disconnected(uint8_t idx) override;
-
-private:
-    struct Slave
-    {
-        uint8_t address{0xFF};
-        SlaveStatus status{SlaveStatus::NC};
-        std::atomic<bool> enabled{false};
-    };
-
-    static constexpr size_t NUM_SLAVES = MAX_GAMEPADS - 1;
-    static_assert(NUM_SLAVES > 0, "I2CMaster::NUM_SLAVES must be greater than 0 to use I2C");
-
-    uint32_t tid_update_slave_;
-    bool update_slave_status_{false};
-
-    std::atomic<bool> i2c_enabled_{false};
-    // std::atomic<bool> notify_deinit_{false};
-    std::array<Slave, NUM_SLAVES> slaves_;  
-
-    static bool slave_detected(uint8_t address);
-
-    void check_slave_status(Slave& slave);
-    bool notify_disable(Slave& slave);
-    SlaveStatus notify_enable(uint8_t address);
-    // bool send_packet_status(uint8_t address, PacketID packet_id);
-    SlaveStatus get_slave_status(uint8_t address);
-
-    bool send_packet_in(Slave& slave, Gamepad& gamepad);
-    bool get_packet_out(Slave& slave, Gamepad& gamepad);
-};
-
-#endif // I2C_MASTER_4CH_H

Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp

@@ -1,168 +0,0 @@
-#include <cstring>
-#include <array>
-
-#include "USBHost/HostManager.h"
-#include "OGXMini/OGXMini.h"
-#include "I2CDriver/4Channel/I2CSlave.h"
-
-I2CSlave* I2CSlave::this_instance_ = nullptr;
-
-void I2CSlave::initialize(uint8_t address) 
-{
-    this_instance_ = this;
-
-    i2c_init(I2C_PORT, I2C_BAUDRATE);
-
-    gpio_init(I2C_SDA_PIN);
-    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(I2C_SDA_PIN);
-
-    gpio_init(I2C_SCL_PIN);
-    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(I2C_SCL_PIN);
-
-    i2c_slave_init(I2C_PORT, address, &slave_handler);
-}
-
-void I2CSlave::notify_tuh_mounted(HostDriver::Type host_type)
-{
-    i2c_disabled_.store(true);
-}
-
-void I2CSlave::notify_tuh_unmounted(HostDriver::Type host_type)
-{
-    i2c_disabled_.store(false);
-}
-
-void I2CSlave::process(Gamepad (&gamepads)[MAX_GAMEPADS])
-{
-    if (i2c_disabled_.load())
-    {
-        return;
-    }
-    //Don't want to hang up the i2c bus by doing this in the slave handler
-    if (packet_in_.packet_id == static_cast<uint8_t>(PacketID::PAD))
-    {
-        if (new_packet_in_)
-        {
-            new_packet_in_ = false;
-            gamepads[0].set_pad_in(packet_in_.pad_in);
-        }
-        
-        if (gamepads[0].new_pad_out())
-        {
-            packet_out_.pad_out = gamepads[0].get_pad_out();
-        } 
-    }
-}
-
-I2CSlave::PacketID I2CSlave::get_packet_id(uint8_t* buffer_in)
-{
-    switch (static_cast<PacketID>(buffer_in[1]))
-    {
-        case PacketID::PAD:
-            if (buffer_in[0] == sizeof(PacketIn))
-            {
-                return PacketID::PAD;
-            }
-            break;
-        case PacketID::DISABLE:
-            if (buffer_in[0] == sizeof(PacketStatus))
-            {
-                return PacketID::DISABLE;
-            }
-            break;
-        case PacketID::ENABLE:
-            if (buffer_in[0] == sizeof(PacketStatus))
-            {
-                return PacketID::ENABLE;
-            }
-            break;
-        case PacketID::STATUS:
-            if (buffer_in[0] == sizeof(PacketStatus))
-            {
-                return PacketID::STATUS;
-            }
-            break;
-        default:
-            break;
-    }
-    return PacketID::UNKNOWN;
-}
-
-void I2CSlave::slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
-{
-    static int count = 0;
-    static std::array<uint8_t, MAX_PACKET_SIZE> buffer_in{0};
-    static std::array<uint8_t, MAX_PACKET_SIZE> buffer_out{0};
-
-    switch (event) 
-    {
-        case I2C_SLAVE_RECEIVE: // master has written
-            if (count < MAX_PACKET_SIZE)
-            {
-                buffer_in.data()[count] = i2c_read_byte_raw(i2c);
-                ++count;
-            }
-            // else // Something's wrong, reset
-            // {
-            //     count = 0;
-            //     buffer_in.fill(0);
-            //     buffer_out.fill(0);
-            // }
-            break;
-
-        case I2C_SLAVE_FINISH: // master signalled Stop / Restart
-            // Each master write has an ID indicating the type of data to send back on the next read request
-            // Every write has an associated read
-            switch (get_packet_id(buffer_in.data())) 
-            {
-                case PacketID::PAD:
-                    this_instance_->packet_in_ = *reinterpret_cast<PacketIn*>(buffer_in.data());
-                    this_instance_->new_packet_in_ = true;
-                    *reinterpret_cast<PacketOut*>(buffer_out.data()) = this_instance_->packet_out_;
-                    break;
-
-                case PacketID::STATUS:
-                    buffer_out.data()[0] = sizeof(PacketStatus);
-                    buffer_out.data()[1] = static_cast<uint8_t>(PacketID::STATUS);
-                    // if something is mounted by tuh, signal that we're not talking to the master
-                    buffer_out.data()[2] = 
-                        this_instance_->i2c_disabled_.load() ? 
-                        static_cast<uint8_t>(SlaveStatus::NOT_READY) : 
-                        static_cast<uint8_t>(SlaveStatus::READY);
-                    break;
-
-                case PacketID::ENABLE:
-                    buffer_out.data()[0] = sizeof(PacketStatus);
-                    buffer_out.data()[1] = static_cast<uint8_t>(PacketID::STATUS);
-                    buffer_out.data()[2] = static_cast<uint8_t>(SlaveStatus::RESP_OK);
-
-                    OGXMini::update_tud_status(true);
-                    break;
-
-                case PacketID::DISABLE:
-                    buffer_out.data()[0] = sizeof(PacketStatus);
-                    buffer_out.data()[1] = static_cast<uint8_t>(PacketID::STATUS);
-                    buffer_out.data()[2] = static_cast<uint8_t>(SlaveStatus::RESP_OK);
-                    if (!this_instance_->i2c_disabled_.load())
-                    {
-                        OGXMini::update_tud_status(false);
-                    }
-                    break;
-
-                default:
-                    break;
-            }
-            count = 0;
-            break;
-
-        case I2C_SLAVE_REQUEST: // master requesting data
-            i2c_write_raw_blocking(i2c, buffer_out.data(), buffer_out.data()[0]);
-            buffer_in.fill(0);
-            break;
-
-        default:
-            break;
-    }
-}

Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h

@@ -1,36 +0,0 @@
-#ifndef I2C_SLAVE_4CH_H
-#define I2C_SLAVE_4CH_H
-
-#include <cstdint>
-#include <atomic>
-#include <cstring>
-#include <hardware/gpio.h>
-#include <hardware/i2c.h>
-#include <pico/i2c_slave.h>
-
-#include "board_config.h"
-#include "Gamepad.h"
-#include "I2CDriver/4Channel/I2CDriver.h"
-
-class I2CSlave : public I2CDriver
-{
-public:
-    ~I2CSlave() = default;
-    void initialize(uint8_t address) override;
-    void process(Gamepad (&gamepads)[MAX_GAMEPADS]) override;
-    void notify_tuh_mounted(HostDriver::Type host_type) override;
-    void notify_tuh_unmounted(HostDriver::Type host_type) override;
-
-private:
-    static I2CSlave* this_instance_;
-
-    PacketIn packet_in_;
-    PacketOut packet_out_;
-    bool new_packet_in_{false};
-    std::atomic<bool> i2c_disabled_{false};
-
-    static PacketID get_packet_id(uint8_t* buffer_in);
-    static void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event);
-};
-
-#endif // I2C_SLAVE_4CH_H

Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp

@@ -1,187 +0,0 @@
-#include <array>
-#include <cstring>
-#include <pico/i2c_slave.h>
-#include <hardware/gpio.h>
-#include <hardware/i2c.h>
-
-#include "Gamepad.h"
-#include "board_config.h"
-#include "Board/board_api.h"
-#include "I2CDriver/ESP32/I2CDriver.h"
-#include "TaskQueue/TaskQueue.h"
-
-namespace I2CDriver {
-
-//May expand commands in the future
-enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
-
-#pragma pack(push, 1)
-struct PacketIn
-{
-    uint8_t packet_len;
-    uint8_t packet_id;
-    uint8_t index;
-    uint8_t gp_data[sizeof(Gamepad::PadIn) - sizeof(Gamepad::PadIn::analog)];
-
-    PacketIn()
-    {
-        std::memset(this, 0, sizeof(PacketIn));
-        packet_len = sizeof(PacketIn);
-        packet_id = static_cast<uint8_t>(PacketID::SET_PAD);
-    }
-};
-static_assert(sizeof(PacketIn) == 16, "i2c_driver_esp::PacketIn size mismatch");
-
-struct PacketOut
-{
-    uint8_t packet_len;
-    uint8_t packet_id;
-    uint8_t index;
-    Gamepad::PadOut pad_out;
-    uint8_t reserved[3];
-
-    PacketOut()
-    {
-        std::memset(this, 0, sizeof(PacketOut));
-        packet_len = sizeof(PacketOut);
-        packet_id = static_cast<uint8_t>(PacketID::GET_PAD);
-    }
-};
-static_assert(sizeof(PacketOut) == 8, "i2c_driver_esp::PacketOut size mismatch");
-#pragma pack(pop)
-
-static constexpr size_t MAX_BUFFER_SIZE = std::max(sizeof(PacketOut), sizeof(PacketIn));
-static constexpr uint8_t I2C_ADDR = 0x01;
-
-Gamepad* gamepads_[MAX_GAMEPADS];
-
-inline void process_in_packet(PacketIn* packet_in)
-{
-    Gamepad::PadIn gp_in;
-    std::memcpy(&gp_in, packet_in->gp_data, sizeof(packet_in->gp_data));
-
-    //This is a bandaid since the ESP32 doesn't have access to user profiles atm
-    //Will update this once I write a BLE server for interfacing with the webapp
-
-    Gamepad* gamepad = gamepads_[packet_in->index];
-    Gamepad::PadIn mapped_gp_in;
-
-    if (gp_in.dpad & Gamepad::DPAD_UP)    mapped_gp_in.dpad |= gamepad->MAP_DPAD_UP;
-    if (gp_in.dpad & Gamepad::DPAD_DOWN)  mapped_gp_in.dpad |= gamepad->MAP_DPAD_DOWN;
-    if (gp_in.dpad & Gamepad::DPAD_LEFT)  mapped_gp_in.dpad |= gamepad->MAP_DPAD_LEFT;
-    if (gp_in.dpad & Gamepad::DPAD_RIGHT) mapped_gp_in.dpad |= gamepad->MAP_DPAD_RIGHT;
-
-    if (gp_in.buttons & Gamepad::BUTTON_START)  mapped_gp_in.buttons |= gamepad->MAP_BUTTON_START;
-    if (gp_in.buttons & Gamepad::BUTTON_BACK)   mapped_gp_in.buttons |= gamepad->MAP_BUTTON_BACK;
-    if (gp_in.buttons & Gamepad::BUTTON_L3)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_L3;
-    if (gp_in.buttons & Gamepad::BUTTON_R3)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_R3;
-    if (gp_in.buttons & Gamepad::BUTTON_LB)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_LB;
-    if (gp_in.buttons & Gamepad::BUTTON_RB)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_RB;
-    if (gp_in.buttons & Gamepad::BUTTON_SYS)    mapped_gp_in.buttons |= gamepad->MAP_BUTTON_SYS;
-    if (gp_in.buttons & Gamepad::BUTTON_A)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_A;
-    if (gp_in.buttons & Gamepad::BUTTON_B)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_B;
-    if (gp_in.buttons & Gamepad::BUTTON_X)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_X;
-    if (gp_in.buttons & Gamepad::BUTTON_Y)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_Y;
-
-    mapped_gp_in.trigger_l = gp_in.trigger_l;
-    mapped_gp_in.trigger_r = gp_in.trigger_r;
-    mapped_gp_in.joystick_lx = gp_in.joystick_lx;
-    mapped_gp_in.joystick_ly = gp_in.joystick_ly;
-    mapped_gp_in.joystick_rx = gp_in.joystick_rx;
-    mapped_gp_in.joystick_ry = gp_in.joystick_ry;
-
-    gamepad->set_pad_in(mapped_gp_in);
-}
-
-inline void fill_out_report(uint8_t index, PacketOut* report_out)
-{
-    if (index >= MAX_GAMEPADS)
-    {
-        return;
-    }
-
-    report_out->packet_len = sizeof(PacketOut);
-    report_out->packet_id = static_cast<uint8_t>(PacketID::GET_PAD);
-    report_out->index = index;
-    report_out->pad_out = gamepads_[index]->get_pad_out();
-}
-
-PacketID get_packet_id(uint8_t* buffer_in)
-{
-    switch (static_cast<PacketID>(buffer_in[1]))
-    {
-        case PacketID::SET_PAD:
-            if (buffer_in[0] == sizeof(PacketIn))
-            {
-                return PacketID::SET_PAD;
-            }
-            break;
-        //Unused ATM
-        // case PacketID::GET_PAD:
-        //     if (buffer_in[0] == sizeof(PacketOut))
-        //     {
-        //         return PacketID::GET_PAD;
-        //     }
-        //     break;
-        default:
-            break;
-    }
-    return PacketID::UNKNOWN;
-}
-
-static inline void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
-{
-    static int count = 0;
-    static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_in{0};
-    static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_out{0};
-
-    switch (event) 
-    {
-        case I2C_SLAVE_RECEIVE: // master has written
-            if (count < sizeof(PacketIn))
-            {
-                buffer_in.data()[count] = i2c_read_byte_raw(i2c);
-                ++count;
-            }
-            break;
-
-        case I2C_SLAVE_FINISH: // master signalled Stop / Restart
-            if (get_packet_id(buffer_in.data()) == PacketID::SET_PAD)
-            {
-                process_in_packet(reinterpret_cast<PacketIn*>(buffer_in.data()));
-            }
-            count = 0;
-            break;
-
-        case I2C_SLAVE_REQUEST: // master requesting data
-            fill_out_report(reinterpret_cast<PacketIn*>(buffer_in.data())->index, reinterpret_cast<PacketOut*>(buffer_out.data()));
-            i2c_write_raw_blocking(i2c, buffer_out.data(), buffer_out.data()[0]);
-            buffer_in.fill(0);
-            break;
-
-        default:
-            break;
-    }
-}
-
-void initialize(Gamepad (&gamepads)[MAX_GAMEPADS])
-{
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        gamepads_[i] = &gamepads[i];
-    }
-
-    i2c_init(I2C_PORT, I2C_BAUDRATE);
-
-    gpio_init(I2C_SDA_PIN);
-    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(I2C_SDA_PIN);
-
-    gpio_init(I2C_SCL_PIN);
-    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(I2C_SCL_PIN);
-
-    i2c_slave_init(I2C_PORT, I2C_ADDR, &slave_handler);
-}
-
-} // namespace i2c_driver_esp32

Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.h

@@ -1,13 +0,0 @@
-#ifndef _I2CDRIVER_ESP_H_
-#define _I2CDRIVER_ESP_H_
-
-#include <cstdint>
-
-#include "Gamepad.h"
-
-namespace I2CDriver
-{
-    void initialize(Gamepad (&gamepads)[MAX_GAMEPADS]);
-}
-
-#endif // _I2CDRIVER_ESP_H_

Firmware/RP2040/src/OGXMini/Board/ESP32_Bluepad32_I2C.cpp

@@ -0,0 +1,201 @@
+#include "Board/Config.h"
+#include "OGXMini/Board/ESP32_Bluepad32_I2C.h"
+#if (OGXM_BOARD == ESP32_BLUEPAD32_I2C)
+
+#include <cstring>
+#include <pico/multicore.h>
+#include <pico/i2c_slave.h>
+#include <hardware/gpio.h>
+#include <hardware/i2c.h>
+
+#include "tusb.h"
+#include "bsp/board_api.h"
+
+#include "USBDevice/DeviceManager.h"
+#include "UserSettings/UserSettings.h"
+#include "Board/board_api.h"
+#include "Board/esp32_api.h"
+#include "Gamepad/Gamepad.h"
+#include "TaskQueue/TaskQueue.h"
+
+enum class PacketID : uint8_t { 
+    UNKNOWN = 0, 
+    SET_PAD, 
+    GET_PAD, 
+    SET_DRIVER 
+};
+
+#pragma pack(push, 1)
+struct PacketIn {
+    uint8_t             packet_len{sizeof(PacketIn)};
+    PacketID            packet_id{PacketID::SET_PAD};
+    uint8_t             index{0};
+    DeviceDriverType    device_type{DeviceDriverType::NONE};
+    Gamepad::PadIn      pad_in{Gamepad::PadIn()};
+    uint8_t             reserved[5]{0};
+};
+static_assert(sizeof(PacketIn) == 32, "i2c_driver_esp::PacketIn size mismatch");
+
+struct PacketOut {
+    uint8_t         packet_len{sizeof(PacketOut)};
+    PacketID        packet_id{PacketID::GET_PAD};
+    uint8_t         index{0};
+    Gamepad::PadOut pad_out{Gamepad::PadOut()};
+    uint8_t         reserved[3]{0};
+};
+static_assert(sizeof(PacketOut) == 8, "i2c_driver_esp::PacketOut size mismatch");
+#pragma pack(pop)
+
+constexpr size_t  MAX_BUFFER_SIZE = std::max(sizeof(PacketOut), sizeof(PacketIn));
+constexpr uint8_t I2C_ADDR = 0x01;
+
+static Gamepad _gamepads[MAX_GAMEPADS];
+static bool _uart_bridge_mode = false;
+
+static inline void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
+    static size_t count = 0;
+    static PacketIn packet_in;
+    static PacketOut packet_out;
+    static DeviceDriverType current_device_type = 
+        UserSettings::get_instance().get_current_driver();
+
+    switch (event) {
+        case I2C_SLAVE_RECEIVE:
+            if (count < sizeof(PacketIn)) {
+                reinterpret_cast<uint8_t*>(&packet_in)[count] = i2c_read_byte_raw(i2c);
+                ++count;
+            }
+            break;
+        case I2C_SLAVE_FINISH:
+            if (count == 0) {
+                break;
+            }
+            switch (packet_in.packet_id) {
+                case PacketID::SET_PAD:
+                    if (packet_in.index < MAX_GAMEPADS) {
+                        _gamepads[packet_in.index].set_pad_in(packet_in.pad_in);
+                    }
+                    break;
+                case PacketID::SET_DRIVER:
+                    if (packet_in.device_type != DeviceDriverType::NONE &&
+                        packet_in.device_type != current_device_type) {
+                        OGXM_LOG("I2C: Driver change detected.\n");
+                        //Any writes to flash should be done on Core0
+                        TaskQueue::Core0::queue_delayed_task(
+                            TaskQueue::Core0::get_new_task_id(), 1000, false, 
+                            [new_device_type = packet_in.device_type] { 
+                                UserSettings::get_instance().store_driver_type(new_device_type);
+                            }
+                        );
+                    }
+                    break;
+                default:
+                    break;
+            }
+            count = 0;
+            break;
+        case I2C_SLAVE_REQUEST:
+            if (packet_in.index < MAX_GAMEPADS) {
+                packet_out.index = packet_in.index;
+                packet_out.pad_out = _gamepads[packet_in.index].get_pad_out();
+            }
+            i2c_write_raw_blocking( i2c, 
+                                    reinterpret_cast<const uint8_t*>(&packet_out), 
+                                    packet_out.packet_len);
+            break;
+        default:
+            break;
+    }
+}
+
+static void core1_task() {
+    i2c_init(I2C_PORT, I2C_BAUDRATE);
+
+    gpio_init(I2C_SDA_PIN);
+    gpio_init(I2C_SCL_PIN);
+    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SDA_PIN);
+    gpio_pull_up(I2C_SCL_PIN);
+
+    i2c_slave_init(I2C_PORT, I2C_ADDR, &slave_handler);
+
+    OGXM_LOG("I2C Driver initialized\n");
+
+    while (true) {
+        tight_loop_contents();
+    }
+}
+
+void run_uart_bridge() {
+    esp32_api::enter_programming_mode();
+
+    OGXM_LOG("Entering UART Bridge mode\n");
+
+    //Runs UART Bridge task, doesn't return unless programming is complete
+    DeviceManager::get_instance().get_driver()->process(0, _gamepads[0]); 
+
+    OGXM_LOG("Exiting UART Bridge mode\n");
+
+    board_api::usb::disconnect_all(); 
+    UserSettings::get_instance().write_datetime();
+    board_api::reboot();
+}
+
+bool update_needed(UserSettings& user_settings) {
+#if defined(OGXM_RETAIL)
+    return !user_settings.verify_datetime();
+#endif
+    return false;
+}
+
+void esp32_bp32_i2c::initialize() {
+    board_api::init_board();
+    esp32_api::init();
+
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
+    //MODE_SEL_PIN is used to determine if UART bridge should be run
+    _uart_bridge_mode = 
+        (esp32_api::uart_bridge_mode() || update_needed(user_settings));
+
+    DeviceDriverType driver_type = 
+        _uart_bridge_mode ? 
+            DeviceDriverType::UART_BRIDGE : user_settings.get_current_driver();
+
+    DeviceManager::get_instance().initialize_driver(driver_type, _gamepads);
+}
+
+void esp32_bp32_i2c::run() {
+    if (_uart_bridge_mode) {
+        run_uart_bridge();
+        return;
+    }
+
+    multicore_reset_core1();
+    multicore_launch_core1(core1_task);
+
+    esp32_api::reset();
+
+    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
+
+    tud_init(BOARD_TUD_RHPORT);
+
+    while (true) {
+        TaskQueue::Core0::process_tasks();
+
+        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i) {
+            device_driver->process(i, _gamepads[i]);
+            tud_task();
+        }
+        sleep_ms(1);
+    }
+}
+
+// #else // OGXM_BOARD == ESP32_BLUEPAD32_I2C
+
+// void esp32_bp32_i2c::initialize() {}
+// void esp32_bp32_i2c::run() {}
+
+#endif // OGXM_BOARD == ESP32_BLUEPAD32_I2C

Firmware/RP2040/src/OGXMini/Board/ESP32_Bluepad32_I2C.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <cstdint>
+
+namespace esp32_bp32_i2c {
+    void initialize();
+    void run();
+} // namespace esp32_bp32_i2c

Firmware/RP2040/src/OGXMini/Board/ESP32_Blueretro_I2C.cpp

@@ -0,0 +1,200 @@
+#include "Board/Config.h"
+#include "OGXMini/Board/ESP32_Blueretro_I2C.h"
+#if (OGXM_BOARD == ESP32_BLUERETRO_I2C)
+
+#include <pico/multicore.h>
+#include <hardware/gpio.h>
+#include <hardware/i2c.h>
+
+#include "tusb.h"
+#include "bsp/board_api.h"
+
+#include "UserSettings/UserSettings.h"
+#include "USBDevice/DeviceManager.h"
+#include "Board/board_api.h"
+#include "Board/esp32_api.h"
+#include "Gamepad/Gamepad.h"
+#include "TaskQueue/TaskQueue.h"
+
+#pragma pack(push, 1)
+struct PacketIn {
+    uint8_t          len{sizeof(PacketIn)};
+    uint8_t          index{0};
+    DeviceDriverType device_type{DeviceDriverType::NONE};
+    uint8_t          gp_data[13]{0};
+};
+static_assert(sizeof(PacketIn) == 16, "i2c_driver_esp::PacketIn size mismatch");
+
+struct PacketOut {
+    uint8_t len{sizeof(PacketOut)};
+    uint8_t rumble_l{0};
+    uint8_t rumble_r{0};
+    uint8_t reserved[5]{0};
+};
+static_assert(sizeof(PacketOut) == 8, "i2c_driver_esp::PacketOut size mismatch");
+#pragma pack(pop)
+
+constexpr uint8_t SLAVE_ADDR = 0x50;
+constexpr uint32_t FEEDBACK_DELAY_MS = 250;
+
+static Gamepad _gamepads[MAX_GAMEPADS];
+static bool _uart_bridge_mode = false;
+
+static void core1_task() {
+    i2c_init(I2C_PORT, I2C_BAUDRATE);
+
+    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SCL_PIN);
+    gpio_pull_up(I2C_SDA_PIN);
+
+    bool slave_ready = false;
+    PacketIn packet_in;
+    PacketOut packet_out;
+    Gamepad::PadIn pad_in;
+    Gamepad& gamepad = _gamepads[0];
+    uint32_t tid = TaskQueue::Core1::get_new_task_id();
+
+    sleep_ms(500); // Wait for ESP32 to start
+
+    TaskQueue::Core1::queue_delayed_task(tid, FEEDBACK_DELAY_MS, true, 
+    [&packet_out, &gamepad, &slave_ready] { 
+        if (!slave_ready) { // Check if slave present
+            uint8_t addr = SLAVE_ADDR;
+            int result = i2c_read_blocking(I2C_PORT, SLAVE_ADDR, &addr, 1, false);
+            slave_ready = (result == 1);
+
+        } else { // Update rumble
+            Gamepad::PadOut pad_out = gamepad.get_pad_out();
+            packet_out.rumble_l = pad_out.rumble_l;
+            packet_out.rumble_r = pad_out.rumble_r;
+            int result = i2c_write_blocking(I2C_PORT, SLAVE_ADDR, 
+                                            reinterpret_cast<const uint8_t*>(&packet_out), 
+                                            sizeof(PacketOut), false);
+
+            if (result != sizeof(PacketOut)) {
+                OGXM_LOG("I2C write failed\n");
+            } else {
+                OGXM_LOG("I2C sent rumble, L: %02X, R: %02X\n", 
+                    packet_out.rumble_l, packet_out.rumble_r);
+                sleep_ms(1);
+            }
+        }
+    });
+
+    OGXM_LOG("I2C Driver initialized\n");
+
+    //Wait for slave to be detected
+    while (!slave_ready) {
+        TaskQueue::Core1::process_tasks();
+        sleep_ms(100);
+    }
+
+    OGXM_LOG("I2C Slave ready\n");
+
+    while (true) {
+        TaskQueue::Core1::process_tasks();
+        int result = i2c_read_blocking( I2C_PORT, SLAVE_ADDR, 
+                                        reinterpret_cast<uint8_t*>(&packet_in), 
+                                        sizeof(PacketIn), false);
+
+        if (result == sizeof(PacketIn)) {
+            std::memcpy(reinterpret_cast<uint8_t*>(&pad_in), 
+                        packet_in.gp_data, 
+                        sizeof(packet_in.gp_data));
+            gamepad.set_pad_in(pad_in);
+
+        } else {
+            OGXM_LOG("I2C read failed\n");
+            return;
+        }
+        sleep_ms(1);
+    }
+}
+
+void set_gp_check_timer(uint32_t task_id) {
+    UserSettings& user_settings = UserSettings::get_instance();
+    
+    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, 
+    [&user_settings] {
+        //Check gamepad inputs for button combo to change usb device driver
+        if (user_settings.check_for_driver_change(_gamepads[0])) {
+            OGXM_LOG("Driver change detected, storing new driver.\n");
+            //This will store the new mode and reboot the pico
+            user_settings.store_driver_type(user_settings.get_current_driver());
+        }
+    });
+}
+
+void run_uart_bridge() {
+    esp32_api::enter_programming_mode();
+
+    OGXM_LOG("Entering UART Bridge mode\n");
+
+    //Runs UART Bridge task, doesn't return unless programming is complete
+    DeviceManager::get_instance().get_driver()->process(0, _gamepads[0]); 
+
+    OGXM_LOG("Exiting UART Bridge mode\n");
+
+    board_api::usb::disconnect_all(); 
+    UserSettings::get_instance().write_datetime();
+    board_api::reboot();
+}
+
+bool update_needed(UserSettings& user_settings) {
+#if defined(OGXM_RETAIL)
+    return !user_settings.verify_datetime();
+#endif
+    return false;
+}
+
+void esp32_br_i2c::initialize() {
+    board_api::init_board();
+    esp32_api::init();
+
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
+    //MODE_SEL_PIN is used to determine if UART bridge should be run
+    _uart_bridge_mode = 
+        (esp32_api::uart_bridge_mode() || update_needed(user_settings));
+
+    DeviceDriverType driver_type = 
+        _uart_bridge_mode ? 
+            DeviceDriverType::UART_BRIDGE : user_settings.get_current_driver();
+
+    DeviceManager::get_instance().initialize_driver(driver_type, _gamepads);
+}
+
+void esp32_br_i2c::run() {
+    if (_uart_bridge_mode) {
+        run_uart_bridge();
+        return;
+    }
+
+    esp32_api::reset();
+
+    multicore_reset_core1();
+    multicore_launch_core1(core1_task);
+
+    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
+    set_gp_check_timer(tid_gp_check);
+
+    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
+
+    tud_init(BOARD_TUD_RHPORT);
+
+    while (true) {
+        TaskQueue::Core0::process_tasks();
+        device_driver->process(0, _gamepads[0]);
+        tud_task();
+        sleep_ms(1);
+    }
+}
+
+// #else // OGXM_BOARD == ESP32_BLUERETRO_I2C
+
+// void esp32_br_i2c::initialize() {}
+// void esp32_br_i2c::run() {}
+
+#endif // OGXM_BOARD == ESP32_BLUERETRO_I2C

Firmware/RP2040/src/OGXMini/Board/ESP32_Blueretro_I2C.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <cstdint>
+
+namespace esp32_br_i2c {
+    void initialize();
+    void run();
+} // namespace esp32_br_i2c

Firmware/RP2040/src/OGXMini/Board/Four_Channel_I2C.cpp

@@ -0,0 +1,470 @@
+#include "Board/Config.h"
+#include "OGXMini/Board/Four_Channel_I2C.h"
+#if ((OGXM_BOARD == INTERNAL_4CH_I2C) || (OGXM_BOARD == EXTERNAL_4CH_I2C))
+
+#include <atomic>
+#include <cstring>
+#include <pico/multicore.h>
+#include <hardware/gpio.h>
+#include <hardware/i2c.h>
+#include <pico/i2c_slave.h>
+
+#include "tusb.h"
+#include "bsp/board_api.h"
+#include "pio_usb.h"
+
+#include "USBDevice/DeviceManager.h"
+#include "USBHost/HostManager.h"
+#include "Board/board_api.h"
+#include "Board/ogxm_log.h"
+#include "UserSettings/UserSettings.h"
+#include "Gamepad/Gamepad.h"
+#include "TaskQueue/TaskQueue.h"
+
+constexpr uint32_t FEEDBACK_DELAY_MS = 250;
+
+Gamepad _gamepads[MAX_GAMEPADS];
+
+namespace I2C {
+    enum class Role {
+        SLAVE = 0,
+        MASTER
+    };
+    enum class PacketID : uint8_t { 
+        UNKNOWN = 0, 
+        PAD, 
+        COMMAND 
+    };
+    enum class Command : uint8_t { 
+        UNKNOWN = 0, 
+        STATUS, 
+        DISABLE 
+    };
+    enum class Status : uint8_t { 
+        UNKNOWN = 0, 
+        NC, 
+        ERROR, 
+        OK, 
+        READY, 
+        NOT_READY 
+    };
+
+    #pragma pack(push, 1)
+    struct PacketIn {
+        uint8_t             packet_len{sizeof(PacketIn)};
+        PacketID            packet_id{PacketID::PAD};
+        Gamepad::PadIn      pad_in{Gamepad::PadIn()};
+        Gamepad::ChatpadIn  chatpad_in{0};
+        uint8_t             reserved[4]{0};
+    };
+    static_assert(sizeof(PacketIn) == 32, "I2CDriver::PacketIn is misaligned");
+
+    struct PacketOut {
+        uint8_t         packet_len{sizeof(PacketOut)};
+        PacketID        packet_id{PacketID::PAD};
+        Gamepad::PadOut pad_out{Gamepad::PadOut()};
+        uint8_t         reserved[4]{0};
+    };
+    static_assert(sizeof(PacketOut) == 8, "I2CDriver::PacketOut is misaligned");
+
+    struct PacketCMD {
+        uint8_t     packet_len{sizeof(PacketCMD)};
+        PacketID    packet_id{PacketID::COMMAND};
+        Command     command{Command::UNKNOWN};
+        Status      status{Status::UNKNOWN};
+        uint8_t     reserved[4]{0};
+    };
+    static_assert(sizeof(PacketCMD) == 8, "I2CDriver::PacketCMD is misaligned");
+    #pragma pack(pop)
+
+    constexpr size_t MAX_PACKET_SIZE = sizeof(PacketIn);
+
+    static Role _i2c_role = Role::SLAVE;
+
+    namespace Slave {
+        static inline PacketID get_packet_id(uint8_t* buffer_in) {
+            switch (static_cast<PacketID>(buffer_in[1])) {
+                case PacketID::PAD:
+                    if (buffer_in[0] == sizeof(PacketIn)) {
+                        return PacketID::PAD;
+                    }
+                    break;
+                case PacketID::COMMAND:
+                    if (buffer_in[0] == sizeof(PacketCMD)) {
+                        return PacketID::COMMAND;
+                    }
+                    break;
+                default:
+                    break;
+            }
+            return PacketID::UNKNOWN;
+        }
+
+        static void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
+            static size_t count = 0;
+            static bool enabled = false;
+            static uint8_t buffer_in[MAX_PACKET_SIZE];
+            static uint8_t buffer_out[MAX_PACKET_SIZE];
+
+            PacketIn  *packet_in_p = reinterpret_cast<PacketIn*>(buffer_in);
+            PacketOut *packet_out_p = reinterpret_cast<PacketOut*>(buffer_out);
+            PacketCMD *packet_cmd_in_p = reinterpret_cast<PacketCMD*>(buffer_in);
+            PacketCMD *packet_cmd_out_p = reinterpret_cast<PacketCMD*>(buffer_out);
+
+            switch (event) {
+                case I2C_SLAVE_RECEIVE: // master has written
+                    if (count < MAX_PACKET_SIZE) {
+                        buffer_in[count] = i2c_read_byte_raw(i2c);
+                        ++count;
+                    }
+                    break;
+
+                case I2C_SLAVE_FINISH:
+                    // Each master write has an ID indicating the type of data to send back on the next read request
+                    // Every write has an associated read
+                    switch (get_packet_id(buffer_in)) {
+                        case PacketID::PAD:
+                            // instance_->packet_in_ = *packet_in_p;
+                            // *packet_out_p = instance_->packet_out_;
+                            // instance_->new_pad_in_.store(true);
+                            _gamepads[0].set_pad_in(packet_in_p->pad_in);
+                            if (_gamepads[0].new_pad_out()) {
+                                packet_out_p->pad_out = _gamepads[0].get_pad_out();
+                            }
+                            break;
+
+                        case PacketID::COMMAND:
+                            switch (packet_cmd_in_p->command) {
+                                case Command::DISABLE:
+                                    packet_cmd_out_p->packet_len = sizeof(PacketCMD);
+                                    packet_cmd_out_p->packet_id = PacketID::COMMAND;
+                                    packet_cmd_out_p->command = Command::DISABLE;
+                                    packet_cmd_out_p->status = Status::OK;
+
+                                    if (!tuh_mounted(BOARD_TUH_RHPORT)) {
+                                        four_ch_i2c::host_mounted(false);
+                                    }
+                                    break;
+
+                                case Command::STATUS:
+                                    packet_cmd_out_p->packet_len = sizeof(PacketCMD);
+                                    packet_cmd_out_p->packet_id = PacketID::COMMAND;
+                                    packet_cmd_out_p->command = Command::STATUS;
+                                    packet_cmd_out_p->status = 
+                                        tuh_mounted(BOARD_TUH_RHPORT) ? Status::NOT_READY : Status::READY;
+
+                                    if (!tuh_mounted(BOARD_TUH_RHPORT) && !enabled) {
+                                        enabled = true;
+                                        four_ch_i2c::host_mounted(true);
+                                    }
+                                    break;
+
+                                default:
+                                    break;
+                            }
+                            break;
+                    }
+                    count = 0;
+                    std::memset(buffer_in, 0, sizeof(buffer_in));
+                    break;
+
+                case I2C_SLAVE_REQUEST:
+                    i2c_write_raw_blocking(i2c, buffer_out, buffer_out[0]);
+                    break;
+
+                default:
+                    break;
+            }
+        }
+    } // namespace Slave
+
+    namespace Master {
+        struct Slave {
+            uint8_t address{0xFF};
+            Status  status{Status::NC};
+            bool    enabled{false};
+        };
+
+        static constexpr size_t NUM_SLAVES = MAX_GAMEPADS - 1;
+        static_assert(NUM_SLAVES > 0, "I2CMaster::NUM_SLAVES must be greater than 0 to use I2C");
+
+        std::array<Slave, NUM_SLAVES> _slaves; 
+
+        static inline bool read_blocking(uint8_t address, void* buffer, size_t len) {
+            return (i2c_read_blocking(  I2C_PORT, address, reinterpret_cast<uint8_t*>(buffer), 
+                                        len, false) == static_cast<int>(len));
+        }
+
+        static inline bool write_blocking(uint8_t address, void* buffer, size_t len) {
+            return (i2c_write_blocking( I2C_PORT, address, reinterpret_cast<uint8_t*>(buffer), 
+                                        len, false) == static_cast<int>(len));
+        }
+
+        static inline bool slave_detected(uint8_t address) {
+            uint8_t dummy = 0;
+            int result = i2c_write_timeout_us(I2C_PORT, address, &dummy, 0, false, 1000);
+            return (result >= 0);
+        }
+
+        static void notify_disable(uint8_t address) {
+            if (!slave_detected(address)) {
+                return;
+            }
+            int retries = 10;
+
+            while (retries--) {
+                PacketCMD packet_cmd;
+                packet_cmd.packet_id = PacketID::COMMAND;
+                packet_cmd.command = Command::DISABLE;
+
+                if (write_blocking(address, &packet_cmd, sizeof(PacketCMD))) {
+                    if (read_blocking(address, &packet_cmd, sizeof(PacketCMD))) {
+                        if (packet_cmd.status == Status::OK) {
+                            break;
+                        }
+                    }
+                }
+                sleep_ms(1);
+            }
+        }
+
+        static void process() {
+            for (uint8_t i = 0; i < NUM_SLAVES; ++i) {
+                Slave& slave = _slaves[i];
+
+                if (!slave.enabled || !slave_detected(slave.address)) {
+                    continue;
+                }
+
+                PacketCMD packet_cmd;
+                packet_cmd.packet_id = PacketID::COMMAND;
+                packet_cmd.command = Command::STATUS;
+
+                if (!write_blocking(slave.address, &packet_cmd, sizeof(PacketCMD)) ||
+                    !read_blocking(slave.address, &packet_cmd, sizeof(PacketCMD))) {
+                    continue;
+                }
+
+                if (packet_cmd.status == Status::READY) {
+                    Gamepad& gamepad = _gamepads[i + 1];
+                    PacketIn packet_in;
+                    packet_in.pad_in = gamepad.get_pad_in();
+                    packet_in.chatpad_in = gamepad.get_chatpad_in();
+
+                    if (write_blocking(slave.address, &packet_in, sizeof(PacketIn))) {
+                        PacketOut packet_out;
+                        if (read_blocking(slave.address, &packet_out, sizeof(PacketOut))) {
+                            gamepad.set_pad_out(packet_out.pad_out);
+                        }
+                    }
+                }
+                sleep_ms(1);
+            }
+        }
+
+        static void xbox360w_connect(bool connected, uint8_t idx) {
+            if (idx < 1 || idx >= MAX_GAMEPADS) {
+                return;
+            }
+            // This function can be called from core1 
+            // so queue on core0 (i2c thread)
+            TaskQueue::Core0::queue_task(
+            [&slave = _slaves[idx - 1], connected]() {
+                slave.enabled = connected;
+                if (!connected) {
+                    notify_disable(slave.address);
+                }
+            });
+        }
+
+        static void tuh_connect(bool connected, HostDriverType host_type) {
+            if (host_type != HostDriverType::XBOX360W) {
+                return;
+            }
+            if (!connected) {
+                //Called from core1 so queue on core0
+                TaskQueue::Core0::queue_task(
+                []() {
+                    for (auto& slave : _slaves) {
+                        slave.enabled = false;
+                        notify_disable(slave.address);
+                    }
+                });
+            }
+        }
+    } // namespace Master
+
+    Role role() {
+        return _i2c_role;
+    }
+
+    uint8_t get_address() {
+        gpio_init(SLAVE_ADDR_PIN_1);
+        gpio_init(SLAVE_ADDR_PIN_2);
+        gpio_pull_up(SLAVE_ADDR_PIN_1);
+        gpio_pull_up(SLAVE_ADDR_PIN_2);
+
+        if (gpio_get(SLAVE_ADDR_PIN_1) && gpio_get(SLAVE_ADDR_PIN_2)) {
+            return 0x00;
+        }
+        else if (gpio_get(SLAVE_ADDR_PIN_1) && !gpio_get(SLAVE_ADDR_PIN_2)) {
+            return 0x01;
+        }
+        else if (!gpio_get(SLAVE_ADDR_PIN_1) && gpio_get(SLAVE_ADDR_PIN_2)) {
+            return 0x02;
+        }
+        else if (!gpio_get(SLAVE_ADDR_PIN_1) && !gpio_get(SLAVE_ADDR_PIN_2)) {
+            return 0x03;
+        }
+        return 0xFF;
+    }
+
+    void initialize() {
+        uint8_t i2c_address = get_address();
+        _i2c_role = (i2c_address == 0xFF) ? Role::MASTER : _i2c_role = Role::SLAVE;
+
+        i2c_init(I2C_PORT, I2C_BAUDRATE);
+
+        gpio_init(I2C_SDA_PIN);
+        gpio_init(I2C_SCL_PIN);
+        gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
+        gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
+        gpio_pull_up(I2C_SDA_PIN);
+        gpio_pull_up(I2C_SCL_PIN);
+
+        if (_i2c_role == Role::SLAVE) {
+            i2c_slave_init(I2C_PORT, i2c_address, &Slave::slave_handler);
+        }
+    }
+} // namespace I2C
+
+void core1_task() {
+    HostManager& host_manager = HostManager::get_instance();
+    host_manager.initialize(_gamepads);
+
+    //Pico-PIO-USB will not reliably detect a hot plug on some boards, 
+    //so monitor pins and init host stack after connection
+    while(!board_api::usb::host_connected()) {
+        sleep_ms(100);
+    }
+
+    pio_usb_configuration_t pio_cfg = PIO_USB_CONFIG;
+    tuh_configure(BOARD_TUH_RHPORT, TUH_CFGID_RPI_PIO_USB_CONFIGURATION, &pio_cfg);
+
+    tuh_init(BOARD_TUH_RHPORT);
+
+    uint32_t tid_feedback = TaskQueue::Core1::get_new_task_id();
+    TaskQueue::Core1::queue_delayed_task(tid_feedback, FEEDBACK_DELAY_MS, true, 
+    [&host_manager] {
+        host_manager.send_feedback();
+    });
+
+    while (true) {
+        TaskQueue::Core1::process_tasks();
+        tuh_task();
+    }
+}
+
+void set_gp_check_timer(uint32_t task_id) {
+    UserSettings& user_settings = UserSettings::get_instance();
+    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, 
+    [&user_settings] {
+        //Check gamepad inputs for button combo to change usb device driver
+        if (user_settings.check_for_driver_change(_gamepads[0]))
+        {
+            //This will store the new mode and reboot the pico
+            user_settings.store_driver_type(user_settings.get_current_driver());
+        }
+    });
+}
+
+void four_ch_i2c::wireless_connected(bool connected, uint8_t idx) {
+    if (I2C::role() == I2C::Role::MASTER) {
+        I2C::Master::xbox360w_connect(connected, idx);
+    }
+}
+
+void four_ch_i2c::host_mounted(bool mounted) {
+    static std::atomic<bool> tud_is_inited = false;
+
+    board_api::set_led(mounted);
+
+    if (!mounted && tud_is_inited.load()) {
+        TaskQueue::Core0::queue_task([]() {
+            OGXM_LOG("Disconnecting USB and rebooting.\n");
+            board_api::usb::disconnect_all();
+            board_api::reboot();
+        });
+    } else if (!tud_is_inited.load() && mounted) {
+        TaskQueue::Core0::queue_task([]() { 
+            OGXM_LOG("Initializing USB device stack.\n");
+            tud_init(BOARD_TUD_RHPORT); 
+            tud_is_inited.store(true);
+        });
+    }
+}
+
+void four_ch_i2c::host_mounted_w_type(bool mounted, HostDriverType host_type) {
+    host_mounted(mounted);
+    if (I2C::role() == I2C::Role::MASTER) {
+        I2C::Master::tuh_connect(mounted, host_type);
+    }
+}
+
+void four_ch_i2c::initialize() {
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
+    board_api::init_board();
+
+    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i) {
+        _gamepads[i].set_profile(user_settings.get_profile_by_index(i));
+    }
+
+    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), _gamepads);
+}
+
+void four_ch_i2c::run() {
+    I2C::initialize();
+    
+    multicore_reset_core1();
+    multicore_launch_core1(core1_task);
+
+    //Wait for something to call tud_init
+    while (!tud_inited()) {
+        TaskQueue::Core0::process_tasks();
+        sleep_ms(100);
+    }
+
+    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
+    set_gp_check_timer(tid_gp_check);
+
+    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
+
+    if (I2C::role() == I2C::Role::MASTER) {
+        while (true) {
+            TaskQueue::Core0::process_tasks();
+            I2C::Master::process();
+            device_driver->process(0, _gamepads[0]);
+            tud_task();
+            sleep_ms(1);
+        }
+    } else {
+        while (true) {
+            TaskQueue::Core0::process_tasks();
+            device_driver->process(0, _gamepads[0]);
+            tud_task();
+            sleep_ms(1);
+        }
+    }
+}
+
+// #else // OGXM_BOARD == INTERNAL_4CH_I2C || OGXM_BOARD == EXTERNAL_4CH_I2C
+
+// void four_ch_i2c::initialize() {}
+// void four_ch_i2c::run() {}
+// void four_ch_i2c::host_mounted_w_type(bool mounted, HostDriverType host_type) {}
+// void four_ch_i2c::host_mounted(bool mounted) {}
+// void four_ch_i2c::wireless_connected(bool connected, uint8_t idx) {}
+
+#endif // OGXM_BOARD == INTERNAL_4CH || OGXM_BOARD == EXTERNAL_4CH

Firmware/RP2040/src/OGXMini/Board/Four_Channel_I2C.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <cstdint>
+
+#include "USBHost/HostDriver/HostDriverTypes.h"
+
+namespace four_ch_i2c {
+    void initialize();
+    void run();
+    void host_mounted_w_type(bool mounted, HostDriverType host_type);
+    void host_mounted(bool mounted);
+    void wireless_connected(bool connected, uint8_t idx);
+} // namespace ext_4ch_i2c

Firmware/RP2040/src/OGXMini/Board/PicoW.cpp

@@ -0,0 +1,81 @@
+#include "Board/Config.h"
+#include "OGXMini/Board/PicoW.h"
+#if (OGXM_BOARD == PI_PICOW)
+
+#include <hardware/clocks.h>
+#include <pico/multicore.h>
+
+#include "tusb.h"
+#include "bsp/board_api.h"
+
+#include "USBDevice/DeviceManager.h"
+#include "UserSettings/UserSettings.h"
+#include "Board/board_api.h"
+#include "Bluepad32/Bluepad32.h"
+#include "BLEServer/BLEServer.h"
+#include "Gamepad/Gamepad.h"
+#include "TaskQueue/TaskQueue.h"
+
+Gamepad _gamepads[MAX_GAMEPADS];
+
+void core1_task() {
+    board_api::init_bluetooth();
+    board_api::set_led(true);
+    BLEServer::init_server(_gamepads);
+    bluepad32::run_task(_gamepads);
+}
+
+void set_gp_check_timer(uint32_t task_id) {
+    UserSettings& user_settings = UserSettings::get_instance();
+    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, 
+    [&user_settings] {
+        //Check gamepad inputs for button combo to change usb device driver
+        if (user_settings.check_for_driver_change(_gamepads[0])) {
+            //This will store the new mode and reboot the pico
+            user_settings.store_driver_type(user_settings.get_current_driver());
+        }
+    });
+}
+
+void pico_w::initialize() {
+    board_api::init_board();
+
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
+    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i) {
+        _gamepads[i].set_profile(user_settings.get_profile_by_index(i));
+    }
+
+    DeviceManager& device_manager = DeviceManager::get_instance();
+    device_manager.initialize_driver(user_settings.get_current_driver(), _gamepads);
+}
+
+void pico_w::run() {
+    multicore_reset_core1();
+    multicore_launch_core1(core1_task);
+
+    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
+    set_gp_check_timer(tid_gp_check);
+
+    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
+
+    tud_init(BOARD_TUD_RHPORT);
+
+    while (true) {
+        TaskQueue::Core0::process_tasks();
+
+        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i) {
+            device_driver->process(i, _gamepads[i]);
+            tud_task();
+        }
+        sleep_ms(1);
+    }
+}
+
+// #else // (OGXM_BOARD == PI_PICOW)
+
+// void pico_w::initialize() {}
+// void pico_w::run() {}
+
+#endif // (OGXM_BOARD == PI_PICOW)

Firmware/RP2040/src/OGXMini/Board/PicoW.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <cstdint>
+
+namespace pico_w {
+    void initialize();
+    void run();
+} // namespace pico_w

Firmware/RP2040/src/OGXMini/Board/Standard.cpp

@@ -0,0 +1,134 @@
+#include "Board/Config.h"
+#include "OGXMini/Board/Standard.h"
+#if ((OGXM_BOARD == PI_PICO) || (OGXM_BOARD == RP2040_ZERO) || (OGXM_BOARD == ADAFRUIT_FEATHER))
+
+#include <pico/multicore.h>
+
+#include "tusb.h"
+#include "bsp/board_api.h"
+#include "pio_usb.h"
+
+#include "USBHost/HostManager.h"
+#include "USBDevice/DeviceManager.h"
+#include "TaskQueue/TaskQueue.h"
+#include "Gamepad/Gamepad.h"
+#include "Board/board_api.h"
+#include "Board/ogxm_log.h"
+
+constexpr uint32_t FEEDBACK_DELAY_MS = 200;
+
+Gamepad _gamepads[MAX_GAMEPADS];
+
+void core1_task() {
+    HostManager& host_manager = HostManager::get_instance();
+    host_manager.initialize(_gamepads);
+
+    //Pico-PIO-USB will not reliably detect a hot plug on some boards, 
+    //monitor and init host stack after connection
+    while(!board_api::usb::host_connected()) {
+        sleep_ms(100);
+    }
+
+    pio_usb_configuration_t pio_cfg = PIO_USB_CONFIG;
+    tuh_configure(BOARD_TUH_RHPORT, TUH_CFGID_RPI_PIO_USB_CONFIGURATION, &pio_cfg);
+
+    tuh_init(BOARD_TUH_RHPORT);
+
+    uint32_t tid_feedback = TaskQueue::Core1::get_new_task_id();
+    TaskQueue::Core1::queue_delayed_task(tid_feedback, FEEDBACK_DELAY_MS, true, 
+    [&host_manager] {
+        host_manager.send_feedback();
+    });
+
+    while (true) {
+        TaskQueue::Core1::process_tasks();
+        tuh_task();
+    }
+}
+
+void set_gp_check_timer(uint32_t task_id) {
+    UserSettings& user_settings = UserSettings::get_instance();
+    
+    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, 
+    [&user_settings] {
+        //Check gamepad inputs for button combo to change usb device driver
+        if (user_settings.check_for_driver_change(_gamepads[0])) {
+            OGXM_LOG("Driver change detected, storing new driver.\n");
+            //This will store the new mode and reboot the pico
+            user_settings.store_driver_type(user_settings.get_current_driver());
+        }
+    });
+}
+
+//Called by tusb host so we know to connect or disconnect usb
+void standard::host_mounted(bool host_mounted) {
+    static std::atomic<bool> tud_is_inited = false;
+    board_api::set_led(host_mounted);
+
+    if (!host_mounted && tud_is_inited.load()) {
+        TaskQueue::Core0::queue_task([]() {
+            OGXM_LOG("USB disconnected, rebooting.\n");
+            board_api::usb::disconnect_all();
+            board_api::reboot();
+        });
+    } else if (!tud_is_inited.load()) {
+        TaskQueue::Core0::queue_task([]() { 
+            tud_init(BOARD_TUD_RHPORT); 
+            tud_is_inited.store(true);
+        });
+    }
+}
+
+void standard::initialize() {
+    board_api::init_board();
+
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
+    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i) {
+        _gamepads[i].set_profile(user_settings.get_profile_by_index(i));
+    }
+
+    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), _gamepads);
+}
+
+void standard::run() {
+    multicore_reset_core1();
+    multicore_launch_core1(core1_task);
+
+    DeviceDriverType current_driver = UserSettings::get_instance().get_current_driver();
+
+    if (current_driver != DeviceDriverType::WEBAPP) {
+        // Wait for something to call host_mounted()
+        while (!tud_inited()) {
+            TaskQueue::Core0::process_tasks();
+            sleep_ms(100);
+        }
+    } else {
+        //Connect immediately in WebApp mode 
+        host_mounted(true);
+    }
+
+    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
+    set_gp_check_timer(tid_gp_check);
+
+    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
+
+    while (true) {
+        TaskQueue::Core0::process_tasks();
+
+        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i) {
+            device_driver->process(i, _gamepads[i]);
+        }
+        tud_task();
+        sleep_ms(1);
+    }
+}
+
+// #else // OGXM_BOARD == PI_PICO || OGXM_BOARD == RP2040_ZERO || OGXM_BOARD == ADAFRUIT_FEATHER
+
+// void standard::host_mounted(bool host_mounted) {}
+// void standard::initialize() {}
+// void standard::run() {}
+
+#endif // OGXM_BOARD == PI_PICO || OGXM_BOARD == RP2040_ZERO || OGXM_BOARD == ADAFRUIT_FEATHER

Firmware/RP2040/src/OGXMini/Board/Standard.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <cstdint>
+
+namespace standard {
+    void initialize();
+    void run();
+    void host_mounted(bool mounted);
+} // namespace standard

Firmware/RP2040/src/OGXMini/Debug.h

@@ -1,55 +0,0 @@
-#ifndef _OGXM_DEBUG_H_
-#define _OGXM_DEBUG_H_
-
-#include "board_config.h"
-#if defined(OGXM_DEBUG)
-
-#include <cstdint>
-#include <string>
-#include <sstream>
-#include <iomanip>
-#include <pico/mutex.h>
-#include <hardware/uart.h>
-
-namespace OGXMini {
-
-    //Don't use this directly, use the OGXM_LOG macro
-    static inline 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);
-        uart_puts(DEBUG_UART_PORT, message.c_str());
-        mutex_exit(&log_mutex);
-    }
-
-    //Don't use this directly, use the OGXM_LOG macro
-    static inline void log_hex(const uint8_t* data, size_t size)
-    {
-        std::ostringstream hex_stream;
-        hex_stream << std::hex << std::setfill('0');
-        for (uint16_t i = 0; i < size; ++i)
-        {
-            hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
-        }
-        log(hex_stream.str());
-    }
-
-} // namespace OGXMini
-
-#endif // defined(OGXM_DEBUG)
-
-#if defined(OGXM_DEBUG)
-    #define OGXM_LOG OGXMini::log
-    #define OGXM_LOG_HEX OGXMini::log_hex
-#else
-    #define OGXM_LOG(...)
-    #define OGXM_LOG_HEX(...)
-#endif // OGXM_DEBUG
-
-#endif // _OGXM_DEBUG_H_

Firmware/RP2040/src/OGXMini/OGXMini.cpp

@@ -0,0 +1,103 @@
+#include "Board/Config.h"
+#include "OGXMini/Board/Standard.h"
+#include "OGXMini/Board/PicoW.h"
+#include "OGXMini/Board/Four_Channel_I2C.h"
+#include "OGXMini/Board/ESP32_Blueretro_I2C.h"
+#include "OGXMini/Board/ESP32_Bluepad32_I2C.h"
+#include "OGXMini/OGXMini.h"
+
+namespace OGXMini {
+    typedef void (*InitFunc)();
+    typedef void (*RunFunc)();
+    typedef void (*HostMountedFunc)(bool mounted);
+    typedef void (*HostMountedWTypeFunc)(bool mounted, HostDriverType host_type);
+    typedef void (*WirelessConnectedFunc)(bool connected, uint8_t idx);
+
+    static constexpr InitFunc init_func[BOARDS_COUNT] = {
+        standard::initialize,       // PI_PICO
+        standard::initialize,       // RP2040_ZERO
+        standard::initialize,       // ADAFRUIT_FEATHER
+        pico_w::initialize,         // PI_PICOW
+        esp32_bp32_i2c::initialize, // ESP32_BLUEPAD32_I2C
+        esp32_br_i2c::initialize,   // ESP32_BLUERETRO_I2C
+        four_ch_i2c::initialize,    // EXTERNAL_4CH_I2C
+        four_ch_i2c::initialize,    // INTERNAL_4CH_I2C
+    };
+
+    static constexpr RunFunc run_func[BOARDS_COUNT] = {
+        standard::run,          // PI_PICO
+        standard::run,          // RP2040_ZERO
+        standard::run,          // ADAFRUIT_FEATHER
+        pico_w::run,            // PI_PICOW
+        esp32_bp32_i2c::run,    // ESP32_BLUEPAD32_I2C
+        esp32_br_i2c::run,      // ESP32_BLUERETRO_I2C
+        four_ch_i2c::run,       // EXTERNAL_4CH_I2C
+        four_ch_i2c::run,       // INTERNAL_4CH_I2C
+    };
+
+    static constexpr HostMountedFunc host_mount_func[BOARDS_COUNT] = {
+        standard::host_mounted,     // PI_PICO
+        standard::host_mounted,     // RP2040_ZERO
+        standard::host_mounted,     // ADAFRUIT_FEATHER
+        nullptr,                    // PI_PICOW
+        nullptr,                    // ESP32_BLUEPAD32_I2C
+        nullptr,                    // ESP32_BLUERETRO_I2C
+        four_ch_i2c::host_mounted,  // EXTERNAL_4CH_I2C
+        four_ch_i2c::host_mounted,  // INTERNAL_4CH_I2C
+    };
+
+    static constexpr HostMountedWTypeFunc host_mount_w_type_func[BOARDS_COUNT] = {
+        nullptr,                            // PI_PICO
+        nullptr,                            // RP2040_ZERO
+        nullptr,                            // ADAFRUIT_FEATHER
+        nullptr,                            // PI_PICOW
+        nullptr,                            // ESP32_BLUEPAD32_I2C
+        nullptr,                            // ESP32_BLUERETRO_I2C
+        four_ch_i2c::host_mounted_w_type,   // EXTERNAL_4CH_I2C
+        four_ch_i2c::host_mounted_w_type,   // INTERNAL_4CH_I2C
+    };
+
+    static constexpr WirelessConnectedFunc wl_conn_func[BOARDS_COUNT] = {
+        nullptr,                            // PI_PICO
+        nullptr,                            // RP2040_ZERO
+        nullptr,                            // ADAFRUIT_FEATHER
+        nullptr,                            // PI_PICOW
+        nullptr,                            // ESP32_BLUEPAD32_I2C
+        nullptr,                            // ESP32_BLUERETRO_I2C
+        four_ch_i2c::wireless_connected,    // EXTERNAL_4CH_I2C
+        four_ch_i2c::wireless_connected,    // INTERNAL_4CH_I2C
+    };
+
+    void initialize() {
+        if (init_func[OGXM_BOARD] != nullptr) {
+            init_func[OGXM_BOARD]();
+        }
+    }
+
+    void run() {
+        if (run_func[OGXM_BOARD] != nullptr) {
+            run_func[OGXM_BOARD]();
+        }
+    }
+
+    void host_mounted(bool mounted, HostDriverType host_type) {
+        if (host_mount_w_type_func[OGXM_BOARD] != nullptr) {
+            host_mount_w_type_func[OGXM_BOARD](mounted, host_type);
+        } else if (host_mount_func[OGXM_BOARD] != nullptr) {
+            host_mount_func[OGXM_BOARD](mounted);
+        }
+    }
+
+    void host_mounted(bool mounted) {
+        if (host_mount_func[OGXM_BOARD] != nullptr) {
+            host_mount_func[OGXM_BOARD](mounted);
+        }
+    }
+
+    void wireless_connected(bool connected, uint8_t idx) {
+        if (wl_conn_func[OGXM_BOARD] != nullptr) {
+            wl_conn_func[OGXM_BOARD](connected, idx);
+        }
+    }
+
+} // namespace OGXMini

Firmware/RP2040/src/OGXMini/OGXMini.h

@@ -1,18 +1,13 @@
-#ifndef _OGX_MINI_H_
-#define _OGX_MINI_H_
+#pragma once
 
 #include <cstdint>
 
-#include "UserSettings/UserSettings.h"
+#include "USBHost/HostDriver/HostDriverTypes.h"
 
-namespace OGXMini
-{
-    static constexpr int32_t FEEDBACK_DELAY_MS = 150;
-    static constexpr int32_t TUD_CHECK_DELAY_MS = 500;
-
-    void run_program();
-    void update_tud_status(bool host_mounted);
-
-} // namespace OGXMini
-
-#endif // _OGX_MINI_H_
+namespace OGXMini {
+    void initialize();
+    void run();
+    void host_mounted(bool mounted, HostDriverType host_type);
+    void host_mounted(bool mounted);
+    void wireless_connected(bool connected, uint8_t idx);
+} // namespace OGXMini

Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp

@@ -1,131 +0,0 @@
-#include "board_config.h"
-#if OGXM_BOARD == INTERNAL_4CH || OGXM_BOARD == EXTERNAL_4CH
-
-#include <pico/multicore.h>
-
-#include "tusb.h"
-#include "bsp/board_api.h"
-#include "pio_usb.h"
-
-#include "USBDevice/DeviceManager.h"
-#include "USBHost/HostManager.h"
-#include "Board/board_api.h"
-#include "OGXMini/OGXMini.h"
-#include "I2CDriver/4Channel/I2CManager.h"
-#include "Gamepad.h"
-#include "TaskQueue/TaskQueue.h"
-
-namespace OGXMini {
-
-Gamepad gamepads_[MAX_GAMEPADS];
-std::atomic<bool> tud_inited_{false};
-
-void core1_task()
-{
-    HostManager& host_manager = HostManager::get_instance();
-    host_manager.initialize(gamepads_);
-
-    pio_usb_configuration_t pio_cfg = PIO_USB_CONFIG;
-    tuh_configure(BOARD_TUH_RHPORT, TUH_CFGID_RPI_PIO_USB_CONFIGURATION, &pio_cfg);
-
-    tuh_init(BOARD_TUH_RHPORT);
-
-    uint32_t tid_feedback = TaskQueue::Core1::get_new_task_id();
-    TaskQueue::Core1::queue_delayed_task(tid_feedback, FEEDBACK_DELAY_MS, true, [&host_manager]
-    {
-        host_manager.send_feedback();
-    });
-
-    while (true) 
-    {
-        TaskQueue::Core1::process_tasks();
-        tuh_task();
-    }
-}
-
-//Called by tusb host or i2c driver so we know to connect or disconnect usb
-void update_tud_status(bool host_mounted)
-{
-    board_api::set_led(host_mounted);
-
-    if (!host_mounted && tud_inited_.load())
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_disconnect();
-            sleep_ms(300);
-            multicore_reset_core1();
-            sleep_ms(300);
-            board_api::reboot();
-        });
-    }
-    else if (!tud_inited_.load())
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_init(BOARD_TUD_RHPORT);
-        });
-    }
-}
-
-void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
-{
-    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
-    {
-        //Check gamepad inputs for button combo to change usb device driver
-        if (user_settings.check_for_driver_change(gamepads_[0]))
-        {
-            //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
-    });
-}
-
-void run_program()
-{
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
-    board_api::init_board();
-
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
-    }
-
-    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
-    I2CManager::get_instance().initialize_driver();
-    
-    multicore_reset_core1();
-    multicore_launch_core1(core1_task);
-
-    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
-    set_gp_check_timer(tid_gp_check, user_settings);
-
-    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
-    I2CDriver* i2c_driver = I2CManager::get_instance().get_driver();
-
-    //Wait for something to call tud_init
-    while (!tud_inited())
-    {
-        TaskQueue::Core0::process_tasks();
-        sleep_ms(10);
-    }
-
-    tud_inited_.store(true);
-
-    while (true)
-    {
-        TaskQueue::Core0::process_tasks();
-
-        i2c_driver->process(gamepads_);
-        device_driver->process(0, gamepads_[0]);
-
-        tud_task();
-        sleep_us(100);
-    }
-}
-
-} // namespace OGXMini
-
-#endif // OGXM_BOARD == INTERNAL_4CH || OGXM_BOARD == EXTERNAL_4CH

Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp

@@ -1,104 +0,0 @@
-#include "board_config.h"
-#if OGXM_BOARD == W_ESP32
-
-#include <pico/multicore.h>
-
-#include "tusb.h"
-#include "bsp/board_api.h"
-
-#include "USBDevice/DeviceManager.h"
-#include "Board/board_api.h"
-#include "OGXMini/OGXMini.h"
-#include "I2CDriver/ESP32/I2CDriver.h"
-#include "Gamepad.h"
-#include "TaskQueue/TaskQueue.h"
-
-namespace OGXMini {
-
-static Gamepad gamepads_[MAX_GAMEPADS];
-
-//Not using this for ESP32 currently
-void update_tud_status(bool host_mounted) { }
-
-void core1_task()
-{
-    I2CDriver::initialize(gamepads_);
-
-    while (true) 
-    {
-        tight_loop_contents();
-    }
-}
-
-void run_esp32_uart_bridge()
-{
-    DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(DeviceDriver::Type::UART_BRIDGE, gamepads_);
-
-    board_api::enter_esp32_prog_mode();
-    
-    device_manager.get_driver()->process(0, gamepads_[0]); //Runs UART Bridge task, doesn't return
-}
-
-void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
-{
-    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
-    {
-        //Check gamepad inputs for button combo to change usb device driver
-        if (user_settings.check_for_driver_change(gamepads_[0]))
-        {
-            //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
-    });
-}
-
-void run_program()
-{
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
-    board_api::init_board();
-
-    if (board_api::uart_bridge_mode())
-    {
-        run_esp32_uart_bridge();
-        return;
-    }
-
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
-    }
-
-    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
-
-    multicore_reset_core1();
-    multicore_launch_core1(core1_task);
-
-    board_api::reset_esp32();
-
-    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
-    set_gp_check_timer(tid_gp_check, user_settings);
-
-    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
-
-    tud_init(BOARD_TUD_RHPORT);
-
-    while (true)
-    {
-        TaskQueue::Core0::process_tasks();
-
-        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-        {
-            device_driver->process(i, gamepads_[i]);
-            tud_task();
-        }
-        
-        sleep_us(100);
-    }
-}
-
-} // namespace OGXMini
-
-#endif // OGXM_BOARD == W_ESP32

Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp

@@ -1,83 +0,0 @@
-#include "board_config.h"
-#if (OGXM_BOARD == PI_PICOW)
-
-#include <pico/multicore.h>
-#include <pico/cyw43_arch.h>
-
-#include "tusb.h"
-#include "bsp/board_api.h"
-
-#include "USBDevice/DeviceManager.h"
-#include "Board/board_api.h"
-#include "Bluepad32/Bluepad32.h"
-#include "OGXMini/OGXMini.h"
-#include "Gamepad.h"
-#include "TaskQueue/TaskQueue.h"
-
-namespace OGXMini {
-
-Gamepad gamepads_[MAX_GAMEPADS];
-
-//Not using this for Pico W currently
-void update_tud_status(bool host_mounted) { }
-
-void core1_task()
-{
-    if (cyw43_arch_init() != 0)
-    {  
-        panic("CYW43 init failed");
-    }
-
-    //Doesn't return, don't do anything with core1 unless it's executing within the BTStack loop
-    bluepad32::run_task(gamepads_);
-}
-
-void run_program()
-{
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
-    board_api::init_board();
-
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
-    }
-
-    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
-
-    multicore_reset_core1();
-    multicore_launch_core1(core1_task);
-
-    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
-    TaskQueue::Core0::queue_delayed_task(tid_gp_check, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
-    {
-        //Check gamepad inputs for button combo to change usb device driver
-        if (user_settings.check_for_driver_change(gamepads_[0]))
-        {
-            //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
-    });
-
-    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
-
-    tud_init(BOARD_TUD_RHPORT);
-
-    while (true)
-    {
-        TaskQueue::Core0::process_tasks();
-
-        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-        {
-            device_driver->process(i, gamepads_[i]);
-            tud_task();
-        }
-
-        sleep_us(100);
-    }
-}
-
-} // namespace OGXMini
-
-#endif // (OGXM_BOARD == PI_PICOW)

Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp

@@ -1,131 +0,0 @@
-#include "board_config.h"
-#if (OGXM_BOARD == ADA_FEATHER) || (OGXM_BOARD == RP_ZERO) || (OGXM_BOARD == PI_PICO) || (OGXM_BOARD == PI_PICO2)
-
-#include <pico/multicore.h>
-
-#include "tusb.h"
-#include "bsp/board_api.h"
-#include "pio_usb.h"
-
-#include "USBHost/HostManager.h"
-#include "USBDevice/DeviceManager.h"
-#include "OGXMini/OGXMini.h"
-#include "TaskQueue/TaskQueue.h"
-#include "Gamepad.h"
-#include "Board/board_api.h"
-
-namespace OGXMini {
-
-Gamepad gamepads_[MAX_GAMEPADS];
-std::atomic<bool> tud_inited_{false};
-
-void core1_task()
-{
-    HostManager& host_manager = HostManager::get_instance();
-    host_manager.initialize(gamepads_);
-
-    pio_usb_configuration_t pio_cfg = PIO_USB_CONFIG;
-    tuh_configure(BOARD_TUH_RHPORT, TUH_CFGID_RPI_PIO_USB_CONFIGURATION, &pio_cfg);
-
-    tuh_init(BOARD_TUH_RHPORT);
-
-    uint32_t tid_feedback = TaskQueue::Core1::get_new_task_id();
-    TaskQueue::Core1::queue_delayed_task(tid_feedback, FEEDBACK_DELAY_MS, true, [&host_manager]
-    {
-        host_manager.send_feedback();
-    });
-
-    while (true)
-    {
-        TaskQueue::Core1::process_tasks();
-        tuh_task();
-    }
-}
-
-//Called by tusb host or i2c driver so we know to connect or disconnect usb
-void update_tud_status(bool host_mounted)
-{
-    board_api::set_led(host_mounted);
-
-    if (!host_mounted && tud_inited_.load())
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_disconnect();
-            sleep_ms(300);
-            multicore_reset_core1();
-            sleep_ms(300);
-            board_api::reboot();
-        });
-    }
-    else if (!tud_inited_.load())
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_init(BOARD_TUD_RHPORT);
-        });
-    }
-}
-
-void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
-{
-    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
-    {
-        //Check gamepad inputs for button combo to change usb device driver
-        if (user_settings.check_for_driver_change(gamepads_[0]))
-        {
-            //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
-    });
-}
-
-void run_program()
-{
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
-    board_api::init_board();
-
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
-    }
-
-    DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
-
-    multicore_reset_core1();
-    multicore_launch_core1(core1_task);
-
-    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
-    set_gp_check_timer(tid_gp_check, user_settings);
-
-    DeviceDriver* device_driver = device_manager.get_driver();
-
-    // Wait for something to call update_tud_status()
-    while (!tud_inited())
-    {
-        TaskQueue::Core0::process_tasks();
-        sleep_ms(100);
-    }
-
-    tud_inited_.store(true);
-
-    while (true)
-    {
-        TaskQueue::Core0::process_tasks();
-
-        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-        {
-            device_driver->process(i, gamepads_[i]);
-        }
-
-        tud_task();
-        sleep_us(100);
-    }
-}
-
-} // namespace OGXMini
-
-#endif // (OGXM_BOARD == ADA_FEATHER) || (OGXM_BOARD == RP_ZERO) || (OGXM_BOARD == PI_PICO) || (OGXM_BOARD == PI_PICO2)

Firmware/RP2040/src/Scale.h

@@ -1,151 +0,0 @@
-#ifndef _SCALE_H_
-#define _SCALE_H_
-
-#include <cstdint>
-
-namespace INT_16
-{
-    static constexpr int16_t MIN = INT16_MIN;
-    static constexpr int16_t MID = 0;
-    static constexpr int16_t MAX = INT16_MAX;
-}
-namespace UINT_16
-{
-    static constexpr uint16_t MIN = 0;
-    static constexpr uint16_t MID = 0x8000;
-    static constexpr uint16_t MAX = 0xFFFF;
-}
-namespace UINT_8
-{
-    static constexpr uint8_t  MAX  = 0xFF;
-    static constexpr uint8_t  MID  = 0x80;
-    static constexpr uint8_t  MIN  = 0x00;
-}
-namespace INT_10
-{
-    static constexpr int32_t  MIN  = -512;
-    static constexpr int32_t  MAX  = 511;
-}
-namespace UINT_10
-{
-    static constexpr int32_t MAX = 1023;
-}
-
-namespace Scale //Scale and invert values
-{
-    static inline uint8_t invert_joy(uint8_t value) 
-    {    
-        return static_cast<uint8_t>(UINT_8::MAX - value); 
-    }
-    static inline int8_t invert_joy(int8_t value) 
-    {    
-        return (value == std::numeric_limits<int8_t>::min()) ? std::numeric_limits<int8_t>::max() : -value; 
-    }
-    static inline uint16_t invert_joy(uint16_t value) 
-    {    
-        return static_cast<uint16_t>(std::numeric_limits<uint16_t>::max() - value); 
-    }
-    static inline int16_t invert_joy(int16_t value) 
-    {    
-        return (value == std::numeric_limits<int16_t>::min()) ? std::numeric_limits<int16_t>::max() : -value; 
-    }
-
-    static inline uint8_t int16_to_uint8(int16_t value)
-    {
-        uint16_t shifted_value = static_cast<uint16_t>(value + UINT_16::MID);
-        return static_cast<uint8_t>(shifted_value >> 8);
-    }
-    static inline uint16_t int16_to_uint16(int16_t value)
-    {
-        return static_cast<uint16_t>(value + UINT_16::MID);
-    }
-    static inline int8_t int16_to_int8(int16_t value)
-    {
-        return static_cast<int8_t>((value + UINT_16::MID) >> 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 - UINT_16::MID);
-    }
-    static inline int8_t uint16_to_int8(uint16_t value)
-    {
-        return static_cast<int8_t>((value >> 8) - UINT_8::MID);
-    }
-
-    static inline int16_t uint8_to_int16(uint8_t value)
-    {
-        return static_cast<int16_t>((static_cast<int32_t>(value) << 8) - UINT_16::MID);
-    }
-    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 - UINT_8::MID);
-    }
-
-    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 + UINT_8::MID) << 8);
-    }
-    static inline uint8_t int8_to_uint8(int8_t value)
-    {
-        return static_cast<uint8_t>(value + UINT_8::MID);
-    }
-
-    static inline uint8_t int10_to_uint8(int32_t value)
-    {
-        value = value - INT_10::MIN;
-
-        if (value >= UINT_10::MAX) 
-        {
-            return UINT_8::MAX;
-        }
-        else if (value <= 0) 
-        {
-            return 0;
-        }
-        return static_cast<uint8_t>(value >> 2);
-    }
-    static inline int16_t int10_to_int16(int32_t value)
-    {
-        constexpr int32_t scale_factor = INT_16::MAX - INT_16::MIN;
-        constexpr int32_t range = INT_10::MAX - INT_10::MIN;
-
-        if (value >= INT_10::MAX)
-        {
-            return INT_16::MAX;
-        }
-        else if (value <= INT_10::MIN)
-        {
-            return INT_16::MIN;
-        }
-
-        int32_t scaled_value = (value - INT_10::MIN) * scale_factor;
-        return static_cast<int16_t>(scaled_value / range + INT_16::MIN);
-    }
-    static inline uint8_t uint10_to_uint8(int32_t value)
-    {
-        if (value > UINT_10::MAX) 
-        {
-            value = UINT_10::MAX;
-        }
-        else if (value < 0) 
-        {
-            value = 0;
-        }
-        return static_cast<uint8_t>(value >> 2);
-    }
-} // namespace Scale
-
-#endif // _SCALE_H_

Firmware/RP2040/src/TaskQueue/TaskQueue.cpp

@@ -1,351 +1,252 @@
-#include <array>
-#include <algorithm>
-#include <atomic>
-#include <pico/stdlib.h>
-#include <hardware/timer.h>
-#include <hardware/irq.h>
-#include <hardware/sync.h>
-
-#include "Board/board_api.h"
-#include "board_config.h"
 #include "TaskQueue/TaskQueue.h"
 
-namespace TaskQueue {
+TaskQueue::TaskQueue(CoreNum core_num) 
+{   
+    alarm_num_ = (core_num == CoreNum::Core0) ? 0 : 1;
+    alarm_num_ += (OGXM_BOARD == PI_PICOW) ? 1 : 0; //BTStack uses alarm 0
 
-class CoreQueue
+    hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
+
+    irq_set_exclusive_handler(
+        TIMER_IRQ(alarm_num_), 
+        (core_num == CoreNum::Core0) ? timer_irq_wrapper_c0 : timer_irq_wrapper_c1);
+
+    irq_set_enabled(TIMER_IRQ(alarm_num_), true);
+}
+
+uint32_t TaskQueue::get_new_task_id()
 {
-public:
-    enum class CoreNum : uint8_t
+    return new_task_id_++;
+}
+
+bool TaskQueue::queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    for (const auto& task : task_queue_delayed_) 
     {
-        Core0 = 0,
-        Core1,
-    };
-
-    CoreQueue(CoreNum core_num) 
-        :   core_num_(core_num)
-    {   
-        uint32_t idx = (core_num_ == CoreNum::Core0) ? 0 : 1;
-#if (OGXM_BOARD != PI_PICOW)
-        alarm_num_ = idx;
-#else
-        alarm_num_ = idx + 1; //BTStack uses alarm 0
-#endif
-        instances_[idx] = this;
-
-        hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
-
-        irq_set_exclusive_handler(
-            TIMER_IRQ(alarm_num_), 
-            (core_num_ == CoreNum::Core0) ? timer_irq_wrapper_c0 : timer_irq_wrapper_c1);
-
-        irq_set_enabled(TIMER_IRQ(alarm_num_), true);
+        if (task.task_id == task_id)
+        {
+            spin_unlock(spinlock_delayed_, irq_state);
+            return false;
+        }
     }
 
-    ~CoreQueue() = default;
+    hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
 
-    uint32_t get_new_task_id()
+    //Might come back to this, there is overflow potential
+    uint64_t target_time = timer_hw->timerawl + static_cast<uint64_t>(delay_ms) * 1000;
+
+    for (auto& task : task_queue_delayed_) 
     {
-        return new_task_id_++;
+        if (!task.function) 
+        {
+            task.target_time = target_time;
+            task.interval_ms = repeating ? delay_ms : 0;
+            task.function = function;
+            task.task_id = task_id;
+
+            auto it = std::min_element(task_queue_delayed_.begin(), task_queue_delayed_.end(), [](const DelayedTask& a, const DelayedTask& b) 
+            {
+                return a.function && (!b.function || a.target_time < b.target_time);
+            });
+
+            if (it != task_queue_delayed_.end() && it->function) 
+            {
+                timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(it->target_time);
+            }
+
+            spin_unlock(spinlock_delayed_, irq_state);
+            return true;
+        }
     }
 
-    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+    spin_unlock(spinlock_delayed_, irq_state);
+    return false;
+}
+
+void TaskQueue::cancel_delayed_task(uint32_t task_id)
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    bool found = false;
+
+    for (auto& task : task_queue_delayed_) 
     {
-        uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
-        for (const auto& task : task_queue_delayed_) 
+        if (task.task_id == task_id) 
         {
-            if (task.task_id == task_id)
-            {
-                spin_unlock(spinlock_delayed_, irq_state);
-                return false;
-            }
-        }
-
-        hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
-
-        //Might come back to this, there is overflow potential
-        uint64_t target_time = timer_hw->timerawl + static_cast<uint64_t>(delay_ms) * 1000;
-
-        for (auto& task : task_queue_delayed_) 
-        {
-            if (!task.function) 
-            {
-                task.target_time = target_time;
-                task.interval_ms = repeating ? delay_ms : 0;
-                task.function = function;
-                task.task_id = task_id;
-
-                auto it = std::min_element(task_queue_delayed_.begin(), task_queue_delayed_.end(), [](const DelayedTask& a, const DelayedTask& b) 
-                {
-                    return a.function && (!b.function || a.target_time < b.target_time);
-                });
-
-                if (it != task_queue_delayed_.end() && it->function) 
-                {
-                    timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(it->target_time);
-                }
-
-                spin_unlock(spinlock_delayed_, irq_state);
-                return true;
-            }
+            task.function = nullptr;
+            task.task_id = 0;
+            task.interval_ms = 0;
+            found = true;
         }
+    }
 
+    if (!found)
+    {
         spin_unlock(spinlock_delayed_, irq_state);
-        return false;
+        return;
     }
 
-    void cancel_delayed_task(uint32_t task_id)
-    {
-        uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
-        bool found = false;
+    hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
 
-        for (auto& task : task_queue_delayed_) 
+    int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
+    if (next_target_time >= 0) 
+    {
+        timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
+    }
+    
+    spin_unlock(spinlock_delayed_, irq_state);
+}
+
+bool TaskQueue::queue_task(const std::function<void()>& function)
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
+    for (auto& task : task_queue_)
+    {
+        if (!task.function)
         {
-            if (task.task_id == task_id) 
+            task.function = function;
+            spin_unlock(spinlock_queue_, irq_state);
+            return true;
+        }
+    }
+    spin_unlock(spinlock_queue_, irq_state);
+    return false;
+}
+
+void TaskQueue::process_tasks()
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
+    for (auto& task : task_queue_)
+    {
+        if (task.function)
+        {
+            auto function = task.function;
+            task.function = nullptr;
+            spin_unlock(spinlock_queue_, irq_state);
+
+            function();
+
+            irq_state = spin_lock_blocking(spinlock_queue_);
+        }
+        else
+        {
+            break; //No more tasks
+        }
+    }
+    spin_unlock(spinlock_queue_, irq_state);
+}
+
+uint64_t TaskQueue::get_time_64_us()
+{
+    static spin_lock_t* spinlock_time_ = nullptr;
+    if (!spinlock_time_)
+    {
+        int spinlock_time_num = spin_lock_claim_unused(true);
+        spinlock_time_ = spin_lock_instance(static_cast<uint>(spinlock_time_num));
+    }
+    uint32_t irq_state = spin_lock_blocking(spinlock_time_);
+    uint32_t lo = timer_hw->timelr;
+    uint32_t hi = timer_hw->timehr;
+    spin_unlock(spinlock_time_, irq_state);
+    return ((uint64_t) hi << 32u) | lo;;
+}
+
+void TaskQueue::timer_irq_handler()
+{
+    hw_clear_bits(&timer_hw->intr, 1u << alarm_num_);
+
+    uint64_t now = get_time_64_us();
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    if (suspended_)
+    {
+        spin_unlock(spinlock_delayed_, irq_state);
+        return;
+    }
+
+    for (auto& task : task_queue_delayed_) 
+    {
+        if (task.function && task.target_time <= now) 
+        {
+            auto function = task.function;
+            if (task.interval_ms) 
+            {
+                task.target_time += (task.interval_ms * 1000);
+            } 
+            else 
             {
                 task.function = nullptr;
                 task.task_id = 0;
-                task.interval_ms = 0;
-                found = true;
             }
-        }
 
-        if (!found)
-        {
             spin_unlock(spinlock_delayed_, irq_state);
-            return;
+            queue_task(function);
+            irq_state = spin_lock_blocking(spinlock_delayed_);
         }
-
-        hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
-
-        int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
-        if (next_target_time >= 0) 
-        {
-            timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
-        }
-        
-        spin_unlock(spinlock_delayed_, irq_state);
     }
 
-    bool queue_task(const std::function<void()>& function)
+    int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
+    if (next_target_time >= 0) 
     {
-        uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
-        for (auto& task : task_queue_)
-        {
-            if (!task.function)
-            {
-                task.function = function;
-                spin_unlock(spinlock_queue_, irq_state);
-                return true;
-            }
-        }
-        spin_unlock(spinlock_queue_, irq_state);
-        return false;
+        timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
     }
 
-    void process_tasks()
-    {
-        uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
-        for (auto& task : task_queue_)
-        {
-            if (task.function)
-            {
-                auto function = task.function;
-                task.function = nullptr;
-                spin_unlock(spinlock_queue_, irq_state);
-
-                function();
-
-                irq_state = spin_lock_blocking(spinlock_queue_);
-            }
-            else
-            {
-                break; //No more tasks
-            }
-        }
-        spin_unlock(spinlock_queue_, irq_state);
-    }
-
-private:
-    CoreQueue& operator=(const CoreQueue&) = delete;
-    CoreQueue(CoreQueue&&) = delete;
-    CoreQueue& operator=(CoreQueue&&) = delete;
-
-    static CoreQueue* instances_[static_cast<uint8_t>(CoreNum::Core1) + 1];
-
-    struct Task
-    {
-        // uint32_t task_id = 0;
-        std::function<void()> function = nullptr;
-    };
-
-    struct DelayedTask
-    {
-        uint32_t task_id = 0;
-        uint32_t interval_ms = 0;
-        uint64_t target_time = 0;
-        std::function<void()> function = nullptr;
-    };
-
-    static constexpr uint8_t MAX_TASKS = 8;
-    static constexpr uint8_t MAX_DELAYED_TASKS = MAX_TASKS * 2;
-
-    CoreNum core_num_;
-    uint32_t alarm_num_;
-    uint32_t new_task_id_ = 1;
-
-    int spinlock_queue_num_ = spin_lock_claim_unused(true);
-    int spinlock_delayed_num_ = spin_lock_claim_unused(true);
-    spin_lock_t* spinlock_queue_ = spin_lock_instance(static_cast<uint>(spinlock_queue_num_));
-    spin_lock_t* spinlock_delayed_ = spin_lock_instance(static_cast<uint>(spinlock_delayed_num_));
-
-    std::array<Task, MAX_TASKS> task_queue_;
-    std::array<DelayedTask, MAX_DELAYED_TASKS> task_queue_delayed_;
-
-    static void timer_irq_wrapper_c0()
-    {
-        instances_[static_cast<uint8_t>(CoreNum::Core0)]->timer_irq_handler();
-    }
-
-    static void timer_irq_wrapper_c1()
-    {
-        instances_[static_cast<uint8_t>(CoreNum::Core1)]->timer_irq_handler();
-    }
-    
-    static inline uint32_t TIMER_IRQ(uint32_t alarm_num)
-    {
-        return timer_hardware_alarm_get_irq_num(timer_hw, alarm_num);
-    }
-
-    static inline uint64_t get_time_64_us()
-    {
-        static spin_lock_t* spinlock_time_ = nullptr;
-        if (!spinlock_time_)
-        {
-            int spinlock_time_num = spin_lock_claim_unused(true);
-            spinlock_time_ = spin_lock_instance(static_cast<uint>(spinlock_time_num));
-        }
-        uint32_t irq_state = spin_lock_blocking(spinlock_time_);
-        uint32_t lo = timer_hw->timelr;
-        uint32_t hi = timer_hw->timehr;
-        spin_unlock(spinlock_time_, irq_state);
-        return ((uint64_t) hi << 32u) | lo;;
-    }
-
-    void timer_irq_handler()
-    {
-        hw_clear_bits(&timer_hw->intr, 1u << alarm_num_);
-
-        uint64_t now = get_time_64_us();
-        uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
-
-        for (auto& task : task_queue_delayed_) 
-        {
-            if (task.function && task.target_time <= now) 
-            {
-                auto function = task.function;
-                if (task.interval_ms) 
-                {
-                    task.target_time += (task.interval_ms * 1000);
-                } 
-                else 
-                {
-                    task.function = nullptr;
-                    task.task_id = 0;
-                }
-
-                spin_unlock(spinlock_delayed_, irq_state);
-                queue_task(function);
-                irq_state = spin_lock_blocking(spinlock_delayed_);
-            }
-        }
-
-        int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
-        if (next_target_time >= 0) 
-        {
-            timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
-        }
-
-        spin_unlock(spinlock_delayed_, irq_state);
-    }
-
-    static inline int64_t get_next_target_time_unsafe(std::array<DelayedTask, MAX_DELAYED_TASKS>& task_queue_delayed)
-    {
-        auto it = std::min_element(task_queue_delayed.begin(), task_queue_delayed.end(), [](const DelayedTask& a, const DelayedTask& b) 
-        {
-            //Get task with the earliest target time
-            return a.function && (!b.function || a.target_time < b.target_time);
-        });
-
-        if (it != task_queue_delayed.end() && it->function) 
-        {
-            return static_cast<int64_t>(it->target_time);
-        }
-        return -1;
-    }
-};
-
-CoreQueue* CoreQueue::instances_[static_cast<uint8_t>(CoreQueue::CoreNum::Core1) + 1]{nullptr};
-
-namespace Core0
-{
-    static inline CoreQueue& get_core_0()
-    {
-        static CoreQueue core(CoreQueue::CoreNum::Core0);
-        return core;
-    }
-    uint32_t get_new_task_id()
-    {
-        return get_core_0().get_new_task_id();
-    }
-    void cancel_delayed_task(uint32_t task_id)
-    {
-        get_core_0().cancel_delayed_task(task_id);
-    }
-    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
-    {
-        return get_core_0().queue_delayed_task(task_id, delay_ms, repeating, function);
-    }
-    bool queue_task(const std::function<void()>& function)
-    {
-        return get_core_0().queue_task(function);
-    }
-    void process_tasks()
-    {
-        get_core_0().process_tasks();
-    }
+    spin_unlock(spinlock_delayed_, irq_state);
 }
 
-#if OGXM_BOARD != PI_PICOW
-
-namespace Core1
+void TaskQueue::suspend_delayed_tasks()
 {
-    static inline CoreQueue& get_core_1()
-    {
-        static CoreQueue core(CoreQueue::CoreNum::Core1);
-        return core;
-    }
-    uint32_t get_new_task_id()
-    {
-        return get_core_1().get_new_task_id();
-    }
-    void cancel_delayed_task(uint32_t task_id)
-    {
-        get_core_1().cancel_delayed_task(task_id);
-    }
-    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
-    {
-        return get_core_1().queue_delayed_task(task_id, delay_ms, repeating, function);
-    }
-    bool queue_task(const std::function<void()>& function)
-    {
-        return get_core_1().queue_task(function);
-    }
-    void process_tasks()
-    {
-        get_core_1().process_tasks();
-    }
+    get_core0().suspend_delayed();
+#if (OGXM_BOARD != PI_PICOW) && (OGXM_BOARD != PI_PICO2W)
+    get_core1().suspend_delayed();
+#endif
 }
 
-#endif // OGXM_BOARD != PI_PICOW
+void TaskQueue::resume_delayed_tasks()
+{
+    get_core0().resume_delayed();
+#if (OGXM_BOARD != PI_PICOW) && (OGXM_BOARD != PI_PICO2W)
+    get_core1().resume_delayed();
+#endif
+}
 
-} // namespace TaskQueue
+void TaskQueue::suspend_delayed()
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    if (suspended_)
+    {
+        spin_unlock(spinlock_delayed_, irq_state);
+        return;
+    }
+    hw_clear_bits(&timer_hw->intr, 1u << alarm_num_);
+    suspended_time_ = get_time_64_us();
+    suspended_ = true;
+    spin_unlock(spinlock_delayed_, irq_state);
+}
+
+void TaskQueue::resume_delayed()
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    if (!suspended_)
+    {
+        spin_unlock(spinlock_delayed_, irq_state);
+        return;
+    }
+
+    uint64_t now = get_time_64_us();
+    uint64_t elapsed_time = now - suspended_time_;
+
+    for (auto& task : task_queue_delayed_) 
+    {
+        if (task.function) 
+        {
+            task.target_time = std::max(task.target_time + elapsed_time, now + 10);
+        }
+    }
+    int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
+    if (next_target_time >= 0) 
+    {
+        timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
+    }
+    suspended_ = false;
+    spin_unlock(spinlock_delayed_, irq_state);
+}