usb devices with multiple configurations/interfaces

@@ -46,8 +46,7 @@ typedef int USBScanFunc(void *opaque, in
 static int usb_host_find_device(int *pbus_num, int *paddr, 
                                 char *product_name, int product_name_size,
                                 const char *devname);
-
-//#define DEBUG
+#define DEBUG
 
 #define USBDEVFS_PATH "/proc/bus/usb"
 #define PRODUCT_NAME_SZ 32

@@ -55,8 +54,88 @@ static int usb_host_find_device(int *pbu
 typedef struct USBHostDevice {
     USBDevice dev;
     int fd;
+    int configuration;
+    uint8_t descr[1024];
+    int descr_len;
 } USBHostDevice;
 
+static int usb_host_update_interfaces(USBHostDevice *dev, int configuration)
+{
+    int dev_descr_len, config_descr_len;
+    int interface, nb_interfaces, nb_configurations;
+    int ret, i;
+
+    if (configuration == 0) // address state - ignore
+				return 1;
+
+    i = 0;
+    dev_descr_len = dev->descr[0];
+    if (dev_descr_len > dev->descr_len)
+				goto fail;
+    nb_configurations = dev->descr[17];
+		
+    i += dev_descr_len;
+		while (i < dev->descr_len) {
+#ifdef DEBUG
+        printf("i is %d, descr_len is %d, dl %d, dt %d\n", i, dev->descr_len,
+               dev->descr[i], dev->descr[i+1]);
+#endif
+        if (dev->descr[i+1] != USB_DT_CONFIG) {
+            i += dev->descr[i];
+            continue;
+        }
+        config_descr_len = dev->descr[i];
+        
+        if (configuration == dev->descr[i + 5])
+            break;
+
+        i += config_descr_len;
+    }
+
+    if (i >= dev->descr_len) {
+        printf("usb_host: error - device has no matching configuration\n");
+        goto fail;
+    }
+    nb_interfaces = dev->descr[i + 4];
+
+#ifdef USBDEVFS_DISCONNECT
+    /* earlier Linux 2.4 do not support that */
+    {
+        struct usbdevfs_ioctl ctrl;
+        for (interface = 0; interface < nb_interfaces; interface++) {
+            ctrl.ioctl_code = USBDEVFS_DISCONNECT;
+            ctrl.ifno = interface;
+            ret = ioctl(dev->fd, USBDEVFS_IOCTL, &ctrl);
+            if (ret < 0 && errno != ENODATA) {
+                perror("USBDEVFS_DISCONNECT");
+                goto fail;
+            }
+        }
+    }
+#endif
+
+    /* XXX: only grab if all interfaces are free */
+    for (interface = 0; interface < nb_interfaces; interface++) {
+        ret = ioctl(dev->fd, USBDEVFS_CLAIMINTERFACE, &interface);
+        if (ret < 0) {
+            if (errno == EBUSY) {
+                fprintf(stderr, "usb_host: warning - device already grabbed\n");
+            } else {
+                perror("USBDEVFS_CLAIMINTERFACE");
+            }
+        fail:
+            return 0;
+        }
+    }
+
+#ifdef DEBUG
+    printf("usb_host: %d interfaces claimed for configuration %d\n", nb_interfaces,
+           configuration);
+#endif
+
+    return 1;
+}
+					
 static void usb_host_handle_reset(USBDevice *dev)
 {
 #if 0

@@ -85,13 +164,25 @@ static int usb_host_handle_control(USBDe
 {
     USBHostDevice *s = (USBHostDevice *)dev;
     struct usb_ctrltransfer ct;
+    int intf_update_required = 0;
     int ret;
 
     if (request == (DeviceOutRequest | USB_REQ_SET_ADDRESS)) {
         /* specific SET_ADDRESS support */
         dev->addr = value;
         return 0;
+    } else if (request == (DeviceOutRequest | USB_REQ_SET_CONFIGURATION)) {
+#ifdef DEBUG
+        printf("usb_host_handle_control: SET_CONFIGURATION request - config %d\n",
+               value & 0xff);
+#endif
+        if (s->configuration != (value & 0xff)) {
+            s->configuration = (value & 0xff);
+            intf_update_required = 1;
+        }
+        goto do_request;
     } else {
+    do_request:
         ct.bRequestType = request >> 8;
         ct.bRequest = request;
         ct.wValue = value;

@@ -108,6 +199,12 @@ static int usb_host_handle_control(USBDe
                 return USB_RET_STALL;
             }
         } else {
+            if (intf_update_required) {
+#ifdef DEBUG
+                printf("usb_host_handle_control: updating interfaces\n");
+#endif                
+                usb_host_update_interfaces(s, value & 0xff);
+            }
             return ret;
         }
    }

@@ -148,15 +245,17 @@ static int usb_host_handle_data(USBDevic
 /* XXX: exclude high speed devices or implement EHCI */
 USBDevice *usb_host_device_open(const char *devname)
 {
-    int fd, interface, ret, i;
-    USBHostDevice *dev;
+    int fd = -1, ret;
+    USBHostDevice *dev = NULL;
     struct usbdevfs_connectinfo ci;
-    uint8_t descr[1024];
     char buf[1024];
-    int descr_len, dev_descr_len, config_descr_len, nb_interfaces;
     int bus_num, addr;
     char product_name[PRODUCT_NAME_SZ];
 
+    dev = qemu_mallocz(sizeof(USBHostDevice));
+    if (!dev)
+        goto fail;
+
     if (usb_host_find_device(&bus_num, &addr, 
                              product_name, sizeof(product_name),
                              devname) < 0) 

@@ -170,55 +269,29 @@ USBDevice *usb_host_device_open(const ch
         return NULL;
     }
 
-    /* read the config description */
-    descr_len = read(fd, descr, sizeof(descr));
-    if (descr_len <= 0) {
-        perror("read descr");
-        goto fail;
-    }
-    
-    i = 0;
-    dev_descr_len = descr[0];
-    if (dev_descr_len > descr_len)
-        goto fail;
-    i += dev_descr_len;
-    config_descr_len = descr[i];
-    if (i + config_descr_len > descr_len)
-        goto fail;
-    nb_interfaces = descr[i + 4];
-    if (nb_interfaces != 1) {
-        /* NOTE: currently we grab only one interface */
-        fprintf(stderr, "usb_host: only one interface supported\n");
+    /* read the device description */
+    dev->descr_len = read(fd, dev->descr, sizeof(dev->descr));
+    if (dev->descr_len <= 0) {
+        perror("usb_host_update_interfaces: reading device data failed");
         goto fail;
     }
 
-#ifdef USBDEVFS_DISCONNECT
-    /* earlier Linux 2.4 do not support that */
+#ifdef DEBUG
     {
-        struct usbdevfs_ioctl ctrl;
-        ctrl.ioctl_code = USBDEVFS_DISCONNECT;
-        ctrl.ifno = 0;
-        ret = ioctl(fd, USBDEVFS_IOCTL, &ctrl);
-        if (ret < 0 && errno != ENODATA) {
-            perror("USBDEVFS_DISCONNECT");
-            goto fail;
-        }
+        int x;
+        printf("=== begin dumping device descriptor data ===\n");
+        for (x = 0; x < dev->descr_len; x++)
+            printf("%02x ", dev->descr[x]);
+        printf("\n=== end dumping device descriptor data ===\n");
     }
 #endif
 
-    /* XXX: only grab if all interfaces are free */
-    interface = 0;
-    ret = ioctl(fd, USBDEVFS_CLAIMINTERFACE, &interface);
-    if (ret < 0) {
-        if (errno == EBUSY) {
-            fprintf(stderr, "usb_host: device already grabbed\n");
-        } else {
-            perror("USBDEVFS_CLAIMINTERFACE");
-        }
-    fail:
-        close(fd);
-        return NULL;
-    }
+    dev->fd = fd;
+    dev->configuration = 1;
+
+    // XXX - do something about initial configuration
+    if (!usb_host_update_interfaces(dev, 1))
+        goto fail;
 
     ret = ioctl(fd, USBDEVFS_CONNECTINFO, &ci);
     if (ret < 0) {

@@ -230,10 +303,6 @@ USBDevice *usb_host_device_open(const ch
     printf("host USB device %d.%d grabbed\n", bus_num, addr);
 #endif    
 
-    dev = qemu_mallocz(sizeof(USBHostDevice));
-    if (!dev)
-        goto fail;
-    dev->fd = fd;
     if (ci.slow)
         dev->dev.speed = USB_SPEED_LOW;
     else

@@ -253,6 +322,11 @@ USBDevice *usb_host_device_open(const ch
                 product_name);
 
     return (USBDevice *)dev;
+fail:
+    if (dev)
+        qemu_free(dev);
+    close(fd);
+    return NULL;
 }
 
 static int get_tag_value(char *buf, int buf_size,