refactor: add SetStateData and audio send priority

src/audio.cpp

@@ -40,22 +40,6 @@ struct audio_raw_element {
     float data[512 * 2];
 };
 
-uint8_t state_data[63] = {
-    0xfd, 0xf7, 0x0, 0x0,
-    0x7f, 0x7f, // Headphones, Speaker
-    0xff, 0x9, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xa,
-    0x7, 0x0, 0x0, 0x2, 0x1,
-    0x00,
-    0xff, 0xd7, 0x00, // RGB LED: R, G, B (Nijika Color!)✨
-};
-
-void set_state_data(const uint8_t* data, const uint8_t len) {
-    memcpy(state_data, data, len);
-}
-
 void set_headset(bool state) {
     plug_headset = state;
 }
@@ -131,22 +115,19 @@ void audio_loop() {
         pkt[8] = buf_len; // 这 4 个字节的作用未知，调整没有效果
         pkt[9] = buf_len; // audio buffer length 只有调整这个字节生效。
         pkt[10] = packetCounter++;
-        pkt[11] = 0x10 | 0 << 6 | 1 << 7;
+        pkt[11] = 0x12 | 0 << 6 | 1 << 7;
-        pkt[12] = 63;
+        pkt[12] = SAMPLE_SIZE;
-        memcpy(pkt + 13, state_data, sizeof(state_data));
+        memcpy(pkt + 13, haptic_buf, SAMPLE_SIZE);
-        pkt[76] = 0x12 | 0 << 6 | 1 << 7;
+        pkt[77] = (plug_headset ? 0x16 : 0x13) | 0 << 6 | 1 << 7; // Speaker: 0x13
-        pkt[77] = SAMPLE_SIZE;
-        memcpy(pkt + 78, haptic_buf, SAMPLE_SIZE);
-        pkt[142] = (plug_headset ? 0x16 : 0x13) | 0 << 6 | 1 << 7; // Speaker: 0x13
         // L Headset Mono: 0x14
         // L Headset R Speaker: 0x15
         // Headset: 0x16
-        pkt[143] = 200;
+        pkt[78] = 200;
         critical_section_enter_blocking(&opus_cs);
-        memcpy(pkt + 144, opus_buf, 200);
+        memcpy(pkt + 79, opus_buf, 200);
         critical_section_exit(&opus_cs);
 
-        bt_write(pkt, sizeof(pkt));
+        bt_write(pkt, sizeof(pkt), true);
         haptic_buf_pos = 0;
     }
 }

src/audio.h

@@ -5,12 +5,9 @@
 #ifndef DS5_BRIDGE_AUDIO_H
 #define DS5_BRIDGE_AUDIO_H
 
-#include <cstdint>
-
 void audio_init();
 void audio_loop();
 void core1_entry();
 void set_headset(bool state);
-void set_state_data(const uint8_t* data, const uint8_t len);
 
 #endif //DS5_BRIDGE_AUDIO_H

src/bt.cpp

@@ -39,6 +39,7 @@ static bt_data_callback_t bt_data_callback = nullptr;
 static bool check_dse = false;
 unordered_map<uint8_t, vector<uint8_t> > feature_data;
 queue_t send_fifo;
+queue_t priority_send_fifo;
 
 struct send_element {
     uint8_t data[512];
@@ -86,6 +87,7 @@ void bt_l2cap_init() {
 
 int bt_init() {
     queue_init(&send_fifo, sizeof(send_element), 10);
+    queue_init(&priority_send_fifo, sizeof(send_element), 10);
 
     bt_l2cap_init();
 
@@ -475,14 +477,20 @@ static void l2cap_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t
         case L2CAP_EVENT_CAN_SEND_NOW: {
             // printf("[L2CAP] L2CAP_EVENT_CAN_SEND_NOW\n");
 
-            static send_element send_packet{};
+            send_element send_packet{};
-            if (queue_try_remove(&send_fifo, &send_packet)) {
+            bool get_data = false;
+            if (!queue_is_empty(&priority_send_fifo)) {
+                get_data = queue_try_remove(&priority_send_fifo, &send_packet);
+            }else {
+                get_data = queue_try_remove(&send_fifo, &send_packet);
+            }
+            if (get_data) {
                 const uint8_t status = l2cap_send(hid_interrupt_cid, send_packet.data, send_packet.len);
                 if (status != 0) {
                     printf("[L2CAP] L2CAP Send Error, Status: 0x%02X\n", status);
                 }
             }
-            if (!queue_is_empty(&send_fifo)) {
+            if (!queue_is_empty(&priority_send_fifo) || !queue_is_empty(&send_fifo)) {
                 l2cap_request_can_send_now_event(hid_interrupt_cid);
             }
             break;
@@ -490,7 +498,7 @@ static void l2cap_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t
     }
 }
 
-void bt_write(uint8_t *data, uint16_t len) {
+void bt_write(const uint8_t *data, const uint16_t len, const bool priority) {
     if (hid_interrupt_cid == 0) return;
     static send_element packet{};
     memset(packet.data, 0, 512);
@@ -499,11 +507,18 @@ void bt_write(uint8_t *data, uint16_t len) {
     memcpy(packet.data + 1, data, len);
     fill_output_report_checksum(packet.data + 1, len);
 
-    if (!queue_try_add(&send_fifo, &packet)) {
+    if (priority) {
-        printf("[L2CAP bt_write] Error: Failed to add packet to send FIFO\n");
+        if (!queue_try_add(&priority_send_fifo, &packet)) {
-        return;
+            printf("[L2CAP bt_write] Error: Failed to add packet to send FIFO\n");
+            return;
+        }
+    }else {
+        if (!queue_try_add(&send_fifo, &packet)) {
+            printf("[L2CAP bt_write] Error: Failed to add packet to send FIFO\n");
+            return;
+        }
     }
-    if (queue_get_level(&send_fifo) == 1) {
+    if (queue_get_level(&send_fifo) + queue_get_level(&priority_send_fifo) == 1) {
         l2cap_request_can_send_now_event(hid_interrupt_cid);
     }
 }

src/bt.h

@@ -19,7 +19,7 @@ int bt_init();
 void bt_register_data_callback(bt_data_callback_t callback);
 void bt_send_packet(uint8_t *data, uint16_t len);
 void bt_send_control(uint8_t *data, uint16_t len);
-void bt_write(uint8_t* data,uint16_t len);
+void bt_write(const uint8_t *data, uint16_t len, bool priority = false);
 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);

src/main.cpp

@@ -23,7 +23,6 @@
 
 int reportSeqCounter = 0;
 uint8_t packetCounter = 0;
-bool spk_active = false;
 
 uint8_t interrupt_in_data[63] = {
     0x7f, 0x7d, 0x7f, 0x7e, 0x00, 0x00, 0xa7,
@@ -132,7 +131,6 @@ bool tud_audio_set_itf_cb(uint8_t rhport, tusb_control_request_t const *p_reques
 
     if (itf == 1) {
         printf("[AUDIO] Set interface Speaker to alternate setting %d\n", alt);
-        spk_active = alt;
     }
 
     return true;
@@ -158,10 +156,6 @@ void tud_hid_set_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t rep
     if (report_id == 0) {
         switch (buffer[0]) {
             case 0x02: {
-                set_state_data(buffer + 1, bufsize - 1);
-                if (spk_active) {
-                    break;
-                }
                 uint8_t outputData[78];
                 outputData[0] = 0x31;
                 outputData[1] = reportSeqCounter << 4;