Major USB stack improvements. It now works at nearly the maximum speed for a full speed connection, and does seem stable.

Still not enabled by default, #define USE_ROCKBOX_USB is still required to enable it.


git-svn-id: svn://svn.rockbox.org/rockbox/trunk@16360 a1c6a512-1295-4272-9138-f99709370657

7 files changed, 736 insertions, 438 deletions

diff --git a/firmware/usbstack/usb_benchmark.c b/firmware/usbstack/usb_benchmark.c
index a6e0e2d..7cd5a3e 100644
--- a/firmware/usbstack/usb_benchmark.c
+++ b/firmware/usbstack/usb_benchmark.c
@@ -22,6 +22,8 @@
 //#define LOGF_ENABLE
 #include "logf.h"
 
+#ifdef USB_BENCHMARK
+
 static int current_length;
 
 static unsigned char _input_buffer[16384];
@@ -56,8 +58,8 @@ void usb_benchmark_control_request(struct usb_ctrlrequest* req)
             logf("bench: read %d", current_length);
             todo = MIN(usb_max_pkt_size, current_length);
             state = SENDING;
-            usb_drv_reset_endpoint(EP_TX, true);
-            usb_drv_send(EP_TX, &input_buffer, todo);
+            usb_drv_reset_endpoint(EP_BENCHMARK, true);
+            usb_drv_send(EP_BENCHMARK, &input_buffer, todo);
             current_length -= todo;
             break;
 
@@ -66,13 +68,13 @@ void usb_benchmark_control_request(struct usb_ctrlrequest* req)
             current_length = req->wValue * req->wIndex;
             logf("bench: write %d", current_length);
             state = RECEIVING;
-            usb_drv_reset_endpoint(EP_RX, false);
-            usb_drv_recv(EP_RX, &input_buffer, sizeof _input_buffer);
+            usb_drv_reset_endpoint(EP_BENCHMARK, false);
+            usb_drv_recv(EP_BENCHMARK, &input_buffer, sizeof _input_buffer);
             break;
     }
 }
 
-void usb_benchmark_transfer_complete(int endpoint, bool in)
+void usb_benchmark_transfer_complete(bool in)
 {
     (void)in;
 
@@ -87,26 +89,26 @@ void usb_benchmark_transfer_complete(int endpoint, bool in)
     {
         case SENDING: {
             int todo = MIN(usb_max_pkt_size, current_length);
-            if (endpoint == EP_RX) {
+            if (in == false) {
                 logf("unexpected ep_rx");
                 break;
             }
     
             logf("bench: %d more tx", current_length);
-            usb_drv_send(EP_TX, &input_buffer, todo);
+            usb_drv_send(EP_BENCHMARK, &input_buffer, todo);
             current_length -= todo;
             input_buffer[0]++;
             break;
         }
 
         case RECEIVING:
-            if (endpoint == EP_TX) {
+            if (in == true) {
                 logf("unexpected ep_tx");
                 break;
             }
 
             /* re-prime endpoint */
-            usb_drv_recv(EP_RX, &input_buffer, sizeof _input_buffer);
+            usb_drv_recv(EP_BENCHMARK, &input_buffer, sizeof _input_buffer);
             input_buffer[0]++;
             break;
 
@@ -123,3 +125,4 @@ static void ack_control(struct usb_ctrlrequest* req)
     else
         usb_drv_send(EP_CONTROL, NULL, 0);
 }
+#endif /*USB_BENCHMARK*/
diff --git a/firmware/usbstack/usb_benchmark.h b/firmware/usbstack/usb_benchmark.h
index 84853a1..12c32a7 100644
--- a/firmware/usbstack/usb_benchmark.h
+++ b/firmware/usbstack/usb_benchmark.h
@@ -21,6 +21,6 @@
 
 void usb_benchmark_init(void);
 void usb_benchmark_control_request(struct usb_ctrlrequest* req);
-void usb_benchmark_transfer_complete(int endpoint, bool in);
+void usb_benchmark_transfer_complete(bool in);
 
 #endif
diff --git a/firmware/usbstack/usb_core.c b/firmware/usbstack/usb_core.c
index 13993f9..c68093b 100644
--- a/firmware/usbstack/usb_core.c
+++ b/firmware/usbstack/usb_core.c
@@ -23,44 +23,42 @@
 //#define LOGF_ENABLE
 #include "logf.h"
 
-#ifndef BOOTLOADER
-//#define USB_SERIAL
-//#define USB_BENCHMARK
-#ifdef USE_ROCKBOX_USB
-#define USB_STORAGE
-#else
-#define USB_CHARGING_ONLY
-#endif /* USE_ROCKBOX_USB */
-#else
-#define USB_CHARGING_ONLY
-#endif
-
 #include "usb_ch9.h"
 #include "usb_drv.h"
 #include "usb_core.h"
 
+#define USB_THREAD
+
 #if defined(USB_STORAGE)
 #include "usb_storage.h"
-#define USB_THREAD
-#elif defined(USB_SERIAL)
-#define USB_THREAD
+#endif
+
+#if defined(USB_SERIAL)
 #include "usb_serial.h"
-#elif defined(USB_BENCHMARK)
+#endif
+
+#if defined(USB_BENCHMARK)
 #include "usb_benchmark.h"
 #endif
 
+/* TODO: Move this target-specific stuff somewhere else (serial number reading) */
+
+#ifdef HAVE_AS3514
+#include "i2c-pp.h"
+#include "as3514.h"
+#endif
+
+
 /*-------------------------------------------------------------------------*/
 /* USB protocol descriptors: */
 
 #define USB_SC_SCSI      0x06            /* Transparent */
 #define USB_PROT_BULK    0x50            /* bulk only */
 
-int usb_max_pkt_size = 512;
-
-static const struct usb_device_descriptor device_descriptor = {
+static const struct usb_device_descriptor device_descriptor= {
     .bLength            = sizeof(struct usb_device_descriptor),
     .bDescriptorType    = USB_DT_DEVICE,
-    .bcdUSB             = 0x0200, /* USB version 2.0 */
+    .bcdUSB             = 0x0200,
     .bDeviceClass       = USB_CLASS_PER_INTERFACE,
     .bDeviceSubClass    = 0,
     .bDeviceProtocol    = 0,
@@ -74,158 +72,34 @@ static const struct usb_device_descriptor device_descriptor = {
     .bNumConfigurations = 1
 };
 
-static const struct {
+static struct {
     struct usb_config_descriptor config_descriptor;
-    struct usb_interface_descriptor interface_descriptor;
-    struct usb_endpoint_descriptor ep1_in_descriptor;
-    struct usb_endpoint_descriptor ep1_out_descriptor;
-} config_data_fs = 
-{
-    {
-        .bLength             = sizeof(struct usb_config_descriptor),
-        .bDescriptorType     = USB_DT_CONFIG,
-        .wTotalLength        = sizeof config_data_fs,
-        .bNumInterfaces      = 1,
-        .bConfigurationValue = 1,
-        .iConfiguration      = 0,
-        .bmAttributes        = USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER,
-        .bMaxPower           = 250, /* 500mA in 2mA units */
-    },
-
 #ifdef USB_CHARGING_ONLY
-    /* dummy interface for charging-only */
-    {
-        .bLength            = sizeof(struct usb_interface_descriptor),
-        .bDescriptorType    = USB_DT_INTERFACE,
-        .bInterfaceNumber   = 0,
-        .bAlternateSetting  = 0,
-        .bNumEndpoints      = 2,
-        .bInterfaceClass    = USB_CLASS_VENDOR_SPEC,
-        .bInterfaceSubClass = 0,
-        .bInterfaceProtocol = 0,
-        .iInterface         = 5
-    },
-
-    {
-        .bLength          = sizeof(struct usb_endpoint_descriptor),
-        .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_TX | USB_DIR_IN,
-        .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
-        .bInterval        = 0
-    },
-    {
-        .bLength          = sizeof(struct usb_endpoint_descriptor),
-        .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_RX | USB_DIR_OUT,
-        .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
-        .bInterval        = 0
-    }
+    struct usb_interface_descriptor charging_interface_descriptor;
+    struct usb_endpoint_descriptor charging_ep_in_descriptor;
+    struct usb_endpoint_descriptor charging_ep_out_descriptor;
 #endif
-
 #ifdef USB_STORAGE
-    /* storage interface */
-    {
-        .bLength            = sizeof(struct usb_interface_descriptor),
-        .bDescriptorType    = USB_DT_INTERFACE,
-        .bInterfaceNumber   = 0,
-        .bAlternateSetting  = 0,
-        .bNumEndpoints      = 2,
-        .bInterfaceClass    = USB_CLASS_MASS_STORAGE,
-        .bInterfaceSubClass = USB_SC_SCSI,
-        .bInterfaceProtocol = USB_PROT_BULK,
-        .iInterface         = 0
-    },
-
-    {
-        .bLength          = sizeof(struct usb_endpoint_descriptor),
-        .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_TX | USB_DIR_IN,
-        .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 16,
-        .bInterval        = 0
-    },
-    {
-        .bLength          = sizeof(struct usb_endpoint_descriptor),
-        .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_RX | USB_DIR_OUT,
-        .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 16,
-        .bInterval        = 0
-    }
+    struct usb_interface_descriptor mass_storage_interface_descriptor;
+    struct usb_endpoint_descriptor mass_storage_ep_in_descriptor;
+    struct usb_endpoint_descriptor mass_storage_ep_out_descriptor;
 #endif
-
 #ifdef USB_SERIAL
-    /* serial interface */
-    {
-        .bLength            = sizeof(struct usb_interface_descriptor),
-        .bDescriptorType    = USB_DT_INTERFACE,
-        .bInterfaceNumber   = 0,
-        .bAlternateSetting  = 0,
-        .bNumEndpoints      = 2,
-        .bInterfaceClass    = USB_CLASS_CDC_DATA,
-        .bInterfaceSubClass = 0,
-        .bInterfaceProtocol = 0,
-        .iInterface         = 0
-    },
-
-    {
-        .bLength          = sizeof(struct usb_endpoint_descriptor),
-        .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_TX | USB_DIR_IN,
-        .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 64,
-        .bInterval        = 0
-    },
-    {
-        .bLength          = sizeof(struct usb_endpoint_descriptor),
-        .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_RX | USB_DIR_OUT,
-        .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 64,
-        .bInterval        = 0
-    }
+    struct usb_interface_descriptor serial_interface_descriptor;
+    struct usb_endpoint_descriptor serial_ep_in_descriptor;
+    struct usb_endpoint_descriptor serial_ep_out_descriptor;
 #endif
-
 #ifdef USB_BENCHMARK
-    /* bulk test interface */
-    {
-        .bLength            = sizeof(struct usb_interface_descriptor),
-        .bDescriptorType    = USB_DT_INTERFACE,
-        .bInterfaceNumber   = 0,
-        .bAlternateSetting  = 0,
-        .bNumEndpoints      = 2,
-        .bInterfaceClass    = USB_CLASS_VENDOR_SPEC,
-        .bInterfaceSubClass = 255,
-        .bInterfaceProtocol = 255,
-        .iInterface         = 4
-    },
-
-    {
-        .bLength          = sizeof(struct usb_endpoint_descriptor),
-        .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_RX | USB_DIR_OUT,
-        .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 64,
-        .bInterval        = 0
-    },
-    {
-        .bLength          = sizeof(struct usb_endpoint_descriptor),
-        .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_TX | USB_DIR_IN,
-        .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 64,
-        .bInterval        = 0
-    }
+    struct usb_interface_descriptor benchmark_interface_descriptor;
+    struct usb_endpoint_descriptor benchmark_ep_in_descriptor;
+    struct usb_endpoint_descriptor benchmark_ep_out_descriptor;
 #endif
-},
-config_data_hs = 
+} __attribute__((packed)) *config_data, _config_data = 
 {
     {
         .bLength             = sizeof(struct usb_config_descriptor),
         .bDescriptorType     = USB_DT_CONFIG,
-        .wTotalLength        = sizeof config_data_hs,
+        .wTotalLength        = sizeof _config_data,
         .bNumInterfaces      = 1,
         .bConfigurationValue = 1,
         .iConfiguration      = 0,
@@ -246,23 +120,23 @@ config_data_hs =
         .bInterfaceProtocol = 0,
         .iInterface         = 5
     },
-
+/* TODO: try with zero endpoints */
     {
         .bLength          = sizeof(struct usb_endpoint_descriptor),
         .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_TX | USB_DIR_IN,
+        .bEndpointAddress = EP_CHARGING_ONLY | USB_DIR_IN,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
+        .wMaxPacketSize   = 16,
         .bInterval        = 0
     },
     {
         .bLength          = sizeof(struct usb_endpoint_descriptor),
         .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_RX | USB_DIR_OUT,
+        .bEndpointAddress = EP_CHARGING_ONLY | USB_DIR_OUT,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
+        .wMaxPacketSize   = 16,
         .bInterval        = 0
-    }
+    },
 #endif
 
 #ifdef USB_STORAGE
@@ -282,19 +156,19 @@ config_data_hs =
     {
         .bLength          = sizeof(struct usb_endpoint_descriptor),
         .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_TX | USB_DIR_IN,
+        .bEndpointAddress = EP_MASS_STORAGE | USB_DIR_IN,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
+        .wMaxPacketSize   = 16,
         .bInterval        = 0
     },
     {
         .bLength          = sizeof(struct usb_endpoint_descriptor),
         .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_RX | USB_DIR_OUT,
+        .bEndpointAddress = EP_MASS_STORAGE | USB_DIR_OUT,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
+        .wMaxPacketSize   = 16,
         .bInterval        = 0
-    }
+    },
 #endif
 
 #ifdef USB_SERIAL
@@ -314,19 +188,19 @@ config_data_hs =
     {
         .bLength          = sizeof(struct usb_endpoint_descriptor),
         .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_TX | USB_DIR_IN,
+        .bEndpointAddress = EP_SERIAL | USB_DIR_IN,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
+        .wMaxPacketSize   = 16,
         .bInterval        = 0
     },
     {
         .bLength          = sizeof(struct usb_endpoint_descriptor),
         .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_RX | USB_DIR_OUT,
+        .bEndpointAddress = EP_SERIAL | USB_DIR_OUT,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
+        .wMaxPacketSize   = 16,
         .bInterval        = 0
-    }
+    },
 #endif
 
 #ifdef USB_BENCHMARK
@@ -346,19 +220,19 @@ config_data_hs =
     {
         .bLength          = sizeof(struct usb_endpoint_descriptor),
         .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_RX | USB_DIR_OUT,
+        .bEndpointAddress = EP_BENCHMARK | USB_DIR_OUT,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
+        .wMaxPacketSize   = 16,
         .bInterval        = 0
     },
     {
         .bLength          = sizeof(struct usb_endpoint_descriptor),
         .bDescriptorType  = USB_DT_ENDPOINT,
-        .bEndpointAddress = EP_TX | USB_DIR_IN,
+        .bEndpointAddress = EP_BENCHMARK | USB_DIR_IN,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 512,
+        .wMaxPacketSize   = 16,
         .bInterval        = 0
-    }
+    },
 #endif
 };
 
@@ -388,13 +262,22 @@ static struct usb_string_descriptor usb_string_iProduct =
     {'R','o','c','k','b','o','x',' ','m','e','d','i','a',' ','p','l','a','y','e','r'}
 };
 
+#if defined(HAVE_AS3514)
 static struct usb_string_descriptor usb_string_iSerial =
 {
-    34, 
-    USB_DT_STRING, 
-    {'0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0'}
+    66,
+    USB_DT_STRING,
+    {'0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0',
+     '0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0'}
 };
-
+#else
+static struct usb_string_descriptor usb_string_iSerial =
+{
+    34,
+    USB_DT_STRING,
+     {'0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0'}
+ };
+#endif
 
 /* Generic for all targets */
 
@@ -443,10 +326,25 @@ static long usbcore_stack[DEFAULT_STACK_SIZE];
 static void usb_core_thread(void);
 #endif
 
-static void ack_control(struct usb_ctrlrequest* req);
+static void usb_core_control_request_handler(struct usb_ctrlrequest* req);
+static int ack_control(struct usb_ctrlrequest* req);
+
+static unsigned char *response_data;
+static unsigned char __response_data[CACHEALIGN_UP(2)] CACHEALIGN_ATTR;
+
+struct usb_core_event 
+{
+    unsigned char endpoint;
+    bool in;
+    int status;
+    int length;
+    void* data;
+};
+
+static struct usb_core_event events[NUM_ENDPOINTS];
 
 #ifdef IPOD_ARCH
-void set_serial_descriptor(void)
+static void set_serial_descriptor(void)
 {
     static short hex[16] = {'0','1','2','3','4','5','6','7',
                             '8','9','A','B','C','D','E','F'};
@@ -471,7 +369,23 @@ void set_serial_descriptor(void)
           x >>= 4;
        }
     }
+}
+#elif defined(HAVE_AS3514)
+static void set_serial_descriptor(void)
+{
+    static short hex[16] = {'0','1','2','3','4','5','6','7',
+                            '8','9','A','B','C','D','E','F'};
 
+    unsigned char serial[16];
+    short* p = usb_string_iSerial.wString;
+    int i;
+
+    i2c_readbytes(AS3514_I2C_ADDR, 0x30, 0x10, serial);
+    for (i = 0; i < 16; i++)
+    {
+        *p++ = hex[(serial[i] >> 4) & 0xF];
+        *p++ = hex[(serial[i] >> 0) & 0xF];
+    }
 }
 #endif
 
@@ -480,9 +394,8 @@ void usb_core_init(void)
     if (initialized)
         return;
 
-#ifdef IPOD_ARCH
-    set_serial_descriptor();
-#endif
+    config_data = (void*)UNCACHED_ADDR(&_config_data);
+    response_data = (void*)UNCACHED_ADDR(&__response_data);
 
     queue_init(&usbcore_queue, false);
     usb_drv_init();
@@ -514,10 +427,11 @@ void usb_core_exit(void)
 {
     if (initialized) {
         usb_drv_exit();
-        queue_delete(&usbcore_queue);
 #ifdef USB_THREAD
-        remove_thread(usbcore_thread);
+        queue_post(&usbcore_queue, USB_CORE_QUIT, 0);
+        thread_wait(usbcore_thread);
 #endif
+        queue_delete(&usbcore_queue);
     }
     data_connection = false;
     initialized = false;
@@ -532,25 +446,52 @@ bool usb_core_data_connection(void)
 #ifdef USB_THREAD
 void usb_core_thread(void)
 {
+#if defined(IPOD_ARCH) || defined(HAVE_AS3514)
+    set_serial_descriptor();
+#endif
+
     while (1) {
         struct queue_event ev;
 
         queue_wait(&usbcore_queue, &ev);
+        if (ev.id == USB_CORE_QUIT) {
+            cancel_cpu_boost();
+            return;
+        }
 
+        if (ev.id == USB_CORE_TRANSFER_COMPLETION) {
+            struct usb_core_event* event = (struct usb_core_event*)ev.data;
+            switch(event->endpoint) {
+                case EP_CONTROL:
+                    logf("ctrl handled %ld",current_tick);
+                    usb_core_control_request_handler((struct usb_ctrlrequest*)event->data);
+                    break;
 #ifdef USB_STORAGE
-        usb_storage_transfer_complete(ev.id);
+                case EP_MASS_STORAGE:
+                    usb_storage_transfer_complete(event->in,event->status,event->length);
+                    break;
 #endif
-
 #ifdef USB_SERIAL
-        usb_serial_transfer_complete(ev.id);
+                case EP_SERIAL:
+                    usb_serial_transfer_complete(event->in,event->status,event->length);
+                    break;
 #endif
-
+#ifdef USB_BENCHMARK
+                case EP_BENCHMARK:
+                    usb_benchmark_transfer_complete(event->in);
+                    break;
+#endif
+#ifdef USB_CHARGING_ONLY
+                case EP_CHARGING_ONLY:
+                    break;
+#endif
+            }
+        }
     }
 }
 #endif
 
-/* called by usb_drv_int() */
-void usb_core_control_request(struct usb_ctrlrequest* req)
+static void usb_core_control_request_handler(struct usb_ctrlrequest* req)
 {
     /* note: interrupt context */
     data_connection = true;
@@ -565,29 +506,31 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
     switch (req->bRequest) {
         case USB_REQ_SET_CONFIGURATION:
             logf("usb_core: SET_CONFIG");
+            usb_drv_cancel_all_transfers();
+            if (req->wValue){
+                usb_state = CONFIGURED;
 #ifdef USB_STORAGE
-            usb_storage_control_request(req);
+                usb_storage_control_request(req);
 #endif
 
 #ifdef USB_SERIAL
-            usb_serial_control_request(req);
+                usb_serial_control_request(req);
 #endif
-            ack_control(req);
-            if (req->wValue)
-                usb_state = CONFIGURED;
-            else
+            }
+            else {
                 usb_state = ADDRESS;
+            }
+            ack_control(req);
             break;
 
         case USB_REQ_GET_CONFIGURATION: {
-            static char confignum;
-            char* tmp = (void*)UNCACHED_ADDR(&confignum);
             logf("usb_core: GET_CONFIG");
             if (usb_state == ADDRESS)
-                *tmp = 0;
+                response_data[0] = 0;
             else
-                *tmp = 1;
-            usb_drv_send(EP_CONTROL, tmp, 1);
+                response_data[0] = 1;
+            if(usb_drv_send(EP_CONTROL, response_data, 1)!= 0)
+                break;
             ack_control(req);
             break;
         }
@@ -597,29 +540,54 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
             ack_control(req);
             break;
 
+        case USB_REQ_GET_INTERFACE:
+            logf("usb_core: GET_INTERFACE");
+            response_data[0] = 0;
+            if(usb_drv_send(EP_CONTROL, response_data, 1)!=0)
+                break;
+            ack_control(req);
+            break;
         case USB_REQ_CLEAR_FEATURE:
             logf("usb_core: CLEAR_FEATURE");
             if (req->wValue)
-                usb_drv_stall(req->wIndex, true);
+                usb_drv_stall(req->wIndex & 0xf, false,(req->wIndex & 0x80) !=0);
             else
-                usb_drv_stall(req->wIndex, false);
+                usb_drv_stall(req->wIndex & 0xf, false,(req->wIndex & 0x80) !=0);
             ack_control(req);
             break;
 
-        case USB_REQ_SET_ADDRESS:
-            usb_address = req->wValue;
-            logf("usb_core: SET_ADR %d", usb_address);
+        case USB_REQ_SET_FEATURE:
+            logf("usb_core: SET_FEATURE");
+            if (req->wValue)
+                usb_drv_stall(req->wIndex & 0xf, true,(req->wIndex & 0x80) !=0);
+            else
+                usb_drv_stall(req->wIndex & 0xf, false,(req->wIndex & 0x80) !=0);
             ack_control(req);
+            break;
+
+        case USB_REQ_SET_ADDRESS: {
+            unsigned char address = req->wValue;
+            logf("usb_core: SET_ADR %d", address);
+            if(ack_control(req)!=0)
+                break;
+            usb_drv_cancel_all_transfers();
+            usb_address = address;
             usb_drv_set_address(usb_address);
             usb_state = ADDRESS;
             break;
+        }
 
         case USB_REQ_GET_STATUS: {
-            static char tmp[2] = {0,0};
-            tmp[0] = 0;
-            tmp[1] = 0;
+            response_data[0]= 0;
+            response_data[1]= 0;
             logf("usb_core: GET_STATUS"); 
-            usb_drv_send(EP_CONTROL, UNCACHED_ADDR(&tmp), 2);
+            if(req->wIndex>0) {
+                if(usb_drv_stalled(req->wIndex&0xf,(req->wIndex&0x80)!=0))
+                    response_data[0] = 1;
+            }
+            logf("usb_core: %X %X",response_data[0],response_data[1]); 
+            if(usb_drv_send(EP_CONTROL, response_data, 2)!=0)
+                break;
             ack_control(req);
             break;
         }
@@ -637,18 +605,59 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
                     size = sizeof device_descriptor;
                     break;
 
-                case USB_DT_CONFIG:
-                    if(usb_drv_port_speed())
-                    {
-                        ptr = &config_data_hs;
-                        size = sizeof config_data_hs;
+                case USB_DT_OTHER_SPEED_CONFIG:
+                case USB_DT_CONFIG: {
+                    int max_packet_size;
+                    int interface_number=0;
+
+                    if(req->wValue >> 8 == USB_DT_CONFIG) {
+                        if(usb_drv_port_speed()) {
+                            max_packet_size=512;
+                        }
+                        else {
+                            max_packet_size=64;
+                        }
+                        config_data->config_descriptor.bDescriptorType=USB_DT_CONFIG;
                     }
-                    else
-                    {
-                        ptr = &config_data_fs;
-                        size = sizeof config_data_fs;
+                    else {
+                        if(usb_drv_port_speed()) {
+                            max_packet_size=64;
+                        }
+                        else {
+                            max_packet_size=512;
+                        }
+                        config_data->config_descriptor.bDescriptorType=USB_DT_OTHER_SPEED_CONFIG;
                     }
+
+#ifdef USB_CHARGING_ONLY
+                    memcpy(&config_data->charging_ep_in_descriptor.wMaxPacketSize,&max_packet_size,sizeof(unsigned short));
+                    memcpy(&config_data->charging_ep_out_descriptor.wMaxPacketSize,&max_packet_size,sizeof(unsigned short));
+                    config_data->charging_interface_descriptor.bInterfaceNumber=interface_number;
+                    interface_number++;
+#endif
+#ifdef USB_STORAGE
+                    memcpy(&config_data->mass_storage_ep_in_descriptor.wMaxPacketSize,&max_packet_size,sizeof(unsigned short));
+                    memcpy(&config_data->mass_storage_ep_out_descriptor.wMaxPacketSize,&max_packet_size,sizeof(unsigned short));
+                    config_data->mass_storage_interface_descriptor.bInterfaceNumber=interface_number;
+                    interface_number++;
+#endif
+#ifdef USB_SERIAL
+                    memcpy(&config_data->serial_ep_in_descriptor.wMaxPacketSize,&max_packet_size,sizeof(unsigned short));
+                    memcpy(&config_data->serial_ep_out_descriptor.wMaxPacketSize,&max_packet_size,sizeof(unsigned short));
+                    config_data->serial_interface_descriptor.bInterfaceNumber=interface_number;
+                    interface_number++;
+#endif
+#ifdef USB_BENCHMARK
+                    memcpy(&config_data->benchmark_ep_in_descriptor.wMaxPacketSize,&max_packet_size,sizeof(unsigned short));
+                    memcpy(&config_data->benchmark_ep_out_descriptor.wMaxPacketSize,&max_packet_size,sizeof(unsigned short));
+                    config_data.benchmark_interface_descriptor.bInterfaceNumber=interface_number;
+                    interface_number++;
+#endif
+                    config_data->config_descriptor.bNumInterfaces=interface_number;
+                    ptr = config_data;
+                    size = sizeof _config_data;
                     break;
+                    }
 
                 case USB_DT_STRING:
                     if ((unsigned)index < (sizeof(usb_strings)/sizeof(struct usb_string_descriptor*))) {
@@ -657,7 +666,7 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
                     }
                     else {
                         logf("bad string id %d", index);
-                        usb_drv_stall(EP_CONTROL, true);
+                        usb_drv_stall(EP_CONTROL, true,true);
                     }
                     break;
 
@@ -668,13 +677,14 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
 
                 default:
                     logf("bad desc %d", req->wValue >> 8);
-                    usb_drv_stall(EP_CONTROL, true);
+                    usb_drv_stall(EP_CONTROL, true,true);
                     break;
             }
 
             if (ptr) {
                 length = MIN(size, length);
-                usb_drv_send(EP_CONTROL, (void*)UNCACHED_ADDR(ptr), length);
+                if(usb_drv_send(EP_CONTROL, (void*)UNCACHED_ADDR(ptr), length)!=0)
+                    break;
             }
             ack_control(req);
             break;
@@ -693,7 +703,7 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
             {
                 /* nope. flag error */
                 logf("usb bad req %d", req->bRequest);
-                usb_drv_stall(EP_CONTROL, true);
+                usb_drv_stall(EP_CONTROL, true,true);
                 ack_control(req);
             }
             break;
@@ -709,9 +719,9 @@ void usb_core_bus_reset(void)
 }
 
 /* called by usb_drv_transfer_completed() */
-void usb_core_transfer_complete(int endpoint, bool in)
+void usb_core_transfer_complete(int endpoint, bool in, int status,int length)
 {
-#ifdef USB_CHARGING_ONLY
+#if defined(USB_CHARGING_ONLY) || defined(USB_STORAGE)
     (void)in;
 #endif
 
@@ -720,25 +730,35 @@ void usb_core_transfer_complete(int endpoint, bool in)
             /* already handled */
             break;
 
-        case EP_RX:
-        case EP_TX:
-#if defined(USB_BENCHMARK)
-            usb_benchmark_transfer_complete(endpoint, in);
-#elif defined(USB_STORAGE) || defined(USB_SERIAL)
-            queue_post(&usbcore_queue, endpoint, 0);
-#endif
-            break;
-
         default:
+            events[endpoint].endpoint=endpoint;
+            events[endpoint].in=in;
+            events[endpoint].data=0;
+            events[endpoint].status=status;
+            events[endpoint].length=length;
+            /* All other endoints. Let the thread deal with it */
+            queue_post(&usbcore_queue, USB_CORE_TRANSFER_COMPLETION, (intptr_t)&events[endpoint]);
             break;
     }
 }
 
-static void ack_control(struct usb_ctrlrequest* req)
+/* called by usb_drv_int() */
+void usb_core_control_request(struct usb_ctrlrequest* req)
+{
+    events[0].endpoint=0;
+    events[0].in=0;
+    events[0].data=(void *)req;
+    events[0].status=0;
+    events[0].length=0;
+    logf("ctrl received %ld",current_tick);
+    queue_post(&usbcore_queue, USB_CORE_TRANSFER_COMPLETION,(intptr_t)&events[0]);
+}
+
+static int ack_control(struct usb_ctrlrequest* req)
 {
     if (req->bRequestType & 0x80)
-        usb_drv_recv(EP_CONTROL, NULL, 0);
+        return usb_drv_recv(EP_CONTROL, NULL, 0);
     else
-        usb_drv_send(EP_CONTROL, NULL, 0);
+        return usb_drv_send(EP_CONTROL, NULL, 0);
 }
 
diff --git a/firmware/usbstack/usb_serial.c b/firmware/usbstack/usb_serial.c
index 5513c56..7787809 100644
--- a/firmware/usbstack/usb_serial.c
+++ b/firmware/usbstack/usb_serial.c
@@ -23,6 +23,8 @@
 //#define LOGF_ENABLE
 #include "logf.h"
 
+#ifdef USB_SERIAL
+
 static unsigned char _transfer_buffer[16];
 static unsigned char* transfer_buffer;
 
@@ -34,20 +36,26 @@ void usb_serial_init(void)
 }
 
 /* called by usb_core_transfer_complete() */
-void usb_serial_transfer_complete(int endpoint)
+void usb_serial_transfer_complete(bool in, int status, int length)
 {
-    switch (endpoint) {
-        case EP_RX:
+    int i;
+    switch (in) {
+        case false:
             logf("serial: %s", transfer_buffer);
-
-            /* re-prime endpoint */
-            usb_drv_recv(EP_RX, transfer_buffer, sizeof _transfer_buffer);
-
-            /* echo back :) */
-            usb_drv_send(EP_TX, transfer_buffer, sizeof transfer_buffer);
+            /* Data received. Send it back */
+            for(i=0;i<length;i++) {
+                if(transfer_buffer[i]>0x40 && transfer_buffer[i]<0x5b)
+                    transfer_buffer[i]+=0x20;
+                else if(transfer_buffer[i]>0x60 && transfer_buffer[i]<0x7b)
+                    transfer_buffer[i]-=0x20;
+            }
+            usb_drv_send_nonblocking(EP_SERIAL, transfer_buffer, length);
             break;
 
-        case EP_TX:
+        case true:
+            /* Data sent out (maybe correctly, but we don't actually care.
+             * Re-prime read endpoint */
+            usb_drv_recv(EP_SERIAL, transfer_buffer, sizeof _transfer_buffer);
             break;
     }
 }
@@ -55,14 +63,12 @@ void usb_serial_transfer_complete(int endpoint)
 /* called by usb_core_control_request() */
 bool usb_serial_control_request(struct usb_ctrlrequest* req)
 {
-    /* note: interrupt context */
-
     bool handled = false;
     switch (req->bRequest) {
         case USB_REQ_SET_CONFIGURATION:
             logf("serial: set config");
             /* prime rx endpoint */
-            usb_drv_recv(EP_RX, transfer_buffer, sizeof _transfer_buffer);
+            usb_drv_recv(EP_SERIAL, transfer_buffer, sizeof _transfer_buffer);
             handled = true;
             break;
 
@@ -72,3 +78,5 @@ bool usb_serial_control_request(struct usb_ctrlrequest* req)
 
     return handled;
 }
+
+#endif /*USB_SERIAL*/
diff --git a/firmware/usbstack/usb_serial.h b/firmware/usbstack/usb_serial.h
index d6f970b..60cede9 100644
--- a/firmware/usbstack/usb_serial.h
+++ b/firmware/usbstack/usb_serial.h
@@ -22,7 +22,7 @@
 #include "usb_ch9.h"
 
 void usb_serial_init(void);
-void usb_serial_transfer_complete(int endpoint);
+void usb_serial_transfer_complete(bool in, int status, int length);
 bool usb_serial_control_request(struct usb_ctrlrequest* req);
 
 #endif
diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c
index a1faf3d..0904e17 100644
--- a/firmware/usbstack/usb_storage.c
+++ b/firmware/usbstack/usb_storage.c
@@ -26,8 +26,22 @@
 #include "hotswap.h"
 #include "disk.h"
 
+#ifdef USB_STORAGE
+
+/* Enable the following define to export only the SD card slot. This
+ * is useful for USBCV MSC tests, as those are destructive.
+ * This won't work right if the device doesn't have a card slot.
+ */
+//#define ONLY_EXPOSE_CARD_SLOT
+
 #define SECTOR_SIZE 512
 
+/* We can currently use up to 20k buffer size. More than that requires
+ * transfer chaining in the driver. Tests on sansa c200 show that the 16k
+ * limitation causes no more than 2% slowdown.
+ */
+#define BUFFER_SIZE 16384
+
 /* bulk-only class specific requests */
 #define USB_BULK_RESET_REQUEST   0xff
 #define USB_BULK_GET_MAX_LUN     0xfe
@@ -40,7 +54,8 @@
 
 #define SCSI_TEST_UNIT_READY      0x00
 #define SCSI_INQUIRY              0x12
-#define SCSI_MODE_SENSE           0x1a
+#define SCSI_MODE_SENSE_6         0x1a
+#define SCSI_MODE_SENSE_10        0x5a
 #define SCSI_REQUEST_SENSE        0x03
 #define SCSI_ALLOW_MEDIUM_REMOVAL 0x1e
 #define SCSI_READ_CAPACITY        0x25
@@ -48,11 +63,23 @@
 #define SCSI_READ_10              0x28
 #define SCSI_WRITE_10             0x2a
 #define SCSI_START_STOP_UNIT      0x1b
+#define SCSI_REPORT_LUNS          0xa0
 
 #define SCSI_STATUS_GOOD            0x00
 #define SCSI_STATUS_FAIL            0x01
 #define SCSI_STATUS_CHECK_CONDITION 0x02
 
+#define SENSE_NOT_READY             0x02
+#define SENSE_MEDIUM_ERROR          0x03
+#define SENSE_ILLEGAL_REQUEST       0x05
+#define SENSE_UNIT_ATTENTION        0x06
+
+#define ASC_MEDIUM_NOT_PRESENT      0x3a
+#define ASC_INVALID_FIELD_IN_CBD    0x24
+#define ASC_LBA_OUT_OF_RANGE        0x21
+#define ASC_WRITE_ERROR             0x0C
+#define ASC_READ_ERROR              0x11
+
 #define SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA 0x02000000
 
 
@@ -69,6 +96,15 @@ struct inquiry_data {
     unsigned char ProductRevisionLevel[4];
 } __attribute__ ((packed));
 
+struct report_lun_data {
+    unsigned int lun_list_length;
+    unsigned int reserved1;
+    unsigned char lun0[8];
+#ifdef HAVE_HOTSWAP
+    unsigned char lun1[8];
+#endif
+} __attribute__ ((packed));
+
 struct sense_data {
     unsigned char ResponseCode;
     unsigned char Obsolete;
@@ -83,6 +119,21 @@ struct sense_data {
     unsigned short SenseKeySpecific;
 } __attribute__ ((packed));
 
+struct mode_sense_header_10 {
+    unsigned short mode_data_length;
+    unsigned char medium_type;
+    unsigned char device_specific;
+    unsigned char reserved1[2];
+    unsigned short block_descriptor_length;
+} __attribute__ ((packed));
+
+struct mode_sense_header_6 {
+    unsigned char mode_data_length;
+    unsigned char medium_type;
+    unsigned char device_specific;
+    unsigned char block_descriptor_length;
+} __attribute__ ((packed));
+
 struct command_block_wrapper {
     unsigned int signature;
     unsigned int tag;
@@ -111,93 +162,195 @@ struct format_capacity {
     unsigned int block_size;
 } __attribute__ ((packed));
 
-/* the ARC USB controller can at most buffer 16KB unaligned data */
-static unsigned char _transfer_buffer[16384*8] __attribute((aligned (4096)));
+static unsigned char _transfer_buffer[2*BUFFER_SIZE] __attribute((aligned (4096)));
 static unsigned char* transfer_buffer;
-static struct inquiry_data _inquiry CACHEALIGN_ATTR;
+
 static struct inquiry_data* inquiry;
-static struct capacity _capacity_data CACHEALIGN_ATTR;
+static unsigned char __inquiry[CACHEALIGN_UP(sizeof(struct inquiry_data))] CACHEALIGN_ATTR;
+
 static struct capacity* capacity_data;
-static struct format_capacity _format_capacity_data CACHEALIGN_ATTR;
+static unsigned char __capacity_data[CACHEALIGN_UP(sizeof(struct capacity))] CACHEALIGN_ATTR;
+
 static struct format_capacity* format_capacity_data;
-static struct sense_data _sense_data CACHEALIGN_ATTR;
+static unsigned char __format_capacity_data[CACHEALIGN_UP(sizeof(struct format_capacity))] CACHEALIGN_ATTR;
+
 static struct sense_data *sense_data;
+static unsigned char __sense_data[CACHEALIGN_UP(sizeof(struct sense_data))] CACHEALIGN_ATTR;
+
+static struct mode_sense_header_6 *mode_sense_data_6;
+static unsigned char __mode_sense_data_6[CACHEALIGN_UP(sizeof(struct mode_sense_header_6))] CACHEALIGN_ATTR;
+
+static struct mode_sense_header_10 *mode_sense_data_10;
+static unsigned char __mode_sense_data_10[CACHEALIGN_UP(sizeof(struct mode_sense_header_10))] CACHEALIGN_ATTR;
+
+static struct report_lun_data *lun_data;
+static unsigned char __lun_data[CACHEALIGN_UP(sizeof(struct report_lun_data))] CACHEALIGN_ATTR;
+
+static struct command_status_wrapper* csw;
+static unsigned char __csw[CACHEALIGN_UP(sizeof(struct command_status_wrapper))] CACHEALIGN_ATTR;
+
+static char *max_lun;
+static unsigned char __max_lun[CACHEALIGN_UP(1)] CACHEALIGN_ATTR;
 
 static struct {
     unsigned int sector;
     unsigned int count;
     unsigned int tag;
     unsigned int lun;
+    unsigned char *data[2];
+    unsigned char data_select;
+    unsigned int last_result;
 } current_cmd;
 
+static struct {
+    unsigned char sense_key;
+    unsigned char information;
+    unsigned char asc;
+} cur_sense_data;
+
 static void handle_scsi(struct command_block_wrapper* cbw);
-static void send_csw(unsigned int tag, int status);
+static void send_csw(int status);
+static void send_command_result(void *data,int size);
+static void send_block_data(void *data,int size);
+static void receive_block_data(void *data,int size);
 static void identify2inquiry(int lun);
+static void send_and_read_next(void);
 
 static enum {
-    IDLE,
-    SENDING,
-    RECEIVING
-} state = IDLE;
+    WAITING_FOR_COMMAND,
+    SENDING_BLOCKS,
+    SENDING_RESULT,
+    RECEIVING_BLOCKS,
+    SENDING_CSW
+} state = WAITING_FOR_COMMAND;
 
 /* called by usb_code_init() */
 void usb_storage_init(void)
 {
-    inquiry = (void*)UNCACHED_ADDR(&_inquiry);
     transfer_buffer = (void*)UNCACHED_ADDR(&_transfer_buffer);
-    capacity_data = (void*)UNCACHED_ADDR(&_capacity_data);
-    format_capacity_data = (void*)UNCACHED_ADDR(&_format_capacity_data);
-    sense_data = (void*)UNCACHED_ADDR(&_sense_data);
-    state = IDLE;
+    inquiry = (void*)UNCACHED_ADDR(&__inquiry);
+    capacity_data = (void*)UNCACHED_ADDR(&__capacity_data);
+    format_capacity_data = (void*)UNCACHED_ADDR(&__format_capacity_data);
+    sense_data = (void*)UNCACHED_ADDR(&__sense_data);
+    mode_sense_data_6 = (void*)UNCACHED_ADDR(&__mode_sense_data_6);
+    mode_sense_data_10 = (void*)UNCACHED_ADDR(&__mode_sense_data_10);
+    lun_data = (void*)UNCACHED_ADDR(&__lun_data);
+    max_lun = (void*)UNCACHED_ADDR(&__max_lun);
+    csw = (void*)UNCACHED_ADDR(&__csw);
     logf("usb_storage_init done");
 }
 
 /* called by usb_core_transfer_complete() */
-void usb_storage_transfer_complete(int endpoint)
+void usb_storage_transfer_complete(bool in,int status,int length)
 {
     struct command_block_wrapper* cbw = (void*)transfer_buffer;
 
-    switch (endpoint) {
-        case EP_RX:
-            //logf("ums: %d bytes in", length);
-            if(state == RECEIVING)
-            {
-                int receive_count=usb_drv_get_last_transfer_length();
-                logf("scsi write %d %d", current_cmd.sector, current_cmd.count);
-                if(usb_drv_get_last_transfer_status()==0)
-                {
-                    if((unsigned int)receive_count!=(SECTOR_SIZE*current_cmd.count))
-                    {
-                        logf("%d >= %d",SECTOR_SIZE*current_cmd.count,receive_count);
-                    }
-                    ata_write_sectors(IF_MV2(current_cmd.lun,)
-                                      current_cmd.sector, current_cmd.count,
-                                      transfer_buffer);
-                    send_csw(current_cmd.tag, SCSI_STATUS_GOOD);
+    //logf("transfer result %X %d", status, length);
+    switch(state) {
+        case RECEIVING_BLOCKS:
+            if(in==true) {
+                logf("IN received in RECEIVING");
+            }
+            logf("scsi write %d %d", current_cmd.sector, current_cmd.count);
+            if(status==0) {
+                if((unsigned int)length!=(SECTOR_SIZE*current_cmd.count)
+                  && (unsigned int)length!=BUFFER_SIZE) {
+                    logf("unexpected length :%d",length);
                 }
-                else
-                {
-                    logf("Transfer failed %X",usb_drv_get_last_transfer_status());
-                    send_csw(current_cmd.tag, SCSI_STATUS_CHECK_CONDITION);
+
+                unsigned int next_sector = current_cmd.sector + (BUFFER_SIZE/SECTOR_SIZE);
+                unsigned int next_count = current_cmd.count - MIN(current_cmd.count,BUFFER_SIZE/SECTOR_SIZE);
+
+                if(next_count!=0) {
+                    /* Ask the host to send more, to the other buffer */
+                    receive_block_data(current_cmd.data[!current_cmd.data_select],
+                                       MIN(BUFFER_SIZE,next_count*SECTOR_SIZE));
                 }
+
+                /* Now write the data that just came in, while the host is sending the next bit */
+                int result = ata_write_sectors(IF_MV2(current_cmd.lun,)
+                                               current_cmd.sector, MIN(BUFFER_SIZE/SECTOR_SIZE,current_cmd.count),
+                                               current_cmd.data[current_cmd.data_select]);
+                if(result != 0) {
+                    send_csw(SCSI_STATUS_CHECK_CONDITION);
+                    cur_sense_data.sense_key=SENSE_MEDIUM_ERROR;
+                    cur_sense_data.asc=ASC_WRITE_ERROR;
+                    break;
+                }
+
+                if(next_count==0) {
+                    send_csw(SCSI_STATUS_GOOD);
+                }
+
+                /* Switch buffers for the next one */
+                current_cmd.data_select=!current_cmd.data_select;
+
+                current_cmd.sector = next_sector;
+                current_cmd.count = next_count;
+
             }
-            else
-            {
-                state = SENDING;
-                handle_scsi(cbw);
+            else {
+                logf("Transfer failed %X",status);
+                send_csw(SCSI_STATUS_CHECK_CONDITION);
+                /* TODO fill in cur_sense_data */
+                cur_sense_data.sense_key=0;
+                cur_sense_data.information=0;
+                cur_sense_data.asc=0;
             }
-            
             break;
-
-        case EP_TX:
-            //logf("ums: out complete");
-            if(state != IDLE)
-            {
-                /* re-prime endpoint. We only need room for commands */
-                state = IDLE;
-                usb_drv_recv(EP_RX, transfer_buffer, 1024);
+        case WAITING_FOR_COMMAND:
+            if(in==true) {
+                logf("IN received in WAITING_FOR_COMMAND");
+            }
+            //logf("command received");
+            handle_scsi(cbw);
+            break;
+        case SENDING_CSW:
+            if(in==false) {
+                logf("OUT received in SENDING_CSW");
+            }
+            //logf("csw sent, now go back to idle");
+            state = WAITING_FOR_COMMAND;
+            usb_drv_recv(EP_MASS_STORAGE, transfer_buffer, 1024);
+            break;
+        case SENDING_RESULT:
+            if(in==false) {
+                logf("OUT received in SENDING");
+            }
+            if(status==0) {
+                //logf("data sent, now send csw");
+                send_csw(SCSI_STATUS_GOOD);
+            }
+            else {
+                logf("Transfer failed %X",status);
+                send_csw(SCSI_STATUS_CHECK_CONDITION);
+                /* TODO fill in cur_sense_data */
+                cur_sense_data.sense_key=0;
+                cur_sense_data.information=0;
+                cur_sense_data.asc=0;
+            }
+            break;
+        case SENDING_BLOCKS:
+            if(in==false) {
+                logf("OUT received in SENDING");
+            }
+            if(status==0) {
+                if(current_cmd.count==0) {
+                    //logf("data sent, now send csw");
+                    send_csw(SCSI_STATUS_GOOD);
+                }
+                else {
+                    send_and_read_next();
+                }
+            }
+            else {
+                logf("Transfer failed %X",status);
+                send_csw(SCSI_STATUS_CHECK_CONDITION);
+                /* TODO fill in cur_sense_data */
+                cur_sense_data.sense_key=0;
+                cur_sense_data.information=0;
+                cur_sense_data.asc=0;
             }
-
             break;
     }
 }
@@ -205,15 +358,24 @@ void usb_storage_transfer_complete(int endpoint)
 /* called by usb_core_control_request() */
 bool usb_storage_control_request(struct usb_ctrlrequest* req)
 {
-    /* note: interrupt context */
-
     bool handled = false;
 
     switch (req->bRequest) {
         case USB_BULK_GET_MAX_LUN: {
-            static char maxlun = NUM_VOLUMES - 1;
+#ifdef ONLY_EXPOSE_CARD_SLOT
+            *max_lun = 0;
+#else
+            *max_lun = NUM_VOLUMES - 1;
+#endif
+#ifdef HAVE_HOTSWAP
+            /* Workaround until we find out how to do removable devices properly */
+            tCardInfo* cinfo = card_get_info(1);
+            if(cinfo->initialized==0) {
+                *max_lun=0;
+            }
+#endif
             logf("ums: getmaxlun");
-            usb_drv_send(EP_CONTROL, UNCACHED_ADDR(&maxlun), 1);
+            usb_drv_send(EP_CONTROL, UNCACHED_ADDR(max_lun), 1);
             usb_drv_recv(EP_CONTROL, NULL, 0); /* ack */
             handled = true;
             break;
@@ -221,8 +383,8 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
 
         case USB_BULK_RESET_REQUEST:
             logf("ums: bulk reset");
-            usb_drv_reset_endpoint(EP_RX, false);
-            usb_drv_reset_endpoint(EP_TX, true);
+            usb_drv_reset_endpoint(EP_MASS_STORAGE, false);
+            usb_drv_reset_endpoint(EP_MASS_STORAGE, true);
             usb_drv_send(EP_CONTROL, NULL, 0);  /* ack */
             handled = true;
             break;
@@ -230,8 +392,8 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
         case USB_REQ_SET_CONFIGURATION:
             logf("ums: set config");
             /* prime rx endpoint. We only need room for commands */
-            state = IDLE;
-            usb_drv_recv(EP_RX, transfer_buffer, 1024);
+            state = WAITING_FOR_COMMAND;
+            usb_drv_recv(EP_MASS_STORAGE, transfer_buffer, 1024);
             handled = true;
             break;
     }
@@ -239,6 +401,32 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
     return handled;
 }
 
+static void send_and_read_next(void)
+{
+    if(current_cmd.last_result!=0) {
+        /* The last read failed. */
+        send_csw(SCSI_STATUS_CHECK_CONDITION);
+        cur_sense_data.sense_key=SENSE_MEDIUM_ERROR;
+        cur_sense_data.asc=ASC_READ_ERROR;
+        return;
+    }
+    send_block_data(current_cmd.data[current_cmd.data_select],
+                    MIN(BUFFER_SIZE,current_cmd.count*SECTOR_SIZE));
+
+    /* Switch buffers for the next one */
+    current_cmd.data_select=!current_cmd.data_select;
+
+    current_cmd.sector+=(BUFFER_SIZE/SECTOR_SIZE);
+    current_cmd.count-=MIN(current_cmd.count,BUFFER_SIZE/SECTOR_SIZE);
+
+    if(current_cmd.count!=0){
+        /* already read the next bit, so we can send it out immediately when the
+         * current transfer completes.  */
+        current_cmd.last_result = ata_read_sectors(IF_MV2(current_cmd.lun,) current_cmd.sector,
+                                  MIN(BUFFER_SIZE/SECTOR_SIZE,current_cmd.count),
+                                  current_cmd.data[current_cmd.data_select]);
+    }
+}
 /****************************************************************************/
 
 static void handle_scsi(struct command_block_wrapper* cbw)
@@ -246,141 +434,196 @@ static void handle_scsi(struct command_block_wrapper* cbw)
     /* USB Mass Storage assumes LBA capability.
        TODO: support 48-bit LBA */
 
-    unsigned int sectors_per_transfer=0;
     unsigned int length = cbw->data_transfer_length;
     unsigned int block_size;
+    unsigned int block_count;
+    bool lun_present=true;
+#ifdef ONLY_EXPOSE_CARD_SLOT
+    unsigned char lun = cbw->lun+1;
+#else
     unsigned char lun = cbw->lun;
+#endif
     unsigned int block_size_mult = 1;
 #ifdef HAVE_HOTSWAP
     tCardInfo* cinfo = card_get_info(lun);
-    block_size = cinfo->blocksize;
-    if(cinfo->initialized==1)
-    {
-       sectors_per_transfer=(sizeof _transfer_buffer/ block_size);
+    if(cinfo->initialized==1) {
+        block_size = cinfo->blocksize;
+        block_count = cinfo->numblocks;
+    }
+    else {
+        lun_present=false;
+        block_size = 0;
+        block_count = 0;
     }
 #else
+    unsigned short* identify = ata_get_identify();
     block_size = SECTOR_SIZE;
-    sectors_per_transfer=(sizeof _transfer_buffer/ block_size);
+    block_count = (identify[61] << 16 | identify[60]);
 #endif
 
 #ifdef MAX_LOG_SECTOR_SIZE
     block_size_mult = disk_sector_multiplier;
 #endif
 
+    current_cmd.tag = cbw->tag;
+    current_cmd.lun = lun;
+
     switch (cbw->command_block[0]) {
         case SCSI_TEST_UNIT_READY:
             logf("scsi test_unit_ready %d",lun);
 #ifdef HAVE_HOTSWAP
             if(cinfo->initialized==1)
-                send_csw(cbw->tag, SCSI_STATUS_GOOD);
-            else
-                send_csw(cbw->tag, SCSI_STATUS_FAIL);
+                send_csw(SCSI_STATUS_GOOD);
+            else {
+                send_csw(SCSI_STATUS_FAIL);
+                cur_sense_data.sense_key=SENSE_NOT_READY;
+                cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
+            }
 #else
-            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+            send_csw(SCSI_STATUS_GOOD);
 #endif
             break;
 
+        case SCSI_REPORT_LUNS: {
+            logf("scsi inquiry %d",lun);
+            int allocation_length=0;
+            allocation_length|=(cbw->command_block[6]<<24);
+            allocation_length|=(cbw->command_block[7]<<16);
+            allocation_length|=(cbw->command_block[8]<<8);
+            allocation_length|=(cbw->command_block[9]);
+            memset(lun_data,0,sizeof(struct report_lun_data));
+#ifdef HAVE_HOTSWAP
+            lun_data->lun_list_length=htobe32(16);
+            lun_data->lun1[1]=1;
+#else
+            lun_data->lun_list_length=htobe32(8);
+#endif
+            lun_data->lun0[1]=0;
+            
+            send_command_result(lun_data, MIN(sizeof(struct report_lun_data), length));
+            break;
+        }
+
         case SCSI_INQUIRY:
             logf("scsi inquiry %d",lun);
             identify2inquiry(lun);
             length = MIN(length, cbw->command_block[4]);
-            usb_drv_send(EP_TX, inquiry, MIN(sizeof _inquiry, length));
-            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+            send_command_result(inquiry, MIN(sizeof(struct inquiry_data), length));
             break;
 
         case SCSI_REQUEST_SENSE: {
-            sense_data->ResponseCode=0x70;
-            sense_data->filemark_eom_ili_sensekey=2;
-            sense_data->Information=2;
+            sense_data->ResponseCode=0x70;/*current error*/
+            sense_data->filemark_eom_ili_sensekey=cur_sense_data.sense_key&0x0f;
+            sense_data->Information=cur_sense_data.information;
             sense_data->AdditionalSenseLength=10;
             sense_data->CommandSpecificInformation=0;
-            sense_data->AdditionalSenseCode=0x3a;
+            sense_data->AdditionalSenseCode=cur_sense_data.asc;
             sense_data->AdditionalSenseCodeQualifier=0;
             sense_data->FieldReplaceableUnitCode=0;
             sense_data->SKSV=0;
             sense_data->SenseKeySpecific=0;
             logf("scsi request_sense %d",lun);
-            usb_drv_send(EP_TX, sense_data,
-                         sizeof(_sense_data));
-            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+            send_command_result(sense_data, sizeof(struct sense_data));
             break;
         }
 
-        case SCSI_MODE_SENSE: {
-            static unsigned char sense_data[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-            logf("scsi mode_sense %d",lun);
-            usb_drv_send(EP_TX, UNCACHED_ADDR(&sense_data),
-                         MIN(sizeof sense_data, length));
-            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+        case SCSI_MODE_SENSE_10: {
+            /*unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;*/
+            unsigned char page_code = cbw->command_block[2] & 0x3f;
+            logf("scsi mode_sense_10 %d %X",lun,page_code);
+            switch(page_code) {
+                case 0x3f:
+                default:
+                    mode_sense_data_10->mode_data_length=0;
+                    mode_sense_data_10->medium_type=0;
+                    mode_sense_data_10->device_specific=0;
+                    mode_sense_data_10->block_descriptor_length=0;
+                    send_command_result(mode_sense_data_10,
+                              MIN(sizeof(struct mode_sense_header_10), length));
+                    break;
+#if 0
+                default:
+                    send_csw(SCSI_STATUS_CHECK_CONDITION);
+                    cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
+                    cur_sense_data.asc=ASC_INVALID_FIELD_IN_CBD;
+                    break;
+#endif
+                }
+            break;
+        }
+        case SCSI_MODE_SENSE_6: {
+            /*unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;*/
+            unsigned char page_code = cbw->command_block[2] & 0x3f;
+            logf("scsi mode_sense_6 %d %X",lun,page_code);
+            switch(page_code) {
+                case 0x3f:
+                default:
+                    /* All supported pages Since we support only one this is easy*/
+                    mode_sense_data_6->mode_data_length=0;
+                    mode_sense_data_6->medium_type=0;
+                    mode_sense_data_6->device_specific=0;
+                    mode_sense_data_6->block_descriptor_length=0;
+                    send_command_result(mode_sense_data_6,
+                              MIN(sizeof(struct mode_sense_header_6), length));
+                    break;
+#if 0
+                default:
+                    send_csw(SCSI_STATUS_CHECK_CONDITION);
+                    cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
+                    cur_sense_data.asc=ASC_INVALID_FIELD_IN_CBD;
+                    break;
+#endif
+            }
             break;
         }
 
         case SCSI_START_STOP_UNIT:
             logf("scsi start_stop unit %d",lun);
-            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+            send_csw(SCSI_STATUS_GOOD);
             break;
 
         case SCSI_ALLOW_MEDIUM_REMOVAL:
             logf("scsi allow_medium_removal %d",lun);
-            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+            /* TODO: use this to show the connect screen ? */
+            send_csw(SCSI_STATUS_GOOD);
             break;
 
         case SCSI_READ_FORMAT_CAPACITY: {
             logf("scsi read_format_capacity %d",lun);
             format_capacity_data->following_length=htobe32(8);
-#ifdef HAVE_HOTSWAP
             /* Careful: "block count" actually means "number of last block" */
-            if(cinfo->initialized==1)
-            {
-                format_capacity_data->block_count = htobe32(cinfo->numblocks - 1);
-                format_capacity_data->block_size = htobe32(cinfo->blocksize);
-            }
-            else
-            {
-                format_capacity_data->block_count = htobe32(0);
-                format_capacity_data->block_size = htobe32(0);
-            }
-#else
-            unsigned short* identify = ata_get_identify();
-            /* Careful: "block count" actually means "number of last block" */
-            format_capacity_data->block_count = htobe32((identify[61] << 16 | identify[60]) / block_size_mult - 1);
-            format_capacity_data->block_size = htobe32(block_size * block_size_mult);
-#endif
+            format_capacity_data->block_count = htobe32(block_count/block_size_mult - 1);
+            format_capacity_data->block_size = htobe32(block_size*block_size_mult);
             format_capacity_data->block_size |= SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA;
 
-            usb_drv_send(EP_TX, format_capacity_data,
-                         MIN(sizeof _format_capacity_data, length));
-            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+            send_command_result(format_capacity_data,
+                      MIN(sizeof(struct format_capacity), length));
+
             break;
         }
 
         case SCSI_READ_CAPACITY: {
             logf("scsi read_capacity %d",lun);
-#ifdef HAVE_HOTSWAP
             /* Careful: "block count" actually means "number of last block" */
-            if(cinfo->initialized==1)
-            {
-                capacity_data->block_count = htobe32(cinfo->numblocks - 1);
-                capacity_data->block_size = htobe32(cinfo->blocksize);
-            }
-            else
-            {
-                capacity_data->block_count = htobe32(0);
-                capacity_data->block_size = htobe32(0);
-            }
-#else
-            unsigned short* identify = ata_get_identify();
-            /* Careful : "block count" actually means the number of the last block */
-            capacity_data->block_count = htobe32((identify[61] << 16 | identify[60]) / block_size_mult - 1);
-            capacity_data->block_size = htobe32(block_size * block_size_mult);
-#endif
-            usb_drv_send(EP_TX, capacity_data,
-                         MIN(sizeof _capacity_data, length));
-            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+            capacity_data->block_count = htobe32(block_count/block_size_mult - 1);
+            capacity_data->block_size = htobe32(block_size*block_size_mult);
+
+            send_command_result(capacity_data, MIN(sizeof(struct capacity), length));
             break;
         }
 
         case SCSI_READ_10:
+            logf("scsi read10 %d",lun);
+            if(! lun_present) {
+                send_csw(SCSI_STATUS_CHECK_CONDITION);
+                cur_sense_data.sense_key=SENSE_NOT_READY;
+                cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
+                break;
+            }
+            trigger_cpu_boost();
+            current_cmd.data[0] = transfer_buffer;
+            current_cmd.data[1] = &transfer_buffer[BUFFER_SIZE];
+            current_cmd.data_select=0;
             current_cmd.sector = block_size_mult *
                (cbw->command_block[2] << 24 |
                 cbw->command_block[3] << 16 |
@@ -389,32 +632,35 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             current_cmd.count = block_size_mult *
                (cbw->command_block[7] << 16 |
                 cbw->command_block[8]);
-            current_cmd.tag = cbw->tag;
-            current_cmd.lun = cbw->lun;
 
-            //logf("scsi read %d %d", current_cmd.sector, current_cmd.count);
+            logf("scsi read %d %d", current_cmd.sector, current_cmd.count);
 
-            //logf("Asked for %d sectors",current_cmd.count);
-            if(current_cmd.count > sectors_per_transfer)
-            {
-                current_cmd.count = sectors_per_transfer;
-            }
-            //logf("Sending %d sectors",current_cmd.count);
-
-            if(current_cmd.count*block_size > sizeof(_transfer_buffer)) {
-                send_csw(current_cmd.tag, SCSI_STATUS_CHECK_CONDITION);
+            if((current_cmd.sector + current_cmd.count) * block_size_mult > block_count) {
+                send_csw(SCSI_STATUS_CHECK_CONDITION);
+                cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
+                cur_sense_data.asc=ASC_LBA_OUT_OF_RANGE;
             }
             else {
-                ata_read_sectors(IF_MV2(lun,) current_cmd.sector,
-                                 current_cmd.count, transfer_buffer);
-                usb_drv_send(EP_TX, transfer_buffer,
-                             current_cmd.count*block_size);
-                send_csw(current_cmd.tag, SCSI_STATUS_GOOD);
+                /* TODO: any way to do this nonblocking ? */
+                current_cmd.last_result = ata_read_sectors(IF_MV2(current_cmd.lun,) current_cmd.sector,
+                                              MIN(BUFFER_SIZE/SECTOR_SIZE,current_cmd.count),
+                                              current_cmd.data[current_cmd.data_select]);
+                send_and_read_next();
             }
             break;
 
         case SCSI_WRITE_10:
-            //logf("scsi write10");
+            logf("scsi write10 %d",lun);
+            if(! lun_present) {
+                send_csw(SCSI_STATUS_CHECK_CONDITION);
+                cur_sense_data.sense_key=SENSE_NOT_READY;
+                cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
+                break;
+            }
+            trigger_cpu_boost();
+            current_cmd.data[0] = transfer_buffer;
+            current_cmd.data[1] = &transfer_buffer[BUFFER_SIZE];
+            current_cmd.data_select=0;
             current_cmd.sector = block_size_mult *
                (cbw->command_block[2] << 24 |
                 cbw->command_block[3] << 16 |
@@ -423,53 +669,74 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             current_cmd.count = block_size_mult *
                (cbw->command_block[7] << 16 |
                 cbw->command_block[8]);
-            current_cmd.tag = cbw->tag;
-            current_cmd.lun = cbw->lun;
             /* expect data */
-            if(current_cmd.count*block_size > sizeof(_transfer_buffer)) {
-                send_csw(current_cmd.tag, SCSI_STATUS_CHECK_CONDITION);
+            if((current_cmd.sector + current_cmd.count) * block_size_mult > block_count) {
+                send_csw(SCSI_STATUS_CHECK_CONDITION);
+                cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
+                cur_sense_data.asc=ASC_LBA_OUT_OF_RANGE;
             }
             else {
-                usb_drv_recv(EP_RX, transfer_buffer,
-                             current_cmd.count*block_size);
-                state = RECEIVING;
+                receive_block_data(current_cmd.data[0],
+                                   MIN(BUFFER_SIZE,current_cmd.count*SECTOR_SIZE));
             }
 
             break;
 
         default:
             logf("scsi unknown cmd %x",cbw->command_block[0x0]);
-            usb_drv_stall(EP_TX, true);
-            send_csw(current_cmd.tag, SCSI_STATUS_GOOD);
+            usb_drv_stall(EP_MASS_STORAGE, true,true);
+            send_csw(SCSI_STATUS_GOOD);
             break;
     }
 }
 
-static void send_csw(unsigned int tag, int status)
+static void send_block_data(void *data,int size)
+{
+    usb_drv_send_nonblocking(EP_MASS_STORAGE, data,size);
+    state = SENDING_BLOCKS;
+}
+
+static void send_command_result(void *data,int size)
+{
+    usb_drv_send_nonblocking(EP_MASS_STORAGE, data,size);
+    state = SENDING_RESULT;
+}
+
+static void receive_block_data(void *data,int size)
 {
-    static struct command_status_wrapper _csw;
-    struct command_status_wrapper* csw = UNCACHED_ADDR(&_csw);
+    usb_drv_recv(EP_MASS_STORAGE, data, size);
+    state = RECEIVING_BLOCKS;
+}
+
+static void send_csw(int status)
+{
+    cancel_cpu_boost();
     csw->signature = CSW_SIGNATURE;
-    csw->tag = tag;
+    csw->tag = current_cmd.tag;
     csw->data_residue = 0;
     csw->status = status;
 
-    //logf("csw %x %x", csw->tag, csw->signature);
-    usb_drv_send(EP_TX, csw, sizeof _csw);
+    usb_drv_send_nonblocking(EP_MASS_STORAGE, csw, sizeof(struct command_status_wrapper));
+    state = SENDING_CSW;
+    logf("CSW: %X",status);
+
+    if(status == SCSI_STATUS_GOOD) {
+        cur_sense_data.sense_key=0;
+        cur_sense_data.information=0;
+        cur_sense_data.asc=0;
+    }
 }
 
 /* convert ATA IDENTIFY to SCSI INQUIRY */
 static void identify2inquiry(int lun)
 {
 #ifdef HAVE_FLASH_STORAGE
-    if(lun==0)
-    {
+    if(lun==0) {
         memcpy(&inquiry->VendorId,"Rockbox ",8);
         memcpy(&inquiry->ProductId,"Internal Storage",16);
         memcpy(&inquiry->ProductRevisionLevel,"0.00",4);
     }
-    else
-    {
+    else {
         memcpy(&inquiry->VendorId,"Rockbox ",8);
         memcpy(&inquiry->ProductId,"SD Card Slot    ",16);
         memcpy(&inquiry->ProductRevisionLevel,"0.00",4);
@@ -480,7 +747,7 @@ static void identify2inquiry(int lun)
     unsigned short* src;
     unsigned short* identify = ata_get_identify();
     (void)lun;
-    memset(inquiry, 0, sizeof _inquiry);
+    memset(inquiry, 0, sizeof(struct inquiry_data));
     
     if (identify[82] & 4)
         inquiry->DeviceTypeModifier = DEVICE_REMOVABLE;
@@ -501,8 +768,8 @@ static void identify2inquiry(int lun)
 
     inquiry->DeviceType = DIRECT_ACCESS_DEVICE;
     inquiry->AdditionalLength = 0x1f;
-    inquiry->Versions = 3; /* ANSI SCSI level 2 */
-    inquiry->Format   = 3; /* ANSI SCSI level 2 INQUIRY format */
+    inquiry->Versions = 4; /* SPC-2 */
+    inquiry->Format   = 2; /* SPC-2/3 inquiry format */
 
 #ifdef HAVE_HOTSWAP
     inquiry->DeviceTypeModifier = DEVICE_REMOVABLE;
@@ -510,3 +777,4 @@ static void identify2inquiry(int lun)
 
 }
 
+#endif /* USB_STORAGE */
diff --git a/firmware/usbstack/usb_storage.h b/firmware/usbstack/usb_storage.h
index 9067c92..23903a8 100644
--- a/firmware/usbstack/usb_storage.h
+++ b/firmware/usbstack/usb_storage.h
@@ -22,8 +22,7 @@
 #include "usb_ch9.h"
 
 void usb_storage_init(void);
-void usb_storage_transfer(void* data);
-void usb_storage_transfer_complete(int endpoint);
+void usb_storage_transfer_complete(bool in,int state,int length);
 bool usb_storage_control_request(struct usb_ctrlrequest* req);
 
 #endif