From f0c25863dccfe5848f0b8417277cbbf5a3310914 Mon Sep 17 00:00:00 2001
From: Charlie Birks <charlie@daft.games>
Date: Sat, 8 Jul 2023 18:46:52 +0100
Subject: [PATCH] pico: partial switch pro controller support

---
 32blit-pico/input_usb_hid.cpp |  1 +
 32blit-pico/usb_host.cpp      | 48 +++++++++++++++++++++++++++++++++--
 2 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/32blit-pico/input_usb_hid.cpp b/32blit-pico/input_usb_hid.cpp
index f1420f37a..745e22507 100644
--- a/32blit-pico/input_usb_hid.cpp
+++ b/32blit-pico/input_usb_hid.cpp
@@ -26,6 +26,7 @@ struct GamepadMapping {
 };
 
 static const GamepadMapping gamepad_mappings[]{
+  {0x057E2009,  3,  2,  1,  0,   17,   16,   19,   18,  8, 12, 11}, // Switch Pro Controller
   {0x15320705,  0,  1,  3,  4, 0xFF, 0xFF, 0xFF, 0xFF, 16, 15, 13}, // Razer Raiju Mobile
   {0x20D6A711,  2,  1,  3,  0, 0xFF, 0xFF, 0xFF, 0xFF,  8, 12, 10}, // PowerA wired Switch pro controller
   {0x00000000,  0,  1,  2,  3, 0xFF, 0xFF, 0xFF, 0xFF,  4,  5,  6}  // probably wrong fallback
diff --git a/32blit-pico/usb_host.cpp b/32blit-pico/usb_host.cpp
index 089408298..25efe36ca 100644
--- a/32blit-pico/usb_host.cpp
+++ b/32blit-pico/usb_host.cpp
@@ -12,6 +12,7 @@
 static int hid_report_id = -1;
 static uint16_t buttons_offset = 0, num_buttons = 0;
 static uint16_t hat_offset = 0xFFFF, stick_offset = 0;
+static uint8_t axis_size = 8;
 
 uint32_t hid_gamepad_id = 0;
 bool hid_keyboard_detected = false;
@@ -20,6 +21,26 @@ uint8_t hid_hat = 8;
 uint32_t hid_buttons = 0;
 uint8_t hid_keys[6]{};
 
+static void switch_pro_mount(uint8_t dev_addr, uint8_t instance) {
+  uint8_t data = 2; // handshake
+  tuh_hid_send_report(dev_addr, instance, 0x80, &data, 1);
+
+  // report descriptor is inaccurate
+  hid_report_id = 0x30;
+  buttons_offset = 2 * 8;
+  num_buttons = 24;
+  stick_offset = 5 * 8;
+  axis_size = 12;
+  hat_offset = 0xFFFF; // no hat, only buttons
+}
+
+static void switch_pro_report(uint8_t dev_addr, uint8_t instance, uint8_t const* report, uint16_t len) {
+  if(report[0] == 0x81 && report[1] == 2) { // handshake
+    uint8_t data = 4; // disable bluetooth / enable usb
+    tuh_hid_send_report(dev_addr, instance, 0x80, &data, 1);
+  }
+}
+
 void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_report, uint16_t desc_len) {
   uint16_t vid = 0, pid = 0;
   tuh_vid_pid_get(dev_addr, &vid, &pid);
@@ -36,6 +57,9 @@ void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_re
     return;
   }
 
+  hat_offset = 0xFFFF;
+  axis_size = 8;
+
   // basic and probably wrong report descriptor parsing
   auto desc_end = desc_report + desc_len;
   auto p = desc_report;
@@ -104,6 +128,10 @@ void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_re
 
   hid_gamepad_id = (vid << 16) | pid;
 
+  // switch pro controller
+  if(hid_gamepad_id == 0x057E2009)
+    switch_pro_mount(dev_addr, instance);
+
   if(!tuh_hid_receive_report(dev_addr, instance)) {
     printf("Cound not request report!\n");
   }
@@ -130,6 +158,10 @@ void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t cons
     return;
   }
 
+  // switch pro controller setup
+  if(hid_gamepad_id == 0x057E2009)
+    switch_pro_report(dev_addr, instance, report, len);
+
   // check report id if we have one
   if(hid_report_id == -1 || report[0] == hid_report_id) {
     // I hope these are reasonably aligned
@@ -138,8 +170,20 @@ void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t cons
     else
       hid_hat = 8;
 
-    hid_joystick[0] = report_data[stick_offset / 8];
-    hid_joystick[1] = report_data[stick_offset / 8 + 1];
+    if(axis_size == 8) {
+      hid_joystick[0] = report_data[stick_offset / 8];
+      hid_joystick[1] = report_data[stick_offset / 8 + 1];
+    } else if(axis_size == 12) {
+      uint16_t x = report_data[stick_offset / 8] | (report_data[stick_offset / 8 + 1] & 0xF) << 8;
+      uint16_t y = report_data[stick_offset / 8 + 1] >> 4 | (report_data[stick_offset / 8 + 2]) << 4;
+      // take the high bits
+      hid_joystick[0] = x >> 4;
+      hid_joystick[1] = y >> 4;
+
+      // FIXME: needs calibration for switch pro controller
+      if(hid_gamepad_id == 0x057E2009)
+        hid_joystick[1] = 0xFF - hid_joystick[1];
+    }
 
     // get up to 32 buttons
     hid_buttons = 0;