Merge pull request #1721 from tannewt/fix_hid_report_lookup

Fix HID buffer lookup

1 files changed, 13 insertions, 7 deletions

diff --git a/shared-module/usb_hid/Device.c b/shared-module/usb_hid/Device.c
index 820e14ad0..0e256cb5e 100644
--- a/shared-module/usb_hid/Device.c
+++ b/shared-module/usb_hid/Device.c
@@ -60,27 +60,33 @@ void common_hal_usb_hid_device_send_report(usb_hid_device_obj_t *self, uint8_t*
     }
 }
 
+static usb_hid_device_obj_t* get_hid_device(uint8_t report_id) {
+    for (uint8_t i = 0; i < USB_HID_NUM_DEVICES; i++) {
+        if (usb_hid_devices[i].report_id == report_id) {
+            return &usb_hid_devices[i];
+        }
+    }
+    return NULL;
+}
+
 // Callbacks invoked when receive Get_Report request through control endpoint
 uint16_t tud_hid_generic_get_report_cb(uint8_t report_id, hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen) {
     // only support Input Report
     if ( report_type != HID_REPORT_TYPE_INPUT ) return 0;
 
-    // index is ID-1
-    uint8_t idx =  ( report_id ? (report_id-1) : 0 );
-
     // fill buffer with current report
-    memcpy(buffer, usb_hid_devices[idx].report_buffer, reqlen);
+    memcpy(buffer, get_hid_device(report_id)->report_buffer, reqlen);
     return reqlen;
 }
 
 // Callbacks invoked when receive Set_Report request through control endpoint
 void tud_hid_generic_set_report_cb(uint8_t report_id, hid_report_type_t report_type, uint8_t const* buffer, uint16_t bufsize) {
-    // index is ID-1
-    uint8_t idx =  ( report_id ? (report_id-1) : 0 );
+    usb_hid_device_obj_t* hid_device = get_hid_device(report_id);
 
     if ( report_type == HID_REPORT_TYPE_OUTPUT ) {
         // Check if it is Keyboard device
-        if ( (usb_hid_devices[idx].usage_page == HID_USAGE_PAGE_DESKTOP) && (usb_hid_devices[idx].usage == HID_USAGE_DESKTOP_KEYBOARD) ) {
+        if (hid_device->usage_page == HID_USAGE_PAGE_DESKTOP &&
+                hid_device->usage == HID_USAGE_DESKTOP_KEYBOARD) {
             // This is LED indicator (CapsLock, NumLock)
             // TODO Light up some LED here
         }