From 645a64b4911d7cadf5749d7375544fc2384e70ba Mon Sep 17 00:00:00 2001
From: Qi Zhou <atmgnd@outlook.com>
Date: Mon, 14 Nov 2022 20:55:44 +0800
Subject: usb: fix wrong init of keyboard/mouse's if first interface is not
 boot protocol

There is always some endpoint descriptors after each interface descriptor, We
should only decrement num_iface if interface type is USB_DT_INTERFACE, see
https://www.beyondlogic.org/usbnutshell/usb5.shtml#ConfigurationDescriptors

Signed-off-by: Qi Zhou <atmgnd@outlook.com>
---
 src/hw/usb-hid.c | 13 ++++++++-----
 src/hw/usb.c     | 12 +++++++-----
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/src/hw/usb-hid.c b/src/hw/usb-hid.c
index 92c6b19..dec198a 100644
--- a/src/hw/usb-hid.c
+++ b/src/hw/usb-hid.c
@@ -22,13 +22,13 @@ struct usb_pipe *mouse_pipe VARFSEG;
 
 // Send USB HID protocol message.
 static int
-set_protocol(struct usb_pipe *pipe, u16 val)
+set_protocol(struct usb_pipe *pipe, u16 val, u16 inferface)
 {
     struct usb_ctrlrequest req;
     req.bRequestType = USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE;
     req.bRequest = HID_REQ_SET_PROTOCOL;
     req.wValue = val;
-    req.wIndex = 0;
+    req.wIndex = inferface;
     req.wLength = 0;
     return usb_send_default_control(pipe, &req, NULL);
 }
@@ -76,9 +76,12 @@ usb_kbd_setup(struct usbdevice_s *usbdev
     }
 
     // Enable "boot" protocol.
-    int ret = set_protocol(usbdev->defpipe, 0);
-    if (ret)
+    int ret = set_protocol(usbdev->defpipe, 0, usbdev->iface->bInterfaceNumber);
+    if (ret) {
+        dprintf(3, "Failed to set boot protocol\n");
         return -1;
+    }
+
     // Periodically send reports to enable key repeat.
     ret = set_idle(usbdev->defpipe, KEYREPEATMS);
     if (ret)
@@ -118,7 +121,7 @@ usb_mouse_setup(struct usbdevice_s *usbdev
     }
 
     // Enable "boot" protocol.
-    int ret = set_protocol(usbdev->defpipe, 0);
+    int ret = set_protocol(usbdev->defpipe, 0, usbdev->iface->bInterfaceNumber);
     if (ret)
         return -1;
 
diff --git a/src/hw/usb.c b/src/hw/usb.c
index 38a866a..409da8c 100644
--- a/src/hw/usb.c
+++ b/src/hw/usb.c
@@ -372,17 +372,19 @@ configure_usb_device(struct usbdevice_s *usbdev)
     void *config_end = (void*)config + config->wTotalLength;
     struct usb_interface_descriptor *iface = (void*)(&config[1]);
     for (;;) {
-        if (!num_iface-- || (void*)iface + iface->bLength > config_end)
+        if (!num_iface || (void*)iface + iface->bLength > config_end)
             // Not a supported device.
             goto fail;
-        if (iface->bDescriptorType == USB_DT_INTERFACE
-            && (iface->bInterfaceClass == USB_CLASS_HUB
+        if (iface->bDescriptorType == USB_DT_INTERFACE) {
+            num_iface--;
+            if (iface->bInterfaceClass == USB_CLASS_HUB
                 || (iface->bInterfaceClass == USB_CLASS_MASS_STORAGE
                     && (iface->bInterfaceProtocol == US_PR_BULK
                         || iface->bInterfaceProtocol == US_PR_UAS))
                 || (iface->bInterfaceClass == USB_CLASS_HID
-                    && iface->bInterfaceSubClass == USB_INTERFACE_SUBCLASS_BOOT)))
-            break;
+                    && iface->bInterfaceSubClass == USB_INTERFACE_SUBCLASS_BOOT))
+                break;
+        }
         iface = (void*)iface + iface->bLength;
     }
 
-- 
cgit v1.2.1