[BT] Support Switch 2 ctrls reconnection & cleanup

Author: darthcloud

main/CMakeLists.txt

@@ -48,6 +48,7 @@ set(cov_srcs
     "bluetooth/hidp/wii.c"
     "bluetooth/hidp/ps.c"
     "bluetooth/hidp/sw.c"
+    "bluetooth/hidp/sw2.c"
     "system/manager.c"
     "tools/devcrypto.c"
     "tools/ps1_gameid.c"

main/bluetooth/att_hid.c

@@ -15,6 +15,7 @@
 #include "zephyr/gatt.h"
 #include "adapter/hid_parser.h"
 #include "adapter/mapping_quirks.h"
+#include "hidp/sw2.h"
 
 enum {
     BT_ATT_HID_DEVICE_NAME = 0,
@@ -471,9 +472,14 @@ static void bt_att_hid_process_current_state(struct bt_dev *device,
 
 static void bt_att_hid_start_next_state(struct bt_dev *device,
         struct bt_att_hid *hid_data) {
-    device->hid_state++;
-    if (device->hid_state < BT_ATT_HID_STATE_MAX && start_state_func[device->hid_state]) {
-        start_state_func[device->hid_state](device, hid_data);
+    if (device->ids.type == BT_SW2 && !atomic_test_bit(&device->flags, BT_DEV_HID_INIT_DONE)) {
+        bt_hid_sw2_init(device);
+    }
+    else {
+        device->hid_state++;
+        if (device->hid_state < BT_ATT_HID_STATE_MAX && start_state_func[device->hid_state]) {
+            start_state_func[device->hid_state](device, hid_data);
+        }
     }
 }
 
@@ -487,9 +493,14 @@ static int32_t bt_att_get_report_index(struct bt_att_hid *hid_data, uint8_t repo
 }
 
 void bt_att_hid_init(struct bt_dev *device) {
-    struct bt_att_hid *hid_data = &att_hid[device->ids.id];
-    memset((uint8_t *)hid_data, 0, sizeof(*hid_data));
-    bt_att_cmd_mtu_req(device->acl_handle, bt_att_get_le_max_mtu());
+    if (!atomic_test_bit(&device->flags, BT_DEV_HID_INTR_PENDING)) {
+        struct bt_att_hid *hid_data = &att_hid[device->ids.id];
+
+        atomic_set_bit(&device->flags, BT_DEV_HID_INTR_PENDING);
+
+        memset((uint8_t *)hid_data, 0, sizeof(*hid_data));
+        bt_att_cmd_mtu_req(device->acl_handle, bt_att_get_le_max_mtu());
+    }
 }
 
 void bt_att_write_hid_report(struct bt_dev *device, uint8_t report_id, uint8_t *data, uint32_t len) {
@@ -530,19 +541,7 @@ void bt_att_hid_hdlr(struct bt_dev *device, struct bt_hci_pkt *bt_hci_acl_pkt, u
             if (!atomic_test_bit(&device->flags, BT_DEV_HID_INTR_READY)) {
                 bt_hci_stop_inquiry();
 
-                if (device->ids.type == BT_SW2) {
-                    uint16_t data = BT_GATT_CCC_NOTIFY;
-                    uint8_t led[16] = {
-                        0x09, 0x91, 0x00, 0x07, 0x00, 0x08, 0x00, 0x00,
-                        bt_hid_led_dev_id_map[device->ids.out_idx],
-                        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-                    bt_att_cmd_write_req(device->acl_handle, 0x001b, (uint8_t *)&data, sizeof(data));
-                    bt_att_cmd_write_cmd(device->acl_handle, 0x0014, (uint8_t *)&led, sizeof(led));
-                    bt_att_cmd_write_req(device->acl_handle, 0x000b, (uint8_t *)&data, sizeof(data));
-                }
-                else {
-                    bt_att_hid_start_first_state(device, hid_data);
-                }
+                bt_att_hid_start_first_state(device, hid_data);
             }
             break;
         }
@@ -675,27 +674,18 @@ void bt_att_hid_hdlr(struct bt_dev *device, struct bt_hci_pkt *bt_hci_acl_pkt, u
         {
             struct bt_att_notify *notify = (struct bt_att_notify *)bt_hci_acl_pkt->att_data;
 
-            if (device->ids.type == BT_SW2) {
-                bt_host_bridge(device, 1, notify->value, att_len - sizeof(notify->handle));
-            }
-            else {
-                for (uint32_t i = 0; i < HID_MAX_REPORT; i++) {
-                    if (notify->handle == hid_data->reports[i].report_hdl) {
-#ifdef CONFIG_BLUERETRO_ADAPTER_RUMBLE_TEST
-                        struct bt_hidp_xb1_rumble rumble = {
-                            .enable = 0x03,
-                            .duration = 0xFF,
-                            .cnt = 0x00,
-                        };
-                        rumble.mag_r = bt_hci_acl_pkt->hidp_data[11];
-                        rumble.mag_l = bt_hci_acl_pkt->hidp_data[9];
-                        bt_hid_cmd_xbox_rumble(device, &rumble);
-#else
-                        bt_host_bridge(device, hid_data->reports[i].id, notify->value, att_len - sizeof(notify->handle));
-#endif
-                        break;
+            switch (device->ids.type) {
+                case BT_SW2:
+                    bt_hid_sw2_hdlr(device, notify->handle, notify->value, att_len - sizeof(notify->handle));
+                    break;
+                default:
+                    for (uint32_t i = 0; i < HID_MAX_REPORT; i++) {
+                        if (notify->handle == hid_data->reports[i].report_hdl) {
+                            bt_host_bridge(device, hid_data->reports[i].id, notify->value, att_len - sizeof(notify->handle));
+                            break;
+                        }
                     }
-                }
+                    break;
             }
             break;
         }

main/bluetooth/hci.c

@@ -1092,7 +1092,6 @@ static void bt_hci_le_meta_evt_hdlr(struct bt_hci_pkt *bt_hci_evt_pkt) {
                 uint8_t *end = data + le_adv_report->adv_info[0].length;
                 uint8_t len, type;
                 uint16_t value;
-                bool sw2 = false;
 
                 printf("# BT_HCI_EVT_LE_ADVERTISING_REPORT\n");
 
@@ -1124,7 +1123,6 @@ static void bt_hci_le_meta_evt_hdlr(struct bt_hci_pkt *bt_hci_evt_pkt) {
                             /* Manufacturer Specific Data */
                             value = *(uint16_t *)&data[1];
                             if (value == 0x0553) {
-                                sw2 = true;
                                 goto connect;
                             }
                             break;
@@ -1140,15 +1138,6 @@ static void bt_hci_le_meta_evt_hdlr(struct bt_hci_pkt *bt_hci_evt_pkt) {
                         bt_host_reset_dev(device);
                         memcpy((uint8_t *)&device->le_remote_bdaddr, (uint8_t *)&le_adv_report->adv_info[0].addr, sizeof(device->le_remote_bdaddr));
                         device->ids.type = BT_HID_GENERIC;
-                        if (sw2) {
-                            device->ids.type = BT_SW2;
-                            struct bt_data *bt_data = &bt_adapter.data[device->ids.id];
-                            if (data) {
-                                bt_data->base.vid = *(uint16_t *)&data[6];
-                                bt_data->base.pid = *(uint16_t *)&data[8];
-                                printf("%s: VID: 0x%04X PID: 0x%04X\n", __FUNCTION__, bt_data->base.vid, bt_data->base.pid);
-                            }
-                        }
                         bt_l2cap_init_dev_scid(device);
                         atomic_set_bit(&device->flags, BT_DEV_DEVICE_FOUND);
                         bt_hci_cmd_le_set_scan_enable(0);

main/bluetooth/hidp/hidp.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2023, Jacques Gagnon
+ * Copyright (c) 2019-2025, Jacques Gagnon
  * SPDX-License-Identifier: Apache-2.0
  */
 
@@ -11,6 +11,7 @@
 #include "wii.h"
 #include "ps.h"
 #include "sw.h"
+#include "sw2.h"
 
 typedef void (*bt_hid_init_t)(struct bt_dev *device);
 typedef void (*bt_hid_hdlr_t)(struct bt_dev *device, struct bt_hci_pkt *bt_hci_acl_pkt, uint32_t len);
@@ -52,6 +53,7 @@ static const struct bt_name_type bt_name_type[] = {
     {"BlueN64 Gamepad", BT_HID_GENERIC, BT_SUBTYPE_DEFAULT, BIT(BT_QUIRK_BLUEN64_N64)},
     {"Hyperkin Pad", BT_SW, BT_SW_HYPERKIN_ADMIRAL, 0},
     {"OUYA Game Controller", BT_HID_GENERIC, BT_SUBTYPE_DEFAULT, BIT(BT_QUIRK_OUYA)},
+    {"DeviceName", BT_SW2, BT_SUBTYPE_DEFAULT, 0},
 #endif
 };
 
@@ -61,6 +63,7 @@ static const bt_hid_init_t bt_hid_init_list[BT_TYPE_MAX] = {
     bt_hid_wii_init, /* BT_WII */
     bt_hid_ps_init, /* BT_PS */
     bt_hid_sw_init, /* BT_SW */
+    bt_hid_sw2_init, /* BT_SW2 */
 };
 
 static const bt_hid_hdlr_t bt_hid_hdlr_list[BT_TYPE_MAX] = {
@@ -69,6 +72,7 @@ static const bt_hid_hdlr_t bt_hid_hdlr_list[BT_TYPE_MAX] = {
     bt_hid_wii_hdlr, /* BT_WII */
     bt_hid_ps_hdlr, /* BT_PS */
     bt_hid_sw_hdlr, /* BT_SW */
+    NULL, /* BT_SW2 */
 };
 
 static const bt_hid_cmd_t bt_hid_feedback_list[BT_TYPE_MAX] = {
@@ -77,6 +81,7 @@ static const bt_hid_cmd_t bt_hid_feedback_list[BT_TYPE_MAX] = {
     bt_hid_cmd_wii_set_feedback, /* BT_WII */
     bt_hid_cmd_ps_set_conf, /* BT_PS */
     bt_hid_cmd_sw_set_conf, /* BT_SW */
+    bt_hid_cmd_sw2_out, /* BT_SW2 */
 };
 
 void bt_hid_set_type_flags_from_name(struct bt_dev *device, const char* name) {