--- qemu/usb-linux.c 2006-06-26 16:00:51.000000000 -0500 +++ qemu/usb-linux.c 2006-07-03 02:25:29.000000000 -0500 @@ -46,8 +46,7 @@ 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 @@ 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, int destroy) { #if 0 @@ -76,13 +155,25 @@ { 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; @@ -99,6 +190,12 @@ 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; } } @@ -140,15 +237,17 @@ /* 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) @@ -162,55 +261,29 @@ 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) { @@ -222,10 +295,6 @@ 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 @@ -244,6 +313,11 @@ 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,