diff --git a/usb-linux.c b/usb-linux.c
index c31d56a..b82c4f6 100644
--- a/usb-linux.c
+++ b/usb-linux.c
@@ -25,28 +25,20 @@
  * 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 "hw/usb.h"
 #include "console.h"
 
 #if defined(__linux__)
 #include <dirent.h>
 #include <sys/ioctl.h>
-#include <linux/usbdevice_fs.h>
-#include <linux/version.h>
 #include <signal.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;
-};
+#include <linux/usb/ch9.h>
+#include <linux/usbdevice_fs.h>
+#include <linux/version.h>
+#include "hw/usb.h"
 
 typedef int USBScanFunc(void *opaque, int bus_num, int addr, int class_id,
                         int vendor_id, int product_id,
@@ -54,7 +46,6 @@
 static int usb_host_find_device(int *pbus_num, int *paddr,
                                 char *product_name, int product_name_size,
                                 const char *devname);
-
 //#define DEBUG
 
 #ifdef DEBUG
@@ -67,14 +58,32 @@
 #define PRODUCT_NAME_SZ 32
 #define MAX_ENDPOINTS 16
 
-struct sigaction sigact;
-
 /* endpoint association data */
 struct endp_data {
     uint8_t type;
     uint8_t halted;
 };
 
+enum {
+    CTRL_STATE_IDLE = 0,
+    CTRL_STATE_SETUP,
+    CTRL_STATE_DATA,
+    CTRL_STATE_ACK
+};
+
+/*
+ * Control transfer state.
+ * Note that 'buffer' _must_ follow 'req' field because 
+ * we need contigious buffer when we submit control URB.
+ */ 
+struct ctrl_struct {
+    uint16_t len;
+    uint16_t offset;
+    uint8_t  state;
+    struct   usb_ctrlrequest req;
+    uint8_t  buffer[1024];
+};
+
 typedef struct USBHostDevice {
     USBDevice dev;
     int       fd;
@@ -82,8 +91,10 @@
     uint8_t   descr[1024];
     int       descr_len;
     int       configuration;
+    int       ninterfaces;
     int       closing;
 
+    struct ctrl_struct ctrl;
     struct endp_data endp_table[MAX_ENDPOINTS];
 
     /* Host side address */
@@ -172,6 +183,26 @@
     qemu_free(aurb);
 }
 
+static void async_complete_ctrl(USBHostDevice *s, USBPacket *p)
+{
+    switch(s->ctrl.state) {
+    case CTRL_STATE_SETUP:
+        if (p->len < s->ctrl.len)
+            s->ctrl.len = p->len;
+        s->ctrl.state = CTRL_STATE_DATA;
+        p->len = 8;
+        break;
+
+    case CTRL_STATE_ACK:
+        s->ctrl.state = CTRL_STATE_IDLE;
+        p->len = 0;
+        break;
+
+    default:
+        break;
+    }
+}
+
 static void async_complete(void *opaque)
 {
     USBHostDevice *s = opaque;
@@ -204,6 +235,8 @@
             switch (aurb->urb.status) {
             case 0:
                 p->len = aurb->urb.actual_length;
+                if (aurb->urb.type == USBDEVFS_URB_TYPE_CONTROL)
+                    async_complete_ctrl(s, p);
                 break;
 
             case -EPIPE:
@@ -237,7 +270,7 @@
     }
 }
 
-static int usb_host_update_interfaces(USBHostDevice *dev, int configuration)
+static int usb_host_claim_interfaces(USBHostDevice *dev, int configuration)
 {
     int dev_descr_len, config_descr_len;
     int interface, nb_interfaces, nb_configurations;
@@ -246,6 +279,8 @@
     if (configuration == 0) /* address state - ignore */
         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)
@@ -265,8 +300,10 @@
 
 	printf("husb: config #%d need %d\n", dev->descr[i + 5], configuration); 
 
-        if (configuration < 0 || configuration == dev->descr[i + 5])
+        if (configuration < 0 || configuration == dev->descr[i + 5]) {
+            configuration = dev->descr[i + 5];
             break;
+        }
 
         i += config_descr_len;
     }
@@ -310,17 +347,37 @@
     printf("husb: %d interfaces claimed for configuration %d\n",
            nb_interfaces, configuration);
 
+    dev->ninterfaces   = nb_interfaces;
+    dev->configuration = configuration;
+    return 1;
+}
+
+static int usb_host_release_interfaces(USBHostDevice *s)
+{
+    int ret, i;
+
+    dprintf("husb: releasing interfaces\n");
+
+    for (i = 0; i < s->ninterfaces; i++) {
+        ret = ioctl(s->fd, USBDEVFS_RELEASEINTERFACE, &i);
+        if (ret < 0) {
+            perror("husb: failed to release interface");
+            return 0;
+        }
+    }
+
     return 1;
 }
 
 static void usb_host_handle_reset(USBDevice *dev)
 {
-    USBHostDevice *s = (USBHostDevice *)dev;
+    USBHostDevice *s = (USBHostDevice *) dev;
 
     dprintf("husb: reset device %u.%u\n", s->bus_num, s->addr);
 
     ioctl(s->fd, USBDEVFS_RESET);
-    usb_host_update_interfaces(s, s->configuration);
+
+    usb_host_claim_interfaces(s, s->configuration);
 }
 
 static void usb_host_handle_destroy(USBDevice *dev)
@@ -343,73 +400,10 @@
 
 static int usb_linux_update_endp_table(USBHostDevice *s);
 
-static int usb_host_handle_control(USBDevice *dev,
-                                   int request,
-                                   int value,
-                                   int index,
-                                   int length,
-                                   uint8_t *data)
+static int usb_host_handle_data(USBHostDevice *s, USBPacket *p)
 {
-    USBHostDevice *s = (USBHostDevice *)dev;
-    struct usb_ctrltransfer ct;
-    struct usbdevfs_setinterface si;
-    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 == ((USB_RECIP_INTERFACE << 8) |
-                           USB_REQ_SET_INTERFACE)) {
-        /* set alternate setting for the interface */
-        si.interface = index;
-        si.altsetting = value;
-        ret = ioctl(s->fd, USBDEVFS_SETINTERFACE, &si);
-        usb_linux_update_endp_table(s);
-    } else if (request == (DeviceOutRequest | USB_REQ_SET_CONFIGURATION)) {
-        dprintf("husb: ctrl set config %d\n", value & 0xff);
-        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;
-        ct.wIndex = index;
-        ct.wLength = length;
-        ct.timeout = 50;
-        ct.data = data;
-        ret = ioctl(s->fd, USBDEVFS_CONTROL, &ct);
-
-        dprintf("husb: ctrl req 0x%x val 0x%x index %u len %u ret %d\n",
-            ct.bRequest, ct.wValue, ct.wIndex, ct.wLength, ret);
-    }
-
-    if (ret < 0) {
-        switch(errno) {
-        case ETIMEDOUT:
-            return USB_RET_NAK;
-        default:
-            return USB_RET_STALL;
-        }
-    } else {
-        if (intf_update_required) {
-            dprintf("husb: updating interfaces\n");
-            usb_host_update_interfaces(s, value & 0xff);
-        }
-        return ret;
-    }
-}
-
-static int usb_host_handle_data(USBDevice *dev, USBPacket *p)
-{
-    USBHostDevice *s = (USBHostDevice *) dev;
-    AsyncURB *aurb;
     struct usbdevfs_urb *urb;
+    AsyncURB *aurb;
     int ret;
 
     aurb = async_alloc();
@@ -474,12 +468,292 @@
     return USB_RET_ASYNC;
 }
 
+static int ctrl_error(void)
+{
+    if (errno == ETIMEDOUT)
+        return USB_RET_NAK;
+    else 
+        return USB_RET_STALL;
+}
+
+static int usb_host_set_address(USBHostDevice *s, int addr)
+{
+    dprintf("husb: ctrl set addr %u\n", addr);
+    s->dev.addr = addr;
+    return 0;
+}
+
+static int usb_host_set_config(USBHostDevice *s, int config)
+{
+    usb_host_release_interfaces(s);
+
+    int ret = ioctl(s->fd, USBDEVFS_SETCONFIGURATION, &config);
+ 
+    dprintf("husb: ctrl set config %d ret %d errno %d\n", config, ret, errno);
+    
+    if (ret < 0)
+        return ctrl_error();
+ 
+    usb_host_claim_interfaces(s, config);
+    return 0;
+}
+
+static int usb_host_set_interface(USBHostDevice *s, int iface, int alt)
+{
+    struct usbdevfs_setinterface si;
+    int ret;
+
+    si.interface  = iface;
+    si.altsetting = alt;
+    ret = ioctl(s->fd, USBDEVFS_SETINTERFACE, &si);
+    
+    dprintf("husb: ctrl set iface %d altset %d ret %d errno %d\n", 
+    	iface, alt, ret, errno);
+    
+    if (ret < 0)
+        return ctrl_error();
+
+    usb_linux_update_endp_table(s);
+    return 0;
+}
+
+static int usb_host_handle_control(USBHostDevice *s, USBPacket *p)
+{
+    struct usbdevfs_urb *urb;
+    AsyncURB *aurb;
+    int ret, value, index;
+
+    /* 
+     * Process certain standard device requests.
+     * These are infrequent and are processed synchronously.
+     */
+    value = le16_to_cpu(s->ctrl.req.wValue);
+    index = le16_to_cpu(s->ctrl.req.wIndex);
+
+    dprintf("husb: ctrl type 0x%x req 0x%x val 0x%x index %u len %u\n",
+        s->ctrl.req.bRequestType, s->ctrl.req.bRequest, value, index, 
+        s->ctrl.len);
+
+    if (s->ctrl.req.bRequestType == 0) {
+        switch (s->ctrl.req.bRequest) {
+        case USB_REQ_SET_ADDRESS:
+            return usb_host_set_address(s, value);
+
+        case USB_REQ_SET_CONFIGURATION:
+            return usb_host_set_config(s, value & 0xff);
+        }
+    }
+
+    if (s->ctrl.req.bRequestType == 1 &&
+                  s->ctrl.req.bRequest == USB_REQ_SET_INTERFACE)
+        return usb_host_set_interface(s, index, value);
+
+    /* The rest are asynchronous */
+
+    aurb = async_alloc();
+    if (!aurb) {
+        dprintf("husb: async malloc failed\n");
+        return USB_RET_NAK;
+    }
+    aurb->hdev   = s;
+    aurb->packet = p;
+
+    /* 
+     * Setup ctrl transfer.
+     *
+     * s->ctrl is layed out such that data buffer immediately follows
+     * 'req' struct which is exactly what usbdevfs expects.
+     */ 
+    urb = &aurb->urb;
+
+    urb->type     = USBDEVFS_URB_TYPE_CONTROL;
+    urb->endpoint = p->devep;
+
+    urb->buffer        = &s->ctrl.req;
+    urb->buffer_length = 8 + s->ctrl.len;
+
+    urb->usercontext = s;
+
+    ret = ioctl(s->fd, USBDEVFS_SUBMITURB, urb);
+
+    dprintf("husb: submit ctrl. len %u aurb %p\n", urb->buffer_length, aurb);
+
+    if (ret < 0) {
+        dprintf("husb: submit failed. errno %d\n", errno);
+        async_free(aurb);
+
+        switch(errno) {
+        case ETIMEDOUT:
+            return USB_RET_NAK;
+        case EPIPE:
+        default:
+            return USB_RET_STALL;
+        }
+    }
+
+    usb_defer_packet(p, async_cancel, aurb);
+    return USB_RET_ASYNC;
+}
+
+static int do_token_setup(USBDevice *dev, USBPacket *p)
+{
+    USBHostDevice *s = (USBHostDevice *) dev;
+    int ret = 0;
+
+    if (p->len != 8)
+        return USB_RET_STALL;
+ 
+    memcpy(&s->ctrl.req, p->data, 8);
+    s->ctrl.len    = le16_to_cpu(s->ctrl.req.wLength);
+    s->ctrl.offset = 0;
+    s->ctrl.state  = CTRL_STATE_SETUP;
+
+    if (s->ctrl.req.bRequestType & USB_DIR_IN) {
+        ret = usb_host_handle_control(s, p);
+        if (ret < 0)
+            return ret;
+
+        if (ret < s->ctrl.len)
+            s->ctrl.len = ret;
+        s->ctrl.state = CTRL_STATE_DATA;
+    } else {
+        if (s->ctrl.len == 0)
+            s->ctrl.state = CTRL_STATE_ACK;
+        else
+            s->ctrl.state = CTRL_STATE_DATA;
+    }
+
+    return ret;
+}
+
+static int do_token_in(USBDevice *dev, USBPacket *p)
+{
+    USBHostDevice *s = (USBHostDevice *) dev;
+    int ret = 0;
+
+    if (p->devep != 0)
+        return usb_host_handle_data(s, p);
+
+    switch(s->ctrl.state) {
+    case CTRL_STATE_ACK:
+        if (!(s->ctrl.req.bRequestType & USB_DIR_IN)) {
+            ret = usb_host_handle_control(s, p);
+            if (ret == USB_RET_ASYNC)
+                return USB_RET_ASYNC;
+
+            s->ctrl.state = CTRL_STATE_IDLE;
+            return ret > 0 ? 0 : ret;
+        }
+
+        return 0;
+
+    case CTRL_STATE_DATA:
+        if (s->ctrl.req.bRequestType & USB_DIR_IN) {
+            int len = s->ctrl.len - s->ctrl.offset;
+            if (len > p->len)
+                len = p->len;
+            memcpy(p->data, s->ctrl.buffer + s->ctrl.offset, len);
+            s->ctrl.offset += len;
+            if (s->ctrl.offset >= s->ctrl.len)
+                s->ctrl.state = CTRL_STATE_ACK;
+            return len;
+        }
+
+        s->ctrl.state = CTRL_STATE_IDLE;
+        return USB_RET_STALL;
+
+    default:
+        return USB_RET_STALL;
+    }
+}
+
+static int do_token_out(USBDevice *dev, USBPacket *p)
+{
+    USBHostDevice *s = (USBHostDevice *) dev;
+
+    if (p->devep != 0)
+        return usb_host_handle_data(s, p);
+
+    switch(s->ctrl.state) {
+    case CTRL_STATE_ACK:
+        if (s->ctrl.req.bRequestType & USB_DIR_IN) {
+            s->ctrl.state = CTRL_STATE_IDLE;
+            /* transfer OK */
+        } else {
+            /* ignore additional output */
+        }
+        return 0;
+
+    case CTRL_STATE_DATA:
+        if (!(s->ctrl.req.bRequestType & USB_DIR_IN)) {
+            int len = s->ctrl.len - s->ctrl.offset;
+            if (len > p->len)
+                len = p->len;
+            memcpy(s->ctrl.buffer + s->ctrl.offset, p->data, len);
+            s->ctrl.offset += len;
+            if (s->ctrl.offset >= s->ctrl.len)
+                s->ctrl.state = CTRL_STATE_ACK;
+            return len;
+        }
+
+        s->ctrl.state = CTRL_STATE_IDLE;
+        return USB_RET_STALL;
+
+    default:
+        return USB_RET_STALL;
+    }
+}
+
+/*
+ * Packet handler.
+ * Called by the HC (host controller).
+ *
+ * Returns length of the transaction or one of the USB_RET_XXX codes.
+ */
+int usb_host_handle_packet(USBDevice *s, USBPacket *p)
+{
+    switch(p->pid) {
+    case USB_MSG_ATTACH:
+        s->state = USB_STATE_ATTACHED;
+        return 0;
+
+    case USB_MSG_DETACH:
+        s->state = USB_STATE_NOTATTACHED;
+        return 0;
+
+    case USB_MSG_RESET:
+        s->remote_wakeup = 0;
+        s->addr = 0;
+        s->state = USB_STATE_DEFAULT;
+        s->handle_reset(s);
+        return 0;
+    }
+
+    /* Rest of the PIDs must match our address */
+    if (s->state < USB_STATE_DEFAULT || p->devaddr != s->addr)
+        return USB_RET_NODEV;
+
+    switch (p->pid) {
+    case USB_TOKEN_SETUP:
+        return do_token_setup(s, p);
+
+    case USB_TOKEN_IN:
+        return do_token_in(s, p);
+
+    case USB_TOKEN_OUT:
+        return do_token_out(s, p);
+ 
+    default:
+        return USB_RET_STALL;
+    }
+}
+
 /* returns 1 on problem encountered or 0 for success */
 static int usb_linux_update_endp_table(USBHostDevice *s)
 {
     uint8_t *descriptors;
     uint8_t devep, type, configuration, alt_interface;
-    struct usb_ctrltransfer ct;
+    struct usbdevfs_ctrltransfer ct;
     int interface, ret, length, i;
 
     ct.bRequestType = USB_DIR_IN;
@@ -624,10 +898,14 @@
 #endif
 
     dev->fd = fd;
-    dev->configuration = 1;
 
-    /* XXX - do something about initial configuration */
-    if (!usb_host_update_interfaces(dev, -1))
+    /* 
+     * Initial configuration is -1 which makes us claim first 
+     * available config. We used to start with 1, which does not
+     * always work. I've seen devices where first config starts 
+     * with 2.
+     */
+    if (!usb_host_claim_interfaces(dev, -1))
         goto fail;
 
     ret = ioctl(fd, USBDEVFS_CONNECTINFO, &ci);
@@ -646,11 +924,9 @@
         dev->dev.speed = USB_SPEED_LOW;
     else
         dev->dev.speed = USB_SPEED_HIGH;
-    dev->dev.handle_packet = usb_generic_handle_packet;
 
-    dev->dev.handle_reset = usb_host_handle_reset;
-    dev->dev.handle_control = usb_host_handle_control;
-    dev->dev.handle_data = usb_host_handle_data;
+    dev->dev.handle_packet  = usb_host_handle_packet;
+    dev->dev.handle_reset   = usb_host_handle_reset;
     dev->dev.handle_destroy = usb_host_handle_destroy;
 
     if (!prod_name || prod_name[0] == '\0')
@@ -1055,6 +1331,8 @@
 
 #else
 
+#include "hw/usb.h"
+
 void usb_host_info(void)
 {
     term_printf("USB host devices not supported\n");