[ADAPTER] Add support for mapping quirks fixup

main/CMakeLists.txt

@@ -4,6 +4,7 @@ idf_component_register(SRCS "main.c"
                             "adapter/hid_parser.c"
                             "adapter/hid_generic.c"
                             "adapter/kb_monitor.c"
+                            "adapter/mapping_quirks.c"
                             "adapter/npiso.c"
                             "adapter/cdi.c"
                             "adapter/saturn.c"

main/adapter/adapter.h

@@ -248,6 +248,8 @@ enum {
 /* BT flags */
 enum {
     BT_INIT = 0,
+    BT_QUIRK_FACE_BTNS_INVERT,
+    BT_QUIRK_FACE_BTNS_ROTATE_RIGHT,
 };
 
 /* Wired flags */
@@ -367,6 +369,12 @@ struct hid_report {
     struct hid_usage usages[REPORT_MAX_USAGE];
 };
 
+struct raw_src_mapping {
+    uint32_t mask[4];
+    uint32_t desc[4];
+    uint32_t btns_mask[32];
+};
+
 struct bt_data {
     /* Bi-directional */
     atomic_t flags;
@@ -378,6 +386,7 @@ struct bt_data {
     uint32_t report_id;
     uint32_t report_cnt;
     int32_t report_type;
+    struct raw_src_mapping raw_src_mappings[REPORT_MAX];
     struct hid_report reports[REPORT_MAX];
     uint8_t input[128];
     int32_t axes_cal[ADAPTER_MAX_AXES];

main/adapter/mapping_quirks.c

@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2021, Jacques Gagnon
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include "zephyr/types.h"
+#include "tools/util.h"
+#include "adapter.h"
+#include "mapping_quirks.h"
+
+static void face_btns_invert(uint32_t btns_mask[32]) {
+    uint32_t tmp = btns_mask[PAD_RB_LEFT];
+
+    btns_mask[PAD_RB_LEFT] = btns_mask[PAD_RB_UP];
+    btns_mask[PAD_RB_UP] = tmp;
+
+    tmp = btns_mask[PAD_RB_DOWN];
+    btns_mask[PAD_RB_DOWN] = btns_mask[PAD_RB_RIGHT];
+    btns_mask[PAD_RB_RIGHT] = tmp;
+}
+
+static void face_btns_rotate_right(uint32_t btns_mask[32]) {
+    uint32_t tmp = btns_mask[PAD_RB_UP];
+
+    btns_mask[PAD_RB_UP] = btns_mask[PAD_RB_LEFT];
+    btns_mask[PAD_RB_LEFT] = btns_mask[PAD_RB_DOWN];
+    btns_mask[PAD_RB_DOWN] = btns_mask[PAD_RB_RIGHT];
+    btns_mask[PAD_RB_RIGHT] = tmp;
+}
+
+void mapping_quirks_apply(struct bt_data *bt_data) {
+    if (atomic_test_bit(&bt_data->flags, BT_QUIRK_FACE_BTNS_INVERT)) {
+        face_btns_invert(bt_data->raw_src_mappings[PAD].btns_mask);
+    }
+    if (atomic_test_bit(&bt_data->flags, BT_QUIRK_FACE_BTNS_ROTATE_RIGHT)) {
+        face_btns_rotate_right(bt_data->raw_src_mappings[PAD].btns_mask);
+    }
+}

main/adapter/mapping_quirks.h

@@ -0,0 +1,12 @@
+/*
+ * Copyright (c) 2021, Jacques Gagnon
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef _MAPPING_QUIRKS_H_
+#define _MAPPING_QUIRKS_H_
+#include "adapter.h"
+
+void mapping_quirks_apply(struct bt_data *bt_data);
+
+#endif /* _MAPPING_QUIRKS_H_ */

main/adapter/sw.c

@@ -6,6 +6,7 @@
 #include <string.h>
 #include "zephyr/types.h"
 #include "tools/util.h"
+#include "mapping_quirks.h"
 #include "sw.h"
 
 #define BT_HIDP_SW_SUBCMD_SET_LED 0x30
@@ -77,30 +78,46 @@ static const uint32_t sw_btns_mask[32] = {
     BIT(SW_ZR), BIT(SW_R), BIT(SW_SR), BIT(SW_RJ),
 };
 
+void sw_pad_init(struct bt_data *bt_data) {
+    struct sw_map *map = (struct sw_map *)bt_data->input;
+
+    memcpy(bt_data->raw_src_mappings[PAD].mask, sw_mask,
+        sizeof(bt_data->raw_src_mappings[PAD].mask));
+    memcpy(bt_data->raw_src_mappings[PAD].desc, sw_desc,
+        sizeof(bt_data->raw_src_mappings[PAD].desc));
+    memcpy(bt_data->raw_src_mappings[PAD].btns_mask, sw_btns_mask,
+        sizeof(bt_data->raw_src_mappings[PAD].btns_mask));
+
+    mapping_quirks_apply(bt_data);
+
+    for (uint32_t i = 0; i < SW_AXES_MAX; i++) {
+        bt_data->axes_cal[i] = -(map->axes[sw_axes_idx[i]] - sw_axes_meta[i].neutral);
+    }
+
+    atomic_set_bit(&bt_data->flags, BT_INIT);
+}
+
 int32_t sw_to_generic(struct bt_data *bt_data, struct generic_ctrl *ctrl_data) {
     struct sw_map *map = (struct sw_map *)bt_data->input;
 
+    if (!atomic_test_bit(&bt_data->flags, BT_INIT)) {
+        sw_pad_init(bt_data);
+    }
+
     memset((void *)ctrl_data, 0, sizeof(*ctrl_data));
 
-    ctrl_data->mask = (uint32_t *)sw_mask;
-    ctrl_data->desc = (uint32_t *)sw_desc;
+    ctrl_data->mask = (uint32_t *)bt_data->raw_src_mappings[PAD].mask;
+    ctrl_data->desc = (uint32_t *)bt_data->raw_src_mappings[PAD].desc;
 
     for (uint32_t i = 0; i < ARRAY_SIZE(generic_btns_mask); i++) {
-        if (map->buttons & sw_btns_mask[i]) {
+        if (map->buttons & bt_data->raw_src_mappings[PAD].btns_mask[i]) {
             ctrl_data->btns[0].value |= generic_btns_mask[i];
         }
     }
 
     /* Convert hat to regular btns */
     ctrl_data->btns[0].value |= hat_to_ld_btns[map->hat & 0xF];
 
-    if (!atomic_test_bit(&bt_data->flags, BT_INIT)) {
-        for (uint32_t i = 0; i < SW_AXES_MAX; i++) {
-            bt_data->axes_cal[i] = -(map->axes[sw_axes_idx[i]] - sw_axes_meta[i].neutral);
-        }
-        atomic_set_bit(&bt_data->flags, BT_INIT);
-    }
-
     for (uint32_t i = 0; i < SW_AXES_MAX; i++) {
         ctrl_data->axes[i].meta = &sw_axes_meta[i];
         ctrl_data->axes[i].value = map->axes[sw_axes_idx[i]] - sw_axes_meta[i].neutral + bt_data->axes_cal[i];

main/bluetooth/att.c

@@ -8,6 +8,7 @@
 #include <esp_system.h>
 #include <esp_timer.h>
 #include "host.h"
+#include "hci.h"
 #include "att.h"
 #include "zephyr/uuid.h"
 #include "zephyr/att.h"
@@ -937,12 +938,13 @@ void bt_att_hid_hdlr(struct bt_dev *device, struct bt_hci_pkt *bt_hci_acl_pkt, u
             switch (device->hid_state) {
                 case BT_ATT_HID_DEVICE_NAME:
                 {
-                    char device_name[32] = {0};
+                    uint8_t device_name[32] = {0};
 
                     if (rsp_len > 31) {
                         rsp_len = 31;
                     }
                     memcpy(device_name, read_type_rsp->data[0].value, rsp_len);
+                    bt_hci_set_type_flags_from_name(device, device_name);
                     printf("# dev: %d type: %d %s\n", device->id, device->type, device_name);
 
                     device->hid_state = BT_ATT_HID_DISCOVERY;

main/bluetooth/hci.c

@@ -26,6 +26,7 @@ typedef void (*bt_cmd_func_t)(void *param);
 struct bt_name_type {
     char name[249];
     int8_t type;
+    atomic_t hid_flags;
 };
 
 struct bt_hci_cmd_cp {
@@ -45,14 +46,16 @@ static const char bt_default_pin[][5] = {
 };
 
 static const struct bt_name_type bt_name_type[] = {
-    {"PLAYSTATION(R)3", PS3_DS3},
-    {"Nintendo RVL-CNT-01-UC", WIIU_PRO}, /* Must be before WII_CORE */
-    {"Nintendo RVL-CNT-01", WII_CORE},
-    {"Wireless Controller", PS4_DS4},
-    {"Xbox Wireless Controller", XB1_S},
-    {"Xbox Adaptive Controller", XB1_ADAPTIVE},
-    {"Pro Controller", SW},
-    {"Joy-Con", SW},
+    {"PLAYSTATION(R)3", PS3_DS3, 0},
+    {"Nintendo RVL-CNT-01-UC", WIIU_PRO, 0}, /* Must be before WII_CORE */
+    {"Nintendo RVL-CNT-01", WII_CORE, 0},
+    {"Wireless Controller", PS4_DS4, 0},
+    {"Xbox Wireless Controller", XB1_S, 0},
+    {"Xbox Adaptive Controller", XB1_ADAPTIVE, 0},
+    {"Pro Controller", SW, 0},
+    {"Joy-Con", SW, 0},
+    {"8Bitdo SF30", HID_GENERIC, BIT(BT_QUIRK_FACE_BTNS_INVERT)},
+    {"Lic Pro Controller", SW, BIT(BT_QUIRK_FACE_BTNS_ROTATE_RIGHT)},
 };
 
 static const struct bt_hci_cp_set_event_filter clr_evt_filter = {
@@ -80,7 +83,6 @@ static uint8_t local_bdaddr[6];
 static uint32_t bt_config_state = 0;
 static RingbufHandle_t randq_hdl, encryptq_hdl;
 
-static int32_t bt_hci_get_type_from_name(const uint8_t* name);
 static void bt_hci_cmd(uint16_t opcode, uint32_t cp_len);
 //static void bt_hci_cmd_inquiry(void *cp);
 //static void bt_hci_cmd_inquiry_cancel(void *cp);
@@ -212,15 +214,6 @@ static const struct bt_hci_cmd_cp bt_hci_config[] = {
     {bt_hci_start_inquiry_cfg_check, NULL},
 };
 
-static int32_t bt_hci_get_type_from_name(const uint8_t* name) {
-    for (uint32_t i = 0; i < sizeof(bt_name_type)/sizeof(*bt_name_type); i++) {
-        if (memcmp(name, bt_name_type[i].name, strlen(bt_name_type[i].name)) == 0) {
-            return bt_name_type[i].type;
-        }
-    }
-    return -1;
-}
-
 static void bt_hci_q_conf(uint32_t next) {
     if (next) {
         bt_config_state++;
@@ -1279,6 +1272,16 @@ void bt_hci_le_conn_update(struct hci_cp_le_conn_update *cp) {
     bt_hci_cmd_le_conn_update(cp);
 }
 
+void bt_hci_set_type_flags_from_name(struct bt_dev *device, const uint8_t* name) {
+    for (uint32_t i = 0; i < sizeof(bt_name_type)/sizeof(*bt_name_type); i++) {
+        if (memcmp(name, bt_name_type[i].name, strlen(bt_name_type[i].name)) == 0) {
+            struct bt_data *bt_data = &bt_adapter.data[device->id];
+            device->type = bt_name_type[i].type;
+            bt_data->flags = bt_name_type[i].hid_flags;
+        }
+    }
+}
+
 void bt_hci_evt_hdlr(struct bt_hci_pkt *bt_hci_evt_pkt) {
     struct bt_dev *device = NULL;
 
@@ -1447,10 +1450,7 @@ void bt_hci_evt_hdlr(struct bt_hci_pkt *bt_hci_evt_pkt) {
                     }
                 }
                 else {
-                    int8_t type = bt_hci_get_type_from_name(remote_name_req_complete->name);
-                    if (type > BT_NONE) {
-                        device->type = bt_hci_get_type_from_name(remote_name_req_complete->name);
-                    }
+                    bt_hci_set_type_flags_from_name(device, remote_name_req_complete->name);
                     if (device->type == HID_GENERIC || device->type == SW) {
                         bt_hci_cmd_read_remote_features(&device->acl_handle);
                     }

main/bluetooth/hci.h

@@ -16,6 +16,7 @@ int32_t bt_hci_get_encrypt(struct bt_dev *device, bt_hci_le_cb_t cb, const uint8
 void bt_hci_start_encryption(uint16_t handle, uint64_t rand, uint16_t ediv, uint8_t *ltk);
 void bt_hci_add_to_accept_list(bt_addr_le_t *le_bdaddr);
 void bt_hci_le_conn_update(struct hci_cp_le_conn_update *cp);
+void bt_hci_set_type_flags_from_name(struct bt_dev *device, const uint8_t* name);
 void bt_hci_evt_hdlr(struct bt_hci_pkt *bt_hci_evt_pkt);
 
 #endif /* _BT_HCI_H_ */