1 files changed, 94 insertions, 31 deletions

diff --git a/utils/hwstub/lib/hwstub.c b/utils/hwstub/lib/hwstub.c
index 67a797ddfe..9f46159026 100644
--- a/utils/hwstub/lib/hwstub.c
+++ b/utils/hwstub/lib/hwstub.c
@@ -30,26 +30,78 @@ struct hwstub_device_t
 {
     libusb_device_handle *handle;
     int intf;
-    int bulk_in;
-    int bulk_out;
-    int int_in;
+    unsigned buf_sz;
+    uint16_t id;
 };
 
+int hwstub_probe(libusb_device *dev)
+{
+    struct libusb_config_descriptor *config = NULL;
+    int ret = -1;
+    if(libusb_get_config_descriptor(dev, 0, &config) != 0)
+        goto Lend;
+    /* search hwstub interface */
+    for(unsigned i = 0; i < config->bNumInterfaces; i++)
+    {
+        /* hwstub interface has only one setting */
+        if(config->interface[i].num_altsetting != 1)
+            continue;
+        const struct libusb_interface_descriptor *intf = &config->interface[i].altsetting[0];
+        /* check class/subclass/protocol */
+        if(intf->bInterfaceClass != HWSTUB_CLASS ||
+                intf->bInterfaceSubClass != HWSTUB_SUBCLASS ||
+                intf->bInterfaceProtocol != HWSTUB_PROTOCOL)
+            continue;
+        /* found ! */
+        ret = i;
+        break;
+    }
+Lend:
+    if(config)
+        libusb_free_config_descriptor(config);
+    return ret;
+}
+
+ssize_t hwstub_get_device_list(libusb_context *ctx, libusb_device ***list)
+{
+    libusb_device **great_list;
+    ssize_t great_cnt = libusb_get_device_list(ctx, &great_list);
+    if(great_cnt < 0)
+        return great_cnt;
+    /* allocate a list (size at least one NULL entry at the end) */
+    libusb_device **mylist = malloc(sizeof(libusb_device *) * (great_cnt + 1));
+    memset(mylist, 0, sizeof(libusb_device *) * (great_cnt + 1));
+    /* list hwstub devices */
+    ssize_t cnt = 0;
+    for(int i = 0; i < great_cnt; i++)
+        if(hwstub_probe(great_list[i]) >= 0)
+        {
+            libusb_ref_device(great_list[i]);
+            mylist[cnt++] = great_list[i];
+        }
+    /* free old list */
+    libusb_free_device_list(great_list, 1);
+    /* return */
+    *list = mylist;
+    return cnt;
+}
+
 struct hwstub_device_t *hwstub_open(libusb_device_handle *handle)
 {
     struct hwstub_device_t *dev = malloc(sizeof(struct hwstub_device_t));
     memset(dev, 0, sizeof(struct hwstub_device_t));
     dev->handle = handle;
+    dev->intf = -1;
+    dev->buf_sz = 1024; /* default size */
     libusb_device *mydev = libusb_get_device(dev->handle);
-
-    int config_id;
-    libusb_get_configuration(dev->handle, &config_id);
-    struct libusb_device_descriptor dev_desc;
-    libusb_get_device_descriptor(mydev, &dev_desc);
-    if(dev_desc.bDeviceClass != HWSTUB_CLASS ||
-            dev_desc.bDeviceSubClass != HWSTUB_SUBCLASS ||
-            dev_desc.bDeviceProtocol != HWSTUB_PROTOCOL)
+    dev->intf = hwstub_probe(mydev);
+    if(dev->intf == -1)
         goto Lerr;
+    /* try to get actual buffer size */
+    struct hwstub_layout_desc_t layout;
+    int sz = hwstub_get_desc(dev, HWSTUB_DT_LAYOUT, &layout, sizeof(layout));
+    if(sz == (int)sizeof(layout))
+        dev->buf_sz = layout->dBufferSize;
     return dev;
 
 Lerr:
@@ -66,49 +118,60 @@ int hwstub_release(struct hwstub_device_t *dev)
 int hwstub_get_desc(struct hwstub_device_t *dev, uint16_t desc, void *info, size_t sz)
 {
     return libusb_control_transfer(dev->handle,
-        LIBUSB_REQUEST_TYPE_STANDARD | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_IN,
-        LIBUSB_REQUEST_GET_DESCRIPTOR, desc << 8, 0, info, sz, 1000);
+        LIBUSB_REQUEST_TYPE_STANDARD | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
+        LIBUSB_REQUEST_GET_DESCRIPTOR, desc << 8, dev->intf, info, sz, 1000);
 }
 
 int hwstub_get_log(struct hwstub_device_t *dev, void *buf, size_t sz)
 {
     return libusb_control_transfer(dev->handle,
-        LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_IN,
-        HWSTUB_GET_LOG, 0, 0, buf, sz, 1000);
+        LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
+        HWSTUB_GET_LOG, 0, dev->intf, buf, sz, 1000);
+}
+
+static int _hwstub_read(struct hwstub_device_t *dev, uint32_t addr, void *buf, size_t sz)
+{
+    struct hwstub_read_req_t read;
+    read.dAddress = addr;
+    int size = libusb_control_transfer(dev->handle,
+        LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
+        HWSTUB_READ, dev->id, dev->intf, &read, sizeof(read), 1000);
+    if(size != (int)sizeof(read))
+        return -1;
+    return libusb_control_transfer(dev->handle,
+        LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
+        HWSTUB_READ2, dev->id++, dev->intf, &read, sizeof(read), 1000);
+}
+
+int hwstub_write(struct hwstub_device_t *dev, uint32_t addr, void *buf, size_t sz)
+{
 }
 
 int hwstub_rw_mem(struct hwstub_device_t *dev, int read, uint32_t addr, void *buf, size_t sz)
 {
-    size_t tot_sz = 0;
-    while(sz)
-    {
-        uint16_t xfer = MIN(1 * 1024, sz);
-        int ret = libusb_control_transfer(dev->handle,
-            LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE |
-            (read ? LIBUSB_ENDPOINT_IN : LIBUSB_ENDPOINT_OUT),
-            HWSTUB_RW_MEM, addr & 0xffff, addr >> 16, buf, xfer, 1000);
-        if(ret != xfer)
-            return ret;
-        sz -= xfer;
-        addr += xfer;
-        buf += xfer;
-        tot_sz += xfer;
-    }
-    return tot_sz;
+    return read ? hwstub_read(dev, addr, buf, sz) : hwstub_write(dev, addr, buf, sz);
 }
 
 int hwstub_call(struct hwstub_device_t *dev, uint32_t addr)
 {
+#if 0
     return libusb_control_transfer(dev->handle,
             LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE |
             LIBUSB_ENDPOINT_OUT, HWSTUB_CALL, addr & 0xffff, addr >> 16, NULL, 0,
             1000);
+#else
+    return -1;
+#endif
 }
 
 int hwstub_jump(struct hwstub_device_t *dev, uint32_t addr)
 {
+#if 0
     return libusb_control_transfer(dev->handle,
             LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE |
             LIBUSB_ENDPOINT_OUT, HWSTUB_JUMP, addr & 0xffff, addr >> 16, NULL, 0,
             1000);
+#else
+    return -1;
+#endif
 }