Archipelago » qemu

/*

 * Linux host USB redirector

 *

 * Copyright (c) 2005 Fabrice Bellard

 *

 * Copyright (c) 2008 Max Krasnyansky

 *      Support for host device auto connect & disconnect

 *      Major rewrite to support fully async operation

 *

 * Copyright 2008 TJ <linux@tjworld.net>

 *      Added flexible support for /dev/bus/usb /sys/bus/usb/devices in addition

 *      to the legacy /proc/bus/usb USB device discovery and handling

 *

 * Permission is hereby granted, free of charge, to any person obtaining a copy

 * of this software and associated documentation files (the "Software"), to deal

 * in the Software without restriction, including without limitation the rights

 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the Software is

 * furnished to do so, subject to the following conditions:

 *

 * The above copyright notice and this permission notice shall be included in

 * all copies or substantial portions of the Software.

 *

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL

 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

 * THE SOFTWARE.

 */

#include "qemu-common.h"

#include "qemu-timer.h"

#include "monitor.h"

#include "sysemu.h"

#include "trace.h"

#include <dirent.h>

#include <sys/ioctl.h>

#include <linux/usbdevice_fs.h>

#include <linux/version.h>

#include "hw/usb.h"

/* We redefine it to avoid version problems */

struct usb_ctrltransfer {

    uint8_t  bRequestType;

    uint8_t  bRequest;

    uint16_t wValue;

    uint16_t wIndex;

    uint16_t wLength;

    uint32_t timeout;

    void *data;

};

typedef int USBScanFunc(void *opaque, int bus_num, int addr, const char *port,

                        int class_id, int vendor_id, int product_id,

                        const char *product_name, int speed);

//#define DEBUG

#ifdef DEBUG

#define DPRINTF printf

#else

#define DPRINTF(...)

#endif

#define PRODUCT_NAME_SZ 32

#define MAX_PORTLEN 16

/* endpoint association data */

#define ISO_FRAME_DESC_PER_URB 32

/* devio.c limits single requests to 16k */

#define MAX_USBFS_BUFFER_SIZE 16384

typedef struct AsyncURB AsyncURB;

struct endp_data {

    uint8_t halted;

    uint8_t iso_started;

    AsyncURB *iso_urb;

    int iso_urb_idx;

    int iso_buffer_used;

    int inflight;

};

struct USBAutoFilter {

    uint32_t bus_num;

    uint32_t addr;

    char     *port;

    uint32_t vendor_id;

    uint32_t product_id;

};

typedef struct USBHostDevice {

    USBDevice dev;

    int       fd;

    int       hub_fd;

    int       hub_port;

    uint8_t   descr[8192];

    int       descr_len;

    int       closing;

    uint32_t  iso_urb_count;

    Notifier  exit;

    struct endp_data ep_in[USB_MAX_ENDPOINTS];

    struct endp_data ep_out[USB_MAX_ENDPOINTS];

    QLIST_HEAD(, AsyncURB) aurbs;

    /* Host side address */

    int bus_num;

    int addr;

    char port[MAX_PORTLEN];

    struct USBAutoFilter match;

    int seen, errcount;

    QTAILQ_ENTRY(USBHostDevice) next;

} USBHostDevice;

static QTAILQ_HEAD(, USBHostDevice) hostdevs = QTAILQ_HEAD_INITIALIZER(hostdevs);

static int usb_host_close(USBHostDevice *dev);

static int parse_filter(const char *spec, struct USBAutoFilter *f);

static void usb_host_auto_check(void *unused);

static int usb_host_read_file(char *line, size_t line_size,

                            const char *device_file, const char *device_name);

static int usb_linux_update_endp_table(USBHostDevice *s);

static int usb_host_usbfs_type(USBHostDevice *s, USBPacket *p)

{

    static const int usbfs[] = {

        [USB_ENDPOINT_XFER_CONTROL] = USBDEVFS_URB_TYPE_CONTROL,

        [USB_ENDPOINT_XFER_ISOC]    = USBDEVFS_URB_TYPE_ISO,

        [USB_ENDPOINT_XFER_BULK]    = USBDEVFS_URB_TYPE_BULK,

        [USB_ENDPOINT_XFER_INT]     = USBDEVFS_URB_TYPE_INTERRUPT,

    };

    uint8_t type = p->ep->type;

    assert(type < ARRAY_SIZE(usbfs));

    return usbfs[type];

}

static int usb_host_do_reset(USBHostDevice *dev)

{

    struct timeval s, e;

    uint32_t usecs;

    int ret;

    gettimeofday(&s, NULL);

    ret = ioctl(dev->fd, USBDEVFS_RESET);

    gettimeofday(&e, NULL);

    usecs = (e.tv_sec  - s.tv_sec) * 1000000;

    usecs += e.tv_usec - s.tv_usec;

    if (usecs > 1000000) {

        /* more than a second, something is fishy, broken usb device? */

        fprintf(stderr, "husb: device %d:%d reset took %d.%06d seconds\n",

                dev->bus_num, dev->addr, usecs / 1000000, usecs % 1000000);

    }

    return ret;

}

static struct endp_data *get_endp(USBHostDevice *s, int pid, int ep)

{

    struct endp_data *eps = pid == USB_TOKEN_IN ? s->ep_in : s->ep_out;

    assert(pid == USB_TOKEN_IN || pid == USB_TOKEN_OUT);

    assert(ep > 0 && ep <= USB_MAX_ENDPOINTS);

    return eps + ep - 1;

}

static int is_isoc(USBHostDevice *s, int pid, int ep)

{

    return usb_ep_get_type(&s->dev, pid, ep) == USB_ENDPOINT_XFER_ISOC;

}

static int is_valid(USBHostDevice *s, int pid, int ep)

{

    return usb_ep_get_type(&s->dev, pid, ep) != USB_ENDPOINT_XFER_INVALID;

}

static int is_halted(USBHostDevice *s, int pid, int ep)

{

    return get_endp(s, pid, ep)->halted;

}

static void clear_halt(USBHostDevice *s, int pid, int ep)

{

    trace_usb_host_ep_clear_halt(s->bus_num, s->addr, ep);

    get_endp(s, pid, ep)->halted = 0;

}

static void set_halt(USBHostDevice *s, int pid, int ep)

{

    if (ep != 0) {

        trace_usb_host_ep_set_halt(s->bus_num, s->addr, ep);

        get_endp(s, pid, ep)->halted = 1;

    }

}

static int is_iso_started(USBHostDevice *s, int pid, int ep)

{

    return get_endp(s, pid, ep)->iso_started;

}

static void clear_iso_started(USBHostDevice *s, int pid, int ep)

{

    trace_usb_host_ep_stop_iso(s->bus_num, s->addr, ep);

    get_endp(s, pid, ep)->iso_started = 0;

}

static void set_iso_started(USBHostDevice *s, int pid, int ep)

{

    struct endp_data *e = get_endp(s, pid, ep);

    trace_usb_host_ep_start_iso(s->bus_num, s->addr, ep);

    if (!e->iso_started) {

        e->iso_started = 1;

        e->inflight = 0;

    }

}

static int change_iso_inflight(USBHostDevice *s, int pid, int ep, int value)

{

    struct endp_data *e = get_endp(s, pid, ep);

    e->inflight += value;

    return e->inflight;

}

static void set_iso_urb(USBHostDevice *s, int pid, int ep, AsyncURB *iso_urb)

{

    get_endp(s, pid, ep)->iso_urb = iso_urb;

}

static AsyncURB *get_iso_urb(USBHostDevice *s, int pid, int ep)

{

    return get_endp(s, pid, ep)->iso_urb;

}

static void set_iso_urb_idx(USBHostDevice *s, int pid, int ep, int i)

{

    get_endp(s, pid, ep)->iso_urb_idx = i;

}

static int get_iso_urb_idx(USBHostDevice *s, int pid, int ep)

{

    return get_endp(s, pid, ep)->iso_urb_idx;

}

static void set_iso_buffer_used(USBHostDevice *s, int pid, int ep, int i)

{

    get_endp(s, pid, ep)->iso_buffer_used = i;

}

static int get_iso_buffer_used(USBHostDevice *s, int pid, int ep)

{

    return get_endp(s, pid, ep)->iso_buffer_used;

}

/*

 * Async URB state.

 * We always allocate iso packet descriptors even for bulk transfers

 * to simplify allocation and casts.

 */

struct AsyncURB

{

    struct usbdevfs_urb urb;

    struct usbdevfs_iso_packet_desc isocpd[ISO_FRAME_DESC_PER_URB];

    USBHostDevice *hdev;

    QLIST_ENTRY(AsyncURB) next;

    /* For regular async urbs */

    USBPacket     *packet;

    int more; /* large transfer, more urbs follow */

    /* For buffered iso handling */

    int iso_frame_idx; /* -1 means in flight */

};

static AsyncURB *async_alloc(USBHostDevice *s)

{

    AsyncURB *aurb = g_malloc0(sizeof(AsyncURB));

    aurb->hdev = s;

    QLIST_INSERT_HEAD(&s->aurbs, aurb, next);

    return aurb;

}

static void async_free(AsyncURB *aurb)

{

    QLIST_REMOVE(aurb, next);

    g_free(aurb);

}

static void do_disconnect(USBHostDevice *s)

{

    usb_host_close(s);

    usb_host_auto_check(NULL);

}

static void async_complete(void *opaque)

{

    USBHostDevice *s = opaque;

    AsyncURB *aurb;

    int urbs = 0;

    while (1) {

        USBPacket *p;

        int r = ioctl(s->fd, USBDEVFS_REAPURBNDELAY, &aurb);

        if (r < 0) {

            if (errno == EAGAIN) {

                if (urbs > 2) {

                    fprintf(stderr, "husb: %d iso urbs finished at once\n", urbs);

                }

                return;

            }

            if (errno == ENODEV) {

                if (!s->closing) {

                    trace_usb_host_disconnect(s->bus_num, s->addr);

                    do_disconnect(s);

                }

                return;

            }

            perror("USBDEVFS_REAPURBNDELAY");

            return;

        }

        DPRINTF("husb: async completed. aurb %p status %d alen %d\n",

                aurb, aurb->urb.status, aurb->urb.actual_length);

        /* If this is a buffered iso urb mark it as complete and don't do

           anything else (it is handled further in usb_host_handle_iso_data) */

        if (aurb->iso_frame_idx == -1) {

            int inflight;

            int pid = (aurb->urb.endpoint & USB_DIR_IN) ?

                USB_TOKEN_IN : USB_TOKEN_OUT;

            int ep = aurb->urb.endpoint & 0xf;

            if (aurb->urb.status == -EPIPE) {

                set_halt(s, pid, ep);

            }

            aurb->iso_frame_idx = 0;

            urbs++;

            inflight = change_iso_inflight(s, pid, ep, -1);

            if (inflight == 0 && is_iso_started(s, pid, ep)) {

                fprintf(stderr, "husb: out of buffers for iso stream\n");

            }

            continue;

        }

        p = aurb->packet;

        trace_usb_host_urb_complete(s->bus_num, s->addr, aurb, aurb->urb.status,

                                    aurb->urb.actual_length, aurb->more);

        if (p) {

            switch (aurb->urb.status) {

            case 0:

                p->result += aurb->urb.actual_length;

                break;

            case -EPIPE:

                set_halt(s, p->pid, p->ep->nr);

                p->result = USB_RET_STALL;

                break;

            default:

                p->result = USB_RET_NAK;

                break;

            }

            if (aurb->urb.type == USBDEVFS_URB_TYPE_CONTROL) {

                trace_usb_host_req_complete(s->bus_num, s->addr, p->result);

                usb_generic_async_ctrl_complete(&s->dev, p);

            } else if (!aurb->more) {

                trace_usb_host_req_complete(s->bus_num, s->addr, p->result);

                usb_packet_complete(&s->dev, p);

            }

        }

        async_free(aurb);

    }

}

static void usb_host_async_cancel(USBDevice *dev, USBPacket *p)

{

    USBHostDevice *s = DO_UPCAST(USBHostDevice, dev, dev);

    AsyncURB *aurb;

    QLIST_FOREACH(aurb, &s->aurbs, next) {

        if (p != aurb->packet) {

            continue;

        }

        DPRINTF("husb: async cancel: packet %p, aurb %p\n", p, aurb);

        /* Mark it as dead (see async_complete above) */

        aurb->packet = NULL;

        int r = ioctl(s->fd, USBDEVFS_DISCARDURB, aurb);

        if (r < 0) {

            DPRINTF("husb: async. discard urb failed errno %d\n", errno);

        }

    }

}

static int usb_host_open_device(int bus, int addr)

{

    const char *usbfs = NULL;

    char filename[32];

    struct stat st;

    int fd, rc;

    rc = stat("/dev/bus/usb", &st);

    if (rc == 0 && S_ISDIR(st.st_mode)) {

        /* udev-created device nodes available */

        usbfs = "/dev/bus/usb";

    } else {

        /* fallback: usbfs mounted below /proc */

        usbfs = "/proc/bus/usb";

    }

    snprintf(filename, sizeof(filename), "%s/%03d/%03d",

             usbfs, bus, addr);

    fd = open(filename, O_RDWR | O_NONBLOCK);

    if (fd < 0) {

        fprintf(stderr, "husb: open %s: %s\n", filename, strerror(errno));

    }

    return fd;

}

static int usb_host_claim_port(USBHostDevice *s)

{

#ifdef USBDEVFS_CLAIM_PORT

    char *h, hub_name[64], line[1024];

    int hub_addr, ret;

    snprintf(hub_name, sizeof(hub_name), "%d-%s",

             s->match.bus_num, s->match.port);

    /* try strip off last ".$portnr" to get hub */

    h = strrchr(hub_name, '.');

    if (h != NULL) {

        s->hub_port = atoi(h+1);

        *h = '\0';

    } else {

        /* no dot in there -> it is the root hub */

        snprintf(hub_name, sizeof(hub_name), "usb%d",

                 s->match.bus_num);

        s->hub_port = atoi(s->match.port);

    }

    if (!usb_host_read_file(line, sizeof(line), "devnum",

                            hub_name)) {

        return -1;

    }

    if (sscanf(line, "%d", &hub_addr) != 1) {

        return -1;

    }

    s->hub_fd = usb_host_open_device(s->match.bus_num, hub_addr);

    if (s->hub_fd < 0) {

        return -1;

    }

    ret = ioctl(s->hub_fd, USBDEVFS_CLAIM_PORT, &s->hub_port);

    if (ret < 0) {

        close(s->hub_fd);

        s->hub_fd = -1;

        return -1;

    }

    trace_usb_host_claim_port(s->match.bus_num, hub_addr, s->hub_port);

    return 0;

#else

    return -1;

#endif

}

static void usb_host_release_port(USBHostDevice *s)

{

    if (s->hub_fd == -1) {

        return;

    }

#ifdef USBDEVFS_RELEASE_PORT

    ioctl(s->hub_fd, USBDEVFS_RELEASE_PORT, &s->hub_port);

#endif

    close(s->hub_fd);

    s->hub_fd = -1;

}

static int usb_host_disconnect_ifaces(USBHostDevice *dev, int nb_interfaces)

{

    /* earlier Linux 2.4 do not support that */

#ifdef USBDEVFS_DISCONNECT

    struct usbdevfs_ioctl ctrl;

    int ret, interface;

    for (interface = 0; interface < nb_interfaces; interface++) {

        ctrl.ioctl_code = USBDEVFS_DISCONNECT;

        ctrl.ifno = interface;

        ctrl.data = 0;

        ret = ioctl(dev->fd, USBDEVFS_IOCTL, &ctrl);

        if (ret < 0 && errno != ENODATA) {

            perror("USBDEVFS_DISCONNECT");

            return -1;

        }

    }

#endif

    return 0;

}

static int usb_linux_get_num_interfaces(USBHostDevice *s)

{

    char device_name[64], line[1024];

    int num_interfaces = 0;

    sprintf(device_name, "%d-%s", s->bus_num, s->port);

    if (!usb_host_read_file(line, sizeof(line), "bNumInterfaces",

                            device_name)) {

        return -1;

    }

    if (sscanf(line, "%d", &num_interfaces) != 1) {

        return -1;

    }

    return num_interfaces;

}

static int usb_host_claim_interfaces(USBHostDevice *dev, int configuration)

{

    const char *op = NULL;

    int dev_descr_len, config_descr_len;

    int interface, nb_interfaces;

    int ret, i;

    for (i = 0; i < USB_MAX_INTERFACES; i++) {

        dev->dev.altsetting[i] = 0;

    }

    if (configuration == 0) { /* address state - ignore */

        dev->dev.ninterfaces   = 0;

        dev->dev.configuration = 0;

        return 1;

    }

    DPRINTF("husb: claiming interfaces. config %d\n", configuration);

    i = 0;

    dev_descr_len = dev->descr[0];

    if (dev_descr_len > dev->descr_len) {

        fprintf(stderr, "husb: update iface failed. descr too short\n");

        return 0;

    }

    i += dev_descr_len;

    while (i < dev->descr_len) {

        DPRINTF("husb: i is %d, descr_len is %d, dl %d, dt %d\n",

                i, dev->descr_len,

               dev->descr[i], dev->descr[i+1]);

        if (dev->descr[i+1] != USB_DT_CONFIG) {

            i += dev->descr[i];

            continue;

        }

        config_descr_len = dev->descr[i];

        DPRINTF("husb: config #%d need %d\n", dev->descr[i + 5], configuration);

        if (configuration == dev->descr[i + 5]) {

            configuration = dev->descr[i + 5];

            break;

        }

        i += config_descr_len;

    }

    if (i >= dev->descr_len) {

        fprintf(stderr,

                "husb: update iface failed. no matching configuration\n");

        return 0;

    }

    nb_interfaces = dev->descr[i + 4];

    if (usb_host_disconnect_ifaces(dev, nb_interfaces) < 0) {

        goto fail;

    }

    /* XXX: only grab if all interfaces are free */

    for (interface = 0; interface < nb_interfaces; interface++) {

        op = "USBDEVFS_CLAIMINTERFACE";

        ret = ioctl(dev->fd, USBDEVFS_CLAIMINTERFACE, &interface);

        if (ret < 0) {

            goto fail;

        }

    }

    trace_usb_host_claim_interfaces(dev->bus_num, dev->addr,

                                    nb_interfaces, configuration);

    dev->dev.ninterfaces   = nb_interfaces;

    dev->dev.configuration = configuration;

    return 1;

fail:

    if (errno == ENODEV) {

        do_disconnect(dev);

    }

    perror(op);

    return 0;

}

static int usb_host_release_interfaces(USBHostDevice *s)

{

    int ret, i;

    trace_usb_host_release_interfaces(s->bus_num, s->addr);

    for (i = 0; i < s->dev.ninterfaces; i++) {

        ret = ioctl(s->fd, USBDEVFS_RELEASEINTERFACE, &i);

        if (ret < 0) {

            perror("USBDEVFS_RELEASEINTERFACE");

            return 0;

        }

    }

    return 1;

}

static void usb_host_handle_reset(USBDevice *dev)

{

    USBHostDevice *s = DO_UPCAST(USBHostDevice, dev, dev);

    trace_usb_host_reset(s->bus_num, s->addr);

    usb_host_do_reset(s);;

    usb_host_claim_interfaces(s, 0);

    usb_linux_update_endp_table(s);

}

static void usb_host_handle_destroy(USBDevice *dev)

{

    USBHostDevice *s = (USBHostDevice *)dev;

    usb_host_release_port(s);

    usb_host_close(s);

    QTAILQ_REMOVE(&hostdevs, s, next);

    qemu_remove_exit_notifier(&s->exit);

}

/* iso data is special, we need to keep enough urbs in flight to make sure

   that the controller never runs out of them, otherwise the device will

   likely suffer a buffer underrun / overrun. */

static AsyncURB *usb_host_alloc_iso(USBHostDevice *s, int pid, uint8_t ep)

{

    AsyncURB *aurb;

    int i, j, len = usb_ep_get_max_packet_size(&s->dev, pid, ep);

    aurb = g_malloc0(s->iso_urb_count * sizeof(*aurb));

    for (i = 0; i < s->iso_urb_count; i++) {

        aurb[i].urb.endpoint      = ep;

        aurb[i].urb.buffer_length = ISO_FRAME_DESC_PER_URB * len;

        aurb[i].urb.buffer        = g_malloc(aurb[i].urb.buffer_length);

        aurb[i].urb.type          = USBDEVFS_URB_TYPE_ISO;

        aurb[i].urb.flags         = USBDEVFS_URB_ISO_ASAP;

        aurb[i].urb.number_of_packets = ISO_FRAME_DESC_PER_URB;

        for (j = 0 ; j < ISO_FRAME_DESC_PER_URB; j++)

            aurb[i].urb.iso_frame_desc[j].length = len;

        if (pid == USB_TOKEN_IN) {

            aurb[i].urb.endpoint |= 0x80;

            /* Mark as fully consumed (idle) */

            aurb[i].iso_frame_idx = ISO_FRAME_DESC_PER_URB;

        }

    }

    set_iso_urb(s, pid, ep, aurb);

    return aurb;

}

static void usb_host_stop_n_free_iso(USBHostDevice *s, int pid, uint8_t ep)

{

    AsyncURB *aurb;

    int i, ret, killed = 0, free = 1;

    aurb = get_iso_urb(s, pid, ep);

    if (!aurb) {

        return;

    }

    for (i = 0; i < s->iso_urb_count; i++) {

        /* in flight? */

        if (aurb[i].iso_frame_idx == -1) {

            ret = ioctl(s->fd, USBDEVFS_DISCARDURB, &aurb[i]);

            if (ret < 0) {

                perror("USBDEVFS_DISCARDURB");

                free = 0;

                continue;

            }

            killed++;

        }

    }

    /* Make sure any urbs we've killed are reaped before we free them */

    if (killed) {

        async_complete(s);

    }

    for (i = 0; i < s->iso_urb_count; i++) {

        g_free(aurb[i].urb.buffer);

    }

    if (free)

        g_free(aurb);

    else

        printf("husb: leaking iso urbs because of discard failure\n");

    set_iso_urb(s, pid, ep, NULL);

    set_iso_urb_idx(s, pid, ep, 0);

    clear_iso_started(s, pid, ep);

}

static int urb_status_to_usb_ret(int status)

{

    switch (status) {

    case -EPIPE:

        return USB_RET_STALL;

    default:

        return USB_RET_NAK;

    }

}

static int usb_host_handle_iso_data(USBHostDevice *s, USBPacket *p, int in)

{

    AsyncURB *aurb;

    int i, j, ret, max_packet_size, offset, len = 0;

    uint8_t *buf;

    max_packet_size = p->ep->max_packet_size;

    if (max_packet_size == 0)

        return USB_RET_NAK;

    aurb = get_iso_urb(s, p->pid, p->ep->nr);

    if (!aurb) {

        aurb = usb_host_alloc_iso(s, p->pid, p->ep->nr);

    }

    i = get_iso_urb_idx(s, p->pid, p->ep->nr);

    j = aurb[i].iso_frame_idx;

    if (j >= 0 && j < ISO_FRAME_DESC_PER_URB) {

        if (in) {

            /* Check urb status  */

            if (aurb[i].urb.status) {

                len = urb_status_to_usb_ret(aurb[i].urb.status);

                /* Move to the next urb */

                aurb[i].iso_frame_idx = ISO_FRAME_DESC_PER_URB - 1;

            /* Check frame status */

            } else if (aurb[i].urb.iso_frame_desc[j].status) {

                len = urb_status_to_usb_ret(

                                        aurb[i].urb.iso_frame_desc[j].status);

            /* Check the frame fits */

            } else if (aurb[i].urb.iso_frame_desc[j].actual_length

                       > p->iov.size) {

                printf("husb: received iso data is larger then packet\n");

                len = USB_RET_NAK;

            /* All good copy data over */

            } else {

                len = aurb[i].urb.iso_frame_desc[j].actual_length;

                buf  = aurb[i].urb.buffer +

                    j * aurb[i].urb.iso_frame_desc[0].length;

                usb_packet_copy(p, buf, len);

            }

        } else {

            len = p->iov.size;

            offset = (j == 0) ? 0 : get_iso_buffer_used(s, p->pid, p->ep->nr);

            /* Check the frame fits */

            if (len > max_packet_size) {

                printf("husb: send iso data is larger then max packet size\n");

                return USB_RET_NAK;

            }

            /* All good copy data over */

            usb_packet_copy(p, aurb[i].urb.buffer + offset, len);

            aurb[i].urb.iso_frame_desc[j].length = len;

            offset += len;

            set_iso_buffer_used(s, p->pid, p->ep->nr, offset);

            /* Start the stream once we have buffered enough data */

            if (!is_iso_started(s, p->pid, p->ep->nr) && i == 1 && j == 8) {

                set_iso_started(s, p->pid, p->ep->nr);

            }

        }

        aurb[i].iso_frame_idx++;

        if (aurb[i].iso_frame_idx == ISO_FRAME_DESC_PER_URB) {

            i = (i + 1) % s->iso_urb_count;

            set_iso_urb_idx(s, p->pid, p->ep->nr, i);

        }

    } else {

        if (in) {

            set_iso_started(s, p->pid, p->ep->nr);

        } else {

            DPRINTF("hubs: iso out error no free buffer, dropping packet\n");

        }

    }

    if (is_iso_started(s, p->pid, p->ep->nr)) {

        /* (Re)-submit all fully consumed / filled urbs */

        for (i = 0; i < s->iso_urb_count; i++) {

            if (aurb[i].iso_frame_idx == ISO_FRAME_DESC_PER_URB) {

                ret = ioctl(s->fd, USBDEVFS_SUBMITURB, &aurb[i]);

                if (ret < 0) {

                    perror("USBDEVFS_SUBMITURB");

                    if (!in || len == 0) {

                        switch(errno) {

                        case ETIMEDOUT:

                            len = USB_RET_NAK;

                            break;

                        case EPIPE:

                        default:

                            len = USB_RET_STALL;

                        }

                    }

                    break;

                }

                aurb[i].iso_frame_idx = -1;

                change_iso_inflight(s, p->pid, p->ep->nr, 1);

            }

        }

    }

    return len;

}

static int usb_host_handle_data(USBDevice *dev, USBPacket *p)

{

    USBHostDevice *s = DO_UPCAST(USBHostDevice, dev, dev);

    struct usbdevfs_urb *urb;

    AsyncURB *aurb;

    int ret, rem, prem, v;

    uint8_t *pbuf;

    uint8_t ep;

    trace_usb_host_req_data(s->bus_num, s->addr,

                            p->pid == USB_TOKEN_IN,

                            p->ep->nr, p->iov.size);

    if (!is_valid(s, p->pid, p->ep->nr)) {

        trace_usb_host_req_complete(s->bus_num, s->addr, USB_RET_NAK);

        return USB_RET_NAK;

    }

    if (p->pid == USB_TOKEN_IN) {

        ep = p->ep->nr | 0x80;

    } else {

        ep = p->ep->nr;

    }

    if (is_halted(s, p->pid, p->ep->nr)) {

        unsigned int arg = ep;

        ret = ioctl(s->fd, USBDEVFS_CLEAR_HALT, &arg);

        if (ret < 0) {

            perror("USBDEVFS_CLEAR_HALT");

            trace_usb_host_req_complete(s->bus_num, s->addr, USB_RET_NAK);

            return USB_RET_NAK;

        }

        clear_halt(s, p->pid, p->ep->nr);

    }

    if (is_isoc(s, p->pid, p->ep->nr)) {

        return usb_host_handle_iso_data(s, p, p->pid == USB_TOKEN_IN);

    }

    v = 0;

    prem = p->iov.iov[v].iov_len;

    pbuf = p->iov.iov[v].iov_base;

    rem = p->iov.size;

    while (rem) {

        if (prem == 0) {

            v++;

            assert(v < p->iov.niov);

            prem = p->iov.iov[v].iov_len;

            pbuf = p->iov.iov[v].iov_base;

            assert(prem <= rem);

        }

        aurb = async_alloc(s);

        aurb->packet = p;

        urb = &aurb->urb;

        urb->endpoint      = ep;

        urb->type          = usb_host_usbfs_type(s, p);

        urb->usercontext   = s;

        urb->buffer        = pbuf;

        urb->buffer_length = prem;

        if (urb->buffer_length > MAX_USBFS_BUFFER_SIZE) {

            urb->buffer_length = MAX_USBFS_BUFFER_SIZE;

        }

        pbuf += urb->buffer_length;