hwstub: remove protocol to make it use its own interface

This way, hwstub can be implemented along with other usb features/interfaces.

Change-Id: I7148cab845049cc0a8b8e740fa0d52d3a385eaed

5 files changed, 347 insertions, 135 deletions

diff --git a/utils/hwstub/hwstub_protocol.h b/utils/hwstub/hwstub_protocol.h
index 2c11025b1e..02c065cbe0 100644
--- a/utils/hwstub/hwstub_protocol.h
+++ b/utils/hwstub/hwstub_protocol.h
@@ -21,46 +21,46 @@
 #ifndef __HWSTUB_PROTOCOL__
 #define __HWSTUB_PROTOCOL__
 
-#define HWSTUB_CLASS        0xff
-#define HWSTUB_SUBCLASS     0xac
-#define HWSTUB_PROTOCOL     0x1d
+/**
+ * HWStub protocol version
+ */
 
-#define HWSTUB_VERSION_MAJOR    3
+#define HWSTUB_VERSION_MAJOR    4
 #define HWSTUB_VERSION_MINOR    0
-#define HWSTUB_VERSION_REV      1
+#define HWSTUB_VERSION_REV      0
 
 #define HWSTUB_VERSION__(maj,min,rev) #maj"."#min"."#rev
 #define HWSTUB_VERSION_(maj,min,rev) HWSTUB_VERSION__(maj,min,rev)
 #define HWSTUB_VERSION HWSTUB_VERSION_(HWSTUB_VERSION_MAJOR,HWSTUB_VERSION_MINOR,HWSTUB_VERSION_REV)
 
+/**
+ * A device can use any VID:PID but in case hwstub is in full control of the
+ * device, the preferred VID:PID is the following.
+ */
+
 #define HWSTUB_USB_VID  0xfee1
 #define HWSTUB_USB_PID  0xdead
 
 /**
- * Control commands
- *
- * These commands are sent to the device, using the standard bRequest field
- * of the SETUP packet. This is to take advantage of both wIndex and wValue
- * although it would have been more correct to send them to the interface.
+ * The device class should be per interface and the hwstub interface must use
+ * the following class, subclass and protocol.
  */
 
-/* list of commands */
-#define HWSTUB_GET_LOG      0 /* optional */
-#define HWSTUB_RW_MEM       1 /* optional */
-#define HWSTUB_CALL         2 /* optional */
-#define HWSTUB_JUMP         3 /* optional */
+#define HWSTUB_CLASS        0xff
+#define HWSTUB_SUBCLASS     0x57
+#define HWSTUB_PROTOCOL     0x0b
 
 /**
- * Descriptors can be retrieve using configuration descriptor or individually
+ * Descriptors can be retrieved using configuration descriptor or individually
  * using the standard GetDescriptor request on the interface.
  */
 
-/* list of possible information */
-#define HWSTUB_DT_VERSION     0x41 /* mandatory */
-#define HWSTUB_DT_LAYOUT      0x42 /* mandatory */
-#define HWSTUB_DT_TARGET      0x43 /* mandatory */
-#define HWSTUB_DT_STMP        0x44 /* optional */
-#define HWSTUB_DT_PP          0x45 /* optional */
+#define HWSTUB_DT_VERSION   0x41 /* mandatory */
+#define HWSTUB_DT_LAYOUT    0x42 /* mandatory */
+#define HWSTUB_DT_TARGET    0x43 /* mandatory */
+#define HWSTUB_DT_STMP      0x44 /* optional */
+#define HWSTUB_DT_PP        0x45 /* optional */
+#define HWSTUB_DT_DEVICE    0x46 /* optional */
 
 struct hwstub_version_desc_t
 {
@@ -120,25 +120,70 @@ struct hwstub_target_desc_t
     char bName[58];
 } __attribute__((packed));
 
+struct hwstub_device_desc_t
+{
+    uint8_t bLength;
+    uint8_t bDescriptorType;
+    /* Give the bRequest value for */
+} __attribute__((packed));
+
+/**
+ * Control commands
+ *
+ * These commands are sent to the interface, using the standard bRequest field
+ * of the SETUP packet. The wIndex contains the interface number. The wValue
+ * contains an ID which is used for requests requiring several transfers.
+ */
+
+#define HWSTUB_GET_LOG  0x40
+#define HWSTUB_READ     0x41
+#define HWSTUB_READ2    0x42
+#define HWSTUB_WRITE    0x43
+#define HWSTUB_EXEC     0x44
+
 /**
  * HWSTUB_GET_LOG:
  * The log is returned as part of the control transfer.
  */
 
 /**
- * HWSTUB_RW_MEM:
- * The 32-bit address is split into two parts.
- * The low 16-bit are stored in wValue and the upper
- * 16-bit are stored in wIndex. Depending on the transfer direction,
- * the transfer is either a read or a write.
- * The read/write on the device are guaranteed to be 16-bit/32-bit when
- * possible, making it suitable to read/write registers. */
+ * HWSTUB_READ and HWSTUB_READ2:
+ * Read a range of memory. The request works in two steps: first the host
+ * sends HWSTUB_READ with the parameters (address, length) and then
+ * a HWSTUB_READ2 to retrieve the buffer. Both requests must use the same
+ * ID in wValue, otherwise the second request will be STALLed.
+ */
+
+struct hwstub_read_req_t
+{
+    uint32_t dAddress;
+} __attribute__((packed));
+
+/**
+ * HWSTUB_WRITE
+ * Write a range of memory. The payload starts with the following header, everything
+ * which follows is data.
+ */
+struct hwstub_write_req_t
+{
+    uint32_t dAddress;
+} __attribute__((packed));
 
 /**
- * HWSTUB_{CALL,JUMP}:
- * The 32-bit address is split into two parts.
- * The low 16-bit are stored in wValue and the upper
- * 16-bit are stored in wIndex. Depending on the transfer direction,
- * the transfer is either a read or a write. */
+ * HWSTUB_EXEC:
+ * Execute code at an address. Several options are available regarding ARM vs Thumb,
+ * jump vs call.
+ */
+
+#define HWSTUB_EXEC_ARM     (0 << 0) /* target code is ARM */
+#define HWSTUB_EXEC_THUMB   (1 << 0) /* target code is Thumb */
+#define HWSTUB_EXEC_JUMP    (0 << 1) /* branch, code will never turn */
+#define HWSTUB_EXEC_CALL    (1 << 1) /* call and expect return */
+
+struct hwstub_exec_req_t
+{
+    uint32_t dAddress;
+    uint16_t bmFlags;
+};
 
 #endif /* __HWSTUB_PROTOCOL__ */
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
 }
diff --git a/utils/hwstub/lib/hwstub.h b/utils/hwstub/lib/hwstub.h
index 69fdc63988..1599f6508b 100644
--- a/utils/hwstub/lib/hwstub.h
+++ b/utils/hwstub/lib/hwstub.h
@@ -36,6 +36,12 @@ extern "C" {
 
 struct hwstub_device_t;
 
+/* Returns hwstub interface, or -1 if none was found */
+int hwstub_probe(libusb_device *dev);
+/* Helper function which returns a list of all hwstub devices found. The caller
+ * must unref all of them when done, possibly using  libusb_free_device_list().
+ * Return number of devices or <0 on error */
+ssize_t hwstub_get_device_list(libusb_context *ctx, libusb_device ***list);
 /* Returns NULL on error */
 struct hwstub_device_t *hwstub_open(libusb_device_handle *handle);
 /* Returns 0 on success. Does *NOT* close the usb handle */
@@ -46,6 +52,8 @@ int hwstub_get_desc(struct hwstub_device_t *dev, uint16_t desc, void *info, size
 /* Returns number of bytes filled */
 int hwstub_get_log(struct hwstub_device_t *dev, void *buf, size_t sz);
 /* Returns number of bytes written/read or <0 on error */
+int hwstub_read(struct hwstub_device_t *dev, uint32_t addr, void *buf, size_t sz);
+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);
 /* Returns <0 on error */
 int hwstub_call(struct hwstub_device_t *dev, uint32_t addr);
diff --git a/utils/hwstub/stub/main.c b/utils/hwstub/stub/main.c
index c831de0196..a8d99fc863 100644
--- a/utils/hwstub/stub/main.c
+++ b/utils/hwstub/stub/main.c
@@ -45,15 +45,14 @@ static bool g_exit = false;
  * 
  */
 
-static struct usb_device_descriptor __attribute__((aligned(2)))
-    device_descriptor=
+static struct usb_device_descriptor device_descriptor=
 {
     .bLength            = sizeof(struct usb_device_descriptor),
     .bDescriptorType    = USB_DT_DEVICE,
     .bcdUSB             = 0x0200,
-    .bDeviceClass       = HWSTUB_CLASS,
-    .bDeviceSubClass    = HWSTUB_SUBCLASS,
-    .bDeviceProtocol    = HWSTUB_PROTOCOL,
+    .bDeviceClass       = USB_CLASS_PER_INTERFACE,
+    .bDeviceSubClass    = 0,
+    .bDeviceProtocol    = 0,
     .bMaxPacketSize0    = 64,
     .idVendor           = HWSTUB_USB_VID,
     .idProduct          = HWSTUB_USB_PID,
@@ -66,29 +65,41 @@ static struct usb_device_descriptor __attribute__((aligned(2)))
 
 #define USB_MAX_CURRENT 200
 
-static struct usb_config_descriptor __attribute__((aligned(2)))
-                                    config_descriptor =
+static struct usb_config_descriptor config_descriptor =
 {
     .bLength             = sizeof(struct usb_config_descriptor),
     .bDescriptorType     = USB_DT_CONFIG,
     .wTotalLength        = 0, /* will be filled in later */
-    .bNumInterfaces      = 0,
+    .bNumInterfaces      = 1,
     .bConfigurationValue = 1,
     .iConfiguration      = 0,
     .bmAttributes        = USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
     .bMaxPower           = (USB_MAX_CURRENT + 1) / 2, /* In 2mA units */
 };
 
-static const struct usb_string_descriptor __attribute__((aligned(2)))
-    usb_string_iManufacturer =
+#define USB_HWSTUB_INTF 0
+
+static struct usb_interface_descriptor interface_descriptor =
+{
+    .bLength            = sizeof(struct usb_interface_descriptor),
+    .bDescriptorType    = USB_DT_INTERFACE,
+    .bInterfaceNumber   = USB_HWSTUB_INTF,
+    .bAlternateSetting  = 0,
+    .bNumEndpoints      = 0,
+    .bInterfaceClass    = HWSTUB_CLASS,
+    .bInterfaceSubClass = HWSTUB_SUBCLASS,
+    .bInterfaceProtocol = HWSTUB_PROTOCOL,
+    .iInterface         = 3
+};
+
+static const struct usb_string_descriptor usb_string_iManufacturer =
 {
     24,
     USB_DT_STRING,
     {'R', 'o', 'c', 'k', 'b', 'o', 'x', '.', 'o', 'r', 'g'}
 };
 
-static const struct usb_string_descriptor __attribute__((aligned(2)))
-    usb_string_iProduct =
+static const struct usb_string_descriptor usb_string_iProduct =
 {
     44,
     USB_DT_STRING,
@@ -97,17 +108,22 @@ static const struct usb_string_descriptor __attribute__((aligned(2)))
      's', 't', 'u', 'b'}
 };
 
+static const struct usb_string_descriptor usb_string_iInterface =
+{
+    14,
+    USB_DT_STRING,
+    {'H', 'W', 'S', 't', 'u', 'b'}
+};
+
 /* this is stringid #0: languages supported */
-static const struct usb_string_descriptor __attribute__((aligned(2)))
-    lang_descriptor =
+static const struct usb_string_descriptor lang_descriptor =
 {
     4,
     USB_DT_STRING,
     {0x0409} /* LANGID US English */
 };
 
-static struct hwstub_version_desc_t __attribute__((aligned(2)))
-    version_descriptor =
+static struct hwstub_version_desc_t version_descriptor =
 {
     sizeof(struct hwstub_version_desc_t),
     HWSTUB_DT_VERSION,
@@ -116,8 +132,7 @@ static struct hwstub_version_desc_t __attribute__((aligned(2)))
     HWSTUB_VERSION_REV
 };
 
-static struct hwstub_layout_desc_t __attribute__((aligned(2)))
-    layout_descriptor =
+static struct hwstub_layout_desc_t layout_descriptor =
 {
     sizeof(struct hwstub_layout_desc_t),
     HWSTUB_DT_LAYOUT,
@@ -131,6 +146,7 @@ static const struct usb_string_descriptor* const usb_strings[USB_NUM_STRINGS] =
    &lang_descriptor,
    &usb_string_iManufacturer,
    &usb_string_iProduct,
+   &usb_string_iInterface
 };
 
 uint8_t *usb_buffer = oc_bufferstart;
@@ -176,6 +192,10 @@ static void handle_std_dev_desc(struct usb_ctrlrequest *req)
             }
             size = sizeof(struct usb_config_descriptor);
 
+            /* interface desc */
+            memcpy(usb_buffer + size, (void *)&interface_descriptor,
+                sizeof(interface_descriptor));
+            size += sizeof(interface_descriptor);
             /* hwstub version */
             memcpy(usb_buffer + size, (void *)&version_descriptor,
                 sizeof(version_descriptor));
@@ -194,7 +214,8 @@ static void handle_std_dev_desc(struct usb_ctrlrequest *req)
             target_get_config_desc(usb_buffer + size, &size);
             /* fix config descriptor */
             config_descriptor.wTotalLength = size;
-            memcpy(usb_buffer, (void *)&config_descriptor, sizeof(config_descriptor));
+            memcpy(usb_buffer, (void *)&config_descriptor,
+                sizeof(config_descriptor));
 
             ptr = usb_buffer;
             break;
@@ -208,22 +229,7 @@ static void handle_std_dev_desc(struct usb_ctrlrequest *req)
             else
                 usb_drv_stall(EP_CONTROL, true, true);
             break;
-        case HWSTUB_DT_VERSION:
-            ptr = &version_descriptor;
-            size = sizeof(version_descriptor);
-            break;
-        case HWSTUB_DT_LAYOUT:
-            ptr = &layout_descriptor;
-            size = sizeof(layout_descriptor);
-            break;
-        case HWSTUB_DT_TARGET:
-            ptr = &target_descriptor;
-            size = sizeof(target_descriptor);
-            break;
         default:
-            target_get_desc(req->wValue >> 8, &ptr);
-            if(ptr != 0)
-                size = ((struct usb_descriptor_header *)ptr)->bLength;
             break;
     }
 
@@ -271,12 +277,70 @@ static void handle_std_dev_req(struct usb_ctrlrequest *req)
     }
 }
 
+static void handle_std_intf_desc(struct usb_ctrlrequest *req)
+{
+    int size;
+    void* ptr = NULL;
+
+    switch(req->wValue >> 8)
+    {
+        case HWSTUB_DT_VERSION:
+            ptr = &version_descriptor;
+            size = sizeof(version_descriptor);
+            break;
+        case HWSTUB_DT_LAYOUT:
+            ptr = &layout_descriptor;
+            size = sizeof(layout_descriptor);
+            break;
+        case HWSTUB_DT_TARGET:
+            ptr = &target_descriptor;
+            size = sizeof(target_descriptor);
+            break;
+        default:
+            target_get_desc(req->wValue >> 8, &ptr);
+            if(ptr != 0)
+                size = ((struct usb_descriptor_header *)ptr)->bLength;
+            break;
+    }
+
+    if(ptr)
+    {
+        int length = MIN(size, req->wLength);
+
+        if(ptr != usb_buffer)
+            memcpy(usb_buffer, ptr, length);
+
+        usb_drv_send(EP_CONTROL, usb_buffer, length);
+        usb_drv_recv(EP_CONTROL, NULL, 0);
+    }
+    else
+        usb_drv_stall(EP_CONTROL, true, true);
+}
+
+static void handle_std_intf_req(struct usb_ctrlrequest *req)
+{
+    unsigned intf = req->wIndex & 0xff;
+    if(intf != USB_HWSTUB_INTF)
+        return usb_drv_stall(EP_CONTROL, true, true);
+
+    switch(req->bRequest)
+    {
+        case USB_REQ_GET_DESCRIPTOR:
+            handle_std_intf_desc(req);
+            break;
+        default:
+            usb_drv_stall(EP_CONTROL, true, true);
+    }
+}
+
 static void handle_std_req(struct usb_ctrlrequest *req)
 {
     switch(req->bRequestType & USB_RECIP_MASK)
     {
         case USB_RECIP_DEVICE:
             return handle_std_dev_req(req);
+        case USB_RECIP_INTERFACE:
+            return handle_std_intf_req(req);
         default:
             usb_drv_stall(EP_CONTROL, true, true);
     }
@@ -291,36 +355,56 @@ static void handle_get_log(struct usb_ctrlrequest *req)
     enable_logf(true);
 }
 
-static void handle_rw_mem(struct usb_ctrlrequest *req)
+static void handle_read(struct usb_ctrlrequest *req)
 {
-    uint32_t addr = req->wValue | req->wIndex << 16;
-    uint16_t length = req->wLength;
-    
-    if(req->bRequestType & USB_DIR_IN)
+    static uint32_t last_addr = 0;
+    static uint16_t last_id = 0xffff;
+    uint16_t id = req->wValue;
+
+    if(req->bRequest == HWSTUB_READ)
     {
-        memcpy(usb_buffer, (void *)addr, length);
+        int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength);
+        if(size != sizeof(struct hwstub_read_req_t))
+            return usb_drv_stall(EP_CONTROL, true, true);
         asm volatile("nop" : : : "memory");
-        usb_drv_send(EP_CONTROL, usb_buffer, length);
-        usb_drv_recv(EP_CONTROL, NULL, 0);
+        struct hwstub_read_req_t *read = (void *)usb_buffer;
+        last_addr = read->dAddress;
+        last_id = id;
+        usb_drv_send(EP_CONTROL, NULL, 0);
     }
     else
     {
-        int size = usb_drv_recv(EP_CONTROL, usb_buffer, length);
+        if(id != last_id)
+            return usb_drv_stall(EP_CONTROL, true, true);
+        memcpy((void *)last_addr, usb_buffer, req->wLength);
+        memcpy(usb_buffer, (void *)last_addr, req->wLength);
         asm volatile("nop" : : : "memory");
-        if(size != length)
-            usb_drv_stall(EP_CONTROL, true, true);
-        else
-        {
-            memcpy((void *)addr, usb_buffer, length);
-            usb_drv_send(EP_CONTROL, NULL, 0);
-        }
+        usb_drv_send(EP_CONTROL, usb_buffer, req->wLength);
+        usb_drv_recv(EP_CONTROL, NULL, 0);
     }
-}
+};
 
-static void handle_call_jump(struct usb_ctrlrequest *req)
+static void handle_write(struct usb_ctrlrequest *req)
 {
-    uint32_t addr = req->wValue | req->wIndex << 16;
+    int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength);
+    asm volatile("nop" : : : "memory");
+    struct hwstub_write_req_t *write = (void *)usb_buffer;
+    int sz_hdr = sizeof(struct hwstub_write_req_t);
+    if(size < sz_hdr)
+        return usb_drv_stall(EP_CONTROL, true, true);
+    memcpy((void *)write->dAddress, usb_buffer + sz_hdr, req->wLength - sz_hdr);
+    usb_drv_send(EP_CONTROL, NULL, 0);
+}
 
+static void handle_exec(struct usb_ctrlrequest *req)
+{
+    int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength);
+    asm volatile("nop" : : : "memory");
+    struct hwstub_exec_req_t *exec = (void *)usb_buffer;
+    if(size != sizeof(struct hwstub_exec_req_t))
+        return usb_drv_stall(EP_CONTROL, true, true);
+    usb_drv_send(EP_CONTROL, NULL, 0);
+#if 0
     if(req->bRequest == HWSTUB_CALL)
         ((void (*)(void))addr)();
     else
@@ -329,23 +413,26 @@ static void handle_call_jump(struct usb_ctrlrequest *req)
         usb_drv_exit();
         asm volatile("bx %0\n" : : "r" (addr) : "memory");
     }
+#endif
 }
 
-static void handle_class_dev_req(struct usb_ctrlrequest *req)
+static void handle_class_intf_req(struct usb_ctrlrequest *req)
 {
+    unsigned intf = req->wIndex & 0xff;
+    if(intf != USB_HWSTUB_INTF)
+        return usb_drv_stall(EP_CONTROL, true, true);
+
     switch(req->bRequest)
     {
         case HWSTUB_GET_LOG:
-            handle_get_log(req);
-            break;
-        case HWSTUB_RW_MEM:
-            handle_rw_mem(req);
-            break;
-        case HWSTUB_CALL:
-        case HWSTUB_JUMP:
-            handle_call_jump(req);
-            break;
-            break;
+            return handle_get_log(req);
+        case HWSTUB_READ:
+        case HWSTUB_READ2:
+            return handle_read(req);
+        case HWSTUB_WRITE:
+            return handle_write(req);
+        case HWSTUB_EXEC:
+            return handle_exec(req);
         default:
             usb_drv_stall(EP_CONTROL, true, true);
     }
@@ -355,8 +442,10 @@ static void handle_class_req(struct usb_ctrlrequest *req)
 {
     switch(req->bRequestType & USB_RECIP_MASK)
     {
+        case USB_RECIP_INTERFACE:
+            return handle_class_intf_req(req);
         case USB_RECIP_DEVICE:
-            return handle_class_dev_req(req);
+            //return handle_class_dev_req(req);
         default:
             usb_drv_stall(EP_CONTROL, true, true);
     }
diff --git a/utils/hwstub/tools/hwstub_shell.cpp b/utils/hwstub/tools/hwstub_shell.cpp
index d0fcdfabc2..a10a1dad4e 100644
--- a/utils/hwstub/tools/hwstub_shell.cpp
+++ b/utils/hwstub/tools/hwstub_shell.cpp
@@ -754,15 +754,22 @@ int main(int argc, char **argv)
 
     // look for device
     if(!g_quiet)
-        printf("Looking for device %#04x:%#04x...\n", HWSTUB_USB_VID, HWSTUB_USB_PID);
-
-    libusb_device_handle *handle = libusb_open_device_with_vid_pid(ctx,
-        HWSTUB_USB_VID, HWSTUB_USB_PID);
-    if(handle == NULL)
+        printf("Looking for hwstub device ...\n");
+    // open first device
+    libusb_device **list;
+    ssize_t cnt = hwstub_get_device_list(ctx, &list);
+    if(cnt <= 0)
     {
         printf("No device found\n");
         return 1;
     }
+    libusb_device_handle *handle;
+    if(libusb_open(list[0], &handle) != 0)
+    {
+        printf("Cannot open device\n");
+        return 1;
+    }
+    libusb_free_device_list(list, 1);
 
     // admin stuff
     libusb_device *mydev = libusb_get_device(handle);