diff --git a/CMakeLists.txt b/CMakeLists.txt
index 93c9ab8..cdd8390 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -88,7 +88,8 @@ target_link_libraries(ds5-bridge
         hardware_interp
         hardware_timer
         pico_btstack_classic
-        pico_cyw43_arch_threadsafe_background
+#        pico_cyw43_arch_threadsafe_background
+        pico_cyw43_arch_poll
         pico_btstack_cyw43
         tinyusb_device
         tinyusb_board
diff --git a/src/bt.cpp b/src/bt.cpp
index 7dffdcb..94f1468 100644
--- a/src/bt.cpp
+++ b/src/bt.cpp
@@ -443,16 +443,16 @@ void bt_write(uint8_t *data, uint16_t len) {
     }
 }
 
-uint8_t *get_feature_data(uint8_t reportId, 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 (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);
         }
-        return 0;
+        return {};
     }
-    return feature_data[reportId].data();
+    return feature_data[reportId];
 }
 
 void init_feature() {
diff --git a/src/bt.h b/src/bt.h
index c77ee46..cc46f27 100644
--- a/src/bt.h
+++ b/src/bt.h
@@ -5,7 +5,8 @@
 #ifndef DS5_BRIDGE_BT_H
 #define DS5_BRIDGE_BT_H
 
-#include <stdint.h>
+#include <cstdint>
+#include <vector>
 
 enum CHANNEL_TYPE {
     INTERRUPT,
@@ -19,7 +20,7 @@ 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);
-uint8_t* get_feature_data(uint8_t reportId,uint16_t len);
+std::vector<uint8_t> get_feature_data(uint8_t reportId,uint16_t len);
 void init_feature();
 
 #endif //DS5_BRIDGE_BT_H
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 630a178..d9c9e9a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -7,9 +7,9 @@
 #include "usb.h"
 #include "bt.h"
 #include "utils.h"
-#include "pico/multicore.h"
 #include "resample.h"
 #include "audio.h"
+#include "pico/cyw43_arch.h"
 
 int reportSeqCounter = 0;
 uint8_t packetCounter = 0;
@@ -53,12 +53,12 @@ uint16_t tud_hid_get_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t
     (void) buffer;
     (void) reqlen;
 
-    uint8_t *feature_data = get_feature_data(report_id, reqlen);
-    if (feature_data) {
-        memcpy(buffer, feature_data, reqlen);
+    std::vector<uint8_t> feature_data = get_feature_data(report_id, reqlen);
+    if (!feature_data.empty()) {
+        memcpy(buffer, feature_data.data() + 1, feature_data.size() - 1);
     }
 
-    return feature_data ? reqlen : 0;
+    return feature_data.empty() ? 0 : feature_data.size() - 1;
 }
 
 // Invoked when received SET_REPORT control request or
@@ -104,6 +104,7 @@ int main() {
     audio_init();
 
     while (1) {
+        cyw43_arch_poll();
         tud_task();
         audio_loop();
         interrupt_loop();