Fix HID buffer lookup

The previous code assumed HID report ids were consecutive. This is
not true in the CircuitPython descriptor where report ids are fixed
for each report type.

Fixes #1617

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
         }