USB: composite: Compute interface numbers correctly when functions are hidden.

Signed-off-by: Mike Lockwood <lockwood@android.com>

diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 5d012b81ac5963bccad1fbf200586a85883d4dfe..5fa948978f20a4c5e352b65a4d1787de2ebe3a86 100644 (file)
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -274,18 +274,19 @@ static int config_buf(struct usb_configuration *config,
                enum usb_device_speed speed, void *buf, u8 type)
 {
        struct usb_config_descriptor    *c = buf;
+       struct usb_interface_descriptor *intf;
        void                            *next = buf + USB_DT_CONFIG_SIZE;
        int                             len = USB_BUFSIZ - USB_DT_CONFIG_SIZE;
        struct usb_function             *f;
        int                             status;
        int                             interfaceCount = 0;
+       u8 *dest;
 
        /* write the config descriptor */
        c = buf;
        c->bLength = USB_DT_CONFIG_SIZE;
        c->bDescriptorType = type;
-       /* wTotalLength is written later */
-       c->bNumInterfaces = config->next_interface_id;
+       /* wTotalLength and bNumInterfaces are written later */
        c->bConfigurationValue = config->bConfigurationValue;
        c->iConfiguration = config->iConfiguration;
        c->bmAttributes = USB_CONFIG_ATT_ONE | config->bmAttributes;
@@ -304,31 +305,35 @@ static int config_buf(struct usb_configuration *config,
        /* add each function's descriptors */
        list_for_each_entry(f, &config->functions, list) {
                struct usb_descriptor_header **descriptors;
+               struct usb_descriptor_header *descriptor;
 
                if (speed == USB_SPEED_HIGH)
                        descriptors = f->hs_descriptors;
                else
                        descriptors = f->descriptors;
-               if (f->hidden || !descriptors || descriptors[0] == NULL) {
-                       for (; f != config->interface[interfaceCount];) {
-                               interfaceCount++;
-                               c->bNumInterfaces--;
-                       }
+               if (f->hidden || !descriptors || descriptors[0] == NULL)
                        continue;
-               }
-               for (; f != config->interface[interfaceCount];)
-                       interfaceCount++;
-
                status = usb_descriptor_fillbuf(next, len,
                        (const struct usb_descriptor_header **) descriptors);
                if (status < 0)
                        return status;
+
+               /* set interface numbers dynamically */
+               dest = next;
+               while ((descriptor = *descriptors++) != NULL) {
+                       intf = (struct usb_interface_descriptor *)dest;
+                       if (intf->bDescriptorType == USB_DT_INTERFACE)
+                               intf->bInterfaceNumber = interfaceCount++;
+                       dest += intf->bLength;
+               }
+
                len -= status;
                next += status;
        }
 
        len = next - buf;
        c->wTotalLength = cpu_to_le16(len);
+       c->bNumInterfaces = interfaceCount;
        return len;
 }