+static int do_token_setup(USBDevice *s, USBPacket *p)
+{
+ int request, value, index;
+ int ret = 0;
+
+ if (p->len != 8)
+ return USB_RET_STALL;
+
+ memcpy(s->setup_buf, p->data, 8);
+ s->setup_len = (s->setup_buf[7] << 8) | s->setup_buf[6];
+ s->setup_index = 0;
+
+ request = (s->setup_buf[0] << 8) | s->setup_buf[1];
+ value = (s->setup_buf[3] << 8) | s->setup_buf[2];
+ index = (s->setup_buf[5] << 8) | s->setup_buf[4];
+
+ if (s->setup_buf[0] & USB_DIR_IN) {
+ ret = s->info->handle_control(s, p, request, value, index,
+ s->setup_len, s->data_buf);
+ if (ret == USB_RET_ASYNC) {
+ s->setup_state = SETUP_STATE_SETUP;
+ return USB_RET_ASYNC;
+ }
+ if (ret < 0)
+ return ret;
+
+ if (ret < s->setup_len)
+ s->setup_len = ret;
+ s->setup_state = SETUP_STATE_DATA;
+ } else {
+ if (s->setup_len > sizeof(s->data_buf)) {
+ fprintf(stderr,
+ "usb_generic_handle_packet: ctrl buffer too small (%d > %zu)\n",
+ s->setup_len, sizeof(s->data_buf));
+ return USB_RET_STALL;
+ }
+ if (s->setup_len == 0)
+ s->setup_state = SETUP_STATE_ACK;
+ else
+ s->setup_state = SETUP_STATE_DATA;
+ }
+
+ return ret;
+}
+
+static int do_token_in(USBDevice *s, USBPacket *p)
+{
+ int request, value, index;
+ int ret = 0;
+
+ if (p->devep != 0)
+ return s->info->handle_data(s, p);
+
+ request = (s->setup_buf[0] << 8) | s->setup_buf[1];
+ value = (s->setup_buf[3] << 8) | s->setup_buf[2];
+ index = (s->setup_buf[5] << 8) | s->setup_buf[4];
+
+ switch(s->setup_state) {
+ case SETUP_STATE_ACK:
+ if (!(s->setup_buf[0] & USB_DIR_IN)) {
+ ret = s->info->handle_control(s, p, request, value, index,
+ s->setup_len, s->data_buf);
+ if (ret == USB_RET_ASYNC) {
+ return USB_RET_ASYNC;
+ }
+ s->setup_state = SETUP_STATE_IDLE;
+ if (ret > 0)
+ return 0;
+ return ret;
+ }
+
+ /* return 0 byte */
+ return 0;
+
+ case SETUP_STATE_DATA:
+ if (s->setup_buf[0] & USB_DIR_IN) {
+ int len = s->setup_len - s->setup_index;
+ if (len > p->len)
+ len = p->len;
+ memcpy(p->data, s->data_buf + s->setup_index, len);
+ s->setup_index += len;
+ if (s->setup_index >= s->setup_len)
+ s->setup_state = SETUP_STATE_ACK;
+ return len;
+ }
+
+ s->setup_state = SETUP_STATE_IDLE;
+ return USB_RET_STALL;
+
+ default:
+ return USB_RET_STALL;
+ }
+}
+
+static int do_token_out(USBDevice *s, USBPacket *p)