feat: support test command

src/bt.cpp

@@ -333,11 +333,16 @@ static void l2cap_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t
                 bt_disconnect();
             }
         } else if (channel == hid_control_cid) {
-            if (size > 1 && packet[0] == 0xA3) {
+            if (packet[0] == 0xA3) {
                 uint8_t report_id = packet[1];
                 feature_data[report_id].assign(packet + 1, packet + size);
                 printf("[L2CAP] Stored Feature Report 0x%02X, len=%u\n", report_id, size - 1);
             }
+            if (packet[0] == 0x00) {
+                printf("[L2CAP] Getting Test Result\n");
+                feature_data.erase(0x81);
+                get_feature_data(0x81,64);
+            }
             printf("[L2CAP] HID Control data len=%u\n", size);
             printf_hexdump(packet, size);
             bt_data_callback(CONTROL, packet, size);
@@ -477,19 +482,33 @@ void bt_write(uint8_t *data, uint16_t len) {
 }
 
 std::vector<uint8_t> get_feature_data(uint8_t reportId, uint16_t len) {
-    if (feature_data.find(reportId) == feature_data.end() || feature_data[reportId].empty()) {
+    if (!feature_data.contains(reportId) || feature_data[reportId].empty()) {
         if (hid_control_cid != 0) {
             uint8_t get_feature[] = {0x43, reportId};
             l2cap_send(hid_control_cid, get_feature, len);
-            printf("[L2CAP] Requesting Feature Report 0x%02X\n", reportId);
+            printf("[L2CAP] Requesting Get Feature Report 0x%02X\n", reportId);
         }
         return {};
     }
     return feature_data[reportId];
 }
 
+void set_feature_data(uint8_t reportId, uint8_t* data,uint16_t len) {
+    if (hid_control_cid != 0) {
+        uint8_t get_feature[len + 2];
+        get_feature[0] = 0x53;
+        get_feature[1] = reportId;
+        memcpy(get_feature + 2,data,len);
+        fill_feature_report_checksum(get_feature + 1,len + 1);
+        l2cap_send(hid_control_cid, get_feature, len + 2);
+        printf("[L2CAP] Requesting Set Feature Report 0x%02X\n", reportId);
+        printf_hexdump(get_feature,len + 2);
+    }
+}
+
 void init_feature() {
     get_feature_data(0x09, 20);
     get_feature_data(0x20, 64);
+    get_feature_data(0x22, 64);
     get_feature_data(0x05, 41);
 }

src/bt.h

@@ -22,5 +22,6 @@ void bt_send_control(uint8_t *data, uint16_t len);
 void bt_write(uint8_t* data,uint16_t len);
 std::vector<uint8_t> get_feature_data(uint8_t reportId,uint16_t len);
 void init_feature();
+void set_feature_data(uint8_t reportId, uint8_t* data,uint16_t len);
 
 #endif //DS5_BRIDGE_BT_H

src/main.cpp

@@ -72,20 +72,27 @@ void tud_hid_set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t rep
     (void) buffer;
     (void) bufsize;
 
-    switch (buffer[0]) {
+    // INTERRUPT OUT
-        case 0x02: {
+    if (report_id == 0) {
-            uint8_t outputData[78];
+        switch (buffer[0]) {
-            outputData[0] = 0x31;
+            case 0x02: {
-            outputData[1] = reportSeqCounter << 4;
+                uint8_t outputData[78];
-            if (++reportSeqCounter == 256) {
+                outputData[0] = 0x31;
-                reportSeqCounter = 0;
+                outputData[1] = reportSeqCounter << 4;
+                if (++reportSeqCounter == 256) {
+                    reportSeqCounter = 0;
+                }
+                outputData[2] = 0x10;
+                memcpy(outputData + 3, buffer + 1, bufsize - 1);
+                bt_write(outputData, sizeof(outputData));
+                break;
             }
-            outputData[2] = 0x10;
-            memcpy(outputData + 3, buffer + 1, bufsize - 1);
-            bt_write(outputData, sizeof(outputData));
-            break;
         }
     }
+    if (report_id == 0x80) {
+        set_feature_data(report_id,const_cast<uint8_t *>(buffer),bufsize);
+        return;
+    }
 }
 
 int main() {

src/utils.h

@@ -110,6 +110,27 @@ inline void fill_output_report_checksum(uint8_t* outputData,size_t len)
     outputData[len - 1] = (crc >> 24) & 0xFF;
 }
 
+inline uint32_t crc32_feature(const uint8_t *data, std::size_t size) {
+    // https://github.com/rafaelvaloto/Dualsense-Multiplatform/blob/main/Source/Private/GCore/Utils/CR32.cpp
+    uint32_t crc = ~0x2060efc3; // 0x53 seed
+
+    while (size--) {
+        crc ^= *data++;
+        for (unsigned i = 0; i < 8; i++)
+            crc = ((crc >> 1) ^ (0xEDB88320 & -(crc & 1)));
+    }
+
+    return ~crc;
+}
+
+inline void fill_feature_report_checksum(uint8_t *data, const size_t len) {
+    uint32_t crc = crc32_feature(data,len - 4);
+    data[len - 4] = (crc >> 0) & 0xFF;
+    data[len - 3] = (crc >> 8) & 0xFF;
+    data[len - 2] = (crc >> 16) & 0xFF;
+    data[len - 1] = (crc >> 24) & 0xFF;
+}
+
 enum PowerState : uint8_t {
     Discharging         = 0x00, // Use PowerPercent
     Charging            = 0x01, // Use PowerPercent
@@ -216,7 +237,7 @@ struct __attribute__((packed)) USBGetStateData { // 63
 /*55  */ uint8_t AesCmac[8];
 };
 
-inline void print_hex(const int16_t* data,size_t size) {
+inline void print_hex(const uint8_t* data,size_t size) {
     for (int i = 0; i < size; i++) {
         std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(data[i]) << " ";
     }