v1.0.0-alpha3

.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

Firmware/ESP32/CMakeLists.txt

@@ -1,16 +1,26 @@
 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}/init_submodules.cmake)
-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})
 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)
 
@@ -24,4 +34,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/main/1btstack_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/BLEServer/BLEServer.cpp

@@ -0,0 +1,264 @@
+#include <cstring>
+#include <string>
+#include <esp_ota_ops.h>
+#include <esp_system.h>
+
+#include "att_server.h"
+#include "btstack.h"
+
+#include "Gamepad.h"
+#include "BLEServer/BLEServer.h"
+#include "BLEServer/att_delayed_response.h"
+#include "UserSettings/UserProfile.h"
+#include "UserSettings/UserSettings.h"
+
+namespace BLEServer {
+
+static constexpr uint16_t PACKET_LEN_MAX = 18;
+
+#pragma pack(push, 1)
+struct SetupPacket
+{
+    uint8_t max_gamepads{1};
+    uint8_t index{0};
+    uint8_t device_type{0};
+    uint8_t profile_id{1};
+};
+static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
+#pragma pack(pop)
+
+SetupPacket setup_packet_;
+
+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 START_UPDATE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
+    static constexpr uint16_t COMMIT_UPDATE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
+
+    static constexpr uint16_t SETUP_PACKET  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT1   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789041_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT2   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789042_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT3   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789043_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::memcpy(name, FIRMWARE_NAME, sizeof(name));
+        }
+    };
+    static_assert(sizeof(Data) == 5 + sizeof(FIRMWARE_NAME) - 1, "BLEServer::ADV::Data struct size mismatch");
+    #pragma pack(pop)
+}
+
+static int verify_write(const uint16_t buffer_size, const uint16_t expected_size, bool pending_write = false, bool expected_pending_write = false)
+{
+    if (buffer_size != expected_size)
+    {
+        return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
+    }
+    if (pending_write != expected_pending_write)
+    {
+        return ATT_ERROR_WRITE_NOT_PERMITTED;
+    }
+    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)
+{
+    static UserProfile profile;
+    SetupPacket setup_packet_resp;
+    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::SETUP_PACKET:
+            if (buffer)
+            {
+                //App has already written a setup packet with the index
+                setup_packet_resp.max_gamepads = static_cast<uint8_t>(MAX_GAMEPADS);
+                setup_packet_resp.index = setup_packet_.index;
+                setup_packet_resp.device_type = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
+                setup_packet_resp.profile_id = UserSettings::get_instance().get_active_profile_id(setup_packet_.index);
+
+                std::memcpy(buffer, &setup_packet_resp, sizeof(setup_packet_resp));
+            }
+            return sizeof(setup_packet_);
+
+        case Handle::PROFILE_PT1:
+            if (buffer)
+            {
+                //App has already written the profile id it wants to the setup packet
+                profile = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
+                std::memcpy(buffer, &profile, PACKET_LEN_MAX);
+            }
+            return PACKET_LEN_MAX;
+
+        case Handle::PROFILE_PT2:
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX, PACKET_LEN_MAX);
+            }
+            return PACKET_LEN_MAX;
+
+        case Handle::PROFILE_PT3:
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX * 2, sizeof(UserProfile) - PACKET_LEN_MAX * 2);
+            }
+            return sizeof(UserProfile) - PACKET_LEN_MAX * 2;
+
+        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)
+{
+    static UserProfile temp_profile;
+    static bool pending_write = false;
+
+    int ret = 0;
+
+    switch (att_handle)
+    {
+        case Handle::START_UPDATE:
+            pending_write = true;
+            break;
+
+        case Handle::SETUP_PACKET:
+            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
+            {
+                break;
+            }
+
+            std::memcpy(&setup_packet_, buffer, buffer_size);
+            if (setup_packet_.index >= MAX_GAMEPADS)
+            {
+                setup_packet_.index = 0;
+                ret = ATT_ERROR_OUT_OF_RANGE;
+            }
+            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
+            {
+                setup_packet_.profile_id = 1;
+                ret = ATT_ERROR_OUT_OF_RANGE;
+            }
+            if (ret)
+            {
+                break;
+            }
+
+            if (pending_write)
+            {
+                //App wants to store a new device driver type
+                UserSettings::get_instance().store_driver_type(static_cast<DeviceDriverType>(setup_packet_.device_type));
+            }
+            break;
+
+        case Handle::PROFILE_PT1:
+            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(&temp_profile, buffer, buffer_size);
+            break;
+
+        case Handle::PROFILE_PT2:
+            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX, buffer, buffer_size);
+            break;
+
+        case Handle::PROFILE_PT3:
+            if ((ret = verify_write(buffer_size, sizeof(UserProfile) - PACKET_LEN_MAX * 2, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX * 2, buffer, buffer_size);
+            break;
+
+        case Handle::COMMIT_UPDATE:
+            if ((ret = verify_write(0, 0, pending_write, true)) != 0)
+            {
+                break;
+            }
+            UserSettings::get_instance().store_profile(setup_packet_.index, temp_profile);
+            pending_write = false;
+            break;
+
+        default:
+            break;
+    }
+    return ret;
+}
+
+void init_server()
+{
+    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/ESP32/main/BLEServer/BLEServer.h

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

Firmware/ESP32/main/BTManager/BTManager.cpp

@@ -0,0 +1,245 @@
+#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"
+
+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);
+
+    //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)
+{
+    OGXM_LOG("BP32: Sending driver type: %s\n", DRIVER_NAME(driver_type).c_str());
+
+    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);
+    }
+}

Firmware/ESP32/main/BTManager/BTManager.h

@@ -0,0 +1,82 @@
+#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.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);
+
+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,187 @@
+#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"
+
+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.trigger_l(uni_gp->brake);
+    packet_in.trigger_r = mapper.trigger_r(uni_gp->throttle);
+
+    packet_in.joystick_lx = mapper.joystick_lx(uni_gp->axis_x);
+    packet_in.joystick_ly = mapper.joystick_ly(uni_gp->axis_y);
+    packet_in.joystick_rx = mapper.joystick_rx(uni_gp->axis_rx);
+    packet_in.joystick_ry = mapper.joystick_ry(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,338 +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
-            );
-    }
-}
-
-//This will have to be changed once full support for multiple devices is added
-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]);
-        
-        //Register a read on i2c thread, with callback to send feedback on btstack thread
-        i2c_driver_.i2c_read_blocking_safe( I2CDriver::MULTI_SLAVE ? i + 1 : 0x01,
-                                            [i](const I2CDriver::PacketOut& packet_out)
-                                            {
-                                                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_.i2c_write_blocking_safe(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
-}
-
-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_.i2c_write_blocking_safe(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
-
-    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;
-}
-
-void run_i2c_task(void* parameter)
-{
-    i2c_driver_.initialize_i2c();
-    i2c_driver_.run_tasks();
-}
-
-//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(); 
-
-    xTaskCreatePinnedToCore(
-        run_i2c_task,
-        "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

@@ -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,56 @@
+#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');
+        for (uint16_t i = 0; i < len; ++i)
+        {
+            hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
+        }
+
+        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,10 +1,16 @@
+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"
     INCLUDE_DIRS 
         "."
     REQUIRES 
@@ -13,3 +19,8 @@ idf_component_register(
         driver 
         nvs_flash
 )
+
+target_compile_definitions(${COMPONENT_LIB} PRIVATE
+        FIRMWARE_NAME=\"${FW_NAME}\"
+        FIRMWARE_VERSION=\"${FW_VERSION}\"
+)

Firmware/ESP32/main/Gamepad.h

@@ -0,0 +1,246 @@
+#ifndef GAMEPAD_H
+#define GAMEPAD_H
+
+#include <cstdint>
+
+#include "sdkconfig.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 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 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;
+}
+
+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_options(profile);
+        set_profile_mappings(profile);
+        set_profile_deadzones(profile);
+    }
+
+    inline int16_t joystick_ly(int32_t value_10bit) 
+    {
+        int16_t value = int10_to_int16(value_10bit);
+        value = profile_invert_ly_ ? invert_joy(value) : value;
+        return (value <= dz_.joystick_l_neg || value >= dz_.joystick_l_pos) ? value : INT_16::MID;
+    };
+
+    inline int16_t joystick_lx(int32_t value_10bit) 
+    {
+        int16_t value = int10_to_int16(value_10bit);
+        return (value <= dz_.joystick_l_neg || value >= dz_.joystick_l_pos) ? value : INT_16::MID;
+    };
+
+    inline int16_t joystick_ry(int32_t value_10bit) 
+    {
+        int16_t value = int10_to_int16(value_10bit);
+        value = profile_invert_ry_ ? invert_joy(value) : value;
+        return (value <= dz_.joystick_r_neg || value >= dz_.joystick_r_pos) ? value : INT_16::MID;
+    };
+
+    inline int16_t joystick_rx(int32_t value_10bit) 
+    {
+        int16_t value = int10_to_int16(value_10bit);
+        return (value <= dz_.joystick_r_neg || value >= dz_.joystick_r_pos) ? value : INT_16::MID;
+    };
+
+    inline uint8_t trigger_l(int32_t value_10bit) 
+    {
+        uint8_t value = uint10_to_uint8(value_10bit);
+        return (value >= dz_.trigger_l) ? value : UINT_8::MIN;
+    };
+
+    inline uint8_t trigger_r(int32_t value_10bit) 
+    {
+        uint8_t value = uint10_to_uint8(value_10bit);
+        return (value >= dz_.trigger_r) ? value : UINT_8::MIN;
+    };
+
+private:
+    bool profile_invert_ly_{false};
+    bool profile_invert_ry_{false};
+
+    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 set_profile_options(const UserProfile& profile)
+    {
+        profile_invert_ly_ = profile.invert_ly ? true : false;
+        profile_invert_ry_ = profile.invert_ry ? true : false;
+    }
+
+    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;
+
+        OGXM_LOG("Mappings: A: %i B: %i X: %i Y: %i L3: %i R3: %i BACK: %i START: %i LB: %i RB: %i SYS: %i MISC: %i\n", 
+            BUTTON_A, BUTTON_B, BUTTON_X, BUTTON_Y, BUTTON_L3, BUTTON_R3, BUTTON_BACK, BUTTON_START, BUTTON_LB, BUTTON_RB, BUTTON_SYS, BUTTON_MISC);
+    }
+
+    void set_profile_deadzones(const UserProfile& profile) //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 = uint8_to_int16(profile.dz_joystick_l / 2);
+        dz_.joystick_l_neg = invert_joy(dz_.joystick_l_pos);
+        dz_.joystick_r_pos = uint8_to_int16(profile.dz_joystick_r / 2);
+        dz_.joystick_r_neg = invert_joy(dz_.joystick_r_pos);
+
+        OGXM_LOG("Deadzones: TL: %i TR: %i JL: %i JR: %i\n", dz_.trigger_l, dz_.trigger_r, dz_.joystick_l_pos, dz_.joystick_r_pos);
+    }
+
+    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);
+    }
+    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 int16_t invert_joy(int16_t value) 
+    {    
+        return (value == INT_16::MIN) ? INT_16::MAX : -value; 
+    }
+
+}; // class GamepadMapper
+
+#endif // GAMEPAD_H

Firmware/ESP32/main/I2CDriver/I2CDriver.cpp

@@ -5,27 +5,35 @@
 #include <esp_log.h>
 
 #include "I2CDriver/I2CDriver.h"
-#include "Bluepad32/Bluepad32.h"
 
 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_)
+    {
+        i2c_driver_delete(i2c_port_);
+    }
+
+    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()
@@ -42,3 +50,23 @@ void I2CDriver::run_tasks()
         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,36 +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
     void run_tasks();
 
-    inline void i2c_write_blocking_safe(uint8_t address, const PacketIn& packet_in) 
-    {
-        task_queue_.push([this, address, packet_in]() 
-        {
-            i2c_write_blocking(address, reinterpret_cast<const uint8_t*>(&packet_in), sizeof(PacketIn));
-        });
-    }
-
-    inline void i2c_read_blocking_safe(uint8_t address, std::function<void(const PacketOut&)> callback) 
-    {
-        task_queue_.push([this, address, callback]() 
-        {
-            PacketOut packet_out;
-            if (i2c_read_blocking(address, reinterpret_cast<uint8_t*>(&packet_out), sizeof(PacketOut)) == ESP_OK)
-            {
-                callback(packet_out);
-            }
-        });
-    }
+    void write_packet(uint8_t address, const PacketIn& data_in);
+    void read_packet(uint8_t address, std::function<void(const PacketOut&)> callback);
 
 private:
-    using TaskQueue = RingBuffer<std::function<void()>, 6>;
-    TaskQueue task_queue_;
+    using TaskQueue = RingBuffer<std::function<void()>, CONFIG_I2C_RING_BUFFER_SIZE>;
     
-    inline esp_err_t i2c_write_blocking(uint8_t address, const uint8_t* buffer, size_t len) 
+    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);
@@ -109,7 +85,7 @@ private:
         return ret;
     }
 
-    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);
@@ -127,6 +103,6 @@ private:
         i2c_cmd_link_delete(cmd);
         return ret;
     }
-};
+}; // class I2CDriver
 
 #endif // _I2C_DRIVER_H_

Firmware/ESP32/main/Kconfig

@@ -1,5 +1,25 @@
 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 9

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/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 const char NVS_NAMESPACE[] = "user_data";
+    
+}; // class NVSHelper
+
+#endif // _NVS_HELPER_H_

Firmware/ESP32/main/UserSettings/UserProfile.cpp

@@ -0,0 +1,48 @@
+#include <cstring>
+
+#include "Gamepad.h"
+#include "UserSettings/UserProfile.h"
+
+UserProfile::UserProfile()
+{
+    id = 1;
+
+    dz_trigger_l = 0;
+    dz_trigger_r = 0;
+    dz_joystick_l = 0;
+    dz_joystick_r = 0;
+
+    invert_ly = 0;
+    invert_ry = 0;
+
+    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 = 1;
+
+    analog_off_up = 0;
+    analog_off_down = 1;
+    analog_off_left = 2;
+    analog_off_right = 3;
+    analog_off_a = 4;
+    analog_off_b = 5;
+    analog_off_x = 6;
+    analog_off_y = 7;
+    analog_off_lb = 8;
+    analog_off_rb = 9;
+}

Firmware/ESP32/main/UserSettings/UserProfile.h

@@ -0,0 +1,56 @@
+#ifndef _USER_PROFILE_H_
+#define _USER_PROFILE_H_
+
+#include <cstdint>
+
+#pragma pack(push, 1)
+struct UserProfile
+{
+    uint8_t id;
+
+    uint8_t dz_trigger_l;
+    uint8_t dz_trigger_r;
+
+    uint8_t dz_joystick_l;
+    uint8_t dz_joystick_r;
+
+    uint8_t invert_ly;
+    uint8_t invert_ry;
+
+    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) == 46, "UserProfile struct size mismatch");
+#pragma pack(pop)
+
+#endif // _USER_PROFILE_H_

Firmware/ESP32/main/UserSettings/UserSettings.cpp

@@ -0,0 +1,267 @@
+#include <cstring>
+#include <array>
+#include <string>
+#include <nvs_flash.h>
+#include <nvs.h>
+#include <esp_err.h>
+#include <esp_log.h>
+
+#include "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");
+}
+
+//Checks for first boot and initializes user profiles, call before tusb is inited.
+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;
+    }
+
+    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, const UserProfile& profile)
+{
+    if (index > MAX_GAMEPADS || profile.id < 1 || profile.id > MAX_PROFILES)
+    {
+        return;
+    }
+
+    if (nvs_helper_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile)) == ESP_OK)
+    {
+        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, const 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 profile;
+    }
+
+    return UserProfile();
+}
+
+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, const UserProfile& profile);
+    void store_profile_and_driver_type(DeviceDriverType new_driver_type, uint8_t index, const 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 = 0x82;
+
+    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/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

@@ -1381,20 +1381,17 @@ CONFIG_HEAP_TRACING_OFF=y
 #
 # Log output
 #
-CONFIG_LOG_DEFAULT_LEVEL_NONE=y
+# CONFIG_LOG_DEFAULT_LEVEL_NONE is not set
 # CONFIG_LOG_DEFAULT_LEVEL_ERROR is not set
 # CONFIG_LOG_DEFAULT_LEVEL_WARN is not set
-# CONFIG_LOG_DEFAULT_LEVEL_INFO is not set
+CONFIG_LOG_DEFAULT_LEVEL_INFO=y
 # CONFIG_LOG_DEFAULT_LEVEL_DEBUG is not set
 # CONFIG_LOG_DEFAULT_LEVEL_VERBOSE is not set
-CONFIG_LOG_DEFAULT_LEVEL=0
+CONFIG_LOG_DEFAULT_LEVEL=3
 CONFIG_LOG_MAXIMUM_EQUALS_DEFAULT=y
-# CONFIG_LOG_MAXIMUM_LEVEL_ERROR is not set
-# CONFIG_LOG_MAXIMUM_LEVEL_WARN is not set
-# CONFIG_LOG_MAXIMUM_LEVEL_INFO is not set
 # CONFIG_LOG_MAXIMUM_LEVEL_DEBUG is not set
 # CONFIG_LOG_MAXIMUM_LEVEL_VERBOSE is not set
-CONFIG_LOG_MAXIMUM_LEVEL=0
+CONFIG_LOG_MAXIMUM_LEVEL=3
 CONFIG_LOG_COLORS=y
 CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y
 # CONFIG_LOG_TIMESTAMP_SOURCE_SYSTEM is not set
@@ -1961,6 +1958,11 @@ CONFIG_WIFI_PROV_STA_ALL_CHANNEL_SCAN=y
 #
 # OGXMini Options
 #
+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
@@ -1978,11 +1980,11 @@ CONFIG_BLUEPAD32_PLATFORM_CUSTOM=y
 # CONFIG_BLUEPAD32_PLATFORM_MAKEFILE is not set
 CONFIG_BLUEPAD32_MAX_DEVICES=1
 CONFIG_BLUEPAD32_GAP_SECURITY=y
-CONFIG_BLUEPAD32_LOG_LEVEL_NONE=y
+# CONFIG_BLUEPAD32_LOG_LEVEL_NONE is not set
 # CONFIG_BLUEPAD32_LOG_LEVEL_ERROR is not set
-# CONFIG_BLUEPAD32_LOG_LEVEL_INFO is not set
+CONFIG_BLUEPAD32_LOG_LEVEL_INFO=y
 # CONFIG_BLUEPAD32_LOG_LEVEL_DEBUG is not set
-CONFIG_BLUEPAD32_LOG_LEVEL=0
+CONFIG_BLUEPAD32_LOG_LEVEL=2
 # CONFIG_BLUEPAD32_USB_CONSOLE_ENABLE is not set
 CONFIG_BLUEPAD32_ENABLE_BLE_BY_DEFAULT=y
 CONFIG_BLUEPAD32_MAX_ALLOWLIST=4

Firmware/FWDefines.cmake

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

Firmware/RP2040/.vscode/settings.json

@@ -1,15 +1,11 @@
 {
     "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_PICO2",
+        "-DOGXM_BOARD=PI_PICOW",
         "-DMAX_GAMEPADS=1"
     ],
+    
     "files.associations": {
         "array": "cpp",
         "atomic": "cpp",

Firmware/RP2040/CMakeLists.txt

@@ -1,25 +1,26 @@
 cmake_minimum_required(VERSION 3.13)
 
-set(FW_NAME "OGX-Mini")
-set(FW_VERSION "v1.0.0a2")
+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}/init_submodules.cmake)
-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)
 
+get_pico_sdk(${EXTERNAL_DIR} ${PICOSDK_VERSION_TAG})
 init_git_submodules(${EXTERNAL_DIR})
 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)
@@ -37,7 +38,13 @@ set(SOURCES_BOARD
 
     ${SRC}/TaskQueue/TaskQueue.cpp
 
+    ${SRC}/Board/ogxm_log.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_esp32.cpp
+    ${SRC}/Board/board_api_private/board_api_usbh.cpp
     
     ${SRC}/UserSettings/UserSettings.cpp
     ${SRC}/UserSettings/UserProfile.cpp
@@ -97,6 +104,16 @@ elseif (OGXM_BOARD STREQUAL "PI_PICO2")
     set(PICO_PLATFORM rp2350)
     set(FLASH_SIZE_MB 4)
 
+elseif(OGXM_BOARD STREQUAL "PI_PICOW")
+    set(EN_BLUETOOTH TRUE)
+    set(PICO_BOARD pico_w)
+
+elseif(OGXM_BOARD STREQUAL "PI_PICO2W")
+    set(EN_BLUETOOTH TRUE)
+    set(PICO_BOARD pico2_w)
+    set(PICO_PLATFORM rp2350)
+    set(FLASH_SIZE_MB 4)
+
 elseif(OGXM_BOARD STREQUAL "ADA_FEATHER")
     set(EN_USB_HOST TRUE)
     set(EN_RGB TRUE)
@@ -115,11 +132,7 @@ elseif(OGXM_BOARD STREQUAL "EXTERNAL_4CH")
     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 "PICO_ESP32")
     set(EN_ESP32 TRUE)
     set(EN_UART_BRIDGE TRUE)
 
@@ -168,8 +181,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}
@@ -229,8 +246,10 @@ if(EN_UART_BRIDGE)
     )
 endif()
 
-add_compile_definitions(PICO_FLASH_SIZE_BYTES=${FLASH_SIZE_MB}*1024*1024)
+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)
@@ -246,8 +265,11 @@ 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)
@@ -259,16 +281,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(CFG_TUSB_DEBUG=2)
     add_compile_definitions(OGXM_DEBUG=1)
+
     target_compile_options(${FW_NAME} PRIVATE
         -Wall                # Enable most warnings
         -Wextra              # Enable extra warnings
@@ -277,12 +315,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

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,304 @@
+#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 = 18;
+
+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 START_UPDATE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
+    static constexpr uint16_t COMMIT_UPDATE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
+
+    static constexpr uint16_t SETUP_PACKET  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT1   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789041_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT2   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789042_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT3   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789043_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
+{
+    uint8_t max_gamepads{1};
+    uint8_t index{0};
+    uint8_t device_type{0};
+    uint8_t profile_id{1};
+};
+static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
+#pragma pack(pop)
+
+SetupPacket setup_packet_;
+
+static int verify_write(const uint16_t buffer_size, const uint16_t expected_size, bool pending_write = false, bool expected_pending_write = false)
+{
+    if (buffer_size != expected_size)
+    {
+        return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
+    }
+    if (pending_write != expected_pending_write)
+    {
+        return ATT_ERROR_WRITE_NOT_PERMITTED;
+    }
+    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)
+{
+    static UserProfile profile;
+    SetupPacket setup_packet_resp;
+    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::SETUP_PACKET:
+            if (buffer)
+            {
+                setup_packet_resp.max_gamepads = static_cast<uint8_t>(MAX_GAMEPADS);
+                setup_packet_resp.index = setup_packet_.index;
+                setup_packet_resp.device_type = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
+                //App has already written a setup packet with the player index
+                setup_packet_resp.profile_id = UserSettings::get_instance().get_active_profile_id(setup_packet_.index);
+
+                std::memcpy(buffer, &setup_packet_resp, sizeof(setup_packet_resp));
+            }
+            return sizeof(setup_packet_);
+
+        case Handle::PROFILE_PT1:
+            if (buffer)
+            {
+                //App has already written the profile id it wants to the setup packet
+                profile = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
+                std::memcpy(buffer, &profile, PACKET_LEN_MAX);
+            }
+            return PACKET_LEN_MAX;
+
+        case Handle::PROFILE_PT2:
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX, PACKET_LEN_MAX);
+            }
+            return PACKET_LEN_MAX;
+
+        case Handle::PROFILE_PT3:
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX * 2, sizeof(UserProfile) - PACKET_LEN_MAX * 2);
+            }
+            return sizeof(UserProfile) - PACKET_LEN_MAX * 2;
+
+        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)
+{
+    static UserProfile temp_profile;
+    static DeviceDriverType temp_driver_type = DeviceDriverType::NONE;
+    static bool pending_write = false;
+
+    int ret = 0;
+
+    switch (att_handle)
+    {
+        case Handle::START_UPDATE:
+            pending_write = true;
+            break;
+
+        case Handle::SETUP_PACKET:
+            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
+            {
+                break;
+            }
+
+            std::memcpy(&setup_packet_, buffer, buffer_size);
+            if (setup_packet_.index >= MAX_GAMEPADS)
+            {
+                setup_packet_.index = 0;
+                ret = ATT_ERROR_OUT_OF_RANGE;
+            }
+            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
+            {
+                setup_packet_.profile_id = 1;
+                ret = ATT_ERROR_OUT_OF_RANGE;
+            }
+            if (ret)
+            {
+                break;
+            }
+
+            if (pending_write)
+            {
+                //App wants to store a new device driver type
+                temp_driver_type = static_cast<DeviceDriverType>(setup_packet_.device_type);
+            }
+            break;
+
+        case Handle::PROFILE_PT1:
+            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(&temp_profile, buffer, buffer_size);
+            break;
+
+        case Handle::PROFILE_PT2:
+            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX, buffer, buffer_size);
+            break;
+
+        case Handle::PROFILE_PT3:
+            if ((ret = verify_write(buffer_size, sizeof(UserProfile) - PACKET_LEN_MAX * 2, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX * 2, buffer, buffer_size);
+            break;
+
+        case Handle::COMMIT_UPDATE:
+            if ((ret = verify_write(0, 0, pending_write, true)) != 0)
+            {
+                break;
+            }
+
+            //Delay until after we've returned
+            queue_disconnect(connection_handle, 500);
+
+            //We don't want to write to the flash from here, it'll reset core1 before writing, queue the task on core0
+            if (temp_driver_type != DeviceDriverType::NONE && temp_driver_type != UserSettings::get_instance().get_current_driver())
+            {
+                TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
+                []{
+                    UserSettings::get_instance().store_profile_and_driver_type(temp_driver_type, setup_packet_.index, temp_profile);
+                });
+            }
+            else
+            {
+                TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
+                []{
+                    UserSettings::get_instance().store_profile(setup_packet_.index, temp_profile);
+                });
+            }
+            
+            temp_driver_type = DeviceDriverType::NONE;
+            pending_write = false;
+            break;
+
+        default:
+            break;
+    }
+    return ret;
+}
+
+void init_server()
+{
+    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,11 @@
+#ifndef BLE_SERVER_H
+#define BLE_SERVER_H
+
+#include <cstdint>
+
+namespace BLEServer 
+{
+    void init_server();
+}
+
+#endif // BLE_SERVER_H

Firmware/RP2040/src/BLEServer/att_delayed_response.gatt

@@ -0,0 +1,32 @@
+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::UPDATE_START
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789030, WRITE | DYNAMIC,
+
+// Handle::UPDATE_COMMIT
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789031, WRITE | DYNAMIC,
+
+// Handle::SETUP_PACKET
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789040, READ | WRITE | DYNAMIC,
+
+// Handle::PROFILE_PT1
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789041, READ | WRITE | DYNAMIC,
+
+// Handle::PROFILE_PT2
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789042, READ | WRITE | DYNAMIC,
+
+// Handle::PROFILE_PT3
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789043, READ | WRITE | DYNAMIC,

Firmware/RP2040/src/Bluepad32/Bluepad32.cpp

@@ -9,7 +9,9 @@
 
 #include "sdkconfig.h"
 #include "Bluepad32/Bluepad32.h"
+#include "BLEServer/BLEServer.h"
 #include "Board/board_api.h"
+#include "Board/ogxm_log.h"
 
 #ifndef CONFIG_BLUEPAD32_PLATFORM_CUSTOM
     #error "Pico W must use BLUEPAD32_PLATFORM_CUSTOM"
@@ -124,14 +126,7 @@ static void check_led_cb(btstack_timer_source *ts)
 
 static void init(int argc, const char** arg_V)
 {
-    if (!led_timer_set_)
-    {
-        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_);
-    }
+
 }
 
 static void init_complete_cb(void) 
@@ -166,8 +161,9 @@ static void device_disconnected_cb(uni_hid_device_t* device)
     }
 
     bt_devices_[idx].connected = false;
+    bt_devices_[idx].gamepad->reset_pad_in();
 
-    if (!led_timer_set_)
+    if (!led_timer_set_ && !any_connected())
     {
         led_timer_set_ = true;
         led_timer_.process = check_led_cb;
@@ -271,13 +267,13 @@ 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.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));
 
-    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.joystick_lx = gamepad->scale_joystick_lx<10>(uni_gp->axis_x);
+    gp_in.joystick_ly = gamepad->scale_joystick_ly<10>(uni_gp->axis_y);
+    gp_in.joystick_rx = gamepad->scale_joystick_rx<10>(uni_gp->axis_rx);
+    gp_in.joystick_ry = gamepad->scale_joystick_ry<10>(uni_gp->axis_ry);
 
     gamepad->set_pad_in(gp_in);
 }
@@ -314,9 +310,17 @@ void run_task(Gamepad (&gamepads)[MAX_GAMEPADS])
         bt_devices_[i].gamepad = &gamepads[i];
     }
 
+    BLEServer::init_server();
+
     uni_platform_set_custom(get_driver());
     uni_init(0, nullptr);
 
+    led_timer_set_ = true;
+    led_timer_.process = check_led_cb;
+    led_timer_.context = nullptr;
+    btstack_run_loop_set_timer(&led_timer_, LED_CHECK_TIME_MS);
+    btstack_run_loop_add_timer(&led_timer_);
+
     btstack_run_loop_execute();
 }
 

Firmware/RP2040/src/Board/board_api.cpp

@@ -1,178 +1,90 @@
 #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 "Board/board_api.h"
-#include "OGXMini/Debug.h"
+#include "tusb.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/board_api.h"
+#include "Board/ogxm_log.h"
+#include "Board/board_api_private/board_api_private.h"
 
 namespace board_api {
 
-bool inited_ = false;
 mutex_t gpio_mutex_;
 
-void init_vcc_en_pin_unsafe()
+bool esp32::uart_bridge_mode()
 {
-#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 
+    bool ret = false;
+    if (board_api_esp32::uart_bridge_mode)
+    {
+        mutex_enter_blocking(&gpio_mutex_);
+        ret = board_api_esp32::uart_bridge_mode();
+        mutex_exit(&gpio_mutex_);
+    }
+    return ret;
 }
 
-void init_rgb_unsafe()
+void esp32::enter_programming_mode()
 {
-#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)
+    if (board_api_esp32::enter_programming_mode)
+    {
+        mutex_enter_blocking(&gpio_mutex_);
+        board_api_esp32::enter_programming_mode();
+        mutex_exit(&gpio_mutex_);
+    }
 }
 
-void init_led_indicator_unsafe()
+void esp32::reset()
 {
-#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)
+    if (board_api_esp32::reset)
+    {
+        mutex_enter_blocking(&gpio_mutex_);
+        board_api_esp32::reset();
+        mutex_exit(&gpio_mutex_);
+    }
 }
 
-void init_esp32_io_unsafe()
+bool usb::host_connected()
 {
-#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)
+    if (board_api_usbh::host_connected)
+    {
+        return board_api_usbh::host_connected();
+    }
+    return false;
 }
 
-void init_uart_bridge_io_unsafe()
+//Only call this from core0
+void usb::disconnect_all()
 {
-#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)
+    OGXM_LOG("Disconnecting USB and resetting Core1\n");
+    tud_disconnect();
+    sleep_ms(300);
+    multicore_reset_core1();
+    sleep_ms(300);
 }
 
+// If using PicoW, only use this method from the core running btstack and after you've called init_bluetooth
 void set_led(bool state)
 {
     mutex_enter_blocking(&gpio_mutex_);
 
-    if (!inited_)
+    if (board_api_led::set_led)
     {
-        mutex_exit(&gpio_mutex_);
-        return;
+        board_api_led::set_led(state);
     }
-
-#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_);
-}
-
-#if defined(CONFIG_EN_UART_BRIDGE)
-bool uart_bridge_mode()
-{
-    if (!inited_)
+    if (board_api_bt::set_led)
     {
-        return false;
+        board_api_bt::set_led(state);
     }
-
-    bool mode = false;
-    mutex_enter_blocking(&gpio_mutex_);
-
-    gpio_pull_up(MODE_SEL_PIN);
-    if (gpio_get(MODE_SEL_PIN) == 0) 
+    if (board_api_rgb::set_led)
     {
-        mode = true;
+        board_api_rgb::set_led(state ? 0x00 : 0xFF, state ? 0xFF : 0x00, 0x00);
     }
 
     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);
-    sleep_ms(500);
-    gpio_put(ESP_RST_PIN, 1);
-    sleep_ms(250);
-}
-
-void reset_esp32() 
-{
-    if (!inited_)
-    {
-        return;
-    }
-    mutex_enter_blocking(&gpio_mutex_);
-    reset_esp32_unsafe();
-    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()
 {
@@ -180,6 +92,8 @@ void reboot()
     #define AIRCR_SYSRESETREQ (1 << 2)
     #define AIRCR_VECTKEY (0x5FA << 16)
 
+    OGXM_LOG("Rebooting\n");
+
     AIRCR_REG = AIRCR_VECTKEY | AIRCR_SYSRESETREQ;
     while(1);
 }
@@ -189,13 +103,18 @@ uint32_t ms_since_boot()
     return to_ms_since_boot(get_absolute_time());
 }
 
+//Call after board is initialized
+void init_bluetooth()
+{
+    if (board_api_bt::init)
+    {
+        board_api_bt::init();
+    }
+}
+
+//Call on core0 before any other method
 void init_board()
 {
-    if (inited_)
-    {
-        return;
-    }
-
     if (!set_sys_clock_khz(SYSCLOCK_KHZ, true))
     {
         if (!set_sys_clock_khz((SYSCLOCK_KHZ / 2), true))
@@ -209,25 +128,32 @@ void init_board()
     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_esp32::init)
+        {
+            board_api_esp32::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) // cyw43_arch needs to be inited from core1 first
-    set_led(false);
-#endif
-
     OGXM_LOG("Board initialized\n");
 }
 

Firmware/RP2040/src/Board/board_api.h

@@ -7,14 +7,23 @@
 namespace board_api
 {
     void init_board();
-    void set_led(bool state);
+    void init_bluetooth();
     void reboot();
-
-    bool uart_bridge_mode();
-    void reset_esp32();
-    void enter_esp32_prog_mode();
-
+    void set_led(bool state);
     uint32_t ms_since_boot();
+
+    namespace usb
+    {
+        bool host_connected();
+        void disconnect_all();
+    }
+
+    namespace esp32
+    {
+        bool uart_bridge_mode();
+        void reset();
+        void enter_programming_mode();
+    }
 }
 
 #endif // _OGXM_BOARD_API_H_

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

@@ -0,0 +1,29 @@
+#include "board_config.h"
+#if defined(CONFIG_EN_BLUETOOTH)
+
+#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 {
+
+void init()
+{
+    if (cyw43_arch_init() != 0)
+    {  
+        panic("CYW43 init failed");
+    }  
+}
+
+void set_led(bool state)
+{
+    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_esp32.cpp

@@ -0,0 +1,54 @@
+#include "board_config.h"
+#if defined(CONFIG_EN_ESP32)
+
+#include <pico/stdlib.h>
+#include <hardware/gpio.h>
+
+#include "Board/board_api_private/board_api_private.h"
+
+namespace board_api_esp32 {
+
+bool uart_bridge_mode()
+{
+    gpio_pull_up(MODE_SEL_PIN);
+    return (gpio_get(MODE_SEL_PIN) == 0);
+}  
+
+void reset() 
+{
+    gpio_put(ESP_RST_PIN, 0);
+    sleep_ms(500);
+    gpio_put(ESP_RST_PIN, 1);
+    sleep_ms(250);
+}
+
+void 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 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);
+}
+
+} // namespace board_api_esp32
+
+#endif // defined(CONFIG_EN_ESP32)

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

@@ -0,0 +1,22 @@
+#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,41 @@
+#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_esp32
+{
+    void init() __attribute__((weak));
+    bool uart_bridge_mode() __attribute__((weak));
+    void reset() __attribute__((weak));
+    void enter_programming_mode() __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,36 @@
+#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,70 @@
+#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/ogxm_log.cpp

@@ -0,0 +1,91 @@
+#include "board_config.h"
+#if defined(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');
+    for (uint16_t i = 0; i < size; ++i)
+    {
+        hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
+    }
+    log(hex_stream.str());
+}
+
+} // namespace ogxm_log
+
+#endif // defined(OGXM_DEBUG)

Firmware/RP2040/src/Board/ogxm_log.h

@@ -0,0 +1,61 @@
+#ifndef BOARD_API_LOG_H
+#define BOARD_API_LOG_H
+
+#include <cstdint>
+
+#include "board_config.h"
+#if defined(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 // 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 // OGXM_DEBUG
+
+#endif // BOARD_API_LOG_H

Firmware/RP2040/src/Descriptors/PS3.h

@@ -5,8 +5,6 @@
 #include <cstring>
 #include <random>
 
-#include "tusb.h"
-
 namespace PS3 
 {
 	static constexpr uint8_t MAGIC_BYTES[8] = { 0x21, 0x26, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00 };
@@ -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,19 +200,18 @@ namespace PS3
 
 		BTInfo()
 		{
-			std::memset(this, 0, sizeof(BTInfo));
 			std::memcpy(device_address, DEFAULT_BT_INFO_HEADER, sizeof(DEFAULT_BT_INFO_HEADER));
 
             std::mt19937 gen(12345);
             std::uniform_int_distribution<uint8_t> dist(0, 0xFF);
 
-			for (uint8_t addr = 0; addr < 3; addr++) 
+			for (uint8_t i = 4; i < sizeof(device_address); i++) 
 			{
-				device_address[4 + addr] = dist(gen);
+				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] = dist(gen);
+				host_address[i] = dist(gen);
 			}
 		}
 	};

Firmware/RP2040/src/Gamepad.h

@@ -8,8 +8,9 @@
 #include <array>
 #include <pico/mutex.h>
 
-#include "Scale.h"
+#include "Range.h"
 #include "UserSettings/UserProfile.h"
+#include "Board/ogxm_log.h"
 
 class Gamepad 
 {
@@ -198,15 +199,6 @@ public:
 
     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);
@@ -251,6 +243,226 @@ public:
         mutex_exit(&chatpad_in_mutex_);
     }
 
+    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
+        <bits> param is optional, used for scaling specific bit values as opposed 
+        to full range values. */
+    template <uint8_t bits = 0, typename T>
+    inline int16_t scale_joystick_rx(T value) const
+    {
+        int16_t joy_value = 0;
+        if constexpr (bits > 0)
+        {
+            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_value = Range::scale<int16_t>(value);
+        }
+        else
+        {
+            joy_value = value;
+        }
+        if (joy_value > dz_.joystick_r_pos)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            dz_.joystick_r_pos, 
+                            Range::MAX<int16_t>, 
+                            Range::MID<int16_t>, 
+                            Range::MAX<int16_t>);
+        }
+        else if (joy_value < dz_.joystick_r_neg)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            Range::MIN<int16_t>, 
+                            dz_.joystick_r_neg, 
+                            Range::MIN<int16_t>, 
+                            Range::MID<int16_t>);
+        }
+        else
+        {
+            joy_value = 0;
+        }
+        return joy_value;
+    }
+
+    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
+        <bits> param is optional, used for scaling specific bit values as opposed 
+        to full range values. */
+    template <uint8_t bits = 0, typename T>
+    inline int16_t scale_joystick_ry(T value, bool invert = false) const
+    {
+        int16_t joy_value = 0;
+        if constexpr (bits > 0)
+        {
+            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_value = Range::scale<int16_t>(value);
+        }
+        else
+        {
+            joy_value = value;
+        }
+        if (joy_value > dz_.joystick_r_pos)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            dz_.joystick_r_pos, 
+                            Range::MAX<int16_t>, 
+                            Range::MID<int16_t>, 
+                            Range::MAX<int16_t>);
+        }
+        else if (joy_value < dz_.joystick_r_neg)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            Range::MIN<int16_t>, 
+                            dz_.joystick_r_neg, 
+                            Range::MIN<int16_t>, 
+                            Range::MID<int16_t>);
+        }
+        else
+        {
+            joy_value = 0;
+        }
+        return profile_invert_ry_ ? (invert ? joy_value : Range::invert(joy_value)) : (invert ? Range::invert(joy_value) : joy_value);
+    }
+
+    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
+        <bits> param is optional, used for scaling specific bit values as opposed 
+        to full range values. */
+    template <uint8_t bits = 0, typename T>
+    inline int16_t scale_joystick_lx(T value) const
+    {
+        int16_t joy_value = 0;
+        if constexpr (bits > 0)
+        {
+            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_value = Range::scale<int16_t>(value);
+        }
+        else
+        {
+            joy_value = value;
+        }
+        if (joy_value > dz_.joystick_l_pos)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            dz_.joystick_l_pos, 
+                            Range::MAX<int16_t>, 
+                            Range::MID<int16_t>, 
+                            Range::MAX<int16_t>);
+        }
+        else if (joy_value < dz_.joystick_l_neg)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            Range::MIN<int16_t>, 
+                            dz_.joystick_l_neg, 
+                            Range::MIN<int16_t>, 
+                            Range::MID<int16_t>);
+        }
+        else
+        {
+            joy_value = 0;
+        }
+        return joy_value;
+    }
+
+    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
+        <bits> param is optional, used for scaling specific bit values as opposed 
+        to full range values. */
+    template <uint8_t bits = 0, typename T>
+    inline int16_t scale_joystick_ly(T value, bool invert = false) const
+    {
+        int16_t joy_value = 0;
+        if constexpr (bits > 0)
+        {
+            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_value = Range::scale<int16_t>(value);
+        }
+        else
+        {
+            joy_value = value;
+        }
+        if (joy_value > dz_.joystick_l_pos)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            dz_.joystick_l_pos, 
+                            Range::MAX<int16_t>, 
+                            Range::MID<int16_t>, 
+                            Range::MAX<int16_t>);
+        }
+        else if (joy_value < dz_.joystick_l_neg)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            Range::MIN<int16_t>, 
+                            dz_.joystick_l_neg, 
+                            Range::MIN<int16_t>, 
+                            Range::MID<int16_t>);
+        }
+        else
+        {
+            joy_value = 0;
+        }
+        return profile_invert_ly_ ? (invert ? joy_value : Range::invert(joy_value)) : (invert ? Range::invert(joy_value) : joy_value);
+    }
+
+    /*  Get trigger value adjusted for deadzones, scaling, and inversion. 
+        <bits> param is optional, used for scaling speicifc bit values 
+        as opposed to full range values */
+    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 trigger_value > dz_.trigger_l ? Range::scale(trigger_value, dz_.trigger_l, Range::MAX<uint8_t>) : 0;
+    }
+
+    /*  Get trigger value adjusted for deadzones, scaling, and inversion. 
+        <bits> param is optional, used for scaling speicifc bit values 
+        as opposed to full range values */
+    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 trigger_value > dz_.trigger_l ? Range::scale(trigger_value, dz_.trigger_l, Range::MAX<uint8_t>) : 0;
+    }
+
 private:
     mutex_t pad_in_mutex_;
     mutex_t pad_out_mutex_;
@@ -330,10 +542,22 @@ private:
         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);
+        OGXM_LOG("dz_.trigger_l: %d\n", dz_.trigger_l);
+        OGXM_LOG("dz_.trigger_r: %d\n", dz_.trigger_r);
+        OGXM_LOG("profile.dz_joystick_l: %d\n", profile.dz_joystick_l);
+        OGXM_LOG("profile.dz_joystick_r: %d\n", profile.dz_joystick_r);
+
+        dz_.joystick_l_pos = profile.dz_joystick_l ? Range::scale<int16_t>(static_cast<int8_t>(profile.dz_joystick_l / 2)) : 0;
+        dz_.joystick_l_neg = Range::invert(dz_.joystick_l_pos);
+        dz_.joystick_r_pos = profile.dz_joystick_r ? Range::scale<int16_t>(static_cast<int8_t>(profile.dz_joystick_r / 2)) : 0;
+        dz_.joystick_r_neg = Range::invert(dz_.joystick_r_pos);
+
+        OGXM_LOG("dz_.trigger_l: %d\n", dz_.trigger_l);
+        OGXM_LOG("dz_.trigger_r: %d\n", dz_.trigger_r);
+        OGXM_LOG("dz_.joystick_l_pos: %d\n", dz_.joystick_l_pos);
+        OGXM_LOG("dz_.joystick_l_neg: %d\n", dz_.joystick_l_neg);
+        OGXM_LOG("dz_.joystick_r_pos: %d\n", dz_.joystick_r_pos);
+        OGXM_LOG("dz_.joystick_r_neg: %d\n", dz_.joystick_r_neg);
     }
 };
 

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

@@ -6,72 +6,56 @@
 
 #include "board_config.h"
 #include "Gamepad.h"
+#include "USBHost/HostDriver/HostDriverTypes.h"
 
-#include "USBHost/HostDriver/HostDriver.h"
-
+//Run on core0
 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) {};
+    virtual void notify_tuh(bool mounted, HostDriverType host_type = HostDriverType::UNKNOWN) = 0;
+    virtual void notify_xbox360w(bool connected, 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 };
+    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;
-        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);
-        }
+        uint8_t packet_len{sizeof(PacketIn)};
+        PacketID packet_id{PacketID::PAD};
+        Gamepad::PadIn pad_in{Gamepad::PadIn()};
+        Gamepad::ChatpadIn chatpad_in{0};
+        std::array<uint8_t, 4> reserved{0};
     };
-    static_assert(sizeof(PacketIn) == 28, "I2CDriver::PacketIn is misaligned");
+    static_assert(sizeof(PacketIn) == 32, "I2CDriver::PacketIn is misaligned");
+    static_assert((sizeof(PacketIn) % 8) == 0, "I2CDriver::PacketIn is not a multiple of 8");
 
     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);
-        }
+        uint8_t packet_len{sizeof(PacketOut)};
+        PacketID packet_id{PacketID::PAD};
+        Gamepad::PadOut pad_out{Gamepad::PadOut()};
+        std::array<uint8_t, 4> reserved{0};
     };
-    static_assert(sizeof(PacketOut) == 4, "I2CDriver::PacketOut is misaligned");
+    static_assert(sizeof(PacketOut) == 8, "I2CDriver::PacketOut is misaligned");
 
-    struct PacketStatus
+    struct PacketCMD
     {
-        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);
-        }
+        uint8_t packet_len{sizeof(PacketCMD)};
+        PacketID packet_id{PacketID::COMMAND};
+        Command command{Command::UNKNOWN};
+        Status status{Status::UNKNOWN};
+        std::array<uint8_t, 4> reserved{0};
     };
-    static_assert(sizeof(PacketStatus) == 3, "I2CDriver::PacketStatus is misaligned");
+    static_assert(sizeof(PacketCMD) == 8, "I2CDriver::PacketCMD is misaligned");
 #pragma pack(pop)
 
-    static constexpr size_t MAX_PACKET_SIZE = std::max(sizeof(PacketStatus), std::max(sizeof(PacketIn), sizeof(PacketOut)));
+    static constexpr size_t MAX_PACKET_SIZE = sizeof(PacketIn);
 };
 
 #endif // I2C_DRIVER_4CH_H

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

@@ -4,6 +4,7 @@
 #include <cstdint>
 #include <memory>
 #include <atomic>   
+#include <hardware/gpio.h>
 
 #include "board_config.h"
 #include "Gamepad.h"

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

@@ -1,14 +1,9 @@
 #include <cstring>
+#include <hardware/gpio.h>
 
 #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);
@@ -25,17 +20,6 @@ void I2CMaster::initialize(uint8_t address)
     {
         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])
@@ -44,69 +28,86 @@ void I2CMaster::process(Gamepad (&gamepads)[MAX_GAMEPADS])
     {
         Slave& slave = slaves_[i];
 
-        if (slave.status != SlaveStatus::READY || !slave_detected(slave.address))
+        if (!slave.enabled.load() || !slave_detected(slave.address))
         {
             continue;
         }
-        if (send_packet_in(slave, gamepads[i + 1]))
+
+        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)))
         {
-            get_packet_out(slave, gamepads[i + 1]);
+            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_us(100);
     }
 }
 
-void I2CMaster::notify_tuh_mounted(HostDriver::Type host_type)
+void I2CMaster::notify_tuh(bool mounted, HostDriverType host_type)
 {
-    if (host_type == HostDriver::Type::XBOX360W)
+    if (host_type != HostDriverType::XBOX360W)
     {
-        i2c_enabled_.store(true);
+        return;
     }
-}
 
-void I2CMaster::notify_tuh_unmounted(HostDriver::Type host_type)
-{
-    if (host_type == HostDriver::Type::XBOX360W)
+    i2c_enabled_.store(mounted);
+
+    if (!mounted)
     {
-        i2c_enabled_.store(false);
-
-        TaskQueue::Core0::queue_task([this]()
-        {
-            for (auto& slave : slaves_)
+        //Called from core1 so queue on core0
+        TaskQueue::Core0::queue_task(
+            [this]()
             {
-                notify_disable(slave);
-            }
-        });
+                for (auto& slave : slaves_)
+                {
+                    notify_disable(slave.address);
+                }
+            });
     }
 }
 
-void I2CMaster::notify_xbox360w_connected(uint8_t idx)
+void I2CMaster::notify_xbox360w(bool 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);
-    });
-}
+    slaves_[idx - 1].enabled.store(connected);
 
-void I2CMaster::notify_xbox360w_disconnected(uint8_t idx)
-{
-    if (idx < 1 || idx >= MAX_GAMEPADS)
+    if (!connected)
     {
-        return;
+        //Called from core1 so queue on core0
+        TaskQueue::Core0::queue_task(
+            [this]()
+            {
+                for (auto& slave : slaves_)
+                {
+                    notify_disable(slave.address);
+                }
+            });
     }
-    slaves_[idx - 1].enabled.store(false);
-
-    TaskQueue::Core0::queue_task([this, idx]()
-    {
-        notify_disable(slaves_[idx - 1]);
-    });
 }
 
 bool I2CMaster::slave_detected(uint8_t address)
@@ -116,95 +117,30 @@ bool I2CMaster::slave_detected(uint8_t address)
     return (result >= 0);
 }
 
-void I2CMaster::check_slave_status(Slave& slave)
+void I2CMaster::notify_disable(uint8_t address)
 {
-    if (!slave_detected(slave.address))
+    if (!slave_detected(address))
     {
-        slave.status = SlaveStatus::NC;
         return;
     }
-    if (slave.enabled.load())
+
+    int retries = 10;
+
+    while (retries--)
     {
-        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--)
+        PacketCMD packet_cmd;
+        packet_cmd.packet_id = PacketID::COMMAND;
+        packet_cmd.command = Command::DISABLE;
+        if (write_blocking(address, &packet_cmd, sizeof(PacketCMD)))
         {
-            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))
+            if (read_blocking(address, &packet_cmd, sizeof(PacketCMD)))
             {
-                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);
+                if (packet_cmd.status == Status::OK)
+                {
+                    break;
+                }
             }
-
-            sleep_ms(1);
         }
-        return success;
+        sleep_ms(1);
     }
-    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

@@ -4,8 +4,6 @@
 #include <cstdint>
 #include <atomic>
 #include <array>
-
-#include <hardware/gpio.h>
 #include <hardware/i2c.h>
 
 #include "board_config.h"
@@ -15,42 +13,38 @@
 class I2CMaster : public I2CDriver
 {
 public:
-    ~I2CMaster() override;
+    ~I2CMaster() = 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;
-    void notify_xbox360w_connected(uint8_t idx) override;
-    void notify_xbox360w_disconnected(uint8_t idx) override;
+    void notify_tuh(bool mounted, HostDriverType host_type) override;
+    void notify_xbox360w(bool connected, uint8_t idx) override;
 
 private:
     struct Slave
     {
         uint8_t address{0xFF};
-        SlaveStatus status{SlaveStatus::NC};
+        Status status{Status::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_; 
 
+    void notify_disable(uint8_t address);
+
     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);
+    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));
+    }
 };
 
 #endif // I2C_MASTER_4CH_H

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

@@ -1,15 +1,16 @@
 #include <cstring>
 #include <array>
+#include <hardware/gpio.h>
 
-#include "USBHost/HostManager.h"
 #include "OGXMini/OGXMini.h"
 #include "I2CDriver/4Channel/I2CSlave.h"
+#include "TaskQueue/TaskQueue.h"
 
-I2CSlave* I2CSlave::this_instance_ = nullptr;
+I2CSlave* I2CSlave::instance_ = nullptr;
 
 void I2CSlave::initialize(uint8_t address) 
 {
-    this_instance_ = this;
+    instance_ = this;
 
     i2c_init(I2C_PORT, I2C_BAUDRATE);
 
@@ -24,35 +25,28 @@ void I2CSlave::initialize(uint8_t address)
     i2c_slave_init(I2C_PORT, address, &slave_handler);
 }
 
-void I2CSlave::notify_tuh_mounted(HostDriver::Type host_type)
+void I2CSlave::notify_tuh(bool mounted, HostDriverType host_type)
 {
-    i2c_disabled_.store(true);
-}
-
-void I2CSlave::notify_tuh_unmounted(HostDriver::Type host_type)
-{
-    i2c_disabled_.store(false);
+    tuh_mounted_.store(mounted);
 }
 
 void I2CSlave::process(Gamepad (&gamepads)[MAX_GAMEPADS])
 {
-    if (i2c_disabled_.load())
+    if (tuh_mounted_.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_pad_in_.load())
     {
-        if (new_packet_in_)
-        {
-            new_packet_in_ = false;
-            gamepads[0].set_pad_in(packet_in_.pad_in);
-        }
+        new_pad_in_.store(false);
+        gamepads[0].set_pad_in(packet_in_.pad_in);
+        gamepads[0].set_chatpad_in(packet_in_.chatpad_in);
+    }
     
-        if (gamepads[0].new_pad_out())
-        {
-            packet_out_.pad_out = gamepads[0].get_pad_out();
-        } 
+    if (gamepads[0].new_pad_out())
+    {
+        packet_out_.pad_out = gamepads[0].get_pad_out();
     } 
 }
 
@@ -66,24 +60,14 @@ I2CSlave::PacketID I2CSlave::get_packet_id(uint8_t* buffer_in)
                 return PacketID::PAD;
             }
             break;
-        case PacketID::DISABLE:
-            if (buffer_in[0] == sizeof(PacketStatus))
+
+        case PacketID::COMMAND:
+            if (buffer_in[0] == sizeof(PacketCMD))
             {
-                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;
+                return PacketID::COMMAND;
             }
             break;
+
         default:
             break;
     }
@@ -92,74 +76,74 @@ I2CSlave::PacketID I2CSlave::get_packet_id(uint8_t* buffer_in)
 
 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};
+    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.data()[count] = i2c_read_byte_raw(i2c);
+                buffer_in[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
+        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.data())) 
+            switch (get_packet_id(buffer_in)) 
             {
                 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_;
+                    instance_->packet_in_ = *packet_in_p;
+                    *packet_out_p = instance_->packet_out_;
+                    instance_->new_pad_in_.store(true);
                     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())
+                case PacketID::COMMAND:
+                    switch (packet_cmd_in_p->command)
                     {
-                        OGXMini::update_tud_status(false);
-                    }
-                    break;
+                        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 (!instance_->tuh_mounted_.load())
+                            {
+                                OGXMini::host_mounted(false);
+                            }
+                            break;
 
-                default:
+                        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 = instance_->tuh_mounted_.load() ? Status::NOT_READY : Status::READY;
+                            if (!instance_->tuh_mounted_.load() && !enabled)
+                            {
+                                enabled = true;
+                                OGXMini::host_mounted(true);
+                            }
+                            break;
+
+                        default:
+                            break;
+                    }
                     break;
             }
             count = 0;
+            std::memset(buffer_in, 0, sizeof(buffer_in));
             break;
 
-        case I2C_SLAVE_REQUEST: // master requesting data
-            i2c_write_raw_blocking(i2c, buffer_out.data(), buffer_out.data()[0]);
-            buffer_in.fill(0);
+        case I2C_SLAVE_REQUEST:
+            i2c_write_raw_blocking(i2c, buffer_out, buffer_out[0]);
             break;
 
         default:

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

@@ -4,7 +4,6 @@
 #include <cstdint>
 #include <atomic>
 #include <cstring>
-#include <hardware/gpio.h>
 #include <hardware/i2c.h>
 #include <pico/i2c_slave.h>
 
@@ -18,16 +17,15 @@ 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;
+    void notify_tuh(bool mounted, HostDriverType host_type) override;
 
 private:
-    static I2CSlave* this_instance_;
+    static I2CSlave* instance_;
 
     PacketIn packet_in_;
     PacketOut packet_out_;
-    bool new_packet_in_{false};
-    std::atomic<bool> i2c_disabled_{false};
+    std::atomic<bool> new_pad_in_{false};
+    std::atomic<bool> tuh_mounted_{false};
 
     static PacketID get_packet_id(uint8_t* buffer_in);
     static void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event);

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

@@ -8,44 +8,33 @@
 #include "board_config.h"
 #include "Board/board_api.h"
 #include "I2CDriver/ESP32/I2CDriver.h"
+#include "UserSettings/UserSettings.h"
 #include "TaskQueue/TaskQueue.h"
+#include "Board/ogxm_log.h"
 
 namespace I2CDriver {
 
-//May expand commands in the future
-enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
+enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD, SET_DRIVER };
 
 #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);
-    }
+    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()};
+    std::array<uint8_t, 5> reserved{0};
 };
-static_assert(sizeof(PacketIn) == 16, "i2c_driver_esp::PacketIn size mismatch");
+static_assert(sizeof(PacketIn) == 32, "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);
-    }
+    uint8_t packet_len{sizeof(PacketOut)};
+    PacketID packet_id{PacketID::GET_PAD};
+    uint8_t index{0};
+    Gamepad::PadOut pad_out{Gamepad::PadOut()};
+    std::array<uint8_t, 3> reserved{0};
 };
 static_assert(sizeof(PacketOut) == 8, "i2c_driver_esp::PacketOut size mismatch");
 #pragma pack(pop)
@@ -55,108 +44,79 @@ 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};
+    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: // master has written
+        case I2C_SLAVE_RECEIVE:
             if (count < sizeof(PacketIn))
             {
-                buffer_in.data()[count] = i2c_read_byte_raw(i2c);
+                reinterpret_cast<uint8_t*>(&packet_in)[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)
+        case I2C_SLAVE_FINISH:
+            if (count == 0)
             {
-                process_in_packet(reinterpret_cast<PacketIn*>(buffer_in.data()));
+                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:
+                    OGXM_LOG("I2C: Received SET_DRIVER packet: " + OGXM_TO_STRING(packet_in.device_type) + "\n");
+
+                    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: // 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);
+        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();
+            }
+            // switch (packet_in.packet_id)
+            // {
+            //     case PacketID::SET_PAD:
+                    
+                    i2c_write_raw_blocking(i2c, reinterpret_cast<const uint8_t*>(&packet_out), packet_out.packet_len);
+            //         break;
+
+            //     default:
+                    // i2c_write_raw_blocking(i2c, reinterpret_cast<const uint8_t*>(&packet_out), 1);
+            //         break;
+            // }
+            
+            // packet_in = PacketIn();
             break;
 
         default:
@@ -182,6 +142,8 @@ void initialize(Gamepad (&gamepads)[MAX_GAMEPADS])
     gpio_pull_up(I2C_SCL_PIN);
 
     i2c_slave_init(I2C_PORT, I2C_ADDR, &slave_handler);
+
+    OGXM_LOG("I2C Driver initialized\n");
 }
 
 } // namespace i2c_driver_esp32

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.h

@@ -11,7 +11,7 @@ namespace OGXMini
     static constexpr int32_t TUD_CHECK_DELAY_MS = 500;
 
     void run_program();
-    void update_tud_status(bool host_mounted);
+    void host_mounted(bool mounted);
 
 } // namespace OGXMini
 

Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp

@@ -10,6 +10,7 @@
 #include "USBDevice/DeviceManager.h"
 #include "USBHost/HostManager.h"
 #include "Board/board_api.h"
+#include "Board/ogxm_log.h"
 #include "OGXMini/OGXMini.h"
 #include "I2CDriver/4Channel/I2CManager.h"
 #include "Gamepad.h"
@@ -18,13 +19,45 @@
 namespace OGXMini {
 
 Gamepad gamepads_[MAX_GAMEPADS];
-std::atomic<bool> tud_inited_{false};
+
+//Called by tusb host or i2c driver so we know to connect or disconnect usb
+void 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("Disconnecting USB and rebooting.\n");
+            board_api::usb::disconnect_all();
+            board_api::reboot();
+        });
+    }
+    else if (!tud_is_inited.load() && host_mounted)
+    {
+        TaskQueue::Core0::queue_task([]() 
+        { 
+            OGXM_LOG("Initializing USB stack.\n");
+            tud_init(BOARD_TUD_RHPORT); 
+            tud_is_inited.store(true);
+        });
+    }
+}
 
 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 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);
 
@@ -43,47 +76,23 @@ void core1_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]))
+    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, 
+        [&user_settings]
         {
-            //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
-    });
+            //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 run_program()
 {
-    UserSettings user_settings;
+    UserSettings& user_settings = UserSettings::get_instance();
     user_settings.initialize_flash();
 
     board_api::init_board();
@@ -93,18 +102,15 @@ void run_program()
         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();
+    DeviceManager& device_manager = DeviceManager::get_instance();
+    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
+    
+    I2CManager& i2c_manager = I2CManager::get_instance();
+    i2c_manager.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())
     {
@@ -112,7 +118,11 @@ void run_program()
         sleep_ms(10);
     }
 
-    tud_inited_.store(true);
+    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();
+    I2CDriver* i2c_driver = i2c_manager.get_driver();
 
     while (true)
     {

Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp

@@ -1,5 +1,5 @@
 #include "board_config.h"
-#if OGXM_BOARD == W_ESP32
+#if (OGXM_BOARD == PICO_ESP32)
 
 #include <pico/multicore.h>
 
@@ -18,7 +18,7 @@ namespace OGXMini {
 static Gamepad gamepads_[MAX_GAMEPADS];
 
 //Not using this for ESP32 currently
-void update_tud_status(bool host_mounted) { }
+void host_mounted(bool host_mounted) { }
 
 void core1_task()
 {
@@ -30,58 +30,39 @@ void core1_task()
     }
 }
 
-void run_esp32_uart_bridge()
+void run_uart_bridge()
 {
     DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(DeviceDriver::Type::UART_BRIDGE, gamepads_);
+    device_manager.initialize_driver(DeviceDriverType::UART_BRIDGE, gamepads_);
 
-    board_api::enter_esp32_prog_mode();
+    board_api::esp32::enter_programming_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())
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
+    //MODE_SEL_PIN is used to determine if UART bridge should be run
+    if (board_api::esp32::uart_bridge_mode())
     {
-        run_esp32_uart_bridge();
+        run_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_);
+    DeviceManager& device_manager = DeviceManager::get_instance();
+    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
 
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
 
-    board_api::reset_esp32();
+    board_api::esp32::reset();
 
-    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();
+    DeviceDriver* device_driver = device_manager.get_driver();
 
     tud_init(BOARD_TUD_RHPORT);
 

Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp

@@ -1,8 +1,8 @@
 #include "board_config.h"
-#if (OGXM_BOARD == PI_PICOW)
+#if (OGXM_BOARD == PI_PICOW) || (OGXM_BOARD == PI_PICO2W)
 
+#include <hardware/clocks.h>
 #include <pico/multicore.h>
-#include <pico/cyw43_arch.h>
 
 #include "tusb.h"
 #include "bsp/board_api.h"
@@ -19,16 +19,12 @@ namespace OGXMini {
 Gamepad gamepads_[MAX_GAMEPADS];
 
 //Not using this for Pico W currently
-void update_tud_status(bool host_mounted) { }
+void host_mounted(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
+    board_api::init_bluetooth();
+    board_api::set_led(true);
     bluepad32::run_task(gamepads_);
 }
 
@@ -40,24 +36,25 @@ void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
         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());
+            user_settings.store_driver_type(user_settings.get_current_driver());
         }
     });
 }
 
 void run_program()
 {
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
     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_);
+    DeviceManager& device_manager = DeviceManager::get_instance();
+    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
 
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
@@ -65,7 +62,7 @@ void run_program()
     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();
+    DeviceDriver* device_driver = device_manager.get_driver();
 
     tud_init(BOARD_TUD_RHPORT);
 

Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp

@@ -13,16 +13,49 @@
 #include "TaskQueue/TaskQueue.h"
 #include "Gamepad.h"
 #include "Board/board_api.h"
+#include "Board/ogxm_log.h"
 
 namespace OGXMini {
 
 Gamepad gamepads_[MAX_GAMEPADS];
 
+//Called by tusb host so we know to connect or disconnect usb
+void 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 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);
 
@@ -41,53 +74,28 @@ void core1_task()
     }
 }
 
-//Called by tusb host or i2c driver so we know to connect or disconnect usb
-void update_tud_status(bool host_mounted)
-{
-    static bool tud_is_inited = false;
-
-    board_api::set_led(host_mounted);
-
-    if (!host_mounted && tud_is_inited)
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_disconnect();
-            sleep_ms(300);
-            multicore_reset_core1();
-            sleep_ms(300);
-            board_api::reboot();
-        });
-    }
-    else if (!tud_is_inited)
-    {
-        if (TaskQueue::Core0::queue_task([]() { tud_init(BOARD_TUD_RHPORT); }))
-        {
-            tud_is_inited = true;
-        }
-    }
-}
-
 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]
     {
+        OGXM_LOG("Checking for driver change.\n");
         //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_safe(user_settings.get_current_driver());
+            user_settings.store_driver_type(user_settings.get_current_driver());
         }
     });
 }
 
 void run_program()
 {
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
     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));
@@ -99,18 +107,18 @@ void run_program()
     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()
+    // Wait for something to call host_mounted()
     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, user_settings);
+
+    DeviceDriver* device_driver = device_manager.get_driver();
+
     while (true)
     {
         TaskQueue::Core0::process_tasks();

Firmware/RP2040/src/Range.h

@@ -0,0 +1,307 @@
+#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) 
+    {
+        if constexpr (std::is_signed_v<From> != std::is_signed_v<To>)
+        {
+            using CommonType = std::common_type_t<To, From>;
+            return static_cast<To>((static_cast<CommonType>(value) < static_cast<CommonType>(Range::MIN<To>)) 
+                                    ? Range::MIN<To> 
+                                    : (static_cast<CommonType>(value) > static_cast<CommonType>(Range::MAX<To>)) 
+                                        ? Range::MAX<To> 
+                                        : static_cast<CommonType>(value));
+        }
+        else
+        {
+            return static_cast<To>((value < Range::MIN<To>) 
+                                    ? Range::MIN<To> 
+                                    : (value > Range::MAX<To>) 
+                                        ? Range::MAX<To> 
+                                        : value);
+        }
+    }
+
+    template <typename T>
+    requires std::is_integral_v<T>
+    static inline T clamp(T value, T min, T max) 
+    {
+        return (value < min) ? min : (value > max) ? max : value;
+    }
+
+    // //Scale value from one range to another, will clamp value to input range
+    // template <typename To, typename From>
+    // requires std::is_integral_v<To> && std::is_integral_v<From>
+    // static inline To scale(From value, From min_from, From max_from, To min_to, To max_to) 
+    // {
+    //     constexpr bool is_signed_from = std::is_signed_v<From>;
+    //     constexpr bool is_signed_to = std::is_signed_v<To>;
+
+    //     constexpr auto shift_from = is_signed_from ? static_cast<uint64_t>(0 - Range::MIN<From>) : 0;
+    //     constexpr auto shift_to = is_signed_to ? static_cast<uint64_t>(0 - Range::MIN<To>) : 0;
+
+    //     const uint64_t u_value = static_cast<uint64_t>(value) + shift_from;
+    //     const uint64_t u_min_from = static_cast<uint64_t>(min_from) + shift_from;
+    //     const uint64_t u_max_from = static_cast<uint64_t>(max_from) + shift_from;
+    //     const uint64_t u_min_to = static_cast<uint64_t>(min_to) + shift_to;
+    //     const uint64_t u_max_to = static_cast<uint64_t>(max_to) + shift_to;
+
+    //     const uint64_t scaled = ((u_value - u_min_from) * (u_max_to - u_min_to)) / (u_max_from - u_min_from) + u_min_to;
+
+    //     return static_cast<To>(scaled - (is_signed_to ? shift_to : 0));
+    // }
+
+    template <typename To, typename From>
+    requires std::is_integral_v<To> && std::is_integral_v<From>
+    static inline To scale(From value, From min_from, From max_from, To min_to, To max_to) 
+    {
+        if constexpr (std::is_unsigned_v<From> && std::is_unsigned_v<To>) 
+        {
+            // Both unsigned
+            uint64_t scaled = static_cast<uint64_t>(value - min_from) * 
+                            (max_to - min_to) / 
+                            (max_from - min_from) + min_to;
+            return static_cast<To>(scaled);
+        } 
+        else if constexpr (std::is_signed_v<From> && std::is_unsigned_v<To>) 
+        {
+            // From signed, To unsigned
+            uint64_t shift_from = static_cast<uint64_t>(-min_from);
+            uint64_t u_value = static_cast<uint64_t>(value) + shift_from;
+            uint64_t u_min_from = static_cast<uint64_t>(min_from) + shift_from;
+            uint64_t u_max_from = static_cast<uint64_t>(max_from) + shift_from;
+
+            uint64_t scaled = (u_value - u_min_from) * 
+                            (max_to - min_to) / 
+                            (u_max_from - u_min_from) + min_to;
+            return static_cast<To>(scaled);
+        } 
+        else if constexpr (std::is_unsigned_v<From> && std::is_signed_v<To>) 
+        {
+            // From unsigned, To signed
+            uint64_t shift_to = static_cast<uint64_t>(-min_to);
+            uint64_t scaled = static_cast<uint64_t>(value - min_from) * 
+                            (static_cast<uint64_t>(max_to) + shift_to - static_cast<uint64_t>(min_to) - shift_to) / 
+                            (max_from - min_from) + static_cast<uint64_t>(min_to) + shift_to;
+            return static_cast<To>(scaled - shift_to);
+        } 
+        else 
+        {
+            // Both signed
+            int64_t shift_from = -min_from;
+            int64_t shift_to = -min_to;
+
+            int64_t scaled = (static_cast<int64_t>(value) + shift_from - (min_from + shift_from)) * 
+                            (max_to + shift_to - (min_to + shift_to)) / 
+                            (max_from - min_from) + (min_to + shift_to);
+            return static_cast<To>(scaled - shift_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>(
+            value, 
+            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>(
+            value, 
+            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>);
+    }
+
+    // static inline uint8_t int10_to_uint8(int32_t value)
+    // {
+    //     return Range::scale<uint8_t>(value, 10);
+    //     // return static_cast<uint8_t>(value >> 2);
+    // }
+    // static inline int16_t int10_to_int16(int32_t value)
+    // {
+    //     return Range::scale<int16_t>(value, 10);
+    //     // constexpr int32_t scale_factor = Range::MAX<int16_t> - Range::MIN<int16_t>;
+    //     // constexpr int32_t range = INT_10::MAX - INT_10::MIN;
+
+    //     // if (value >= INT_10::MAX)
+    //     // {
+    //     //     return Range::MAX<int16_t>;
+    //     // }
+    //     // else if (value <= INT_10::MIN)
+    //     // {
+    //     //     return Range::MIN<int16_t>;
+    //     // }
+
+    //     // int32_t scaled_value = (value - INT_10::MIN) * scale_factor;
+    //     // return static_cast<int16_t>(scaled_value / range + Range::MIN<int16_t>);
+    // }
+    // 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 // _RANGE_H_

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,189 @@
-#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 {
-
-class CoreQueue
+TaskQueue::TaskQueue(CoreNum core_num) 
 {   
-public:
-    enum class CoreNum : uint8_t
+    alarm_num_ = (core_num == CoreNum::Core0) ? 0 : 1;
+    alarm_num_ += ((OGXM_BOARD == PI_PICOW) || (OGXM_BOARD == PI_PICO2W)) ? 1 : 0; //BTStack uses alarm 0
+
+    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()
+{
+    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);
-    }
-
-    ~CoreQueue() = default;
-
-    uint32_t get_new_task_id()
-    {
-        return new_task_id_++;
-    }
-
-    bool 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_) 
-        {
-            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;
-            }
-        }
-
-        spin_unlock(spinlock_delayed_, irq_state);
-        return false;
-    }
-
-    void 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_) 
-        {
-            if (task.task_id == task_id) 
-            {
-                task.function = nullptr;
-                task.task_id = 0;
-                task.interval_ms = 0;
-                found = true;
-            }
-        }
-
-        if (!found)
+        if (task.task_id == task_id)
         {
             spin_unlock(spinlock_delayed_, irq_state);
-            return;
+            return false;
         }
-
-        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)
+    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_) 
     {
-        uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
-        for (auto& task : task_queue_)
+        if (!task.function) 
         {
-            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) 
             {
-                task.function = function;
-                spin_unlock(spinlock_queue_, irq_state);
-                return true;
+                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;
         }
-        spin_unlock(spinlock_queue_, irq_state);
-        return false;
     }
 
-    void process_tasks()
+    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_queue_);
-        for (auto& task : task_queue_)
+        if (task.task_id == task_id) 
         {
-            if (task.function)
+            task.function = nullptr;
+            task.task_id = 0;
+            task.interval_ms = 0;
+            found = true;
+        }
+    }
+
+    if (!found)
+    {
+        spin_unlock(spinlock_delayed_, irq_state);
+        return;
+    }
+
+    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 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)
+        {
+            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_);
+
+    for (auto& task : task_queue_delayed_) 
+    {
+        if (task.function && task.target_time <= now) 
+        {
+            auto function = task.function;
+            if (task.interval_ms) 
             {
-                auto function = task.function;
-                task.function = nullptr;
-                spin_unlock(spinlock_queue_, irq_state);
-
-                function();
-
-                irq_state = spin_lock_blocking(spinlock_queue_);
+                task.target_time += (task.interval_ms * 1000);
             } 
             else 
             {
-                break; //No more tasks
+                task.function = nullptr;
+                task.task_id = 0;
             }
+
+            spin_unlock(spinlock_delayed_, irq_state);
+            queue_task(function);
+            irq_state = spin_lock_blocking(spinlock_delayed_);
         }
-        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
+    int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
+    if (next_target_time >= 0) 
     {
-        // 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();
+        timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
     }
 
-    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;
-    }
-}; // class CoreQueue
-
-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
-{
-    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();
-    }
-}
-
-#endif // OGXM_BOARD != PI_PICOW
-
-} // namespace TaskQueue

Firmware/RP2040/src/TaskQueue/TaskQueue.h

@@ -3,33 +3,157 @@
 
 #include <cstdint>
 #include <functional>
+#include <memory>
+#include <pico/stdlib.h>
+#include <hardware/timer.h>
+#include <hardware/irq.h>
+#include <hardware/sync.h>
 
 #include "board_config.h"
 
-/*  Queue tasks to be executed with process_tasks() in the main loop on either core.
-    Don't use this on the core running BTStack, that is not safe. */
-namespace TaskQueue 
+class TaskQueue
 {
-    namespace Core0 
+public:
+    struct Core0
     {
-        uint32_t get_new_task_id();
-        void cancel_delayed_task(uint32_t task_id);
-        bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function);
-        bool queue_task(const std::function<void()>& function);
-        void process_tasks();
-    }
+        static inline uint32_t get_new_task_id()
+        {
+            return get_core0().get_new_task_id();
+        }
+        static inline void cancel_delayed_task(uint32_t task_id)
+        {
+            get_core0().cancel_delayed_task(task_id);
+        }
+        static inline bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+        {
+            return get_core0().queue_delayed_task(task_id, delay_ms, repeating, function);
+        }
+        static inline bool queue_task(const std::function<void()>& function)
+        {
+            return get_core0().queue_task(function);
+        }
+        static inline void process_tasks()
+        {
+            get_core0().process_tasks();
+        }
+    };
 
-#if OGXM_BOARD != PI_PICOW
-    namespace Core1 
+#if (OGXM_BOARD != PI_PICOW) && (OGXM_BOARD != PI_PICO2W) //BTstack uses core1
+    struct Core1
     {
-        uint32_t get_new_task_id();
-        void cancel_delayed_task(uint32_t task_id);
-        bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function);
-        bool queue_task(const std::function<void()>& function);
-        void process_tasks();
-    }
+        static inline uint32_t get_new_task_id()
+        {
+            return get_core1().get_new_task_id();
+        }
+        static inline void cancel_delayed_task(uint32_t task_id)
+        {
+            get_core1().cancel_delayed_task(task_id);
+        }
+        static inline bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+        {
+            return get_core1().queue_delayed_task(task_id, delay_ms, repeating, function);
+        }
+        static inline bool queue_task(const std::function<void()>& function)
+        {
+            return get_core1().queue_task(function);
+        }
+        static inline void process_tasks()
+        {
+            get_core1().process_tasks();
+        }
+    }; // Core1
 #endif // OGXM_BOARD != PI_PICOW
 
-} // namespace TaskQueue
+private:
+    enum class CoreNum : uint8_t
+    {
+        Core0 = 0,
+        Core1 = 1
+    };
+
+    TaskQueue& operator=(const TaskQueue&) = delete;
+    TaskQueue(TaskQueue&&) = delete;
+    TaskQueue& operator=(TaskQueue&&) = delete;
+
+    TaskQueue(CoreNum core_num);
+    ~TaskQueue() = default;
+
+    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 TaskQueue& get_core0()
+    {
+        static TaskQueue core(CoreNum::Core0);
+        return core;
+    }
+    static TaskQueue& get_core1()
+    {
+        static TaskQueue core(CoreNum::Core1);
+        return core;
+    }
+
+    uint32_t get_new_task_id();
+    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function);
+    void cancel_delayed_task(uint32_t task_id);
+    bool queue_task(const std::function<void()>& function);
+    void process_tasks();
+
+    void timer_irq_handler();
+    static uint64_t get_time_64_us();
+
+    static inline void timer_irq_wrapper_c0()
+    {
+        get_core0().timer_irq_handler();
+    }
+    static inline void timer_irq_wrapper_c1()
+    {
+        get_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 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;
+    }
+
+}; // class TaskQueue
 
 #endif // TASK_QUEUE_H

Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h

@@ -18,21 +18,6 @@
 class DeviceDriver
 {
 public:
-    enum class Type : uint8_t
-    {
-        NONE = 0,
-        PS3 = 1,
-        DINPUT = 2,
-        SWITCH = 4,
-        PSCLASSIC = 5,
-        XINPUT = 6,
-        XBOXOG = 7,
-        XBOXOG_SB = 8,
-        XBOXOG_XR = 9,
-        WEBAPP = 100,
-        UART_BRIDGE = 101
-    };
-
     virtual void initialize() = 0;
     virtual void process(const uint8_t idx, Gamepad& gamepad) = 0;
     virtual uint16_t get_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t* buffer, uint16_t req_len) = 0;

Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriverTypes.h

@@ -0,0 +1,21 @@
+#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
+};
+
+#endif // _DEVICE_DRIVER_TYPES_H_

Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp

@@ -123,7 +123,7 @@ void PS3Device::process(const uint8_t idx, Gamepad& gamepad)
     {
         Gamepad::PadOut gp_out;
         gp_out.rumble_l = report_out_.rumble.left_motor_force;
-        gp_out.rumble_r = report_out_.rumble.right_motor_on ? UINT_8::MAX : 0;
+        gp_out.rumble_r = report_out_.rumble.right_motor_on ? Range::MAX<uint8_t> : 0;
         gamepad.set_pad_out(gp_out);
         new_report_out_ = false;
     }

Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp

@@ -54,9 +54,9 @@ void PSClassicDevice::process(const uint8_t idx, Gamepad& gamepad)
         }
 
         int16_t joy_lx = gp_in.joystick_lx;
-        int16_t joy_ly = Scale::invert_joy(gp_in.joystick_ly);
+        int16_t joy_ly = Range::invert(gp_in.joystick_ly);
         int16_t joy_rx = gp_in.joystick_rx;
-        int16_t joy_ry = Scale::invert_joy(gp_in.joystick_ry);
+        int16_t joy_ry = Range::invert(gp_in.joystick_ry);
 
         if (meets_pos_threshold(joy_lx, joy_rx))
         {

Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp

@@ -57,13 +57,13 @@ void WebAppDevice::process(const uint8_t idx, Gamepad& gamepad)
             break;
 
         case ReportID::WRITE_PROFILE:
-            if (user_settings_.valid_mode(static_cast<DeviceDriver::Type>(in_report_.input_mode)))
+            if (user_settings_.is_valid_driver(static_cast<DeviceDriverType>(in_report_.input_mode)))
             {
-                success = user_settings_.store_profile_and_driver_type_safe(static_cast<DeviceDriver::Type>(in_report_.input_mode), in_report_.player_idx, in_report_.profile);
+                success = user_settings_.store_profile_and_driver_type(static_cast<DeviceDriverType>(in_report_.input_mode), in_report_.player_idx, in_report_.profile);
             }
             else
             {
-                success = user_settings_.store_profile_safe(in_report_.player_idx, in_report_.profile);
+                success = user_settings_.store_profile(in_report_.player_idx, in_report_.profile);
             }
             if (!success)
             {

Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h

@@ -41,9 +41,9 @@ private:
     static_assert(sizeof(Report) == 50, "WebApp report size mismatch");
     #pragma pack(pop)
 
-    UserSettings user_settings_{UserSettings()};
+    UserSettings& user_settings_{UserSettings::get_instance()};
     Report in_report_{Report()};
-    DeviceDriver::Type driver_type_{DeviceDriver::Type::WEBAPP};
+    DeviceDriverType driver_type_{DeviceDriverType::WEBAPP};
 };
 
 #endif // _WEBAAPP_DEVICE_H_

Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.cpp

@@ -64,9 +64,9 @@ void XInputDevice::process(const uint8_t idx, Gamepad& gamepad)
         in_report_.trigger_r = gp_in.trigger_r;
 
         in_report_.joystick_lx = gp_in.joystick_lx;
-        in_report_.joystick_ly = Scale::invert_joy(gp_in.joystick_ly);
+        in_report_.joystick_ly = Range::invert(gp_in.joystick_ly);
         in_report_.joystick_rx = gp_in.joystick_rx;
-        in_report_.joystick_ry = Scale::invert_joy(gp_in.joystick_ry);
+        in_report_.joystick_ry = Range::invert(gp_in.joystick_ry);
 
         if (tud_suspended())
         {

Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_GP.cpp

@@ -78,9 +78,9 @@ void XboxOGDevice::process(const uint8_t idx, Gamepad& gamepad)
         in_report_.trigger_r = gp_in.trigger_r;
 
         in_report_.joystick_lx = gp_in.joystick_lx;
-        in_report_.joystick_ly = Scale::invert_joy(gp_in.joystick_ly);
+        in_report_.joystick_ly = Range::invert(gp_in.joystick_ly);
         in_report_.joystick_rx = gp_in.joystick_rx;
-        in_report_.joystick_ry = Scale::invert_joy(gp_in.joystick_ry);
+        in_report_.joystick_ry = Range::invert(gp_in.joystick_ry);
 
         if (tud_suspended())
         {

Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp

@@ -5,7 +5,6 @@
 #include "Descriptors/XInput.h"
 #include "USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid.h"
 #include "USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h"
-#include "OGXMini/Debug.h"
 
 static constexpr std::array<XboxOGSBDevice::ButtonMap, 9> GP_MAP = 
 {{
@@ -232,10 +231,11 @@ void XboxOGSBDevice::process(const uint8_t idx, Gamepad& gamepad)
     in_report_.leftPedal    = Scale::uint8_to_uint16(gp_in.trigger_l);
     in_report_.rightPedal   = Scale::uint8_to_uint16(gp_in.trigger_r);
     in_report_.middlePedal  = chatpad_pressed(  gp_in_chatpad, XInput::Chatpad::CODE_BACK) ? 0xFF00 : 0x0000;
-    in_report_.rotationLever= chatpad_pressed(  gp_in_chatpad, XInput::Chatpad::CODE_MESSENGER) ? 0 :
-                                                (gp_in.buttons & Gamepad::BUTTON_BACK) ? 0 :
-                                                (gp_in.dpad & Gamepad::DPAD_LEFT) ? INT_16::MIN :
-                                                (gp_in.dpad & Gamepad::DPAD_RIGHT) ? INT_16::MAX : 0;
+    in_report_.rotationLever= chatpad_pressed(  gp_in_chatpad, XInput::Chatpad::CODE_MESSENGER) 
+                                                    ? 0 : (gp_in.buttons & Gamepad::BUTTON_BACK) 
+                                                        ? 0 : (gp_in.dpad & Gamepad::DPAD_LEFT)  
+                                                            ? Range::MIN<int16_t> : (gp_in.dpad & Gamepad::DPAD_RIGHT) 
+                                                                ? Range::MAX<int16_t> : 0;
 
     in_report_.sightChangeX = gp_in.joystick_lx;
     in_report_.sightChangeY = gp_in.joystick_ly;
@@ -246,15 +246,15 @@ void XboxOGSBDevice::process(const uint8_t idx, Gamepad& gamepad)
         vmouse_x_ += axis_value_x / sensitivity_;
     }
 
-    int32_t axis_value_y = static_cast<int32_t>(Scale::invert_joy(gp_in.joystick_ry));
+    int32_t axis_value_y = static_cast<int32_t>(Range::invert(gp_in.joystick_ry));
     if (std::abs(axis_value_y) > DEFAULT_DEADZONE)
     {
         vmouse_y_ -= axis_value_y / sensitivity_;
     }
 
     if (vmouse_x_ < 0) vmouse_x_ = 0;
-    if (vmouse_x_ > UINT_16::MAX) vmouse_x_ = UINT_16::MAX;
-    if (vmouse_y_ > UINT_16::MAX) vmouse_y_ = UINT_16::MAX;
+    if (vmouse_x_ > Range::MAX<uint16_t>) vmouse_x_ = Range::MAX<uint16_t>;
+    if (vmouse_y_ > Range::MAX<uint16_t>) vmouse_y_ = Range::MAX<uint16_t>;
     if (vmouse_y_ < 0) vmouse_y_ = 0;
 
     if (gp_in.buttons & Gamepad::BUTTON_L3)

Firmware/RP2040/src/USBDevice/DeviceManager.cpp

@@ -16,43 +16,43 @@
 #include "USBDevice/DeviceDriver/UARTBridge/UARTBridge.h"
 #endif // defined(CONFIG_EN_UART_BRIDGE)
 
-void DeviceManager::initialize_driver(DeviceDriver::Type driver_type, Gamepad(&gamepads)[MAX_GAMEPADS])
+void DeviceManager::initialize_driver(DeviceDriverType driver_type, Gamepad(&gamepads)[MAX_GAMEPADS])
 {
     bool has_analog = false; //TODO: Put gamepad setup in the drivers themselves
     switch (driver_type)
     {
-        case DeviceDriver::Type::DINPUT:
+        case DeviceDriverType::DINPUT:
             has_analog = true;
             device_driver_ = std::make_unique<DInputDevice>();
             break;
-        case DeviceDriver::Type::PS3:
+        case DeviceDriverType::PS3:
             has_analog = true;
             device_driver_ = std::make_unique<PS3Device>();
             break;
-        case DeviceDriver::Type::PSCLASSIC:
+        case DeviceDriverType::PSCLASSIC:
             device_driver_ = std::make_unique<PSClassicDevice>();
             break;
-        case DeviceDriver::Type::SWITCH:
+        case DeviceDriverType::SWITCH:
             device_driver_ = std::make_unique<SwitchDevice>();
             break;
-        case DeviceDriver::Type::XINPUT:
+        case DeviceDriverType::XINPUT:
             device_driver_ = std::make_unique<XInputDevice>();
             break;
-        case DeviceDriver::Type::XBOXOG:
+        case DeviceDriverType::XBOXOG:
             has_analog = true;
             device_driver_ = std::make_unique<XboxOGDevice>();
             break;
-        case DeviceDriver::Type::XBOXOG_SB:
+        case DeviceDriverType::XBOXOG_SB:
             device_driver_ = std::make_unique<XboxOGSBDevice>();
             break;
-        case DeviceDriver::Type::XBOXOG_XR:
+        case DeviceDriverType::XBOXOG_XR:
             device_driver_ = std::make_unique<XboxOGXRDevice>();
             break;
-        case DeviceDriver::Type::WEBAPP:
+        case DeviceDriverType::WEBAPP:
             device_driver_ = std::make_unique<WebAppDevice>();
             break;
 #if defined(CONFIG_EN_UART_BRIDGE)
-        case DeviceDriver::Type::UART_BRIDGE:
+        case DeviceDriverType::UART_BRIDGE:
             device_driver_ = std::make_unique<UARTBridgeDevice>();
             break;
 #endif //defined(CONFIG_EN_UART_BRIDGE)

Firmware/RP2040/src/USBDevice/DeviceManager.h

@@ -5,6 +5,7 @@
 #include <memory>
 
 #include "Gamepad.h"
+#include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 
 class DeviceManager 
@@ -20,7 +21,7 @@ public:
 	}
 
 	//Must be called before any other method
-	void initialize_driver(DeviceDriver::Type driver_type, Gamepad(&gamepads)[MAX_GAMEPADS]);
+	void initialize_driver(DeviceDriverType driver_type, Gamepad(&gamepads)[MAX_GAMEPADS]);
 	
 	DeviceDriver* get_driver() { return device_driver_.get(); }
 	

Firmware/RP2040/src/USBHost/HardwareIDs.h

@@ -71,19 +71,19 @@ struct HostTypeMap
 {
     const HardwareID* ids;
     size_t num_ids;
-    HostDriver::Type type;
+    HostDriverType type;
 };
 
 static const HostTypeMap HOST_TYPE_MAP[] = 
 {
-    { DINPUT_IDS, sizeof(DINPUT_IDS) / sizeof(HardwareID), HostDriver::Type::DINPUT },
-    { PS4_IDS, sizeof(PS4_IDS) / sizeof(HardwareID), HostDriver::Type::PS4 },
-    { PS5_IDS, sizeof(PS5_IDS) / sizeof(HardwareID), HostDriver::Type::PS5 },
-    { PS3_IDS, sizeof(PS3_IDS) / sizeof(HardwareID), HostDriver::Type::PS3 },
-    { SWITCH_WIRED_IDS, sizeof(SWITCH_WIRED_IDS) / sizeof(HardwareID), HostDriver::Type::SWITCH },
-    { SWITCH_PRO_IDS, sizeof(SWITCH_PRO_IDS) / sizeof(HardwareID), HostDriver::Type::SWITCH_PRO },
-    { PSCLASSIC_IDS, sizeof(PSCLASSIC_IDS) / sizeof(HardwareID), HostDriver::Type::PSCLASSIC },
-    { N64_IDS, sizeof(N64_IDS) / sizeof(HardwareID), HostDriver::Type::N64 },
+    { DINPUT_IDS, sizeof(DINPUT_IDS) / sizeof(HardwareID), HostDriverType::DINPUT },
+    { PS4_IDS, sizeof(PS4_IDS) / sizeof(HardwareID), HostDriverType::PS4 },
+    { PS5_IDS, sizeof(PS5_IDS) / sizeof(HardwareID), HostDriverType::PS5 },
+    { PS3_IDS, sizeof(PS3_IDS) / sizeof(HardwareID), HostDriverType::PS3 },
+    { SWITCH_WIRED_IDS, sizeof(SWITCH_WIRED_IDS) / sizeof(HardwareID), HostDriverType::SWITCH },
+    { SWITCH_PRO_IDS, sizeof(SWITCH_PRO_IDS) / sizeof(HardwareID), HostDriverType::SWITCH_PRO },
+    { PSCLASSIC_IDS, sizeof(PSCLASSIC_IDS) / sizeof(HardwareID), HostDriverType::PSCLASSIC },
+    { N64_IDS, sizeof(N64_IDS) / sizeof(HardwareID), HostDriverType::N64 },
 };
 
 #endif // _HW_ID_H_

Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp

@@ -81,25 +81,25 @@ void DInputHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t insta
 
     if (in_report->l2_axis > 0)
     {
-        gp_in.trigger_l = in_report->l2_axis;
+        gp_in.trigger_l = gamepad.scale_trigger_l(in_report->l2_axis);
     }
     else
     {
-        gp_in.trigger_l = (in_report->buttons[0] & DInput::Buttons0::L2) ? UINT_8::MAX : UINT_8::MIN;
+        gp_in.trigger_l = (in_report->buttons[0] & DInput::Buttons0::L2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
     }
     if (in_report->r2_axis > 0)
     {
-        gp_in.trigger_r = in_report->r2_axis;
+        gp_in.trigger_r = gamepad.scale_trigger_r(in_report->r2_axis);
     }
     else
     {
-        gp_in.trigger_r = (in_report->buttons[0] & DInput::Buttons0::R2) ? UINT_8::MAX : UINT_8::MIN;
+        gp_in.trigger_r = (in_report->buttons[0] & DInput::Buttons0::R2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
     }
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report->joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report->joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp

@@ -1,5 +1,9 @@
+#include <cstring>
 #include <memory>
 
+#include "host/usbh.h"
+#include "class/hid/hid_host.h"
+
 #include "USBHost/HIDParser/HIDReportDescriptor.h"
 #include "USBHost/HostDriver/HIDGeneric/HIDGeneric.h"
 
@@ -64,10 +68,10 @@ void HIDHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
             break;
     }
 
-    gp_in.joystick_lx = hid_joystick_data_.X;
-    gp_in.joystick_ly = hid_joystick_data_.Y;
-    gp_in.joystick_rx = hid_joystick_data_.Z;
-    gp_in.joystick_ry = hid_joystick_data_.Rz;
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(hid_joystick_data_.X);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(hid_joystick_data_.Y);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(hid_joystick_data_.Z);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(hid_joystick_data_.Rz);
 
     if (hid_joystick_data_.buttons[1])  gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (hid_joystick_data_.buttons[2])  gp_in.buttons |= gamepad.MAP_BUTTON_A;
@@ -75,8 +79,8 @@ void HIDHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     if (hid_joystick_data_.buttons[4])  gp_in.buttons |= gamepad.MAP_BUTTON_Y;
     if (hid_joystick_data_.buttons[5])  gp_in.buttons |= gamepad.MAP_BUTTON_LB;
     if (hid_joystick_data_.buttons[6])  gp_in.buttons |= gamepad.MAP_BUTTON_RB;
-    if (hid_joystick_data_.buttons[7])  gp_in.trigger_l = UINT_8::MAX;
-    if (hid_joystick_data_.buttons[8])  gp_in.trigger_r = UINT_8::MAX;
+    if (hid_joystick_data_.buttons[7])  gp_in.trigger_l = Range::MAX<uint8_t>;
+    if (hid_joystick_data_.buttons[8])  gp_in.trigger_r = Range::MAX<uint8_t>;
     if (hid_joystick_data_.buttons[9])  gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
     if (hid_joystick_data_.buttons[10]) gp_in.buttons |= gamepad.MAP_BUTTON_START;
     if (hid_joystick_data_.buttons[11]) gp_in.buttons |= gamepad.MAP_BUTTON_L3;

Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h

@@ -6,30 +6,12 @@
 #include "UserSettings/UserProfile.h"
 #include "UserSettings/UserSettings.h"
 #include "Gamepad.h"
+#include "USBHost/HostDriver/HostDriverTypes.h"
 
 //Use HostManager, don't use this directly
 class HostDriver
 {
 public:
-    enum class Type
-    {
-        UNKNOWN = 0,
-        SWITCH_PRO,
-        SWITCH,
-        PSCLASSIC,
-        DINPUT,
-        PS3,
-        PS4,
-        PS5,
-        N64,
-        XBOXOG,
-        XBOXONE,
-        XBOX360W,
-        XBOX360,
-        XBOX360_CHATPAD,
-        HID_GENERIC
-    };
-
     HostDriver(uint8_t idx)
         : idx_(idx) {}
 
@@ -49,12 +31,12 @@ protected:
     {
         Gamepad::PadOut gp_out = gamepad.get_pad_out();
         bool reset = false;
-        if (gp_out.rumble_l != UINT_8::MAX)
+        if (gp_out.rumble_l != Range::MAX<uint8_t>)
         {
             gp_out.rumble_l = 0;
             reset = true;
         }
-        if (gp_out.rumble_r != UINT_8::MAX)
+        if (gp_out.rumble_r != Range::MAX<uint8_t>)
         {
             gp_out.rumble_r = 0;
             reset = true;

Firmware/RP2040/src/USBHost/HostDriver/HostDriverTypes.h

@@ -0,0 +1,25 @@
+#ifndef HOST_DRIVER_TYPES_H
+#define HOST_DRIVER_TYPES_H
+
+#include <cstdint>
+
+enum class HostDriverType
+{
+    UNKNOWN = 0,
+    SWITCH_PRO,
+    SWITCH,
+    PSCLASSIC,
+    DINPUT,
+    PS3,
+    PS4,
+    PS5,
+    N64,
+    XBOXOG,
+    XBOXONE,
+    XBOX360W,
+    XBOX360,
+    XBOX360_CHATPAD,
+    HID_GENERIC
+};
+
+#endif // HOST_DRIVER_TYPES_H

Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp

@@ -1,5 +1,8 @@
 #include <cstring>
 
+#include "host/usbh.h"
+#include "class/hid/hid_host.h"
+
 #include "USBHost/HostDriver/N64/N64.h"
 
 void N64Host::initialize(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report_desc, uint16_t desc_len)
@@ -91,13 +94,13 @@ void N64Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
             break;
     }
 
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_y);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_x);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_y);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_x);
 
-    gp_in.trigger_l = (in_report->buttons & N64::Buttons::L) ? UINT_8::MAX : UINT_8::MIN;
+    gp_in.trigger_l = (in_report->buttons & N64::Buttons::L) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
-    gp_in.joystick_ly = Scale::uint8_to_int16(joy_ry);
-    gp_in.joystick_lx = Scale::uint8_to_int16(joy_rx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(joy_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(joy_rx);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp

@@ -111,17 +111,17 @@ void PS3Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     if (in_report->buttons[0] & PS3::Buttons0::DPAD_LEFT)  gp_in.dpad |= gamepad.MAP_DPAD_LEFT;
     if (in_report->buttons[0] & PS3::Buttons0::DPAD_RIGHT) gp_in.dpad |= gamepad.MAP_DPAD_RIGHT;
 
-    if (in_report->buttons[0] & PS3::Buttons0::SELECT)      gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
-    if (in_report->buttons[0] & PS3::Buttons0::START)       gp_in.buttons |= gamepad.MAP_BUTTON_START;
-    if (in_report->buttons[0] & PS3::Buttons0::L3)          gp_in.buttons |= gamepad.MAP_BUTTON_L3;
-    if (in_report->buttons[0] & PS3::Buttons0::R3)          gp_in.buttons |= gamepad.MAP_BUTTON_R3;
-    if (in_report->buttons[1] & PS3::Buttons1::L1)          gp_in.buttons |= gamepad.MAP_BUTTON_LB;
-    if (in_report->buttons[1] & PS3::Buttons1::R1)          gp_in.buttons |= gamepad.MAP_BUTTON_RB;
-    if (in_report->buttons[1] & PS3::Buttons1::TRIANGLE)    gp_in.buttons |= gamepad.MAP_BUTTON_Y;
-    if (in_report->buttons[1] & PS3::Buttons1::CIRCLE)      gp_in.buttons |= gamepad.MAP_BUTTON_B;
-    if (in_report->buttons[1] & PS3::Buttons1::CROSS)       gp_in.buttons |= gamepad.MAP_BUTTON_A;
-    if (in_report->buttons[1] & PS3::Buttons1::SQUARE)      gp_in.buttons |= gamepad.MAP_BUTTON_X;
-    if (in_report->buttons[2] & PS3::Buttons2::SYS)         gp_in.buttons |= gamepad.MAP_BUTTON_SYS;
+    if (in_report->buttons[0] & PS3::Buttons0::SELECT)   gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
+    if (in_report->buttons[0] & PS3::Buttons0::START)    gp_in.buttons |= gamepad.MAP_BUTTON_START;
+    if (in_report->buttons[0] & PS3::Buttons0::L3)       gp_in.buttons |= gamepad.MAP_BUTTON_L3;
+    if (in_report->buttons[0] & PS3::Buttons0::R3)       gp_in.buttons |= gamepad.MAP_BUTTON_R3;
+    if (in_report->buttons[1] & PS3::Buttons1::L1)       gp_in.buttons |= gamepad.MAP_BUTTON_LB;
+    if (in_report->buttons[1] & PS3::Buttons1::R1)       gp_in.buttons |= gamepad.MAP_BUTTON_RB;
+    if (in_report->buttons[1] & PS3::Buttons1::TRIANGLE) gp_in.buttons |= gamepad.MAP_BUTTON_Y;
+    if (in_report->buttons[1] & PS3::Buttons1::CIRCLE)   gp_in.buttons |= gamepad.MAP_BUTTON_B;
+    if (in_report->buttons[1] & PS3::Buttons1::CROSS)    gp_in.buttons |= gamepad.MAP_BUTTON_A;
+    if (in_report->buttons[1] & PS3::Buttons1::SQUARE)   gp_in.buttons |= gamepad.MAP_BUTTON_X;
+    if (in_report->buttons[2] & PS3::Buttons2::SYS)      gp_in.buttons |= gamepad.MAP_BUTTON_SYS;
 
     if (gamepad.analog_enabled())
     {
@@ -137,13 +137,13 @@ void PS3Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
         gp_in.analog[gamepad.MAP_ANALOG_OFF_RB] = in_report->r1_axis;
     }
 
-    gp_in.trigger_l = in_report->l2_axis;
-    gp_in.trigger_r = in_report->r2_axis;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report->l2_axis);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report->r2_axis);
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report->joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report->joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ly(in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp

@@ -69,13 +69,13 @@ void PS4Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     if (in_report_.buttons[2] & PS4::Buttons2::PS)       gp_in.buttons |= gamepad.MAP_BUTTON_SYS;
     if (in_report_.buttons[2] & PS4::Buttons2::TP)       gp_in.buttons |= gamepad.MAP_BUTTON_MISC;
 
-    gp_in.trigger_l = in_report_.trigger_l;
-    gp_in.trigger_r = in_report_.trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report_.trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report_.trigger_r);
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report_.joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report_.joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report_.joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report_.joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report_.joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report_.joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report_.joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report_.joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp

@@ -86,13 +86,13 @@ void PS5Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     if (in_report->buttons[2] & PS5::Buttons2::PS)       gp_in.buttons |= gamepad.MAP_BUTTON_SYS;
     if (in_report->buttons[2] & PS5::Buttons2::MUTE)     gp_in.buttons |= gamepad.MAP_BUTTON_MISC;
 
-    gp_in.trigger_l = in_report->trigger_l;
-    gp_in.trigger_r = in_report->trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report->joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report->joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
     
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/PSClassic/PSClassic.cpp

@@ -60,8 +60,8 @@ void PSClassicHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t in
     if (in_report->buttons & PSClassic::Buttons::SELECT)   gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
     if (in_report->buttons & PSClassic::Buttons::START)    gp_in.buttons |= gamepad.MAP_BUTTON_START;
 
-    gp_in.trigger_l = (in_report->buttons & PSClassic::Buttons::L2) ? UINT_8::MAX : UINT_8::MIN;
-    gp_in.trigger_r = (in_report->buttons & PSClassic::Buttons::R2) ? UINT_8::MAX : UINT_8::MIN;
+    gp_in.trigger_l = (in_report->buttons & PSClassic::Buttons::L2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+    gp_in.trigger_r = (in_report->buttons & PSClassic::Buttons::R2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp

@@ -12,18 +12,6 @@ void SwitchProHost::initialize(Gamepad& gamepad, uint8_t address, uint8_t instan
     init_switch_host(gamepad, address, instance);
 }
 
-// bool SwitchProHost::send_handshake(uint8_t address, uint8_t instance)
-// {
-//     std::array<uint8_t, 2> handshake = { SwitchPro::CMD::HID, SwitchPro::CMD::HANDSHAKE };
-//     return tuh_hid_send_report(address, instance, 0, handshake.data(), handshake.size());
-// }
-
-// bool SwitchProHost::disable_timeout(uint8_t address, uint8_t instance)
-// {
-//     std::array<uint8_t, 2> timeout = { SwitchPro::CMD::HID, SwitchPro::CMD::DISABLE_TIMEOUT };
-//     return tuh_hid_send_report(address, instance, 0, timeout.data(), timeout.size());
-// }
-
 uint8_t SwitchProHost::get_output_sequence_counter()
 {
     uint8_t counter = sequence_counter_;
@@ -127,13 +115,14 @@ void SwitchProHost::init_switch_host(Gamepad& gamepad, uint8_t address, uint8_t
             if (tuh_hid_send_report(address, instance, 0, &out_report_, report_size))
             {
                 init_state_ = InitState::DONE;
+                tuh_hid_receive_report(address, instance);
             }
             break;
         default:
             break;
     }
 
-    tuh_hid_receive_report(address, instance);
+    // tuh_hid_receive_report(address, instance);
 }
 
 void SwitchProHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
@@ -171,13 +160,18 @@ void SwitchProHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t in
     if (in_report->buttons[2] & SwitchPro::Buttons2::DPAD_LEFT)  gp_in.dpad |= gamepad.MAP_DPAD_LEFT;
     if (in_report->buttons[2] & SwitchPro::Buttons2::DPAD_RIGHT) gp_in.dpad |= gamepad.MAP_DPAD_RIGHT;
 
-    gp_in.trigger_l = in_report->buttons[2] & SwitchPro::Buttons2::ZL ? UINT_8::MAX : UINT_8::MIN;
-    gp_in.trigger_r = in_report->buttons[0] & SwitchPro::Buttons0::ZR ? UINT_8::MAX : UINT_8::MIN;
+    gp_in.trigger_l = in_report->buttons[2] & SwitchPro::Buttons2::ZL ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+    gp_in.trigger_r = in_report->buttons[0] & SwitchPro::Buttons0::ZR ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
-    gp_in.joystick_lx = normalize_axis(in_report->joysticks[0] | ((in_report->joysticks[1] & 0xF) << 8));
-    gp_in.joystick_ly = Scale::invert_joy(normalize_axis((in_report->joysticks[1] >> 4) | (in_report->joysticks[2] << 4)));
-    gp_in.joystick_rx = normalize_axis(in_report->joysticks[3] | ((in_report->joysticks[4] & 0xF) << 8));
-    gp_in.joystick_ry = Scale::invert_joy(normalize_axis((in_report->joysticks[4] >> 4) | (in_report->joysticks[5] << 4)));
+    uint16_t joy_lx = in_report->joysticks[0] | ((in_report->joysticks[1] & 0xF) << 8);
+    uint16_t joy_ly = (in_report->joysticks[1] >> 4) | (in_report->joysticks[2] << 4);
+    uint16_t joy_rx = in_report->joysticks[3] | ((in_report->joysticks[4] & 0xF) << 8);
+    uint16_t joy_ry = (in_report->joysticks[4] >> 4) | (in_report->joysticks[5] << 4);
+
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(normalize_axis(joy_lx));
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(normalize_axis(joy_ly), true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(normalize_axis(joy_rx));
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(normalize_axis(joy_ry), true);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.h

@@ -5,6 +5,7 @@
 
 #include "Descriptors/SwitchPro.h"
 #include "USBHost/HostDriver/HostDriver.h"
+#include "Board/ogxm_log.h"
 
 class SwitchProHost : public HostDriver
 {
@@ -35,28 +36,18 @@ private:
     SwitchPro::OutReport out_report_{};
 
     void init_switch_host(Gamepad& gamepad, uint8_t address, uint8_t instance);
-    // bool send_handshake(uint8_t address, uint8_t instance);
-    // bool disable_timeout(uint8_t address, uint8_t instance);
     uint8_t get_output_sequence_counter();
 
     static inline int16_t normalize_axis(uint16_t value)
     {
-        /*  12bit value from the controller doesnt cover the full 12bit range seemingly
-            isn't completely centered at 2047 either so I may be missing something here
-            tried to get as close as possible with the multiplier */
+        /*  12bit value from the controller doesnt cover the full 12bit range seemingly,
+            isn't completely centered at 2047 either so I may be missing something here.
+            Tried to get as close as possible with the multiplier */
             
+        OGXM_LOG("Value: %d\n", value);
         int32_t normalized_value = (value - 2047) * 22;
-
-        if (normalized_value < INT16_MIN) 
-        {
-            return INT16_MIN;
-        } 
-        else if (normalized_value > INT16_MAX) 
-        {
-            return INT16_MAX;
-        }
-
-        return static_cast<int16_t>(normalized_value); 
+        OGXM_LOG("Normalized value: %d\n", normalized_value);
+        return Range::clamp<int16_t>(normalized_value);
     }
 };
 

Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp

@@ -64,13 +64,13 @@ void SwitchWiredHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t
     if (in_report->buttons & SwitchWired::Buttons::L3)      gp_in.buttons |= gamepad.MAP_BUTTON_L3;
     if (in_report->buttons & SwitchWired::Buttons::R3)      gp_in.buttons |= gamepad.MAP_BUTTON_R3;
 
-    gp_in.trigger_l = (in_report->buttons & SwitchWired::Buttons::ZL) ? UINT_8::MAX : UINT_8::MIN;
-    gp_in.trigger_r = (in_report->buttons & SwitchWired::Buttons::ZR) ? UINT_8::MAX : UINT_8::MIN;
+    gp_in.trigger_l = (in_report->buttons & SwitchWired::Buttons::ZL) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+    gp_in.trigger_r = (in_report->buttons & SwitchWired::Buttons::ZR) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report->joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report->joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp

@@ -7,7 +7,6 @@
 
 void Xbox360Host::initialize(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report_desc, uint16_t desc_len)
 {
-    std::memset(&prev_in_report_, 0, sizeof(XInput::InReport));
     tuh_xinput::set_led(address, instance, idx_ + 1, true);
     tuh_xinput::receive_report(address, instance);
 }
@@ -40,13 +39,13 @@ void Xbox360Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t inst
     if (in_report_->buttons[1] & XInput::Buttons1::X)      gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (in_report_->buttons[1] & XInput::Buttons1::Y)      gp_in.buttons |= gamepad.MAP_BUTTON_Y;
 
-    gp_in.trigger_l = in_report_->trigger_l;    
-    gp_in.trigger_r = in_report_->trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report_->trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report_->trigger_r);
 
-    gp_in.joystick_lx = in_report_->joystick_lx;
-    gp_in.joystick_ly = Scale::invert_joy(in_report_->joystick_ly);
-    gp_in.joystick_rx = in_report_->joystick_rx;
-    gp_in.joystick_ry = Scale::invert_joy(in_report_->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report_->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report_->joystick_ly, true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report_->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report_->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp

@@ -8,7 +8,7 @@
 #include "TaskQueue/TaskQueue.h"
 #include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
 #include "USBHost/HostDriver/XInput/Xbox360W.h"
-#include "OGXMini/Debug.h"
+#include "Board/ogxm_log.h"
 
 Xbox360WHost::~Xbox360WHost()
 {
@@ -71,13 +71,13 @@ void Xbox360WHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t ins
     if (in_report->buttons[1] & XInput::Buttons1::X)      gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (in_report->buttons[1] & XInput::Buttons1::Y)      gp_in.buttons |= gamepad.MAP_BUTTON_Y;
 
-    gp_in.trigger_l = in_report->trigger_l;
-    gp_in.trigger_r = in_report->trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = in_report->joystick_lx;
-    gp_in.joystick_ly = Scale::invert_joy(in_report->joystick_ly);
-    gp_in.joystick_rx = in_report->joystick_rx;
-    gp_in.joystick_ry = Scale::invert_joy(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp

@@ -48,19 +48,19 @@ void XboxOGHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t insta
         gp_in.analog[gamepad.MAP_ANALOG_OFF_Y]  = in_report->y;
         gp_in.analog[gamepad.MAP_ANALOG_OFF_LB] = in_report->black;
         gp_in.analog[gamepad.MAP_ANALOG_OFF_RB] = in_report->white;
-        gp_in.analog[gamepad.MAP_ANALOG_OFF_UP]    = (in_report->buttons & XboxOG::GP::Buttons::DPAD_UP) ? UINT_8::MAX : UINT_8::MIN;
-        gp_in.analog[gamepad.MAP_ANALOG_OFF_DOWN]  = (in_report->buttons & XboxOG::GP::Buttons::DPAD_DOWN) ? UINT_8::MAX : UINT_8::MIN;
-        gp_in.analog[gamepad.MAP_ANALOG_OFF_LEFT]  = (in_report->buttons & XboxOG::GP::Buttons::DPAD_LEFT) ? UINT_8::MAX : UINT_8::MIN;
-        gp_in.analog[gamepad.MAP_ANALOG_OFF_RIGHT] = (in_report->buttons & XboxOG::GP::Buttons::DPAD_RIGHT) ? UINT_8::MAX : UINT_8::MIN;
+        gp_in.analog[gamepad.MAP_ANALOG_OFF_UP]    = (in_report->buttons & XboxOG::GP::Buttons::DPAD_UP) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+        gp_in.analog[gamepad.MAP_ANALOG_OFF_DOWN]  = (in_report->buttons & XboxOG::GP::Buttons::DPAD_DOWN) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+        gp_in.analog[gamepad.MAP_ANALOG_OFF_LEFT]  = (in_report->buttons & XboxOG::GP::Buttons::DPAD_LEFT) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+        gp_in.analog[gamepad.MAP_ANALOG_OFF_RIGHT] = (in_report->buttons & XboxOG::GP::Buttons::DPAD_RIGHT) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
     }
 
-    gp_in.trigger_l = in_report->trigger_l;
-    gp_in.trigger_r = in_report->trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = in_report->joystick_lx;
-    gp_in.joystick_ly = Scale::invert_joy(in_report->joystick_ly);
-    gp_in.joystick_rx = in_report->joystick_rx;
-    gp_in.joystick_ry = Scale::invert_joy(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp

@@ -39,13 +39,13 @@ void XboxOneHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t inst
     if (in_report->buttons[0] & XboxOne::Buttons0::X)     gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (in_report->buttons[0] & XboxOne::Buttons0::Y)     gp_in.buttons |= gamepad.MAP_BUTTON_Y;
 
-    gp_in.trigger_l = static_cast<uint8_t>(in_report->trigger_l >> 2);
-    gp_in.trigger_r = static_cast<uint8_t>(in_report->trigger_r >> 2);
+    gp_in.trigger_l = gamepad.scale_trigger_l(static_cast<uint8_t>(in_report->trigger_l >> 2));
+    gp_in.trigger_r = gamepad.scale_trigger_r(static_cast<uint8_t>(in_report->trigger_r >> 2));
 
-    gp_in.joystick_lx = in_report->joystick_lx;
-    gp_in.joystick_ly = Scale::invert_joy(in_report->joystick_ly);
-    gp_in.joystick_rx = in_report->joystick_rx;
-    gp_in.joystick_ry = Scale::invert_joy(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 

Firmware/RP2040/src/USBHost/HostManager.h

@@ -53,7 +53,7 @@ public:
 	}
 
 	//XInput doesn't need report_desc or desc_len
-	inline bool setup_driver(const HostDriver::Type driver_type, const uint8_t address, const uint8_t instance, uint8_t const* report_desc = nullptr, uint16_t desc_len = 0)
+	inline bool setup_driver(const HostDriverType driver_type, const uint8_t address, const uint8_t instance, uint8_t const* report_desc = nullptr, uint16_t desc_len = 0)
 	{
 		uint8_t gp_idx = find_free_gamepad();
 		if (gp_idx == INVALID_IDX || instance >= MAX_INTERFACES)
@@ -74,40 +74,40 @@ public:
 
 		switch (driver_type)
 		{
-			case HostDriver::Type::PS5:
+			case HostDriverType::PS5:
 				interface.driver = std::make_unique<PS5Host>(gp_idx);
 				break;
-			case HostDriver::Type::PS4:
+			case HostDriverType::PS4:
 				interface.driver = std::make_unique<PS4Host>(gp_idx);
 				break;
-			case HostDriver::Type::PS3:
+			case HostDriverType::PS3:
 				interface.driver = std::make_unique<PS3Host>(gp_idx);
 				break;
-			case HostDriver::Type::DINPUT:
+			case HostDriverType::DINPUT:
 				interface.driver = std::make_unique<DInputHost>(gp_idx);
 				break;
-			case HostDriver::Type::SWITCH:
+			case HostDriverType::SWITCH:
 				interface.driver = std::make_unique<SwitchWiredHost>(gp_idx);
 				break;
-			case HostDriver::Type::SWITCH_PRO:
+			case HostDriverType::SWITCH_PRO:
 				interface.driver = std::make_unique<SwitchProHost>(gp_idx);
 				break;
-			case HostDriver::Type::N64:
+			case HostDriverType::N64:
 				interface.driver = std::make_unique<N64Host>(gp_idx);
 				break;
-			case HostDriver::Type::PSCLASSIC:
+			case HostDriverType::PSCLASSIC:
 				interface.driver = std::make_unique<PSClassicHost>(gp_idx);
 				break;
-			case HostDriver::Type::XBOXOG:
+			case HostDriverType::XBOXOG:
 				interface.driver = std::make_unique<XboxOGHost>(gp_idx);
 				break;
-			case HostDriver::Type::XBOXONE:
+			case HostDriverType::XBOXONE:
 				interface.driver = std::make_unique<XboxOneHost>(gp_idx);
 				break;
-			case HostDriver::Type::XBOX360:
+			case HostDriverType::XBOX360:
 				interface.driver = std::make_unique<Xbox360Host>(gp_idx);
 				break;
-			case HostDriver::Type::XBOX360W: //Composite device, takes up all 4 gamepads when mounted
+			case HostDriverType::XBOX360W: //Composite device, takes up all 4 gamepads when mounted
 				interface.driver = std::make_unique<Xbox360WHost>(gp_idx);
 				break;
 			default:
@@ -130,7 +130,7 @@ public:
 		return true;
 	}
 
-	inline void process_report(DriverClass driver_class, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
+	inline void process_report(uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
 	{
 		for (auto& device_slot : device_slots_)
 		{
@@ -143,7 +143,7 @@ public:
 		}
 	}
 
-	inline void connect_cb(DriverClass driver_class, uint8_t address, uint8_t instance)
+	inline void connect_cb(uint8_t address, uint8_t instance)
 	{
 		for (auto& device_slot : device_slots_)
 		{
@@ -156,7 +156,7 @@ public:
 		}
 	}
 
-	inline void disconnect_cb(DriverClass driver_class, uint8_t address, uint8_t instance)
+	inline void disconnect_cb(uint8_t address, uint8_t instance)
 	{
 		for (auto& device_slot : device_slots_)
 		{
@@ -180,7 +180,7 @@ public:
 			}
 			for (uint8_t i = 0; i < MAX_INTERFACES; ++i)
 			{
-				if (device_slot.interfaces[i].driver != nullptr && device_slot.interfaces[i].gamepad->new_pad_out())
+				if (device_slot.interfaces[i].driver && device_slot.interfaces[i].gamepad->new_pad_out())
 				{
 					device_slot.interfaces[i].driver->send_feedback(*device_slot.interfaces[i].gamepad, device_slot.address, i);
 					tuh_task();
@@ -200,7 +200,7 @@ public:
 		}
 	}
 
-	static inline HostDriver::Type get_type(const HardwareID& ids)
+	static inline HostDriverType get_type(const HardwareID& ids)
 	{
 		for (const auto& map : HOST_TYPE_MAP)
 		{
@@ -212,25 +212,25 @@ public:
 				}
 			}
 		}
-		return HostDriver::Type::UNKNOWN;
+		return HostDriverType::UNKNOWN;
 	}
 
-	static inline HostDriver::Type get_type(const tuh_xinput::DevType xinput_type)
+	static inline HostDriverType get_type(const tuh_xinput::DevType xinput_type)
 	{
 		switch (xinput_type)
 		{
 			case tuh_xinput::DevType::XBOXONE:
-				return HostDriver::Type::XBOXONE;
+				return HostDriverType::XBOXONE;
 			case tuh_xinput::DevType::XBOX360W:
-				return HostDriver::Type::XBOX360W;
+				return HostDriverType::XBOX360W;
 			case tuh_xinput::DevType::XBOX360:
-				return HostDriver::Type::XBOX360;
+				return HostDriverType::XBOX360;
 			// case tuh_xinput::DevType::XBOX360_CHATPAD:
-			// 	return HostDriver::Type::XBOX360_CHATPAD;
+			// 	return HostDriverType::XBOX360_CHATPAD;
 			case tuh_xinput::DevType::XBOXOG:
-				return HostDriver::Type::XBOXOG;
+				return HostDriverType::XBOXOG;
 			default:
-				return HostDriver::Type::UNKNOWN;
+				return HostDriverType::UNKNOWN;
 		}
 	}
 

Firmware/RP2040/src/USBHost/tuh_callbacks.cpp

@@ -12,11 +12,11 @@
 
 #if defined(CONFIG_EN_4CH)
 #include "I2CDriver/4Channel/I2CManager.h"
-#endif //defined(CONFIG_EN_4CH)
+#endif 
 
 usbh_class_driver_t const* usbh_app_driver_get_cb(uint8_t* driver_count)
 {
-    *driver_count += 1;
+    *driver_count = 1;
     return tuh_xinput::class_driver();
 }
 
@@ -32,10 +32,10 @@ void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_re
     if (host_manager.setup_driver(HostManager::get_type({ vid, pid }), dev_addr, instance, desc_report, desc_len))
     {
 #if defined(CONFIG_EN_4CH)
-        I2CManager::get_instance().get_driver()->notify_tuh_mounted();
-#endif //defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh(true);
+#endif
 
-        OGXMini::update_tud_status(true);
+        OGXMini::host_mounted(true);
     }
 }
 
@@ -47,16 +47,16 @@ void tuh_hid_umount_cb(uint8_t dev_addr, uint8_t instance)
     if (!host_manager.any_mounted())
     {
 #if defined(CONFIG_EN_4CH)
-        I2CManager::get_instance().get_driver()->notify_tuh_unmounted();
-#endif //defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh(false);
+#endif
 
-        OGXMini::update_tud_status(false);
+        OGXMini::host_mounted(false);
     }
 }
 
 void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report, uint16_t len)
 {
-    HostManager::get_instance().process_report(HostManager::DriverClass::HID, dev_addr, instance, report, len);
+    HostManager::get_instance().process_report(dev_addr, instance, report, len);
 }
 
 //XINPUT
@@ -64,15 +64,15 @@ void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t cons
 void tuh_xinput::mount_cb(uint8_t dev_addr, uint8_t instance, const tuh_xinput::Interface* interface)
 {
     HostManager& host_manager = HostManager::get_instance();
-    HostDriver::Type host_type = HostManager::get_type(interface->dev_type);
+    HostDriverType host_type = HostManager::get_type(interface->dev_type);
 
     if (host_manager.setup_driver(host_type, dev_addr, instance))
     {
 #if defined(CONFIG_EN_4CH)
-        I2CManager::get_instance().get_driver()->notify_tuh_mounted(host_type);
-#endif //defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh(true, host_type);
+#endif
 
-        OGXMini::update_tud_status(true);
+        OGXMini::host_mounted(true);
     }
 }
 
@@ -84,34 +84,34 @@ void tuh_xinput::unmount_cb(uint8_t dev_addr, uint8_t instance, const tuh_xinput
     if (!host_manager.any_mounted())
     {
 #if defined(CONFIG_EN_4CH)
-        I2CManager::get_instance().get_driver()->notify_tuh_mounted(host_manager.get_type(interface->dev_type));
-#endif //defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh(false, host_manager.get_type(interface->dev_type));
+#endif
 
-        OGXMini::update_tud_status(false);
+        OGXMini::host_mounted(false);
     }
 }
 
 void tuh_xinput::report_received_cb(uint8_t dev_addr, uint8_t instance, const uint8_t* report, uint16_t len)
 {
-    HostManager::get_instance().process_report(HostManager::DriverClass::XINPUT, dev_addr, instance, report, len);
+    HostManager::get_instance().process_report(dev_addr, instance, report, len);
 }
 
 void tuh_xinput::xbox360w_connect_cb(uint8_t dev_addr, uint8_t instance)
 {
 #if defined(CONFIG_EN_4CH)
     uint8_t idx = HostManager::get_instance().get_gamepad_idx(HostManager::DriverClass::XINPUT, dev_addr, instance);
-    I2CManager::get_instance().get_driver()->notify_xbox360w_connected(idx);
-#endif //defined(CONFIG_EN_4CH)
+    I2CManager::get_instance().get_driver()->notify_xbox360w(true, idx);
+#endif
 
-    HostManager::get_instance().connect_cb(HostManager::DriverClass::XINPUT, dev_addr, instance);
+    HostManager::get_instance().connect_cb(dev_addr, instance);
 }
 
 void tuh_xinput::xbox360w_disconnect_cb(uint8_t dev_addr, uint8_t instance)
 {
 #if defined(CONFIG_EN_4CH)
     uint8_t idx = HostManager::get_instance().get_gamepad_idx(HostManager::DriverClass::XINPUT, dev_addr, instance);
-    I2CManager::get_instance().get_driver()->notify_xbox360w_disconnected(idx);
-#endif //defined(CONFIG_EN_4CH)
+    I2CManager::get_instance().get_driver()->notify_xbox360w(false, idx);
+#endif
 
-    HostManager::get_instance().disconnect_cb(HostManager::DriverClass::XINPUT, dev_addr, instance);
+    HostManager::get_instance().disconnect_cb(dev_addr, instance);
 }

Firmware/RP2040/src/UserSettings/NVSTool.h

@@ -0,0 +1,175 @@
+#ifndef _NVS_TOOL_H_
+#define _NVS_TOOL_H_
+
+#include <cstdint>
+#include <string>
+#include <array>
+#include <cstring>
+#include <hardware/flash.h>
+#include <pico/mutex.h>
+
+/* Define NVS_SECTORS (number of sectors to allocate to storage) either here or with CMake */
+
+class NVSTool
+{
+public:
+    static constexpr size_t KEY_LEN_MAX = 16; //Including null terminator
+    static constexpr size_t VALUE_LEN_MAX = FLASH_PAGE_SIZE - KEY_LEN_MAX;
+    static constexpr uint32_t MAX_ENTRIES = ((NVS_SECTORS * FLASH_SECTOR_SIZE) / FLASH_PAGE_SIZE) - 1;
+
+    static NVSTool& get_instance()
+    {
+        static NVSTool instance;
+        return instance;
+    }
+
+    bool write(const std::string& key, const void* value, size_t len)
+    {
+        if (!valid_args(key, len))
+        {
+            return false;
+        }
+
+        mutex_enter_blocking(&nvs_mutex_);
+
+        for (uint32_t i = 1; i < MAX_ENTRIES; ++i) 
+        {
+            Entry* read_entry = get_entry(i);
+
+            if (!is_valid_entry(read_entry) || std::strcmp(read_entry->key, key.c_str()) == 0)
+            {
+                update_entry(i, key, value, len);
+
+                mutex_exit(&nvs_mutex_);
+                return true;
+            }
+        }
+
+        mutex_exit(&nvs_mutex_);
+        return false; // No space for new entry
+    }
+
+    bool read(const std::string& key, void* value, size_t len)
+    {
+        if (!valid_args(key, len))
+        {
+            return false;
+        }
+
+        mutex_enter_blocking(&nvs_mutex_);
+
+        for (uint32_t i = 1; i < MAX_ENTRIES; ++i) 
+        {
+            Entry* read_entry = get_entry(i);
+
+            if (std::strcmp(read_entry->key, key.c_str()) == 0)
+            {
+                std::memcpy(value, read_entry->value, len);
+
+                mutex_exit(&nvs_mutex_);
+                return true;
+            }
+            else if (!is_valid_entry(read_entry))
+            {
+                // Key not found
+                break;
+            }
+        }
+
+        mutex_exit(&nvs_mutex_);
+        return false;
+    }
+
+private:
+    NVSTool()
+    {
+        mutex_init(&nvs_mutex_);
+
+        Entry* initial_entry = get_entry(0);
+
+        if (std::strcmp(initial_entry->key, INVALID_KEY) != 0) 
+        {
+            mutex_enter_blocking(&nvs_mutex_);
+
+            for (uint32_t i = 0; i < NVS_SECTORS; ++i) 
+            {
+                flash_range_erase(NVS_START_OFFSET + i * FLASH_SECTOR_SIZE, FLASH_SECTOR_SIZE);
+            }
+
+            Entry entry;
+
+            for (uint32_t i = 0; i < MAX_ENTRIES; ++i) 
+            {
+                flash_range_program(NVS_START_OFFSET + i * sizeof(Entry), 
+                                    reinterpret_cast<const uint8_t*>(&entry), 
+                                    sizeof(Entry));
+            }
+
+            mutex_exit(&nvs_mutex_);
+        }
+    }
+
+    ~NVSTool() = default;
+    NVSTool(const NVSTool&) = delete;
+    NVSTool& operator=(const NVSTool&) = delete;
+
+    struct Entry 
+    {
+        char key[KEY_LEN_MAX];
+        uint8_t value[VALUE_LEN_MAX];
+
+        Entry()
+        {
+            std::fill(std::begin(key), std::end(key), '\0');
+            std::strncpy(key, INVALID_KEY, sizeof(key) - 1);
+            std::fill(std::begin(value), std::end(value), 0xFF);
+        }
+    };
+    static_assert(sizeof(Entry) == FLASH_PAGE_SIZE, "NVSTool::Entry size mismatch");
+
+    static constexpr const char INVALID_KEY[KEY_LEN_MAX] = "INVALID";
+    static constexpr uint32_t NVS_START_OFFSET = PICO_FLASH_SIZE_BYTES - FLASH_SECTOR_SIZE * NVS_SECTORS;
+
+    mutex_t nvs_mutex_;
+
+    inline Entry* get_entry(const uint32_t index) 
+    {
+        return reinterpret_cast<Entry*>(XIP_BASE + NVS_START_OFFSET + index * sizeof(Entry));
+    }
+
+    inline bool is_valid_entry(const Entry* entry) 
+    {
+        return std::strcmp(entry->key, INVALID_KEY) != 0;
+    }
+
+    inline bool valid_args(const std::string& key, size_t len)
+    {
+        return (key.size() < KEY_LEN_MAX - 1 && len <= sizeof(Entry::value) && std::strcmp(key.c_str(), INVALID_KEY) != 0);
+    }
+
+    void update_entry(uint32_t index, const std::string& key, const void* buffer, size_t len)
+    {
+        uint32_t entry_offset = index * sizeof(Entry);
+        uint32_t sector_offset = ((NVS_START_OFFSET + entry_offset) / FLASH_SECTOR_SIZE) * FLASH_SECTOR_SIZE;
+
+        std::array<uint8_t, FLASH_SECTOR_SIZE> sector_buffer;
+        std::memcpy(sector_buffer.data(), reinterpret_cast<const uint8_t*>(XIP_BASE + sector_offset), FLASH_SECTOR_SIZE);
+
+        flash_range_erase(sector_offset, FLASH_SECTOR_SIZE);
+
+        Entry* entry_to_write = reinterpret_cast<Entry*>(sector_buffer.data() + entry_offset);
+
+        *entry_to_write = Entry();
+        std::strncpy(entry_to_write->key, key.c_str(), key.size());
+        entry_to_write->key[key.size()] = '\0';
+        std::memcpy(entry_to_write->value, buffer, len);
+
+        for (uint32_t i = 0; i < FLASH_SECTOR_SIZE / FLASH_PAGE_SIZE; ++i) 
+        {
+            flash_range_program(sector_offset + i * FLASH_PAGE_SIZE, sector_buffer.data() + i * FLASH_PAGE_SIZE, FLASH_PAGE_SIZE);
+        }
+    }
+
+}; // class NVSTool
+
+#endif // _NVS_TOOL_H_

Firmware/RP2040/src/UserSettings/UserSettings.cpp

@@ -1,241 +1,117 @@
 #include <cstring>
 #include <array>
-#include <pico/stdlib.h>
 #include <pico/multicore.h>
-#include <hardware/flash.h>
-#include <hardware/regs/addressmap.h>
 
 #include "tusb.h"
 
+#include "Board/ogxm_log.h"
 #include "Board/board_api.h"
 #include "UserSettings/UserSettings.h"
 
-static constexpr uint32_t button_combo(const uint16_t& buttons, const uint8_t& dpad = 0)
+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 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 DeviceDriver::Type VALID_DRIVER_TYPES[] =
+static constexpr DeviceDriverType VALID_DRIVER_TYPES[] =
 {
 #if defined(CONFIG_EN_4CH)
-    DeviceDriver::Type::XBOXOG, 
-    DeviceDriver::Type::XBOXOG_SB, 
-    DeviceDriver::Type::XINPUT,
-    DeviceDriver::Type::PS3,
-    DeviceDriver::Type::PSCLASSIC, 
-    DeviceDriver::Type::WEBAPP,
+    DeviceDriverType::XBOXOG, 
+    DeviceDriverType::XBOXOG_SB, 
+    DeviceDriverType::XINPUT,
+    DeviceDriverType::PS3,
+    DeviceDriverType::PSCLASSIC, 
+    DeviceDriverType::WEBAPP,
     #if defined(XREMOTE_ROM_AVAILABLE)
-        DeviceDriver::Type::XBOXOG_XR,
+    DeviceDriverType::XBOXOG_XR,
     #endif
 
 #elif MAX_GAMEPADS > 1
-    DeviceDriver::Type::DINPUT, 
-    DeviceDriver::Type::SWITCH, 
-    DeviceDriver::Type::WEBAPP,
+    DeviceDriverType::DINPUT, 
+    DeviceDriverType::SWITCH, 
+    DeviceDriverType::WEBAPP,
 
 #else // MAX_GAMEPADS == 1
-    DeviceDriver::Type::XBOXOG, 
-    DeviceDriver::Type::XBOXOG_SB, 
-    DeviceDriver::Type::DINPUT, 
-    DeviceDriver::Type::SWITCH, 
-    DeviceDriver::Type::WEBAPP,
-    DeviceDriver::Type::PS3,
-    DeviceDriver::Type::PSCLASSIC, 
-    DeviceDriver::Type::XINPUT,
+    DeviceDriverType::XBOXOG, 
+    DeviceDriverType::XBOXOG_SB, 
+    DeviceDriverType::DINPUT, 
+    DeviceDriverType::SWITCH, 
+    DeviceDriverType::WEBAPP,
+    DeviceDriverType::PS3,
+    DeviceDriverType::PSCLASSIC, 
+    DeviceDriverType::XINPUT,
     #if defined(XREMOTE_ROM_AVAILABLE)
-        DeviceDriver::Type::XBOXOG_XR,
+    DeviceDriverType::XBOXOG_XR,
     #endif
 
 #endif
 };
 
-struct ComboMap { uint32_t combo; DeviceDriver::Type driver; };
+struct ComboMap { uint32_t combo; DeviceDriverType driver; };
 static constexpr std::array<ComboMap, 9> BUTTON_COMBO_MAP = 
 {{
-    { ButtonCombo::XBOXOG,    DeviceDriver::Type::XBOXOG    },
-    { ButtonCombo::XBOXOG_SB, DeviceDriver::Type::XBOXOG_SB },
-    { ButtonCombo::XBOXOG_XR, DeviceDriver::Type::XBOXOG_XR },
-    { ButtonCombo::WEBAPP,    DeviceDriver::Type::WEBAPP    },
-    { ButtonCombo::DINPUT,    DeviceDriver::Type::DINPUT    },
-    { ButtonCombo::SWITCH,    DeviceDriver::Type::SWITCH    },
-    { ButtonCombo::XINPUT,    DeviceDriver::Type::XINPUT    },
-    { ButtonCombo::PS3,       DeviceDriver::Type::PS3       },
-    { ButtonCombo::PSCLASSIC, DeviceDriver::Type::PSCLASSIC }
+    { 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 }
 }};
 
-mutex_t UserSettings::flash_mutex_;
-DeviceDriver::Type UserSettings::current_driver_ = DeviceDriver::Type::NONE;
-
-//Checks for first boot and initializes user profiles, call before tusb is inited.
-void UserSettings::initialize_flash()
+const std::string UserSettings::INIT_FLAG_KEY()
 {
-    if (!mutex_is_initialized(&flash_mutex_))
-    {
-        mutex_init(&flash_mutex_);
-    }
-
-    mutex_enter_blocking(&flash_mutex_);
-
-    const uint8_t* read_init_flag = reinterpret_cast<const uint8_t*>(XIP_BASE + flash_offset(INIT_FLAG_SECTOR));
-
-    if (*read_init_flag == INIT_FLAG)
-    {
-        mutex_exit(&flash_mutex_);
-        return;
-    }
-
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-
-    // {
-    //     std::array<uint8_t, FLASH_PAGE_SIZE> device_mode_buffer;
-    //     device_mode_buffer.fill(0xFF);
-
-    //     // device_mode_buffer[0] = static_cast<uint8_t>(DeviceDriver::Type::XBOXOG);
-
-    //     flash_range_erase(flash_offset(DEVICE_MODE_SECTOR), FLASH_SECTOR_SIZE);
-    //     flash_range_program(flash_offset(DEVICE_MODE_SECTOR), device_mode_buffer.data(), FLASH_PAGE_SIZE);
-    // }
-    
-    {
-        std::array<uint8_t, FLASH_PAGE_SIZE> profile_ids;
-        profile_ids.fill(0xFF);
-        std::memset(profile_ids.data(), 0x01, 4);
-
-        flash_range_erase(flash_offset(ACTIVE_PROFILES_SECTOR), FLASH_SECTOR_SIZE);
-        flash_range_program(flash_offset(ACTIVE_PROFILES_SECTOR), profile_ids.data(), FLASH_PAGE_SIZE);
-    }
-
-    {    
-        std::array<uint8_t, FLASH_PAGE_SIZE> profile_buffer;
-        profile_buffer.fill(0);
-
-        flash_range_erase(flash_offset(PROFILES_START_SECTOR), FLASH_SECTOR_SIZE);
-
-        for (uint8_t i = 0; i < MAX_PROFILES; i++)
-        {
-            UserProfile profile = UserProfile();
-            profile.id = i + 1;
-            std::memcpy(profile_buffer.data(), reinterpret_cast<const uint8_t*>(&profile), sizeof(UserProfile));
-            flash_range_program(flash_offset(PROFILES_START_SECTOR) + profile_offset(profile.id), profile_buffer.data(), FLASH_PAGE_SIZE);
-        }
-    }
-
-    {
-        std::array<uint8_t, FLASH_PAGE_SIZE> init_flag_buffer;
-        init_flag_buffer.fill(INIT_FLAG);
-
-        flash_range_erase(flash_offset(INIT_FLAG_SECTOR), FLASH_SECTOR_SIZE);
-        flash_range_program(flash_offset(INIT_FLAG_SECTOR), init_flag_buffer.data(), FLASH_PAGE_SIZE);
-    }
-
-    restore_interrupts(saved_interrupts);
-    mutex_exit(&flash_mutex_);
+    return std::string("init_flag");
 }
 
-bool UserSettings::verify_firmware_version()
+const std::string UserSettings::PROFILE_KEY(const uint8_t profile_id)
 {
-    mutex_enter_blocking(&flash_mutex_);
-
-    const char* flash_firmware_version = reinterpret_cast<const char*>(XIP_BASE + flash_offset(FIRMWARE_VER_FLAG_SECTOR));
-    bool match = std::memcmp(flash_firmware_version, FIRMWARE_VERSION, sizeof(FIRMWARE_VERSION)) == 0;
-
-    mutex_exit(&flash_mutex_);
-    return match;
+    return std::string("profile_") + std::to_string(profile_id);
 }
 
-bool UserSettings::write_firmware_version_safe()
+const std::string UserSettings::ACTIVE_PROFILE_KEY(const uint8_t index)
 {
-    mutex_enter_blocking(&flash_mutex_);
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-
-    flash_range_erase(flash_offset(FIRMWARE_VER_FLAG_SECTOR), FLASH_SECTOR_SIZE);
-    flash_range_program(flash_offset(FIRMWARE_VER_FLAG_SECTOR), reinterpret_cast<const uint8_t*>(FIRMWARE_VERSION), sizeof(FIRMWARE_VERSION));
-
-    restore_interrupts(saved_interrupts);
-    mutex_exit(&flash_mutex_);
-
-    return verify_firmware_version();
+    return std::string("active_id_") + std::to_string(index);
 }
 
-DeviceDriver::Type UserSettings::get_current_driver()
+const std::string UserSettings::DRIVER_TYPE_KEY()
 {
-    if (current_driver_ != DeviceDriver::Type::NONE)
-    {
-        return current_driver_;
-    }
-
-    mutex_enter_blocking(&flash_mutex_);
-
-    int stored_value = *reinterpret_cast<const int*>(XIP_BASE + flash_offset(DRIVER_TYPE_SECTOR));
-
-    mutex_exit(&flash_mutex_);
-
-    for (const auto& driver : VALID_DRIVER_TYPES)
-    {
-        if (stored_value == static_cast<int>(driver))
-        {
-            current_driver_ = driver;
-            return current_driver_;
-        }
-    }
-
-    current_driver_ = get_default_driver();
-
-    return current_driver_;
+    return std::string("driver_type");
 }
 
-void UserSettings::store_driver_type_unsafe(const DeviceDriver::Type new_mode)
+const std::string UserSettings::FIRMWARE_VER_KEY()
 {
-    std::array<int, FLASH_PAGE_SIZE / sizeof(int)> mode_buffer;
-    mode_buffer.fill(static_cast<int>(new_mode));
-
-    mutex_enter_blocking(&flash_mutex_);
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-    
-    flash_range_erase(flash_offset(DRIVER_TYPE_SECTOR), FLASH_SECTOR_SIZE);
-    flash_range_program(flash_offset(DRIVER_TYPE_SECTOR), reinterpret_cast<uint8_t*>(mode_buffer.data()), FLASH_PAGE_SIZE);
-
-    restore_interrupts(saved_interrupts); 
-    mutex_exit(&flash_mutex_);   
+    return std::string("firmware_ver");
 }
 
-//Disconnects usb and resets pico, thread safe
-void UserSettings::store_driver_type_safe(DeviceDriver::Type new_mode) 
+DeviceDriverType UserSettings::DEFAULT_DRIVER()
 {
-    if (tud_connected())
-    {
-        tud_disconnect();
-        sleep_ms(300);
-    }
-
-    multicore_reset_core1();
-
-    store_driver_type_unsafe(new_mode);
-
-    board_api::reboot();
+    return VALID_DRIVER_TYPES[0];
 }
 
 //Checks if button combo has been held for 3 seconds, returns true if mode has been changed
 bool UserSettings::check_for_driver_change(Gamepad& gamepad)
 {
     Gamepad::PadIn gp_in = gamepad.get_pad_in();
-    static uint32_t last_button_combo = button_combo(gp_in.buttons, gp_in.dpad);
+    static uint32_t last_button_combo = BUTTON_COMBO(gp_in.buttons, gp_in.dpad);
     static uint8_t call_count = 0;
 
-    uint32_t current_button_combo = button_combo(gp_in.buttons, gp_in.dpad);
+    uint32_t current_button_combo = BUTTON_COMBO(gp_in.buttons, gp_in.dpad);
 
     if (!(current_button_combo & (static_cast<uint32_t>(Gamepad::BUTTON_START) << 16)) || 
         last_button_combo != current_button_combo)
@@ -254,24 +130,18 @@ bool UserSettings::check_for_driver_change(Gamepad& gamepad)
 
     call_count = 0;
 
-    DeviceDriver::Type new_driver = DeviceDriver::Type::NONE;
+    DeviceDriverType new_driver = DeviceDriverType::NONE;
 
-    for (const auto& combo : BUTTON_COMBO_MAP)
+    for (const auto& combo_map : BUTTON_COMBO_MAP)
     {
-        if (combo.combo == current_button_combo)
+        if (combo_map.combo == current_button_combo && is_valid_driver(combo_map.driver))
         {
-            for (const auto& driver : VALID_DRIVER_TYPES)
-            {
-                if (combo.driver == driver)
-                {
-                    new_driver = combo.driver;
-                    break;
-                }
-            }
+            new_driver = combo_map.driver;
+            break;
         }
     }
 
-    if (new_driver == DeviceDriver::Type::NONE || new_driver == current_driver_)
+    if (new_driver == DeviceDriverType::NONE || new_driver == current_driver_)
     {
         return false;
     }
@@ -281,50 +151,8 @@ bool UserSettings::check_for_driver_change(Gamepad& gamepad)
     return true;
 }
 
-void UserSettings::store_profile_unsafe(const UserProfile& profile)
-{
-    mutex_enter_blocking(&flash_mutex_);
-
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-    
-    std::array<uint8_t, FLASH_PAGE_SIZE * MAX_PROFILES> profiles_buffer;
-
-    std::memcpy(profiles_buffer.data(), reinterpret_cast<const uint8_t*>(XIP_BASE + flash_offset(PROFILES_START_SECTOR)), FLASH_PAGE_SIZE * MAX_PROFILES);
-    std::memcpy(profiles_buffer.data() + profile_offset(profile.id), reinterpret_cast<const uint8_t*>(&profile), sizeof(UserProfile));
-
-    flash_range_erase(flash_offset(PROFILES_START_SECTOR), FLASH_SECTOR_SIZE);
-
-    for (uint8_t i = 0; i < MAX_PROFILES; i++)
-    {
-        flash_range_program(flash_offset(PROFILES_START_SECTOR) + (i * FLASH_PAGE_SIZE), profiles_buffer.data() + (i * FLASH_PAGE_SIZE), FLASH_PAGE_SIZE);
-    }
-
-    restore_interrupts(saved_interrupts);
-    mutex_exit(&flash_mutex_);
-}
-
-void UserSettings::store_active_profile_id_unsafe(const uint8_t index, const uint8_t profile_id)
-{
-    mutex_enter_blocking(&flash_mutex_);
-
-    std::array<uint8_t, FLASH_PAGE_SIZE> read_profile_ids;
-    read_profile_ids.fill(0xFF);
-
-    std::memcpy(read_profile_ids.data(), reinterpret_cast<const uint8_t*>(XIP_BASE + flash_offset(ACTIVE_PROFILES_SECTOR)), 4);
-
-    read_profile_ids[index] = profile_id;
-
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-    
-    flash_range_erase(flash_offset(ACTIVE_PROFILES_SECTOR), FLASH_SECTOR_SIZE);
-    flash_range_program(flash_offset(ACTIVE_PROFILES_SECTOR), read_profile_ids.data(), FLASH_PAGE_SIZE);
-
-    restore_interrupts(saved_interrupts);
-    mutex_exit(&flash_mutex_);
-}
-
-//Disconnects usb and resets pico, thread safe
-bool UserSettings::store_profile_safe(uint8_t index, const UserProfile& profile)
+//Disconnects usb and resets pico, call from core0
+bool UserSettings::store_profile(uint8_t index, const UserProfile& profile)
 {
     if (profile.id < 1 || profile.id > MAX_PROFILES)
     {
@@ -335,23 +163,18 @@ bool UserSettings::store_profile_safe(uint8_t index, const UserProfile& profile)
         index = 0;
     }
 
-    if (tud_connected())
-    {
-        tud_disconnect();
-        sleep_ms(300);
-    }
+    board_api::usb::disconnect_all();
 
-    multicore_reset_core1();
-
-    store_active_profile_id_unsafe(index, profile.id);
-    store_profile_unsafe(profile);
+    nvs_tool_.write(ACTIVE_PROFILE_KEY(index), &profile.id, sizeof(uint8_t));
+    nvs_tool_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile));
 
     board_api::reboot();
+
     return true;
 }
 
-//Disconnects usb and resets pico, thread safe
-bool UserSettings::store_profile_and_driver_type_safe(DeviceDriver::Type new_driver_type, uint8_t index, const UserProfile& profile)
+//Disconnects usb and resets pico, call from core0
+bool UserSettings::store_profile_and_driver_type(DeviceDriverType new_driver_type, uint8_t index, const UserProfile& profile)
 {
     if (profile.id < 1 || profile.id > MAX_PROFILES)
     {
@@ -362,45 +185,58 @@ bool UserSettings::store_profile_and_driver_type_safe(DeviceDriver::Type new_dri
         index = 0;
     }
 
-    bool found = false;
+    bool valid_driver = false;
     for (const auto& driver : VALID_DRIVER_TYPES)
     {
         if (new_driver_type == driver)
         {
-            found = true;
+            valid_driver = true;
             break;
         }
     }
-    if (!found)
+    if (!valid_driver)
     {
-        new_driver_type = get_default_driver();
+        new_driver_type = DEFAULT_DRIVER();
     }
 
-    if (tud_connected())
-    {
-        tud_disconnect();
-        sleep_ms(300);
-    }
+    board_api::usb::disconnect_all();
 
-    multicore_reset_core1();
-
-    store_driver_type_unsafe(new_driver_type);
-    store_active_profile_id_unsafe(index, profile.id);
-    store_profile_unsafe(profile);
+    nvs_tool_.write(ACTIVE_PROFILE_KEY(index), &profile.id, sizeof(uint8_t));
+    nvs_tool_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile));
+    nvs_tool_.write(DRIVER_TYPE_KEY(), &new_driver_type, sizeof(uint8_t));
 
     board_api::reboot();
     
     return true;
 }
 
+//Disconnects usb and resets pico if it's a new & valid mode, call from core0
+void UserSettings::store_driver_type(DeviceDriverType new_driver) 
+{
+    if (!is_valid_driver(new_driver))
+    {
+        OGXM_LOG("Invalid driver type detected during store: " + OGXM_TO_STRING(new_driver) + "\n");
+        return;
+    }
+
+    OGXM_LOG("Storing new driver type: " + OGXM_TO_STRING(new_driver) + "\n");
+
+    board_api::usb::disconnect_all();
+
+    nvs_tool_.write(DRIVER_TYPE_KEY(), &new_driver, sizeof(uint8_t));
+
+    board_api::reboot();
+}
+
 uint8_t UserSettings::get_active_profile_id(const uint8_t index)
 {
-    mutex_enter_blocking(&flash_mutex_);
+    if (index > MAX_GAMEPADS - 1)
+    {
+        return 0x01;
+    }
 
-    const uint8_t* base_address = (const uint8_t*)(XIP_BASE + flash_offset(ACTIVE_PROFILES_SECTOR));
-    uint8_t read_profile_id = base_address[index];
-
-    mutex_exit(&flash_mutex_);
+    uint8_t read_profile_id = 0;
+    nvs_tool_.read(ACTIVE_PROFILE_KEY(index), &read_profile_id, sizeof(uint8_t));
 
     if (read_profile_id < 1 || read_profile_id > MAX_PROFILES)
     {
@@ -416,14 +252,8 @@ UserProfile UserSettings::get_profile_by_index(const uint8_t index)
 
 UserProfile UserSettings::get_profile_by_id(const uint8_t profile_id)
 {
-    mutex_enter_blocking(&flash_mutex_);
-
-    const uint8_t* base_address = reinterpret_cast<const uint8_t*>(XIP_BASE + flash_offset(PROFILES_START_SECTOR) + profile_offset(profile_id));
-
     UserProfile profile;
-    std::memcpy(&profile, base_address, sizeof(UserProfile));
-
-    mutex_exit(&flash_mutex_);
+    nvs_tool_.read(PROFILE_KEY(profile_id), &profile, sizeof(UserProfile));
 
     if (profile.id != profile_id)
     {
@@ -432,19 +262,110 @@ UserProfile UserSettings::get_profile_by_id(const uint8_t profile_id)
     return profile;
 }
 
-DeviceDriver::Type UserSettings::get_default_driver()
+bool UserSettings::is_valid_driver(DeviceDriverType driver)
 {
-    return VALID_DRIVER_TYPES[0];
-}
-
-bool UserSettings::valid_mode(DeviceDriver::Type mode)
-{
-    for (const auto& driver : VALID_DRIVER_TYPES)
+    for (const auto& valid_driver : VALID_DRIVER_TYPES)
     {
-        if (mode == driver)
+        if (driver == valid_driver)
         {
             return true;
         }
     }
     return false;
 }
+
+DeviceDriverType UserSettings::get_current_driver()
+{
+    if (current_driver_ != DeviceDriverType::NONE)
+    {
+        return current_driver_;
+    }
+
+    uint8_t stored_value = 0;
+    nvs_tool_.read(DRIVER_TYPE_KEY(), &stored_value, sizeof(uint8_t));
+
+    if (is_valid_driver(static_cast<DeviceDriverType>(stored_value)))
+    {
+        OGXM_LOG("Driver type read from flash: " + OGXM_TO_STRING(static_cast<DeviceDriverType>(stored_value)) + "\n");
+
+        current_driver_ = static_cast<DeviceDriverType>(stored_value);
+        return current_driver_;
+    }
+
+    OGXM_LOG("Invalid driver type read from flash, setting default driver\n");
+
+    current_driver_ = DEFAULT_DRIVER();
+    return current_driver_;
+}
+
+bool UserSettings::verify_firmware_version()
+{
+    std::string fw_version = FIRMWARE_VERSION;
+    char read_fw_version[fw_version.size()];
+    std::fill(read_fw_version, read_fw_version + fw_version.size(), '\0');
+
+    nvs_tool_.read(FIRMWARE_VER_KEY(), read_fw_version, fw_version.size());
+
+    return (std::memcmp(read_fw_version, fw_version.c_str(), fw_version.size()) == 0);
+}
+
+bool UserSettings::write_firmware_version()
+{
+    std::string fw_version = FIRMWARE_VERSION;
+    nvs_tool_.write(FIRMWARE_VER_KEY(), fw_version.c_str(), fw_version.size());
+    return verify_firmware_version();
+}
+
+//Checks for first boot and initializes user profiles, call before tusb is inited.
+void UserSettings::initialize_flash()
+{
+    OGXM_LOG("Initializing flash\n");
+
+    uint8_t read_init_flag = 0;
+    nvs_tool_.read(INIT_FLAG_KEY(), &read_init_flag, sizeof(uint8_t));
+
+    if (read_init_flag == FLASH_INIT_FLAG)
+    {
+        OGXM_LOG("Flash already initialized\n");
+
+        if (!verify_firmware_version())
+        {
+            OGXM_LOG("Firmware version mismatch, writing new version\n");
+            write_firmware_version();
+        }
+        return;
+    }
+
+    OGXM_LOG("Writing default driver\n");
+
+    uint8_t device_mode_buffer = static_cast<uint8_t>(DEFAULT_DRIVER());
+    nvs_tool_.write(DRIVER_TYPE_KEY(), &device_mode_buffer, sizeof(uint8_t));
+
+    OGXM_LOG("Writing default profile ids\n");
+
+    for (uint8_t i = 0; i < MAX_GAMEPADS; i++)
+    {
+        uint8_t profile_id = i + 1;
+        nvs_tool_.write(ACTIVE_PROFILE_KEY(i), &profile_id, sizeof(uint8_t));
+    }
+
+    OGXM_LOG("Writing default profiles\n");
+
+    {
+        UserProfile profile = UserProfile();
+        for (uint8_t i = 0; i < MAX_PROFILES; i++)
+        {
+            profile.id = i + 1;
+            nvs_tool_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile));
+        }
+    }
+
+    OGXM_LOG("Writing init flag\n");    
+
+    uint8_t init_flag_buffer = FLASH_INIT_FLAG;
+    nvs_tool_.write(INIT_FLAG_KEY(), &init_flag_buffer, sizeof(uint8_t));
+
+    write_firmware_version();
+
+    OGXM_LOG("Flash initialized\n");
+}