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 11944bd..46a6508 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -278,18 +278,19 @@
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;
@@ -308,31 +309,35 @@
/* 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;
}