diff --git a/hw/usb.c b/hw/usb.c
index be4d66d..c17266d 100644
--- a/hw/usb.c
+++ b/hw/usb.c
@@ -3,6 +3,8 @@
  *
  * Copyright (c) 2005 Fabrice Bellard
  *
+ * 2008 Generic packet handler rewrite by Max Krasnyansky
+ *
  * 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
@@ -30,6 +32,7 @@
 }
 
 /**********************/
+
 /* generic USB device helpers (you are not forced to use them when
    writing your USB device driver, but they help handling the
    protocol)
@@ -39,141 +42,164 @@
 #define SETUP_STATE_DATA 1
 #define SETUP_STATE_ACK  2
 
+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->handle_control(s, request, value, index, 
+                                s->setup_len, s->data_buf);
+        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 == 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->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)) {
+            s->setup_state = SETUP_STATE_IDLE;
+            ret = s->handle_control(s, request, value, index,
+                                    s->setup_len, s->data_buf);
+            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)
+{
+    if (p->devep != 0)
+        return s->handle_data(s, p);
+
+    switch(s->setup_state) {
+    case SETUP_STATE_ACK:
+        if (s->setup_buf[0] & USB_DIR_IN) {
+            s->setup_state = SETUP_STATE_IDLE;
+            /* transfer OK */
+        } else {
+            /* ignore additional output */
+        }
+        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(s->data_buf + s->setup_index, p->data, 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;
+    }
+}
+
+/*
+ * Generic packet handler.
+ * Called by the HC (host controller).
+ *
+ * Returns length of the transaction or one of the USB_RET_XXX codes.
+ */
 int usb_generic_handle_packet(USBDevice *s, USBPacket *p)
 {
-    int l, ret = 0;
-    int len = p->len;
-    uint8_t *data = p->data;
-
     switch(p->pid) {
     case USB_MSG_ATTACH:
         s->state = USB_STATE_ATTACHED;
-        break;
+        return 0;
+
     case USB_MSG_DETACH:
         s->state = USB_STATE_NOTATTACHED;
-        break;
+        return 0;
+
     case USB_MSG_RESET:
         s->remote_wakeup = 0;
         s->addr = 0;
         s->state = USB_STATE_DEFAULT;
         s->handle_reset(s);
-        break;
-    case USB_TOKEN_SETUP:
-        if (s->state < USB_STATE_DEFAULT || p->devaddr != s->addr)
-            return USB_RET_NODEV;
-        if (len != 8)
-            goto fail;
-        memcpy(s->setup_buf, data, 8);
-        s->setup_len = (s->setup_buf[7] << 8) | s->setup_buf[6];
-        s->setup_index = 0;
-        if (s->setup_buf[0] & USB_DIR_IN) {
-            ret = s->handle_control(s,
-                                    (s->setup_buf[0] << 8) | s->setup_buf[1],
-                                    (s->setup_buf[3] << 8) | s->setup_buf[2],
-                                    (s->setup_buf[5] << 8) | s->setup_buf[4],
-                                    s->setup_len,
-                                    s->data_buf);
-            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 == 0)
-                s->setup_state = SETUP_STATE_ACK;
-            else
-                s->setup_state = SETUP_STATE_DATA;
-        }
-        break;
-    case USB_TOKEN_IN:
-        if (s->state < USB_STATE_DEFAULT || p->devaddr != s->addr)
-            return USB_RET_NODEV;
-        switch(p->devep) {
-        case 0:
-            switch(s->setup_state) {
-            case SETUP_STATE_ACK:
-                if (!(s->setup_buf[0] & USB_DIR_IN)) {
-                    s->setup_state = SETUP_STATE_IDLE;
-                    ret = s->handle_control(s,
-                                      (s->setup_buf[0] << 8) | s->setup_buf[1],
-                                      (s->setup_buf[3] << 8) | s->setup_buf[2],
-                                      (s->setup_buf[5] << 8) | s->setup_buf[4],
-                                      s->setup_len,
-                                      s->data_buf);
-                    if (ret > 0)
-                        ret = 0;
-                } else {
-                    /* return 0 byte */
-                }
-                break;
-            case SETUP_STATE_DATA:
-                if (s->setup_buf[0] & USB_DIR_IN) {
-                    l = s->setup_len - s->setup_index;
-                    if (l > len)
-                        l = len;
-                    memcpy(data, s->data_buf + s->setup_index, l);
-                    s->setup_index += l;
-                    if (s->setup_index >= s->setup_len)
-                        s->setup_state = SETUP_STATE_ACK;
-                    ret = l;
-                } else {
-                    s->setup_state = SETUP_STATE_IDLE;
-                    goto fail;
-                }
-                break;
-            default:
-                goto fail;
-            }
-            break;
-        default:
-            ret = s->handle_data(s, p);
-            break;
-        }
-        break;
-    case USB_TOKEN_OUT:
-        if (s->state < USB_STATE_DEFAULT || p->devaddr != s->addr)
-            return USB_RET_NODEV;
-        switch(p->devep) {
-        case 0:
-            switch(s->setup_state) {
-            case SETUP_STATE_ACK:
-                if (s->setup_buf[0] & USB_DIR_IN) {
-                    s->setup_state = SETUP_STATE_IDLE;
-                    /* transfer OK */
-                } else {
-                    /* ignore additional output */
-                }
-                break;
-            case SETUP_STATE_DATA:
-                if (!(s->setup_buf[0] & USB_DIR_IN)) {
-                    l = s->setup_len - s->setup_index;
-                    if (l > len)
-                        l = len;
-                    memcpy(s->data_buf + s->setup_index, data, l);
-                    s->setup_index += l;
-                    if (s->setup_index >= s->setup_len)
-                        s->setup_state = SETUP_STATE_ACK;
-                    ret = l;
-                } else {
-                    s->setup_state = SETUP_STATE_IDLE;
-                    goto fail;
-                }
-                break;
-            default:
-                goto fail;
-            }
-            break;
-        default:
-            ret = s->handle_data(s, p);
-            break;
-        }
-        break;
-    default:
-    fail:
-        ret = USB_RET_STALL;
-        break;
+        return 0;
     }
-    return ret;
+
+    /* 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;
+    }
 }
 
 /* XXX: fix overflow */
@@ -200,5 +226,6 @@
     memset(&p, 0, sizeof(p));
     p.pid = msg;
     dev->handle_packet(dev, &p);
-}
 
+    /* This _must_ be synchronous */
+}
diff --git a/hw/usb.h b/hw/usb.h
index 4a009a5..55b7e58 100644
--- a/hw/usb.h
+++ b/hw/usb.h
@@ -120,18 +120,49 @@
 /* definition of a USB device */
 struct USBDevice {
     void *opaque;
+
+    /* 
+     * Process USB packet. 
+     * Called by the HC (Host Controller).
+     *
+     * Returns length of the transaction 
+     * or one of the USB_RET_XXX codes.
+     */ 
     int (*handle_packet)(USBDevice *dev, USBPacket *p);
+
+    /* 
+     * Called when device is destroyed.
+     */
     void (*handle_destroy)(USBDevice *dev);
 
     int speed;
 
     /* The following fields are used by the generic USB device
-       layer. They are here just to avoid creating a new structure for
-       them. */
+       layer. They are here just to avoid creating a new structure 
+       for them. */
+
+    /*
+     * Reset the device
+     */  
     void (*handle_reset)(USBDevice *dev);
+
+    /*
+     * Process control request.
+     * Called from handle_packet().
+     *
+     * Returns length or one of the USB_RET_ codes.
+     */
     int (*handle_control)(USBDevice *dev, int request, int value,
                           int index, int length, uint8_t *data);
+
+    /*
+     * Process data transfers (both BULK and ISOC).
+     * Called from handle_packet().
+     *
+     * Returns length or one of the USB_RET_ codes.
+     */
     int (*handle_data)(USBDevice *dev, USBPacket *p);
+
     uint8_t addr;
     char devname[32];