diff --git a/.gitignore b/.gitignore
index 630fed8..daef46b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,13 +1,11 @@
+Firmware/.vscode
 Firmware/RP2040/build
 Firmware/RP2040/.ignore
 Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid_xremote_rom.h
-
 Firmware/ESP32/.ignore
 Firmware/ESP32/build
 Firmware/ESP32/components/btstack
 Firmware/ESP32/sdkconfig.old
-
 Firmware/external/pico-sdk
 Firmware/external/picotool
-
 Tools/dvd-dongle-rom.bin
\ No newline at end of file
diff --git a/.gitmodules b/.gitmodules
index b830911..39242a9 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -7,3 +7,6 @@
 [submodule "Tools/dump-dvd-kit"]
 	path = Tools/dump-dvd-kit
 	url = https://github.com/XboxDev/dump-dvd-kit.git
+[submodule "Firmware/external/Pico-PIO-USB"]
+	path = Firmware/external/Pico-PIO-USB
+	url = https://github.com/wiredopposite/Pico-PIO-USB.git
diff --git a/Firmware/ESP32/main/Bluepad32/Bluepad32.cpp b/Firmware/ESP32/main/Bluepad32/Bluepad32.cpp
index 60bfdff..bcd4c10 100644
--- a/Firmware/ESP32/main/Bluepad32/Bluepad32.cpp
+++ b/Firmware/ESP32/main/Bluepad32/Bluepad32.cpp
@@ -1,12 +1,4 @@
-#include <cstdint>
-#include <atomic>
-#include <cstring>
-#include <array>
-#include <freertos/FreeRTOS.h>
-#include <freertos/task.h>
-#include <freertos/timers.h>
-#include <freertos/queue.h>
-#include <esp_log.h>
+#include <functional>
 
 #include "btstack_port_esp32.h"
 #include "btstack_run_loop.h"
@@ -14,60 +6,82 @@
 #include "uni.h"
 
 #include "sdkconfig.h"
-#include "Board/board_api.h"
 #include "I2CDriver/I2CDriver.h"
+#include "Board/board_api.h"
 #include "Bluepad32/Gamepad.h"
 #include "Bluepad32/Bluepad32.h"
 
-namespace bluepad32 {
-
-// I2CDriver i2c_driver;
+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;
 
-struct Device
+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)
 {
-    std::atomic<bool> connected{false};
-    std::atomic<bool> new_report_in{false};
-    std::atomic<ReportIn> report_in{ReportIn()};
-    std::atomic<ReportOut> report_out{ReportOut()};
-};
+    I2CDriver::PacketOut packet_out = reinterpret_cast<std::atomic<I2CDriver::PacketOut>*>(context)->load();
+    uni_hid_device_t* bp_device = nullptr;
 
-std::array<Device, CONFIG_BLUEPAD32_MAX_DEVICES> devices_;
-
-static inline void send_feedback_cb(btstack_timer_source *ts)
-{
-    uni_hid_device_t* bp_device;
-
-    for (uint8_t i = 0; i < CONFIG_BLUEPAD32_MAX_DEVICES; ++i)
+    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)
     {
-        bp_device = uni_hid_device_get_instance_for_idx(i);
-        if (!bp_device || !bp_device->report_parser.play_dual_rumble)
+        return;
+    }
+
+    if (packet_out.rumble_l || packet_out.rumble_r)
+    {
+        bp_device->report_parser.play_dual_rumble(
+            bp_device, 
+            0, 
+            FEEDBACK_TIME_MS, 
+            packet_out.rumble_l, 
+            packet_out.rumble_r
+            );
+    }
+}
+
+static inline void feedback_timer_cb(btstack_timer_source *ts)
+{
+    static btstack_context_callback_registration_t cb_registration[MAX_DEVICES];
+    static std::atomic<I2CDriver::PacketOut> packets_out[MAX_DEVICES];
+
+    uni_hid_device_t* bp_device = nullptr;
+
+    for (uint8_t i = 0; i < MAX_DEVICES; ++i)
+    {
+        if (!(bp_device = uni_hid_device_get_instance_for_idx(i)) || 
+            !uni_bt_conn_is_connected(&bp_device->conn))
         {
             continue;
         }
 
-        ReportOut report_out = devices_[i].report_out.load();
-        if (!report_out.rumble_l && !report_out.rumble_r)
+        cb_registration[i].callback = send_feedback_cb;
+        cb_registration[i].context = reinterpret_cast<void*>(&packets_out[i]);
+        
+        //Get i2c rumble packet then register callback on BTStack thread
+        i2c_driver_.push_task([i]
         {
-            continue;
-        }
-        // ReportOut report_out;
-        // report_out.index = i;
-        // i2c_driver.i2c_read_blocking(i2c_driver.MULTI_SLAVE ? i + 1 : 1, reinterpret_cast<uint8_t*>(&report_out), sizeof(ReportOut));
-        // if (!report_out.rumble_l && !report_out.rumble_r)
-        // {
-        //     continue;
-        // }
-        bp_device->report_parser.play_dual_rumble(bp_device, 0, FEEDBACK_TIME_MS, report_out.rumble_l, report_out.rumble_r);
+            I2CDriver::PacketOut packet_out;
+            if (i2c_driver_.i2c_read_blocking(  I2CDriver::MULTI_SLAVE ? i + 1 : 0x01, 
+                                                reinterpret_cast<uint8_t*>(&packet_out), 
+                                                sizeof(I2CDriver::PacketOut)) == ESP_OK)
+            {
+                packets_out[i].store(packet_out);
+                btstack_run_loop_execute_on_main_thread(&cb_registration[i]);
+            }
+        });
     }
 
     btstack_run_loop_set_timer(ts, FEEDBACK_TIME_MS);
     btstack_run_loop_add_timer(ts);
 }
 
-static inline void check_led_cb(btstack_timer_source *ts)
+inline void check_led_cb(btstack_timer_source *ts)
 {
     static bool led_state = false;
     led_state = !led_state;
@@ -80,7 +94,7 @@ static inline void check_led_cb(btstack_timer_source *ts)
     {
         for (uint8_t i = 0; i < board_api::NUM_LEDS; ++i)
         {
-            board_api::set_led(i, devices_[i].connected.load() ? 1 : (led_state ? 1 : 0));
+            board_api::set_led(i, devs_conn_[i].load() ? 1 : (led_state ? 1 : 0));
         }
     }
 
@@ -90,24 +104,21 @@ static inline void check_led_cb(btstack_timer_source *ts)
 
 //BT Driver
 
-static void init(int argc, const char** arg_V)
-{
-
-}
+static void init(int argc, const char** arg_V) {}
 
 static void init_complete_cb(void) 
 {
     uni_bt_enable_new_connections_unsafe(true);
 
-    // // Based on runtime condition, you can delete or list the stored BT keys.
-    // if (1)
-    // {
+    // 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();
-    // }
+    }
+    else
+    {
+        uni_bt_list_keys_unsafe();
+    }
 
     uni_property_dump_all();
 }
@@ -126,46 +137,59 @@ 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
-
-    int idx = uni_hid_device_get_idx_for_instance(device);
-    if (idx >= CONFIG_BLUEPAD32_MAX_DEVICES || idx < 0)
-    {
-        return;
-    }
-    devices_[idx].connected.store(true);
 }
 
-static void device_disconnected_cb(uni_hid_device_t* device) 
+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 >= CONFIG_BLUEPAD32_MAX_DEVICES || idx < 0)
+    if (idx >= MAX_DEVICES || idx < 0)
     {
         return;
     }
 
-    ReportIn report_in = ReportIn();
-    report_in.index = static_cast<uint8_t>(idx);
-    devices_[idx].report_in.store(report_in);
-    devices_[idx].connected.store(false);
+    devs_conn_[idx].store(false);
+    if (!any_connected())
+    {
+        btstack_run_loop_remove_timer(&feedback_timer_);
+    }
+
+    I2CDriver::PacketIn packet_in = I2CDriver::PacketIn();
+    packet_in.index = static_cast<uint8_t>(idx);
+    
+    i2c_driver_.push_task([packet_in]
+    {
+        i2c_driver_.i2c_write_blocking( I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, 
+                                        reinterpret_cast<const uint8_t*>(&packet_in), 
+                                        sizeof(I2CDriver::PacketIn));
+    });
 }
 
 static uni_error_t device_ready_cb(uni_hid_device_t* device) 
 {
+    int idx = uni_hid_device_get_idx_for_instance(device);
+    if (idx >= MAX_DEVICES || idx < 0)
+    {
+        return UNI_ERROR_IGNORE_DEVICE;
+    }
+
+    devs_conn_[idx].store(true);
+
+    feedback_timer_.process = feedback_timer_cb;
+    feedback_timer_.context = nullptr;
+
+    btstack_run_loop_set_timer(&feedback_timer_, FEEDBACK_TIME_MS);
+    btstack_run_loop_add_timer(&feedback_timer_);
+
     return UNI_ERROR_SUCCESS;
 }
 
-static void oob_event_cb(uni_platform_oob_event_t event, void* data) 
+static inline void controller_data_cb(uni_hid_device_t* device, uni_controller_t* controller) 
 {
-	return;
-}
-
-static void controller_data_cb(uni_hid_device_t* device, uni_controller_t* controller) 
-{
-    static uni_gamepad_t prev_uni_gp[CONFIG_BLUEPAD32_MAX_DEVICES] = {};
+    static uni_gamepad_t prev_uni_gps[MAX_DEVICES]{0};
 
     if (controller->klass != UNI_CONTROLLER_CLASS_GAMEPAD)
     {
@@ -175,77 +199,85 @@ static void controller_data_cb(uni_hid_device_t* device, uni_controller_t* contr
     uni_gamepad_t *uni_gp = &controller->gamepad;
     int idx = uni_hid_device_get_idx_for_instance(device);
 
-    if (idx >= CONFIG_BLUEPAD32_MAX_DEVICES || idx < 0 || std::memcmp(uni_gp, &prev_uni_gp[idx], sizeof(uni_gamepad_t)) == 0)
+    if (idx < 0 || std::memcmp(uni_gp, &prev_uni_gps[idx], sizeof(uni_gamepad_t)) == 0)
     {
         return;
     }
 
-    ReportIn report_in;
-    report_in.index = static_cast<uint8_t>(idx);
+    I2CDriver::PacketIn packet_in;
+    packet_in.index = static_cast<uint8_t>(idx);
 
     switch (uni_gp->dpad) 
     {
         case DPAD_UP:
-            report_in.dpad = Gamepad::DPad::UP;
+            packet_in.dpad = Gamepad::DPad::UP;
             break;
         case DPAD_DOWN:
-            report_in.dpad = Gamepad::DPad::DOWN;
+            packet_in.dpad = Gamepad::DPad::DOWN;
             break;
         case DPAD_LEFT:
-            report_in.dpad = Gamepad::DPad::LEFT;
+            packet_in.dpad = Gamepad::DPad::LEFT;
             break;
         case DPAD_RIGHT:
-            report_in.dpad = Gamepad::DPad::RIGHT;
+            packet_in.dpad = Gamepad::DPad::RIGHT;
             break;
         case (DPAD_UP | DPAD_RIGHT):
-            report_in.dpad = Gamepad::DPad::UP_RIGHT;
+            packet_in.dpad = Gamepad::DPad::UP_RIGHT;
             break;
         case (DPAD_DOWN | DPAD_RIGHT):
-            report_in.dpad = Gamepad::DPad::DOWN_RIGHT;
+            packet_in.dpad = Gamepad::DPad::DOWN_RIGHT;
             break;
         case (DPAD_DOWN | DPAD_LEFT):
-            report_in.dpad = Gamepad::DPad::DOWN_LEFT;
+            packet_in.dpad = Gamepad::DPad::DOWN_LEFT;
             break;
         case (DPAD_UP | DPAD_LEFT):
-            report_in.dpad = Gamepad::DPad::UP_LEFT;
+            packet_in.dpad = Gamepad::DPad::UP_LEFT;
             break;
         default:
             break;
     }
 
-    if (uni_gp->buttons & BUTTON_A) report_in.buttons |= Gamepad::Button::A;
-    if (uni_gp->buttons & BUTTON_B) report_in.buttons |= Gamepad::Button::B;
-    if (uni_gp->buttons & BUTTON_X) report_in.buttons |= Gamepad::Button::X;
-    if (uni_gp->buttons & BUTTON_Y) report_in.buttons |= Gamepad::Button::Y;
-    if (uni_gp->buttons & BUTTON_SHOULDER_L) report_in.buttons |= Gamepad::Button::LB;
-    if (uni_gp->buttons & BUTTON_SHOULDER_R) report_in.buttons |= Gamepad::Button::RB;
-    if (uni_gp->buttons & BUTTON_THUMB_L)    report_in.buttons |= Gamepad::Button::L3;
-    if (uni_gp->buttons & BUTTON_THUMB_R)    report_in.buttons |= Gamepad::Button::R3;
-    if (uni_gp->misc_buttons & MISC_BUTTON_BACK)    report_in.buttons |= Gamepad::Button::BACK;
-    if (uni_gp->misc_buttons & MISC_BUTTON_START)   report_in.buttons |= Gamepad::Button::START;
-    if (uni_gp->misc_buttons & MISC_BUTTON_SYSTEM)  report_in.buttons |= Gamepad::Button::SYS;
-    if (uni_gp->misc_buttons & MISC_BUTTON_CAPTURE) report_in.buttons |= Gamepad::Button::MISC;
+    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;
 
-    report_in.trigger_l = Gamepad::scale_trigger(uni_gp->brake);
-    report_in.trigger_r = Gamepad::scale_trigger(uni_gp->throttle);
+    packet_in.trigger_l = Scale::uint10_to_uint8(uni_gp->brake);
+    packet_in.trigger_r = Scale::uint10_to_uint8(uni_gp->throttle);
 
-    report_in.joystick_lx = static_cast<int16_t>(uni_gp->axis_x);
-    report_in.joystick_ly = static_cast<int16_t>(uni_gp->axis_y);
-    report_in.joystick_rx = static_cast<int16_t>(uni_gp->axis_rx);
-    report_in.joystick_ry = static_cast<int16_t>(uni_gp->axis_ry);
+    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);
 
-    devices_[idx].report_in.store(report_in);
-    devices_[idx].new_report_in.store(true);
-    // i2c_driver.i2c_write_blocking(i2c_driver.MULTI_SLAVE ? idx + 1 : 1, reinterpret_cast<const uint8_t*>(&report_in), sizeof(ReportIn));
+    i2c_driver_.push_task([packet_in]
+    {
+        i2c_driver_.i2c_write_blocking( I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, 
+                                        reinterpret_cast<const uint8_t*>(&packet_in), 
+                                        sizeof(I2CDriver::PacketIn));
+    });
 
-    std::memcpy(uni_gp, &prev_uni_gp[idx], sizeof(uni_gamepad_t));
+    std::memcpy(&prev_uni_gps[idx], uni_gp, sizeof(uni_gamepad_t));
 }
 
-const uni_property_t* get_property_cb(uni_property_idx_t idx) 
+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 = 
@@ -264,49 +296,45 @@ uni_platform* get_driver()
     return &driver;
 }
 
-//Public 
+//Public
 
-ReportIn get_report_in(uint8_t index)
+bool any_connected()
 {
-    devices_[index].new_report_in.store(false);
-    return devices_[index].report_in.load();
+    for (auto& connected : devs_conn_)
+    {
+        if (connected.load())
+        {
+            return true;
+        }
+    }
+    return false;
 }
 
-void set_report_out(const ReportOut& report_out)
+bool connected(uint8_t index)
 {
-    if (report_out.index >= CONFIG_BLUEPAD32_MAX_DEVICES)
-    {
-        return;
-    }
-    devices_[report_out.index].report_out.store(report_out);
+    return devs_conn_[index].load();
 }
 
 void run_task()
 {
-    for (uint8_t i = 0; i < CONFIG_BLUEPAD32_MAX_DEVICES; ++i)
-    {
-        ReportIn report_in;
-        report_in.index = i;
-        devices_[i].report_in.store(report_in);
-        devices_[i].report_out.store(ReportOut());
-    }
-
     board_api::init_pins(); 
-    
-    // i2c_driver.initialize_i2c();
+
+    i2c_driver_.initialize_i2c();
+
+    xTaskCreatePinnedToCore(
+        i2c_driver_.run_tasks,
+        "i2c",
+        2048 * 2,
+        nullptr,
+        configMAX_PRIORITIES-8,
+        nullptr,
+        1 );
     
     btstack_init();
 
     uni_platform_set_custom(get_driver());
     uni_init(0, nullptr);
 
-    btstack_timer_source_t feedback_timer;
-    feedback_timer.process = send_feedback_cb;
-    feedback_timer.context = nullptr;
-
-    btstack_run_loop_set_timer(&feedback_timer, FEEDBACK_TIME_MS);
-    btstack_run_loop_add_timer(&feedback_timer);
-
     btstack_timer_source_t led_timer;
     led_timer.process = check_led_cb;
     led_timer.context = nullptr;
@@ -317,26 +345,4 @@ void run_task()
     btstack_run_loop_execute();
 }
 
-bool any_connected()
-{
-    for (auto& device : devices_)
-    {
-        if (device.connected.load())
-        {
-            return true;
-        }
-    }
-    return false;
-}
-
-bool connected(uint8_t index)
-{
-    return devices_[index].connected.load();
-}
-
-bool new_report_in(uint8_t index)
-{
-    return devices_[index].new_report_in.load();
-}
-
-} // namespace bluepad32 
\ No newline at end of file
+} // namespace BP32
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Bluepad32/Bluepad32.h b/Firmware/ESP32/main/Bluepad32/Bluepad32.h
index 2a9f781..3c7d2cc 100644
--- a/Firmware/ESP32/main/Bluepad32/Bluepad32.h
+++ b/Firmware/ESP32/main/Bluepad32/Bluepad32.h
@@ -3,18 +3,11 @@
 
 #include <cstdint>
 
-#include "sdkconfig.h"
-#include "I2CDriver/I2CDriver.h"
-#include "Reports.h"
-
-namespace bluepad32 
+namespace BP32
 {
     void run_task();
-    bool connected(uint8_t index); 
     bool any_connected();
-    bool new_report_in(uint8_t index);
-    ReportIn get_report_in(uint8_t index);
-    void set_report_out(const ReportOut& report);
+    bool connected(uint8_t index);
 }
 
 #endif // _BLUEPAD32_DRIVER_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Bluepad32/Gamepad.h b/Firmware/ESP32/main/Bluepad32/Gamepad.h
index 0c6794c..6bcb7f4 100644
--- a/Firmware/ESP32/main/Bluepad32/Gamepad.h
+++ b/Firmware/ESP32/main/Bluepad32/Gamepad.h
@@ -33,50 +33,68 @@ namespace Gamepad
         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 MID = 0x80;
-        static constexpr uint8_t MAX = 0xFF;
+        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 uint8_t scale_joystick(int32_t value)
+    static inline int16_t int10_to_int16(int32_t value)
     {
-        value = value - INT_10::MIN;
+        constexpr int32_t scale_factor = INT_16::MAX - INT_16::MIN;
+        constexpr int32_t range = INT_10::MAX - INT_10::MIN;
 
-        if (value >= UINT_10::MAX) 
+        if (value >= INT_10::MAX)
         {
-            return UINT_8::MAX;
+            return INT_16::MAX;
         }
-        else if (value <= 0) 
+        else if (value <= INT_10::MIN)
         {
-            return 0;
+            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 uint8_t scale_trigger(int32_t value)
-    {
-        if (value >= UINT_10::MAX) 
-        {
-            return UINT_8::MAX;
-        }
-        else if (value <= 0) 
-        {
-            return 0;
-        }
-        return static_cast<uint8_t>(value >> 2);
-    }
-
-} // namespace Gamepad
+    
+} // namespace Scale
 
 #endif // GAMEPAD_H
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Board/board_api.h b/Firmware/ESP32/main/Board/board_api.h
index 41a2422..37989a3 100644
--- a/Firmware/ESP32/main/Board/board_api.h
+++ b/Firmware/ESP32/main/Board/board_api.h
@@ -38,7 +38,6 @@ namespace board_api
     void init_pins();
     void set_led(uint8_t index, bool state);
     void set_led(bool state);
-    void check_reset_pin();
 }
 
 #endif // _BOARD_API_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp b/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
index 6a8e13b..ca18fd4 100644
--- a/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
+++ b/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
@@ -1,4 +1,3 @@
-#include <array>
 #include <freertos/FreeRTOS.h>
 #include <freertos/task.h>
 #include <freertos/timers.h>
@@ -8,6 +7,13 @@
 #include "I2CDriver/I2CDriver.h"
 #include "Bluepad32/Bluepad32.h"
 
+I2CDriver::TaskQueue I2CDriver::task_queue_;
+
+I2CDriver::~I2CDriver()
+{
+    i2c_driver_delete(I2C_NUM_0);
+}
+
 void I2CDriver::initialize_i2c()
 {
     i2c_config_t conf;
@@ -18,46 +24,21 @@ void I2CDriver::initialize_i2c()
     conf.scl_io_num = GPIO_NUM_22;
     conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
     conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
-    conf.master.clk_speed = 400 * 1000;
+    conf.master.clk_speed = 1000 * 1000;
 
     i2c_param_config(I2C_NUM_0, &conf);
     i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0);
 }
 
-void I2CDriver::run_task()
+void I2CDriver::run_tasks(void* parameter)
 {
-    ReportIn report_in;
-    ReportOut report_out;
-
-    initialize_i2c();
+    std::function<void()> task;
 
     while (true)
-    {
-        for (uint8_t i = 0; i < CONFIG_BLUEPAD32_MAX_DEVICES; ++i)
+    {   
+        while (task_queue_.pop(task))
         {
-            if (!bluepad32::connected(i))
-            {
-                vTaskDelay(1);
-                continue;
-            }
-
-            if (bluepad32::new_report_in(i))
-            {
-                report_in = bluepad32::get_report_in(i);
-                if (i2c_write_blocking(MULTI_SLAVE ? (i + 1) : 0x01, reinterpret_cast<const uint8_t*>(&report_in), sizeof(ReportIn)) != ESP_OK)
-                {
-                    continue;
-                }
-            }
-
-            // vTaskDelay(1);
-
-            if (i2c_read_blocking(MULTI_SLAVE ? (i + 1) : 0x01, reinterpret_cast<uint8_t*>(&report_out), sizeof(ReportOut)) != ESP_OK)
-            {
-                continue;
-            }
-
-            bluepad32::set_report_out(report_out);
+            task();
         }
 
         vTaskDelay(1);
diff --git a/Firmware/ESP32/main/I2CDriver/I2CDriver.h b/Firmware/ESP32/main/I2CDriver/I2CDriver.h
index b77fd52..6246a9b 100644
--- a/Firmware/ESP32/main/I2CDriver/I2CDriver.h
+++ b/Firmware/ESP32/main/I2CDriver/I2CDriver.h
@@ -3,12 +3,12 @@
 
 #include <cstdint>
 #include <cstring>
-#include <atomic>
+#include <functional>
 #include <driver/i2c.h>
 
-#include "Reports.h"
+#include "sdkconfig.h"
+#include "RingBuffer.h"
 
-//Will probably refactor this to be event driven
 class I2CDriver 
 {
 public:
@@ -19,14 +19,67 @@ public:
         true;
 #endif
 
-    I2CDriver() = default;
-    ~I2CDriver() { i2c_driver_delete(I2C_NUM_0); }
+    enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
 
-    void run_task();
+    #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);
+        }
+    };
+    static_assert(sizeof(PacketIn) == 16, "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);
+        }
+    };
+    static_assert(sizeof(PacketOut) == 8, "PacketOut is misaligned");
+    #pragma pack(pop)
+
+    I2CDriver() = default;
+    ~I2CDriver();
 
     void initialize_i2c();
 
-    static inline esp_err_t i2c_write_blocking(uint8_t address, const uint8_t* buffer, size_t len) 
+    //Does not return
+    static void run_tasks(void* parameter);
+
+    //Thread safe
+    inline void push_task(std::function<void()> task)
+    { 
+        task_queue_.push(task); 
+    }
+
+    //Don't call directly from another thread, use in push_task
+    inline esp_err_t i2c_write_blocking(uint8_t address, const uint8_t* buffer, size_t len) 
     {
         i2c_cmd_handle_t cmd = i2c_cmd_link_create();
         i2c_master_start(cmd);
@@ -39,7 +92,8 @@ public:
         return ret;
     }
 
-    static inline esp_err_t i2c_read_blocking(uint8_t address, uint8_t* buffer, size_t len) 
+    //Don't call directly from another thread, use in push_task
+    inline esp_err_t i2c_read_blocking(uint8_t address, uint8_t* buffer, size_t len) 
     {
         i2c_cmd_handle_t cmd = i2c_cmd_link_create();
         i2c_master_start(cmd);
@@ -59,17 +113,8 @@ public:
     }
 
 private:
-//     static constexpr bool MULTI_SLAVE = 
-// #if CONFIG_MULTI_SLAVE_MODE == 0
-//         false;
-// #else
-//         true;
-// #endif
-    // std::array<ReportIn, CONFIG_BLUEPAD32_MAX_DEVICES> report_in_buffer_{};
-    // std::array<std::atomic<bool>, CONFIG_BLUEPAD32_MAX_DEVICES> new_report_in_{false};
-    // std::array<ReportOut, CONFIG_BLUEPAD32_MAX_DEVICES> report_out_buffer_{};
-
-
+    using TaskQueue = RingBuffer<std::function<void()>, 6>;
+    static TaskQueue task_queue_;
 };
 
 #endif // _I2C_DRIVER_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Reports.h b/Firmware/ESP32/main/Reports.h
deleted file mode 100644
index af092c0..0000000
--- a/Firmware/ESP32/main/Reports.h
+++ /dev/null
@@ -1,134 +0,0 @@
-#ifndef _REPORTS_H_
-#define _REPORTS_H_
-
-#include <cstdint>
-// #include <array>
-#include <cstring>
-// #include "freertos/FreeRTOS.h"
-// #include "freertos/semphr.h"
-
-enum class ReportID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
-
-#pragma pack(push, 1)
-struct ReportIn
-{
-    uint8_t report_len;
-    uint8_t report_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;
-
-    ReportIn()
-    {
-        std::memset(this, 0, sizeof(ReportIn));
-        report_len = sizeof(ReportIn);
-        report_id = static_cast<uint8_t>(ReportID::SET_PAD);
-    }
-};
-static_assert(sizeof(ReportIn) == 16, "ReportIn is misaligned");
-
-struct ReportOut
-{
-    uint8_t report_len;
-    uint8_t report_id;
-    uint8_t index;
-    uint8_t rumble_l;
-    uint8_t rumble_r;
-
-    ReportOut()
-    {
-        std::memset(this, 0, sizeof(ReportOut));
-        report_len = sizeof(ReportOut);
-        report_id = static_cast<uint8_t>(ReportID::GET_PAD);
-    }
-};
-static_assert(sizeof(ReportOut) == 5, "ReportOut is misaligned");
-#pragma pack(pop)
-
-// class ReportQueue
-// {
-// public:
-//     ReportQueue()
-//     {
-//         report_in_mutex_ = xSemaphoreCreateMutex();
-//         report_out_mutex_ = xSemaphoreCreateMutex();
-//     }
-
-//     ~ReportQueue()
-//     {
-//         vSemaphoreDelete(report_in_mutex_);
-//         vSemaphoreDelete(report_out_mutex_);
-//     }
-
-//     void set_report_in(const ReportIn* report)
-//     {
-//         if (xSemaphoreTake(report_in_mutex_, portMAX_DELAY))
-//         {
-//             std::memcpy(&report_in_, report, sizeof(ReportIn));
-//             new_report_in_ = true;
-//             xSemaphoreGive(report_in_mutex_);
-//         }
-//     }
-
-//     void set_report_out(const ReportOut* report)
-//     {
-//         if (xSemaphoreTake(report_out_mutex_, portMAX_DELAY))
-//         {
-//             std::memcpy(&report_out_, report, sizeof(ReportOut));
-//             new_report_out_ = true;
-//             xSemaphoreGive(report_out_mutex_);
-//         }
-//     }
-
-//     //Return false if no new data
-//     bool get_report_in(ReportIn* report)
-//     {
-//         if (xSemaphoreTake(report_in_mutex_, portMAX_DELAY))
-//         {
-//             if (new_report_in_)
-//             {
-//                 std::memcpy(report, &report_in_, sizeof(ReportIn));
-//                 new_report_in_ = false;
-//                 xSemaphoreGive(report_in_mutex_);
-//                 return true;
-//             }
-//             xSemaphoreGive(report_in_mutex_);
-//         }
-//         return false;
-//     }
-
-//     //Return false if no new data
-//     bool get_report_out(ReportOut* report)
-//     {
-//         if (xSemaphoreTake(report_out_mutex_, portMAX_DELAY))
-//         {
-//             if (new_report_out_)
-//             {
-//                 std::memcpy(report, &report_out_, sizeof(ReportOut));
-//                 new_report_out_ = false;
-//                 xSemaphoreGive(report_out_mutex_);
-//                 return true;
-//             }
-//             xSemaphoreGive(report_out_mutex_);
-//         }
-//         return false;
-//     }
-
-// private:
-//     SemaphoreHandle_t report_in_mutex_;
-//     SemaphoreHandle_t report_out_mutex_;
-
-//     ReportIn report_in_;
-//     ReportOut report_out_;
-
-//     bool new_report_in_{false};
-//     bool new_report_out_{false};
-// };
-
-#endif // _REPORTS_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/RingBuffer.h b/Firmware/ESP32/main/RingBuffer.h
new file mode 100644
index 0000000..61cde39
--- /dev/null
+++ b/Firmware/ESP32/main/RingBuffer.h
@@ -0,0 +1,57 @@
+#ifndef _RING_BUFFER_H_
+#define _RING_BUFFER_H_
+
+#include <cstdint>
+#include <atomic>
+#include <array>
+
+template<typename Type, size_t Size>
+class RingBuffer
+{
+public:
+    RingBuffer() : head_(0), tail_(0) {}
+
+    RingBuffer(const RingBuffer&) = delete;
+    RingBuffer& operator=(const RingBuffer&) = delete;
+
+    RingBuffer(RingBuffer&&) = default;
+    RingBuffer& operator=(RingBuffer&&) = default;
+
+    bool push(const Type& item)
+    {
+        size_t head = head_.load(std::memory_order_relaxed);
+        size_t next_head = (head + 1) % SIZE;
+
+        if (next_head == tail_.load(std::memory_order_acquire))
+        {
+            tail_.store((tail_.load(std::memory_order_relaxed) + 1) % SIZE, std::memory_order_release);
+        }
+
+        buffer_[head] = item;
+        head_.store(next_head, std::memory_order_release);
+        return true;
+    }
+
+    bool pop(Type& item)
+    {
+        size_t tail = tail_.load(std::memory_order_relaxed);
+
+        if (tail == head_.load(std::memory_order_acquire))
+        {
+            return false;
+        }
+
+        item = buffer_[tail];
+        tail_.store((tail + 1) % SIZE, std::memory_order_release);
+        return true;
+    }
+
+private:
+    const size_t SIZE = Size;
+
+    std::array<Type, Size> buffer_;
+    std::atomic<size_t> head_;
+    std::atomic<size_t> tail_;
+};
+
+#endif // _RING_BUFFER_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/c_wrapper.cpp b/Firmware/ESP32/main/c_wrapper.cpp
index 55eb5a2..8ea03df 100644
--- a/Firmware/ESP32/main/c_wrapper.cpp
+++ b/Firmware/ESP32/main/c_wrapper.cpp
@@ -1,14 +1,24 @@
+#include <functional>
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+
 #include "c_wrapper.h"
-#include "I2CDriver/I2CDriver.h"
+#include "sdkconfig.h"
 #include "Bluepad32/Bluepad32.h"
 
-void run_bluepad32()
+void bp32_run_task(void* parameter)
 {
-    bluepad32::run_task();
+    BP32::run_task();
 }
 
-void run_i2c()
+void cpp_main()
 {
-    I2CDriver i2c_driver;
-    i2c_driver.run_task();
+    xTaskCreatePinnedToCore(
+        bp32_run_task,
+        "bp32",
+        2048 * 4,
+        NULL,
+        configMAX_PRIORITIES-4,
+        NULL,
+        0 );
 }
\ No newline at end of file
diff --git a/Firmware/ESP32/main/c_wrapper.h b/Firmware/ESP32/main/c_wrapper.h
index 2356b5a..a972233 100644
--- a/Firmware/ESP32/main/c_wrapper.h
+++ b/Firmware/ESP32/main/c_wrapper.h
@@ -7,8 +7,7 @@
 extern "C" {
 #endif
 
-void run_bluepad32();
-void run_i2c();
+void cpp_main();
 
 #ifdef __cplusplus
 }
diff --git a/Firmware/ESP32/main/main.c b/Firmware/ESP32/main/main.c
index 5c776bc..09f9591 100644
--- a/Firmware/ESP32/main/main.c
+++ b/Firmware/ESP32/main/main.c
@@ -1,31 +1,9 @@
 #include <stdlib.h>
 #include <stdint.h>
-#include <freertos/FreeRTOS.h>
-#include <freertos/task.h>
 
-#include "sdkconfig.h"
 #include "c_wrapper.h"
 
-#define STACK_MULTIPLIER (2048 * CONFIG_BLUEPAD32_MAX_DEVICES)
-
 void app_main(void) 
 {
-    // run_bluepad32();
-    xTaskCreatePinnedToCore(
-        run_bluepad32,
-        "bp32",
-        STACK_MULTIPLIER * 4,
-        NULL,
-        configMAX_PRIORITIES-6,
-        NULL,
-        0 );
-
-    xTaskCreatePinnedToCore(
-        run_i2c,
-        "i2c",
-        STACK_MULTIPLIER * 2,
-        NULL,
-        configMAX_PRIORITIES-8,
-        NULL,
-        1 );
+    cpp_main();
 }   
\ No newline at end of file
diff --git a/Firmware/ESP32/sdkconfig b/Firmware/ESP32/sdkconfig
index b3d81ff..1363948 100644
--- a/Firmware/ESP32/sdkconfig
+++ b/Firmware/ESP32/sdkconfig
@@ -1,6 +1,6 @@
 #
 # Automatically generated file. DO NOT EDIT.
-# Espressif IoT Development Framework (ESP-IDF) 5.1.5 Project Configuration
+# Espressif IoT Development Framework (ESP-IDF)  Project Configuration
 #
 CONFIG_SOC_BROWNOUT_RESET_SUPPORTED="Not determined"
 CONFIG_SOC_TWAI_BRP_DIV_SUPPORTED="Not determined"
@@ -362,11 +362,11 @@ CONFIG_ESP_CUSTOM_BOARD=y
 # CONFIG_COMPILER_OPTIMIZATION_SIZE is not set
 CONFIG_COMPILER_OPTIMIZATION_PERF=y
 # CONFIG_COMPILER_OPTIMIZATION_NONE is not set
-CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_ENABLE=y
+# CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_ENABLE is not set
 # CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_SILENT is not set
-# CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_DISABLE is not set
+CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_DISABLE=y
 CONFIG_COMPILER_FLOAT_LIB_FROM_GCCLIB=y
-CONFIG_COMPILER_OPTIMIZATION_ASSERTION_LEVEL=2
+CONFIG_COMPILER_OPTIMIZATION_ASSERTION_LEVEL=0
 # CONFIG_COMPILER_OPTIMIZATION_CHECKS_SILENT is not set
 CONFIG_COMPILER_HIDE_PATHS_MACROS=y
 # CONFIG_COMPILER_CXX_EXCEPTIONS is not set
@@ -1358,9 +1358,7 @@ CONFIG_FREERTOS_DEBUG_OCDAWARE=y
 #
 CONFIG_HAL_ASSERTION_EQUALS_SYSTEM=y
 # CONFIG_HAL_ASSERTION_DISABLE is not set
-# CONFIG_HAL_ASSERTION_SILENT is not set
-# CONFIG_HAL_ASSERTION_ENABLE is not set
-CONFIG_HAL_DEFAULT_ASSERTION_LEVEL=2
+CONFIG_HAL_DEFAULT_ASSERTION_LEVEL=0
 CONFIG_HAL_SPI_MASTER_FUNC_IN_IRAM=y
 CONFIG_HAL_SPI_SLAVE_FUNC_IN_IRAM=y
 # CONFIG_HAL_ECDSA_GEN_SIG_CM is not set
@@ -1546,7 +1544,6 @@ CONFIG_LWIP_DNS_MAX_SERVERS=3
 # end of DNS
 
 CONFIG_LWIP_BRIDGEIF_MAX_PORTS=7
-CONFIG_LWIP_ESP_LWIP_ASSERT=y
 
 #
 # Hooks
@@ -1981,11 +1978,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 is not set
-CONFIG_BLUEPAD32_LOG_LEVEL_ERROR=y
+CONFIG_BLUEPAD32_LOG_LEVEL_NONE=y
+# CONFIG_BLUEPAD32_LOG_LEVEL_ERROR is not set
 # CONFIG_BLUEPAD32_LOG_LEVEL_INFO is not set
 # CONFIG_BLUEPAD32_LOG_LEVEL_DEBUG is not set
-CONFIG_BLUEPAD32_LOG_LEVEL=1
+CONFIG_BLUEPAD32_LOG_LEVEL=0
 # CONFIG_BLUEPAD32_USB_CONSOLE_ENABLE is not set
 CONFIG_BLUEPAD32_ENABLE_BLE_BY_DEFAULT=y
 CONFIG_BLUEPAD32_MAX_ALLOWLIST=4
@@ -2025,10 +2022,10 @@ CONFIG_MONITOR_BAUD=115200
 # CONFIG_COMPILER_OPTIMIZATION_LEVEL_DEBUG is not set
 # CONFIG_OPTIMIZATION_LEVEL_RELEASE is not set
 # CONFIG_COMPILER_OPTIMIZATION_LEVEL_RELEASE is not set
-CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y
+# CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED is not set
 # CONFIG_OPTIMIZATION_ASSERTIONS_SILENT is not set
-# CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED is not set
-CONFIG_OPTIMIZATION_ASSERTION_LEVEL=2
+CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED=y
+CONFIG_OPTIMIZATION_ASSERTION_LEVEL=0
 # CONFIG_CXX_EXCEPTIONS is not set
 CONFIG_STACK_CHECK_NONE=y
 # CONFIG_STACK_CHECK_NORM is not set
@@ -2372,7 +2369,6 @@ CONFIG_TIMER_TASK_PRIORITY=1
 CONFIG_TIMER_TASK_STACK_DEPTH=4096
 CONFIG_TIMER_QUEUE_LENGTH=10
 # CONFIG_ENABLE_STATIC_TASK_CLEAN_UP_HOOK is not set
-# CONFIG_HAL_ASSERTION_SILIENT is not set
 # CONFIG_L2_TO_L3_COPY is not set
 CONFIG_ESP_GRATUITOUS_ARP=y
 CONFIG_GARP_TMR_INTERVAL=60
diff --git a/Firmware/RP2040/.vscode/settings.json b/Firmware/RP2040/.vscode/settings.json
index 67c86c2..da0fd3d 100644
--- a/Firmware/RP2040/.vscode/settings.json
+++ b/Firmware/RP2040/.vscode/settings.json
@@ -1,15 +1,23 @@
 {
     "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
+    "cmake.buildEnvironment": {
+        "PICO_SDK_PATH": "C:/Programming/pico-sdk"
+    },
+    "cmake.environment": {
+        "PICO_SDK_PATH": "C:/Programming/pico-sdk"
+    },
+    "cmake.configureArgs": [
+        "-DOGXM_BOARD=PI_PICO",
+        "-DMAX_GAMEPADS=1"
+    ],
     "files.associations": {
         "array": "cpp",
-        "any": "cpp",
         "atomic": "cpp",
-        "barrier": "cpp",
+        "any": "cpp",
         "bit": "cpp",
         "*.tcc": "cpp",
         "bitset": "cpp",
         "cctype": "cpp",
-        "cfenv": "cpp",
         "charconv": "cpp",
         "chrono": "cpp",
         "cinttypes": "cpp",
@@ -17,11 +25,7 @@
         "cmath": "cpp",
         "codecvt": "cpp",
         "compare": "cpp",
-        "complex": "cpp",
         "concepts": "cpp",
-        "condition_variable": "cpp",
-        "coroutine": "cpp",
-        "csetjmp": "cpp",
         "csignal": "cpp",
         "cstdarg": "cpp",
         "cstddef": "cpp",
@@ -40,91 +44,44 @@
         "set": "cpp",
         "string": "cpp",
         "unordered_map": "cpp",
-        "unordered_set": "cpp",
         "vector": "cpp",
         "exception": "cpp",
-        "expected": "cpp",
         "algorithm": "cpp",
         "functional": "cpp",
         "iterator": "cpp",
         "memory": "cpp",
         "memory_resource": "cpp",
-        "netfwd": "cpp",
         "numeric": "cpp",
         "optional": "cpp",
         "random": "cpp",
         "ratio": "cpp",
         "regex": "cpp",
-        "source_location": "cpp",
         "string_view": "cpp",
         "system_error": "cpp",
         "tuple": "cpp",
         "type_traits": "cpp",
         "utility": "cpp",
-        "rope": "cpp",
-        "slist": "cpp",
         "format": "cpp",
         "fstream": "cpp",
-        "future": "cpp",
         "initializer_list": "cpp",
         "iomanip": "cpp",
         "iosfwd": "cpp",
         "iostream": "cpp",
         "istream": "cpp",
-        "latch": "cpp",
         "limits": "cpp",
-        "mutex": "cpp",
         "new": "cpp",
         "numbers": "cpp",
         "ostream": "cpp",
         "ranges": "cpp",
-        "scoped_allocator": "cpp",
         "semaphore": "cpp",
-        "shared_mutex": "cpp",
         "span": "cpp",
-        "spanstream": "cpp",
         "sstream": "cpp",
-        "stacktrace": "cpp",
         "stdexcept": "cpp",
-        "stdfloat": "cpp",
         "stop_token": "cpp",
         "streambuf": "cpp",
-        "syncstream": "cpp",
         "thread": "cpp",
-        "typeindex": "cpp",
         "typeinfo": "cpp",
         "valarray": "cpp",
-        "variant": "cpp",
-        "xlocmon": "cpp",
-        "xtr1common": "cpp",
-        "xutility": "cpp",
-        "ios": "cpp",
-        "locale": "cpp",
-        "queue": "cpp",
-        "xfacet": "cpp",
-        "xhash": "cpp",
-        "xiosbase": "cpp",
-        "xlocale": "cpp",
-        "xlocbuf": "cpp",
-        "xlocinfo": "cpp",
-        "xlocmes": "cpp",
-        "xlocnum": "cpp",
-        "xloctime": "cpp",
-        "xmemory": "cpp",
-        "xstring": "cpp",
-        "filesystem": "cpp",
-        "flash.h": "c",
-        "uart_bridge.h": "c",
-        "usbh.h": "c"
-    },
-    "cmake.buildEnvironment": {
-        "PICO_SDK_PATH": "C:/Programming/pico-sdk"
-    },
-    "cmake.environment": {
-        "PICO_SDK_PATH": "C:/Programming/pico-sdk"
-    },
-    "cmake.configureArgs": [
-        "-DOGXM_BOARD=EXTERNAL_4CH",
-        "-DMAX_GAMEPADS=4"
-    ]
+        "variant": "cpp"
+    }
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/CMakeLists.txt b/Firmware/RP2040/CMakeLists.txt
index f923a36..6ab9fde 100644
--- a/Firmware/RP2040/CMakeLists.txt
+++ b/Firmware/RP2040/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(FW_NAME "OGX-Mini")
-set(FW_VERSION "v1.0.0-alpha")
+set(FW_VERSION "v1.0.0a")
 
 set(CMAKE_C_COMPILER arm-none-eabi-gcc)
 set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
@@ -19,14 +19,6 @@ include(${CMAKE_CURRENT_LIST_DIR}/cmake/get_pico_sdk.cmake)
 apply_lib_patches(${EXTERNAL_DIR})
 get_pico_sdk(${EXTERNAL_DIR})
 
-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}")
-endif()
-
-project(${FW_NAME} C CXX ASM)
-
 set(PICO_PIO_USB_PATH ${EXTERNAL_DIR}/Pico-PIO-USB)
 set(PICO_TINYUSB_PATH ${EXTERNAL_DIR}/tinyusb)
 set(BLUEPAD32_ROOT ${EXTERNAL_DIR}/bluepad32)
@@ -37,7 +29,7 @@ set(SOURCES_BOARD
     ${SRC}/main.cpp
 
     ${SRC}/OGXMini/OGXMini_Standard.cpp
-    ${SRC}/OGXMini/OGXMini_4Ch.cpp
+    ${SRC}/OGXMini/OGXMini_4Channel.cpp
     ${SRC}/OGXMini/OGXMini_PicoW.cpp
     ${SRC}/OGXMini/OGXMini_ESP32.cpp
 
@@ -81,6 +73,7 @@ set(INC_DIRS_BOARD
 )
 
 # Config options
+
 # Max gamepads
 set(MAX_GAMEPADS 1 CACHE STRING "Set number of gamepads, 1 to 4")
 if (MAX_GAMEPADS GREATER 4 OR MAX_GAMEPADS LESS 1)
@@ -97,6 +90,11 @@ set(PICO_BOARD none)
 if (OGXM_BOARD STREQUAL "PI_PICO")
     set(EN_USB_HOST TRUE)
 
+elseif (OGXM_BOARD STREQUAL "PI_PICO2")
+    set(EN_USB_HOST TRUE)
+    set(PICO_PLATFORM rp2350)
+    set(FLASH_SIZE_MB 4)
+
 elseif(OGXM_BOARD STREQUAL "ADA_FEATHER")
     set(EN_USB_HOST TRUE)
     set(EN_RGB TRUE)
@@ -135,28 +133,29 @@ if(EN_USB_HOST)
     add_compile_definitions(CONFIG_EN_USB_HOST=1)
     list(APPEND SOURCES_BOARD
         ${SRC}/USBHost/tuh_callbacks.cpp
-
-        # ${SRC}/USBHost/HostDriver/DInput/DInput.cpp
-        # ${SRC}/USBHost/HostDriver/PSClassic/PSClassic.cpp
-        # ${SRC}/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
-        # ${SRC}/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
-        # ${SRC}/USBHost/HostDriver/PS5/PS5.cpp
-        # ${SRC}/USBHost/HostDriver/PS4/PS4.cpp
-        # ${SRC}/USBHost/HostDriver/PS3/PS3.cpp
-        # ${SRC}/USBHost/HostDriver/N64/N64.cpp
-        # ${SRC}/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
-
-        ${SRC}/USBHost/HostDriver/XInput/XboxOG.cpp
-        ${SRC}/USBHost/HostDriver/XInput/XboxOne.cpp
-        ${SRC}/USBHost/HostDriver/XInput/Xbox360.cpp
-        ${SRC}/USBHost/HostDriver/XInput/Xbox360W.cpp
-        ${SRC}/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.cpp
+        # HID
+        ${SRC}/USBHost/HostDriver/DInput/DInput.cpp
+        ${SRC}/USBHost/HostDriver/PSClassic/PSClassic.cpp
+        ${SRC}/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
+        ${SRC}/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
+        ${SRC}/USBHost/HostDriver/PS5/PS5.cpp
+        ${SRC}/USBHost/HostDriver/PS4/PS4.cpp
+        ${SRC}/USBHost/HostDriver/PS3/PS3.cpp
+        ${SRC}/USBHost/HostDriver/N64/N64.cpp
+        ${SRC}/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
 
         ${SRC}/USBHost/HIDParser/HIDJoystick.cpp
         ${SRC}/USBHost/HIDParser/HIDReportDescriptor.cpp
         ${SRC}/USBHost/HIDParser/HIDReportDescriptorElements.cpp
         ${SRC}/USBHost/HIDParser/HIDReportDescriptorUsages.cpp
         ${SRC}/USBHost/HIDParser/HIDUtils.cpp
+
+        # XInput
+        ${SRC}/USBHost/HostDriver/XInput/XboxOG.cpp
+        ${SRC}/USBHost/HostDriver/XInput/XboxOne.cpp
+        ${SRC}/USBHost/HostDriver/XInput/Xbox360.cpp
+        ${SRC}/USBHost/HostDriver/XInput/Xbox360W.cpp
+        ${SRC}/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.cpp
     )
     list(APPEND LIBS_BOARD
         tinyusb_host
@@ -211,7 +210,7 @@ if(EN_ESP32)
     add_compile_definitions(CONFIG_EN_ESP32=1)
     message(STATUS "ESP32 enabled.")
     list(APPEND SOURCES_BOARD
-        ${SRC}/I2CDriver/i2c_driver_esp32.cpp
+        ${SRC}/I2CDriver/ESP32/I2CDriver.cpp
     )
     list(APPEND LIBS_BOARD
         hardware_i2c
@@ -238,31 +237,45 @@ if(EXISTS ${SRC}/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid_xremote_rom.h)
 endif()
 
 if(NOT EN_BLUETOOTH)
-    # add_compile_definitions(PICO_BOOT_STAGE2_CHOOSE_GENERIC_03H=1 PICO_XOSC_STARTUP_DELAY_MULTIPLIER=64)
     add_compile_definitions(PICO_XOSC_STARTUP_DELAY_MULTIPLIER=64)
 endif()
 
+include(${PICO_SDK_PATH}/pico_sdk_init.cmake)
+
+message("PICO_SDK_VERSION_STRING: ${PICO_SDK_VERSION_STRING}")
+if(PICO_SDK_VERSION_STRING VERSION_LESS "2.1.0")
+    message(FATAL_ERROR "Raspberry Pi Pico SDK version 2.1.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}")
+endif()
+
+project(${FW_NAME} C CXX ASM)
+
 pico_sdk_init()
-# pico_enable_stdio_usb(${FW_NAME} 0)
 
 add_executable(${FW_NAME} ${SOURCES_BOARD})
 
-if(CMAKE_BUILD_TYPE STREQUAL "Debug")
-    pico_enable_stdio_uart(${FW_NAME} 1)
-    add_compile_definitions(CFG_TUSB_DEBUG=2)
-    add_compile_definitions(LOG=2)
-    # target_compile_options(${FW_NAME} PRIVATE
-    #     -Wall                # Enable most warnings
-    #     -Wextra              # Enable extra warnings
-    #     -Wconversion         # Warn on type conversion issues
-    #     -Wsign-conversion    # Warn on sign conversion issues
-    #     # -Werror              # Treat warnings as errors
-    # )
+set(BUILD_STR "")
 
-    add_link_options(
-        -Wl,--gc-sections
+if(CMAKE_BUILD_TYPE STREQUAL "Debug")
+    set(BUILD_STR "-Debug")
+    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
+    )
+    add_compile_definitions(LOG=1)
+    add_compile_definitions(CFG_TUSB_DEBUG=1)
+    add_compile_definitions(OGXM_DEBUG=1)
+    target_compile_options(${FW_NAME} PRIVATE
+        -Wall                # Enable most warnings
+        -Wextra              # Enable extra warnings
+        -Wconversion         # Warn on type conversion issues
+        -Wsign-conversion    # Warn on sign conversion issues
+        -Wno-unused-parameter # Disable warnings for unused parameters
+        # -Werror              # Treat warnings as errors
     )
 elseif(CMAKE_BUILD_TYPE STREQUAL "Release")
+    pico_enable_stdio_uart(${FW_NAME} 0)
     add_compile_definitions(CFG_TUSB_DEBUG=0)
     add_compile_options(
         -O3                     # Optimize for speed
@@ -301,7 +314,7 @@ endif()
 
 target_link_libraries(${FW_NAME} PRIVATE ${LIBS_BOARD})
 
-set(EXE_FILENAME "${FW_NAME}-${FW_VERSION}-${OGXM_BOARD}")
+set(EXE_FILENAME "${FW_NAME}-${FW_VERSION}-${OGXM_BOARD}${BUILD_STR}")
 set_target_properties(${FW_NAME} PROPERTIES OUTPUT_NAME ${EXE_FILENAME})
 
 pico_add_extra_outputs(${FW_NAME})
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp b/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
index c0b0ea4..8beba8e 100644
--- a/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
+++ b/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
@@ -29,8 +29,10 @@ struct BTDevice
 };
 
 BTDevice bt_devices_[MAX_GAMEPADS];
+btstack_timer_source_t feedback_timer_;
 btstack_timer_source_t led_timer_;
 bool led_timer_set_{false};
+bool feedback_timer_set_{false};
 
 //This solves a function pointer/crash issue with bluepad32
 void set_rumble(uni_hid_device_t* bp_device, uint16_t length, uint8_t rumble_l, uint8_t rumble_r)
@@ -117,11 +119,7 @@ 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_bt_del_keys_unsafe();
 
     uni_property_dump_all();
 }
@@ -155,6 +153,11 @@ static void device_disconnected_cb(uni_hid_device_t* device)
         btstack_run_loop_set_timer(&led_timer_, LED_CHECK_TIME_MS);
         btstack_run_loop_add_timer(&led_timer_);
     }
+    if (feedback_timer_set_ && !any_connected())
+    {
+        feedback_timer_set_ = false;
+        btstack_run_loop_remove_timer(&feedback_timer_);
+    }
 }
 
 static uni_error_t device_ready_cb(uni_hid_device_t* device) 
@@ -172,6 +175,14 @@ static uni_error_t device_ready_cb(uni_hid_device_t* device)
         btstack_run_loop_remove_timer(&led_timer_);
         board_api::set_led(true);
     }
+    if (!feedback_timer_set_)
+    {
+        feedback_timer_set_ = true;
+        feedback_timer_.process = send_feedback_cb;
+        feedback_timer_.context = nullptr;
+        btstack_run_loop_set_timer(&feedback_timer_, FEEDBACK_TIME_MS);
+        btstack_run_loop_add_timer(&feedback_timer_);
+    }
     return UNI_ERROR_SUCCESS;
 }
 
@@ -283,12 +294,6 @@ void run_task(Gamepad (&gamepads)[MAX_GAMEPADS])
     uni_platform_set_custom(get_driver());
     uni_init(0, nullptr);
 
-    btstack_timer_source_t feedback_timer;
-    feedback_timer.process = send_feedback_cb;
-    feedback_timer.context = nullptr;
-    btstack_run_loop_set_timer(&feedback_timer, FEEDBACK_TIME_MS);
-    btstack_run_loop_add_timer(&feedback_timer);
-
     led_timer_set_ = true;
     led_timer_.process = check_led_cb;
     led_timer_.context = nullptr;
@@ -298,16 +303,6 @@ void run_task(Gamepad (&gamepads)[MAX_GAMEPADS])
     btstack_run_loop_execute();
 }
 
-std::array<bool, MAX_GAMEPADS> get_connected_map()
-{
-    std::array<bool, MAX_GAMEPADS> mounted_map;
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        mounted_map[i] = bt_devices_[i].connected;
-    }
-    return mounted_map;
-}
-
 bool any_connected()
 {
     for (auto& device : bt_devices_)
diff --git a/Firmware/RP2040/src/Bluepad32/Bluepad32.h b/Firmware/RP2040/src/Bluepad32/Bluepad32.h
index 2579b50..2d7e76a 100644
--- a/Firmware/RP2040/src/Bluepad32/Bluepad32.h
+++ b/Firmware/RP2040/src/Bluepad32/Bluepad32.h
@@ -14,7 +14,6 @@
 namespace bluepad32 
 {
     void run_task(Gamepad (&gamepads)[MAX_GAMEPADS]);
-    // std::array<bool, MAX_GAMEPADS> get_connected_map();
     bool any_connected();
 
 } //namespace bluepad32
diff --git a/Firmware/RP2040/src/Board/Pico_WS2812/WS2812.cpp b/Firmware/RP2040/src/Board/Pico_WS2812/WS2812.cpp
index f852e64..e3998db 100644
--- a/Firmware/RP2040/src/Board/Pico_WS2812/WS2812.cpp
+++ b/Firmware/RP2040/src/Board/Pico_WS2812/WS2812.cpp
@@ -1,12 +1,11 @@
+/*  
+    Copyright ForsakenNGS 2021
+    https://github.com/ForsakenNGS/Pico_WS2812 
+*/
+
 #include "Board/Pico_WS2812/WS2812.hpp"
 #include "WS2812.pio.h"
 
-//#define DEBUG
-
-#ifdef DEBUG
-#include <stdio.h>
-#endif
-
 WS2812::WS2812(uint pin, uint length, PIO pio, uint sm)  {
     initialize(pin, length, pio, sm, NONE, GREEN, RED, BLUE);
 }
@@ -42,16 +41,14 @@ void WS2812::initialize(uint pin, uint length, PIO pio, uint sm, DataByte b1, Da
     this->length = length;
     this->pio = pio;
     this->sm = sm;
-    this->data.fill(0);
+    this->data = new uint32_t[length];
     this->bytes[0] = b1;
     this->bytes[1] = b2;
     this->bytes[2] = b3;
     this->bytes[3] = b4;
     uint offset = pio_add_program(pio, &ws2812_program);
     uint bits = (b1 == NONE ? 24 : 32);
-    #ifdef DEBUG
-    printf("WS2812 / Initializing SM %u with offset %X at pin %u and %u data bits...\n", sm, offset, pin, bits);
-    #endif
+
     ws2812_program_init(pio, sm, offset, pin, 800000, bits);
 }
 
@@ -111,11 +108,6 @@ void WS2812::fill(uint32_t color, uint first, uint count) {
 }
 
 void WS2812::show() {
-    #ifdef DEBUG
-    for (uint i = 0; i < length; i++) {
-        printf("WS2812 / Put data: %08X\n", data[i]);
-    }
-    #endif
     for (uint i = 0; i < length; i++) {
         pio_sm_put_blocking(pio, sm, data[i]);
     }
diff --git a/Firmware/RP2040/src/Board/Pico_WS2812/WS2812.hpp b/Firmware/RP2040/src/Board/Pico_WS2812/WS2812.hpp
index bb3a165..0420eac 100644
--- a/Firmware/RP2040/src/Board/Pico_WS2812/WS2812.hpp
+++ b/Firmware/RP2040/src/Board/Pico_WS2812/WS2812.hpp
@@ -1,3 +1,8 @@
+/*  
+    Copyright ForsakenNGS 2021
+    https://github.com/ForsakenNGS/Pico_WS2812 
+*/
+
 #ifndef WS2812_H
 #define WS2812_H
 
@@ -48,7 +53,7 @@ class WS2812 {
         PIO pio;
         uint sm;
         DataByte bytes[4];
-        std::array<uint32_t, 20> data;
+        uint32_t *data;
 
         void initialize(uint pin, uint length, PIO pio, uint sm, DataByte b1, DataByte b2, DataByte b3, DataByte b4);
         uint32_t convertData(uint32_t rgbw);
diff --git a/Firmware/RP2040/src/Board/board_api.cpp b/Firmware/RP2040/src/Board/board_api.cpp
index 178bb63..2850ee8 100644
--- a/Firmware/RP2040/src/Board/board_api.cpp
+++ b/Firmware/RP2040/src/Board/board_api.cpp
@@ -1,11 +1,11 @@
-/* This is too messy I'll clean it up at some point */
-
 #include <pico/stdlib.h>
 #include <pico/mutex.h>
+#include <hardware/clocks.h>
 #include <hardware/gpio.h>
 #include <hardware/watchdog.h>
 
 #include "Board/board_api.h"
+#include "OGXMini/Debug.h"
 #include "board_config.h"
 
 #if defined(CONFIG_EN_BLUETOOTH)
@@ -22,45 +22,35 @@ namespace board_api {
 bool inited_ = false;
 mutex_t gpio_mutex_;
 
-#if defined(CONFIG_EN_RGB)
-WS2812 ws2812_ = WS2812(RGB_PXL_PIN, 1, pio1, 0, WS2812::FORMAT_GRB);
-#endif // defined(CONFIG_EN_RGB)
-
-void init_gpio()
-{   
-    if (inited_)
-    {
-        return;
-    }
-    if (!mutex_is_initialized(&gpio_mutex_))
-    {
-        mutex_init(&gpio_mutex_); 
-    }
-    mutex_enter_blocking(&gpio_mutex_);
-
-#ifdef VCC_EN_PIN
+void init_vcc_en_pin_unsafe()
+{
+#if defined(VCC_EN_PIN)
     gpio_init(VCC_EN_PIN);
     gpio_set_dir(VCC_EN_PIN, GPIO_OUT);
     gpio_put(VCC_EN_PIN, 1);
 #endif 
+}
 
-#if defined(CONFIG_EN_RGB)
-    #ifdef RGB_PWR_PIN
+void init_rgb_unsafe()
+{
+#if defined(CONFIG_EN_RGB) && defined(RGB_PWR_PIN)
         gpio_init(RGB_PWR_PIN);
         gpio_set_dir(RGB_PWR_PIN, GPIO_OUT);
         gpio_put(RGB_PWR_PIN, 1);
-    #endif
+#endif // defined(CONFIG_EN_RGB) && defined(RGB_PWR_PIN)
+}
 
-#elif defined(CONFIG_EN_BLUETOOTH)
-    //
-
-#elif defined(LED_INDICATOR_PIN)    
+void init_led_indicator_unsafe()
+{
+#if defined(LED_INDICATOR_PIN) && !defined(CONFIG_EN_BLUETOOTH)   
     gpio_init(LED_INDICATOR_PIN);
     gpio_set_dir(LED_INDICATOR_PIN, GPIO_OUT);
     gpio_put(LED_INDICATOR_PIN, 0);
+#endif // defined(LED_INDICATOR_PIN)
+}
 
-#endif
-
+void init_esp32_io_unsafe()
+{
 #if defined(CONFIG_EN_ESP32)
     gpio_init(ESP_PROG_PIN);
     gpio_set_dir(ESP_PROG_PIN, GPIO_OUT);
@@ -71,31 +61,46 @@ void init_gpio()
     gpio_put(ESP_RST_PIN, 1);
 
 #endif //defined(CONFIG_EN_ESP32)
+}
 
+void init_uart_bridge_io_unsafe()
+{
 #if defined(CONFIG_EN_UART_BRIDGE)
     gpio_init(MODE_SEL_PIN);
     gpio_set_dir(MODE_SEL_PIN, GPIO_IN);
     gpio_pull_up(MODE_SEL_PIN);
 
 #endif // defined(CONFIG_EN_UART_BRIDGE)
+}
 
-    inited_ = true;
-    mutex_exit(&gpio_mutex_);    
+void init_uart_debug_unsafe()
+{
+#if defined(OGXM_DEBUG)
+    uart_init(DEBUG_UART_PORT, PICO_DEFAULT_UART_BAUD_RATE);
+    gpio_set_function(PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART);
+    gpio_set_function(PICO_DEFAULT_UART_RX_PIN, GPIO_FUNC_UART);
+#endif // defined(OGXM_DEBUG)
 }
 
 void set_led(bool state)
 {
-    if (!inited_)
-    {
-        return;
-    }
     mutex_enter_blocking(&gpio_mutex_);
 
-#if defined(CONFIG_EN_RGB)
-    ws2812_.setPixelColor(0, state ? WS2812::RGB(0x00, 0xFF, 0x00) : WS2812::RGB(0xFF, 0, 0));
-    ws2812_.show();
+    if (!inited_)
+    {
+        mutex_exit(&gpio_mutex_);
+        return;
+    }
 
-#elif defined(CONFIG_EN_BLUETOOTH)
+#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)  
@@ -177,13 +182,43 @@ void reboot()
 
     AIRCR_REG = AIRCR_VECTKEY | AIRCR_SYSRESETREQ;
     while(1);
+}
 
-    // watchdog_enable(500, true); // 10-second timeout
-    // while (true)
-    // {
-    //     // Your main loop logic
-    //     watchdog_update(); // Reset the watchdog timer
-    // }
+void init_board()
+{
+    if (inited_)
+    {
+        return;
+    }
+
+    if (!set_sys_clock_khz(SYSCLOCK_KHZ, true) && !set_sys_clock_khz(120000, true))
+    {
+        panic("Failed to set sys clock");
+    }
+
+    stdio_init_all();
+
+    if (!mutex_is_initialized(&gpio_mutex_))
+    {
+        mutex_init(&gpio_mutex_); 
+    }
+    
+    mutex_enter_blocking(&gpio_mutex_);
+
+    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_);
+
+    set_led(false);
+
+    OGXM_LOG("Board initialized\n");
 }
 
 } // namespace board_api
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/board_api.h b/Firmware/RP2040/src/Board/board_api.h
index 23527c2..0eab62d 100644
--- a/Firmware/RP2040/src/Board/board_api.h
+++ b/Firmware/RP2040/src/Board/board_api.h
@@ -1,11 +1,12 @@
-#ifndef _BOARD_API_H_
-#define _BOARD_API_H_
+#ifndef _OGXM_BOARD_API_H_
+#define _OGXM_BOARD_API_H_
 
 #include <cstdint>
+#include <string>
 
 namespace board_api
 {
-    void init_gpio();
+    void init_board();
     void set_led(bool state);
     void reboot();
 
@@ -14,4 +15,4 @@ namespace board_api
     void enter_esp32_prog_mode();
 }
 
-#endif // _BOARD_API_H_
\ No newline at end of file
+#endif // _OGXM_BOARD_API_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Descriptors/CDCDev.h b/Firmware/RP2040/src/Descriptors/CDCDev.h
index 439888d..ed981bb 100644
--- a/Firmware/RP2040/src/Descriptors/CDCDev.h
+++ b/Firmware/RP2040/src/Descriptors/CDCDev.h
@@ -9,73 +9,58 @@
 namespace CDCDesc 
 {
     #define _PID_MAP(itf, n)  ( (CFG_TUD_##itf) << (n) )
-    #define USB_PID           (0x4000 | _PID_MAP(CDC, 0) | _PID_MAP(MSC, 1) | _PID_MAP(HID, 2) | \
-                              _PID_MAP(MIDI, 3) | _PID_MAP(VENDOR, 4) )
 
-    // #define USB_VID   0xCafe
-    // #define USB_BCD   0x0200
+    static const uint16_t VID = 0xCafe;
+    static const uint16_t PID = (0x4000 | _PID_MAP(CDC, 0));
+    static const uint16_t USB_VER = 0x0200;
 
-    static const tusb_desc_device_t DEVICE_DESCRIPTORS =
+    const tusb_desc_device_t DESC_DEVICE = 
     {
         .bLength            = sizeof(tusb_desc_device_t),
         .bDescriptorType    = TUSB_DESC_DEVICE,
-        .bcdUSB             = 0x0200,
-
+        .bcdUSB             = USB_VER,
         .bDeviceClass       = TUSB_CLASS_MISC,
         .bDeviceSubClass    = MISC_SUBCLASS_COMMON,
         .bDeviceProtocol    = MISC_PROTOCOL_IAD,
-
         .bMaxPacketSize0    = CFG_TUD_ENDPOINT0_SIZE,
-
-        .idVendor           = 0xCafe,
-        .idProduct          = USB_PID,
+        .idVendor           = VID,
+        .idProduct          = PID,
         .bcdDevice          = 0x0100,
-
         .iManufacturer      = 0x01,
         .iProduct           = 0x02,
         .iSerialNumber      = 0x03,
-
         .bNumConfigurations = 0x01
     };
 
     enum Itf
     {
-        NUM_CDC = 0,
-        NUM_CDC_DATA,
-        NUM_TOTAL
+        CDC_0 = 0,
+        CDC_0_DATA,
+        ITF_TOTAL
     };
 
-    #define EPNUM_CDC_NOTIF   0x81
-    #define EPNUM_CDC_OUT     0x02
-    #define EPNUM_CDC_IN      0x82
+    static const int CONFIG_LEN = (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN);
 
-    static constexpr int CONFIG_LEN = (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN);
-
-    static const uint8_t CONFIGURATION_DESCRIPTORS[] =
+    static const uint8_t DESC_CONFIG[] =
     {
-        TUD_CONFIG_DESCRIPTOR(1, Itf::NUM_TOTAL, 0, CONFIG_LEN, 0x00, 500),
-        TUD_CDC_DESCRIPTOR(Itf::NUM_CDC, 4, EPNUM_CDC_NOTIF, 8, EPNUM_CDC_OUT, EPNUM_CDC_IN, 64),
+        TUD_CONFIG_DESCRIPTOR(1, Itf::ITF_TOTAL, 0, CONFIG_LEN, 0x00, 500),
+        TUD_CDC_DESCRIPTOR(Itf::CDC_0, 4, 0x80 | (Itf::CDC_0 + 1), 8, (Itf::CDC_0 + 2), 0x80 | (Itf::CDC_0 + 2), 64),
     };
 
-    enum
+    static const uint8_t STRING_DESC_LANGUAGE[] = { 0x09, 0x04 };
+    static const uint8_t STRING_MANUFACTURER[]  = "Wired Opposite";
+    static const uint8_t STRING_PRODUCT[]       = "OGX-Mini";
+    static const uint8_t STRING_INTERFACE[]     = "OGX-Mini CDC";
+
+    static const uint8_t *DESC_STRING[] =
     {
-        STRID_LANGID = 0,
-        STRID_MANUFACTURER,
-        STRID_PRODUCT,
-        STRID_SERIAL,
+        STRING_DESC_LANGUAGE,
+        STRING_MANUFACTURER,
+        STRING_PRODUCT,
+        nullptr, //Serial
+        STRING_INTERFACE
     };
 
-    static const char *STRING_DESCRIPTORS[] =
-    {
-        (const char[]) { 0x09, 0x04 }, // 0: is supported language is English (0x0409)
-        "TinyUSB",                     // 1: Manufacturer
-        "TinyUSB Device",              // 2: Product
-        NULL,                          // 3: Serials will use unique ID if possible
-        "TinyUSB CDC",                 // 4: CDC Interface
-    };
-
-    static uint16_t _desc_str[32 + 1];
-
 }; // namespace CDCDesc
 
 #endif // _CDC_DEV_DESCRIPTORS_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Descriptors/DInput.h b/Firmware/RP2040/src/Descriptors/DInput.h
index 20dc4e0..e15c7ac 100644
--- a/Firmware/RP2040/src/Descriptors/DInput.h
+++ b/Firmware/RP2040/src/Descriptors/DInput.h
@@ -75,6 +75,11 @@ namespace DInput
         uint8_t r1_axis;
         uint8_t l2_axis;
         uint8_t r2_axis;
+
+        InReport()
+        {
+            std::memset(this, 0, sizeof(InReport));
+        }
     };
     #pragma pack(pop)
     static_assert(sizeof(InReport) == 19, "DInput::InReport is misaligned");
diff --git a/Firmware/RP2040/src/Descriptors/PS3.h b/Firmware/RP2040/src/Descriptors/PS3.h
index 129bbe6..e7fce0b 100644
--- a/Firmware/RP2040/src/Descriptors/PS3.h
+++ b/Firmware/RP2040/src/Descriptors/PS3.h
@@ -10,7 +10,7 @@
 namespace PS3 
 {
 	static constexpr uint8_t MAGIC_BYTES[8] = { 0x21, 0x26, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00 };
-	static constexpr uint8_t JOYSTICK_MID = 0x80;
+	static constexpr uint8_t JOYSTICK_MID = 0x7F;
 	static constexpr uint16_t SIXAXIS_MID = 0xFF01;
 
 	namespace ReportID 
@@ -94,22 +94,19 @@ namespace PS3
 	struct InReport
 	{
 		uint8_t report_id;
-		uint8_t reserved0;
+		uint8_t unk0;
 
 		uint8_t buttons[3];
-
-		uint8_t reserved1;
+		uint8_t unk1;
 
 		uint8_t joystick_lx;
 		uint8_t joystick_ly;
 		uint8_t joystick_rx;
 		uint8_t joystick_ry;
 
-		uint8_t reserved2[2];
-
+		uint8_t unk2[2];
 		uint8_t move_power_status;
-
-		uint8_t reserved3;
+		uint8_t unk3;
 
 		uint8_t up_axis;
 		uint8_t right_axis;
@@ -126,7 +123,7 @@ namespace PS3
 		uint8_t cross_axis;
 		uint8_t square_axis;
 
-		uint8_t reserved4[3];
+		uint8_t unk4[3];
 
 		uint8_t plugged;
 		uint8_t power_status;
@@ -137,8 +134,7 @@ namespace PS3
 		uint16_t acceler_x;
 		uint16_t acceler_y;
 		uint16_t acceler_z;
-
-		uint16_t velocity_z;
+		uint16_t gyro_z;
 
 		InReport()
 		{
@@ -151,44 +147,50 @@ namespace PS3
 			plugged = PlugState::PLUGGED;
 			power_status = PowerState::FULL;
 			rumble_status = RumbleState::WIRED_RUMBLE;
-			acceler_x = acceler_y = acceler_z = velocity_z = SIXAXIS_MID;
+			acceler_x = acceler_y = acceler_z = gyro_z = SIXAXIS_MID;
 		}
 	};
 	static_assert(sizeof(InReport) == 49, "PS3::InReport size mismatch");
 
-	struct LEDs {
-		uint8_t time_enabled; /* the total time the led is active (0xff means forever) */
-		uint8_t duty_length;  /* how long a cycle is in deciseconds (0 means "really fast") */
-		uint8_t enabled;
-		uint8_t duty_off; /* % of duty_length the led is off (0xff means 100%) */
-		uint8_t duty_on;  /* % of duty_length the led is on (0xff mean 100%) */
-	};
-	static_assert(sizeof(LEDs) == 5, "PS3::LEDs size mismatch");
-
-	struct Rumble {
-		uint8_t reserved;
-		uint8_t right_duration; /* Right motor duration (0xff means forever) */
-		uint8_t right_motor_on; /* Right (small) motor on/off, only supports values of 0 or 1 (off/on) */
-		uint8_t left_duration;    /* Left motor duration (0xff means forever) */
-		uint8_t left_motor_force; /* left (large) motor, supports force values from 0 to 255 */
-	};
-	static_assert(sizeof(Rumble) == 5, "PS3::Rumble size mismatch");
-
 	struct OutReport 
 	{
-		struct Rumble rumble;
-		uint8_t padding[4];
-		uint8_t leds_bitmap; /* bitmap of enabled LEDs: LED_1 = 0x02, LED_2 = 0x04, ... */
-		struct LEDs led[4];    /* LEDx at (4 - x) */
-		struct LEDs reserved; /* LED5, not actually soldered */
+		//uint8_t report_id;
+		uint8_t reserved0;
+		struct Rumble 
+		{
+			uint8_t right_duration;   /* Right motor duration (0xff means forever) */
+			uint8_t right_motor_on;   /* Right (small) motor on/off, only supports values of 0 or 1 (off/on) */
+			uint8_t left_duration;    /* Left motor duration (0xff means forever) */
+			uint8_t left_motor_force; /* left (large) motor, supports force values from 0 to 255 */
+		} rumble;
+		uint8_t reserved1[4];
+		uint8_t leds_bitmap;      /* bitmap of enabled LEDs: LED_1 = 0x02, LED_2 = 0x04, ... */
+		struct LEDs 
+		{
+			uint8_t time_enabled; /* the total time the led is active (0xff means forever) */
+			uint8_t duty_length;  /* how long a cycle is in deciseconds (0 means "really fast") */
+			uint8_t enabled;
+			uint8_t duty_off;     /* % of duty_length the led is off (0xff means 100%) */
+			uint8_t duty_on;      /* % of duty_length the led is on (0xff mean 100%) */
+		} leds[4];                /* LEDx at (4 - x) */
+		struct LEDs unused;       /* LED5, not actually soldered */
+		uint8_t reserved2[13];
 
 		OutReport()
 		{
-			std::memcpy(this, DEFAULT_OUT_REPORT, sizeof(OutReport));
+			std::memset(this, 0, sizeof(OutReport));
+			std::memcpy(this, DEFAULT_OUT_REPORT, sizeof(DEFAULT_OUT_REPORT));
 		}
 	};
-	static_assert(sizeof(OutReport) == 35, "PS3::OutReport size mismatch");
-	static_assert(sizeof(OutReport) == sizeof(DEFAULT_OUT_REPORT));
+	static_assert(sizeof(OutReport) == 48, "PS3::OutReport size mismatch");
+	static_assert(sizeof(OutReport) >= sizeof(DEFAULT_OUT_REPORT));
+
+	static constexpr uint8_t DEFAULT_BT_INFO_HEADER[] =
+	{
+		0xFF, 0xFF,
+		0x00, 0x20, 0x40, 0xCE, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	};
 
 	struct BTInfo
 	{
@@ -200,20 +202,19 @@ namespace PS3
 		BTInfo()
 		{
 			std::memset(this, 0, sizeof(BTInfo));
-			std::memset(reserved0, 0xFF, sizeof(reserved0));
-			uint8_t addr[] = { 0x00, 0x20, 0x40, 0xCE, 0x00, 0x00, 0x00 };
-			std::memcpy(device_address, addr, sizeof(addr));
+			std::memcpy(device_address, DEFAULT_BT_INFO_HEADER, sizeof(DEFAULT_BT_INFO_HEADER));
 			for (uint8_t addr = 0; addr < 3; addr++) 
 			{
-				device_address[4 + addr] = static_cast<uint8_t>(get_rand_32() % 0xFF);
+				device_address[4 + addr] = static_cast<uint8_t>(get_rand_32() % 0xff);
 			}
 			for (uint8_t addr = 0; addr < 6; addr++) 
 			{
-				host_address[1 + addr] = static_cast<uint8_t>(get_rand_32() % 0xFF);
+				host_address[1 + addr] = static_cast<uint8_t>(get_rand_32() % 0xff);
 			}
 		}
 	};
 	static_assert(sizeof(BTInfo) == 17, "PS3::BTInfo size mismatch");
+	static_assert(sizeof(BTInfo) >= sizeof(DEFAULT_BT_INFO_HEADER));
 	#pragma pack(pop)
 
 	static const uint8_t STRING_LANGUAGE[]     = { 0x09, 0x04 };
@@ -327,128 +328,10 @@ namespace PS3
 		0xB1, 0x02,        //     Feature (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position,Non-volatile)
 		0xC0,              //   End Collection
 		0xC0,              // End Collection
-		// 0x05, 0x01,        // Usage Page (Generic Desktop Ctrls)
-		// 0x09, 0x04,        // Usage (Joystick)
-		// 0xA1, 0x01,        // Collection (Application)
-		// 0xA1, 0x02,        //   Collection (Logical)
-		// 0x85, 0x01,        //     Report ID (1)
-		// 0x75, 0x08,        //     Report Size (8)
-		// 0x95, 0x01,        //     Report Count (1)
-		// 0x15, 0x00,        //     Logical Minimum (0)
-		// 0x26, 0xFF, 0x00,  //     Logical Maximum (255)
-		// 0x81, 0x03,        //     Input (Const,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-		// 0x75, 0x01,        //     Report Size (1)
-		// 0x95, 0x13,        //     Report Count (19)
-		// 0x15, 0x00,        //     Logical Minimum (0)
-		// 0x25, 0x01,        //     Logical Maximum (1)
-		// 0x35, 0x00,        //     Physical Minimum (0)
-		// 0x45, 0x01,        //     Physical Maximum (1)
-		// 0x05, 0x09,        //     Usage Page (Button)
-		// 0x19, 0x01,        //     Usage Minimum (0x01)
-		// 0x29, 0x13,        //     Usage Maximum (0x13)
-		// 0x81, 0x02,        //     Input (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-		// 0x75, 0x01,        //     Report Size (1)
-		// 0x95, 0x0D,        //     Report Count (13)
-		// 0x06, 0x00, 0xFF,  //     Usage Page (Vendor Defined 0xFF00)
-		// 0x81, 0x03,        //     Input (Const,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-		// 0x15, 0x00,        //     Logical Minimum (0)
-		// 0x26, 0xFF, 0x00,  //     Logical Maximum (255)
-		// 0x05, 0x01,        //     Usage Page (Generic Desktop Ctrls)
-		// 0x09, 0x01,        //     Usage (Pointer)
-		// 0xA1, 0x00,        //     Collection (Physical)
-		// 0x75, 0x08,        //       Report Size (8)
-		// 0x95, 0x04,        //       Report Count (4)
-		// 0x35, 0x00,        //       Physical Minimum (0)
-		// 0x46, 0xFF, 0x00,  //       Physical Maximum (255)
-		// 0x09, 0x30,        //       Usage (X)
-		// 0x09, 0x31,        //       Usage (Y)
-		// 0x09, 0x32,        //       Usage (Z)
-		// 0x09, 0x35,        //       Usage (Rz)
-		// 0x81, 0x02,        //       Input (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-		// 0xC0,              //     End Collection
-		// 0x05, 0x01,        //     Usage Page (Generic Desktop Ctrls)
-		// 0x75, 0x08,        //     Report Size (8)
-		// 0x95, 0x27,        //     Report Count (39)
-		// 0x09, 0x01,        //     Usage (Pointer)
-		// 0x81, 0x02,        //     Input (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position)
-		// 0x75, 0x08,        //     Report Size (8)
-		// 0x95, 0x30,        //     Report Count (48)
-		// 0x09, 0x01,        //     Usage (Pointer)
-		// 0x91, 0x02,        //     Output (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position,Non-volatile)
-		// 0x75, 0x08,        //     Report Size (8)
-		// 0x95, 0x30,        //     Report Count (48)
-		// 0x09, 0x01,        //     Usage (Pointer)
-		// 0xB1, 0x02,        //     Feature (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position,Non-volatile)
-		// 0xC0,              //   End Collection
-		// 0xA1, 0x02,        //   Collection (Logical)
-		// 0x85, 0x02,        //     Report ID (2)
-		// 0x75, 0x08,        //     Report Size (8)
-		// 0x95, 0x30,        //     Report Count (48)
-		// 0x09, 0x01,        //     Usage (Pointer)
-		// 0xB1, 0x02,        //     Feature (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position,Non-volatile)
-		// 0xC0,              //   End Collection
-		// 0xA1, 0x02,        //   Collection (Logical)
-		// 0x85, 0xEE,        //     Report ID (-18)
-		// 0x75, 0x08,        //     Report Size (8)
-		// 0x95, 0x30,        //     Report Count (48)
-		// 0x09, 0x01,        //     Usage (Pointer)
-		// 0xB1, 0x02,        //     Feature (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position,Non-volatile)
-		// 0xC0,              //   End Collection
-		// 0xA1, 0x02,        //   Collection (Logical)
-		// 0x85, 0xEF,        //     Report ID (-17)
-		// 0x75, 0x08,        //     Report Size (8)
-		// 0x95, 0x30,        //     Report Count (48)
-		// 0x09, 0x01,        //     Usage (Pointer)
-		// 0xB1, 0x02,        //     Feature (Data,Var,Abs,No Wrap,Linear,Preferred State,No Null Position,Non-volatile)
-		// 0xC0,              //   End Collection
-		// 0xC0,              // End Collection
 	};
 
-	// #define CONFIG_TOTAL_LEN (TUD_CONFIG_DESC_LEN + TUD_HID_DESC_LEN)
-
 	static const uint8_t CONFIGURATION_DESCRIPTORS[] =
 	{
-		// 0x09,        // bLength
-		// 0x02,        // bDescriptorType (Configuration)
-		// 0x29, 0x00,  // wTotalLength 41
-		// 0x01,        // bNumInterfaces 1
-		// 0x01,        // bConfigurationValue
-		// 0x00,        // iConfiguration (String Index)
-		// 0x80,        // bmAttributes
-		// 0xFA,        // bMaxPower 500mA
-
-		// 0x09,        // bLength
-		// 0x04,        // bDescriptorType (Interface)
-		// 0x00,        // bInterfaceNumber 0
-		// 0x00,        // bAlternateSetting
-		// 0x02,        // bNumEndpoints 2
-		// 0x03,        // bInterfaceClass
-		// 0x00,        // bInterfaceSubClass
-		// 0x00,        // bInterfaceProtocol
-		// 0x00,        // iInterface (String Index)
-
-		// 0x09,        // bLength
-		// 0x21,        // bDescriptorType (HID)
-		// 0x11, 0x01,  // bcdHID 1.11
-		// 0x00,        // bCountryCode
-		// 0x01,        // bNumDescriptors
-		// 0x22,        // bDescriptorType[0] (HID)
-		// 0x94, 0x00,  // wDescriptorLength[0] 148
-
-		// 0x07,        // bLength
-		// 0x05,        // bDescriptorType (Endpoint)
-		// 0x02,        // bEndpointAddress (OUT/H2D)
-		// 0x03,        // bmAttributes (Interrupt)
-		// 0x40, 0x00,  // wMaxPacketSize 64
-		// 0x0A,        // bInterval 10 (unit depends on device speed)
-
-		// 0x07,        // bLength
-		// 0x05,        // bDescriptorType (Endpoint)
-		// 0x81,        // bEndpointAddress (IN/D2H)
-		// 0x03,        // bmAttributes (Interrupt)
-		// 0x40, 0x00,  // wMaxPacketSize 64
-		// 0x0A,        // bInterval 10 (unit depends on device speed)
-
 		0x09,        // bLength
 		0x02,        // bDescriptorType (Configuration)
 		0x29, 0x00,  // wTotalLength 41
@@ -490,6 +373,68 @@ namespace PS3
 		0x40, 0x00,  // wMaxPacketSize 64
 		0x01,        // bInterval 1 (unit depends on device speed)
 	};
+
+	static constexpr uint8_t OUTPUT_0x01[] = 
+	{
+		0x01, 0x04, 0x00, 0x0b, 0x0c, 0x01, 0x02, 0x18, 
+		0x18, 0x18, 0x18, 0x09, 0x0a, 0x10, 0x11, 0x12,
+		0x13, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02,
+		0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x04, 0x04,
+		0x04, 0x04, 0x00, 0x00, 0x04, 0x00, 0x01, 0x02,
+		0x07, 0x00, 0x17, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+
+	// calibration data
+	static constexpr uint8_t OUTPUT_0xEF[] = 
+	{
+		0xef, 0x04, 0x00, 0x0b, 0x03, 0x01, 0xa0, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x01, 0xff, 0x01, 0xff, 0x01, 0xff, 0x01, 0xff,
+		0x01, 0xff, 0x01, 0xff, 0x01, 0xff, 0x01, 0xff,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06,
+	};
+
+	// unknown
+	static constexpr uint8_t OUTPUT_0xF5[] = 
+	{
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // host address - must match 0xf2
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+
+	// unknown
+	static constexpr uint8_t OUTPUT_0xF7[] = 
+	{
+		0x02, 0x01, 0xf8, 0x02, 0xe2, 0x01, 0x05, 0xff,
+		0x04, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
+
+	// unknown
+	static constexpr uint8_t OUTPUT_0xF8[] = 
+	{
+		0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+	};
 } // namespace PS3
 
 #endif // _PS3_DESCRIPTORS_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Descriptors/PS5.h b/Firmware/RP2040/src/Descriptors/PS5.h
index 0c3d039..a70bee3 100644
--- a/Firmware/RP2040/src/Descriptors/PS5.h
+++ b/Firmware/RP2040/src/Descriptors/PS5.h
@@ -7,9 +7,13 @@
 namespace PS5
 {
     static constexpr uint8_t DPAD_MASK = 0x0F;
-    static constexpr size_t IN_REPORT_CMP_SIZE = 12;
     static constexpr uint8_t JOYSTICK_MID = 0x80;
-    static constexpr uint8_t OUT_REPORT_ID = 5;
+
+    namespace OutReportID
+    {
+        static constexpr uint8_t CONTROL = 0x02;
+        static constexpr uint8_t RUMBLE = 0x05;
+    };
 
     namespace Buttons0
     {
@@ -68,30 +72,34 @@ namespace PS5
         uint16_t gyro[3]; // Gyroscope data for x, y, z axes
         uint16_t accel[3]; // Accelerometer data for x, y, z axes
         uint32_t sensor_timestamp; // Timestamp for sensor data
-        uint8_t reserved2;
+        uint8_t reserved0;
 
-        // Touchpad
-        struct dualsense_touch_point {
-            uint8_t counter : 7; // Incremented every time a finger touches the touchpad
-            uint8_t touching : 1; // Indicates if a finger is currently touching the touchpad
-            uint16_t x : 12; // X coordinate of the touchpoint
-            uint16_t y : 12; // Y coordinate of the touchpoint
+        struct Touchpad 
+        {
+            uint32_t counter : 7; // Incremented every time a finger touches the touchpad
+            uint32_t touching : 1; // Indicates if a finger is currently touching the touchpad
+            uint32_t x : 12; // X coordinate of the touchpoint
+            uint32_t y : 12; // Y coordinate of the touchpoint
         } points[2]; // Array of touchpoints (up to 2)
+        // uint32_t touchpad[2];
 
-        uint8_t reserved3[12];
+        uint8_t reserved1[12];
 
         uint8_t status; // ?
 
-        uint8_t reserved4[10];
+        uint8_t reserved2[10];
+
+        InReport()
+        {
+            std::memset(this, 0, sizeof(InReport));
+        }
     };
-    // static_assert(sizeof(InReport) == 52, "PS5::InReport is not correct size");
+    static_assert(sizeof(InReport) == 60, "PS5::InReport is not correct size");
 
     struct OutReport
     {
         uint8_t report_id;
-
-        uint8_t valid_flag0;
-        uint8_t valid_flag1;
+        uint8_t control_flag[2];
 
         /* For DualShock 4 compatibility mode. */
         uint8_t motor_right;
@@ -120,14 +128,19 @@ namespace PS5
         uint8_t audio_flags2; /* 3 first bits: speaker pre-gain */
 
         /* LEDs and lightbar */
-        uint8_t valid_flag2;
+        uint8_t led_control_flag;
         uint8_t reserved3[2];
-        uint8_t lightbar_setup;
+        uint8_t pulse_option;
         uint8_t led_brightness;
-        uint8_t player_leds;
+        uint8_t player_number;
         uint8_t lightbar_red;
         uint8_t lightbar_green;
         uint8_t lightbar_blue;
+
+        OutReport()
+        {
+            std::memset(this, 0, sizeof(OutReport));
+        }
     };
     static_assert(sizeof(OutReport) == 48);
     #pragma pack(pop)
diff --git a/Firmware/RP2040/src/Descriptors/XInput.h b/Firmware/RP2040/src/Descriptors/XInput.h
index ecc911f..a9fa589 100644
--- a/Firmware/RP2040/src/Descriptors/XInput.h
+++ b/Firmware/RP2040/src/Descriptors/XInput.h
@@ -1,7 +1,7 @@
 #ifndef _XINPUT_DESCRIPTORS_H_
 #define _XINPUT_DESCRIPTORS_H_
 
-#include <stdint.h>
+#include <cstdint>
 #include <cstring>
 
 namespace XInput
@@ -133,6 +133,11 @@ namespace XInput
 		uint8_t reserved[6];
 		uint8_t chatpad_status;
 		uint8_t chatpad[3];
+
+		InReportWireless()
+		{
+			std::memset(this, 0, sizeof(InReportWireless));
+		}
 	};
 	static_assert(sizeof(InReportWireless) == 28, "XInput::InReportWireless is not the correct size");
 
diff --git a/Firmware/RP2040/src/Descriptors/XboxOG.h b/Firmware/RP2040/src/Descriptors/XboxOG.h
index f45d428..1ca26be 100644
--- a/Firmware/RP2040/src/Descriptors/XboxOG.h
+++ b/Firmware/RP2040/src/Descriptors/XboxOG.h
@@ -99,15 +99,12 @@ namespace XboxOG
             .bDeviceSubClass = 0x00,
             .bDeviceProtocol = 0x00,
             .bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
-
             .idVendor = 0x045E,
             .idProduct = 0x0289,
             .bcdDevice = 0x0121,
-
             .iManufacturer = 0x00,
             .iProduct = 0x00,
             .iSerialNumber = 0x00,
-
             .bNumConfigurations = 0x01
         };
 
@@ -184,8 +181,6 @@ namespace XboxOG
     {
         static constexpr uint16_t AIMING_MID = 32768;
         static constexpr uint16_t BUTTONS2_TOGGLE_MID = 0xFFFC;
-        static constexpr int16_t DEFAULT_DEADZONE = 7500;
-        static constexpr uint16_t DEFAULT_SENSE = 400;
 
         namespace Buttons0
         {
diff --git a/Firmware/RP2040/src/Gamepad.h b/Firmware/RP2040/src/Gamepad.h
index fb134f4..0c6ecf0 100644
--- a/Firmware/RP2040/src/Gamepad.h
+++ b/Firmware/RP2040/src/Gamepad.h
@@ -5,6 +5,7 @@
 #include <atomic>
 #include <limits>
 #include <cstring>
+#include <array>
 #include <pico/mutex.h>
 
 #include "Scale.h"
@@ -38,16 +39,16 @@ public:
     static constexpr uint16_t BUTTON_SYS   = 0x0400;
     static constexpr uint16_t BUTTON_MISC  = 0x0800;
 
-    static constexpr uint8_t ANALOG_OFF_UP    = 1;
-    static constexpr uint8_t ANALOG_OFF_DOWN  = 2;
-    static constexpr uint8_t ANALOG_OFF_LEFT  = 3;
-    static constexpr uint8_t ANALOG_OFF_RIGHT = 4;
-    static constexpr uint8_t ANALOG_OFF_A     = 5;
-    static constexpr uint8_t ANALOG_OFF_B     = 6;
-    static constexpr uint8_t ANALOG_OFF_X     = 7;
-    static constexpr uint8_t ANALOG_OFF_Y     = 8;
-    static constexpr uint8_t ANALOG_OFF_LB    = 9;
-    static constexpr uint8_t ANALOG_OFF_RB    = 10;
+    static constexpr uint8_t ANALOG_OFF_UP    = 0;
+    static constexpr uint8_t ANALOG_OFF_DOWN  = 1;
+    static constexpr uint8_t ANALOG_OFF_LEFT  = 2;
+    static constexpr uint8_t ANALOG_OFF_RIGHT = 3;
+    static constexpr uint8_t ANALOG_OFF_A     = 4;
+    static constexpr uint8_t ANALOG_OFF_B     = 5;
+    static constexpr uint8_t ANALOG_OFF_X     = 6;
+    static constexpr uint8_t ANALOG_OFF_Y     = 7;
+    static constexpr uint8_t ANALOG_OFF_LB    = 8;
+    static constexpr uint8_t ANALOG_OFF_RB    = 9;
 
     //Mappings used by host to set buttons
 
@@ -97,7 +98,7 @@ public:
         int16_t joystick_ly;
         int16_t joystick_rx;
         int16_t joystick_ry;
-        uint8_t chatpad[3];
+        // uint8_t chatpad[3];
         uint8_t analog[10];
     
         PadIn()
@@ -116,15 +117,20 @@ public:
             std::memset(this, 0, sizeof(PadOut));
         }
     };
+
+    using ChatpadIn = std::array<uint8_t, 3>;
+
 #pragma pack(pop)
 
     Gamepad()
     {
         mutex_init(&pad_in_mutex_);
         mutex_init(&pad_out_mutex_);
+        mutex_init(&chatpad_in_mutex_);
         reset_pad_in();
         reset_pad_out();
-        setup_deadzones(profile_);
+        reset_chatpad_in();
+        setup_deadzones();
     };
 
     ~Gamepad() = default;
@@ -132,42 +138,70 @@ public:
     //Get
     inline bool new_pad_in() const { return new_pad_in_.load(); }
     inline bool new_pad_out() const { return new_pad_out_.load(); }
+
+    //True if both host and device have enabled analog
     inline bool analog_enabled() const { return analog_enabled_.load(std::memory_order_relaxed); }
 
     inline PadIn get_pad_in()
     {
         PadIn pad_in;
-        {
-            mutex_enter_blocking(&pad_in_mutex_);
-            pad_in = pad_in_;
-            new_pad_in_.store(false);
-            mutex_exit(&pad_in_mutex_);
-        }
+
+        mutex_enter_blocking(&pad_in_mutex_);
+        pad_in = pad_in_;
+        new_pad_in_.store(false);
+        mutex_exit(&pad_in_mutex_);
+
         return pad_in;
     }
 
     inline PadOut get_pad_out()
     {
         PadOut pad_out;
+
         mutex_enter_blocking(&pad_out_mutex_);
         pad_out = pad_out_;
         new_pad_out_.store(false);
         mutex_exit(&pad_out_mutex_);
+
         return pad_out;
     }
 
+    inline ChatpadIn get_chatpad_in()
+    {
+        ChatpadIn chatpad_in;
+
+        mutex_enter_blocking(&chatpad_in_mutex_);
+        chatpad_in = chatpad_in_;
+        mutex_exit(&chatpad_in_mutex_);
+
+        return chatpad_in;
+    }
+
     //Set
 
-    void set_analog_enabled(bool value) 
+    void set_analog_device(bool value) 
     { 
-        analog_enabled_.store(value); 
+        analog_device_.store(value); 
+        if (analog_host_.load() && analog_device_.load() && profile_.analog_enabled)
+        {
+            analog_enabled_.store(true);
+        }
+    }
+
+    void set_analog_host(bool value) 
+    { 
+        analog_host_.store(value); 
+        if (analog_host_.load() && analog_device_.load() && profile_.analog_enabled)
+        {
+            analog_enabled_.store(true);
+        }
     }
 
     void set_profile(const UserProfile& user_profile) 
     { 
         profile_ = user_profile; 
-        setup_mappings(profile_);
-        setup_deadzones(profile_);
+        setup_mappings();
+        setup_deadzones();
     }
 
     inline void set_pad_in(PadIn pad_in)
@@ -180,26 +214,32 @@ public:
         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;
-            mutex_exit(&pad_in_mutex_);
-        }
+
+        mutex_enter_blocking(&pad_in_mutex_);
+        pad_in_ = pad_in;
         new_pad_in_.store(true);
+        mutex_exit(&pad_in_mutex_);
     }
 
-    inline void set_pad_out(PadOut pad_out)
+    inline void set_pad_out(const PadOut& pad_out)
     {
         mutex_enter_blocking(&pad_out_mutex_);
         pad_out_ = pad_out;
-        mutex_exit(&pad_out_mutex_);
         new_pad_out_.store(true);
+        mutex_exit(&pad_out_mutex_);
+    }
+
+    inline void set_chatpad_in(const ChatpadIn& chatpad_in)
+    {
+        mutex_enter_blocking(&chatpad_in_mutex_);
+        chatpad_in_ = chatpad_in;
+        mutex_exit(&chatpad_in_mutex_);
     }
 
     inline void reset_pad_in() 
 	{ 
         mutex_enter_blocking(&pad_in_mutex_);
-        std::memset(&pad_in_, 0, sizeof(pad_in_));
+        std::memset(reinterpret_cast<void*>(&pad_in_), 0, sizeof(pad_in_));
         mutex_exit(&pad_in_mutex_);
         new_pad_in_.store(true);
     }
@@ -207,21 +247,33 @@ public:
     inline void reset_pad_out()
     {
         mutex_enter_blocking(&pad_out_mutex_);
-        std::memset(&pad_out_, 0, sizeof(pad_out_));
-        mutex_exit(&pad_out_mutex_);
+        std::memset(reinterpret_cast<void*>(&pad_out_), 0, sizeof(pad_out_));
         new_pad_out_.store(true);
+        mutex_exit(&pad_out_mutex_);
+    }
+
+    inline void reset_chatpad_in()
+    {
+        mutex_enter_blocking(&chatpad_in_mutex_);
+        chatpad_in_.fill(0);
+        mutex_exit(&chatpad_in_mutex_);
     }
 
 private:
     mutex_t pad_in_mutex_;
     mutex_t pad_out_mutex_;
+    mutex_t chatpad_in_mutex_;
 
     PadOut pad_out_;
     PadIn pad_in_;
+    ChatpadIn chatpad_in_;
 
     std::atomic<bool> new_pad_in_{false};
     std::atomic<bool> new_pad_out_{false};
+
     std::atomic<bool> analog_enabled_{false};
+    std::atomic<bool> analog_host_{false};
+    std::atomic<bool> analog_device_{false};
 
     UserProfile profile_;
 
@@ -235,44 +287,44 @@ private:
         int16_t joystick_r_pos{0};
     } dz_;
 
-    void setup_mappings(const UserProfile& profile)
+    void setup_mappings()
     {
-        MAP_DPAD_UP         = profile.dpad_up;
-        MAP_DPAD_DOWN       = profile.dpad_down;
-        MAP_DPAD_LEFT       = profile.dpad_left;
-        MAP_DPAD_RIGHT      = profile.dpad_right;
-        MAP_DPAD_UP_LEFT    = profile.dpad_up | profile.dpad_left;
-        MAP_DPAD_UP_RIGHT   = profile.dpad_up | profile.dpad_right;
-        MAP_DPAD_DOWN_LEFT  = profile.dpad_down | profile.dpad_left;
-        MAP_DPAD_DOWN_RIGHT = profile.dpad_down | profile.dpad_right;
+        MAP_DPAD_UP         = profile_.dpad_up;
+        MAP_DPAD_DOWN       = profile_.dpad_down;
+        MAP_DPAD_LEFT       = profile_.dpad_left;
+        MAP_DPAD_RIGHT      = profile_.dpad_right;
+        MAP_DPAD_UP_LEFT    = profile_.dpad_up | profile_.dpad_left;
+        MAP_DPAD_UP_RIGHT   = profile_.dpad_up | profile_.dpad_right;
+        MAP_DPAD_DOWN_LEFT  = profile_.dpad_down | profile_.dpad_left;
+        MAP_DPAD_DOWN_RIGHT = profile_.dpad_down | profile_.dpad_right;
         MAP_DPAD_NONE       = 0;
 
-        MAP_BUTTON_A     = profile.button_a;
-        MAP_BUTTON_B     = profile.button_b;
-        MAP_BUTTON_X     = profile.button_x;
-        MAP_BUTTON_Y     = profile.button_y;
-        MAP_BUTTON_L3    = profile.button_l3;
-        MAP_BUTTON_R3    = profile.button_r3;
-        MAP_BUTTON_BACK  = profile.button_back;
-        MAP_BUTTON_START = profile.button_start;
-        MAP_BUTTON_LB    = profile.button_lb;
-        MAP_BUTTON_RB    = profile.button_rb;
-        MAP_BUTTON_SYS   = profile.button_sys;
-        MAP_BUTTON_MISC  = profile.button_misc;
+        MAP_BUTTON_A     = profile_.button_a;
+        MAP_BUTTON_B     = profile_.button_b;
+        MAP_BUTTON_X     = profile_.button_x;
+        MAP_BUTTON_Y     = profile_.button_y;
+        MAP_BUTTON_L3    = profile_.button_l3;
+        MAP_BUTTON_R3    = profile_.button_r3;
+        MAP_BUTTON_BACK  = profile_.button_back;
+        MAP_BUTTON_START = profile_.button_start;
+        MAP_BUTTON_LB    = profile_.button_lb;
+        MAP_BUTTON_RB    = profile_.button_rb;
+        MAP_BUTTON_SYS   = profile_.button_sys;
+        MAP_BUTTON_MISC  = profile_.button_misc;
 
-        MAP_ANALOG_OFF_UP    = profile.analog_off_up;
-        MAP_ANALOG_OFF_DOWN  = profile.analog_off_down;
-        MAP_ANALOG_OFF_LEFT  = profile.analog_off_left;
-        MAP_ANALOG_OFF_RIGHT = profile.analog_off_right;
-        MAP_ANALOG_OFF_A     = profile.analog_off_a;
-        MAP_ANALOG_OFF_B     = profile.analog_off_b;
-        MAP_ANALOG_OFF_X     = profile.analog_off_x;
-        MAP_ANALOG_OFF_Y     = profile.analog_off_y;
-        MAP_ANALOG_OFF_LB    = profile.analog_off_lb;
-        MAP_ANALOG_OFF_RB    = profile.analog_off_rb;
+        MAP_ANALOG_OFF_UP    = profile_.analog_off_up;
+        MAP_ANALOG_OFF_DOWN  = profile_.analog_off_down;
+        MAP_ANALOG_OFF_LEFT  = profile_.analog_off_left;
+        MAP_ANALOG_OFF_RIGHT = profile_.analog_off_right;
+        MAP_ANALOG_OFF_A     = profile_.analog_off_a;
+        MAP_ANALOG_OFF_B     = profile_.analog_off_b;
+        MAP_ANALOG_OFF_X     = profile_.analog_off_x;
+        MAP_ANALOG_OFF_Y     = profile_.analog_off_y;
+        MAP_ANALOG_OFF_LB    = profile_.analog_off_lb;
+        MAP_ANALOG_OFF_RB    = profile_.analog_off_rb;
     }
 
-    void setup_deadzones(const UserProfile& profile) //Deadzones in the profile are 0-255 (0-100%)
+    void setup_deadzones() //Deadzones in the profile are 0-255 (0-100%)
     {
         dz_.trigger_l = profile_.dz_trigger_l;
         dz_.trigger_r = profile_.dz_trigger_r;
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h
index c37ccbd..51669db 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h
@@ -12,16 +12,16 @@
 class I2CDriver
 {
 public:
-    virtual ~I2CDriver() = default;
+    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) = 0;
-    virtual void notify_xbox360w_disconnected(uint8_t idx) = 0;
+    virtual void notify_xbox360w_connected(uint8_t idx) {};
+    virtual void notify_xbox360w_disconnected(uint8_t idx) {};
 
 protected:
-    enum class PacketID : uint8_t { UNKNOWN = 0, PAD, STATUS, ENABLE, DISABLE };
+    enum class PacketID : uint8_t { UNKNOWN = 0, PAD, STATUS, ENABLE, DISABLE, REBOOT };
     enum class SlaveStatus : uint8_t { NC = 0, NOT_READY, READY, RESP_OK };
 
 #pragma pack(push, 1)
@@ -38,7 +38,7 @@ protected:
             packet_id = static_cast<uint8_t>(PacketID::PAD);
         }
     };
-static_assert(sizeof(PacketIn) == 28, "I2CDriver::PacketIn is misaligned");
+    static_assert(sizeof(PacketIn) == 28, "I2CDriver::PacketIn is misaligned");
 
     struct PacketOut
     {
@@ -53,7 +53,7 @@ static_assert(sizeof(PacketIn) == 28, "I2CDriver::PacketIn is misaligned");
             packet_id = static_cast<uint8_t>(PacketID::PAD);
         }
     };
-static_assert(sizeof(PacketOut) == 4, "I2CDriver::PacketOut is misaligned");
+    static_assert(sizeof(PacketOut) == 4, "I2CDriver::PacketOut is misaligned");
 
     struct PacketStatus
     {
@@ -68,7 +68,7 @@ static_assert(sizeof(PacketOut) == 4, "I2CDriver::PacketOut is misaligned");
             status = static_cast<uint8_t>(SlaveStatus::NC);
         }
     };
-static_assert(sizeof(PacketStatus) == 3, "I2CDriver::PacketStatus is misaligned");
+    static_assert(sizeof(PacketStatus) == 3, "I2CDriver::PacketStatus is misaligned");
 #pragma pack(pop)
 
     static constexpr size_t MAX_PACKET_SIZE = std::max(sizeof(PacketStatus), std::max(sizeof(PacketIn), sizeof(PacketOut)));
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
index fb9adcf..76244f3 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
@@ -23,7 +23,7 @@ public:
         return instance;
     }
 
-    bool initialize_driver() //Gamepad idx 0 for slave to update on interrupt
+    bool initialize_driver()
     {
         uint8_t i2c_address = get_i2c_address();
         if (i2c_address == 0xFF)
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp
index 9f48bf8..9f11d0d 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp
@@ -1,8 +1,14 @@
 #include <cstring>
 
 #include "TaskQueue/TaskQueue.h"
+#include "OGXMini/OGXMini.h"    
 #include "I2CDriver/4Channel/I2CMaster.h"
 
+I2CMaster::~I2CMaster()
+{
+    TaskQueue::Core0::cancel_delayed_task(tid_update_slave_);
+}
+
 void I2CMaster::initialize(uint8_t address) 
 {
     i2c_init(I2C_PORT, I2C_BAUDRATE);
@@ -21,11 +27,12 @@ void I2CMaster::initialize(uint8_t address)
     }
 
     tid_update_slave_ = TaskQueue::Core0::get_new_task_id();
-    TaskQueue::Core0::queue_delayed_task(tid_update_slave_, 1000, true, [this]
+
+    TaskQueue::Core0::queue_delayed_task(tid_update_slave_, 2000, true, [this]
     {
         for (auto& slave : slaves_)
         {
-            update_slave_status(slave);
+            check_slave_status(slave);
             sleep_us(10);
         }
     });
@@ -33,26 +40,17 @@ void I2CMaster::initialize(uint8_t address)
 
 void I2CMaster::process(Gamepad (&gamepads)[MAX_GAMEPADS])
 {
-    if (notify_deinit_.load())
-    {
-        notify_tud_deinit();
-        return;
-    }
     for (uint8_t i = 0; i < NUM_SLAVES; ++i)
     {
         Slave& slave = slaves_[i];
 
-        if (!slave.enabled.load() || !slave_detected(slave.address))
+        if (slave.status != SlaveStatus::READY || !slave_detected(slave.address))
         {
             continue;
         }
-    
-        if (slave.status == SlaveStatus::READY)
+        if (send_packet_in(slave, gamepads[i + 1]))
         {
-            if (send_packet_in(slave, gamepads[i + 1]))
-            {
-                get_packet_out(slave, gamepads[i + 1]);
-            }
+            get_packet_out(slave, gamepads[i + 1]);
         }
 
         sleep_us(100);
@@ -69,8 +67,18 @@ void I2CMaster::notify_tuh_mounted(HostDriver::Type host_type)
 
 void I2CMaster::notify_tuh_unmounted(HostDriver::Type host_type)
 {
-    i2c_enabled_.store(false);
-    notify_deinit_.store(true);
+    if (host_type == HostDriver::Type::XBOX360W)
+    {
+        i2c_enabled_.store(false);
+
+        TaskQueue::Core0::queue_task([this]()
+        {
+            for (auto& slave : slaves_)
+            {
+                notify_disable(slave);
+            }
+        });
+    }
 }
 
 void I2CMaster::notify_xbox360w_connected(uint8_t idx)
@@ -80,6 +88,11 @@ void I2CMaster::notify_xbox360w_connected(uint8_t idx)
         return;
     }
     slaves_[idx - 1].enabled.store(true);
+
+    TaskQueue::Core0::queue_task([this, idx]()
+    {
+        notify_enable(slaves_[idx - 1].address);
+    });
 }
 
 void I2CMaster::notify_xbox360w_disconnected(uint8_t idx)
@@ -89,6 +102,11 @@ void I2CMaster::notify_xbox360w_disconnected(uint8_t idx)
         return;
     }
     slaves_[idx - 1].enabled.store(false);
+
+    TaskQueue::Core0::queue_task([this, idx]()
+    {
+        notify_disable(slaves_[idx - 1]);
+    });
 }
 
 bool I2CMaster::slave_detected(uint8_t address)
@@ -98,53 +116,70 @@ bool I2CMaster::slave_detected(uint8_t address)
     return (result >= 0);
 }
 
-void I2CMaster::update_slave_status(Slave& slave)
+void I2CMaster::check_slave_status(Slave& slave)
 {
-    bool slave_enabled = slave.enabled.load();
     if (!slave_detected(slave.address))
     {
         slave.status = SlaveStatus::NC;
         return;
     }
-    if (!update_slave_enabled(slave.address, slave_enabled))
+    if (slave.enabled.load())
     {
-        slave.status = SlaveStatus::NOT_READY;
-        return;
-    }
-    if (slave_enabled)
-    {
-        PacketStatus status_packet = PacketStatus();
-        int count = 0;
-
-        status_packet.packet_id = static_cast<uint8_t>(PacketID::STATUS);
-        count = i2c_write_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-        if (count == sizeof(PacketStatus))
-        {
-            count = i2c_read_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-            slave.status = (count > 0) ? static_cast<SlaveStatus>(status_packet.status) : SlaveStatus::NOT_READY;
-        }
-        else
-        {
-            slave.status = SlaveStatus::NOT_READY;
-        }
-    }
-    else
-    {
-        slave.status = SlaveStatus::NOT_READY;
+        slave.status = get_slave_status(slave.address);
     }
+    slave.status = SlaveStatus::NOT_READY;
 }
 
-//Tell slave if it's enabled or not
-bool I2CMaster::update_slave_enabled(uint8_t address, bool enabled)
+I2CMaster::SlaveStatus I2CMaster::get_slave_status(uint8_t address)
 {
     PacketStatus status_packet;
-    status_packet.packet_id = enabled ? static_cast<uint8_t>(PacketID::ENABLE) : static_cast<uint8_t>(PacketID::DISABLE);
+    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) == SlaveStatus::RESP_OK);
+        return static_cast<SlaveStatus>(status_packet.status);
+    }
+    return SlaveStatus::NC;
+}
+
+I2CMaster::SlaveStatus I2CMaster::notify_enable(uint8_t address)
+{
+    PacketStatus status_packet;
+    status_packet.packet_id = static_cast<uint8_t>(PacketID::ENABLE);
+
+    int count = i2c_write_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
+    if (count == sizeof(PacketStatus))
+    {
+        count = i2c_read_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
+        return static_cast<SlaveStatus>(status_packet.status);
+    }
+    return SlaveStatus::NC;
+}
+
+bool I2CMaster::notify_disable(Slave& slave)
+{
+    if (slave_detected(slave.address) && slave.enabled.load())
+    {
+        int retries = 6;
+        bool success = false;
+
+        while (!success && retries--)
+        {
+            PacketStatus status_packet;
+            status_packet.packet_id = static_cast<uint8_t>(PacketID::DISABLE);
+
+            int count = i2c_write_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
+            if (count == sizeof(PacketStatus))
+            {
+                count = i2c_read_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
+                success = (static_cast<SlaveStatus>(status_packet.status) == SlaveStatus::RESP_OK);
+            }
+
+            sleep_ms(1);
+        }
+        return success;
     }
     return false;
 }
@@ -153,11 +188,10 @@ 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;
+    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);
-    // int count = i2c_write_timeout_us(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&packet_in), sizeof(packet_in), false, 2000);
     return (count == sizeof(PacketIn));
 }
 
@@ -173,20 +207,4 @@ bool I2CMaster::get_packet_out(Slave& slave, Gamepad& gamepad)
 
     gamepad.set_pad_out(packet_out.pad_out);
     return true;
-}
-
-void I2CMaster::notify_tud_deinit()
-{
-    for (auto& slave : slaves_)
-    {
-        if (slave.status != SlaveStatus::NC && slave_detected(slave.address) && slave.enabled.load())
-        {
-            int retries = 0;
-            while (!update_slave_enabled(slave.address, false) && retries < 6)
-            {
-                sleep_ms(1);
-                ++retries;
-            }
-        }
-    }
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
index 465b302..6331fd6 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
@@ -5,7 +5,6 @@
 #include <atomic>
 #include <array>
 
-#include <pico/time.h>
 #include <hardware/gpio.h>
 #include <hardware/i2c.h>
 
@@ -17,7 +16,6 @@ class I2CMaster : public I2CDriver
 {
 public:
     ~I2CMaster() override;
-
     void initialize(uint8_t address) override;
     void process(Gamepad (&gamepads)[MAX_GAMEPADS]) override;
     void notify_tuh_mounted(HostDriver::Type host_type) override;
@@ -36,22 +34,23 @@ private:
     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");
 
-    // repeating_timer_t update_slave_timer_;
     uint32_t tid_update_slave_;
     bool update_slave_status_{false};
 
     std::atomic<bool> i2c_enabled_{false};
-    std::atomic<bool> notify_deinit_{false};
+    // std::atomic<bool> notify_deinit_{false};
     std::array<Slave, NUM_SLAVES> slaves_;  
 
     static bool slave_detected(uint8_t address);
-    static void update_slave_status(Slave& slave);
-    static bool update_slave_enabled(uint8_t address, bool enabled);
-    static bool update_slave_timer_cb(repeating_timer_t* rt);
+
+    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);
-    void notify_tud_deinit();
 };
 
 #endif // I2C_MASTER_4CH_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp
index 24579e3..ea7f40c 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp
@@ -1,7 +1,7 @@
 #include <cstring>
 #include <array>
 
-#include "Board/board_api.h"
+#include "USBHost/HostManager.h"
 #include "OGXMini/OGXMini.h"
 #include "I2CDriver/4Channel/I2CSlave.h"
 
@@ -40,10 +40,15 @@ void I2CSlave::process(Gamepad (&gamepads)[MAX_GAMEPADS])
     {
         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))
     {
-        gamepads[0].set_pad_in(packet_in_.pad_in);
-
+        if (new_packet_in_)
+        {
+            new_packet_in_ = false;
+            gamepads[0].set_pad_in(packet_in_.pad_in);
+        }
+        
         if (gamepads[0].new_pad_out())
         {
             packet_out_.pad_out = gamepads[0].get_pad_out();
@@ -114,34 +119,38 @@ void I2CSlave::slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
             {
                 case PacketID::PAD:
                     this_instance_->packet_in_ = *reinterpret_cast<PacketIn*>(buffer_in.data());
+                    this_instance_->new_packet_in_ = true;
                     *reinterpret_cast<PacketOut*>(buffer_out.data()) = this_instance_->packet_out_;
                     break;
+
                 case PacketID::STATUS:
                     buffer_out.data()[0] = sizeof(PacketStatus);
                     buffer_out.data()[1] = static_cast<uint8_t>(PacketID::STATUS);
-                    // if something is mounted by tuh, signal to not send gamepad data
-                    buffer_out.data()[2] = this_instance_->i2c_disabled_.load() ? static_cast<uint8_t>(SlaveStatus::NOT_READY) : static_cast<uint8_t>(SlaveStatus::READY);
+                    // 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);
-                    if (!this_instance_->i2c_disabled_.load())
-                    {
-                        // If no TUH devices are mounted, signal to connect usb
-                        OGXMini::update_tuh_status(true);
-                    }
+
+                    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())
                     {
-                        // If no TUH devices are mounted, signal to disconnect usb reset the pico
-                        OGXMini::update_tuh_status(false);
+                        OGXMini::update_tud_status(false);
                     }
                     break;
+
                 default:
                     break;
             }
@@ -155,6 +164,5 @@ void I2CSlave::slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
 
         default:
             break;
-
     }
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
index 984e8ea..78c5ddd 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
@@ -4,7 +4,6 @@
 #include <cstdint>
 #include <atomic>
 #include <cstring>
-#include <pico/time.h>
 #include <hardware/gpio.h>
 #include <hardware/i2c.h>
 #include <pico/i2c_slave.h>
@@ -16,18 +15,19 @@
 class I2CSlave : public I2CDriver
 {
 public:
+    ~I2CSlave() = default;
     void initialize(uint8_t address) override;
     void process(Gamepad (&gamepads)[MAX_GAMEPADS]) override;
     void notify_tuh_mounted(HostDriver::Type host_type) override;
     void notify_tuh_unmounted(HostDriver::Type host_type) override;
-    void notify_xbox360w_connected(uint8_t idx) override {};
-    void notify_xbox360w_disconnected(uint8_t idx) override {};
 
 private:
     static I2CSlave* this_instance_;
+
     PacketIn packet_in_;
     PacketOut packet_out_;
-    std::atomic<bool> i2c_disabled_;
+    bool new_packet_in_{false};
+    std::atomic<bool> i2c_disabled_{false};
 
     static PacketID get_packet_id(uint8_t* buffer_in);
     static void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event);
diff --git a/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp
new file mode 100644
index 0000000..a7dd33c
--- /dev/null
+++ b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp
@@ -0,0 +1,187 @@
+#include <array>
+#include <cstring>
+#include <pico/i2c_slave.h>
+#include <hardware/gpio.h>
+#include <hardware/i2c.h>
+
+#include "Gamepad.h"
+#include "board_config.h"
+#include "Board/board_api.h"
+#include "I2CDriver/ESP32/I2CDriver.h"
+#include "TaskQueue/TaskQueue.h"
+
+namespace I2CDriver {
+
+//May expand commands in the future
+enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
+
+#pragma pack(push, 1)
+struct PacketIn
+{
+    uint8_t packet_len;
+    uint8_t packet_id;
+    uint8_t index;
+    uint8_t gp_data[sizeof(Gamepad::PadIn) - sizeof(Gamepad::PadIn::analog) - sizeof(Gamepad::PadIn::chatpad)];
+
+    PacketIn()
+    {
+        std::memset(this, 0, sizeof(PacketIn));
+        packet_len = sizeof(PacketIn);
+        packet_id = static_cast<uint8_t>(PacketID::SET_PAD);
+    }
+};
+static_assert(sizeof(PacketIn) == 16, "i2c_driver_esp::PacketIn size mismatch");
+
+struct PacketOut
+{
+    uint8_t packet_len;
+    uint8_t packet_id;
+    uint8_t index;
+    Gamepad::PadOut pad_out;
+    uint8_t reserved[3];
+
+    PacketOut()
+    {
+        std::memset(this, 0, sizeof(PacketOut));
+        packet_len = sizeof(PacketOut);
+        packet_id = static_cast<uint8_t>(PacketID::GET_PAD);
+    }
+};
+static_assert(sizeof(PacketOut) == 8, "i2c_driver_esp::PacketOut size mismatch");
+#pragma pack(pop)
+
+static constexpr size_t MAX_BUFFER_SIZE = std::max(sizeof(PacketOut), sizeof(PacketIn));
+static constexpr uint8_t I2C_ADDR = 0x01;
+
+Gamepad* gamepads_[MAX_GAMEPADS];
+
+inline void process_in_packet(PacketIn* packet_in)
+{
+    Gamepad::PadIn gp_in;
+    std::memcpy(&gp_in, packet_in->gp_data, sizeof(packet_in->gp_data));
+
+    //This is a bandaid since the ESP32 doesn't have access to user profiles atm
+    //Will update this once I write a BLE server for interfacing with the webapp
+
+    Gamepad* gamepad = gamepads_[packet_in->index];
+    Gamepad::PadIn mapped_gp_in;
+
+    if (gp_in.dpad & Gamepad::DPAD_UP)    mapped_gp_in.dpad |= gamepad->MAP_DPAD_UP;
+    if (gp_in.dpad & Gamepad::DPAD_DOWN)  mapped_gp_in.dpad |= gamepad->MAP_DPAD_DOWN;
+    if (gp_in.dpad & Gamepad::DPAD_LEFT)  mapped_gp_in.dpad |= gamepad->MAP_DPAD_LEFT;
+    if (gp_in.dpad & Gamepad::DPAD_RIGHT) mapped_gp_in.dpad |= gamepad->MAP_DPAD_RIGHT;
+
+    if (gp_in.buttons & Gamepad::BUTTON_START)  mapped_gp_in.buttons |= gamepad->MAP_BUTTON_START;
+    if (gp_in.buttons & Gamepad::BUTTON_BACK)   mapped_gp_in.buttons |= gamepad->MAP_BUTTON_BACK;
+    if (gp_in.buttons & Gamepad::BUTTON_L3)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_L3;
+    if (gp_in.buttons & Gamepad::BUTTON_R3)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_R3;
+    if (gp_in.buttons & Gamepad::BUTTON_LB)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_LB;
+    if (gp_in.buttons & Gamepad::BUTTON_RB)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_RB;
+    if (gp_in.buttons & Gamepad::BUTTON_SYS)    mapped_gp_in.buttons |= gamepad->MAP_BUTTON_SYS;
+    if (gp_in.buttons & Gamepad::BUTTON_A)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_A;
+    if (gp_in.buttons & Gamepad::BUTTON_B)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_B;
+    if (gp_in.buttons & Gamepad::BUTTON_X)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_X;
+    if (gp_in.buttons & Gamepad::BUTTON_Y)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_Y;
+
+    mapped_gp_in.trigger_l = gp_in.trigger_l;
+    mapped_gp_in.trigger_r = gp_in.trigger_r;
+    mapped_gp_in.joystick_lx = gp_in.joystick_lx;
+    mapped_gp_in.joystick_ly = gp_in.joystick_ly;
+    mapped_gp_in.joystick_rx = gp_in.joystick_rx;
+    mapped_gp_in.joystick_ry = gp_in.joystick_ry;
+
+    gamepad->set_pad_in(mapped_gp_in);
+}
+
+inline void fill_out_report(uint8_t index, PacketOut* report_out)
+{
+    if (index >= MAX_GAMEPADS)
+    {
+        return;
+    }
+
+    report_out->packet_len = sizeof(PacketOut);
+    report_out->packet_id = static_cast<uint8_t>(PacketID::GET_PAD);
+    report_out->index = index;
+    report_out->pad_out = gamepads_[index]->get_pad_out();
+}
+
+PacketID get_packet_id(uint8_t* buffer_in)
+{
+    switch (static_cast<PacketID>(buffer_in[1]))
+    {
+        case PacketID::SET_PAD:
+            if (buffer_in[0] == sizeof(PacketIn))
+            {
+                return PacketID::SET_PAD;
+            }
+            break;
+        //Unused ATM
+        // case PacketID::GET_PAD:
+        //     if (buffer_in[0] == sizeof(PacketOut))
+        //     {
+        //         return PacketID::GET_PAD;
+        //     }
+        //     break;
+        default:
+            break;
+    }
+    return PacketID::UNKNOWN;
+}
+
+static inline void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
+{
+    static int count = 0;
+    static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_in{0};
+    static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_out{0};
+
+    switch (event) 
+    {
+        case I2C_SLAVE_RECEIVE: // master has written
+            if (count < sizeof(PacketIn))
+            {
+                buffer_in.data()[count] = i2c_read_byte_raw(i2c);
+                ++count;
+            }
+            break;
+
+        case I2C_SLAVE_FINISH: // master signalled Stop / Restart
+            if (get_packet_id(buffer_in.data()) == PacketID::SET_PAD)
+            {
+                process_in_packet(reinterpret_cast<PacketIn*>(buffer_in.data()));
+            }
+            count = 0;
+            break;
+
+        case I2C_SLAVE_REQUEST: // master requesting data
+            fill_out_report(reinterpret_cast<PacketIn*>(buffer_in.data())->index, reinterpret_cast<PacketOut*>(buffer_out.data()));
+            i2c_write_raw_blocking(i2c, buffer_out.data(), buffer_out.data()[0]);
+            buffer_in.fill(0);
+            break;
+
+        default:
+            break;
+    }
+}
+
+void initialize(Gamepad (&gamepads)[MAX_GAMEPADS])
+{
+    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
+    {
+        gamepads_[i] = &gamepads[i];
+    }
+
+    i2c_init(I2C_PORT, I2C_BAUDRATE);
+
+    gpio_init(I2C_SDA_PIN);
+    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SDA_PIN);
+
+    gpio_init(I2C_SCL_PIN);
+    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SCL_PIN);
+
+    i2c_slave_init(I2C_PORT, I2C_ADDR, &slave_handler);
+}
+
+} // namespace i2c_driver_esp32
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/i2c_driver_esp32.h b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.h
similarity index 51%
rename from Firmware/RP2040/src/I2CDriver/i2c_driver_esp32.h
rename to Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.h
index ea9e3b7..61191fa 100644
--- a/Firmware/RP2040/src/I2CDriver/i2c_driver_esp32.h
+++ b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.h
@@ -5,10 +5,9 @@
 
 #include "Gamepad.h"
 
-namespace i2c_driver_esp32
+namespace I2CDriver
 {
-    void initialize(std::array<Gamepad, MAX_GAMEPADS>& gamepad);
-    // bool pad_connected();
+    void initialize(Gamepad (&gamepads)[MAX_GAMEPADS]);
 }
 
 #endif // _I2CDRIVER_ESP_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/i2c_4ch_master.cpp b/Firmware/RP2040/src/I2CDriver/i2c_4ch_master.cpp
deleted file mode 100644
index e69de29..0000000
diff --git a/Firmware/RP2040/src/I2CDriver/i2c_driver_4ch.cpp b/Firmware/RP2040/src/I2CDriver/i2c_driver_4ch.cpp
deleted file mode 100644
index 427aa1f..0000000
--- a/Firmware/RP2040/src/I2CDriver/i2c_driver_4ch.cpp
+++ /dev/null
@@ -1,627 +0,0 @@
-// #include <cstdint>
-// #include <array>
-// #include <cstring>
-// #include <atomic>
-
-// #include <pico/i2c_slave.h>
-// #include <pico/time.h>
-// #include <hardware/gpio.h>
-// #include <hardware/i2c.h>
-
-// #include "board_config.h"
-// #include "OGXMini/OGXMini.h"
-// #include "I2CDriver/i2c_driver_4ch.h"
-
-// namespace i2c {
-
-// enum class Mode { MASTER = 0, SLAVE };
-// enum class ReportID : uint8_t { UNKNOWN = 0, PAD, STATUS, CONNECT, DISCONNECT };
-// enum class SlaveStatus : uint8_t { NC = 0, NOT_READY, READY, RESP_OK };
-
-// #pragma pack(push, 1)
-// struct ReportIn
-// {
-//     uint8_t report_len;
-//     uint8_t report_id;
-//     uint8_t pad_in[sizeof(Gamepad::PadIn) - sizeof(Gamepad::PadIn::analog)];
-
-//     ReportIn()
-//     {
-//         std::memset(this, 0, sizeof(ReportIn));
-//         report_len = sizeof(ReportIn);
-//         report_id = static_cast<uint8_t>(ReportID::PAD);
-//     }
-// };
-// static_assert(sizeof(ReportIn) == 18, "I2CDriver::ReportIn size mismatch");
-
-// struct ReportOut
-// {
-//     uint8_t report_len;
-//     uint8_t report_id;
-//     uint8_t pad_out[sizeof(Gamepad::PadOut)];
-
-//     ReportOut()
-//     {
-//         std::memset(this, 0, sizeof(ReportOut));
-//         report_len = sizeof(ReportOut);
-//         report_id = static_cast<uint8_t>(ReportID::PAD);
-//     }
-// };
-// static_assert(sizeof(ReportOut) == 4, "I2CDriver::ReportOut size mismatch");
-
-// struct ReportStatus
-// {
-//     uint8_t report_len;
-//     uint8_t report_id;
-//     uint8_t status;
-
-//     ReportStatus()
-//     {
-//         report_len = sizeof(ReportStatus);
-//         report_id = static_cast<uint8_t>(ReportID::STATUS);
-//         status = static_cast<uint8_t>(SlaveStatus::NC);
-//     }
-// };
-// static_assert(sizeof(ReportStatus) == 3, "I2CDriver::ReportStatus size mismatch");
-// #pragma pack(pop)
-
-// static constexpr size_t MAX_BUFFER_SIZE = std::max(sizeof(ReportStatus), std::max(sizeof(ReportIn), sizeof(ReportOut)));
-
-// driver driver_;
-
-// namespace i2c_master
-// {
-//     struct Master
-//     {
-//         std::atomic<bool> enabled{false};
-
-//         struct Slave
-//         {
-//             uint8_t address{0xFF};
-//             Gamepad* gamepad{nullptr};
-
-//             SlaveStatus status{SlaveStatus::NC};
-//             std::atomic<bool> enabled{false};
-//             uint32_t last_update{0};
-//         };
-
-//         std::array<Slave, MAX_GAMEPADS - 1> slaves;
-//     };
-
-//     Master master_;
-
-//     bool slave_detected(uint8_t address);
-//     bool send_in_report(Master::Slave& slave);
-//     bool get_out_report(Master::Slave& slave);
-//     bool update_slave_enabled(uint8_t address, bool connected);
-//     void notify_tud_deinit();
-//     void update_slave_status(Master::Slave& slave);
-    
-//     void notify_tuh_mounted(HostDriver::Type host_type);
-//     void notify_tuh_unmounted(HostDriver::Type host_type);
-//     void notify_xbox360w_connected(uint8_t idx);
-//     void notify_xbox360w_disconnected(uint8_t idx);
-//     bool is_active();
-//     void process();
-//     void init(Gamepad (&gamepads)[MAX_GAMEPADS]);
-// }
-
-// namespace i2c_slave
-// {
-//     struct Slave
-//     {
-//         uint8_t address{0xFF};
-//         Gamepad* gamepad{nullptr};
-//         std::atomic<bool> i2c_enabled{false};
-//         std::atomic<bool> tuh_enabled{false};
-//     };
-
-//     Slave slave_;
-
-//     void process_in_report(ReportIn* report_in);
-//     void fill_out_report(ReportOut* report_out);
-//     ReportID get_report_id(uint8_t* buffer_in);
-//     void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event);
-
-//     bool is_active();
-//     void notify_tuh_mounted();
-//     void notify_tuh_unmounted();
-//     bool is_active();
-//     void process() {}
-//     void init(uint8_t address, Gamepad& gamepad);
-// }
-
-// namespace i2c_master {
-
-// bool is_active()
-// {
-//     return master_.enabled.load();
-// }
-
-// bool slave_detected(uint8_t address)
-// {
-//     uint8_t dummy_data = 0;
-//     int result = i2c_write_timeout_us(I2C_PORT, address, &dummy_data, 0, false, 1000);
-//     return (result >= 0);
-// }
-
-// void notify_tuh_mounted(HostDriver::Type host_type)
-// {
-//     if (host_type == HostDriver::Type::XBOX360W)
-//     {
-//         master_.enabled.store(true);
-//     }
-// }
-
-// void notify_tuh_unmounted(HostDriver::Type host_type)
-// {
-//     master_.enabled.store(false);
-// }
-
-// void notify_xbox360w_connected(uint8_t idx)
-// {
-//     if (idx < 1 || idx >= MAX_GAMEPADS)
-//     {
-//         return;
-//     }
-//     master_.slaves[idx - 1].enabled.store(true);
-// }
-
-// void notify_xbox360w_disconnected(uint8_t idx)
-// {
-//     if (idx < 1 || idx >= MAX_GAMEPADS)
-//     {
-//         return;
-//     }
-//     master_.slaves[idx - 1].enabled.store(false);
-//     // bool any_connected = false;
-//     // for (auto& slave : master_.slaves)
-//     // {
-//     //     if (slave.enabled.load())
-//     //     {
-//     //         any_connected = true;
-//     //         break;
-//     //     }
-//     // }
-//     // if (!any_connected)
-//     // {
-//     //     master_.enabled.store(false);
-//     // }
-// }
-
-// bool send_in_report(Master::Slave& slave)
-// {
-//     static ReportIn report_in = ReportIn();
-//     Gamepad::PadIn pad_in = slave.gamepad->get_pad_in();
-
-//     std::memcpy(report_in.pad_in, &pad_in, sizeof(ReportIn::pad_in));
-
-//     int count = i2c_write_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&report_in), sizeof(report_in), false);
-//     return (count == sizeof(ReportIn));
-// }
-
-// bool get_out_report(Master::Slave& slave)
-// {
-//     static ReportOut report_out = ReportOut();
-
-//     int count = i2c_read_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&report_out), sizeof(ReportOut), false);
-//     if (count != sizeof(ReportOut))
-//     {
-//         return false;
-//     }
-
-//     Gamepad::PadOut pad_out;
-//     std::memcpy(&pad_out, report_out.pad_out, sizeof(ReportOut::pad_out));
-
-//     return true;
-// }
-
-// bool update_slave_enabled(uint8_t address, bool connected)
-// {
-//     ReportStatus report;
-//     report.report_id = connected ? static_cast<uint8_t>(ReportID::CONNECT) : static_cast<uint8_t>(ReportID::DISCONNECT);
-//     int count = i2c_write_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&report), sizeof(ReportStatus), false);
-//     if (count == sizeof(ReportStatus))
-//     {
-//         count = i2c_read_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&report), sizeof(ReportStatus), false);
-//         return (static_cast<SlaveStatus>(report.status) == SlaveStatus::RESP_OK);
-//     }
-//     return false;
-// }
-
-// void notify_tud_deinit()
-// {
-//     for (auto& slave : master_.slaves)
-//     {
-//         if (slave.status != SlaveStatus::NC)
-//         {
-//             int retries = 0;
-//             while (!update_slave_enabled(slave.address, false) && retries < 6)
-//             {
-//                 sleep_ms(1);
-//                 ++retries;
-//             }
-//         }
-//     }
-// }
-
-// void update_slave_status(Master::Slave& slave)
-// {
-//     bool slave_enabled = slave.enabled.load();
-
-//     if (!slave_detected(slave.address))
-//     {
-//         slave.status = SlaveStatus::NC;
-//         return;
-//     }
-//     if (!update_slave_enabled(slave.address, slave_enabled))
-//     {
-//         slave.status = SlaveStatus::NOT_READY;
-//         return;
-//     }
-//     if (slave_enabled)
-//     {
-//         ReportStatus report = ReportStatus();
-//         int count = 0;
-
-//         report.report_id = static_cast<uint8_t>(ReportID::STATUS);
-//         count = i2c_write_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&report), sizeof(ReportStatus), false);
-//         if (count == sizeof(ReportStatus))
-//         {
-//             count = i2c_read_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&report), sizeof(ReportStatus), false);
-//             slave.status = (count == sizeof(ReportStatus)) ? static_cast<SlaveStatus>(report.status) : SlaveStatus::NC;
-//         }
-//         else
-//         {
-//             slave.status = SlaveStatus::NOT_READY;
-//         }
-//     }
-//     else
-//     {
-//         slave.status = SlaveStatus::NOT_READY;
-//     }
-// }
-
-// void process()
-// {
-//     for (auto& slave : master_.slaves)
-//     {
-//         if ((time_us_32() / 1000) - slave.last_update > 1000)
-//         {
-//             update_slave_status(slave);
-//             slave.last_update = time_us_32() / 1000;
-//         }
-//         sleep_us(100);
-//     }
-//     if (!master_.enabled.load())
-//     {
-//         return;
-//     }
-//     for (auto& slave : master_.slaves)
-//     {
-//         if (slave.status == SlaveStatus::READY)
-//         {
-//             if (send_in_report(slave))
-//             {
-//                 get_out_report(slave);
-//             }
-//             sleep_us(100);
-//         }
-//     }
-// }
-
-// void init(Gamepad (&gamepads)[MAX_GAMEPADS])
-// {
-//     for (uint8_t i = 0; i < master_.slaves.size(); ++i)
-//     {
-//         master_.slaves[i].address = i + 1;
-//         master_.slaves[i].gamepad = &gamepads[i + 1];
-//     }
-// }
-
-// } // namespace i2c_master
-
-// namespace i2c_slave {
-
-// bool is_active()
-// {
-//     return slave_.i2c_enabled.load();
-// }
-
-// void notify_tuh_mounted()
-// {
-//     slave_.tuh_enabled.store(true);
-// }
-
-// void notify_tuh_unmounted()
-// {
-//     slave_.tuh_enabled.store(false);
-// }
-
-// void process_in_report(ReportIn* report_in)
-// {
-//     Gamepad& gamepad = *slave_.gamepad;
-//     Gamepad::PadIn pad_in;
-
-//     std::memcpy(&pad_in, report_in->pad_in, sizeof(ReportIn::pad_in));
-    
-//     gamepad.set_pad_in(pad_in);
-// }
-
-// void fill_out_report(ReportOut* report_out)
-// {
-//     Gamepad& gamepad = *slave_.gamepad;
-//     Gamepad::PadOut pad_out = gamepad.get_pad_out();
-
-//     report_out->report_len = sizeof(ReportOut);
-//     report_out->report_id = static_cast<uint8_t>(ReportID::PAD);
-//     std::memcpy(report_out->pad_out, &pad_out, sizeof(ReportOut::pad_out));
-// }
-
-// ReportID get_report_id(uint8_t* buffer_in)
-// {
-//     switch (static_cast<ReportID>(buffer_in[1]))
-//     {
-//         case ReportID::PAD:
-//             if (buffer_in[0] == sizeof(ReportIn))
-//             {
-//                 return ReportID::PAD;
-//             }
-//             break;
-//         case ReportID::DISCONNECT:
-//             if (buffer_in[0] == sizeof(ReportStatus))
-//             {
-//                 return ReportID::DISCONNECT;
-//             }
-//             break;
-//         case ReportID::CONNECT:
-//             if (buffer_in[0] == sizeof(ReportStatus))
-//             {
-//                 return ReportID::CONNECT;
-//             }
-//             break;
-//         case ReportID::STATUS:
-//             if (buffer_in[0] == sizeof(ReportStatus))
-//             {
-//                 return ReportID::STATUS;
-//             }
-//             break;
-//         default:
-//             break;
-//     }
-//     return ReportID::UNKNOWN;
-// }
-
-// void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
-// {
-//     static int count = 0;
-//     static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_in{0};
-//     static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_out{0};
-
-//     switch (event) 
-//     {
-//         case I2C_SLAVE_RECEIVE: // master has written
-//             if (count < MAX_BUFFER_SIZE)
-//             {
-//                 buffer_in.data()[count] = i2c_read_byte_raw(i2c);
-//                 ++count;
-//             }
-//             break;
-
-//         case I2C_SLAVE_FINISH: // master signalled Stop / Restart
-//             // Each master write has an ID indicating the type of data to send back on the next request
-//             // Every write has an associated read
-//             switch (get_report_id(buffer_in.data())) 
-//             {
-//                 case ReportID::PAD:
-//                     process_in_report(reinterpret_cast<ReportIn*>(buffer_in.data()));
-//                     fill_out_report(reinterpret_cast<ReportOut*>(buffer_out.data()));
-//                     break;
-//                 case ReportID::STATUS:
-//                     buffer_out.data()[0] = sizeof(ReportStatus);
-//                     buffer_out.data()[1] = static_cast<uint8_t>(ReportID::STATUS);
-//                     // if something is mounted by tuh, signal to not send gamepad data
-//                     buffer_out.data()[2] = slave_.tuh_enabled.load() ? static_cast<uint8_t>(SlaveStatus::NOT_READY) : static_cast<uint8_t>(SlaveStatus::READY);
-//                     break;
-//                 case ReportID::CONNECT:
-//                     slave_.i2c_enabled.store(true);
-//                     buffer_out.data()[0] = sizeof(ReportStatus);
-//                     buffer_out.data()[1] = static_cast<uint8_t>(ReportID::STATUS);
-//                     buffer_out.data()[2] = static_cast<uint8_t>(SlaveStatus::RESP_OK);
-//                     break;
-//                 case ReportID::DISCONNECT:
-//                     slave_.i2c_enabled.store(false);
-//                     buffer_out.data()[0] = sizeof(ReportStatus);
-//                     buffer_out.data()[1] = static_cast<uint8_t>(ReportID::STATUS);
-//                     buffer_out.data()[2] = static_cast<uint8_t>(SlaveStatus::RESP_OK);
-//                     break;
-//                 default:
-//                     break;
-//             }
-//             count = 0;
-//             break;
-
-//         case I2C_SLAVE_REQUEST: // master requesting data
-//             i2c_write_raw_blocking(i2c, buffer_out.data(), buffer_out.data()[0]);
-//             buffer_in.fill(0);
-//             break;
-
-//         default:
-//             break;
-
-//     }
-// }
-
-// void init(uint8_t address, Gamepad& gamepad)
-// {
-//     slave_.address = address;
-//     slave_.gamepad = &gamepad;
-//     i2c_slave_init(I2C_PORT, slave_.address, &slave_handler);
-// }
-
-// } // namespace i2c_slave
-
-// Mode mode_{Mode::MASTER};
-
-// uint8_t get_address()
-// {
-//     gpio_init(SLAVE_ADDR_PIN_1);
-//     gpio_init(SLAVE_ADDR_PIN_2);
-//     gpio_pull_up(SLAVE_ADDR_PIN_1);
-//     gpio_pull_up(SLAVE_ADDR_PIN_2);
-
-//     if (gpio_get(SLAVE_ADDR_PIN_1) == 1 && gpio_get(SLAVE_ADDR_PIN_2) == 1)
-//     {
-//         return 0x00; //Master
-//     }
-//     else if (gpio_get(SLAVE_ADDR_PIN_1) == 1 && gpio_get(SLAVE_ADDR_PIN_2) == 0)
-//     {
-//         return 0x01;
-//     }
-//     else if (gpio_get(SLAVE_ADDR_PIN_1) == 0 && gpio_get(SLAVE_ADDR_PIN_2) == 1)
-//     {
-//         return 0x02;
-//     }
-//     else if (gpio_get(SLAVE_ADDR_PIN_1) == 0 && gpio_get(SLAVE_ADDR_PIN_2) == 0)
-//     {
-//         return 0x03;
-//     }
-
-//     return 0xFF;
-// }
-
-// //Public methods
-
-// //Call before core1 starts
-// void initialize(Gamepad (&gamepads)[MAX_GAMEPADS])
-// {
-//     uint8_t address = get_address();
-//     if (address == 0xFF)
-//     {
-//         return;
-//     }
-
-//     i2c_init(I2C_PORT, I2C_BAUDRATE);
-
-//     gpio_init(I2C_SDA_PIN);
-//     gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
-//     gpio_pull_up(I2C_SDA_PIN);
-
-//     gpio_init(I2C_SCL_PIN);
-//     gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
-//     gpio_pull_up(I2C_SCL_PIN);
-
-//     if (address < 1)
-//     {
-//         mode_ = Mode::MASTER;
-//         driver_.process = i2c_master::process;
-//         driver_.is_active = i2c_master::is_active;
-//         driver_.notify_tud_deinit = i2c_master::notify_tud_deinit;
-//         driver_.notify_tuh_mounted = i2c_master::notify_tuh_mounted;
-//         driver_.notify_tuh_unmounted = i2c_master::notify_tuh_unmounted;
-//         driver_.notify_xbox360w_connected = i2c_master::notify_xbox360w_connected;
-//         driver_.notify_xbox360w_disconnected = i2c_master::notify_xbox360w_disconnected;
-//         i2c_master::init(gamepads);
-//     }
-//     else
-//     {
-//         mode_ = Mode::SLAVE;
-        
-//         driver_.process = i2c_slave::is_active;
-//         driver_.is_active = i2c_slave::is_active;
-//         driver_.notify_tud_deinit = i2c_slave::notify_tuh_unmounted;
-//         driver_.notify_tuh_mounted = i2c_slave::notify_tuh_mounted;
-//         driver_.notify_tuh_unmounted = i2c_slave::notify_tuh_unmounted;
-//         driver_.notify_xbox360w_connected = nullptr;
-//         i2c_slave::init(address, gamepads[0]);
-//     }
-// }
-
-// void process()
-// {
-//     switch (mode_)
-//     {
-//         case Mode::MASTER:
-//             i2c_master::process();
-//             break;
-//         case Mode::SLAVE:
-//             return;
-//     }
-// }
-
-// bool is_active()
-// {
-//     switch (mode_)
-//     {
-//         case Mode::MASTER:
-//             return i2c_master::is_active();
-//         case Mode::SLAVE:
-//             return i2c_slave::is_active();
-//     }
-//     return false;
-// }
-
-// //Called from core1
-// void notify_tuh_mounted(HostDriver::Type host_type)
-// {
-//     switch (mode_)
-//     {
-//         case Mode::MASTER:
-//             i2c_master::notify_tuh_mounted(host_type);
-//             break;
-//         case Mode::SLAVE:
-//             i2c_slave::notify_tuh_mounted();
-//             break;
-//     }
-// }
-// //Called from core1
-// void notify_tuh_unmounted(HostDriver::Type host_type)
-// {
-//     switch (mode_)
-//     {
-//         case Mode::MASTER:
-//             i2c_master::notify_tuh_unmounted(host_type);
-//             break;
-//         case Mode::SLAVE:
-//             i2c_slave::notify_tuh_unmounted();
-//             break;
-//     }
-// }
-// //Called from core1
-// void notify_xbox360w_connected(uint8_t idx)
-// {
-//     switch (mode_)
-//     {
-//         case Mode::MASTER:
-//             i2c_master::notify_xbox360w_connected(idx);
-//             break;
-//         case Mode::SLAVE:
-//             return;
-//     }
-// }
-// //Called from core1
-// void notify_xbox360w_disconnected(uint8_t idx)
-// {
-//     switch (mode_)
-//     {
-//         case Mode::MASTER:
-//             i2c_master::notify_xbox360w_disconnected(idx);
-//             break;
-//         case Mode::SLAVE:
-//             return;
-//     }
-// }
-
-// void notify_tud_deinit()
-// {
-//     switch (mode_)
-//     {
-//         case Mode::MASTER:
-//             i2c_master::notify_tud_deinit();
-//             break;
-//         case Mode::SLAVE:
-//             return;
-//     }
-// }
-
-// } // namespace I2CDriver
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/i2c_driver_4ch.h b/Firmware/RP2040/src/I2CDriver/i2c_driver_4ch.h
deleted file mode 100644
index aa66257..0000000
--- a/Firmware/RP2040/src/I2CDriver/i2c_driver_4ch.h
+++ /dev/null
@@ -1,145 +0,0 @@
-// #ifndef _I2C_DRIVER_H_
-// #define _I2C_DRIVER_H_
-
-// #include <cstdint>
-
-// #include "Gamepad.h"
-// #include "USBHost/HostManager.h"
-
-// // /*  Master will only write/read slave if an Xbox360 wireless controller is connected for its corresponding slave.
-// //     Slaves will read/write from the bus only if they don't have a device mounted by tusb host.
-// //     Could be made to work so that a USB hub could be connected to port 1 and host 4 controllers, maybe later.  
-// //     This is run on core0 with the device stack, but some values are checked and modified by core1 (atomic types). */
-// // namespace i2c
-// // {
-// //     void initialize(Gamepad (&gamepads)[MAX_GAMEPADS]);
-
-// //     struct driver 
-// //     {
-// //         void (*process)(){nullptr};
-// //         bool (*is_active)(){nullptr};
-// //         void (*notify_tud_deinit)(){nullptr};
-// //         void (*notify_tuh_mounted)(HostDriver::Type host_type){nullptr};
-// //         void (*notify_tuh_unmounted)(HostDriver::Type host_type){nullptr};
-// //         void (*notify_xbox360w_connected)(uint8_t idx){nullptr};
-// //         void (*notify_xbox360w_disconnected)(uint8_t idx){nullptr};
-// //     };
-    
-// //     driver* get_driver();
-
-// //     // void process();
-// //     // bool is_active();
-
-// //     // void notify_tud_deinit();
-
-// //     // void notify_tuh_mounted(HostDriver::Type host_type = HostDriver::Type::UNKNOWN);
-// //     // void notify_tuh_unmounted(HostDriver::Type host_type = HostDriver::Type::UNKNOWN);
-
-// //     // void notify_xbox360w_connected(uint8_t idx);
-// //     // void notify_xbox360w_disconnected(uint8_t idx);
-// // }
-// class I2CDriver4Ch
-// {
-// public:
-//     I2CDriver4Ch& get_instance()
-//     {
-//         static I2CDriver4Ch instance;
-//         return instance;
-//     }
-    
-//     I2CDriver4Ch(const I2CDriver4Ch&) = delete;
-//     I2CDriver4Ch& operator=(const I2CDriver4Ch&) = delete;
-
-//     void initialize(Gamepad (&gamepads)[MAX_GAMEPADS]);
-
-// private:
-//     I2CDriver4Ch() = default;
-//     ~I2CDriver4Ch() = default;
-
-//     I2CDriver4Ch(const I2CDriver4Ch&) = delete;
-//     I2CDriver4Ch& operator=(const I2CDriver4Ch&) = delete;
-
-//     enum class Mode { MASTER = 0, SLAVE };
-//     enum class ReportID : uint8_t { UNKNOWN = 0, PAD, STATUS, CONNECT, DISCONNECT };
-//     enum class SlaveStatus : uint8_t { NC = 0, NOT_READY, READY, RESP_OK };
-
-//     #pragma pack(push, 1)
-//     struct ReportIn
-//     {
-//         uint8_t report_len;
-//         uint8_t report_id;
-//         uint8_t pad_in[sizeof(Gamepad::PadIn) - sizeof(Gamepad::PadIn::analog)];
-
-//         ReportIn()
-//         {
-//             std::memset(this, 0, sizeof(ReportIn));
-//             report_len = sizeof(ReportIn);
-//             report_id = static_cast<uint8_t>(ReportID::PAD);
-//         }
-//     };
-//     static_assert(sizeof(ReportIn) == 18, "I2CDriver::ReportIn size mismatch");
-
-//     struct ReportOut
-//     {
-//         uint8_t report_len;
-//         uint8_t report_id;
-//         uint8_t pad_out[sizeof(Gamepad::PadOut)];
-
-//         ReportOut()
-//         {
-//             std::memset(this, 0, sizeof(ReportOut));
-//             report_len = sizeof(ReportOut);
-//             report_id = static_cast<uint8_t>(ReportID::PAD);
-//         }
-//     };
-//     static_assert(sizeof(ReportOut) == 4, "I2CDriver::ReportOut size mismatch");
-
-//     struct ReportStatus
-//     {
-//         uint8_t report_len;
-//         uint8_t report_id;
-//         uint8_t status;
-
-//         ReportStatus()
-//         {
-//             report_len = sizeof(ReportStatus);
-//             report_id = static_cast<uint8_t>(ReportID::STATUS);
-//             status = static_cast<uint8_t>(SlaveStatus::NC);
-//         }
-//     };
-//     static_assert(sizeof(ReportStatus) == 3, "I2CDriver::ReportStatus size mismatch");
-//     #pragma pack(pop)
-
-//     static constexpr size_t MAX_BUFFER_SIZE = std::max(sizeof(ReportStatus), std::max(sizeof(ReportIn), sizeof(ReportOut)));
-
-//     struct Master
-//     {
-//         std::atomic<bool> enabled{false};
-
-//         struct Slave
-//         {
-//             uint8_t address{0xFF};
-//             Gamepad* gamepad{nullptr};
-
-//             SlaveStatus status{SlaveStatus::NC};
-//             std::atomic<bool> enabled{false};
-//             uint32_t last_update{0};
-//         };
-
-//         std::array<Slave, MAX_GAMEPADS - 1> slaves;
-//     };
-
-//     struct Slave
-//     {
-//         uint8_t address{0xFF};
-//         Gamepad* gamepad{nullptr};
-//         std::atomic<bool> i2c_enabled{false};
-//         std::atomic<bool> tuh_enabled{false};
-//     };
-
-//     Slave slave_;
-
-//     static Gamepad* gamepads_[MAX_GAMEPADS];
-// };
-
-// #endif // _I2C_DRIVER_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/i2c_driver_esp32.cpp b/Firmware/RP2040/src/I2CDriver/i2c_driver_esp32.cpp
deleted file mode 100644
index 5bae2a9..0000000
--- a/Firmware/RP2040/src/I2CDriver/i2c_driver_esp32.cpp
+++ /dev/null
@@ -1,177 +0,0 @@
-#include <array>
-#include <cstring>
-#include <pico/i2c_slave.h>
-#include <hardware/gpio.h>
-#include <hardware/i2c.h>
-
-#include "Gamepad.h"
-#include "board_config.h"
-#include "Board/board_api.h"
-#include "I2CDriver/i2c_driver_esp32.h"
-
-namespace i2c_driver_esp32 {
-
-//May expand commands in the future
-enum class ReportID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
-
-#pragma pack(push, 1)
-struct ReportIn
-{
-    uint8_t report_len;
-    uint8_t report_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;
-
-    ReportIn()
-    {
-        std::memset(this, 0, sizeof(ReportIn));
-        report_len = sizeof(ReportIn);
-        report_id = static_cast<uint8_t>(ReportID::SET_PAD);
-    }
-};
-static_assert(sizeof(ReportIn) == 16, "i2c_driver_esp::ReportIn size mismatch");
-
-struct ReportOut
-{
-    uint8_t report_len;
-    uint8_t report_id;
-    uint8_t index;
-    uint8_t rumble_l;
-    uint8_t rumble_r;
-
-    ReportOut()
-    {
-        std::memset(this, 0, sizeof(ReportOut));
-        report_len = sizeof(ReportOut);
-        report_id = static_cast<uint8_t>(ReportID::GET_PAD);
-    }
-};
-static_assert(sizeof(ReportOut) == 5, "i2c_driver_esp::ReportOut size mismatch");
-#pragma pack(pop)
-
-static constexpr size_t MAX_BUFFER_SIZE = std::max(sizeof(ReportOut), sizeof(ReportIn));
-static constexpr uint8_t I2C_ADDR = 0x01;
-
-std::array<Gamepad*, MAX_GAMEPADS> gamepads_{nullptr};
-
-inline void process_in_report(ReportIn* report_in)
-{
-    if (report_in->index >= MAX_GAMEPADS)
-    {
-        return;
-    }
-
-    Gamepad* gamepad = gamepads_[report_in->index];
-
-    gamepad->set_buttons(report_in->buttons);
-    gamepad->set_dpad(report_in->dpad);
-    gamepad->set_trigger_l(report_in->trigger_l);
-    gamepad->set_trigger_r(report_in->trigger_r);
-    gamepad->set_joystick_lx_int10(static_cast<int32_t>(report_in->joystick_lx));
-    gamepad->set_joystick_ly_int10(static_cast<int32_t>(report_in->joystick_ly));
-    gamepad->set_joystick_rx_int10(static_cast<int32_t>(report_in->joystick_rx));
-    gamepad->set_joystick_ry_int10(static_cast<int32_t>(report_in->joystick_ry));
-    gamepad->set_new_report(true);
-}
-
-inline void fill_out_report(uint8_t index, ReportOut* report_out)
-{
-    if (index >= MAX_GAMEPADS)
-    {
-        return;
-    }
-
-    Gamepad* gamepad = gamepads_[index];
-    
-    report_out->report_len = sizeof(ReportOut);
-    report_out->report_id = static_cast<uint8_t>(ReportID::GET_PAD);
-    report_out->rumble_l = gamepad->get_rumble_l().uint8();
-    report_out->rumble_r = gamepad->get_rumble_r().uint8();
-}
-
-ReportID get_report_id(uint8_t* buffer_in)
-{
-    switch (static_cast<ReportID>(buffer_in[1]))
-    {
-        case ReportID::SET_PAD:
-            if (buffer_in[0] == sizeof(ReportIn))
-            {
-                return ReportID::SET_PAD;
-            }
-            break;
-        //Unused ATM
-        // case ReportID::GET_PAD:
-        //     if (buffer_in[0] == sizeof(ReportOut))
-        //     {
-        //         return ReportID::GET_PAD;
-        //     }
-        //     break;
-        default:
-            break;
-    }
-    return ReportID::UNKNOWN;
-}
-
-static inline void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
-{
-    static int count = 0;
-    static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_in{0};
-    static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_out{0};
-
-    switch (event) 
-    {
-        case I2C_SLAVE_RECEIVE: // master has written
-            if (count < MAX_BUFFER_SIZE)
-            {
-                buffer_in.data()[count] = i2c_read_byte_raw(i2c);
-                ++count;
-            }
-            break;
-
-        case I2C_SLAVE_FINISH: // master signalled Stop / Restart
-            if (get_report_id(buffer_in.data()) == ReportID::SET_PAD)
-            {
-                process_in_report(reinterpret_cast<ReportIn*>(buffer_in.data()));
-            }
-            count = 0;
-            break;
-
-        case I2C_SLAVE_REQUEST: // master requesting data
-            fill_out_report(reinterpret_cast<ReportIn*>(buffer_in.data())->index, reinterpret_cast<ReportOut*>(buffer_out.data()));
-            i2c_write_raw_blocking(i2c, buffer_out.data(), buffer_out.data()[0]);
-            buffer_in.fill(0);
-            break;
-
-        default:
-            break;
-    }
-}
-
-void initialize(std::array<Gamepad, MAX_GAMEPADS>& gamepad)
-{
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        gamepads_[i] = &gamepad[i];
-    }
-
-    i2c_init(I2C_PORT, I2C_BAUDRATE);
-
-    gpio_init(I2C_SDA_PIN);
-    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(I2C_SDA_PIN);
-
-    gpio_init(I2C_SCL_PIN);
-    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(I2C_SCL_PIN);
-
-    i2c_slave_init(I2C_PORT, I2C_ADDR, &slave_handler);
-}
-
-} // namespace i2c_driver_esp32
\ No newline at end of file
diff --git a/Firmware/RP2040/src/OGXMini/Debug.h b/Firmware/RP2040/src/OGXMini/Debug.h
new file mode 100644
index 0000000..7f9c2b4
--- /dev/null
+++ b/Firmware/RP2040/src/OGXMini/Debug.h
@@ -0,0 +1,55 @@
+#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_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini.h b/Firmware/RP2040/src/OGXMini/OGXMini.h
index b5ab986..24a6f43 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini.h
+++ b/Firmware/RP2040/src/OGXMini/OGXMini.h
@@ -7,8 +7,6 @@
 
 namespace OGXMini
 {
-    enum class TUDStatus { INIT, DEINIT, NOCHANGE };
-
     static constexpr int32_t FEEDBACK_DELAY_MS = 150;
     static constexpr int32_t TUD_CHECK_DELAY_MS = 500;
 
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_4Ch.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp
similarity index 76%
rename from Firmware/RP2040/src/OGXMini/OGXMini_4Ch.cpp
rename to Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp
index 9621958..6512839 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_4Ch.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp
@@ -18,28 +18,7 @@
 namespace OGXMini {
 
 Gamepad gamepads_[MAX_GAMEPADS];
-
-//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)
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_disconnect();
-            sleep_ms(300);
-            multicore_reset_core1();
-            sleep_ms(300);
-            board_api::reboot();
-        });
-    }
-    else if (!tud_inited())
-    {
-        tud_init(BOARD_TUD_RHPORT);
-    }
-}
+std::atomic<bool> tud_inited_{false};
 
 void core1_task()
 {
@@ -64,31 +43,34 @@ void core1_task()
     }
 }
 
-void run_program()
+//Called by tusb host or i2c driver so we know to connect or disconnect usb
+void update_tud_status(bool host_mounted)
 {
-    UserSettings user_settings;
-    user_settings.initialize_flash();
+    board_api::set_led(host_mounted);
 
-    board_init();
-    board_api::init_gpio();
-    board_api::set_led(false);
-
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
+    if (!host_mounted && tud_inited_.load())
     {
-        gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
+        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);
+        });
+    }
+}
 
-    DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(user_settings.get_current_driver());
-
-    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();
-    TaskQueue::Core0::queue_delayed_task(tid_gp_check, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
+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]))
@@ -97,9 +79,31 @@ void run_program()
             user_settings.store_driver_type_safe(user_settings.get_current_driver());
         }
     });
+}
 
-    DeviceDriver* device_driver = device_manager.get_driver();
-    I2CDriver* i2c_driver = i2c_manager.get_driver();
+void run_program()
+{
+    UserSettings user_settings;
+    user_settings.initialize_flash();
+
+    board_api::init_board();
+
+    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
+    {
+        gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
+    }
+
+    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
+    I2CManager::get_instance().initialize_driver();
+    
+    multicore_reset_core1();
+    multicore_launch_core1(core1_task);
+
+    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
+    set_gp_check_timer(tid_gp_check, user_settings);
+
+    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
+    I2CDriver* i2c_driver = I2CManager::get_instance().get_driver();
 
     //Wait for something to call tud_init
     while (!tud_inited())
@@ -108,11 +112,15 @@ void run_program()
         sleep_ms(10);
     }
 
+    tud_inited_.store(true);
+
     while (true)
     {
         TaskQueue::Core0::process_tasks();
+
         i2c_driver->process(gamepads_);
         device_driver->process(0, gamepads_[0]);
+
         tud_task();
         sleep_us(100);
     }
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
index 6c67881..db8efa2 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
@@ -1,11 +1,7 @@
 #include "board_config.h"
 #if OGXM_BOARD == W_ESP32
 
-#include <array>
-#include <pico/stdlib.h>
 #include <pico/multicore.h>
-#include <hardware/gpio.h>
-#include <hardware/sync.h>
 
 #include "tusb.h"
 #include "bsp/board_api.h"
@@ -13,36 +9,48 @@
 #include "USBDevice/DeviceManager.h"
 #include "Board/board_api.h"
 #include "OGXMini/OGXMini.h"
-#include "I2CDriver/i2c_driver_esp32.h"
+#include "I2CDriver/ESP32/I2CDriver.h"
 #include "Gamepad.h"
+#include "TaskQueue/TaskQueue.h"
 
 namespace OGXMini {
 
-std::array<Gamepad, MAX_GAMEPADS> gamepads_;
+static Gamepad gamepads_[MAX_GAMEPADS];
+
+//Not using this for ESP32 currently
+void update_tud_status(bool host_mounted) { }
+
+void core1_task()
+{
+    I2CDriver::initialize(gamepads_);
+
+    while (true) 
+    {
+        tight_loop_contents();
+    }
+}
 
 void run_esp32_uart_bridge()
 {
     DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(DeviceDriver::Type::UART_BRIDGE);
+    device_manager.initialize_driver(DeviceDriver::Type::UART_BRIDGE, gamepads_);
+
     board_api::enter_esp32_prog_mode();
-    device_manager.get_driver()->process(0, gamepads_.front()); //Runs UART Bridge task
+    
+    device_manager.get_driver()->process(0, gamepads_[0]); //Runs UART Bridge task, doesn't return
 }
 
-void core1_task()
+void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
 {
-    i2c_driver_esp32::initialize(gamepads_);
-
-    while (true) 
+    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
     {
-        __wfi();
-    }
-}
-
-bool gp_check_cb(repeating_timer_t* rt)
-{
-    GPCheckContext* gp_check_ctx = static_cast<GPCheckContext*>(rt->user_data);
-    gp_check_ctx->driver_changed = gp_check_ctx->user_settings.check_for_driver_change(gamepads_.front());
-    return true;
+        //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()
@@ -50,56 +58,44 @@ void run_program()
     UserSettings user_settings;
     user_settings.initialize_flash();
 
-    board_api::init_gpio();
+    board_api::init_board();
 
     if (board_api::uart_bridge_mode())
     {
         run_esp32_uart_bridge();
         return;
     }
-    // else if (!user_settings.verify_firmware_version())
-    // {
-    //     user_settings.write_firmware_version();
-    // }
 
-    board_init();
-
-    for (uint8_t i = 0; i < gamepads_.size(); ++i)
+    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
     {
         gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
     }
 
-    DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(user_settings.get_current_driver());
+    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
 
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
 
     board_api::reset_esp32();
 
-    GPCheckContext gp_check_ctx = { false, user_settings };
-    repeating_timer_t gp_check_timer;
-    add_repeating_timer_ms(UserSettings::GP_CHECK_DELAY_MS, gp_check_cb, &gp_check_ctx, &gp_check_timer);
+    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();
+    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
 
     tud_init(BOARD_TUD_RHPORT);
 
     while (true)
     {
-        for (uint8_t i = 0; i < gamepads_.size(); ++i)
+        TaskQueue::Core0::process_tasks();
+
+        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
         {
             device_driver->process(i, gamepads_[i]);
             tud_task();
         }
         
         sleep_us(100);
-
-        if (gp_check_ctx.driver_changed)
-        {
-            cancel_repeating_timer(&gp_check_timer);
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
     }
 }
 
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
index 0775f27..e60f8e4 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
@@ -1,11 +1,7 @@
 #include "board_config.h"
 #if (OGXM_BOARD == PI_PICOW)
 
-#include <pico/stdlib.h>
-#include <hardware/sync.h>
 #include <pico/multicore.h>
-#include <hardware/gpio.h>
-#include <hardware/clocks.h>
 #include <pico/cyw43_arch.h>
 
 #include "tusb.h"
@@ -15,57 +11,63 @@
 #include "Board/board_api.h"
 #include "Bluepad32/Bluepad32.h"
 #include "OGXMini/OGXMini.h"
+#include "Gamepad.h"
+#include "TaskQueue/TaskQueue.h"
 
 namespace OGXMini {
 
 Gamepad gamepads_[MAX_GAMEPADS];
 
+//Not using this for Pico W currently
+void update_tud_status(bool host_mounted) { }
+
 void core1_task()
 {
     if (cyw43_arch_init() != 0)
-    {
+    {        
         return;
     }
 
+    //Doesn't return, don't do anything with core1 unless it's executing within the BTStack loop
     bluepad32::run_task(gamepads_);
 }
 
-bool gp_check_cb(repeating_timer_t* rt)
-{
-    GPCheckContext* gp_check_ctx = static_cast<GPCheckContext*>(rt->user_data);
-    gp_check_ctx->driver_changed = gp_check_ctx->user_settings.check_for_driver_change(gamepads_[0]);
-    return true;
-}
-
 void run_program()
 {
     UserSettings user_settings;
     user_settings.initialize_flash();
 
-    board_init();
-    board_api::init_gpio();
+    board_api::init_board();
 
     for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
     {
         gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
     }
 
-    DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(user_settings.get_current_driver());
+    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
 
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
 
-    GPCheckContext gp_check_ctx = { false, user_settings };
-    repeating_timer_t gp_check_timer;
-    add_repeating_timer_ms(UserSettings::GP_CHECK_DELAY_MS, gp_check_cb, &gp_check_ctx, &gp_check_timer);
+    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
+    TaskQueue::Core0::queue_delayed_task(tid_gp_check, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
+    {
+        //Check gamepad inputs for button combo to change usb device driver
+        if (user_settings.check_for_driver_change(gamepads_[0]))
+        {
+            //This will store the new mode and reboot the pico
+            user_settings.store_driver_type_safe(user_settings.get_current_driver());
+        }
+    });
 
-    DeviceDriver* device_driver = device_manager.get_driver();
+    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
 
     tud_init(BOARD_TUD_RHPORT);
 
     while (true)
     {
+        TaskQueue::Core0::process_tasks();
+
         for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
         {
             device_driver->process(i, gamepads_[i]);
@@ -73,12 +75,6 @@ void run_program()
         }
 
         sleep_us(100);
-
-        if (gp_check_ctx.driver_changed)
-        {
-            cancel_repeating_timer(&gp_check_timer);
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
     }
 }
 
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
index 1dc2942..60a6cef 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
@@ -1,11 +1,7 @@
 #include "board_config.h"
-#if (OGXM_BOARD == ADA_FEATHER) || (OGXM_BOARD == RP_ZERO) || (OGXM_BOARD == PI_PICO)
+#if (OGXM_BOARD == ADA_FEATHER) || (OGXM_BOARD == RP_ZERO) || (OGXM_BOARD == PI_PICO) || (OGXM_BOARD == PI_PICO2)
 
-#include <array>
-#include <atomic>
 #include <pico/multicore.h>
-#include <hardware/gpio.h>
-#include <hardware/clocks.h>
 
 #include "tusb.h"
 #include "bsp/board_api.h"
@@ -13,26 +9,15 @@
 
 #include "USBHost/HostManager.h"
 #include "USBDevice/DeviceManager.h"
-#include "Board/board_api.h"
 #include "OGXMini/OGXMini.h"
+#include "TaskQueue/TaskQueue.h"
 #include "Gamepad.h"
+#include "Board/board_api.h"
 
 namespace OGXMini {
 
-std::atomic<TUDStatus> tud_status_ = TUDStatus::NOCHANGE;
 Gamepad gamepads_[MAX_GAMEPADS];
-
-void update_tuh_status(bool mounted)
-{
-    tud_status_.store(mounted ? TUDStatus::INIT : TUDStatus::DEINIT);
-    board_api::set_led(mounted);
-}
-
-bool feedback_cb(repeating_timer_t* rt)
-{
-    static_cast<HostManager*>(rt->user_data)->send_feedback();
-    return true;
-}
+std::atomic<bool> tud_inited_{false};
 
 void core1_task()
 {
@@ -44,49 +29,63 @@ void core1_task()
 
     tuh_init(BOARD_TUH_RHPORT);
 
-    repeating_timer_t feedback_timer;
-    add_repeating_timer_ms(FEEDBACK_DELAY_MS, feedback_cb, &host_manager, &feedback_timer);
-
-    while (true) 
+    uint32_t tid_feedback = TaskQueue::Core1::get_new_task_id();
+    TaskQueue::Core1::queue_delayed_task(tid_feedback, FEEDBACK_DELAY_MS, true, [&host_manager]
     {
+        host_manager.send_feedback();
+    });
+
+    while (true)
+    {
+        TaskQueue::Core1::process_tasks();
         tuh_task();
-        sleep_us(100);
     }
 }
 
-// bool gp_check_cb(repeating_timer_t* rt)
-// {
-//     GPCheckContext* gp_check_ctx = static_cast<GPCheckContext*>(rt->user_data);
-//     // gp_check_ctx->driver_changed = gp_check_ctx->user_settings.check_for_driver_change(gamepads_[0]);
-//     if (gp_check_ctx->user_settings.check_for_driver_change(gamepads_[0]))
-//     {
-//         gp_check_ctx->user_settings.store_driver_type_safe(gp_check_ctx->user_settings.get_current_driver());
-//         return false;
-//     }
-//     return true;
-// }
-bool gp_check_cb(repeating_timer_t* rt)
+//Called by tusb host or i2c driver so we know to connect or disconnect usb
+void update_tud_status(bool host_mounted)
 {
-    UserSettings* user_settings = static_cast<UserSettings*>(rt->user_data);
-    if (user_settings->check_for_driver_change(gamepads_[0]))
+    board_api::set_led(host_mounted);
+
+    if (!host_mounted && tud_inited_.load())
     {
-        user_settings->store_driver_type_safe(user_settings->get_current_driver());
-        return false;
+        TaskQueue::Core0::queue_task([]()
+        {
+            tud_disconnect();
+            sleep_ms(300);
+            multicore_reset_core1();
+            sleep_ms(300);
+            board_api::reboot();
+        });
     }
-    return true;
+    else if (!tud_inited_.load())
+    {
+        TaskQueue::Core0::queue_task([]()
+        {
+            tud_init(BOARD_TUD_RHPORT);
+        });
+    }
+}
+
+void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
+{
+    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
+    {
+        //Check gamepad inputs for button combo to change usb device driver
+        if (user_settings.check_for_driver_change(gamepads_[0]))
+        {
+            //This will store the new mode and reboot the pico
+            user_settings.store_driver_type_safe(user_settings.get_current_driver());
+        }
+    });
 }
 
 void run_program()
 {
-    set_sys_clock_khz(250000, true);
-
     UserSettings user_settings;
     user_settings.initialize_flash();
 
-    board_init();
-
-    board_api::init_gpio();
-    board_api::set_led(false);
+    board_api::init_board();
 
     for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
     {
@@ -94,52 +93,39 @@ void run_program()
     }
 
     DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(user_settings.get_current_driver());
+    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
 
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
 
-    // GPCheckContext gp_check_ctx = { false, user_settings };
-    // repeating_timer_t gp_check_timer;
-    // add_repeating_timer_ms(UserSettings::GP_CHECK_DELAY_MS, gp_check_cb, &gp_check_ctx, &gp_check_timer);
-
-    repeating_timer_t gp_check_timer;
-    add_repeating_timer_ms(UserSettings::GP_CHECK_DELAY_MS, gp_check_cb, &user_settings, &gp_check_timer);
+    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 (tud_status_.load() != TUDStatus::INIT)
+    // Wait for something to call update_tud_status()
+    while (!tud_inited())
     {
-        sleep_ms(1);
+        TaskQueue::Core0::process_tasks();
+        sleep_ms(100);
     }
 
-    tud_init(BOARD_TUD_RHPORT);
+    tud_inited_.store(true);
 
     while (true)
     {
-        switch(tud_status_.load())
-        {
-            case TUDStatus::DEINIT:
-                tud_disconnect();
-                sleep_ms(300);
-                multicore_reset_core1();
-                sleep_ms(300);
-                board_api::reboot();
-                break;
-            default:
-                break;
-        }
+        TaskQueue::Core0::process_tasks();
 
         for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
         {
             device_driver->process(i, gamepads_[i]);
-            tud_task();
         }
 
+        tud_task();
         sleep_us(100);
     }
 }
 
 } // namespace OGXMini
 
-#endif // (OGXM_BOARD == ADA_FEATHER) || (OGXM_BOARD == RP_ZERO) || (OGXM_BOARD == PI_PICO)
\ No newline at end of file
+#endif // (OGXM_BOARD == ADA_FEATHER) || (OGXM_BOARD == RP_ZERO) || (OGXM_BOARD == PI_PICO) || (OGXM_BOARD == PI_PICO2)
\ No newline at end of file
diff --git a/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp b/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
index 1ee7244..374e935 100644
--- a/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
+++ b/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
@@ -1,73 +1,254 @@
 #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 {
 
-struct Task
+class CoreQueue
 {
-    std::function<void()> function = nullptr;
-};
+public:
+    enum class CoreNum : uint8_t
+    {
+        Core0 = 0,
+        Core1,
+    };
 
-struct DelayedTask
-{
-    uint32_t task_id = 0;
-    uint32_t interval_ms = 0;
-    uint64_t target_time = 0;
-    std::function<void()> function = nullptr;
-};
+    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;
 
-static constexpr uint8_t MAX_TASKS = 8;
-static constexpr uint8_t MAX_DELAYED_TASKS = MAX_TASKS * 2;
+        hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
 
-static inline uint32_t TIMER_IRQ(int alarm_num)
-{
-    return timer_hardware_alarm_get_irq_num(timer_hw, alarm_num);
-}
+        irq_set_exclusive_handler(
+            TIMER_IRQ(alarm_num_), 
+            (core_num_ == CoreNum::Core0) ? timer_irq_wrapper_c0 : timer_irq_wrapper_c1);
 
-static uint64_t get_time_64_us()
-{
-    static spin_lock_t* spinlock_64_us = spin_lock_instance(4);
-    uint32_t irq_state = spin_lock_blocking(spinlock_64_us);
-    uint32_t lo = timer_hw->timelr;
-    uint32_t hi = timer_hw->timehr;
-    spin_unlock(spinlock_64_us, irq_state);
-    return ((uint64_t) hi << 32u) | lo;;
-}
+        irq_set_enabled(TIMER_IRQ(alarm_num_), true);
+    }
 
-namespace Core1 {
-
-    static constexpr uint32_t CORE = 1;
-    static constexpr uint32_t ALARM_NUM = CORE;
-
-    static std::array<Task, MAX_TASKS> tasks_;
-    static std::array<DelayedTask, MAX_DELAYED_TASKS> delayed_tasks_;
-
-    static bool delay_irq_set_ = false;
-    static uint32_t new_task_id_ = 1;
-
-    static spin_lock_t* spinlock_ = spin_lock_instance(CORE);
-    static spin_lock_t* spinlock_delayed_ = spin_lock_instance(CORE + 2);
+    ~CoreQueue() = default;
 
     uint32_t get_new_task_id()
     {
         return new_task_id_++;
     }
 
-    static void timer_irq_handler()
+    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
     {
-        hw_clear_bits(&timer_hw->intr, 1u << ALARM_NUM);
+        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)
+        {
+            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 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 process_tasks()
+    {
+        uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
+        for (auto& task : task_queue_)
+        {
+            if (task.function)
+            {
+                auto function = task.function;
+                task.function = nullptr;
+                spin_unlock(spinlock_queue_, irq_state);
+
+                function();
+
+                irq_state = spin_lock_blocking(spinlock_queue_);
+            }
+            else
+            {
+                break; //No more tasks
+            }
+        }
+        spin_unlock(spinlock_queue_, irq_state);
+    }
+
+private:
+    CoreQueue& operator=(const CoreQueue&) = delete;
+    CoreQueue(CoreQueue&&) = delete;
+    CoreQueue& operator=(CoreQueue&&) = delete;
+
+    static CoreQueue* instances_[static_cast<uint8_t>(CoreNum::Core1) + 1];
+
+    struct Task
+    {
+        // uint32_t task_id = 0;
+        std::function<void()> function = nullptr;
+    };
+
+    struct DelayedTask
+    {
+        uint32_t task_id = 0;
+        uint32_t interval_ms = 0;
+        uint64_t target_time = 0;
+        std::function<void()> function = nullptr;
+    };
+
+    static constexpr uint8_t MAX_TASKS = 8;
+    static constexpr uint8_t MAX_DELAYED_TASKS = MAX_TASKS * 2;
+
+    CoreNum core_num_;
+    uint32_t alarm_num_;
+    uint32_t new_task_id_ = 1;
+
+    int spinlock_queue_num_ = spin_lock_claim_unused(true);
+    int spinlock_delayed_num_ = spin_lock_claim_unused(true);
+    spin_lock_t* spinlock_queue_ = spin_lock_instance(static_cast<uint>(spinlock_queue_num_));
+    spin_lock_t* spinlock_delayed_ = spin_lock_instance(static_cast<uint>(spinlock_delayed_num_));
+
+    std::array<Task, MAX_TASKS> task_queue_;
+    std::array<DelayedTask, MAX_DELAYED_TASKS> task_queue_delayed_;
+
+    static void timer_irq_wrapper_c0()
+    {
+        instances_[static_cast<uint8_t>(CoreNum::Core0)]->timer_irq_handler();
+    }
+
+    static void timer_irq_wrapper_c1()
+    {
+        instances_[static_cast<uint8_t>(CoreNum::Core1)]->timer_irq_handler();
+    }
+    
+    static inline uint32_t TIMER_IRQ(uint32_t alarm_num)
+    {
+        // switch (alarm_num)
+        // {
+        //     case 1:
+        //         return TIMER_IRQ_1;
+        //     case 2:
+        //         return TIMER_IRQ_2;
+        //     default:
+        //         return TIMER_IRQ_0;
+        // }
+        
+        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 : delayed_tasks_) 
+        for (auto& task : task_queue_delayed_) 
         {
             if (task.function && task.target_time <= now) 
             {
@@ -88,339 +269,531 @@ namespace Core1 {
             }
         }
 
-        auto it = std::min_element(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
+        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 != delayed_tasks_.end() && it->function) 
+        if (it != task_queue_delayed.end() && it->function) 
         {
-            timer_hw->alarm[ALARM_NUM] = static_cast<uint32_t>(it->target_time);
+            return static_cast<int64_t>(it->target_time);
         }
-
-        spin_unlock(spinlock_delayed_, irq_state);
+        return -1;
     }
+};
 
-    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+CoreQueue* CoreQueue::instances_[static_cast<uint8_t>(CoreQueue::CoreNum::Core1) + 1]{nullptr};
+
+namespace Core0
+{
+    static inline CoreQueue& get_core_0()
     {
-        uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
-        for (const auto& task : delayed_tasks_) 
-        {
-            if (task.task_id == task_id)
-            {
-                spin_unlock(spinlock_delayed_, irq_state);
-                return false;
-            }
-        }
-        spin_unlock(spinlock_delayed_, irq_state);
-
-        hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
-        
-        if (!delay_irq_set_)
-        {
-            delay_irq_set_ = true;
-            irq_set_exclusive_handler(TIMER_IRQ(ALARM_NUM), timer_irq_handler);
-            irq_set_enabled(TIMER_IRQ(ALARM_NUM), true);
-        }
-
-        irq_state = spin_lock_blocking(spinlock_delayed_);
-        
-        uint64_t target_time = timer_hw->timerawl + static_cast<uint64_t>(delay_ms) * 1000;
-
-        for (auto& task : delayed_tasks_) 
-        {
-            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(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
-                {
-                    return a.function && (!b.function || a.target_time < b.target_time);
-                });
-
-                if (it != delayed_tasks_.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;
+        static CoreQueue core(CoreQueue::CoreNum::Core0);
+        return core;
     }
-
-    void cancel_delayed_task(uint32_t task_id)
-    {
-        uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
-        bool found = false;
-
-        for (auto& task : delayed_tasks_) 
-        {
-            if (task.task_id == task_id) 
-            {
-                task.function = nullptr;
-                task.task_id = 0;
-                found = true;
-            }
-        }
-
-        if (!found || !delay_irq_set_)
-        {
-            spin_unlock(spinlock_delayed_, irq_state);
-            return;
-        }
-
-        hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
-
-        auto it = std::min_element(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
-        {
-            return a.function && (!b.function || a.target_time < b.target_time);
-        });
-
-        if (it != delayed_tasks_.end() && it->function) 
-        {
-            timer_hw->alarm[ALARM_NUM] = static_cast<uint32_t>(it->target_time);
-        }
-        
-        spin_unlock(spinlock_delayed_, irq_state);
-    }
-
-    bool queue_task(const std::function<void()>& function)
-    {
-        uint32_t irq_state = spin_lock_blocking(spinlock_);
-        for (auto& task : tasks_)
-        {
-            if (!task.function)
-            {
-                task.function = function;
-                spin_unlock(spinlock_, irq_state);
-                return true;
-            }
-        }
-        spin_unlock(spinlock_, irq_state);
-        return false;
-    }
-
-    void process_tasks()
-    {
-        uint32_t irq_state = spin_lock_blocking(spinlock_);
-        for (auto& task : tasks_)
-        {
-            if (task.function)
-            {
-                auto function = task.function;
-                task.function = nullptr;
-                spin_unlock(spinlock_, irq_state);
-
-                function();
-
-                irq_state = spin_lock_blocking(spinlock_);
-            }
-            else
-            {
-                break; //No more tasks
-            }
-        }
-        spin_unlock(spinlock_, irq_state);
-    }
-
-} // namespace Core1
-
-namespace Core0 {
-
-    static constexpr uint32_t CORE = 0;
-    static constexpr uint32_t ALARM_NUM = CORE;
-
-    static std::array<Task, MAX_TASKS> tasks_;
-    static std::array<DelayedTask, MAX_DELAYED_TASKS> delayed_tasks_;
-
-    static bool delay_irq_set_ = false;
-    static uint32_t new_task_id_ = 1;
-
-    static spin_lock_t* spinlock_ = spin_lock_instance(CORE);
-    static spin_lock_t* spinlock_delayed_ = spin_lock_instance(CORE + 2);
-
     uint32_t get_new_task_id()
     {
-        return new_task_id_++;
+        return get_core_0().get_new_task_id();
     }
-
-    static 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 : delayed_tasks_) 
-        {
-            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_);
-            }
-        }
-
-        auto it = std::min_element(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
-        {
-            return a.function && (!b.function || a.target_time < b.target_time);
-        });
-
-        if (it != delayed_tasks_.end() && it->function) 
-        {
-            timer_hw->alarm[ALARM_NUM] = static_cast<uint32_t>(it->target_time);
-        }
-
-        spin_unlock(spinlock_delayed_, irq_state);
-    }
-
-    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 : delayed_tasks_) 
-        {
-            if (task.task_id == task_id)
-            {
-                spin_unlock(spinlock_delayed_, irq_state);
-                return false;
-            }
-        }
-        spin_unlock(spinlock_delayed_, irq_state);
-
-        hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
-        
-        if (!delay_irq_set_)
-        {
-            delay_irq_set_ = true;
-            irq_set_exclusive_handler(TIMER_IRQ(ALARM_NUM), timer_irq_handler);
-            irq_set_enabled(TIMER_IRQ(ALARM_NUM), true);
-        }
-
-        irq_state = spin_lock_blocking(spinlock_delayed_);
-        
-        uint64_t target_time = timer_hw->timerawl + static_cast<uint64_t>(delay_ms) * 1000;
-
-        for (auto& task : delayed_tasks_) 
-        {
-            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(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
-                {
-                    return a.function && (!b.function || a.target_time < b.target_time);
-                });
-
-                if (it != delayed_tasks_.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 : delayed_tasks_) 
-        {
-            if (task.task_id == task_id) 
-            {
-                task.function = nullptr;
-                task.task_id = 0;
-                found = true;
-            }
-        }
-
-        if (!found || !delay_irq_set_)
-        {
-            spin_unlock(spinlock_delayed_, irq_state);
-            return;
-        }
-
-        hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
-
-        auto it = std::min_element(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
-        {
-            return a.function && (!b.function || a.target_time < b.target_time);
-        });
-
-        if (it != delayed_tasks_.end() && it->function) 
-        {
-            timer_hw->alarm[ALARM_NUM] = static_cast<uint32_t>(it->target_time);
-        }
-        
-        spin_unlock(spinlock_delayed_, irq_state);
+        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)
     {
-        uint32_t irq_state = spin_lock_blocking(spinlock_);
-        for (auto& task : tasks_)
-        {
-            if (!task.function)
-            {
-                task.function = function;
-                spin_unlock(spinlock_, irq_state);
-                return true;
-            }
-        }
-        spin_unlock(spinlock_, irq_state);
-        return false;
+        return get_core_0().queue_task(function);
     }
-
     void process_tasks()
     {
-        uint32_t irq_state = spin_lock_blocking(spinlock_);
-        for (auto& task : tasks_)
-        {
-            if (task.function)
-            {
-                auto function = task.function;
-                task.function = nullptr;
-                spin_unlock(spinlock_, irq_state);
-
-                function();
-
-                irq_state = spin_lock_blocking(spinlock_);
-            }
-            else
-            {
-                break; //No more tasks
-            }
-        }
-        spin_unlock(spinlock_, irq_state);
+        get_core_0().process_tasks();
     }
+}
 
-} // namespace Core0
+#if OGXM_BOARD != PI_PICOW
 
-} // namespace TaskQueue
\ No newline at end of file
+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
+
+// #include <array>
+// #include <algorithm>
+// #include <pico/stdlib.h>
+// #include <hardware/timer.h>
+// #include <hardware/irq.h>
+// #include <hardware/sync.h>
+
+// #include "Board/board_api.h"
+// #include "TaskQueue/TaskQueue.h"
+
+// namespace TaskQueue {
+
+// 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;
+
+// static inline uint32_t TIMER_IRQ(int alarm_num)
+// {
+//     return timer_hardware_alarm_get_irq_num(timer_hw, alarm_num);
+// }
+
+// static uint64_t get_time_64_us()
+// {
+//     static spin_lock_t* spinlock_64_us = spin_lock_instance(4);
+//     uint32_t irq_state = spin_lock_blocking(spinlock_64_us);
+//     uint32_t lo = timer_hw->timelr;
+//     uint32_t hi = timer_hw->timehr;
+//     spin_unlock(spinlock_64_us, irq_state);
+//     return ((uint64_t) hi << 32u) | lo;;
+// }
+
+// #if OGXM_BOARD != PI_PICOW
+
+// namespace Core1 {
+
+//     static constexpr uint32_t ALARM_NUM = 1;
+
+//     static std::array<Task, MAX_TASKS> tasks_;
+//     static std::array<DelayedTask, MAX_DELAYED_TASKS> delayed_tasks_;
+
+//     static bool delay_irq_set_ = false;
+//     static uint32_t new_task_id_ = 1;
+
+//     static spin_lock_t* spinlock_ = spin_lock_instance(1);
+//     static spin_lock_t* spinlock_delayed_ = spin_lock_instance(2);
+
+//     bool queue_task(const std::function<void()>& function); 
+
+//     uint32_t get_new_task_id()
+//     {
+//         return new_task_id_++;
+//     }
+
+//     static 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 : delayed_tasks_) 
+//         {
+//             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_);
+//             }
+//         }
+
+//         auto it = std::min_element(delayed_tasks_.begin(), delayed_tasks_.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 != delayed_tasks_.end() && it->function) 
+//         {
+//             timer_hw->alarm[ALARM_NUM] = static_cast<uint32_t>(it->target_time);
+//         }
+
+//         spin_unlock(spinlock_delayed_, irq_state);
+//     }
+
+//     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 : delayed_tasks_) 
+//         {
+//             if (task.task_id == task_id)
+//             {
+//                 spin_unlock(spinlock_delayed_, irq_state);
+//                 return false;
+//             }
+//         }
+//         spin_unlock(spinlock_delayed_, irq_state);
+
+//         hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
+        
+//         if (!delay_irq_set_)
+//         {
+//             delay_irq_set_ = true;
+//             irq_set_exclusive_handler(TIMER_IRQ(ALARM_NUM), timer_irq_handler);
+//             irq_set_enabled(TIMER_IRQ(ALARM_NUM), true);
+//         }
+
+//         irq_state = spin_lock_blocking(spinlock_delayed_);
+        
+//         //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 : delayed_tasks_) 
+//         {
+//             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(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
+//                 {
+//                     return a.function && (!b.function || a.target_time < b.target_time);
+//                 });
+
+//                 if (it != delayed_tasks_.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 : delayed_tasks_) 
+//         {
+//             if (task.task_id == task_id) 
+//             {
+//                 task.function = nullptr;
+//                 task.task_id = 0;
+//                 found = true;
+//             }
+//         }
+
+//         if (!found || !delay_irq_set_)
+//         {
+//             spin_unlock(spinlock_delayed_, irq_state);
+//             return;
+//         }
+
+//         hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
+
+//         auto it = std::min_element(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
+//         {
+//             return a.function && (!b.function || a.target_time < b.target_time);
+//         });
+
+//         if (it != delayed_tasks_.end() && it->function) 
+//         {
+//             timer_hw->alarm[ALARM_NUM] = static_cast<uint32_t>(it->target_time);
+//         }
+        
+//         spin_unlock(spinlock_delayed_, irq_state);
+//     }
+
+//     bool queue_task(const std::function<void()>& function)
+//     {
+//         uint32_t irq_state = spin_lock_blocking(spinlock_);
+//         for (auto& task : tasks_)
+//         {
+//             if (!task.function)
+//             {
+//                 task.function = function;
+//                 spin_unlock(spinlock_, irq_state);
+//                 return true;
+//             }
+//         }
+//         spin_unlock(spinlock_, irq_state);
+//         return false;
+//     }
+
+//     void process_tasks()
+//     {
+//         uint32_t irq_state = spin_lock_blocking(spinlock_);
+//         for (auto& task : tasks_)
+//         {
+//             if (task.function)
+//             {
+//                 auto function = task.function;
+//                 task.function = nullptr;
+//                 spin_unlock(spinlock_, irq_state);
+
+//                 function();
+
+//                 irq_state = spin_lock_blocking(spinlock_);
+//             }
+//             else
+//             {
+//                 break; //No more tasks
+//             }
+//         }
+//         spin_unlock(spinlock_, irq_state);
+//     }
+
+// } // namespace Core1
+
+// #endif // OGXM_BOARD != PI_PICOW
+
+// namespace Core0 {
+
+// #if OGXM_BOARD == PI_PICOW
+//     static constexpr uint32_t ALARM_NUM = 1;
+// #else
+//     static constexpr uint32_t ALARM_NUM = 0;
+// #endif
+
+//     static std::array<Task, MAX_TASKS> tasks_;
+//     static std::array<DelayedTask, MAX_DELAYED_TASKS> delayed_tasks_;
+
+//     static bool delay_irq_set_ = false;
+//     static uint32_t new_task_id_ = 1;
+
+//     static spin_lock_t* spinlock_ = spin_lock_instance(3);
+//     static spin_lock_t* spinlock_delayed_ = spin_lock_instance(4);
+
+//     uint32_t get_new_task_id()
+//     {
+//         return new_task_id_++;
+//     }
+
+//     static 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 : delayed_tasks_) 
+//         {
+//             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_);
+//             }
+//         }
+
+//         auto it = std::min_element(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
+//         {
+//             return a.function && (!b.function || a.target_time < b.target_time);
+//         });
+
+//         if (it != delayed_tasks_.end() && it->function) 
+//         {
+//             timer_hw->alarm[ALARM_NUM] = static_cast<uint32_t>(it->target_time);
+//         }
+
+//         spin_unlock(spinlock_delayed_, irq_state);
+//     }
+
+//     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 : delayed_tasks_) 
+//         {
+//             if (task.task_id == task_id)
+//             {
+//                 spin_unlock(spinlock_delayed_, irq_state);
+//                 return false;
+//             }
+//         }
+//         spin_unlock(spinlock_delayed_, irq_state);
+
+//         hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
+        
+//         if (!delay_irq_set_)
+//         {
+//             delay_irq_set_ = true;
+//             irq_set_exclusive_handler(TIMER_IRQ(ALARM_NUM), timer_irq_handler);
+//             irq_set_enabled(TIMER_IRQ(ALARM_NUM), true);
+//         }
+
+//         irq_state = spin_lock_blocking(spinlock_delayed_);
+        
+//         uint64_t target_time = timer_hw->timerawl + static_cast<uint64_t>(delay_ms) * 1000;
+
+//         for (auto& task : delayed_tasks_) 
+//         {
+//             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(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
+//                 {
+//                     return a.function && (!b.function || a.target_time < b.target_time);
+//                 });
+
+//                 if (it != delayed_tasks_.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 : delayed_tasks_) 
+//         {
+//             if (task.task_id == task_id) 
+//             {
+//                 task.function = nullptr;
+//                 task.task_id = 0;
+//                 found = true;
+//             }
+//         }
+
+//         if (!found || !delay_irq_set_)
+//         {
+//             spin_unlock(spinlock_delayed_, irq_state);
+//             return;
+//         }
+
+//         hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
+
+//         auto it = std::min_element(delayed_tasks_.begin(), delayed_tasks_.end(), [](const DelayedTask& a, const DelayedTask& b) 
+//         {
+//             return a.function && (!b.function || a.target_time < b.target_time);
+//         });
+
+//         if (it != delayed_tasks_.end() && it->function) 
+//         {
+//             timer_hw->alarm[ALARM_NUM] = static_cast<uint32_t>(it->target_time);
+//         }
+        
+//         spin_unlock(spinlock_delayed_, irq_state);
+//     }
+
+//     bool queue_task(const std::function<void()>& function)
+//     {
+//         uint32_t irq_state = spin_lock_blocking(spinlock_);
+//         for (auto& task : tasks_)
+//         {
+//             if (!task.function)
+//             {
+//                 task.function = function;
+//                 spin_unlock(spinlock_, irq_state);
+//                 return true;
+//             }
+//         }
+//         spin_unlock(spinlock_, irq_state);
+//         return false;
+//     }
+
+//     void process_tasks()
+//     {
+//         uint32_t irq_state = spin_lock_blocking(spinlock_);
+//         for (auto& task : tasks_)
+//         {
+//             if (task.function)
+//             {
+//                 auto function = task.function;
+//                 task.function = nullptr;
+//                 spin_unlock(spinlock_, irq_state);
+
+//                 function();
+
+//                 irq_state = spin_lock_blocking(spinlock_);
+//             }
+//             else
+//             {
+//                 break; //No more tasks
+//             }
+//         }
+//         spin_unlock(spinlock_, irq_state);
+//     }
+
+// } // namespace Core0
+
+// } // namespace TaskQueue
\ No newline at end of file
diff --git a/Firmware/RP2040/src/TaskQueue/TaskQueue.h b/Firmware/RP2040/src/TaskQueue/TaskQueue.h
index 6d416a1..15b2b10 100644
--- a/Firmware/RP2040/src/TaskQueue/TaskQueue.h
+++ b/Firmware/RP2040/src/TaskQueue/TaskQueue.h
@@ -4,7 +4,10 @@
 #include <cstdint>
 #include <functional>
 
-/* Queue tasks to be executed with process_tasks() */
+#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 
 {
     namespace Core0 
@@ -16,6 +19,7 @@ namespace TaskQueue
         void process_tasks();
     }
 
+#if OGXM_BOARD != PI_PICOW
     namespace Core1 
     {
         uint32_t get_new_task_id();
@@ -24,6 +28,7 @@ namespace TaskQueue
         bool queue_task(const std::function<void()>& function);
         void process_tasks();
     }
+#endif // OGXM_BOARD != PI_PICOW
 
 } // namespace TaskQueue
 
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/DInput/DInput.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/DInput/DInput.cpp
index c650496..2665cdd 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/DInput/DInput.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/DInput/DInput.cpp
@@ -20,7 +20,7 @@ void DInputDevice::initialize()
 {
     for (auto& in_report : in_reports_)
     {
-        std::memset(&in_report, 0, sizeof(DInput::InReport));
+        std::memset(reinterpret_cast<void*>(&in_report), 0, sizeof(DInput::InReport));
         in_report.dpad = DInput::DPad::CENTER;
         in_report.joystick_lx = DInput::AXIS_MID;
         in_report.joystick_ly = DInput::AXIS_MID;
@@ -29,10 +29,9 @@ void DInputDevice::initialize()
     }
 
 	class_driver_ = {
-	#if CFG_TUSB_DEBUG >= 2
-		.name = "HID",
-	#endif
+		.name = TUD_DRV_NAME("HID"),
 		.init = hidd_init,
+        .deinit = hidd_deinit,
 		.reset = hidd_reset,
 		.open = hidd_open,
 		.control_xfer_cb = DInputDevice::control_xfer_cb,
@@ -43,84 +42,84 @@ void DInputDevice::initialize()
 
 void DInputDevice::process(const uint8_t idx, Gamepad& gamepad)
 {
-    if (!gamepad.new_pad_in())
-    {
-        return;
-    }
-    Gamepad::PadIn gp_in = gamepad.get_pad_in();
     DInput::InReport& in_report = in_reports_[idx];
 
-    switch (gp_in.dpad)
+    if (gamepad.new_pad_in())
     {
-        case Gamepad::DPAD_UP:
-            in_report.dpad = DInput::DPad::UP;
-            break;
-        case Gamepad::DPAD_DOWN:
-            in_report.dpad = DInput::DPad::DOWN;
-            break;
-        case Gamepad::DPAD_LEFT:
-            in_report.dpad = DInput::DPad::LEFT;
-            break;
-        case Gamepad::DPAD_RIGHT:
-            in_report.dpad = DInput::DPad::RIGHT;
-            break;
-        case Gamepad::DPAD_UP_LEFT:
-            in_report.dpad = DInput::DPad::UP_LEFT;
-            break;
-        case Gamepad::DPAD_UP_RIGHT:
-            in_report.dpad = DInput::DPad::UP_RIGHT;
-            break;
-        case Gamepad::DPAD_DOWN_LEFT:
-            in_report.dpad = DInput::DPad::DOWN_LEFT;
-            break;
-        case Gamepad::DPAD_DOWN_RIGHT:
-            in_report.dpad = DInput::DPad::DOWN_RIGHT;
-            break;
-        default:
-            in_report.dpad = DInput::DPad::CENTER;
-            break;
+        Gamepad::PadIn gp_in = gamepad.get_pad_in();
+
+        switch (gp_in.dpad)
+        {
+            case Gamepad::DPAD_UP:
+                in_report.dpad = DInput::DPad::UP;
+                break;
+            case Gamepad::DPAD_DOWN:
+                in_report.dpad = DInput::DPad::DOWN;
+                break;
+            case Gamepad::DPAD_LEFT:
+                in_report.dpad = DInput::DPad::LEFT;
+                break;
+            case Gamepad::DPAD_RIGHT:
+                in_report.dpad = DInput::DPad::RIGHT;
+                break;
+            case Gamepad::DPAD_UP_LEFT:
+                in_report.dpad = DInput::DPad::UP_LEFT;
+                break;
+            case Gamepad::DPAD_UP_RIGHT:
+                in_report.dpad = DInput::DPad::UP_RIGHT;
+                break;
+            case Gamepad::DPAD_DOWN_LEFT:
+                in_report.dpad = DInput::DPad::DOWN_LEFT;
+                break;
+            case Gamepad::DPAD_DOWN_RIGHT:
+                in_report.dpad = DInput::DPad::DOWN_RIGHT;
+                break;
+            default:
+                in_report.dpad = DInput::DPad::CENTER;
+                break;
+        }
+
+        std::memset(in_report.buttons, 0, sizeof(in_report.buttons));
+
+        if (gp_in.buttons & Gamepad::BUTTON_A)   in_report.buttons[0] |= DInput::Buttons0::CROSS;
+        if (gp_in.buttons & Gamepad::BUTTON_B)   in_report.buttons[0] |= DInput::Buttons0::CIRCLE;
+        if (gp_in.buttons & Gamepad::BUTTON_X)   in_report.buttons[0] |= DInput::Buttons0::SQUARE;
+        if (gp_in.buttons & Gamepad::BUTTON_Y)   in_report.buttons[0] |= DInput::Buttons0::TRIANGLE;
+        if (gp_in.buttons & Gamepad::BUTTON_LB)  in_report.buttons[0] |= DInput::Buttons0::L1;
+        if (gp_in.buttons & Gamepad::BUTTON_RB)  in_report.buttons[0] |= DInput::Buttons0::R1;
+
+        if (gp_in.buttons & Gamepad::BUTTON_L3)    in_report.buttons[1] |= DInput::Buttons1::L3;
+        if (gp_in.buttons & Gamepad::BUTTON_R3)    in_report.buttons[1] |= DInput::Buttons1::R3;
+        if (gp_in.buttons & Gamepad::BUTTON_BACK)  in_report.buttons[1] |= DInput::Buttons1::SELECT;
+        if (gp_in.buttons & Gamepad::BUTTON_START) in_report.buttons[1] |= DInput::Buttons1::START;
+        if (gp_in.buttons & Gamepad::BUTTON_SYS)   in_report.buttons[1] |= DInput::Buttons1::PS;
+        if (gp_in.buttons & Gamepad::BUTTON_MISC)  in_report.buttons[1] |= DInput::Buttons1::TP;
+
+        if (gamepad.analog_enabled())
+        {
+            in_report.up_axis    = gp_in.analog[Gamepad::ANALOG_OFF_UP];
+            in_report.down_axis  = gp_in.analog[Gamepad::ANALOG_OFF_DOWN];
+            in_report.right_axis = gp_in.analog[Gamepad::ANALOG_OFF_RIGHT];
+            in_report.left_axis  = gp_in.analog[Gamepad::ANALOG_OFF_LEFT];
+
+            in_report.triangle_axis = gp_in.analog[Gamepad::ANALOG_OFF_Y];
+            in_report.circle_axis   = gp_in.analog[Gamepad::ANALOG_OFF_X];
+            in_report.cross_axis    = gp_in.analog[Gamepad::ANALOG_OFF_B];
+            in_report.square_axis   = gp_in.analog[Gamepad::ANALOG_OFF_A];
+
+            in_report.r1_axis = gp_in.analog[Gamepad::ANALOG_OFF_RB];
+            in_report.l1_axis = gp_in.analog[Gamepad::ANALOG_OFF_LB];
+        }
+
+        in_report.joystick_lx = Scale::int16_to_uint8(gp_in.joystick_lx);
+        in_report.joystick_ly = Scale::int16_to_uint8(gp_in.joystick_ly);
+        in_report.joystick_rx = Scale::int16_to_uint8(gp_in.joystick_rx);
+        in_report.joystick_ry = Scale::int16_to_uint8(gp_in.joystick_ry);
+
+        in_report.l2_axis = gp_in.trigger_l;
+        in_report.r2_axis = gp_in.trigger_r;
     }
 
-    std::memset(in_report.buttons, 0, sizeof(in_report.buttons));
-
-    if (gp_in.buttons & Gamepad::BUTTON_A)   in_report.buttons[0] |= DInput::Buttons0::CROSS;
-    if (gp_in.buttons & Gamepad::BUTTON_B)   in_report.buttons[0] |= DInput::Buttons0::CIRCLE;
-    if (gp_in.buttons & Gamepad::BUTTON_X)   in_report.buttons[0] |= DInput::Buttons0::SQUARE;
-    if (gp_in.buttons & Gamepad::BUTTON_Y)   in_report.buttons[0] |= DInput::Buttons0::TRIANGLE;
-    if (gp_in.buttons & Gamepad::BUTTON_LB)  in_report.buttons[0] |= DInput::Buttons0::L1;
-    if (gp_in.buttons & Gamepad::BUTTON_RB)  in_report.buttons[0] |= DInput::Buttons0::R1;
-
-    if (gp_in.buttons & Gamepad::BUTTON_L3)    in_report.buttons[1] |= DInput::Buttons1::L3;
-    if (gp_in.buttons & Gamepad::BUTTON_R3)    in_report.buttons[1] |= DInput::Buttons1::R3;
-    if (gp_in.buttons & Gamepad::BUTTON_BACK)  in_report.buttons[1] |= DInput::Buttons1::SELECT;
-    if (gp_in.buttons & Gamepad::BUTTON_START) in_report.buttons[1] |= DInput::Buttons1::START;
-    if (gp_in.buttons & Gamepad::BUTTON_SYS)   in_report.buttons[1] |= DInput::Buttons1::PS;
-    if (gp_in.buttons & Gamepad::BUTTON_MISC)  in_report.buttons[1] |= DInput::Buttons1::TP;
-
-    if (gamepad.analog_enabled())
-    {
-        in_report.up_axis    = gp_in.analog[Gamepad::ANALOG_OFF_UP];
-        in_report.down_axis  = gp_in.analog[Gamepad::ANALOG_OFF_DOWN];
-        in_report.right_axis = gp_in.analog[Gamepad::ANALOG_OFF_RIGHT];
-        in_report.left_axis  = gp_in.analog[Gamepad::ANALOG_OFF_LEFT];
-
-        in_report.triangle_axis = gp_in.analog[Gamepad::ANALOG_OFF_Y];
-        in_report.circle_axis   = gp_in.analog[Gamepad::ANALOG_OFF_X];
-        in_report.cross_axis    = gp_in.analog[Gamepad::ANALOG_OFF_B];
-        in_report.square_axis   = gp_in.analog[Gamepad::ANALOG_OFF_A];
-
-        in_report.r1_axis = gp_in.analog[Gamepad::ANALOG_OFF_RB];
-        in_report.l1_axis = gp_in.analog[Gamepad::ANALOG_OFF_LB];
-    }
-
-    in_report.joystick_lx = Scale::int16_to_uint8(gp_in.joystick_lx);
-    in_report.joystick_ly = Scale::int16_to_uint8(gp_in.joystick_ly);
-    in_report.joystick_rx = Scale::int16_to_uint8(gp_in.joystick_rx);
-    in_report.joystick_ry = Scale::int16_to_uint8(gp_in.joystick_ry);
-
-    in_report.l2_axis = gp_in.trigger_l;
-    in_report.r2_axis = gp_in.trigger_r;
-
     if (tud_suspended())
     {
         tud_remote_wakeup();
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.cpp
index ab796a2..3312af3 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.cpp
@@ -1,3 +1,5 @@
+#include "class/cdc/cdc_device.h"
+#include "bsp/board_api.h"
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 
 uint16_t* DeviceDriver::get_string_descriptor(const char* value, uint8_t index)
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
index 493ff8a..654d66a 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
@@ -9,6 +9,12 @@
 
 #include "Gamepad.h"
 
+#if CFG_TUSB_DEBUG >= CFG_TUD_LOG_LEVEL
+    #define TUD_DRV_NAME(name) name
+#else
+    #define TUD_DRV_NAME(name) NULL
+#endif
+
 class DeviceDriver
 {
 public:
@@ -28,7 +34,7 @@ public:
     };
 
     virtual void initialize() = 0;
-    virtual void process(const uint8_t idx, Gamepad& gamepad)  = 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;
     virtual void set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t const* buffer, uint16_t buffer_size) = 0;
     virtual bool vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const* request) = 0;
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp
index ca2f57d..bbf6e4c 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp
@@ -1,4 +1,5 @@
 #include <cstring>
+#include <algorithm>
 
 #include "USBDevice/DeviceDriver/PS3/PS3.h"
 
@@ -6,9 +7,7 @@ void PS3Device::initialize()
 {
 	class_driver_ = 
     {
-	#if CFG_TUSB_DEBUG >= 2
-		.name = "PS3",
-	#endif
+		.name = TUD_DRV_NAME("PS3"),
 		.init = hidd_init,
         .deinit = hidd_deinit,
 		.reset = hidd_reset,
@@ -17,25 +16,6 @@ void PS3Device::initialize()
 		.xfer_cb = hidd_xfer_cb,
 		.sof = NULL
 	};
-
-    in_report_ = PS3::InReport();
-
-    // bt_info_ = 
-    // {
-    //     .reserved0 = {0xFF,0xFF},
-    //     .device_address = { 0x00, 0x20, 0x40, 0xCE, 0x00, 0x00, 0x00 },
-    //     .host_address = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
-    // };
-
-    // for (uint8_t addr = 0; addr < 3; addr++) 
-    // {
-    //     bt_info_.device_address[4 + addr] = static_cast<uint8_t>(get_rand_32() % 0xFF);
-    // }
-
-    // for (uint8_t addr = 0; addr < 6; addr++) 
-    // {
-    //     bt_info_.host_address[1 + addr] = static_cast<uint8_t>(get_rand_32() % 0xFF);
-    // }
 }
 
 void PS3Device::process(const uint8_t idx, Gamepad& gamepad) 
@@ -43,180 +23,117 @@ void PS3Device::process(const uint8_t idx, Gamepad& gamepad)
     if (gamepad.new_pad_in())
     {
         Gamepad::PadIn gp_in = gamepad.get_pad_in();
-        std::memset(in_report_.buttons, 0, sizeof(in_report_.buttons));
+        report_in_ = PS3::InReport();
 
         switch (gp_in.dpad)
         {
             case Gamepad::DPAD_UP:
-                in_report_.buttons[0] = PS3::Buttons0::DPAD_UP;
+                report_in_.buttons[0] = PS3::Buttons0::DPAD_UP;
                 break;
             case Gamepad::DPAD_DOWN:
-                in_report_.buttons[0] = PS3::Buttons0::DPAD_DOWN;
+                report_in_.buttons[0] = PS3::Buttons0::DPAD_DOWN;
                 break;
             case Gamepad::DPAD_LEFT:
-                in_report_.buttons[0] = PS3::Buttons0::DPAD_LEFT;
+                report_in_.buttons[0] = PS3::Buttons0::DPAD_LEFT;
                 break;
             case Gamepad::DPAD_RIGHT:
-                in_report_.buttons[0] = PS3::Buttons0::DPAD_RIGHT;
+                report_in_.buttons[0] = PS3::Buttons0::DPAD_RIGHT;
                 break;
             case Gamepad::DPAD_UP_LEFT:
-                in_report_.buttons[0] = PS3::Buttons0::DPAD_UP | PS3::Buttons0::DPAD_LEFT;
+                report_in_.buttons[0] = PS3::Buttons0::DPAD_UP | PS3::Buttons0::DPAD_LEFT;
                 break;
             case Gamepad::DPAD_UP_RIGHT:
-                in_report_.buttons[0] = PS3::Buttons0::DPAD_UP | PS3::Buttons0::DPAD_RIGHT;
+                report_in_.buttons[0] = PS3::Buttons0::DPAD_UP | PS3::Buttons0::DPAD_RIGHT;
                 break;
             case Gamepad::DPAD_DOWN_LEFT:
-                in_report_.buttons[0] = PS3::Buttons0::DPAD_DOWN | PS3::Buttons0::DPAD_LEFT;
+                report_in_.buttons[0] = PS3::Buttons0::DPAD_DOWN | PS3::Buttons0::DPAD_LEFT;
                 break;
             case Gamepad::DPAD_DOWN_RIGHT:
-                in_report_.buttons[0] = PS3::Buttons0::DPAD_DOWN | PS3::Buttons0::DPAD_RIGHT;
+                report_in_.buttons[0] = PS3::Buttons0::DPAD_DOWN | PS3::Buttons0::DPAD_RIGHT;
                 break;
             default:
                 break;
         }
 
-        if (gp_in.buttons & Gamepad::BUTTON_X)        in_report_.buttons[1] |= PS3::Buttons1::SQUARE;
-        if (gp_in.buttons & Gamepad::BUTTON_A)        in_report_.buttons[1] |= PS3::Buttons1::CROSS;
-        if (gp_in.buttons & Gamepad::BUTTON_Y)        in_report_.buttons[1] |= PS3::Buttons1::TRIANGLE;
-        if (gp_in.buttons & Gamepad::BUTTON_B)        in_report_.buttons[1] |= PS3::Buttons1::CIRCLE;
-        if (gp_in.buttons & Gamepad::BUTTON_LB)       in_report_.buttons[1] |= PS3::Buttons1::L1;
-        if (gp_in.buttons & Gamepad::BUTTON_RB)       in_report_.buttons[1] |= PS3::Buttons1::R1;
-        if (gp_in.buttons & Gamepad::BUTTON_BACK)     in_report_.buttons[0] |= PS3::Buttons0::SELECT;
-        if (gp_in.buttons & Gamepad::BUTTON_START)    in_report_.buttons[0] |= PS3::Buttons0::START;
-        if (gp_in.buttons & Gamepad::BUTTON_L3)       in_report_.buttons[0] |= PS3::Buttons0::L3;
-        if (gp_in.buttons & Gamepad::BUTTON_R3)       in_report_.buttons[0] |= PS3::Buttons0::R3;
-        if (gp_in.buttons & Gamepad::BUTTON_SYS)      in_report_.buttons[2] |= PS3::Buttons2::PS;
-        if (gp_in.buttons & Gamepad::BUTTON_MISC)     in_report_.buttons[2] |= PS3::Buttons2::TP;
+        if (gp_in.buttons & Gamepad::BUTTON_X)        report_in_.buttons[1] |= PS3::Buttons1::SQUARE;
+        if (gp_in.buttons & Gamepad::BUTTON_A)        report_in_.buttons[1] |= PS3::Buttons1::CROSS;
+        if (gp_in.buttons & Gamepad::BUTTON_Y)        report_in_.buttons[1] |= PS3::Buttons1::TRIANGLE;
+        if (gp_in.buttons & Gamepad::BUTTON_B)        report_in_.buttons[1] |= PS3::Buttons1::CIRCLE;
+        if (gp_in.buttons & Gamepad::BUTTON_LB)       report_in_.buttons[1] |= PS3::Buttons1::L1;
+        if (gp_in.buttons & Gamepad::BUTTON_RB)       report_in_.buttons[1] |= PS3::Buttons1::R1;
+        if (gp_in.buttons & Gamepad::BUTTON_BACK)     report_in_.buttons[0] |= PS3::Buttons0::SELECT;
+        if (gp_in.buttons & Gamepad::BUTTON_START)    report_in_.buttons[0] |= PS3::Buttons0::START;
+        if (gp_in.buttons & Gamepad::BUTTON_L3)       report_in_.buttons[0] |= PS3::Buttons0::L3;
+        if (gp_in.buttons & Gamepad::BUTTON_R3)       report_in_.buttons[0] |= PS3::Buttons0::R3;
+        if (gp_in.buttons & Gamepad::BUTTON_SYS)      report_in_.buttons[2] |= PS3::Buttons2::PS;
+        if (gp_in.buttons & Gamepad::BUTTON_MISC)     report_in_.buttons[2] |= PS3::Buttons2::TP;
 
-        if (gp_in.trigger_l) in_report_.buttons[1] |= PS3::Buttons1::L2;
-        if (gp_in.trigger_r) in_report_.buttons[1] |= PS3::Buttons1::R2;
+        if (gp_in.trigger_l) report_in_.buttons[1] |= PS3::Buttons1::L2;
+        if (gp_in.trigger_r) report_in_.buttons[1] |= PS3::Buttons1::R2;
 
-        in_report_.joystick_lx = Scale::int16_to_uint8(gp_in.joystick_lx);
-        in_report_.joystick_ly = Scale::int16_to_uint8(gp_in.joystick_ly);
-        in_report_.joystick_rx = Scale::int16_to_uint8(gp_in.joystick_rx);
-        in_report_.joystick_ry = Scale::int16_to_uint8(gp_in.joystick_ry);
+        report_in_.joystick_lx = Scale::int16_to_uint8(gp_in.joystick_lx);
+        report_in_.joystick_ly = Scale::int16_to_uint8(gp_in.joystick_ly);
+        report_in_.joystick_rx = Scale::int16_to_uint8(gp_in.joystick_rx);
+        report_in_.joystick_ry = Scale::int16_to_uint8(gp_in.joystick_ry);
 
         if (gamepad.analog_enabled())
         {
-            in_report_.up_axis      = gp_in.analog[Gamepad::ANALOG_OFF_UP];
-            in_report_.down_axis    = gp_in.analog[Gamepad::ANALOG_OFF_DOWN];
-            in_report_.right_axis   = gp_in.analog[Gamepad::ANALOG_OFF_RIGHT];
-            in_report_.left_axis    = gp_in.analog[Gamepad::ANALOG_OFF_LEFT];
+            report_in_.up_axis      = gp_in.analog[Gamepad::ANALOG_OFF_UP];
+            report_in_.down_axis    = gp_in.analog[Gamepad::ANALOG_OFF_DOWN];
+            report_in_.right_axis   = gp_in.analog[Gamepad::ANALOG_OFF_RIGHT];
+            report_in_.left_axis    = gp_in.analog[Gamepad::ANALOG_OFF_LEFT];
 
-            in_report_.triangle_axis = gp_in.analog[Gamepad::ANALOG_OFF_Y];
-            in_report_.circle_axis   = gp_in.analog[Gamepad::ANALOG_OFF_B];
-            in_report_.cross_axis    = gp_in.analog[Gamepad::ANALOG_OFF_A];
-            in_report_.square_axis   = gp_in.analog[Gamepad::ANALOG_OFF_X];
+            report_in_.triangle_axis = gp_in.analog[Gamepad::ANALOG_OFF_Y];
+            report_in_.circle_axis   = gp_in.analog[Gamepad::ANALOG_OFF_B];
+            report_in_.cross_axis    = gp_in.analog[Gamepad::ANALOG_OFF_A];
+            report_in_.square_axis   = gp_in.analog[Gamepad::ANALOG_OFF_X];
 
-            in_report_.r1_axis = gp_in.analog[Gamepad::ANALOG_OFF_RB];
-            in_report_.l1_axis = gp_in.analog[Gamepad::ANALOG_OFF_LB];
+            report_in_.r1_axis = gp_in.analog[Gamepad::ANALOG_OFF_RB];
+            report_in_.l1_axis = gp_in.analog[Gamepad::ANALOG_OFF_LB];
         }
         else
         {
-            in_report_.up_axis       = (gp_in.dpad & Gamepad::DPAD_UP)    ? 0xFF : 0;
-            in_report_.down_axis     = (gp_in.dpad & Gamepad::DPAD_DOWN)  ? 0xFF : 0;
-            in_report_.right_axis    = (gp_in.dpad & Gamepad::DPAD_RIGHT) ? 0xFF : 0;
-            in_report_.left_axis     = (gp_in.dpad & Gamepad::DPAD_LEFT)  ? 0xFF : 0;
+            report_in_.up_axis       = (gp_in.dpad & Gamepad::DPAD_UP)    ? 0xFF : 0;
+            report_in_.down_axis     = (gp_in.dpad & Gamepad::DPAD_DOWN)  ? 0xFF : 0;
+            report_in_.right_axis    = (gp_in.dpad & Gamepad::DPAD_RIGHT) ? 0xFF : 0;
+            report_in_.left_axis     = (gp_in.dpad & Gamepad::DPAD_LEFT)  ? 0xFF : 0;
 
-            in_report_.triangle_axis = (gp_in.buttons & Gamepad::BUTTON_Y) ? 0xFF : 0;
-            in_report_.circle_axis   = (gp_in.buttons & Gamepad::BUTTON_X) ? 0xFF : 0;
-            in_report_.cross_axis    = (gp_in.buttons & Gamepad::BUTTON_B) ? 0xFF : 0;
-            in_report_.square_axis   = (gp_in.buttons & Gamepad::BUTTON_A) ? 0xFF : 0;
+            report_in_.triangle_axis = (gp_in.buttons & Gamepad::BUTTON_Y) ? 0xFF : 0;
+            report_in_.circle_axis   = (gp_in.buttons & Gamepad::BUTTON_X) ? 0xFF : 0;
+            report_in_.cross_axis    = (gp_in.buttons & Gamepad::BUTTON_B) ? 0xFF : 0;
+            report_in_.square_axis   = (gp_in.buttons & Gamepad::BUTTON_A) ? 0xFF : 0;
 
-            in_report_.r1_axis = (gp_in.buttons & Gamepad::BUTTON_RB) ? 0xFF : 0;
-            in_report_.l1_axis = (gp_in.buttons & Gamepad::BUTTON_LB) ? 0xFF : 0;
-        }
-
-        if (tud_suspended())
-        {
-            tud_remote_wakeup();
-        }
-
-        if (tud_hid_ready())
-        {
-            tud_hid_report(0, reinterpret_cast<uint8_t*>(&in_report_), sizeof(PS3::InReport));
+            report_in_.r1_axis = (gp_in.buttons & Gamepad::BUTTON_RB) ? 0xFF : 0;
+            report_in_.l1_axis = (gp_in.buttons & Gamepad::BUTTON_LB) ? 0xFF : 0;
         }
     }
 
-    if (new_out_report_)
+    if (tud_suspended())
+    {
+        tud_remote_wakeup();
+    }
+
+    if (tud_hid_ready())
+    {
+        //PS3 seems to start using stale data if a report isn't sent every frame
+        tud_hid_report(0, reinterpret_cast<uint8_t*>(&report_in_), sizeof(PS3::InReport));
+    }
+
+    if (new_report_out_)
     {
         Gamepad::PadOut gp_out;
-        gp_out.rumble_l = out_report_.rumble.left_motor_force;
-        gp_out.rumble_r = out_report_.rumble.right_motor_on ? 0xFF : 0;
+        gp_out.rumble_l = report_out_.rumble.left_motor_force;
+        gp_out.rumble_r = report_out_.rumble.right_motor_on ? UINT_8::MAX : 0;
         gamepad.set_pad_out(gp_out);
-        new_out_report_ = false;
+        new_report_out_ = false;
     }
 }
 
-static constexpr uint8_t output_ps3_0x01[] = 
-{
-    0x01, 0x04, 0x00, 0x0b, 0x0c, 0x01, 0x02, 0x18, 
-    0x18, 0x18, 0x18, 0x09, 0x0a, 0x10, 0x11, 0x12,
-    0x13, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02,
-    0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x04, 0x04,
-    0x04, 0x04, 0x00, 0x00, 0x04, 0x00, 0x01, 0x02,
-    0x07, 0x00, 0x17, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
-};
-
-// calibration data
-static constexpr uint8_t output_ps3_0xef[] = 
-{
-    0xef, 0x04, 0x00, 0x0b, 0x03, 0x01, 0xa0, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x01, 0xff, 0x01, 0xff, 0x01, 0xff, 0x01, 0xff,
-    0x01, 0xff, 0x01, 0xff, 0x01, 0xff, 0x01, 0xff,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06,
-};
-
-// unknown
-static constexpr uint8_t output_ps3_0xf5[] = 
-{
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // host address - must match 0xf2
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
-};
-
-// unknown
-static constexpr uint8_t output_ps3_0xf7[] = 
-{
-    0x02, 0x01, 0xf8, 0x02, 0xe2, 0x01, 0x05, 0xff,
-    0x04, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
-};
-
-// unknown
-static constexpr uint8_t output_ps3_0xf8[] = 
-{
-    0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
-};
-
-/* Based on: https://github.com/OpenStickCommunity/GP2040-CE/blob/main/src/drivers/ps3/PS3Driver.cpp */
-
 uint16_t PS3Device::get_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t *buffer, uint16_t reqlen) 
 {
     if (report_type == HID_REPORT_TYPE_INPUT) 
     {
-        std::memcpy(buffer, &in_report_, sizeof(PS3::InReport));
+        std::memcpy(buffer, &report_in_, sizeof(PS3::InReport));
         return sizeof(PS3::InReport);
     } 
     else if (report_type == HID_REPORT_TYPE_FEATURE) 
@@ -228,43 +145,37 @@ uint16_t PS3Device::get_report_cb(uint8_t itf, uint8_t report_id, hid_report_typ
         {
             case PS3::ReportID::FEATURE_01:
                 resp_len = reqlen;
-                std::memcpy(buffer, output_ps3_0x01, resp_len);
+                std::memcpy(buffer, PS3::OUTPUT_0x01, resp_len);
                 return resp_len;
-
             case PS3::ReportID::FEATURE_EF:
                 resp_len = reqlen;
-                std::memcpy(buffer, output_ps3_0xef, resp_len);
+                std::memcpy(buffer, PS3::OUTPUT_0xEF, resp_len);
                 buffer[6] = ef_byte_;
                 return resp_len;
-
             case PS3::ReportID::GET_PAIRING_INFO:
                 resp_len = reqlen;
                 std::memcpy(buffer, &bt_info_, resp_len);
                 return resp_len;
-
             case PS3::ReportID::FEATURE_F5:
                 resp_len = reqlen;
-                std::memcpy(buffer, output_ps3_0xf5, resp_len);
-
+                std::memcpy(buffer, PS3::OUTPUT_0xF5, resp_len);
                 for (ctr = 0; ctr < 6; ctr++) 
                 {
-                    buffer[1 + ctr] = bt_info_.host_address[ctr];
+                    buffer[1+ctr] = bt_info_.host_address[ctr];
                 }
                 return resp_len;
-
             case PS3::ReportID::FEATURE_F7:
                 resp_len = reqlen;
-                std::memcpy(buffer, output_ps3_0xf7, resp_len);
+                std::memcpy(buffer, PS3::OUTPUT_0xF7, resp_len);
                 return resp_len;
-
             case PS3::ReportID::FEATURE_F8:
                 resp_len = reqlen;
-                std::memcpy(buffer, output_ps3_0xf8, resp_len);
+                std::memcpy(buffer, PS3::OUTPUT_0xF8, resp_len);
                 buffer[6] = ef_byte_;
                 return resp_len;
         }
     }
-    return -1;
+    return 0;
 }
 
 void PS3Device::set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t const *buffer, uint16_t bufsize) 
@@ -278,7 +189,7 @@ void PS3Device::set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t
                 break;
         }
     } 
-    else if (report_type == HID_REPORT_TYPE_OUTPUT ) 
+    else if (report_type == HID_REPORT_TYPE_OUTPUT) 
     {
         // DS3 command
         uint8_t const *buf = buffer;
@@ -288,12 +199,11 @@ void PS3Device::set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t
             bufsize = bufsize - 1;
             buf = &buffer[1];
         }
-
         switch(report_id) 
         {
             case PS3::ReportID::FEATURE_01:
-                std::memcpy(&out_report_, buf, std::min(bufsize, static_cast<uint16_t>(sizeof(PS3::OutReport))));
-                new_out_report_ = true;
+                new_report_out_ = true;
+                std::memcpy(&report_out_, buf, std::min(bufsize, static_cast<uint16_t>(sizeof(PS3::OutReport))));
                 break;
         }
     }
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.h
index 95d2bc1..97f511a 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.h
@@ -22,11 +22,11 @@ public:
     const uint8_t* get_descriptor_device_qualifier_cb() override;
 
 private:
-    PS3::InReport in_report_;
-    PS3::OutReport out_report_;
+    PS3::InReport report_in_;
+    PS3::OutReport report_out_;
     PS3::BTInfo bt_info_;
     uint8_t ef_byte_;
-    bool new_out_report_{false};
+    bool new_report_out_{false};
 };
 
 #endif // _PS3_DEVICE_H_
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp
index 2e4a84e..db5fdd5 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp
@@ -6,10 +6,9 @@ void PSClassicDevice::initialize()
 {
 	class_driver_ = 
     {
-	#if CFG_TUSB_DEBUG >= 2
-		.name = "PSClassic",
-	#endif
+		.name = TUD_DRV_NAME("PSClassic"),
 		.init = hidd_init,
+        .deinit = hidd_deinit,
 		.reset = hidd_reset,
 		.open = hidd_open,
 		.control_xfer_cb = hidd_control_xfer_cb,
@@ -20,98 +19,96 @@ void PSClassicDevice::initialize()
 
 void PSClassicDevice::process(const uint8_t idx, Gamepad& gamepad)
 {
-    if (!gamepad.new_pad_in())
+    if (gamepad.new_pad_in())
     {
-        return;
-    }
+        Gamepad::PadIn gp_in = gamepad.get_pad_in();
+        switch (gp_in.dpad)
+        {
+            case Gamepad::DPAD_UP:
+                in_report_.buttons = PSClassic::Buttons::UP;
+                break;
+            case Gamepad::DPAD_DOWN:
+                in_report_.buttons = PSClassic::Buttons::DOWN;
+                break;
+            case Gamepad::DPAD_LEFT:
+                in_report_.buttons = PSClassic::Buttons::LEFT;
+                break;
+            case Gamepad::DPAD_RIGHT:
+                in_report_.buttons = PSClassic::Buttons::RIGHT;
+                break;
+            case Gamepad::DPAD_UP_LEFT:
+                in_report_.buttons = PSClassic::Buttons::UP_LEFT;
+                break;
+            case Gamepad::DPAD_UP_RIGHT:
+                in_report_.buttons = PSClassic::Buttons::UP_RIGHT;
+                break;
+            case Gamepad::DPAD_DOWN_LEFT:
+                in_report_.buttons = PSClassic::Buttons::DOWN_LEFT;
+                break;
+            case Gamepad::DPAD_DOWN_RIGHT:
+                in_report_.buttons = PSClassic::Buttons::DOWN_RIGHT;
+                break;
+            default:
+                in_report_.buttons = PSClassic::Buttons::CENTER;
+                break;
+        }
 
-    Gamepad::PadIn gp_in = gamepad.get_pad_in();
-    switch (gp_in.dpad)
-    {
-        case Gamepad::DPAD_UP:
-            in_report_.buttons = PSClassic::Buttons::UP;
-            break;
-        case Gamepad::DPAD_DOWN:
+        int16_t joy_lx = gp_in.joystick_lx;
+        int16_t joy_ly = Scale::invert_joy(gp_in.joystick_ly);
+        int16_t joy_rx = gp_in.joystick_rx;
+        int16_t joy_ry = Scale::invert_joy(gp_in.joystick_ry);
+
+        if (meets_pos_threshold(joy_lx, joy_rx))
+        {
+            if (meets_neg_45_threshold(joy_ly, joy_ry))
+            {
+                in_report_.buttons = PSClassic::Buttons::DOWN_RIGHT;
+            }
+            else if (meets_pos_45_threshold(joy_ly, joy_ry))
+            {
+                in_report_.buttons = PSClassic::Buttons::UP_RIGHT;
+            }
+            else
+            {
+                in_report_.buttons = PSClassic::Buttons::RIGHT;
+            }
+        }
+        else if (meets_neg_threshold(joy_lx, joy_rx))
+        {
+            if (meets_neg_45_threshold(joy_ly, joy_ry))
+            {
+                in_report_.buttons = PSClassic::Buttons::DOWN_LEFT;
+            }
+            else if (meets_pos_45_threshold(joy_ly, joy_ry))
+            {
+                in_report_.buttons = PSClassic::Buttons::UP_LEFT;
+            }
+            else
+            {
+                in_report_.buttons = PSClassic::Buttons::LEFT;
+            }
+        }
+        else if (meets_neg_threshold(joy_ly, joy_ry))
+        {
             in_report_.buttons = PSClassic::Buttons::DOWN;
-            break;
-        case Gamepad::DPAD_LEFT:
-            in_report_.buttons = PSClassic::Buttons::LEFT;
-            break;
-        case Gamepad::DPAD_RIGHT:
-            in_report_.buttons = PSClassic::Buttons::RIGHT;
-            break;
-        case Gamepad::DPAD_UP_LEFT:
-            in_report_.buttons = PSClassic::Buttons::UP_LEFT;
-            break;
-        case Gamepad::DPAD_UP_RIGHT:
-            in_report_.buttons = PSClassic::Buttons::UP_RIGHT;
-            break;
-        case Gamepad::DPAD_DOWN_LEFT:
-            in_report_.buttons = PSClassic::Buttons::DOWN_LEFT;
-            break;
-        case Gamepad::DPAD_DOWN_RIGHT:
-            in_report_.buttons = PSClassic::Buttons::DOWN_RIGHT;
-            break;
-        default:
-            in_report_.buttons = PSClassic::Buttons::CENTER;
-            break;
-    }
+        }
+        else if (meets_pos_threshold(joy_ly, joy_ry))
+        {
+            in_report_.buttons = PSClassic::Buttons::UP;
+        }
 
-    int16_t joy_lx = gp_in.joystick_lx;
-    int16_t joy_ly = Scale::invert_joy(gp_in.joystick_ly);
-    int16_t joy_rx = gp_in.joystick_rx;
-    int16_t joy_ry = Scale::invert_joy(gp_in.joystick_ry);
-
-    if (meets_pos_threshold(joy_lx, joy_rx))
-    {
-        if (meets_neg_45_threshold(joy_ly, joy_ry))
-        {
-            in_report_.buttons = PSClassic::Buttons::DOWN_RIGHT;
-        }
-        else if (meets_pos_45_threshold(joy_ly, joy_ry))
-        {
-            in_report_.buttons = PSClassic::Buttons::UP_RIGHT;
-        }
-        else
-        {
-            in_report_.buttons = PSClassic::Buttons::RIGHT;
-        }
+        if (gp_in.buttons & Gamepad::BUTTON_A) in_report_.buttons |= PSClassic::Buttons::CROSS;
+        if (gp_in.buttons & Gamepad::BUTTON_B) in_report_.buttons |= PSClassic::Buttons::CIRCLE;
+        if (gp_in.buttons & Gamepad::BUTTON_X) in_report_.buttons |= PSClassic::Buttons::SQUARE;
+        if (gp_in.buttons & Gamepad::BUTTON_Y) in_report_.buttons |= PSClassic::Buttons::TRIANGLE;
+        if (gp_in.buttons & Gamepad::BUTTON_LB)    in_report_.buttons |= PSClassic::Buttons::L1;
+        if (gp_in.buttons & Gamepad::BUTTON_RB)    in_report_.buttons |= PSClassic::Buttons::R1;
+        if (gp_in.buttons & Gamepad::BUTTON_BACK)  in_report_.buttons |= PSClassic::Buttons::SELECT;
+        if (gp_in.buttons & Gamepad::BUTTON_START) in_report_.buttons |= PSClassic::Buttons::START;
+        
+        if (gp_in.trigger_l) in_report_.buttons |= PSClassic::Buttons::L2;
+        if (gp_in.trigger_r) in_report_.buttons |= PSClassic::Buttons::R2;
     }
-    else if (meets_neg_threshold(joy_lx, joy_rx))
-    {
-        if (meets_neg_45_threshold(joy_ly, joy_ry))
-        {
-            in_report_.buttons = PSClassic::Buttons::DOWN_LEFT;
-        }
-        else if (meets_pos_45_threshold(joy_ly, joy_ry))
-        {
-            in_report_.buttons = PSClassic::Buttons::UP_LEFT;
-        }
-        else
-        {
-            in_report_.buttons = PSClassic::Buttons::LEFT;
-        }
-    }
-    else if (meets_neg_threshold(joy_ly, joy_ry))
-    {
-        in_report_.buttons = PSClassic::Buttons::DOWN;
-    }
-    else if (meets_pos_threshold(joy_ly, joy_ry))
-    {
-        in_report_.buttons = PSClassic::Buttons::UP;
-    }
-
-    if (gp_in.buttons & Gamepad::BUTTON_A) in_report_.buttons |= PSClassic::Buttons::CROSS;
-    if (gp_in.buttons & Gamepad::BUTTON_B) in_report_.buttons |= PSClassic::Buttons::CIRCLE;
-    if (gp_in.buttons & Gamepad::BUTTON_X) in_report_.buttons |= PSClassic::Buttons::SQUARE;
-    if (gp_in.buttons & Gamepad::BUTTON_Y) in_report_.buttons |= PSClassic::Buttons::TRIANGLE;
-    if (gp_in.buttons & Gamepad::BUTTON_LB)    in_report_.buttons |= PSClassic::Buttons::L1;
-    if (gp_in.buttons & Gamepad::BUTTON_RB)    in_report_.buttons |= PSClassic::Buttons::R1;
-    if (gp_in.buttons & Gamepad::BUTTON_BACK)  in_report_.buttons |= PSClassic::Buttons::SELECT;
-    if (gp_in.buttons & Gamepad::BUTTON_START) in_report_.buttons |= PSClassic::Buttons::START;
-    
-    if (gp_in.trigger_l) in_report_.buttons |= PSClassic::Buttons::L2;
-    if (gp_in.trigger_r) in_report_.buttons |= PSClassic::Buttons::R2;
 
     if (tud_suspended())
     {
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/Switch/Switch.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/Switch/Switch.cpp
index f0c9cfe..df1dddd 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/Switch/Switch.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/Switch/Switch.cpp
@@ -6,10 +6,9 @@ void SwitchDevice::initialize()
 {
 	class_driver_ = 
     {
-	#if CFG_TUSB_DEBUG >= 2
-		.name = "SWITCH",
-	#endif
+		.name = TUD_DRV_NAME("SWITCH"),
 		.init = hidd_init,
+        .deinit = hidd_deinit,
 		.reset = hidd_reset,
 		.open = hidd_open,
 		.control_xfer_cb = hidd_control_xfer_cb,
@@ -22,68 +21,67 @@ void SwitchDevice::initialize()
 
 void SwitchDevice::process(const uint8_t idx, Gamepad& gamepad) 
 {
-    if (!gamepad.new_pad_in())
-    {
-        return;
-    }
-
-    Gamepad::PadIn gp_in = gamepad.get_pad_in();
     SwitchWired::InReport& in_report = in_report_[idx];
 
-    switch (gp_in.dpad)
+    if (gamepad.new_pad_in())
     {
-        case Gamepad::DPAD_UP:
-            in_report.dpad = SwitchWired::DPad::UP;
-            break;
-        case Gamepad::DPAD_DOWN:
-            in_report.dpad = SwitchWired::DPad::DOWN;
-            break;
-        case Gamepad::DPAD_LEFT:
-            in_report.dpad = SwitchWired::DPad::LEFT;
-            break;
-        case Gamepad::DPAD_RIGHT:
-            in_report.dpad = SwitchWired::DPad::RIGHT;
-            break;
-        case Gamepad::DPAD_UP_LEFT:
-            in_report.dpad = SwitchWired::DPad::UP_LEFT;
-            break;
-        case Gamepad::DPAD_UP_RIGHT:
-            in_report.dpad = SwitchWired::DPad::UP_RIGHT;
-            break;
-        case Gamepad::DPAD_DOWN_LEFT:
-            in_report.dpad = SwitchWired::DPad::DOWN_LEFT;
-            break;
-        case Gamepad::DPAD_DOWN_RIGHT:
-            in_report.dpad = SwitchWired::DPad::DOWN_RIGHT;
-            break;
-        default:
-            in_report.dpad = SwitchWired::DPad::CENTER;
-            break;
+        Gamepad::PadIn gp_in = gamepad.get_pad_in();
+    
+        switch (gp_in.dpad)
+        {
+            case Gamepad::DPAD_UP:
+                in_report.dpad = SwitchWired::DPad::UP;
+                break;
+            case Gamepad::DPAD_DOWN:
+                in_report.dpad = SwitchWired::DPad::DOWN;
+                break;
+            case Gamepad::DPAD_LEFT:
+                in_report.dpad = SwitchWired::DPad::LEFT;
+                break;
+            case Gamepad::DPAD_RIGHT:
+                in_report.dpad = SwitchWired::DPad::RIGHT;
+                break;
+            case Gamepad::DPAD_UP_LEFT:
+                in_report.dpad = SwitchWired::DPad::UP_LEFT;
+                break;
+            case Gamepad::DPAD_UP_RIGHT:
+                in_report.dpad = SwitchWired::DPad::UP_RIGHT;
+                break;
+            case Gamepad::DPAD_DOWN_LEFT:
+                in_report.dpad = SwitchWired::DPad::DOWN_LEFT;
+                break;
+            case Gamepad::DPAD_DOWN_RIGHT:
+                in_report.dpad = SwitchWired::DPad::DOWN_RIGHT;
+                break;
+            default:
+                in_report.dpad = SwitchWired::DPad::CENTER;
+                break;
+        }
+
+        in_report.buttons = 0;
+
+        if (gp_in.buttons & Gamepad::BUTTON_X)        in_report.buttons |= SwitchWired::Buttons::Y;
+        if (gp_in.buttons & Gamepad::BUTTON_A)        in_report.buttons |= SwitchWired::Buttons::B;
+        if (gp_in.buttons & Gamepad::BUTTON_Y)        in_report.buttons |= SwitchWired::Buttons::X;
+        if (gp_in.buttons & Gamepad::BUTTON_B)        in_report.buttons |= SwitchWired::Buttons::A;
+        if (gp_in.buttons & Gamepad::BUTTON_LB)       in_report.buttons |= SwitchWired::Buttons::L;
+        if (gp_in.buttons & Gamepad::BUTTON_RB)       in_report.buttons |= SwitchWired::Buttons::R;
+        if (gp_in.buttons & Gamepad::BUTTON_BACK)     in_report.buttons |= SwitchWired::Buttons::MINUS;
+        if (gp_in.buttons & Gamepad::BUTTON_START)    in_report.buttons |= SwitchWired::Buttons::PLUS;
+        if (gp_in.buttons & Gamepad::BUTTON_L3)       in_report.buttons |= SwitchWired::Buttons::L3;
+        if (gp_in.buttons & Gamepad::BUTTON_R3)       in_report.buttons |= SwitchWired::Buttons::R3;
+        if (gp_in.buttons & Gamepad::BUTTON_SYS)      in_report.buttons |= SwitchWired::Buttons::HOME;
+        if (gp_in.buttons & Gamepad::BUTTON_MISC)     in_report.buttons |= SwitchWired::Buttons::CAPTURE;
+
+        if (gp_in.trigger_l) in_report.buttons |= SwitchWired::Buttons::ZL;
+        if (gp_in.trigger_r) in_report.buttons |= SwitchWired::Buttons::ZR;
+
+        in_report.joystick_lx = Scale::int16_to_uint8(gp_in.joystick_lx);
+        in_report.joystick_ly = Scale::int16_to_uint8(gp_in.joystick_ly);
+        in_report.joystick_rx = Scale::int16_to_uint8(gp_in.joystick_rx);
+        in_report.joystick_ry = Scale::int16_to_uint8(gp_in.joystick_ry);
     }
 
-    in_report.buttons = 0;
-
-    if (gp_in.buttons & Gamepad::BUTTON_X)        in_report.buttons |= SwitchWired::Buttons::Y;
-    if (gp_in.buttons & Gamepad::BUTTON_A)        in_report.buttons |= SwitchWired::Buttons::B;
-    if (gp_in.buttons & Gamepad::BUTTON_Y)        in_report.buttons |= SwitchWired::Buttons::X;
-    if (gp_in.buttons & Gamepad::BUTTON_B)        in_report.buttons |= SwitchWired::Buttons::A;
-    if (gp_in.buttons & Gamepad::BUTTON_LB)       in_report.buttons |= SwitchWired::Buttons::L;
-    if (gp_in.buttons & Gamepad::BUTTON_RB)       in_report.buttons |= SwitchWired::Buttons::R;
-    if (gp_in.buttons & Gamepad::BUTTON_BACK)     in_report.buttons |= SwitchWired::Buttons::MINUS;
-    if (gp_in.buttons & Gamepad::BUTTON_START)    in_report.buttons |= SwitchWired::Buttons::PLUS;
-    if (gp_in.buttons & Gamepad::BUTTON_L3)       in_report.buttons |= SwitchWired::Buttons::L3;
-    if (gp_in.buttons & Gamepad::BUTTON_R3)       in_report.buttons |= SwitchWired::Buttons::R3;
-    if (gp_in.buttons & Gamepad::BUTTON_SYS)      in_report.buttons |= SwitchWired::Buttons::HOME;
-    if (gp_in.buttons & Gamepad::BUTTON_MISC)     in_report.buttons |= SwitchWired::Buttons::CAPTURE;
-
-    if (gp_in.trigger_l) in_report.buttons |= SwitchWired::Buttons::ZL;
-    if (gp_in.trigger_r) in_report.buttons |= SwitchWired::Buttons::ZR;
-
-    in_report.joystick_lx = Scale::int16_to_uint8(gp_in.joystick_lx);
-    in_report.joystick_ly = Scale::int16_to_uint8(gp_in.joystick_ly);
-    in_report.joystick_rx = Scale::int16_to_uint8(gp_in.joystick_rx);
-    in_report.joystick_ry = Scale::int16_to_uint8(gp_in.joystick_ry);
-
 	if (tud_suspended())
     {
 		tud_remote_wakeup();
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/UARTBridge.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/UARTBridge.cpp
index 17bca2e..9d15a37 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/UARTBridge.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/UARTBridge.cpp
@@ -1,33 +1,37 @@
+#include "class/cdc/cdc_device.h"
+#include "bsp/board_api.h"
+
+#include "Descriptors/CDCDev.h"
 #include "USBDevice/DeviceDriver/UARTBridge/UARTBridge.h"
 #include "USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h"
 
 void UARTBridgeDevice::initialize()
 {
-    class_driver_ = {
-	#if CFG_TUSB_DEBUG >= 2
-		.name = "UART",
-	#endif
-		.init = cdcd_init,
-		.reset = cdcd_reset,
-		.open = cdcd_open,
-		.control_xfer_cb = cdcd_control_xfer_cb,
-		.xfer_cb = cdcd_xfer_cb,
-		.sof = NULL
-	};
+    class_driver_ = 
+    {
+        .name = TUD_DRV_NAME("UART_BRIDGE"),
+        .init = cdcd_init,
+        .deinit = cdcd_deinit,
+        .reset = cdcd_reset,
+        .open = cdcd_open,
+        .control_xfer_cb = cdcd_control_xfer_cb,
+        .xfer_cb = cdcd_xfer_cb,
+        .sof = NULL
+    };
 }
 
 void UARTBridgeDevice::process(const uint8_t idx, Gamepad& gamepad)
 {
-    if (!uart_initialized_)
+    if (!task_running_)
     {
-        uart_initialized_ = true;
+        task_running_ = true;
         uart_bridge_run();
     }
 }
 
 uint16_t UARTBridgeDevice::get_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t *buffer, uint16_t reqlen)
 {
-	return sizeof(buffer);
+	return reqlen;
 }
 
 void UARTBridgeDevice::set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t const *buffer, uint16_t bufsize) 
@@ -42,12 +46,47 @@ bool UARTBridgeDevice::vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tus
 
 const uint16_t * UARTBridgeDevice::get_descriptor_string_cb(uint8_t index, uint16_t langid) 
 {
-	return uart_bridge_descriptor_string_cb(index, langid);
+    static uint16_t desc_str[32 + 1];
+    size_t char_count = 0;
+
+    switch(index)
+    {
+        case 0:
+            std::memcpy(&desc_str[1], CDCDesc::DESC_STRING[0], 2);
+            char_count = 1;
+            break;
+
+        case 3:
+            char_count = board_usb_get_serial(&desc_str[1], 32);
+            break;
+
+        default:
+            if (index >= sizeof(CDCDesc::DESC_STRING) / sizeof(CDCDesc::DESC_STRING[0]))
+            {
+                return nullptr;
+            }
+            const char *str = reinterpret_cast<const char *>(CDCDesc::DESC_STRING[index]);
+            char_count = std::strlen(str);
+            const size_t max_count = sizeof(desc_str) / sizeof(desc_str[0]) - 1;    
+            if (char_count > max_count)
+            {
+                char_count = max_count;
+            }
+
+            for (size_t i = 0; i < char_count; i++)
+            {
+                desc_str[1 + i] = str[i];
+            }
+            break;
+    }
+
+    desc_str[0] = static_cast<uint16_t>((TUSB_DESC_STRING << 8) | (2 * char_count + 2));
+    return desc_str;
 }
 
 const uint8_t * UARTBridgeDevice::get_descriptor_device_cb() 
 {
-    return uart_bridge_descriptor_device_cb();
+	return reinterpret_cast<const uint8_t*>(&CDCDesc::DESC_DEVICE);
 }
 
 const uint8_t * UARTBridgeDevice::get_hid_descriptor_report_cb(uint8_t itf) 
@@ -57,7 +96,7 @@ const uint8_t * UARTBridgeDevice::get_hid_descriptor_report_cb(uint8_t itf)
 
 const uint8_t * UARTBridgeDevice::get_descriptor_configuration_cb(uint8_t index) 
 {
-    return uart_bridge_descriptor_configuration_cb(index);
+	return CDCDesc::DESC_CONFIG;
 }
 
 const uint8_t * UARTBridgeDevice::get_descriptor_device_qualifier_cb() 
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/UARTBridge.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/UARTBridge.h
index c3cdac3..c089ecc 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/UARTBridge.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/UARTBridge.h
@@ -20,7 +20,7 @@ public:
     const uint8_t* get_descriptor_device_qualifier_cb() override;
 
 private:
-    bool uart_initialized_ = false;
+    bool task_running_ = false;
 };
 
 #endif // _UART_BRIDGE_DEVICE_H_
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c
index 6cd2d66..004d534 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.c
@@ -13,6 +13,7 @@
 #include <hardware/structs/sio.h>
 #include <hardware/uart.h>
 #include <hardware/flash.h>
+#include <hardware/clocks.h>
 #include <pico/multicore.h>
 #include <pico/stdlib.h>
 #include <string.h>
@@ -302,13 +303,10 @@ void core1_entry(void)
 	}
 }
 
-void usbd_serial_init(void);
-
 int uart_bridge_run(void)
 {
 	set_sys_clock_khz(125000, false);
 
-	usbd_serial_init();
 	tud_init(BOARD_TUD_RHPORT);
 
 	multicore_reset_core1();
@@ -328,129 +326,4 @@ int uart_bridge_run(void)
 	}
 
 	return 0;
-}
-
-#define DESC_STR_MAX 20
-
-#define USBD_VID 0x2E8A /* Raspberry Pi */
-#define USBD_PID 0x000A /* Raspberry Pi Pico SDK CDC */
-
-#define USBD_DESC_LEN (TUD_CONFIG_DESC_LEN + (TUD_CDC_DESC_LEN * CFG_TUD_CDC))
-#define USBD_MAX_POWER_MA 500
-
-enum 
-{
-	USBD_ITF_CDC_0_1 = 0,
-	USBD_ITF_CDC_0_2,
-#if CFG_TUD_CDC > 1	
-	USBD_ITF_CDC_1_1,
-	USBD_ITF_CDC_1_2,
-#endif
-	USBD_ITF_MAX,
-};
-
-#define USBD_CDC_0_EP_CMD 0x81
-#define USBD_CDC_1_EP_CMD 0x83
-
-#define USBD_CDC_0_EP_OUT 0x01
-#define USBD_CDC_1_EP_OUT 0x03
-
-#define USBD_CDC_0_EP_IN 0x82
-#define USBD_CDC_1_EP_IN 0x84
-
-#define USBD_CDC_CMD_MAX_SIZE 8
-#define USBD_CDC_IN_OUT_MAX_SIZE 64
-
-#define USBD_STR_0 0x00
-#define USBD_STR_MANUF 0x01
-#define USBD_STR_PRODUCT 0x02
-#define USBD_STR_SERIAL 0x03
-#define USBD_STR_SERIAL_LEN 17
-#define USBD_STR_CDC 0x04
-
-static const tusb_desc_device_t usbd_desc_device = 
-{
-	.bLength = sizeof(tusb_desc_device_t),
-	.bDescriptorType = TUSB_DESC_DEVICE,
-	.bcdUSB = 0x0200,
-	.bDeviceClass = TUSB_CLASS_MISC,
-	.bDeviceSubClass = MISC_SUBCLASS_COMMON,
-	.bDeviceProtocol = MISC_PROTOCOL_IAD,
-	.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
-	.idVendor = USBD_VID,
-	.idProduct = USBD_PID,
-	.bcdDevice = 0x0100,
-	.iManufacturer = USBD_STR_MANUF,
-	.iProduct = USBD_STR_PRODUCT,
-	.iSerialNumber = USBD_STR_SERIAL,
-	.bNumConfigurations = 1,
-};
-
-static const uint8_t usbd_desc_cfg[USBD_DESC_LEN] = {
-	TUD_CONFIG_DESCRIPTOR(1, USBD_ITF_MAX, USBD_STR_0, USBD_DESC_LEN,
-		TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, USBD_MAX_POWER_MA),
-
-	TUD_CDC_DESCRIPTOR(USBD_ITF_CDC_0_1, USBD_STR_CDC, USBD_CDC_0_EP_CMD,
-		USBD_CDC_CMD_MAX_SIZE, USBD_CDC_0_EP_OUT, USBD_CDC_0_EP_IN,
-		USBD_CDC_IN_OUT_MAX_SIZE),
-
-#if CFG_TUD_CDC > 1	
-	TUD_CDC_DESCRIPTOR(USBD_ITF_CDC_1_1, USBD_STR_CDC, USBD_CDC_1_EP_CMD,
-		USBD_CDC_CMD_MAX_SIZE, USBD_CDC_1_EP_OUT, USBD_CDC_1_EP_IN,
-		USBD_CDC_IN_OUT_MAX_SIZE),
-#endif
-};
-
-static char usbd_serial[USBD_STR_SERIAL_LEN] = "000000000000";
-
-static const char *const usbd_desc_str[] = {
-	[USBD_STR_MANUF] = "Raspberry Pi",
-	[USBD_STR_PRODUCT] = "Pico",
-	[USBD_STR_SERIAL] = usbd_serial,
-	[USBD_STR_CDC] = "Board CDC",
-};
-
-const uint8_t *uart_bridge_descriptor_device_cb(void)
-{
-	return (const uint8_t *) &usbd_desc_device;
-}
-
-const uint8_t *uart_bridge_descriptor_configuration_cb(uint8_t index)
-{
-	return usbd_desc_cfg;
-}
-
-const uint16_t *uart_bridge_descriptor_string_cb(uint8_t index, uint16_t langid)
-{
-	static uint16_t desc_str[DESC_STR_MAX];
-	uint8_t len;
-
-	if (index == 0) {
-		desc_str[1] = 0x0409;
-		len = 1;
-	} else {
-		const char *str;
-		char serial[USBD_STR_SERIAL_LEN];
-
-		if (index >= sizeof(usbd_desc_str) / sizeof(usbd_desc_str[0]))
-			return NULL;
-
-		str = usbd_desc_str[index];
-		for (len = 0; len < DESC_STR_MAX - 1 && str[len]; ++len)
-			desc_str[1 + len] = str[len];
-	}
-
-	desc_str[0] = (TUSB_DESC_STRING << 8) | (2 * len + 2);
-
-	return desc_str;
-}
-
-void usbd_serial_init(void)
-{
-	uint8_t id[8];
-
-	flash_get_unique_id(id);
-
-	snprintf(usbd_serial, USBD_STR_SERIAL_LEN, "%02X%02X%02X%02X%02X%02X%02X%02X",
-		 id[0], id[1], id[2], id[3], id[4], id[5], id[6], id[7]);
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h
index b5ad90e..6a31d49 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/UARTBridge/uart_bridge/uart_bridge.h
@@ -6,9 +6,9 @@ extern "C" {
 #endif
 
 int uart_bridge_run(void);
-const uint8_t *uart_bridge_descriptor_device_cb(void);
-const uint8_t *uart_bridge_descriptor_configuration_cb(uint8_t index);
-const uint16_t *uart_bridge_descriptor_string_cb(uint8_t index, uint16_t langid);
+// const uint8_t *uart_bridge_descriptor_device_cb(void);
+// const uint8_t *uart_bridge_descriptor_configuration_cb(uint8_t index);
+// const uint16_t *uart_bridge_descriptor_string_cb(uint8_t index, uint16_t langid);
 
 #ifdef __cplusplus
 }
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
index e38624e..526d6b3 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
@@ -8,10 +8,9 @@ void WebAppDevice::initialize()
 {
     class_driver_ = 
     {
-    #if CFG_TUSB_DEBUG >= 2
-        .name = "WEBAPP",
-    #endif
+        .name = TUD_DRV_NAME("WEBAPP"),
         .init = cdcd_init,
+        .deinit = cdcd_deinit,
         .reset = cdcd_reset,
         .open = cdcd_open,
         .control_xfer_cb = cdcd_control_xfer_cb,
@@ -36,7 +35,7 @@ void WebAppDevice::process(const uint8_t idx, Gamepad& gamepad)
     switch (in_report_.report_id)
     {
         case ReportID::INIT_READ:
-            in_report_.input_mode = static_cast<uint8_t>(driver_type_);
+            in_report_.input_mode = static_cast<uint8_t>(user_settings_.get_current_driver());
             in_report_.player_idx = 0;
             in_report_.report_id = ReportID::RESP_OK;
             in_report_.max_gamepads = MAX_GAMEPADS;
@@ -49,7 +48,7 @@ void WebAppDevice::process(const uint8_t idx, Gamepad& gamepad)
             break;
 
         case ReportID::READ_PROFILE:
-            in_report_.input_mode = static_cast<uint8_t>(driver_type_);
+            in_report_.input_mode = static_cast<uint8_t>(user_settings_.get_current_driver());
             in_report_.profile = user_settings_.get_profile_by_id(in_report_.profile.id);
             in_report_.report_id = ReportID::RESP_OK;
 
@@ -58,7 +57,7 @@ void WebAppDevice::process(const uint8_t idx, Gamepad& gamepad)
             break;
 
         case ReportID::WRITE_PROFILE:
-            if (in_report_.input_mode != static_cast<uint8_t>(driver_type_) && in_report_.input_mode != 0)
+            if (user_settings_.valid_mode(static_cast<DeviceDriver::Type>(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);
             }
@@ -93,50 +92,47 @@ bool WebAppDevice::vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_co
 
 const uint16_t * WebAppDevice::get_descriptor_string_cb(uint8_t index, uint16_t langid) 
 {
-	size_t chr_count;
+    static uint16_t desc_str[32 + 1];
+    size_t char_count = 0;
 
-	switch ( index ) 
-	{
-		case CDCDesc::STRID_LANGID:
-			std::memcpy(&CDCDesc::_desc_str[1], CDCDesc::STRING_DESCRIPTORS[0], 2);
-			chr_count = 1;
-			break;
+    switch(index)
+    {
+        case 0:
+            std::memcpy(&desc_str[1], CDCDesc::DESC_STRING[0], 2);
+            char_count = 1;
+            break;
 
-		case CDCDesc::STRID_SERIAL:
-			chr_count = board_usb_get_serial(CDCDesc::_desc_str + 1, 32);
-			break;
+        case 3:
+            char_count = board_usb_get_serial(&desc_str[1], 32);
+            break;
 
-		default:
-			if ( !(index < sizeof(CDCDesc::STRING_DESCRIPTORS) / sizeof(CDCDesc::STRING_DESCRIPTORS[0])) ) 
+        default:
+            if (index >= sizeof(CDCDesc::DESC_STRING) / sizeof(CDCDesc::DESC_STRING[0]))
             {
-                return NULL;
+                return nullptr;
+            }
+            const char *str = reinterpret_cast<const char *>(CDCDesc::DESC_STRING[index]);
+            char_count = std::strlen(str);
+            const size_t max_count = sizeof(desc_str) / sizeof(desc_str[0]) - 1;    
+            if (char_count > max_count)
+            {
+                char_count = max_count;
             }
 
-			const char *str = CDCDesc::STRING_DESCRIPTORS[index];
-
-			chr_count = strlen(str);
-			size_t const max_count = sizeof(CDCDesc::_desc_str) / sizeof(CDCDesc::_desc_str[0]) - 1;
-
-			if ( chr_count > max_count ) 
+            for (size_t i = 0; i < char_count; i++)
             {
-                chr_count = max_count;
+                desc_str[1 + i] = str[i];
             }
+            break;
+    }
 
-			for ( size_t i = 0; i < chr_count; i++ ) 
-            {
-				CDCDesc::_desc_str[1 + i] = str[i];
-			}
-			break;
-	}
-
-	CDCDesc::_desc_str[0] = (uint16_t) ((TUSB_DESC_STRING << 8) | (2 * chr_count + 2));
-
-	return CDCDesc::_desc_str;
+    desc_str[0] = static_cast<uint16_t>((TUSB_DESC_STRING << 8) | (2 * char_count + 2));
+    return desc_str;
 }
 
 const uint8_t * WebAppDevice::get_descriptor_device_cb() 
 {
-    return reinterpret_cast<const uint8_t*>(&CDCDesc::DEVICE_DESCRIPTORS);
+    return reinterpret_cast<const uint8_t*>(&CDCDesc::DESC_DEVICE);
 }
 
 const uint8_t * WebAppDevice::get_hid_descriptor_report_cb(uint8_t itf) 
@@ -146,7 +142,7 @@ const uint8_t * WebAppDevice::get_hid_descriptor_report_cb(uint8_t itf)
 
 const uint8_t * WebAppDevice::get_descriptor_configuration_cb(uint8_t index) 
 {
-    return CDCDesc::CONFIGURATION_DESCRIPTORS;
+    return CDCDesc::DESC_CONFIG;
 }
 
 const uint8_t * WebAppDevice::get_descriptor_device_qualifier_cb() 
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.h
index cc8c708..2e97d89 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.h
@@ -1,8 +1,6 @@
 #ifndef _XINPUT_DEVICE_H_
 #define _XINPUT_DEVICE_H_
 
-#include <pico/time.h>
-
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 #include "Descriptors/XInput.h"
 
@@ -23,8 +21,6 @@ public:
 private:
     XInput::InReport in_report_;
     XInput::OutReport out_report_;
-    XInput::InReport prev_in_report_;
-    XInput::OutReport prev_out_report_;
 };
 
 #endif // _XINPUT_DEVICE_H_
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/tud_xinput/tud_xinput.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/tud_xinput/tud_xinput.cpp
index 6d4250e..81e0455 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/tud_xinput/tud_xinput.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/tud_xinput/tud_xinput.cpp
@@ -29,6 +29,12 @@ static void init(void)
     std::memset(out_buffer_, 0, ENDPOINT_SIZE);
 }
 
+static bool deinit(void)
+{
+    init();
+    return true;
+}
+
 static void reset(uint8_t rhport)
 {
     init();
@@ -84,10 +90,13 @@ const usbd_class_driver_t* class_driver()
 {
     static const usbd_class_driver_t tud_class_driver_ =
     {
-        #if CFG_TUSB_DEBUG >= 2
+    #if CFG_TUSB_DEBUG >= 2
         .name = "XINPUT",
-        #endif
+    #else
+        .name = NULL,
+    #endif
         .init = init,
+        .deinit = deinit,
         .reset = reset,
         .open = open,
         .control_xfer_cb = control_xfer_cb,
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp
index efa59a6..f9b85e0 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp
@@ -1,10 +1,10 @@
 #include <cstring>
-#include <vector>
-#include "pico/time.h"
+#include <pico/time.h>
 
 #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 = 
 {{
@@ -82,118 +82,55 @@ void XboxOGSBDevice::initialize()
     in_report_.bLength = sizeof(XboxOG::SB::InReport);
     in_report_.gearLever = XboxOG::SB::Gear::N;
 
-    std::memcpy(&prev_in_report_, &in_report_, sizeof(XboxOG::SB::InReport));
+    prev_in_report_ = in_report_;
 }
 
 void XboxOGSBDevice::process(const uint8_t idx, Gamepad& gamepad) 
 {
-    if (gamepad.new_pad_in())
+    Gamepad::PadIn gp_in = gamepad.get_pad_in();
+    Gamepad::ChatpadIn gp_in_chatpad = gamepad.get_chatpad_in();
+
+    in_report_.dButtons[0] = 0;
+    in_report_.dButtons[1] = 0;
+    in_report_.dButtons[2] &= XboxOG::SB::BUTTONS2_TOGGLE_MID;
+
+    for (const auto& map : GP_MAP)
     {
-        Gamepad::PadIn gp_in = gamepad.get_pad_in();
-
-        in_report_.dButtons[0] = 0;
-        in_report_.dButtons[1] = 0;
-        in_report_.dButtons[2] &= XboxOG::SB::BUTTONS2_TOGGLE_MID;
-
-        // uint16_t gp_buttons = gamepad.get_buttons();
-
-        for (const auto& map : GP_MAP)
+        if (gp_in.buttons & map.gp_mask)
         {
-            if (gp_in.buttons & map.gp_mask)
+            in_report_.dButtons[map.button_offset] |= map.sb_mask;
+        }
+    }
+
+    for (const auto& map : CHATPAD_MAP)
+    {
+        if (chatpad_pressed(gp_in_chatpad, map.gp_mask))
+        {
+            in_report_.dButtons[map.button_offset] |= map.sb_mask;
+        }
+    }
+
+    static std::array<bool, CHATPAD_TOGGLE_MAP.size() + 1> toggle_pressed{false};
+
+    for (uint8_t i = 0; i < CHATPAD_TOGGLE_MAP.size(); i++)
+    {
+        if (chatpad_pressed(gp_in_chatpad, CHATPAD_TOGGLE_MAP[i].gp_mask))
+        {
+            if (!toggle_pressed[i])
             {
-                in_report_.dButtons[map.button_offset] |= map.sb_mask;
+                in_report_.dButtons[CHATPAD_TOGGLE_MAP[i].button_offset] ^= CHATPAD_TOGGLE_MAP[i].sb_mask;
+                toggle_pressed[i] = true;
             }
         }
-
-        // Gamepad::Chatpad gp_chatpad = gamepad.get_chatpad();
-
-        for (const auto& map : CHATPAD_MAP)
+        else
         {
-            if (chatpad_pressed(gp_in.chatpad, map.gp_mask))
-            {
-                in_report_.dButtons[map.button_offset] |= map.sb_mask;
-            }
+            toggle_pressed[i] = false;
         }
+    }
 
-        for (const auto& map : CHATPAD_TOGGLE_MAP)
-        {
-            if (chatpad_pressed(gp_in.chatpad, map.gp_mask))
-            {
-                in_report_.dButtons[map.button_offset] |= map.sb_mask;
-            }
-        }
-
-        if (gp_in.buttons & Gamepad::BUTTON_X)
-        {
-            if (out_report_.Chaff_Extinguisher & 0x0F)
-            {
-                in_report_.dButtons[1] |= XboxOG::SB::Buttons1::EXTINGUISHER;
-            }
-            if (out_report_.Comm1_MagazineChange & 0x0F)
-            {
-                in_report_.dButtons[1] |= XboxOG::SB::Buttons1::WEAPONCONMAGAZINE;
-            }
-            if (out_report_.Washing_LineColorChange & 0xF0)
-            {
-                in_report_.dButtons[1] |= XboxOG::SB::Buttons1::WASHING;
-            }
-        }
-
-        if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_MESSENGER) || gp_in.buttons & Gamepad::BUTTON_BACK)
-        {
-            for (const auto& map : CHATPAD_MAP_ALT1)
-            {
-                if (chatpad_pressed(gp_in.chatpad, map.gp_mask))
-                {
-                    in_report_.dButtons[map.button_offset] |= map.sb_mask;
-                }
-            }
-            // for (uint8_t i = 0; i < sizeof(CHATPAD_MAP_ALT1) / sizeof(CHATPAD_MAP_ALT1[0]); i++)
-            // {
-            //     if (chatpad_pressed(gp_in.chatpad, CHATPAD_MAP_ALT1[i].gp_mask))
-            //     {
-            //         in_report_.dButtons[CHATPAD_MAP_ALT1[i].button_offset] |= CHATPAD_MAP_ALT1[i].sb_mask;
-            //     }
-            // }
-            if (gp_in.dpad & Gamepad::DPAD_UP || gp_in.dpad & Gamepad::DPAD_RIGHT)
-            {
-                in_report_.tunerDial += (prev_in_report_.tunerDial < 15) ? 1 : -15;
-            }
-            if (gp_in.dpad & Gamepad::DPAD_DOWN || gp_in.dpad & Gamepad::DPAD_LEFT)
-            {
-                in_report_.tunerDial -= (prev_in_report_.tunerDial > 0) ? 1 : -15;
-            }
-        }
-        else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_ORANGE))
-        {
-            for (const auto& map : CHATPAD_MAP_ALT2)
-            {
-                if (chatpad_pressed(gp_in.chatpad, map.gp_mask))
-                {
-                    in_report_.dButtons[map.button_offset] |= map.sb_mask;
-                }
-            }
-            // for (uint8_t i = 0; i < sizeof(CHATPAD_MAP_ALT2) / sizeof(CHATPAD_MAP_ALT2[0]); i++)
-            // {
-            //     if (chatpad_pressed(gp_in.chatpad, CHATPAD_MAP_ALT2[i].gp_mask))
-            //     {
-            //         in_report_.dButtons[CHATPAD_MAP_ALT2[i].button_offset] |= CHATPAD_MAP_ALT2[i].sb_mask;
-            //     }
-            // }
-            if (!(gp_in.dpad & Gamepad::DPAD_LEFT) && !(gp_in.dpad & Gamepad::DPAD_RIGHT))
-            {
-                if (gp_in.dpad & Gamepad::DPAD_UP)
-                {
-                    in_report_.gearLever += (prev_in_report_.gearLever < XboxOG::SB::Gear::G5) ? 1 : 0;
-                }
-                if (gp_in.dpad & Gamepad::DPAD_DOWN)
-                {
-                    in_report_.gearLever -= (prev_in_report_.gearLever > XboxOG::SB::Gear::R) ? 1 : 0;
-                }
-            }
-        }
-
-        if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_SHIFT))
+    if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_SHIFT))
+    {
+        if (!toggle_pressed.back())
         {
             if (in_report_.dButtons[2] & XboxOG::SB::BUTTONS2_TOGGLE_MID)
             {
@@ -203,108 +140,193 @@ void XboxOGSBDevice::process(const uint8_t idx, Gamepad& gamepad)
             {
                 in_report_.dButtons[2] |= XboxOG::SB::BUTTONS2_TOGGLE_MID;
             }
+            toggle_pressed.back() = true;
+        }
+    }
+    else
+    {
+        toggle_pressed.back() = false;
+    }
+
+    if (gp_in.buttons & Gamepad::BUTTON_X)
+    {
+        if (out_report_.Chaff_Extinguisher & 0x0F)
+        {
+            in_report_.dButtons[1] |= XboxOG::SB::Buttons1::EXTINGUISHER;
+        }
+        if (out_report_.Comm1_MagazineChange & 0x0F)
+        {
+            in_report_.dButtons[1] |= XboxOG::SB::Buttons1::WEAPONCONMAGAZINE;
+        }
+        if (out_report_.Washing_LineColorChange & 0xF0)
+        {
+            in_report_.dButtons[1] |= XboxOG::SB::Buttons1::WASHING;
+        }
+    }
+
+    if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_MESSENGER) || gp_in.buttons & Gamepad::BUTTON_BACK)
+    {
+        for (const auto& map : CHATPAD_MAP_ALT1)
+        {
+            if (chatpad_pressed(gp_in_chatpad, map.gp_mask))
+            {
+                in_report_.dButtons[map.button_offset] |= map.sb_mask;
+            }
         }
 
-        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) ? INT16_MIN :
-                                    (gp_in.dpad & Gamepad::DPAD_RIGHT) ? INT16_MAX : 0;
-
-        in_report_.sightChangeX = Scale::invert_joy(gp_in.joystick_lx);
-        in_report_.sightChangeY = gp_in.joystick_ly;
-
-        int32_t axis_value = static_cast<int32_t>(Scale::invert_joy(gp_in.joystick_rx));
-
-        if (axis_value > XboxOG::SB::DEFAULT_DEADZONE)
+        if (gp_in.dpad & Gamepad::DPAD_UP && dpad_reset_)
         {
-            vmouse_x_ += axis_value / sensitivity_;
+            in_report_.tunerDial = (in_report_.tunerDial + 1) % 16;
+            dpad_reset_ = false;
+        }
+        else if (gp_in.dpad & Gamepad::DPAD_DOWN && dpad_reset_)
+        {
+            in_report_.tunerDial = (in_report_.tunerDial + 15) % 16;
+            dpad_reset_ = false;
+        }
+        else if (!(gp_in.dpad & Gamepad::DPAD_DOWN) && !(gp_in.dpad & Gamepad::DPAD_UP))
+        {
+            dpad_reset_ = true;
+        }
+    }
+    else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_ORANGE))
+    {
+        for (const auto& map : CHATPAD_MAP_ALT2)
+        {
+            if (chatpad_pressed(gp_in_chatpad, map.gp_mask))
+            {
+                in_report_.dButtons[map.button_offset] |= map.sb_mask;
+            }
         }
 
-        axis_value = static_cast<int32_t>(Scale::invert_joy(gp_in.joystick_ry));
+        // if (!(gp_in.dpad & Gamepad::DPAD_LEFT) && !(gp_in.dpad & Gamepad::DPAD_RIGHT))
+        // {
+            if (gp_in.dpad & Gamepad::DPAD_UP && dpad_reset_)
+            {
+                if (in_report_.gearLever < XboxOG::SB::Gear::G5)
+                {
+                    in_report_.gearLever++;
+                }
+                dpad_reset_ = false;
+            }
+            else if (gp_in.dpad & Gamepad::DPAD_DOWN && dpad_reset_)
+            {
+                if (in_report_.gearLever > XboxOG::SB::Gear::R)
+                {
+                    in_report_.gearLever--;
+                }
+                dpad_reset_ = false;
+            }
+            else if (!(gp_in.dpad & Gamepad::DPAD_DOWN) && !(gp_in.dpad & Gamepad::DPAD_UP))
+            {
+                dpad_reset_ = true;
+            }
+        // }
+    }
+    else
+    {
+        dpad_reset_ = true;
+    }
 
-        if (axis_value > XboxOG::SB::DEFAULT_DEADZONE)
+    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_.sightChangeX = gp_in.joystick_lx;
+    in_report_.sightChangeY = gp_in.joystick_ly;
+
+    int32_t axis_value_x = static_cast<int32_t>(gp_in.joystick_rx);
+    if (std::abs(axis_value_x) > DEFAULT_DEADZONE)
+    {
+        vmouse_x_ += axis_value_x / sensitivity_;
+    }
+
+    int32_t axis_value_y = static_cast<int32_t>(Scale::invert_joy(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_y_ < 0) vmouse_y_ = 0;
+
+    if (gp_in.buttons & Gamepad::BUTTON_L3)
+    {
+        if ((time_us_32() / 1000) - aim_reset_timer_ > 500)
         {
-            vmouse_y_ -= axis_value / sensitivity_;
+            vmouse_x_ = XboxOG::SB::AIMING_MID;
+            vmouse_y_ = XboxOG::SB::AIMING_MID;
+        }
+    }
+    else
+    {
+        aim_reset_timer_ = time_us_32() / 1000;
+    }
+
+    in_report_.aimingX = static_cast<uint16_t>(vmouse_x_);
+    in_report_.aimingY = static_cast<uint16_t>(vmouse_y_);
+
+    if (tud_suspended())
+    {
+        tud_remote_wakeup();
+    }
+    if (tud_xid::send_report_ready(0) &&
+        std::memcmp(&prev_in_report_, &in_report_, sizeof(XboxOG::SB::InReport)) &&
+        tud_xid::send_report(0, reinterpret_cast<uint8_t*>(&in_report_), sizeof(XboxOG::SB::InReport)))
+    {
+        std::memcpy(&prev_in_report_, &in_report_, sizeof(XboxOG::SB::InReport));
+    }
+
+    if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_ORANGE))
+    {
+        uint16_t new_sense = 0;
+
+        if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_9))
+        {
+            new_sense = 200;
+        }
+        else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_8))
+        {
+            new_sense = 250;
+        }
+        else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_7))
+        {
+            new_sense = 300;
+        }
+        else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_6))
+        {
+            new_sense = 350;
+        }
+        else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_5))
+        {
+            new_sense = 400;
+        }
+        else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_4))
+        {
+            new_sense = 650;
+        }
+        else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_3))
+        {
+            new_sense = 800;
+        }
+        else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_2))
+        {
+            new_sense = 1000;
+        }
+        else if (chatpad_pressed(gp_in_chatpad, XInput::Chatpad::CODE_1))
+        {
+            new_sense = 1200;
         }
 
-        if (vmouse_x_ < 0) vmouse_x_ = 0;
-        if (vmouse_x_ > UINT16_MAX) vmouse_x_ = UINT16_MAX;
-        if (vmouse_y_ > UINT16_MAX) vmouse_y_ = UINT16_MAX;
-        if (vmouse_y_ < 0) vmouse_y_ = 0;
-
-        if (gp_in.buttons & Gamepad::BUTTON_L3)
+        if (new_sense != 0)
         {
-            if ((time_us_32() / 1000) - aim_reset_timer_ > 500)
-            {
-                vmouse_x_ = XboxOG::SB::AIMING_MID;
-                vmouse_y_ = XboxOG::SB::AIMING_MID;
-            }
-        }
-        else
-        {
-            aim_reset_timer_ = time_us_32() / 1000;
-        }
-
-        in_report_.aimingX = static_cast<uint16_t>(vmouse_x_);
-        in_report_.aimingY = static_cast<uint16_t>(vmouse_y_);
-
-        if (tud_suspended())
-        {
-            tud_remote_wakeup();
-        }
-        if (tud_xid::send_report_ready(0))
-        {
-            tud_xid::send_report(0, reinterpret_cast<uint8_t*>(&in_report_), sizeof(XboxOG::SB::InReport));
-        }
-
-        if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_ORANGE))
-        {
-            uint16_t new_sense = 0;
-
-            if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_9))
-            {
-                new_sense = 200;
-            }
-            else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_8))
-            {
-                new_sense = 250;
-            }
-            else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_7))
-            {
-                new_sense = 300;
-            }
-            else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_6))
-            {
-                new_sense = 350;
-            }
-            else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_5))
-            {
-                new_sense = 400;
-            }
-            else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_4))
-            {
-                new_sense = 650;
-            }
-            else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_3))
-            {
-                new_sense = 800;
-            }
-            else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_2))
-            {
-                new_sense = 1000;
-            }
-            else if (chatpad_pressed(gp_in.chatpad, XInput::Chatpad::CODE_1))
-            {
-                new_sense = 1200;
-            }
-
-            if (new_sense != 0 && new_sense != sensitivity_)
-            {
-                sensitivity_ = new_sense;
-            }
+            sensitivity_ = new_sense;
         }
     }
 
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h
index 1290037..02f99b1 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h
@@ -30,22 +30,27 @@ public:
     const uint8_t* get_descriptor_device_qualifier_cb() override;
 
 private:
+    static constexpr int16_t DEFAULT_DEADZONE = 7500;
+    static constexpr uint16_t DEFAULT_SENSE = 400;
+
     int32_t vmouse_x_ = XboxOG::SB::AIMING_MID;
     int32_t vmouse_y_ = XboxOG::SB::AIMING_MID;
-    uint16_t sensitivity_ = XboxOG::SB::DEFAULT_SENSE;
+    uint16_t sensitivity_ = DEFAULT_SENSE;
     uint32_t aim_reset_timer_ = 0;
+    bool dpad_reset_ = true;
+    
 
     XboxOG::SB::InReport in_report_;
     XboxOG::SB::InReport prev_in_report_;
     XboxOG::SB::OutReport out_report_;
 
-    static inline bool chatpad_pressed(const uint8_t* chatpad_array, const uint16_t keycode)
+    static inline bool chatpad_pressed(const Gamepad::ChatpadIn& chatpad, const uint16_t keycode)
     {
-        if (std::accumulate(chatpad_array, chatpad_array + 3, 0) == 0) 
+        if (std::accumulate(std::begin(chatpad), std::end(chatpad), 0) == 0) 
         {
             return false;
         }
-        else if (keycode < 17 && (chatpad_array[0] & keycode)) 
+        else if (keycode < 17 && (chatpad[0] & keycode)) 
         {
             return true;
         }
@@ -53,11 +58,11 @@ private:
         {
             return false;
         }
-        else if (chatpad_array[1] == keycode)
+        else if (chatpad[1] == keycode)
         {
             return true;
         }
-        else if (chatpad_array[2] == keycode)
+        else if (chatpad[2] == keycode)
         {
             return true;
         }
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid.cpp
index a5e514c..71709bc 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid.cpp
@@ -609,6 +609,12 @@ static void xid_init()
     }
 }
 
+static bool xid_deinit()
+{
+    xid_init();
+    return true;
+}
+
 static void xid_reset(uint8_t rhport)
 {
     xid_init();
@@ -834,8 +840,11 @@ static const usbd_class_driver_t tud_xid_class_driver_ =
 {
 #if CFG_TUSB_DEBUG >= 2
     .name = "XID DRIVER (DUKE,SB OR XREMOTE)",
+#else
+    .name = nullptr,
 #endif
     .init = xid_init,
+    .deinit = xid_deinit,
     .reset = xid_reset,
     .open = xid_open,
     .control_xfer_cb = xid_control_xfer_cb,
diff --git a/Firmware/RP2040/src/USBDevice/DeviceManager.cpp b/Firmware/RP2040/src/USBDevice/DeviceManager.cpp
index 38017ee..0f9fa30 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceManager.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceManager.cpp
@@ -16,45 +16,57 @@
 #include "USBDevice/DeviceDriver/UARTBridge/UARTBridge.h"
 #endif // defined(CONFIG_EN_UART_BRIDGE)
 
-void DeviceManager::initialize_driver(DeviceDriver::Type driver_type)
+void DeviceManager::initialize_driver(DeviceDriver::Type 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:
-            device_driver_ = new DInputDevice();
+            has_analog = true;
+            device_driver_ = std::make_unique<DInputDevice>();
             break;
         case DeviceDriver::Type::PS3:
-            device_driver_ = new PS3Device();
+            has_analog = true;
+            device_driver_ = std::make_unique<PS3Device>();
             break;
         case DeviceDriver::Type::PSCLASSIC:
-            device_driver_ = new PSClassicDevice();
+            device_driver_ = std::make_unique<PSClassicDevice>();
             break;
         case DeviceDriver::Type::SWITCH:
-            device_driver_ = new SwitchDevice();
+            device_driver_ = std::make_unique<SwitchDevice>();
             break;
         case DeviceDriver::Type::XINPUT:
-            device_driver_ = new XInputDevice();
+            device_driver_ = std::make_unique<XInputDevice>();
             break;
         case DeviceDriver::Type::XBOXOG:
-            device_driver_ = new XboxOGDevice();
+            has_analog = true;
+            device_driver_ = std::make_unique<XboxOGDevice>();
             break;
         case DeviceDriver::Type::XBOXOG_SB:
-            device_driver_ = new XboxOGSBDevice();
+            device_driver_ = std::make_unique<XboxOGSBDevice>();
             break;
         case DeviceDriver::Type::XBOXOG_XR:
-            device_driver_ = new XboxOGXRDevice();
+            device_driver_ = std::make_unique<XboxOGXRDevice>();
             break;
         case DeviceDriver::Type::WEBAPP:
-            device_driver_ = new WebAppDevice();
+            device_driver_ = std::make_unique<WebAppDevice>();
             break;
 #if defined(CONFIG_EN_UART_BRIDGE)
         case DeviceDriver::Type::UART_BRIDGE:
-            device_driver_ = new UARTBridgeDevice();
+            device_driver_ = std::make_unique<UARTBridgeDevice>();
             break;
 #endif //defined(CONFIG_EN_UART_BRIDGE)
         default:
             return;
     }
 
+    if (has_analog)
+    {
+        for (size_t i = 0; i < MAX_GAMEPADS; ++i)
+        {
+            gamepads[i].set_analog_device(true);
+        }
+    }
+
     device_driver_->initialize();
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBDevice/DeviceManager.h b/Firmware/RP2040/src/USBDevice/DeviceManager.h
index 7a96020..227fcf1 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceManager.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceManager.h
@@ -1,8 +1,10 @@
 #ifndef _DEVICE_MANAGER_H_
 #define _DEVICE_MANAGER_H_
 
+#include <cstdint>
 #include <memory>
 
+#include "Gamepad.h"
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 
 class DeviceManager 
@@ -18,14 +20,15 @@ public:
 	}
 
 	//Must be called before any other method
-	void initialize_driver(DeviceDriver::Type driver_type);
+	void initialize_driver(DeviceDriver::Type driver_type, Gamepad(&gamepads)[MAX_GAMEPADS]);
 	
-	DeviceDriver* get_driver() { return device_driver_; }
+	DeviceDriver* get_driver() { return device_driver_.get(); }
 	
 private:
-    DeviceManager() {}
-	~DeviceManager() { delete device_driver_; }
-    DeviceDriver* device_driver_{nullptr};
+    DeviceManager() = default;
+	~DeviceManager() = default;
+
+	std::unique_ptr<DeviceDriver> device_driver_{nullptr};
 };
 
 #endif // _DEVICE_MANAGER_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDJoystick.cpp b/Firmware/RP2040/src/USBHost/HIDParser/HIDJoystick.cpp
index c369b6e..da379c8 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDJoystick.cpp
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDJoystick.cpp
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #include "USBHost/HIDParser/HIDJoystick.h"
 #include "USBHost/HIDParser/HIDUtils.h"
 #include <cstring>
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDJoystick.h b/Firmware/RP2040/src/USBHost/HIDParser/HIDJoystick.h
index 4d5a83d..d6b7a1a 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDJoystick.h
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDJoystick.h
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #include "USBHost/HIDParser/HIDReportDescriptor.h"
 #include <memory>
 #include <vector>
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptor.cpp b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptor.cpp
index a46d11b..fdc5e85 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptor.cpp
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptor.cpp
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #include "USBHost/HIDParser/HIDReportDescriptor.h"
 #include "USBHost/HIDParser/HIDReportDescriptorElements.h"
 #include "USBHost/HIDParser/HIDReportDescriptorUsages.h"
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptor.h b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptor.h
index 9239fe9..1415ef6 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptor.h
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptor.h
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #pragma once
 
 #include <stdint.h>
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorElements.cpp b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorElements.cpp
index 507797e..381bf9b 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorElements.cpp
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorElements.cpp
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #include "USBHost/HIDParser/HIDReportDescriptorElements.h"
 #include <cstring>
 
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorElements.h b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorElements.h
index 10c004e..a369991 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorElements.h
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorElements.h
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #pragma once
 #include <memory>
 #include <vector>
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorUsages.cpp b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorUsages.cpp
index 4cb047b..3319e45 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorUsages.cpp
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorUsages.cpp
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #include "USBHost/HIDParser/HIDReportDescriptorUsages.h"
 #include <iostream>
 #include <vector>
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorUsages.h b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorUsages.h
index a84000b..4eb172f 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorUsages.h
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDReportDescriptorUsages.h
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #pragma once
 #include "USBHost/HIDParser/HIDReportDescriptorElements.h"
 #include <vector>
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDUtils.cpp b/Firmware/RP2040/src/USBHost/HIDParser/HIDUtils.cpp
index c97dcc8..e926002 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDUtils.cpp
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDUtils.cpp
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #include "USBHost/HIDParser/HIDUtils.h"
 
 uint32_t HIDUtils::readBitsLE(uint8_t *buffer, uint32_t bitOffset, uint32_t bitLength) {
diff --git a/Firmware/RP2040/src/USBHost/HIDParser/HIDUtils.h b/Firmware/RP2040/src/USBHost/HIDParser/HIDUtils.h
index 4478676..5ac7cde 100644
--- a/Firmware/RP2040/src/USBHost/HIDParser/HIDUtils.h
+++ b/Firmware/RP2040/src/USBHost/HIDParser/HIDUtils.h
@@ -1,3 +1,27 @@
+/*
+    MIT License
+
+    Copyright (c) 2024 o0zz
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in all
+    copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+    SOFTWARE.
+*/
+
 #pragma once
 #include <stdint.h>
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp b/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
index 51220c2..53354fc 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
@@ -7,7 +7,7 @@
 
 void DInputHost::initialize(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report_desc, uint16_t desc_len) 
 {
-    gamepad.set_analog_enabled(true);
+    gamepad.set_analog_host(true);
     tuh_hid_receive_report(address, instance);
 }
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h b/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
index 50550ad..265b0ae 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
+++ b/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
@@ -39,6 +39,9 @@ public:
     virtual void process_report(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len) = 0;
     virtual bool send_feedback(Gamepad& gamepad, uint8_t address, uint8_t instance) = 0;
 
+    virtual void connect_cb(Gamepad& gamepad, uint8_t address, uint8_t instance) {}; //Wireless specific
+    virtual void disconnect_cb(Gamepad& gamepad, uint8_t address, uint8_t instance) {}; //Wireless specific
+
 protected:
     const uint8_t idx_;
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
index d415fc5..55a5ff7 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
@@ -16,12 +16,14 @@ const tusb_control_request_t PS3Host::RUMBLE_REQUEST =
 
 void PS3Host::initialize(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report_desc, uint16_t desc_len) 
 {
-    gamepad.set_analog_enabled(true);
+    gamepad.set_analog_host(true);
     
-    std::memcpy(&out_report_, PS3::DEFAULT_OUT_REPORT, std::min(sizeof(PS3::OutReport), sizeof(PS3::DEFAULT_OUT_REPORT)));
+    std::memcpy(reinterpret_cast<uint8_t*>(&out_report_), 
+                PS3::DEFAULT_OUT_REPORT, 
+                std::min(sizeof(PS3::OutReport), sizeof(PS3::DEFAULT_OUT_REPORT)));
 
     out_report_.leds_bitmap = 0x1 << (idx_ + 1);
-    out_report_.led[idx_].time_enabled = 0xFF;
+    out_report_.leds[idx_].time_enabled = 0xFF;
 
     init_state_.out_report = &out_report_;
     init_state_.dev_addr = address;
@@ -154,12 +156,13 @@ bool PS3Host::send_feedback(Gamepad& gamepad, uint8_t address, uint8_t instance)
     static uint32_t last_rumble_ms = 0;
 
     uint32_t current_ms = time_us_32() / 1000;
-    Gamepad::PadOut gp_out = gamepad.get_pad_out();
     
-    //Spamming control xfers doesn't work, limit the rate
+    //Spamming set_report doesn't work, limit the rate
     if (init_state_.reports_enabled &&
         current_ms - last_rumble_ms >= 300)
     {
+        Gamepad::PadOut gp_out = gamepad.get_pad_out();
+
         out_report_.rumble.right_duration    = (gp_out.rumble_r > 0) ? 20 : 0;
         out_report_.rumble.right_motor_on    = (gp_out.rumble_r > 0) ? 1  : 0;
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
index 95f5bab..510ae3a 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
@@ -7,21 +7,35 @@
 
 void PS5Host::initialize(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report_desc, uint16_t desc_len) 
 {
-    std::memset(&out_report_, 0, sizeof(out_report_));
-    out_report_.report_id = PS5::OUT_REPORT_ID;
-    out_report_.valid_flag0 = 0x02;
-    out_report_.valid_flag1 = 0x02;
-    out_report_.valid_flag2 = 0x04;
+    out_report_.report_id = PS5::OutReportID::CONTROL;
+    out_report_.control_flag[0] = 2;
+    out_report_.control_flag[1] = 2;
+    out_report_.led_control_flag = 0x01 | 0x02;
+    out_report_.pulse_option = 1;
+    out_report_.led_brightness = 0xFF;
+    out_report_.player_number = idx_ + 1;
+    out_report_.lightbar_blue = 0xFF;
+
+    while (!tuh_hid_send_report(address, instance, 0, &out_report_, sizeof(PS5::OutReport)))
+    {
+        tuh_task();
+    }
+
+    out_report_ = PS5::OutReport();
+    out_report_.report_id = PS5::OutReportID::RUMBLE;
+    out_report_.control_flag[0] = 2;
+    out_report_.control_flag[1] = 2;
+    out_report_.led_control_flag = 0x04;
     
     tuh_hid_receive_report(address, instance);
 }
 
 void PS5Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
 {
-    in_report_ = *reinterpret_cast<const PS5::InReport*>(report);
-    in_report_.seq_number = 0;
+    const PS5::InReport* in_report = reinterpret_cast<const PS5::InReport*>(report);
 
-    if (std::memcmp(&prev_in_report_, &in_report_, PS5::IN_REPORT_CMP_SIZE) == 0)
+    if (std::memcmp(&prev_in_report_.joystick_lx, &in_report->joystick_lx, sizeof(uint8_t) * 6) == 0 &&
+        std::memcmp(prev_in_report_.buttons, in_report->buttons, sizeof(in_report->buttons)) == 0)
     {
         tuh_hid_receive_report(address, instance);
         return;
@@ -29,7 +43,7 @@ void PS5Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
 
     Gamepad::PadIn gp_in;   
 
-    switch (in_report_.buttons[0] & PS5::DPAD_MASK)
+    switch (in_report->buttons[0] & PS5::DPAD_MASK)
     {
         case PS5::Buttons0::DPAD_UP:
             gp_in.dpad |= gamepad.MAP_DPAD_UP;
@@ -59,31 +73,31 @@ void PS5Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
             break;
     }
 
-    if (in_report_.buttons[0] & PS5::Buttons0::SQUARE)   gp_in.buttons |= gamepad.MAP_BUTTON_X;
-    if (in_report_.buttons[0] & PS5::Buttons0::CROSS)    gp_in.buttons |= gamepad.MAP_BUTTON_A;
-    if (in_report_.buttons[0] & PS5::Buttons0::CIRCLE)   gp_in.buttons |= gamepad.MAP_BUTTON_B;
-    if (in_report_.buttons[0] & PS5::Buttons0::TRIANGLE) gp_in.buttons |= gamepad.MAP_BUTTON_Y;
-    if (in_report_.buttons[1] & PS5::Buttons1::L1)       gp_in.buttons |= gamepad.MAP_BUTTON_LB;
-    if (in_report_.buttons[1] & PS5::Buttons1::R1)       gp_in.buttons |= gamepad.MAP_BUTTON_RB;
-    if (in_report_.buttons[1] & PS5::Buttons1::L3)       gp_in.buttons |= gamepad.MAP_BUTTON_L3;
-    if (in_report_.buttons[1] & PS5::Buttons1::R3)       gp_in.buttons |= gamepad.MAP_BUTTON_R3;
-    if (in_report_.buttons[1] & PS5::Buttons1::SHARE)    gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
-    if (in_report_.buttons[1] & PS5::Buttons1::OPTIONS)  gp_in.buttons |= gamepad.MAP_BUTTON_START;
-    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;
+    if (in_report->buttons[0] & PS5::Buttons0::SQUARE)   gp_in.buttons |= gamepad.MAP_BUTTON_X;
+    if (in_report->buttons[0] & PS5::Buttons0::CROSS)    gp_in.buttons |= gamepad.MAP_BUTTON_A;
+    if (in_report->buttons[0] & PS5::Buttons0::CIRCLE)   gp_in.buttons |= gamepad.MAP_BUTTON_B;
+    if (in_report->buttons[0] & PS5::Buttons0::TRIANGLE) gp_in.buttons |= gamepad.MAP_BUTTON_Y;
+    if (in_report->buttons[1] & PS5::Buttons1::L1)       gp_in.buttons |= gamepad.MAP_BUTTON_LB;
+    if (in_report->buttons[1] & PS5::Buttons1::R1)       gp_in.buttons |= gamepad.MAP_BUTTON_RB;
+    if (in_report->buttons[1] & PS5::Buttons1::L3)       gp_in.buttons |= gamepad.MAP_BUTTON_L3;
+    if (in_report->buttons[1] & PS5::Buttons1::R3)       gp_in.buttons |= gamepad.MAP_BUTTON_R3;
+    if (in_report->buttons[1] & PS5::Buttons1::SHARE)    gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
+    if (in_report->buttons[1] & PS5::Buttons1::OPTIONS)  gp_in.buttons |= gamepad.MAP_BUTTON_START;
+    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 = in_report->trigger_l;
+    gp_in.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 = 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);
     
     gamepad.set_pad_in(gp_in);
 
     tuh_hid_receive_report(address, instance);
-    std::memcpy(&prev_in_report_, &in_report_, PS5::IN_REPORT_CMP_SIZE);
+    std::memcpy(&prev_in_report_, in_report, sizeof(PS5::InReport));
 }
 
 bool PS5Host::send_feedback(Gamepad& gamepad, uint8_t address, uint8_t instance)
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.h b/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.h
index a7b6fbc..fef2687 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.h
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.h
@@ -17,7 +17,6 @@ public:
     bool send_feedback(Gamepad& gamepad, uint8_t address, uint8_t instance) override;
 
 private:
-    PS5::InReport in_report_{};
     PS5::InReport prev_in_report_{};
     PS5::OutReport out_report_{};
 };
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
index 87b8462..4d4e500 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
@@ -5,36 +5,48 @@
 
 #include "host/usbh.h"
 
+#include "TaskQueue/TaskQueue.h"
 #include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
 #include "USBHost/HostDriver/XInput/Xbox360W.h"
+#include "OGXMini/Debug.h"
 
 Xbox360WHost::~Xbox360WHost()
 {
-    TaskQueue::Core1::cancel_delayed_task(timer_info_.task_id);
+    TaskQueue::Core1::cancel_delayed_task(tid_chatpad_keepalive_);
 }
 
 void Xbox360WHost::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::InReportWireless));
-
-    timer_info_.address = address;
-    timer_info_.instance = instance;
-    timer_info_.led_quadrant = idx_ + 1;
-    timer_info_.task_id = TaskQueue::Core1::get_new_task_id();
-
-    //Repeatedly set the LED incase of disconnect, may rework the XInput driver to handle this
-    TaskQueue::Core1::queue_delayed_task(timer_info_.task_id, 1000, true, [this]
-    {
-        tuh_xinput::set_led(timer_info_.address, timer_info_.instance, timer_info_.led_quadrant, false);
-    });
-    
     tuh_xinput::receive_report(address, instance);
 }
 
 void Xbox360WHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
 {
     const XInput::InReportWireless* in_report = reinterpret_cast<const XInput::InReportWireless*>(report);
-    if (std::memcmp(&prev_in_report_, in_report, std::min(static_cast<size_t>(len), sizeof(XInput::InReportWireless))) == 0)
+
+    if (in_report->command[1] & 2)
+    {
+        if (in_report->chatpad_status == 0x00)
+        {
+            Gamepad::ChatpadIn gp_in_chatpad;
+            gp_in_chatpad[0] = in_report->chatpad[0];
+            gp_in_chatpad[1] = in_report->chatpad[1];
+            gp_in_chatpad[2] = in_report->chatpad[2];
+
+            gamepad.set_chatpad_in(gp_in_chatpad);
+        }
+        else if (in_report->chatpad_status == 0xF0 && in_report->chatpad[0] == 0x03)
+        {
+            tuh_xinput::xbox360_chatpad_init(address, instance);
+        }
+
+        tuh_xinput::receive_report(address, instance);
+        return;
+    }
+
+    if (!(in_report->command[1] & 1) ||
+        !(in_report->report_size == 0x13) ||
+        std::memcmp(&prev_in_report_, in_report, std::min(static_cast<size_t>(len), sizeof(XInput::InReportWireless))) == 0)
     {
         tuh_xinput::receive_report(address, instance);
         return;
@@ -67,21 +79,39 @@ void Xbox360WHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t ins
     gp_in.joystick_rx = in_report->joystick_rx;
     gp_in.joystick_ry = Scale::invert_joy(in_report->joystick_ry);
 
-    if ((in_report->command[1] & 2) && (in_report->chatpad_status == 0x00))
-    {
-        gp_in.chatpad[0] = in_report->chatpad[0];
-        gp_in.chatpad[1] = in_report->chatpad[1];
-        gp_in.chatpad[2] = in_report->chatpad[2];
-    }
-
     gamepad.set_pad_in(gp_in);
 
     tuh_xinput::receive_report(address, instance);
-    std::memcpy(&prev_in_report_, in_report, sizeof(XInput::InReportWireless));
+    prev_in_report_ = *in_report;
 }
 
 bool Xbox360WHost::send_feedback(Gamepad& gamepad, uint8_t address, uint8_t instance)
 {
     Gamepad::PadOut gp_out = gamepad.get_pad_out(); 
     return tuh_xinput::set_rumble(address, instance, gp_out.rumble_l, gp_out.rumble_r, false);
+}
+
+void Xbox360WHost::connect_cb(Gamepad& gamepad, uint8_t address, uint8_t instance)
+{
+    tid_chatpad_keepalive_ = TaskQueue::Core1::get_new_task_id();
+
+    //Might not be ready for leds yet, needs delay
+    TaskQueue::Core1::queue_delayed_task(TaskQueue::Core1::get_new_task_id(), 1000, false, 
+    [address, instance, this]
+    {
+        tuh_xinput::set_led(address, instance, idx_ + 1, false);
+        tuh_xinput::xbox360_chatpad_init(address, instance);
+        
+        TaskQueue::Core1::queue_delayed_task(tid_chatpad_keepalive_, tuh_xinput::KEEPALIVE_MS, true, 
+        [address, instance]
+        {
+            OGXM_LOG("XInput Chatpad Keepalive\r\n");
+            tuh_xinput::xbox360_chatpad_keepalive(address, instance);
+        });
+    });
+}
+
+void Xbox360WHost::disconnect_cb(Gamepad& gamepad, uint8_t address, uint8_t instance)
+{
+    TaskQueue::Core1::cancel_delayed_task(tid_chatpad_keepalive_);
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.h b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.h
index f832113..e048f2d 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.h
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.h
@@ -3,7 +3,6 @@
 
 #include <cstdint>
 
-#include "TaskQueue/TaskQueue.h"
 #include "Descriptors/XInput.h"
 #include "USBHost/HostDriver/HostDriver.h"
 
@@ -19,19 +18,12 @@ public:
     void process_report(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len) override;
     bool send_feedback(Gamepad& gamepad, uint8_t address, uint8_t instance) override;
 
+    void connect_cb(Gamepad& gamepad, uint8_t address, uint8_t instance) override;
+    void disconnect_cb(Gamepad& gamepad, uint8_t address, uint8_t instance) override;
+
 private:
-    struct TimerInfo
-    {
-        uint8_t address;
-        uint8_t instance;
-        uint8_t led_quadrant;
-        uint32_t task_id;
-    };
-
-    TimerInfo timer_info_;
+    uint32_t tid_chatpad_keepalive_{0};
     XInput::InReportWireless prev_in_report_;
-
-    static bool timer_cb(struct repeating_timer *t);
 };
 
 #endif // _XBOX360_WIRELESS_HOST_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
index cd6bbe1..37085d2 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
@@ -7,7 +7,7 @@
 
 void XboxOGHost::initialize(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report_desc, uint16_t desc_len)
 {
-    gamepad.set_analog_enabled(true);
+    gamepad.set_analog_host(true);
     std::memset(&prev_in_report_, 0, sizeof(XboxOG::GP::InReport));
     tuh_xinput::receive_report(address, instance);
 }
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.cpp
index 04a53a1..afc5f70 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.cpp
@@ -3,6 +3,7 @@
 #if (TUSB_OPT_HOST_ENABLED && CFG_TUH_XINPUT)
 
 #include <cstring>
+#include <chrono>
 
 #include "Board/board_api.h"
 #include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
@@ -21,49 +22,65 @@ struct Device
 
 std::array<Device, MAX_DEVICES> devices_;
 
-// TU_ATTR_ALWAYS_INLINE static inline Device* get_device_by_addr(uint8_t dev_addr)
-// {
-//     TU_VERIFY((dev_addr <= devices_.size() && dev_addr > 0), nullptr);
-//     return &devices_[dev_addr - 1];
-// }
-
-// TU_ATTR_ALWAYS_INLINE static inline Interface* get_itf_by_instance(uint8_t dev_addr, uint8_t instance)
-// {
-//     Device* device = get_device_by_addr(dev_addr);
-//     TU_VERIFY(device != nullptr, nullptr);
-//     TU_VERIFY(instance < device->interfaces.size(), nullptr);
-//     return &device->interfaces[instance];
-// }
-
-// TU_ATTR_ALWAYS_INLINE static inline Interface* get_free_interface(uint8_t dev_addr)
-// {
-//     Device* device = get_device_by_addr(dev_addr);
-//     TU_VERIFY(device != nullptr, nullptr);
-
-//     for (auto& interface : device->interfaces)
-//     {
-//         if (interface.itf_num == 0xFF)
-//         {
-//             return &interface;
-//         }
-//     }
-//     return nullptr;
-// }
-
-TU_ATTR_ALWAYS_INLINE static inline uint8_t get_device_idx_by_addr(uint8_t dev_addr)
+TU_ATTR_ALWAYS_INLINE static inline Device* get_device_by_addr(uint8_t dev_addr)
 {
-    TU_VERIFY((dev_addr <= devices_.size() && dev_addr > 0), INVALID_IDX);
-    return dev_addr - 1;
+    TU_VERIFY((dev_addr <= devices_.size() && dev_addr > 0), nullptr);
+    return &devices_[dev_addr - 1];
+}
+
+TU_ATTR_ALWAYS_INLINE static inline Interface* get_itf_by_itf_num(uint8_t dev_addr, uint8_t itf_num)
+{
+    Device* device = get_device_by_addr(dev_addr);
+    TU_VERIFY(device != nullptr, nullptr);
+
+    for (auto& interface : device->interfaces)
+    {
+        if (interface.itf_num == itf_num)
+        {
+            return &interface;
+        }
+    }
+    return nullptr;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline Interface* get_itf_by_ep(uint8_t dev_addr, uint8_t ep_addr)
+{
+    Device* device = get_device_by_addr(dev_addr);
+    TU_VERIFY(device != nullptr, nullptr);
+
+    for (auto& interface : device->interfaces)
+    {
+        if (interface.ep_in == ep_addr || interface.ep_out == ep_addr)
+        {
+            return &interface;
+        }
+    }
+    return nullptr;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline Interface* get_free_itf(uint8_t dev_addr)
+{
+    Device* device = get_device_by_addr(dev_addr);
+    TU_VERIFY(device != nullptr, nullptr);
+
+    for (auto& interface : device->interfaces)
+    {
+        if (interface.itf_num == INVALID_IDX)
+        {
+            return &interface;
+        }
+    }
+    return nullptr;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint8_t get_instance_by_itf_num(uint8_t dev_addr, uint8_t itf_num)
 {
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX, INVALID_IDX);
+    Device* device = get_device_by_addr(dev_addr);
+    TU_VERIFY(device != nullptr, INVALID_IDX);
 
-    for (uint8_t i = 0; i < devices_[dev_idx].interfaces.size(); ++i)
+    for (uint8_t i = 0; i < device->interfaces.size(); ++i)
     {
-        if (devices_[dev_idx].interfaces[i].itf_num == itf_num)
+        if (device->interfaces[i].itf_num == itf_num)
         {
             return i;
         }
@@ -71,34 +88,22 @@ TU_ATTR_ALWAYS_INLINE static inline uint8_t get_instance_by_itf_num(uint8_t dev_
     return INVALID_IDX;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint8_t get_instance_by_ep(uint8_t dev_addr, uint8_t ep_addr)
+TU_ATTR_ALWAYS_INLINE static inline Interface* get_itf_by_instance(uint8_t dev_addr, uint8_t instance)
 {
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX, INVALID_IDX);
-
-    for (uint8_t i = 0; i < devices_[dev_idx].interfaces.size(); ++i)
-    {
-        if (devices_[dev_idx].interfaces[i].ep_in == ep_addr || devices_[dev_idx].interfaces[i].ep_out == ep_addr)
-        {
-            return i;
-        }
-    }
-    return INVALID_IDX;
+    Device* device = get_device_by_addr(dev_addr);
+    TU_VERIFY(device != nullptr, nullptr);
+    TU_VERIFY(instance < device->interfaces.size(), nullptr);
+    return &device->interfaces[instance];
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint8_t get_free_itf_idx(uint8_t dev_addr)
+static void std_sleep_ms(uint32_t ms)
 {
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX, INVALID_IDX);
-
-    for (uint8_t i = 0; i < devices_[dev_idx].interfaces.size(); ++i)
+    auto start = std::chrono::high_resolution_clock::now();
+    while ( std::chrono::duration_cast<std::chrono::milliseconds>(
+                std::chrono::high_resolution_clock::now() - start).count() < ms) 
     {
-        if (devices_[dev_idx].interfaces[i].itf_num == INVALID_IDX)
-        {
-            return i;
-        }
+        tuh_task();
     }
-    return INVALID_IDX;
 }
 
 static void wait_for_tx_complete(uint8_t dev_addr, uint8_t ep_addr)
@@ -123,69 +128,6 @@ bool send_ctrl_xfer(uint8_t dev_addr, const tusb_control_request_t* request, uin
     return tuh_control_xfer(&transfer);
 }
 
-static void xbox360w_chatpad_init(Interface *interface, uint8_t address, uint8_t instance)
-{
-    send_report(address, instance, Xbox360W::INQUIRE_PRESENT, sizeof(Xbox360W::INQUIRE_PRESENT));
-    wait_for_tx_complete(address, interface->ep_out);
-    send_report(address, instance, Xbox360W::CONTROLLER_INFO, sizeof(Xbox360W::CONTROLLER_INFO));
-    wait_for_tx_complete(address, interface->ep_out);
-    send_report(address, instance, Xbox360W::Chatpad::INIT, sizeof(Xbox360W::Chatpad::INIT));
-    wait_for_tx_complete(address, interface->ep_out);
-
-    uint8_t led_ctrl[4];
-    std::memcpy(led_ctrl, Xbox360W::Chatpad::LED_CTRL, sizeof(Xbox360W::Chatpad::LED_CTRL));
-    led_ctrl[2] = Xbox360W::Chatpad::LED_ON[0];
-
-    send_report(address, instance, led_ctrl, sizeof(led_ctrl));
-    wait_for_tx_complete(address, interface->ep_out);
-
-    interface->chatpad_inited = true;
-    interface->chatpad_stage = ChatpadStage::KEEPALIVE_1;
-}
-
-bool xbox360_chatpad_keepalive(uint8_t address, uint8_t instance)
-{   
-    uint8_t dev_idx = get_device_idx_by_addr(address);
-    uint8_t itf_idx = get_instance_by_itf_num(address, instance);
-    TU_VERIFY(dev_idx != INVALID_IDX && itf_idx != INVALID_IDX, false);
-
-    Interface& interface = devices_[dev_idx].interfaces[itf_idx];
-    TU_VERIFY(interface.connected, false);
-
-    switch (interface.chatpad_stage)
-    {
-        case ChatpadStage::KEEPALIVE_1:
-            switch (interface.dev_type)
-            {
-                case DevType::XBOX360:
-                    send_ctrl_xfer(interface.dev_addr, &Xbox360::Chatpad::KEEPALIVE_1, nullptr, nullptr, 0);
-                    break;
-                case DevType::XBOX360W:
-                    send_report(interface.dev_addr, instance, Xbox360W::Chatpad::KEEPALIVE_1, sizeof(Xbox360W::Chatpad::KEEPALIVE_1));
-                    break;
-                default:
-                    break;
-            }
-            interface.chatpad_stage = ChatpadStage::KEEPALIVE_2;
-            break;
-        case ChatpadStage::KEEPALIVE_2:
-            switch (interface.dev_type)
-            {
-                case DevType::XBOX360:
-                    send_ctrl_xfer(interface.dev_addr, &Xbox360::Chatpad::KEEPALIVE_2, nullptr, nullptr, 0);
-                    break;
-                case DevType::XBOX360W:
-                    send_report(interface.dev_addr, instance, Xbox360W::Chatpad::KEEPALIVE_2, sizeof(Xbox360W::Chatpad::KEEPALIVE_2));
-                    break;
-                default:
-                    break;
-            }
-            interface.chatpad_stage = ChatpadStage::KEEPALIVE_1;
-            break;
-    }
-    return true;
-}
-
 static void xboxone_init(Interface *interface, uint8_t dev_addr, uint8_t instance)
 {
     uint16_t PID, VID;
@@ -216,14 +158,14 @@ static void xboxone_init(Interface *interface, uint8_t dev_addr, uint8_t instanc
 
 static bool init()
 {
-    TU_LOG2("XInput Init\r\n");
+    TU_LOG1("XInput Init\r\n");
     devices_.fill(Device());
     return true;
 } 
 
 static bool open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
 {
-    TU_LOG2("XInput Open\r\n");
+    TU_LOG1("XInput Open\r\n");
     TU_VERIFY(desc_itf->bNumEndpoints > 0);
 
     DevType dev_type = DevType::UNKNOWN;
@@ -250,13 +192,10 @@ static bool open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *
         dev_type = DevType::XBOXOG;
     }
 
-    TU_VERIFY(dev_type != DevType::UNKNOWN);
+    TU_VERIFY(dev_type != DevType::UNKNOWN && itf_type != ItfType::UNKNOWN);
 
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    uint8_t itf_idx = get_free_itf_idx(dev_addr);
-    TU_VERIFY(itf_idx != INVALID_IDX && dev_idx != INVALID_IDX);
-
-    Interface& interface = devices_[dev_idx].interfaces[itf_idx];
+    Interface* interface = get_free_itf(dev_addr);
+    TU_VERIFY(interface != nullptr);
 
     const uint8_t *p_desc = reinterpret_cast<const uint8_t*>(desc_itf);
     int endpoint = 0;
@@ -275,20 +214,20 @@ static bool open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *
         TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType);
         TU_ASSERT(tuh_edpt_open(dev_addr, desc_ep));
 
-        interface.itf_num = desc_itf->bInterfaceNumber;
-        interface.itf_type = itf_type;
-        interface.dev_type = dev_type;
-        interface.dev_addr = dev_addr;
+        interface->itf_num = desc_itf->bInterfaceNumber;
+        interface->itf_type = itf_type;
+        interface->dev_type = dev_type;
+        interface->dev_addr = dev_addr;
 
         if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_OUT)
         {
-            interface.ep_out = desc_ep->bEndpointAddress;
-            interface.ep_out_size = tu_edpt_packet_size(desc_ep);
+            interface->ep_out = desc_ep->bEndpointAddress;
+            interface->ep_out_size = tu_edpt_packet_size(desc_ep);
         }
         else
         {
-            interface.ep_in = desc_ep->bEndpointAddress;
-            interface.ep_in_size = tu_edpt_packet_size(desc_ep);
+            interface->ep_in = desc_ep->bEndpointAddress;
+            interface->ep_in_size = tu_edpt_packet_size(desc_ep);
         }
 
         endpoint++;
@@ -301,22 +240,23 @@ static bool open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *
 
 static bool set_config(uint8_t dev_addr, uint8_t itf_num)
 {
+    TU_LOG1("XInput Set Config\r\n");
+    
+    Interface* interface = get_itf_by_itf_num(dev_addr, itf_num);
     uint8_t instance = get_instance_by_itf_num(dev_addr, itf_num);
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(instance != INVALID_IDX && dev_idx != INVALID_IDX);
-    Interface& interface = devices_[dev_idx].interfaces[instance];
+    TU_VERIFY(instance != INVALID_IDX && interface != nullptr);
 
-    interface.connected = true;
+    interface->connected = true;
 
-    switch (interface.dev_type)
+    switch (interface->dev_type)
     {
         case DevType::XBOX360W:
-            interface.connected = false;
+            interface->connected = false;
             send_report(dev_addr, instance, Xbox360W::INQUIRE_PRESENT, sizeof(Xbox360W::INQUIRE_PRESENT));
-            wait_for_tx_complete(dev_addr, interface.ep_out);
+            wait_for_tx_complete(dev_addr, interface->ep_out);
             break;
         case DevType::XBOXONE:
-            xboxone_init(&interface, dev_addr, instance);
+            xboxone_init(interface, dev_addr, instance);
             break;
         default:
             break;
@@ -324,40 +264,39 @@ static bool set_config(uint8_t dev_addr, uint8_t itf_num)
 
     if (mount_cb)
     {
-        mount_cb(dev_addr, instance, &interface);
+        mount_cb(dev_addr, instance, interface);
     }
 
-    usbh_driver_set_config_complete(dev_addr, interface.itf_num);
+    usbh_driver_set_config_complete(dev_addr, interface->itf_num);
     return true;
 }
 
 static bool xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
 {
+    Interface* interface = get_itf_by_ep(dev_addr, ep_addr);
+    uint8_t instance = get_instance_by_itf_num(dev_addr, interface->itf_num);
+    TU_VERIFY(interface != nullptr && instance != INVALID_IDX);
+    
     const uint8_t dir = tu_edpt_dir(ep_addr);
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    uint8_t instance = get_instance_by_ep(dev_addr, ep_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX && instance != INVALID_IDX);
-
-    Interface& interface = devices_[dev_idx].interfaces[instance];
 
     if (result != XFER_RESULT_SUCCESS)
     {
         if (dir == TUSB_DIR_IN)
         {
-            report_received_cb(dev_addr, instance, interface.ep_in_buffer.data(), static_cast<uint16_t>(interface.ep_in_size));
+            report_received_cb(dev_addr, instance, interface->ep_in_buffer.data(), static_cast<uint16_t>(interface->ep_in_size));
         }
         else if (report_sent_cb)
         {
-            report_sent_cb(dev_addr, instance, interface.ep_out_buffer.data(), static_cast<uint16_t>(interface.ep_out_size));
+            report_sent_cb(dev_addr, instance, interface->ep_out_buffer.data(), static_cast<uint16_t>(interface->ep_out_size));
         }
     }
 
     if (dir == TUSB_DIR_IN)
     {
         bool new_pad_data = false;
-        uint8_t* in_buffer = interface.ep_in_buffer.data();
+        uint8_t* in_buffer = interface->ep_in_buffer.data();
 
-        switch (interface.dev_type)
+        switch (interface->dev_type)
         {
             case DevType::XBOX360:
                 if (in_buffer[1] == 0x14)
@@ -368,20 +307,27 @@ static bool xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uin
             case DevType::XBOX360W:
                 if (in_buffer[0] & 0x08)
                 {
-                    if (in_buffer[1] != 0x00 && !interface.connected)
+                    if (in_buffer[1] != 0x00 && !interface->connected)
                     {
-                        interface.connected = true;
-                        // xbox360w_chatpad_init(interface, dev_addr, instance);
+                        interface->connected = true;
+
+                        TU_LOG1("Xbox 360 wireless controller connected\n");
+
+                        //I think some 3rd party adapters need this:
+                        std_sleep_ms(1000);
                         send_report(dev_addr, instance, Xbox360W::RUMBLE_ENABLE, sizeof(Xbox360W::RUMBLE_ENABLE));
+
                         if (xbox360w_connect_cb)
                         {
                             xbox360w_connect_cb(dev_addr, instance);
                         }
                     }
-                    else if (in_buffer[1] == 0x00 && interface.connected)
+                    else if (in_buffer[1] == 0x00 && interface->connected)
                     {
-                        interface.connected = false;
-                        interface.chatpad_inited = false;
+                        interface->connected = false;
+                        interface->chatpad_inited = false;
+
+                        TU_LOG1("Xbox 360 wireless controller disconnected\n");
 
                         if (xbox360w_disconnect_cb)
                         {
@@ -389,7 +335,7 @@ static bool xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uin
                         }
                     }
                 }
-                if ((in_buffer[1] & 1) && in_buffer[5] == 0x13)
+                if ((in_buffer[1] & 1 && in_buffer[5] == 0x13) || (in_buffer[1] & 2))
                 {
                     new_pad_data = true;
                 }
@@ -413,7 +359,7 @@ static bool xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uin
                         }
                         break;
                     case XboxOne::GIP_CMD_ANNOUNCE:
-                        xboxone_init(&interface, dev_addr, instance);
+                        xboxone_init(interface, dev_addr, instance);
                         break;
                 }
                 break;
@@ -439,7 +385,7 @@ static bool xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uin
     {
         if (report_sent_cb)
         {
-            report_sent_cb(dev_addr, instance, interface.ep_out_buffer.data(), static_cast<uint16_t>(xferred_bytes));
+            report_sent_cb(dev_addr, instance, interface->ep_out_buffer.data(), static_cast<uint16_t>(xferred_bytes));
         }
     }
     return true;
@@ -447,30 +393,28 @@ static bool xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uin
 
 bool deinit()
 {
-    TU_LOG2("XInput deinit\r\n");
+    TU_LOG1("XInput deinit\r\n");
     return true;
 }
 
 void close(uint8_t dev_addr)
 {
-    TU_LOG2("XInput close\r\n");
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX, );
-    TU_LOG2("XInput dev addr verified\r\n");
-    Device& device = devices_[dev_idx];
+    TU_LOG1("XInput close\r\n");
 
-    for (uint8_t i = 0; i < device.interfaces.size(); ++i)
+    Device* device = get_device_by_addr(dev_addr);
+    TU_VERIFY(device != nullptr, );
+
+    for (uint8_t i = 0; i < device->interfaces.size(); ++i)
     {
-        if (device.interfaces[i].itf_num != 0xFF && unmount_cb)
+        if (device->interfaces[i].itf_num != 0xFF && unmount_cb)
         {
-            TU_LOG2("XInput unmounting\r\n");
-            unmount_cb(dev_addr, i, &device.interfaces[i]);
-            TU_LOG2("XInput unmount\r\n");
-            device.interfaces[i].itf_num = 0xFF;
-            device.interfaces[i].connected = false;
+            TU_LOG1("XInput unmounting\r\n");
+            unmount_cb(dev_addr, i, &device->interfaces[i]);
+            TU_LOG1("XInput unmount\r\n");
+            device->interfaces[i].itf_num = 0xFF;
+            device->interfaces[i].connected = false;
         }
     }
-    // device = Device();
 }
 
 //Public API
@@ -479,6 +423,11 @@ const usbh_class_driver_t* class_driver()
 {
     static const usbh_class_driver_t class_driver =
     {
+    #if CFG_TUSB_DEBUG >= 2
+        .name       = "XInput",
+    #else
+        .name       = nullptr,
+    #endif
         .init       = init,
         .deinit     = deinit,
         .open       = open,
@@ -491,18 +440,15 @@ const usbh_class_driver_t* class_driver()
 
 bool send_report(uint8_t dev_addr, uint8_t instance, const uint8_t *buffer, uint16_t len)
 {
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX && instance < MAX_INTERFACES, false);
+    Interface* interface = get_itf_by_instance(dev_addr, instance);
+    TU_VERIFY(interface != nullptr);
+    TU_VERIFY(usbh_edpt_claim(dev_addr, interface->ep_out));
 
-    Interface& interface = devices_[dev_idx].interfaces[instance];
+    std::memcpy(interface->ep_out_buffer.data(), buffer, len);
 
-    TU_VERIFY(usbh_edpt_claim(dev_addr, interface.ep_out));
-
-    std::memcpy(interface.ep_out_buffer.data(), buffer, len);
-
-    if (!usbh_edpt_xfer(dev_addr, interface.ep_out, interface.ep_out_buffer.data(), len))
+    if (!usbh_edpt_xfer(dev_addr, interface->ep_out, interface->ep_out_buffer.data(), len))
     {
-        usbh_edpt_release(dev_addr, interface.ep_out);
+        usbh_edpt_release(dev_addr, interface->ep_out);
         return false;
     }
     return true;
@@ -510,14 +456,12 @@ bool send_report(uint8_t dev_addr, uint8_t instance, const uint8_t *buffer, uint
 
 bool receive_report(uint8_t dev_addr, uint8_t instance)
 {
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX && instance < MAX_INTERFACES, false);
+    Interface* interface = get_itf_by_instance(dev_addr, instance);
+    TU_VERIFY(interface != nullptr);
 
-    Interface& interface = devices_[dev_idx].interfaces[instance];
-
-    if (!usbh_edpt_xfer(dev_addr, interface.ep_in, interface.ep_in_buffer.data(), interface.ep_in_size))
+    if (!usbh_edpt_xfer(dev_addr, interface->ep_in, interface->ep_in_buffer.data(), interface->ep_in_size))
     {
-        usbh_edpt_release(dev_addr, interface.ep_in);
+        usbh_edpt_release(dev_addr, interface->ep_in);
         return false;
     }
     return true;
@@ -525,15 +469,13 @@ bool receive_report(uint8_t dev_addr, uint8_t instance)
 
 bool set_led(uint8_t dev_addr, uint8_t instance, uint8_t quadrant, bool block)
 {
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX && instance < MAX_INTERFACES, false);
-    
-    Interface& interface = devices_[dev_idx].interfaces[instance];
+    Interface* interface = get_itf_by_instance(dev_addr, instance);
+    TU_VERIFY(interface != nullptr);
 
     uint8_t buffer[32];
     uint16_t len;
 
-    switch (interface.dev_type)
+    switch (interface->dev_type)
     {
         case DevType::XBOX360W:
             std::memcpy(buffer, Xbox360W::LED, sizeof(Xbox360W::LED));
@@ -552,22 +494,20 @@ bool set_led(uint8_t dev_addr, uint8_t instance, uint8_t quadrant, bool block)
     bool ret = send_report(dev_addr, instance, buffer, len);
     if (block && ret)
     {
-        wait_for_tx_complete(dev_addr, interface.ep_out);
+        wait_for_tx_complete(dev_addr, interface->ep_out);
     }
     return ret;
 }
 
 bool set_rumble(uint8_t dev_addr, uint8_t instance, uint8_t rumble_l, uint8_t rumble_r, bool block)
 {
-    uint8_t dev_idx = get_device_idx_by_addr(dev_addr);
-    TU_VERIFY(dev_idx != INVALID_IDX && instance < MAX_INTERFACES, false);
-
-    Interface& interface = devices_[dev_idx].interfaces[instance];
+    Interface* interface = get_itf_by_instance(dev_addr, instance);
+    TU_VERIFY(interface != nullptr);
 
     uint8_t buffer[32];
     uint16_t len;
 
-    switch (interface.dev_type)
+    switch (interface->dev_type)
     {
         case DevType::XBOX360W:
             std::memcpy(buffer, Xbox360W::RUMBLE, sizeof(Xbox360W::RUMBLE));
@@ -602,718 +542,79 @@ bool set_rumble(uint8_t dev_addr, uint8_t instance, uint8_t rumble_l, uint8_t ru
     bool ret = send_report(dev_addr, instance, buffer, len);
     if (block && ret)
     {
-        wait_for_tx_complete(dev_addr, interface.ep_out);
+        wait_for_tx_complete(dev_addr, interface->ep_out);
+    }
+    return true;
+}
+
+void xbox360_chatpad_init(uint8_t address, uint8_t instance)
+{
+    TU_LOG1("XInput Chatpad Init\r\n");
+
+    Interface* interface = get_itf_by_itf_num(address, instance);
+    TU_VERIFY(interface != nullptr && interface->connected, );
+    TU_VERIFY(interface->dev_type == DevType::XBOX360W, ); //Only supported on Xbox 360 Wireless atm, wired is more complicated
+
+    send_report(address, instance, Xbox360W::CONTROLLER_INFO, sizeof(Xbox360W::CONTROLLER_INFO));
+    wait_for_tx_complete(address, interface->ep_out);
+    send_report(address, instance, Xbox360W::Chatpad::INIT, sizeof(Xbox360W::Chatpad::INIT));
+    wait_for_tx_complete(address, interface->ep_out);
+    send_report(address, instance, Xbox360W::RUMBLE_ENABLE, sizeof(Xbox360W::RUMBLE_ENABLE));
+    wait_for_tx_complete(address, interface->ep_out);
+
+    uint8_t led_ctrl[4];
+    std::memcpy(led_ctrl, Xbox360W::Chatpad::LED_CTRL, sizeof(Xbox360W::Chatpad::LED_CTRL));
+    led_ctrl[2] = Xbox360W::Chatpad::LED_ON[0];
+
+    send_report(address, instance, led_ctrl, sizeof(led_ctrl));
+    wait_for_tx_complete(address, interface->ep_out);
+
+    interface->chatpad_inited = true;
+    interface->chatpad_stage = ChatpadStage::KEEPALIVE_1;
+}
+
+bool xbox360_chatpad_keepalive(uint8_t address, uint8_t instance)
+{   
+    Interface* interface = get_itf_by_itf_num(address, instance);
+    TU_VERIFY(interface != nullptr, false);
+    TU_VERIFY(interface->connected && interface->chatpad_inited, false);
+
+    switch (interface->chatpad_stage)
+    {
+        case ChatpadStage::KEEPALIVE_1:
+            switch (interface->dev_type)
+            {
+                case DevType::XBOX360:
+                    send_ctrl_xfer(interface->dev_addr, &Xbox360::Chatpad::KEEPALIVE_1, nullptr, nullptr, 0);
+                    break;
+                case DevType::XBOX360W:
+                    send_report(interface->dev_addr, instance, Xbox360W::Chatpad::KEEPALIVE_1, sizeof(Xbox360W::Chatpad::KEEPALIVE_1));
+                    break;
+                default:
+                    break;
+            }
+            interface->chatpad_stage = ChatpadStage::KEEPALIVE_2;
+            break;
+        case ChatpadStage::KEEPALIVE_2:
+            switch (interface->dev_type)
+            {
+                case DevType::XBOX360:
+                    send_ctrl_xfer(interface->dev_addr, &Xbox360::Chatpad::KEEPALIVE_2, nullptr, nullptr, 0);
+                    break;
+                case DevType::XBOX360W:
+                    send_report(interface->dev_addr, instance, Xbox360W::Chatpad::KEEPALIVE_2, sizeof(Xbox360W::Chatpad::KEEPALIVE_2));
+                    break;
+                default:
+                    break;
+            }
+            interface->chatpad_stage = ChatpadStage::KEEPALIVE_1;
+            break;
+        default:
+            break;
     }
     return true;
 }
 
 } // namespace tuh_xinput
 
-// #ifndef CFG_TUH_XINPUT_WIRED_CHATPAD_EN
-// #define CFG_TUH_XINPUT_WIRED_CHATPAD_EN 0
-// #endif
-
-// #include <cstring>
-
-// #include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
-// #include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput_cmd.h"
-
-// namespace tuh_xinput {
-
-// static constexpr uint8_t INTERFACE_MULT = (CFG_TUH_XINPUT_WIRED_CHATPAD_EN > 0) ? 2 : 1;
-// static constexpr uint8_t INTERFACE_MAX = CFG_TUH_XINPUT * INTERFACE_MULT;
-
-// enum class ChatpadStage
-// {
-//     INIT_1 = 0,
-//     INIT_2,
-//     INIT_3,
-//     INIT_4,
-//     INIT_5,
-//     INIT_6,
-//     INIT_1E,
-//     INIT_1F,
-//     LED_REQUEST
-// };
-
-// struct Device
-// {
-//     uint8_t inst_count{0};
-//     std::array<Interface, INTERFACE_MAX> interfaces_;
-// };
-
-// static std::array<Device, CFG_TUH_DEVICE_MAX> devices_;
-
-// TU_ATTR_ALWAYS_INLINE static inline Device* get_device(uint8_t dev_addr)
-// {
-//     return &devices_[dev_addr - 1];
-// }
-
-// TU_ATTR_ALWAYS_INLINE static inline Interface* get_interface_by_inst(uint8_t dev_addr, uint8_t instance)
-// {
-//     return &get_device(dev_addr)->interfaces_[instance];
-// }
-
-// TU_ATTR_ALWAYS_INLINE static inline uint8_t get_instance_by_epaddr(uint8_t dev_addr, uint8_t ep_addr)
-// {
-//     for (uint8_t inst = 0; inst < INTERFACE_MAX; inst++)
-//     {
-//         Interface *interface = get_interface_by_inst(dev_addr, inst);
-
-//         if ((ep_addr == interface->ep_in) || (ep_addr == interface->ep_out))
-//         {
-//             return inst;
-//         }
-//     }
-//     return 0xff;
-// }
-
-// TU_ATTR_ALWAYS_INLINE static inline uint8_t get_instance_by_itf(uint8_t dev_addr, uint8_t itf_num)
-// {
-//     for (uint8_t inst = 0; inst < INTERFACE_MAX; inst++)
-//     {
-//         Interface *interface = get_interface_by_inst(dev_addr, inst);
-
-//         if (itf_num == interface->itf_num)
-//         {
-//             return inst;
-//         }
-//     }
-//     return 0xff;
-// }
-
-// static void wait_for_tx_complete(uint8_t dev_addr, uint8_t ep_addr)
-// {
-//     while (usbh_edpt_busy(dev_addr, ep_addr))
-//     {
-//         tuh_task();
-//     }
-// }
-
-// void ctrl_xfer_cb(tuh_xfer_t* xfer) { return; }
-
-// bool send_ctrl_xfer(uint8_t dev_addr, const tusb_control_request_t* setup, uint8_t* buffer, bool block = true)
-// {
-//     tuh_xfer_t xfer = 
-//     {
-//         .daddr = dev_addr,
-//         .ep_addr = 0,
-//         .setup = setup,
-//         .buffer = buffer,
-//         .complete_cb = block ? nullptr : ctrl_xfer_cb,
-//         .user_data = 0
-//     };
-//     if (tuh_control_xfer(&xfer))
-//     {
-//         return true;
-//     }
-//     return false;
-// }
-
-// bool run_tuh_task(uint32_t duration_ms)
-// {
-//     if (!time_in_milliseconds_cb)
-//     {
-//         return false;
-//     }
-//     uint32_t end_time = time_in_milliseconds_cb() + duration_ms;
-//     while (end_time > time_in_milliseconds_cb())
-//     {
-//         tuh_task();
-//     }
-//     return true;
-// }
-
-// static void xboxone_init(Interface *xid_itf, uint8_t dev_addr, uint8_t instance)
-// {
-//     uint16_t pid, vid;
-//     tuh_vid_pid_get(dev_addr, &vid, &pid);
-
-//     send_report(dev_addr, instance, XboxOne::POWER_ON, sizeof(XboxOne::POWER_ON));
-//     wait_for_tx_complete(dev_addr, xid_itf->ep_out);
-//     send_report(dev_addr, instance, XboxOne::S_INIT, sizeof(XboxOne::S_INIT));
-//     wait_for_tx_complete(dev_addr, xid_itf->ep_out);
-
-//     if (vid == 0x045e && (pid == 0x0b00))
-//     {
-//         send_report(dev_addr, instance, XboxOne::EXTRA_INPUT_PACKET_INIT, sizeof(XboxOne::EXTRA_INPUT_PACKET_INIT));
-//         wait_for_tx_complete(dev_addr, xid_itf->ep_out);
-//     }
-
-//     //Required for PDP aftermarket controllers
-//     if (vid == 0x0e6f)
-//     {
-//         send_report(dev_addr, instance, XboxOne::PDP_LED_ON, sizeof(XboxOne::PDP_LED_ON));
-//         wait_for_tx_complete(dev_addr, xid_itf->ep_out);
-//         send_report(dev_addr, instance, XboxOne::PDP_AUTH, sizeof(XboxOne::PDP_AUTH));
-//         wait_for_tx_complete(dev_addr, xid_itf->ep_out);
-//     }
-// }
-
-// static void xbox360w_chatpad_init(uint8_t address, uint8_t instance)
-// {
-//     Interface *xid_itf = get_interface_by_inst(address, instance);
-
-//     send_report(address, instance, Xbox360W::INQUIRE_PRESENT, sizeof(Xbox360W::INQUIRE_PRESENT));
-//     wait_for_tx_complete(address, xid_itf->ep_out);
-//     send_report(address, instance, Xbox360W::CONTROLLER_INFO, sizeof(Xbox360W::CONTROLLER_INFO));
-//     wait_for_tx_complete(address, xid_itf->ep_out);
-//     send_report(address, instance, Xbox360W::Chatpad::INIT, sizeof(Xbox360W::Chatpad::INIT));
-//     wait_for_tx_complete(address, xid_itf->ep_out);
-
-//     uint8_t led_ctrl[4];
-//     std::memcpy(led_ctrl, Xbox360W::Chatpad::LED_CTRL, sizeof(Xbox360W::Chatpad::LED_CTRL));
-//     led_ctrl[2] = Xbox360W::Chatpad::LED_ON[0];
-
-//     send_report(address, instance, led_ctrl, sizeof(led_ctrl));
-//     wait_for_tx_complete(address, xid_itf->ep_out);
-
-//     xid_itf->chatpad_initialized = true;
-// }
-
-// #if (CFG_TUH_XINPUT_WIRED_CHATPAD_EN > 0)
-// static void xbox360_chatpad_init(uint8_t address, uint8_t instance)
-// {
-//     if (!time_in_milliseconds_cb)
-//     {
-//         return;
-//     }
-
-//     Interface *xid_itf = get_interface_by_inst(address, instance);
-//     ChatpadStage init_stage = ChatpadStage::INIT_1;
-
-//     tusb_desc_device_t desc_device;
-//     tuh_descriptor_get_device(address, &desc_device, 18, nullptr, 0);
-
-//     uint8_t buffer[2] = {0x01, 0x02};
-
-//     // if (desc_device.bcdDevice == 0x0114)
-//     // {
-//         buffer[0] = 0x09;
-//         buffer[1] = 0x00;
-//     // }
-
-//     while (!xid_itf->chatpad_initialized)
-//     {
-//         switch (init_stage)
-//         {
-//             case ChatpadStage::INIT_1:
-//                 send_ctrl_xfer(address, &Xbox360::Chatpad::INIT_1, nullptr);
-//                 init_stage = ChatpadStage::INIT_2;
-//                 break;
-//             case ChatpadStage::INIT_2:
-//                 send_ctrl_xfer(address, &Xbox360::Chatpad::INIT_2, nullptr);
-//                 init_stage = ChatpadStage::INIT_3;
-//                 break;
-//             case ChatpadStage::INIT_3:
-//                 send_ctrl_xfer(address, &Xbox360::Chatpad::INIT_3, nullptr);
-//                 init_stage = ChatpadStage::INIT_4;
-//                 break;
-//             case ChatpadStage::INIT_4:
-//                 if (send_ctrl_xfer(address, &Xbox360::Chatpad::INIT_4, buffer))
-//                 {
-//                     init_stage = ChatpadStage::INIT_5;
-//                 }
-//                 break;
-//             case ChatpadStage::INIT_5:
-//                 if (send_ctrl_xfer(address, &Xbox360::Chatpad::INIT_5, buffer))
-//                 {
-//                     init_stage = ChatpadStage::INIT_6;
-//                 }
-//                 break;
-//             case ChatpadStage::INIT_6:
-//                 if (send_ctrl_xfer(address, &Xbox360::Chatpad::INIT_6, buffer))
-//                 {
-//                     init_stage = ChatpadStage::INIT_1E;
-//                 }
-//                 break;
-//             case ChatpadStage::INIT_1E:
-//                 run_tuh_task(1000);
-//                 if (send_ctrl_xfer(address, &Xbox360::Chatpad::KEEPALIVE_2, nullptr))
-//                 {
-//                     init_stage = ChatpadStage::INIT_1F;
-//                 }
-//                 break;
-//             case ChatpadStage::INIT_1F:
-//                 run_tuh_task(1000);
-//                 if (send_ctrl_xfer(address, &Xbox360::Chatpad::KEEPALIVE_1, nullptr))
-//                 {
-//                     init_stage = ChatpadStage::LED_REQUEST;
-//                 }
-//                 break;
-//             case ChatpadStage::LED_REQUEST:
-//                 if (send_ctrl_xfer(address, &Xbox360::Chatpad::LEDS_1B, nullptr))
-//                 {
-//                     xid_itf->chatpad_initialized = true;
-//                 }
-//                 break;
-//         }
-//     }
-// }
-// #endif // CFG_TUH_XINPUT_WIRED_CHATPAD_EN
-
-// //Class driver
-
-// bool xinputh_init()
-// {
-//     for (auto& device : devices_)
-//     {
-//         device.inst_count = 0;
-//         device.interfaces_.fill(Interface());
-//     }
-//     return true;
-// }
-
-// bool xinputh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
-// {
-//     TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX);
-
-//     Type type = Type::UNKNOWN;
-
-//     if (desc_itf->bNumEndpoints < 1)
-//     {
-//         type = Type::UNKNOWN;
-//     }
-//     else if (desc_itf->bInterfaceSubClass == 0x5D && desc_itf->bInterfaceProtocol == 0x81) 
-//     {
-//         type = Type::XBOX360_WIRELESS;
-//     }
-//     else if (desc_itf->bInterfaceSubClass == 0x5D && desc_itf->bInterfaceProtocol == 0x01) 
-//     {
-//         type = Type::XBOX360_WIRED;
-//     }
-// #if (CFG_TUH_XINPUT_WIRED_CHATPAD_EN > 0)
-//     else if (desc_itf->bInterfaceNumber == 2 && desc_itf->bInterfaceSubClass == 0x5d && desc_itf->bInterfaceProtocol == 0x02)
-//     {
-//         uint16_t vid, pid;
-//         tuh_vid_pid_get(dev_addr, &vid, &pid);
-//         if (vid == 0x045e && pid == 0x028e)
-//         {
-//             type = Type::XBOX360_WIRED_CHATPAD;
-//         }
-//     }
-// #endif // CFG_TUH_XINPUT_WIRED_CHATPAD_EN
-//     else if (desc_itf->bInterfaceSubClass == 0x47 && desc_itf->bInterfaceProtocol == 0xD0) 
-//     {
-//         type = Type::XBOXONE;
-//     }
-//     else if (desc_itf->bInterfaceClass == 0x58 && desc_itf->bInterfaceSubClass == 0x42) 
-//     {
-//         type = Type::XBOXOG;
-//     }
-
-//     if (type == Type::UNKNOWN)
-//     {
-//         TU_LOG2("XINPUT: Not a valid interface\n");
-//         return false;
-//     }
-
-//     TU_LOG2("XINPUT opening Interface %u (addr = %u)\r\n", desc_itf->bInterfaceNumber, dev_addr);
-
-//     Device *xinput_dev = get_device(dev_addr);
-//     bool dual_interface = (type == Type::XBOX360_WIRED_CHATPAD || type == Type::XBOX360_WIRED);
-//     TU_ASSERT(xinput_dev->inst_count < (dual_interface ? INTERFACE_MAX : CFG_TUH_XINPUT), 0);
-
-//     Interface *xid_itf = get_interface_by_inst(dev_addr, xinput_dev->inst_count);
-//     xid_itf->itf_num = desc_itf->bInterfaceNumber;
-//     xid_itf->type = type;
-
-//     //Parse descriptor for all endpoints and open them
-//     uint8_t const *p_desc = (uint8_t const *)desc_itf;
-//     int endpoint = 0;
-//     int pos = 0;
-
-//     while (endpoint < desc_itf->bNumEndpoints && pos < max_len)
-//     {
-//         if (tu_desc_type(p_desc) != TUSB_DESC_ENDPOINT)
-//         {
-//             pos += tu_desc_len(p_desc);
-//             p_desc = tu_desc_next(p_desc);
-//             continue;
-//         }
-
-//         tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *)p_desc;
-//         TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType);
-//         TU_ASSERT(tuh_edpt_open(dev_addr, desc_ep));
-
-//         if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_OUT)
-//         {
-//             xid_itf->ep_out = desc_ep->bEndpointAddress;
-//             xid_itf->ep_out_size = static_cast<uint8_t>(tu_edpt_packet_size(desc_ep));
-//         }
-//         else
-//         {
-//             xid_itf->ep_in = desc_ep->bEndpointAddress;
-//             xid_itf->ep_in_size = static_cast<uint8_t>(tu_edpt_packet_size(desc_ep));
-//         }
-
-//         endpoint++;
-//         pos += tu_desc_len(p_desc);
-//         p_desc = tu_desc_next(p_desc);
-//     }
-
-//     xinput_dev->inst_count++;
-//     return true;
-// }
-
-// bool xinputh_set_config(uint8_t dev_addr, uint8_t itf_num)
-// {
-//     uint8_t instance = get_instance_by_itf(dev_addr, itf_num);
-//     Interface *xid_itf = get_interface_by_inst(dev_addr, instance);
-//     xid_itf->connected = true;
-
-//     if (xid_itf->type == Type::XBOX360_WIRELESS)
-//     {
-//         xid_itf->connected = false;
-//         send_report(dev_addr, instance, Xbox360W::INQUIRE_PRESENT, sizeof(Xbox360W::INQUIRE_PRESENT));
-//         wait_for_tx_complete(dev_addr, xid_itf->ep_out);
-//     }
-//     else if (xid_itf->type == Type::XBOX360_WIRED)
-//     {
-//     }
-// #if (CFG_TUH_XINPUT_WIRED_CHATPAD_EN > 0)
-//     else if (xid_itf->type == Type::XBOX360_WIRED_CHATPAD)
-//     {
-//         xbox360_chatpad_init(dev_addr, instance);
-//     }
-// #endif
-//     else if (xid_itf->type == Type::XBOXONE)
-//     {
-//         xboxone_init(xid_itf, dev_addr, instance);
-//     }
-
-//     if (mount_cb)
-//     {
-//         mount_cb(dev_addr, instance, xid_itf);
-//     }
-
-//     usbh_driver_set_config_complete(dev_addr, xid_itf->itf_num);
-//     return true;
-// }
-
-// bool xinputh_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-// {
-//     uint8_t const dir = tu_edpt_dir(ep_addr);
-//     uint8_t const instance = get_instance_by_epaddr(dev_addr, ep_addr);
-//     Interface *xid_itf = get_interface_by_inst(dev_addr, instance);
-//     uint8_t *rdata = xid_itf->ep_in_buffer.data();
-//     bool new_pad_data = false;
-
-//     xid_itf->last_xfer_result = result;
-//     xid_itf->last_xfer_size = xferred_bytes;
-
-//     // On transfer error, bail early but notify the application
-//     if (result != XFER_RESULT_SUCCESS)
-//     {
-//         // if (dir == TUSB_DIR_IN)
-//         // {
-//         //     report_received_cb(dev_addr, instance, xid_itf->ep_in_buffer.data(), xid_itf->ep_in_buffer.size());
-//         // }
-//         // else if (report_sent_cb)
-//         // {
-//         //     report_sent_cb(dev_addr, instance, xid_itf->ep_out_buffer.data(), xferred_bytes);
-//         // }
-//         return false;
-//     }
-
-//     if (xid_itf->type == Type::XBOX360_WIRED_CHATPAD)
-//     {
-//         TU_LOG1("XINPUT: Chatpad data received (%u bytes)\n", xferred_bytes);
-//     }
-
-//     if (dir == TUSB_DIR_IN)
-//     {
-//         TU_LOG2("Get Report callback (%u, %u, %u bytes)\r\n", dev_addr, instance, xferred_bytes);
-
-//         switch (xid_itf->type)
-//         {
-//             case Type::XBOX360_WIRED:
-//                 if (rdata[1] == 0x14)
-//                 {
-//                     std::memcpy(xid_itf->gp_report_buffer.data(), rdata, xferred_bytes);
-//                     xid_itf->gp_report_size = xferred_bytes;
-//                     new_pad_data = true;
-//                 }    
-//                 break;
-//             case Type::XBOX360_WIRELESS:
-//                 if (rdata[0] & 0x08)
-//                 {
-//                     if (rdata[1] != 0x00 && xid_itf->connected == false)
-//                     {
-//                         TU_LOG2("XINPUT: WIRELESS CONTROLLER CONNECTED\n");
-//                         xid_itf->connected = true;
-//                         xbox360w_chatpad_init(dev_addr, instance);
-//                     }
-//                     else if (rdata[1] == 0x00 && xid_itf->connected == true)
-//                     {
-//                         TU_LOG2("XINPUT: WIRELESS CONTROLLER DISCONNECTED\n");
-//                         xid_itf->connected = false;
-//                     }
-//                 }
-//                 if ((rdata[1] & 1) && rdata[5] == 0x13)
-//                 {
-//                     std::memcpy(xid_itf->gp_report_buffer.data(), rdata, xferred_bytes);
-//                     xid_itf->gp_report_size = xferred_bytes;
-//                     new_pad_data = true;
-//                 }
-//                 break;
-//             case Type::XBOXONE:
-//                 if (rdata[0] == XboxOne::GIP_CMD_INPUT)
-//                 {
-//                     std::memcpy(xid_itf->gp_report_buffer.data(), rdata, xferred_bytes);
-//                     xid_itf->gp_report_size = xferred_bytes;
-//                     new_pad_data = true;
-//                 }
-//                 else if (rdata[0] == XboxOne::GIP_CMD_VIRTUAL_KEY)
-//                 {
-//                     if (rdata[4] == 0x01 && !(xid_itf->gp_report_buffer[4] & (1 << 1))) 
-//                     {
-//                         xid_itf->gp_report_buffer[4] |= (1 << 1);
-//                         new_pad_data = true;
-//                     }
-//                     else if (rdata[4] == 0x00 && (xid_itf->gp_report_buffer[4] & (1 << 1))) 
-//                     {
-//                         xid_itf->gp_report_buffer[4] &= ~(1 << 1);
-//                         new_pad_data = true;
-//                     }
-//                 }
-//                 else if (rdata[0] == XboxOne::GIP_CMD_ANNOUNCE)
-//                 {
-//                     xboxone_init(xid_itf, dev_addr, instance);
-//                 }
-//                 break;
-//             case Type::XBOXOG:
-//                 if (rdata[1] == 0x14)
-//                 {
-//                     std::memcpy(xid_itf->gp_report_buffer.data(), rdata, xferred_bytes);
-//                     xid_itf->gp_report_size = xferred_bytes;
-//                     new_pad_data = true;
-//                 }
-//                 break;
-// #if (CFG_TUH_XINPUT_WIRED_CHATPAD_EN > 0)
-//             case Type::XBOX360_WIRED_CHATPAD:
-//                 std::memcpy(xid_itf->gp_report_buffer.data(), rdata, xferred_bytes);
-//                 xid_itf->gp_report_size = xferred_bytes;
-//                 new_pad_data = true;
-//                 break;
-// #endif // CFG_TUH_XINPUT_WIRED_CHATPAD_EN
-//             default:
-//                 break;
-//         }
-
-//         if (new_pad_data)
-//         {
-//             report_received_cb(dev_addr, instance, xid_itf->gp_report_buffer.data(), static_cast<uint16_t>(xid_itf->gp_report_size));
-//         } 
-//         else 
-//         {
-//             receive_report(dev_addr, instance);
-//         }
-//     }
-//     else
-//     {
-//         if (report_sent_cb)
-//         {
-//             report_sent_cb(dev_addr, instance, xid_itf->ep_out_buffer.data(), static_cast<uint16_t>(xferred_bytes));
-//         }
-//     }
-
-//     return true;
-// }
-
-// void xinputh_close(uint8_t dev_addr)
-// {
-//     TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX, );
-//     Device *xinput_dev = get_device(dev_addr);
-
-//     for (uint8_t i = 0; i < xinput_dev->interfaces_.size(); i++)
-//     {
-//         if (umount_cb)
-//         {
-//             umount_cb(dev_addr, i, &xinput_dev->interfaces_[i]);
-//         }
-//     }
-
-//     *xinput_dev = Device();
-//     xinput_dev->interfaces_.fill(Interface());
-// }
-
-// #ifndef DRIVER_NAME
-// #if CFG_TUSB_DEBUG >= CFG_TUH_LOG_LEVEL
-//   #define DRIVER_NAME(_name)    .name = _name,
-// #else
-//   #define DRIVER_NAME(_name)
-// #endif
-// #endif
-
-// const usbh_class_driver_t usbh_xinput_class_driver =
-// {
-//     DRIVER_NAME("XINPUT")
-//     .init       = xinputh_init,
-//     .open       = xinputh_open,
-//     .set_config = xinputh_set_config,
-//     .xfer_cb    = xinputh_xfer_cb,
-//     .close      = xinputh_close
-// };
-
-// //Public API
-
-// const usbh_class_driver_t* class_driver()
-// {
-//     return &usbh_xinput_class_driver;
-// }
-
-// const Interface& get_interface(uint8_t address, uint8_t instance)
-// {
-//     return *get_interface_by_inst(address, instance);
-// }
-
-// bool receive_report(uint8_t dev_addr, uint8_t instance)
-// {
-//     Interface *xid_itf = get_interface_by_inst(dev_addr, instance);
-//     TU_VERIFY(usbh_edpt_claim(dev_addr, xid_itf->ep_in));
-
-//     if (!usbh_edpt_xfer(dev_addr, xid_itf->ep_in, xid_itf->ep_in_buffer.data(), xid_itf->ep_in_size))
-//     {
-//         usbh_edpt_release(dev_addr, xid_itf->ep_in);
-//         return false;
-//     }
-//     return true;
-// }
-
-// bool send_report(uint8_t dev_addr, uint8_t instance, const uint8_t *report, uint16_t len)
-// {
-//     Interface *xid_itf = get_interface_by_inst(dev_addr, instance);
-
-//     TU_ASSERT(len <= xid_itf->ep_out_size);
-//     TU_VERIFY(usbh_edpt_claim(dev_addr, xid_itf->ep_out));
-
-//     std::memcpy(xid_itf->ep_out_buffer.data(), report, len);
-
-//     if (!usbh_edpt_xfer(dev_addr, xid_itf->ep_out, xid_itf->ep_out_buffer.data(), len))
-//     {
-//         usbh_edpt_release(dev_addr, xid_itf->ep_out);
-//         return false;
-//     }
-//     return true;
-// }
-
-// bool set_led(uint8_t dev_addr, uint8_t instance, uint8_t led_number, bool block)
-// {
-//     Interface *xid_itf = get_interface_by_inst(dev_addr, instance);
-//     uint8_t buffer[32];
-//     uint16_t len;
-
-//     switch (xid_itf->type)
-//     {
-//         case Type::XBOX360_WIRELESS:
-//             std::memcpy(buffer, Xbox360W::LED, sizeof(Xbox360W::LED));
-//             buffer[3] = (led_number == 0) ? 0x40 : (0x40 | (led_number + 5));
-//             len = sizeof(Xbox360W::LED);
-//             break;
-//         case Type::XBOX360_WIRED:
-//             std::memcpy(buffer, Xbox360::LED, sizeof(Xbox360::LED));
-//             buffer[2] = (led_number == 0) ? 0 : (led_number + 5);
-//             len = sizeof(Xbox360::LED);
-//             break;
-//         default:
-//             return true;
-//     }
-//     bool ret = send_report(dev_addr, instance, buffer, len);
-//     if (block && ret)
-//     {
-//         wait_for_tx_complete(dev_addr, xid_itf->ep_out);
-//     }
-//     return ret;
-// }
-
-// bool set_rumble(uint8_t dev_addr, uint8_t instance, uint8_t rumble_l, uint8_t rumble_r, bool block)
-// {
-//     Interface *xid_itf = get_interface_by_inst(dev_addr, instance);
-//     uint8_t buffer[32];
-//     uint16_t len;
-
-//     switch (xid_itf->type)
-//     {
-//         case Type::XBOX360_WIRELESS:
-//             std::memcpy(buffer, Xbox360W::RUMBLE, sizeof(Xbox360W::RUMBLE));
-//             buffer[5] = rumble_l;
-//             buffer[6] = rumble_r;
-//             len = sizeof(Xbox360W::RUMBLE);
-//             break;
-//         case Type::XBOX360_WIRED:
-//             std::memcpy(buffer, Xbox360::RUMBLE, sizeof(Xbox360::RUMBLE));
-//             buffer[3] = rumble_l;
-//             buffer[4] = rumble_r;
-//             len = sizeof(Xbox360::RUMBLE);
-//             break;
-//         case Type::XBOXONE:
-//             std::memcpy(buffer, XboxOne::RUMBLE, sizeof(XboxOne::RUMBLE));
-//             buffer[8] = rumble_l / 2; // 0 - 128
-//             buffer[9] = rumble_r / 2; // 0 - 128
-//             len = sizeof(XboxOne::RUMBLE);
-//             break;
-//         case Type::XBOXOG:
-//             std::memcpy(buffer, XboxOG::RUMBLE, sizeof(XboxOG::RUMBLE));
-//             buffer[2] = rumble_l;
-//             buffer[3] = rumble_l;
-//             buffer[4] = rumble_r;
-//             buffer[5] = rumble_r;
-//             len = sizeof(XboxOG::RUMBLE);
-//             break;
-//         default:
-//             return true;
-//     }
-//     bool ret = send_report(dev_addr, instance, buffer, len);
-//     if (block && ret)
-//     {
-//         wait_for_tx_complete(dev_addr, xid_itf->ep_out);
-//     }
-//     return true;
-// }
-
-// //Call every TUHXInput::CHATPAD_KEEPALIVE_MS if using chatpad
-// void chatpad_keepalive(uint8_t address, uint8_t instance)
-// {
-//     Interface *xid_itf = get_interface_by_inst(address, instance);
-//     if ((xid_itf->type != Type::XBOX360_WIRED_CHATPAD && xid_itf->type != Type::XBOX360_WIRELESS) ||
-//         !xid_itf->chatpad_initialized)
-//     {
-//         return;
-//     }
-//     switch (xid_itf->chatpad_keepalive)
-//     {
-//         case KeepaliveStage::KEEPALIVE_1:
-//             switch (xid_itf->type)
-//             {
-//                 case Type::XBOX360_WIRED_CHATPAD:
-//                     send_ctrl_xfer(address, &Xbox360::Chatpad::KEEPALIVE_1, nullptr, false);
-//                     break;
-//                 case Type::XBOX360_WIRELESS:
-//                     send_report(address, instance, Xbox360W::Chatpad::KEEPALIVE_1, sizeof(Xbox360W::Chatpad::KEEPALIVE_1));
-//                     break;
-//                 default:
-//                     break;
-//             }
-//             xid_itf->chatpad_keepalive = KeepaliveStage::KEEPALIVE_2;
-//             break;
-//         case KeepaliveStage::KEEPALIVE_2:
-//             switch (xid_itf->type)
-//             {
-//                 case Type::XBOX360_WIRED_CHATPAD:
-//                     send_ctrl_xfer(address, &Xbox360::Chatpad::KEEPALIVE_2, nullptr, false);
-//                     break;
-//                 case Type::XBOX360_WIRELESS:
-//                     send_report(address, instance, Xbox360W::Chatpad::KEEPALIVE_2, sizeof(Xbox360W::Chatpad::KEEPALIVE_2));
-//                     break;
-//                 default:
-//                     break;
-//             }
-//             xid_itf->chatpad_keepalive = KeepaliveStage::KEEPALIVE_1;
-//             break;
-//     }
-// }
-
-// }; // namespace TUHXInput
-
 #endif // (TUSB_OPT_HOST_ENABLED && CFG_TUH_XINPUT)
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h b/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h
index 83c4f5f..76d46bc 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h
@@ -72,8 +72,8 @@ namespace tuh_xinput
         uint8_t ep_in{0xFF};
         uint8_t ep_out{0xFF};
 
-        uint8_t ep_in_size{0xFF};
-        uint8_t ep_out_size{0xFF};
+        uint16_t ep_in_size{0xFF};
+        uint16_t ep_out_size{0xFF};
 
         std::array<uint8_t, ENDPOINT_SIZE> ep_in_buffer{0};
         std::array<uint8_t, ENDPOINT_SIZE> ep_out_buffer{0};
@@ -82,10 +82,14 @@ namespace tuh_xinput
     // API
 
     const usbh_class_driver_t* class_driver();
+
     bool send_report(uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len);
     bool receive_report(uint8_t address, uint8_t instance);
     bool set_rumble(uint8_t address, uint8_t instance, uint8_t rumble_l, uint8_t rumble_r, bool block);
     bool set_led(uint8_t address, uint8_t instance, uint8_t led_number, bool block);
+
+    //Wireless only atm
+    void xbox360_chatpad_init(uint8_t address, uint8_t instance); 
     bool xbox360_chatpad_keepalive(uint8_t address, uint8_t instance);
 
     // User implemented callbacks
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput_cmd.h b/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput_cmd.h
index 7265920..17c2bab 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput_cmd.h
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput_cmd.h
@@ -77,10 +77,10 @@ namespace tuh_xinput
             static constexpr tusb_control_request_t INIT_5   = { .bmRequestType = 0x40, .bRequest = 0xA1, .wValue = 0x0000, .wIndex = 0xE416, .wLength = 2 };
             static constexpr tusb_control_request_t INIT_6   = { .bmRequestType = 0xC0, .bRequest = 0xA1, .wValue = 0x0000, .wIndex = 0xE416, .wLength = 2 };
             //wValue in xbox360_wired_chatpad_command can be replaced with CHATPAD_CMD constants for different functions
-            static constexpr tusb_control_request_t CMD          = { .bmRequestType = 0x41, .bRequest = 0x00, .wValue = 0x0000, .wIndex = 0x0002, .wLength = 0 };
+            static constexpr tusb_control_request_t CMD         = { .bmRequestType = 0x41, .bRequest = 0x00, .wValue = 0x0000, .wIndex = 0x0002, .wLength = 0 };
             static constexpr tusb_control_request_t KEEPALIVE_1 = { .bmRequestType = 0x41, .bRequest = 0x00, .wValue = 0x001F, .wIndex = 0x0002, .wLength = 0 };
             static constexpr tusb_control_request_t KEEPALIVE_2 = { .bmRequestType = 0x41, .bRequest = 0x00, .wValue = 0x001E, .wIndex = 0x0002, .wLength = 0 };
-            static constexpr tusb_control_request_t LEDS_1B      = { .bmRequestType = 0x41, .bRequest = 0x00, .wValue = 0x001B, .wIndex = 0x0002, .wLength = 0 };
+            static constexpr tusb_control_request_t LEDS_1B     = { .bmRequestType = 0x41, .bRequest = 0x00, .wValue = 0x001B, .wIndex = 0x0002, .wLength = 0 };
         }
     }
 
diff --git a/Firmware/RP2040/src/USBHost/HostManager.h b/Firmware/RP2040/src/USBHost/HostManager.h
index 05e92e5..efa3622 100644
--- a/Firmware/RP2040/src/USBHost/HostManager.h
+++ b/Firmware/RP2040/src/USBHost/HostManager.h
@@ -1,3 +1,383 @@
+#ifndef _HOST_MANAGER_H_
+#define _HOST_MANAGER_H_
+
+#include <cstdint>
+#include <memory>	
+#include <hardware/regs/usb.h>
+#include <hardware/irq.h>
+#include <hardware/structs/usb.h>
+#include <hardware/resets.h>
+
+#include "board_config.h"
+#include "Gamepad.h"
+#include "OGXMini/OGXMini.h"
+#include "USBHost/HardwareIDs.h"
+#include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
+#include "USBHost/HostDriver/HostDriver.h"
+#include "USBHost/HostDriver/PS5/PS5.h"
+#include "USBHost/HostDriver/PS4/PS4.h"
+#include "USBHost/HostDriver/PS3/PS3.h"
+#include "USBHost/HostDriver/PSClassic/PSClassic.h"
+#include "USBHost/HostDriver/DInput/DInput.h"
+#include "USBHost/HostDriver/SwitchWired/SwitchWired.h"
+#include "USBHost/HostDriver/SwitchPro/SwitchPro.h"
+#include "USBHost/HostDriver/XInput/XboxOne.h"
+#include "USBHost/HostDriver/XInput/Xbox360.h"
+#include "USBHost/HostDriver/XInput/Xbox360W.h"
+#include "USBHost/HostDriver/XInput/XboxOG.h"
+#include "USBHost/HostDriver/N64/N64.h"
+#include "USBHost/HostDriver/HIDGeneric/HIDGeneric.h"
+
+#define MAX_INTERFACES MAX_GAMEPADS //This may change if support is added for audio or other chatpads beside 360 wireless
+
+class HostManager 
+{
+public:
+	enum class DriverClass { NONE, HID, XINPUT };
+
+	HostManager(HostManager const&) = delete;
+	void operator=(HostManager const&)  = delete;
+
+    static HostManager& get_instance() 
+    {
+		static HostManager instance;
+		return instance;
+	}
+
+	inline void initialize(Gamepad (&gamepads)[MAX_GAMEPADS]) 
+	{ 
+		for (size_t i = 0; i < MAX_GAMEPADS; ++i)
+		{
+			gamepads_[i] = &gamepads[i];
+		}
+	}
+
+	//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)
+	{
+		uint8_t gp_idx = find_free_gamepad();
+		if (gp_idx == INVALID_IDX || instance >= MAX_INTERFACES)
+		{
+			return false;
+		}
+
+		//Check if this device is already mounted
+		uint8_t dev_idx = get_device_slot(address);
+		//If not, find a free device slot
+		if (dev_idx == INVALID_IDX && ((dev_idx = find_free_device_slot()) == INVALID_IDX)) 
+		{
+			return false;
+		}
+
+		Device& device_slot = device_slots_[dev_idx];
+		Interface& interface = device_slot.interfaces[instance];
+
+		switch (driver_type)
+		{
+			case HostDriver::Type::PS5:
+				interface.driver = std::make_unique<PS5Host>(gp_idx);
+				break;
+			case HostDriver::Type::PS4:
+				interface.driver = std::make_unique<PS4Host>(gp_idx);
+				break;
+			case HostDriver::Type::PS3:
+				interface.driver = std::make_unique<PS3Host>(gp_idx);
+				break;
+			case HostDriver::Type::DINPUT:
+				interface.driver = std::make_unique<DInputHost>(gp_idx);
+				break;
+			case HostDriver::Type::SWITCH:
+				interface.driver = std::make_unique<SwitchWiredHost>(gp_idx);
+				break;
+			case HostDriver::Type::SWITCH_PRO:
+				interface.driver = std::make_unique<SwitchProHost>(gp_idx);
+				break;
+			case HostDriver::Type::N64:
+				interface.driver = std::make_unique<N64Host>(gp_idx);
+				break;
+			case HostDriver::Type::PSCLASSIC:
+				interface.driver = std::make_unique<PSClassicHost>(gp_idx);
+				break;
+			case HostDriver::Type::XBOXOG:
+				interface.driver = std::make_unique<XboxOGHost>(gp_idx);
+				break;
+			case HostDriver::Type::XBOXONE:
+				interface.driver = std::make_unique<XboxOneHost>(gp_idx);
+				break;
+			case HostDriver::Type::XBOX360:
+				interface.driver = std::make_unique<Xbox360Host>(gp_idx);
+				break;
+			case HostDriver::Type::XBOX360W: //Composite device, takes up all 4 gamepads when mounted
+				interface.driver = std::make_unique<Xbox360WHost>(gp_idx);
+				break;
+			default:
+				if (is_hid_gamepad(report_desc, desc_len))
+				{
+					interface.driver = std::make_unique<HIDHost>(gp_idx);
+				}
+				else
+				{
+					return false;
+				}
+				break;
+		}
+
+		device_slot.address = address;
+		interface.gamepad_idx = gp_idx;
+		interface.gamepad = gamepads_[gp_idx];
+		interface.driver->initialize(*interface.gamepad, device_slot.address, instance, report_desc, desc_len);
+
+		return true;
+	}
+
+	inline void process_report(DriverClass driver_class, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
+	{
+		for (auto& device_slot : device_slots_)
+		{
+			if (device_slot.address == address && 
+				device_slot.interfaces[instance].driver &&
+				device_slot.interfaces[instance].gamepad)
+			{
+				device_slot.interfaces[instance].driver->process_report(*device_slot.interfaces[instance].gamepad, address, instance, report, len);
+			}
+		}
+	}
+
+	inline void connect_cb(DriverClass driver_class, uint8_t address, uint8_t instance)
+	{
+		for (auto& device_slot : device_slots_)
+		{
+			if (device_slot.address == address && 
+				device_slot.interfaces[instance].driver &&
+				device_slot.interfaces[instance].gamepad)
+			{
+				device_slot.interfaces[instance].driver->connect_cb(*device_slot.interfaces[instance].gamepad, address, instance);
+			}
+		}
+	}
+
+	inline void disconnect_cb(DriverClass driver_class, uint8_t address, uint8_t instance)
+	{
+		for (auto& device_slot : device_slots_)
+		{
+			if (device_slot.address == address && 
+				device_slot.interfaces[instance].driver &&
+				device_slot.interfaces[instance].gamepad)
+			{
+				device_slot.interfaces[instance].driver->disconnect_cb(*device_slot.interfaces[instance].gamepad, address, instance);
+			}
+		}
+	}
+
+	//Call on a timer
+	inline void send_feedback()
+	{
+		for (auto& device_slot : device_slots_)
+		{
+			if (device_slot.address == INVALID_IDX)
+			{
+				continue;
+			}
+			for (uint8_t i = 0; i < MAX_INTERFACES; ++i)
+			{
+				if (device_slot.interfaces[i].driver != nullptr && 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();
+				}
+			}
+		}
+	}
+
+    void deinit_driver(DriverClass driver_class, uint8_t address, uint8_t instance)
+	{
+		for (auto& device_slot : device_slots_)
+		{
+			if (device_slot.address == address)
+			{
+				TU_LOG2("Deinit driver\r\n");
+				device_slot.reset();
+				TU_LOG2("Driver deinitialized\r\n");
+			}
+		}
+	}
+
+	static inline HostDriver::Type get_type(const HardwareID& ids)
+	{
+		for (const auto& map : HOST_TYPE_MAP)
+		{
+			for (size_t i = 0; i < map.num_ids; i++)
+			{
+				if (ids.pid == map.ids[i].pid && ids.vid == map.ids[i].vid)
+				{
+					return map.type;
+				}
+			}
+		}
+		return HostDriver::Type::UNKNOWN;
+	}
+
+	static inline HostDriver::Type get_type(const tuh_xinput::DevType xinput_type)
+	{
+		switch (xinput_type)
+		{
+			case tuh_xinput::DevType::XBOXONE:
+				return HostDriver::Type::XBOXONE;
+			case tuh_xinput::DevType::XBOX360W:
+				return HostDriver::Type::XBOX360W;
+			case tuh_xinput::DevType::XBOX360:
+				return HostDriver::Type::XBOX360;
+			// case tuh_xinput::DevType::XBOX360_CHATPAD:
+			// 	return HostDriver::Type::XBOX360_CHATPAD;
+			case tuh_xinput::DevType::XBOXOG:
+				return HostDriver::Type::XBOXOG;
+			default:
+				return HostDriver::Type::UNKNOWN;
+		}
+	}
+
+	inline uint8_t get_gamepad_idx(DriverClass driver_class, uint8_t address, uint8_t instance)
+	{
+		for (auto& device_slot : device_slots_)
+		{
+			if (device_slot.address == address && instance < MAX_INTERFACES)
+			{
+				return device_slot.interfaces[instance].gamepad_idx;
+			}
+		}
+		return INVALID_IDX;
+	}
+
+	inline bool any_mounted() 
+	{ 
+		for (auto& device_slot : device_slots_)
+		{
+			if (device_slot.address != INVALID_IDX)
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
+private:
+	static constexpr uint8_t INVALID_IDX = 0xFF;
+
+	struct Interface
+	{
+		std::unique_ptr<HostDriver> driver{nullptr};
+		Gamepad* gamepad{nullptr};
+		uint8_t gamepad_idx{INVALID_IDX};
+	};
+	struct Device
+	{
+		uint8_t address{INVALID_IDX};
+		Interface interfaces[MAX_INTERFACES];
+
+		void reset()
+		{
+			address = INVALID_IDX;
+			for (auto& interface : interfaces)
+			{
+				interface.driver.reset();
+				interface.gamepad_idx = INVALID_IDX;
+				interface.gamepad = nullptr;
+			}
+		}
+	};
+
+	Device device_slots_[MAX_GAMEPADS];
+	Gamepad* gamepads_[MAX_GAMEPADS];
+
+    HostManager() {}
+
+	inline uint8_t find_free_device_slot()
+	{
+		for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
+		{
+			if (device_slots_[i].address == INVALID_IDX)
+			{
+				return i;
+			}
+		}
+		return INVALID_IDX;
+	}
+
+	inline uint8_t find_free_gamepad()
+	{
+		uint8_t count = 0;
+
+		for (auto& device_slot : device_slots_)
+		{
+			for (auto& interface : device_slot.interfaces)
+			{
+				if (interface.gamepad_idx != INVALID_IDX)
+				{
+					++count;
+				}
+			}
+		}
+		return (count < MAX_GAMEPADS) ? count : INVALID_IDX;
+	}
+
+	inline uint8_t get_device_slot(uint8_t address)
+	{
+		if (address > MAX_GAMEPADS)
+		{
+			return INVALID_IDX;
+		}
+		return address - 1;
+	}
+
+	// inline DriverClass determine_driver_class(HostDriver::Type host_type)
+	// {
+	// 	switch (host_type)
+	// 	{
+	// 		case HostDriver::Type::XBOXOG:
+	// 		case HostDriver::Type::XBOXONE:
+	// 		case HostDriver::Type::XBOX360:
+	// 		case HostDriver::Type::XBOX360W:
+	// 			return DriverClass::XINPUT;
+	// 		default:
+	// 			return DriverClass::HID;
+	// 	}
+	// }
+
+	bool is_hid_gamepad(const uint8_t* report_desc, uint16_t desc_len)
+	{
+		std::array<uint8_t, 6> start_bytes = { 0x05, 0x01, 0x09, 0x05, 0xA1, 0x01 };
+		if (desc_len < start_bytes.size())
+		{
+			return false;
+		}
+		for (size_t i = 0; i < start_bytes.size(); ++i)
+		{
+			if (report_desc[i] != start_bytes[i])
+			{
+				return false;
+			}
+		}
+		return true;
+	}
+
+	inline HostDriver* get_driver_by_gamepad(uint8_t gamepad_idx)
+	{
+		for (const auto& device_slot : device_slots_)
+		{
+			for (const auto& interface : device_slot.interfaces)
+			{
+				if (interface.gamepad_idx == gamepad_idx)
+				{
+					return interface.driver.get();
+				}
+			}
+		}
+		return nullptr;
+	}
+};
+
+#endif // _HOST_MANAGER_H_
+
 // #ifndef _HOST_MANAGER_H_
 // #define _HOST_MANAGER_H_
 
@@ -361,368 +741,4 @@
 // 	}
 // };
 
-// #endif // _HOST_MANAGER_H_
-
-#ifndef _HOST_MANAGER_H_
-#define _HOST_MANAGER_H_
-
-#include <cstdint>
-#include <memory>	
-#include <hardware/regs/usb.h>
-#include <hardware/irq.h>
-#include <hardware/structs/usb.h>
-#include <hardware/resets.h>
-
-#include "board_config.h"
-#include "Gamepad.h"
-#include "OGXMini/OGXMini.h"
-#include "USBHost/HardwareIDs.h"
-#include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
-#include "USBHost/HostDriver/HostDriver.h"
-// #include "USBHost/HostDriver/PS5/PS5.h"
-// #include "USBHost/HostDriver/PS4/PS4.h"
-// #include "USBHost/HostDriver/PS3/PS3.h"
-// #include "USBHost/HostDriver/PSClassic/PSClassic.h"
-// #include "USBHost/HostDriver/DInput/DInput.h"
-// #include "USBHost/HostDriver/SwitchWired/SwitchWired.h"
-// #include "USBHost/HostDriver/SwitchPro/SwitchPro.h"
-#include "USBHost/HostDriver/XInput/XboxOne.h"
-#include "USBHost/HostDriver/XInput/Xbox360.h"
-#include "USBHost/HostDriver/XInput/Xbox360W.h"
-#include "USBHost/HostDriver/XInput/XboxOG.h"
-// #include "USBHost/HostDriver/N64/N64.h"
-// #include "USBHost/HostDriver/HIDGeneric/HIDGeneric.h"
-
-#define MAX_INTERFACES MAX_GAMEPADS //This may change if support is added for audio or other chatpads beside 360 wireless
-
-class HostManager 
-{
-public:
-	enum class DriverClass { NONE, HID, XINPUT };
-
-	HostManager(HostManager const&) = delete;
-	void operator=(HostManager const&)  = delete;
-
-    static HostManager& get_instance() 
-    {
-		static HostManager instance;
-		return instance;
-	}
-
-	inline void initialize(Gamepad (&gamepads)[MAX_GAMEPADS]) 
-	{ 
-		for (size_t i = 0; i < MAX_GAMEPADS; ++i)
-		{
-			gamepads_[i] = &gamepads[i];
-		}
-	}
-
-	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)
-	{
-		uint8_t gp_idx = find_free_gamepad();
-		if (gp_idx == INVALID_IDX || instance >= MAX_INTERFACES)
-		{
-			return false;
-		}
-
-		// DriverClass driver_class = determine_driver_class(driver_type);
-		uint8_t dev_idx = get_device_slot(address);
-
-		//This is a new device, else it's another interface on an already mounted device
-		if (dev_idx == INVALID_IDX && ((dev_idx = find_free_device_slot()) == INVALID_IDX)) 
-		{
-			return false;
-		}
-
-		Device& device_slot = device_slots_[dev_idx];
-		Interface& interface = device_slot.interfaces[instance];
-
-		switch (driver_type)
-		{
-			// case HostDriver::Type::PS5:
-			// 	interface.driver = std::make_unique<PS5Host>(gp_idx);
-			// 	break;
-			// case HostDriver::Type::PS4:
-			// 	interface.driver = std::make_unique<PS4Host>(gp_idx);
-			// 	break;
-			// case HostDriver::Type::PS3:
-			// 	interface.driver = std::make_unique<PS3Host>(gp_idx);
-			// 	break;
-			// case HostDriver::Type::DINPUT:
-			// 	interface.driver = std::make_unique<DInputHost>(gp_idx);
-			// 	break;
-			// case HostDriver::Type::SWITCH:
-			// 	interface.driver = std::make_unique<SwitchWiredHost>(gp_idx);
-			// 	break;
-			// case HostDriver::Type::SWITCH_PRO:
-			// 	interface.driver = std::make_unique<SwitchProHost>(gp_idx);
-			// 	break;
-			// case HostDriver::Type::N64:
-			// 	interface.driver = std::make_unique<N64Host>(gp_idx);
-			// 	break;
-			// case HostDriver::Type::PSCLASSIC:
-			// 	interface.driver = std::make_unique<PSClassicHost>(gp_idx);
-			// 	break;
-			case HostDriver::Type::XBOXOG:
-				interface.driver = std::make_unique<XboxOGHost>(gp_idx);
-				break;
-			case HostDriver::Type::XBOXONE:
-				interface.driver = std::make_unique<XboxOneHost>(gp_idx);
-				break;
-			case HostDriver::Type::XBOX360:
-				interface.driver = std::make_unique<Xbox360Host>(gp_idx);
-				break;
-			case HostDriver::Type::XBOX360W: //Composite device, takes up all 4 gamepads when mounted
-				interface.driver = std::make_unique<Xbox360WHost>(gp_idx);
-				break;
-			default:
-				// if (is_hid_gamepad(report_desc, desc_len))
-				// {
-				// 	interface.driver = std::make_unique<HIDHost>(gp_idx);
-				// }
-				// else
-				{
-					return false;
-				}
-				break;
-		}
-
-		device_slot.address = address;
-		interface.gamepad_idx = gp_idx;
-		interface.gamepad = gamepads_[gp_idx];
-		interface.driver->initialize(*interface.gamepad, device_slot.address, instance, report_desc, desc_len);
-
-		return true;
-	}
-
-	inline void process_report(DriverClass driver_class, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
-	{
-		for (auto& device_slot : device_slots_)
-		{
-			if (device_slot.address == address && 
-				device_slot.interfaces[instance].driver &&
-				device_slot.interfaces[instance].gamepad)
-			{
-				device_slot.interfaces[instance].driver->process_report(*device_slot.interfaces[instance].gamepad, address, instance, report, len);
-			}
-		}
-	}
-
-	//Call on a timer
-	inline void send_feedback()
-	{
-		for (auto& device_slot : device_slots_)
-		{
-			if (device_slot.address == INVALID_IDX)
-			{
-				continue;
-			}
-			for (uint8_t i = 0; i < MAX_INTERFACES; ++i)
-			{
-				if (device_slot.interfaces[i].driver != nullptr && 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();
-				}
-			}
-		}
-	}
-
-    void deinit_driver(DriverClass driver_class, uint8_t address, uint8_t instance)
-	{
-		for (auto& device_slot : device_slots_)
-		{
-			if (device_slot.address == address)
-			{
-				TU_LOG2("Deinit driver\r\n");
-				device_slot.reset();
-				TU_LOG2("Driver deinitialized\r\n");
-			}
-		}
-
-		// OGXMini::update_tuh_status(any_mounted());
-	}
-
-	static inline HostDriver::Type get_type(const HardwareID& ids)
-	{
-		for (const auto& map : HOST_TYPE_MAP)
-		{
-			for (size_t i = 0; i < map.num_ids; i++)
-			{
-				if (ids.pid == map.ids[i].pid && ids.vid == map.ids[i].vid)
-				{
-					return map.type;
-				}
-			}
-		}
-		return HostDriver::Type::UNKNOWN;
-	}
-
-	static inline HostDriver::Type get_type(const tuh_xinput::DevType xinput_type)
-	{
-		switch (xinput_type)
-		{
-			case tuh_xinput::DevType::XBOXONE:
-				return HostDriver::Type::XBOXONE;
-			case tuh_xinput::DevType::XBOX360W:
-				return HostDriver::Type::XBOX360W;
-			case tuh_xinput::DevType::XBOX360:
-				return HostDriver::Type::XBOX360;
-			// case tuh_xinput::DevType::XBOX360_CHATPAD:
-			// 	return HostDriver::Type::XBOX360_CHATPAD;
-			case tuh_xinput::DevType::XBOXOG:
-				return HostDriver::Type::XBOXOG;
-			default:
-				return HostDriver::Type::UNKNOWN;
-		}
-	}
-
-	inline uint8_t get_gamepad_idx(DriverClass driver_class, uint8_t address, uint8_t instance)
-	{
-		for (auto& device_slot : device_slots_)
-		{
-			if (device_slot.address == address && instance < MAX_INTERFACES)
-			{
-				return device_slot.interfaces[instance].gamepad_idx;
-			}
-		}
-		return INVALID_IDX;
-	}
-
-	inline bool any_mounted() 
-	{ 
-		for (auto& device_slot : device_slots_)
-		{
-			if (device_slot.address != INVALID_IDX)
-			{
-				return true;
-			}
-		}
-		return false;
-	}
-
-private:
-	static constexpr uint8_t INVALID_IDX = 0xFF;
-
-	struct Interface
-	{
-		std::unique_ptr<HostDriver> driver{nullptr};
-		Gamepad* gamepad{nullptr};
-		uint8_t gamepad_idx{INVALID_IDX};
-	};
-	struct Device
-	{
-		uint8_t address{INVALID_IDX};
-		Interface interfaces[MAX_INTERFACES];
-
-		void reset()
-		{
-			address = INVALID_IDX;
-			for (auto& interface : interfaces)
-			{
-				interface.driver.reset();
-				interface.gamepad_idx = INVALID_IDX;
-				interface.gamepad = nullptr;
-			}
-		}
-	};
-
-	Device device_slots_[MAX_GAMEPADS];
-	Gamepad* gamepads_[MAX_GAMEPADS];
-
-    HostManager() 
-	{
-		// irq_set_exclusive_handler(USBCTRL_IRQ, disconnect_irq_handler);
-		// irq_set_enabled(USBCTRL_IRQ, true);
-	}
-
-	inline uint8_t find_free_device_slot()
-	{
-		for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-		{
-			if (device_slots_[i].address == INVALID_IDX)
-			{
-				return i;
-			}
-		}
-		return INVALID_IDX;
-	}
-
-	inline uint8_t find_free_gamepad()
-	{
-		uint8_t count = 0;
-
-		for (auto& device_slot : device_slots_)
-		{
-			for (auto& interface : device_slot.interfaces)
-			{
-				if (interface.gamepad_idx != INVALID_IDX)
-				{
-					++count;
-				}
-			}
-		}
-		return (count < MAX_GAMEPADS) ? count : INVALID_IDX;
-	}
-
-	inline uint8_t get_device_slot(uint8_t address)
-	{
-		if (address > MAX_GAMEPADS)
-		{
-			return INVALID_IDX;
-		}
-		return address - 1;
-	}
-
-	// inline DriverClass determine_driver_class(HostDriver::Type host_type)
-	// {
-	// 	switch (host_type)
-	// 	{
-	// 		case HostDriver::Type::XBOXOG:
-	// 		case HostDriver::Type::XBOXONE:
-	// 		case HostDriver::Type::XBOX360:
-	// 		case HostDriver::Type::XBOX360W:
-	// 			return DriverClass::XINPUT;
-	// 		default:
-	// 			return DriverClass::HID;
-	// 	}
-	// }
-
-	bool is_hid_gamepad(const uint8_t* report_desc, uint16_t desc_len)
-	{
-		std::array<uint8_t, 6> start_bytes = { 0x05, 0x01, 0x09, 0x05, 0xA1, 0x01 };
-		if (desc_len < start_bytes.size())
-		{
-			return false;
-		}
-		for (size_t i = 0; i < start_bytes.size(); ++i)
-		{
-			if (report_desc[i] != start_bytes[i])
-			{
-				return false;
-			}
-		}
-		return true;
-	}
-
-	/* 	This should not be necessary, but Pico-PIO-USB/TinyUSB 
-		cannot detect a disconnect correctly, sad! */
-	static void __isr disconnect_irq_handler()
-	{
-		// uint32_t portsc = usb_hw->portsc1;
-		
-		// // Check for connect status change
-		// if (portsc & USBHS_PORTSC1_CSC) 
-		// {
-		// 	if (!(portsc & USBHS_PORTSC1_CCS)) 
-		// 	{
-		// 		// Device disconnected
-		// 		printf("Device disconnected!\n");
-		// 	}
-		// 	// Clear the CSC flag
-		// 	usb_hw->portsc1 |= USBHS_PORTSC1_CSC;
-		// }
-	}
-};
-
-#endif // _HOST_MANAGER_H_
\ No newline at end of file
+// #endif // _HOST_MANAGER_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBHost/tuh_callbacks.cpp b/Firmware/RP2040/src/USBHost/tuh_callbacks.cpp
index da38fec..2c42ff1 100644
--- a/Firmware/RP2040/src/USBHost/tuh_callbacks.cpp
+++ b/Firmware/RP2040/src/USBHost/tuh_callbacks.cpp
@@ -9,7 +9,6 @@
 #include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
 #include "USBHost/HostManager.h"
 #include "OGXMini/OGXMini.h"
-#include "Board/board_api.h"
 
 #if defined(CONFIG_EN_4CH)
 #include "I2CDriver/4Channel/I2CManager.h"
@@ -36,7 +35,7 @@ void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_re
         I2CManager::get_instance().get_driver()->notify_tuh_mounted();
 #endif //defined(CONFIG_EN_4CH)
 
-        OGXMini::update_tuh_status(true);
+        OGXMini::update_tud_status(true);
     }
 }
 
@@ -47,11 +46,11 @@ 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)
+#if defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh_unmounted();
+#endif //defined(CONFIG_EN_4CH)
 
-        OGXMini::update_tuh_status(false);
+        OGXMini::update_tud_status(false);
     }
 }
 
@@ -69,11 +68,11 @@ void tuh_xinput::mount_cb(uint8_t dev_addr, uint8_t instance, const tuh_xinput::
 
     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)
-    
-        OGXMini::update_tuh_status(true);
+#if defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh_mounted(host_type);
+#endif //defined(CONFIG_EN_4CH)
+
+        OGXMini::update_tud_status(true);
     }
 }
 
@@ -84,11 +83,11 @@ 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)
+#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)
 
-        OGXMini::update_tuh_status(false);
+        OGXMini::update_tud_status(false);
     }
 }
 
@@ -99,16 +98,20 @@ void tuh_xinput::report_received_cb(uint8_t dev_addr, uint8_t instance, const ui
 
 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)
+#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)
+
+    HostManager::get_instance().connect_cb(HostManager::DriverClass::XINPUT, 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)
+#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)
+
+    HostManager::get_instance().disconnect_cb(HostManager::DriverClass::XINPUT, dev_addr, instance);
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/UserProfile.cpp b/Firmware/RP2040/src/UserSettings/UserProfile.cpp
index 1f77b59..8375735 100644
--- a/Firmware/RP2040/src/UserSettings/UserProfile.cpp
+++ b/Firmware/RP2040/src/UserSettings/UserProfile.cpp
@@ -35,14 +35,14 @@ UserProfile::UserProfile()
 
     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;
+    analog_off_up = Gamepad::ANALOG_OFF_UP;
+    analog_off_down = Gamepad::ANALOG_OFF_DOWN;
+    analog_off_left = Gamepad::ANALOG_OFF_LEFT;
+    analog_off_right = Gamepad::ANALOG_OFF_RIGHT;
+    analog_off_a = Gamepad::ANALOG_OFF_A;
+    analog_off_b = Gamepad::ANALOG_OFF_B;
+    analog_off_x = Gamepad::ANALOG_OFF_X;
+    analog_off_y = Gamepad::ANALOG_OFF_Y;
+    analog_off_lb = Gamepad::ANALOG_OFF_LB;
+    analog_off_rb = Gamepad::ANALOG_OFF_RB;
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/UserSettings.cpp b/Firmware/RP2040/src/UserSettings/UserSettings.cpp
index b928193..e5caaaf 100644
--- a/Firmware/RP2040/src/UserSettings/UserSettings.cpp
+++ b/Firmware/RP2040/src/UserSettings/UserSettings.cpp
@@ -33,25 +33,31 @@ static constexpr DeviceDriver::Type VALID_DRIVER_TYPES[] =
 #if defined(CONFIG_EN_4CH)
     DeviceDriver::Type::XBOXOG, 
     DeviceDriver::Type::XBOXOG_SB, 
-    DeviceDriver::Type::XBOXOG_XR,
+    DeviceDriver::Type::XINPUT,
     DeviceDriver::Type::PS3,
+    DeviceDriver::Type::PSCLASSIC, 
     DeviceDriver::Type::WEBAPP,
-    
+    #if defined(XREMOTE_ROM_AVAILABLE)
+        DeviceDriver::Type::XBOXOG_XR,
+    #endif
+
 #elif MAX_GAMEPADS > 1
     DeviceDriver::Type::DINPUT, 
     DeviceDriver::Type::SWITCH, 
     DeviceDriver::Type::WEBAPP,
 
-#else
+#else // MAX_GAMEPADS == 1
     DeviceDriver::Type::XBOXOG, 
     DeviceDriver::Type::XBOXOG_SB, 
-    DeviceDriver::Type::XBOXOG_XR,
-    // DeviceDriver::Type::DINPUT, 
+    DeviceDriver::Type::DINPUT, 
     DeviceDriver::Type::SWITCH, 
     DeviceDriver::Type::WEBAPP,
     DeviceDriver::Type::PS3,
     DeviceDriver::Type::PSCLASSIC, 
     DeviceDriver::Type::XINPUT,
+    #if defined(XREMOTE_ROM_AVAILABLE)
+        DeviceDriver::Type::XBOXOG_XR,
+    #endif
 
 #endif
 };
@@ -59,14 +65,14 @@ static constexpr DeviceDriver::Type VALID_DRIVER_TYPES[] =
 struct ComboMap { uint32_t combo; DeviceDriver::Type driver; };
 static constexpr std::array<ComboMap, 9> BUTTON_COMBO_MAP = 
 {{
-    { ButtonCombo::XBOXOG, DeviceDriver::Type::XBOXOG },
+    { 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::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 }
 }};
 
@@ -166,8 +172,6 @@ bool UserSettings::write_firmware_version_safe()
 
 DeviceDriver::Type UserSettings::get_current_driver()
 {
-    return DeviceDriver::Type::XINPUT;
-    
     if (current_driver_ != DeviceDriver::Type::NONE)
     {
         return current_driver_;
@@ -431,4 +435,16 @@ UserProfile UserSettings::get_profile_by_id(const uint8_t profile_id)
 DeviceDriver::Type UserSettings::get_default_driver()
 {
     return VALID_DRIVER_TYPES[0];
+}
+
+bool UserSettings::valid_mode(DeviceDriver::Type mode)
+{
+    for (const auto& driver : VALID_DRIVER_TYPES)
+    {
+        if (mode == driver)
+        {
+            return true;
+        }
+    }
+    return false;
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/UserSettings.h b/Firmware/RP2040/src/UserSettings/UserSettings.h
index 0a9fa82..f54794a 100644
--- a/Firmware/RP2040/src/UserSettings/UserSettings.h
+++ b/Firmware/RP2040/src/UserSettings/UserSettings.h
@@ -26,6 +26,8 @@ public:
     bool verify_firmware_version();
     bool write_firmware_version_safe();
 
+    bool valid_mode(DeviceDriver::Type mode);
+
     DeviceDriver::Type get_current_driver();
     bool check_for_driver_change(Gamepad& gamepad);
     
diff --git a/Firmware/RP2040/src/board_config.h b/Firmware/RP2040/src/board_config.h
index c6d5b8d..d52fd61 100644
--- a/Firmware/RP2040/src/board_config.h
+++ b/Firmware/RP2040/src/board_config.h
@@ -1,8 +1,9 @@
 #ifndef _BOARD_CONFIG_H_
 #define _BOARD_CONFIG_H_
 
-/*  OGXM_BOARD and MAX_GAMEPADS are both defined as Cmake configure args,
-    add args -DOGXM_BOARD=PI_PICO and -DMAX_GAMEPADS=1 (or = whatever option you want).
+/*  Don't edit this file directly, instead use CMake to configure the board.
+    Add args -DOGXM_BOARD=PI_PICO and -DMAX_GAMEPADS=1 (or = whatever option you want)
+    to set the board and the number of gamepads.
     If you're setting MAX_GAMEPADS > 1 only D-Input, Switch, and WebApp device drivers will work. */
 
 #define ADA_FEATHER 1
@@ -13,7 +14,7 @@
 #define EXTERNAL_4CH 6
 #define W_ESP32 7
 
-#define SYSCLOCK_HZ 250000
+#define SYSCLOCK_KHZ 240000
 
 #ifndef MAX_GAMEPADS
     #define MAX_GAMEPADS 1
@@ -31,7 +32,7 @@
     #define LED_INDICATOR_PIN 13
     #define VCC_EN_PIN        18
 
-#elif OGXM_BOARD == PI_PICO
+#elif OGXM_BOARD == PI_PICO || OGXM_BOARD == PI_PICO2
     #define PIO_USB_DP_PIN      0 // DM = 1
     #define LED_INDICATOR_PIN   25
 
@@ -62,6 +63,7 @@
 #elif OGXM_BOARD == W_ESP32
     #define I2C_SDA_PIN     18 // SCL = 19
     #define UART0_TX_PIN    16 // RX = 17
+    #define UART0_RX_PIN (UART0_TX_PIN + 1)
     #define MODE_SEL_PIN    21
     #define ESP_PROG_PIN    20 // ESP32 IO0
     #define ESP_RST_PIN     8  // ESP32 EN
@@ -73,10 +75,6 @@
 
 #endif // OGXM_BOARD
 
-#if defined(UART0_TX_PIN)
-    #define UART0_RX_PIN (UART0_TX_PIN + 1)
-#endif // defined(UART0_TX_PIN)
-
 #if defined(I2C_SDA_PIN)
     #define I2C_BAUDRATE 400 * 1000
     #define I2C_SCL_PIN (I2C_SDA_PIN + 1)
@@ -97,19 +95,23 @@
 
 #if defined(CONFIG_EN_USB_HOST)
     #define PIO_USB_CONFIG { \
-                PIO_USB_DP_PIN, \
-                PIO_USB_TX_DEFAULT, \
-                PIO_SM_USB_TX_DEFAULT, \
-                PIO_USB_DMA_TX_DEFAULT, \
-                PIO_USB_RX_DEFAULT, \
-                PIO_SM_USB_RX_DEFAULT, \
-                PIO_SM_USB_EOP_DEFAULT, \
-                NULL, \
-                PIO_USB_DEBUG_PIN_NONE, \
-                PIO_USB_DEBUG_PIN_NONE, \
-                false, \
-                PIO_USB_PINOUT_DPDM \
-            }
+        PIO_USB_DP_PIN, \
+        PIO_USB_TX_DEFAULT, \
+        PIO_SM_USB_TX_DEFAULT, \
+        PIO_USB_DMA_TX_DEFAULT, \
+        PIO_USB_RX_DEFAULT, \
+        PIO_SM_USB_RX_DEFAULT, \
+        PIO_SM_USB_EOP_DEFAULT, \
+        NULL, \
+        PIO_USB_DEBUG_PIN_NONE, \
+        PIO_USB_DEBUG_PIN_NONE, \
+        false, \
+        PIO_USB_PINOUT_DPDM \
+    }
 #endif // PIO_USB_DP_PIN
 
+#if defined(OGXM_DEBUG)
+    #define DEBUG_UART_PORT __CONCAT(uart,PICO_DEFAULT_UART)
+#endif // defined(OGXM_DEBUG)
+
 #endif // _BOARD_CONFIG_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/main.cpp b/Firmware/RP2040/src/main.cpp
index 3112b1d..4a87066 100644
--- a/Firmware/RP2040/src/main.cpp
+++ b/Firmware/RP2040/src/main.cpp
@@ -6,8 +6,5 @@
 
 int main()
 {
-    if (set_sys_clock_hz(SYSCLOCK_HZ*1000, true))
-    {
-        OGXMini::run_program();
-    }
+    OGXMini::run_program();
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/tusb_config.h b/Firmware/RP2040/src/tusb_config.h
index de50ac1..19ee410 100644
--- a/Firmware/RP2040/src/tusb_config.h
+++ b/Firmware/RP2040/src/tusb_config.h
@@ -130,27 +130,24 @@
 // Enable host stack with pio-usb if Pico-PIO-USB library is available
 #define CFG_TUH_ENABLED     1
 #define CFG_TUH_RPI_PIO_USB 1
-// #define PICO_DEFAULT_PIO_USB_DP_PIN PIO_USB_DP_PIN
 
 #define TUH_OPT_RHPORT 1
 
 // Size of buffer to hold descriptors and other data used for enumeration
 #define CFG_TUH_ENUMERATION_BUFSIZE 512
 
-#define CFG_TUH_HUB             0
+#define CFG_TUH_HUB             1
 #define CFG_TUH_CDC             0
 
 #if defined(CONFIG_EN_4CH)
-#define CFG_TUH_HID             0
+#define CFG_TUH_HID             1
 #else
 #define CFG_TUH_HID             MAX_GAMEPADS
 #endif
 
 #define CFG_TUH_MSC             0
 #define CFG_TUH_VENDOR          0
-#define CFG_TUH_XINPUT          4
-
-#define CFG_TUH_XINPUT_WIRED_CHATPAD_EN 0
+#define CFG_TUH_XINPUT          MAX_GAMEPADS
 
 // max device support (excluding hub device)
 #define CFG_TUH_DEVICE_MAX          (CFG_TUH_HUB ? 4 : 1) // hub typically has 4 ports
diff --git a/Firmware/external/Pico-PIO-USB b/Firmware/external/Pico-PIO-USB
new file mode 160000
index 0000000..d17f8ef
--- /dev/null
+++ b/Firmware/external/Pico-PIO-USB
@@ -0,0 +1 @@
+Subproject commit d17f8efe9e6ee266d0b818634ca0ac256825579f
diff --git a/Firmware/external/patch_libs.cmake b/Firmware/external/patch_libs.cmake
index 7b7122e..d991aea 100644
--- a/Firmware/external/patch_libs.cmake
+++ b/Firmware/external/patch_libs.cmake
@@ -40,25 +40,4 @@ function(apply_lib_patches EXTERNAL_DIR)
     else ()
         message(FATAL_ERROR "Failed to apply Bluepad32 patch: ${BLUEPAD32_PATCH_ERROR}")
     endif ()
-
-    # set(TUSB_PATCH "${EXTERNAL_DIR}/patches/tinyusb_disable_hidh.diff")
-    # set(TUSB_PATH "${EXTERNAL_DIR}/tinyusb")
-
-    # message(STATUS "Applying tinyusb patch: ${TUSB_PATCH}")
-
-    # execute_process(
-    #     COMMAND git apply --ignore-whitespace ${TUSB_PATCH}
-    #     WORKING_DIRECTORY ${TUSB_PATH}
-    #     RESULT_VARIABLE TUSB_PATCH_RESULT
-    #     OUTPUT_VARIABLE TUSB_PATCH_OUTPUT
-    #     ERROR_VARIABLE TUSB_PATCH_ERROR
-    # )
-
-    # if (TUSB_PATCH_RESULT EQUAL 0)
-    #     message(STATUS "tinyusb patch applied successfully.")
-    # elseif (TUSB_PATCH_ERROR MATCHES "patch does not apply")
-    #     message(STATUS "tinyusb patch already applied.")
-    # else ()
-    #     message(FATAL_ERROR "Failed to apply tinyusb patch: ${TUSB_PATCH_ERROR}")
-    # endif ()
 endfunction()
\ No newline at end of file
diff --git a/Firmware/external/patches/tinyusb_disable_hidh.diff b/Firmware/external/patches/tinyusb_disable_hidh.diff
deleted file mode 100644
index 0c04d8a..0000000
--- a/Firmware/external/patches/tinyusb_disable_hidh.diff
+++ /dev/null
@@ -1,25 +0,0 @@
-diff --git a/src/class/hid/hid_host.h b/src/class/hid/hid_host.h
-index 9681c704b..ad055105e 100644
---- a/src/class/hid/hid_host.h
-+++ b/src/class/hid/hid_host.h
-@@ -28,6 +28,7 @@
- #define _TUSB_HID_HOST_H_
- 
- #include "hid.h"
-+#include "host/usbh.h"
- 
- #ifdef __cplusplus
- extern "C" {
-diff --git a/src/host/usbh.c b/src/host/usbh.c
-index a2994cde7..052f17d1c 100644
---- a/src/host/usbh.c
-+++ b/src/host/usbh.c
-@@ -176,7 +176,7 @@ static usbh_class_driver_t const usbh_class_drivers[] = {
-   },
-   #endif
- 
--  #if CFG_TUH_HID
-+  #if CFG_TUH_HID_NO
-   {
-       .name       = DRIVER_NAME("HID"),
-       .init       = hidh_init,
diff --git a/README.md b/README.md
index ce1b2d9..b94777a 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,7 @@
 # OGX-Mini
 ![OGX-Mini Boards](images/OGX-Mini-github.jpg "OGX-Mini Boards")
 
-Firmware for the RP2040, capable of emulating gamepads for several game consoles. The firmware comes in 6 flavors, the [Adafruit Feather USB Host board](https://www.adafruit.com/product/5723), Pi Pico, Waveshare RP2040-Zero, Pi Pico W, RP2040/ESP32 hybrid, and 4-Channel RP2040-Zero.
+Firmware for the RP2040, capable of emulating gamepads for several game consoles. The firmware comes in many flavors, supported on the [Adafruit Feather USB Host board](https://www.adafruit.com/product/5723), Pi Pico, Waveshare RP2040-Zero, Pi Pico W, RP2040/ESP32 hybrid, and a 4-Channel RP2040-Zero setup.
 
 ## Supported platforms
 - Original Xbox
@@ -9,6 +9,7 @@ Firmware for the RP2040, capable of emulating gamepads for several game consoles
 - Nintendo Switch (docked)
 - XInput (use [UsbdSecPatch](https://github.com/InvoxiPlayGames/UsbdSecPatch) for Xbox 360, or select the patch in J-Runner while flashing your NAND)
 - Playstation Classic
+- DInput
 
 ## Supported devices
 ### Wired controllers
@@ -68,14 +69,15 @@ Start = Plus (Switch) = Options (Dualsense/DS4)
 After a new mode is stored, the RP2040 will reset itself so you don't need to unplug it.
 
 ## Features new to v1.0.0
+- Bluetooth functionality for the Pico W and Pico+ESP32.
 - Web application for configuring deadzones and buttons mappings, supports up to 8 saved profiles.
-- Bluetooth functionality for the Pico W and ESP32 (in combination with an RP2040).
-- 4 channel functionality, connect 4 Picos via I2C and use your Xbox 360 wireless adapter.
-- Delayed USB mount until a controller is plugged in, useful for internal installation. 
+- Pi Pico 2 (RP2350) support.
+- 4 channel functionality, connect 4 Picos and use one Xbox 360 wireless adapter to control all 4.
+- Delayed USB mount until a controller is plugged in, useful for internal installation (non-Bluetooth boards only). 
 - Generic HID controller support.
 - Dualshock 3 emulation (minus gyros), rumble now works.
 - Steel Battalion controller emulation with a wireless Xbox 360 chatpad.
-- Xbox DVD dongle emulation, you must provide or dump your own firmware, see the Tools directory.
+- Xbox DVD dongle emulation. You must provide or dump your own firmware, see the Tools directory.
 - Analog button support on OG Xbox and PS3.
 - RGB LED support for RP2040-Zero and Adafruit Feather boards.
 
@@ -85,13 +87,12 @@ After a new mode is stored, the RP2040 will reset itself so you don't need to un
 - Anti-deadzone settings
 - More accurate report parser for unknown HID controllers
 - Hardware design for internal OG Xbox install
-- Hardware design for 4 channel adapter
+- Hardware design for 4 channel RP2040-Zero adapter
 - Wired Xbox 360 chatpad support
 - Wired Xbox One chatpad support
 - Switch (as input) rumble support
-- OG Xbox communicator support (in some form, will probably require custom hardware)
+- OG Xbox communicator support (in some form)
 - Generic bluetooth dongle support
-- Removal of some abstraction between TinyUSB class drivers and the rest of the app for decreased memory usage and (possible) speed improvement
 
 ## Hardware
 For Pi Pico, RP2040-Zero, 4 channel, and ESP32 configurations, please see the hardware folder for diagrams.
diff --git a/Tools/README.md b/Tools/README.md
index d7245ed..9b18136 100644
--- a/Tools/README.md
+++ b/Tools/README.md
@@ -1,2 +1,2 @@
-#Dumping Xbox DVD dongle firmware
-The firmware for the DVD Playback Kit is not included here, but you can dump your own or place a `.BIN` dump in this directory. Whichever you do, you'll have to run  `dump-xremote-firmware.py` to have it included with the firmware.
\ No newline at end of file
+# Dumping Xbox DVD dongle firmware
+The firmware for the DVD Playback Kit is not included here, but you can dump your own or place a `.BIN` dump in this directory. Whichever you do, you'll have to run  `dump-xremote-firmware.py` to have it included with the firmware when you compile it.
\ No newline at end of file
diff --git a/WebApp/constants.js b/WebApp/constants.js
index b58d15c..dec17a5 100644
--- a/WebApp/constants.js
+++ b/WebApp/constants.js
@@ -4,10 +4,10 @@ export const DEVICE_MODE_KEY_VALUE =
     "Xbox OG: Steel Battalion" : 8,
     "Xbox OG: XRemote" : 9,
     "XInput" : 6,
-    "PS3" : 2,
+    "PS3" : 1,
     "PS Classic" : 5,
     "Switch" : 4,
-    "WebApp" : 99,
+    "WebApp" : 100,
 };
 
 export const PROFILE_ID_KEY_VALUE = 
diff --git a/WebApp/start.bat b/WebApp/start.bat
new file mode 100644
index 0000000..f266b01
--- /dev/null
+++ b/WebApp/start.bat
@@ -0,0 +1 @@
+python -m http.server 8000
\ No newline at end of file
diff --git a/hardware/README.md b/hardware/README.md
index 33405c3..41ce683 100644
--- a/hardware/README.md
+++ b/hardware/README.md
@@ -1,3 +1,13 @@
+# Pi Pico
+![](../Images/DiagramPiPico.png)
+
+# Pi Pico 2
+The Pico 2 will likely require 4.7k resistors between the USB data lines and ground to work correctly.
+![](../Images/DiagramPiPico2.png)
+
+# RP2040-Zero
+![](../images/DiagramRPZero.png)
+
 Gerber, BOM, and schematic for an RP2040-Zero interposer board you can make yourself. LED1 and R3 are both optional. 
 
 The RP2040-Zero board can be found on Amazon and AliExpress.
diff --git a/hardware/BOM_OGX-Mini_RP2040-Zero_Interposer.csv b/hardware/RP2040-Zero_PCB/BOM_OGX-Mini_RP2040-Zero_Interposer.csv
similarity index 100%
rename from hardware/BOM_OGX-Mini_RP2040-Zero_Interposer.csv
rename to hardware/RP2040-Zero_PCB/BOM_OGX-Mini_RP2040-Zero_Interposer.csv
diff --git a/hardware/Gerber_OGX-Mini_RP2040-Zero_Interposer.zip b/hardware/RP2040-Zero_PCB/Gerber_OGX-Mini_RP2040-Zero_Interposer.zip
similarity index 100%
rename from hardware/Gerber_OGX-Mini_RP2040-Zero_Interposer.zip
rename to hardware/RP2040-Zero_PCB/Gerber_OGX-Mini_RP2040-Zero_Interposer.zip
diff --git a/hardware/Schematic_OGX-Mini_RP2040-Zero_Interposer.pdf b/hardware/RP2040-Zero_PCB/Schematic_OGX-Mini_RP2040-Zero_Interposer.pdf
similarity index 100%
rename from hardware/Schematic_OGX-Mini_RP2040-Zero_Interposer.pdf
rename to hardware/RP2040-Zero_PCB/Schematic_OGX-Mini_RP2040-Zero_Interposer.pdf
diff --git a/images/DiagramPico.png b/images/DiagramPico.png
new file mode 100644
index 0000000..52c91f5
Binary files /dev/null and b/images/DiagramPico.png differ
diff --git a/images/DiagramPico2.png b/images/DiagramPico2.png
new file mode 100644
index 0000000..5d36b88
Binary files /dev/null and b/images/DiagramPico2.png differ
diff --git a/images/DiagramRPZero.png b/images/DiagramRPZero.png
new file mode 100644
index 0000000..d31bd8d
Binary files /dev/null and b/images/DiagramRPZero.png differ
diff --git a/images/pi_pico_diagram.png b/images/pi_pico_diagram.png
deleted file mode 100644
index 85047be..0000000
Binary files a/images/pi_pico_diagram.png and /dev/null differ