reorganise the usb stack to provide a clean separation between core and class drivers

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

6 files changed, 760 insertions, 545 deletions

diff --git a/firmware/usbstack/usb_class_driver.h b/firmware/usbstack/usb_class_driver.h
new file mode 100644
index 0000000000..631d5a3bc1
--- /dev/null
+++ b/firmware/usbstack/usb_class_driver.h
@@ -0,0 +1,59 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id:  $
+ *
+ * Copyright (C) 2008 Frank Gevaerts
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+#ifndef _USB_CLASS_DRIVER_H_
+#define _USB_CLASS_DRIVER_H_
+
+/* Common api, implemented by all class drivers */
+
+struct usb_class_driver {
+    bool enabled;
+    bool needs_exclusive_ata;
+    int usb_endpoint;
+    int usb_interface;
+
+    /* Asks the driver to put the interface descriptor and all other
+       needed descriptor for this driver at dest, for the given settings.
+       Returns the number of bytes taken by these descriptors. */
+    int (*get_config_descriptor)(unsigned char *dest,
+              int max_packet_size, int interface_number, int endpoint);
+
+    /* Tells the driver that a usb connection has been set up and is now
+       ready to use. */
+    void (*init_connection)(int interface,int endpoint);
+
+    /* Initialises the driver. This can be called multiple times,
+       and should not perform any action that can disturb other threads
+       (like getting the audio buffer) */
+    void (*init)(void);
+
+    /* Tells the driver that the usb connection is no longer active */
+    void (*disconnect)(void);
+
+    /* Tells the driver that a usb transfer has been completed. Note that "in"
+       is relative to the host */
+    void (*transfer_complete)(bool in, int status, int length);
+
+    /* Tells the driver that a control request has come in. If the driver is
+       able to handle it, it should ack the request, and return true. Otherwise
+       it should return false. */
+    bool (*control_request)(struct usb_ctrlrequest* req);
+};
+
+#endif
diff --git a/firmware/usbstack/usb_core.c b/firmware/usbstack/usb_core.c
index 96a5994417..ff54e4269f 100644
--- a/firmware/usbstack/usb_core.c
+++ b/firmware/usbstack/usb_core.c
@@ -27,6 +27,7 @@
 #include "usb_ch9.h"
 #include "usb_drv.h"
 #include "usb_core.h"
+#include "usb_class_driver.h"
 
 #if defined(USB_STORAGE)
 #include "usb_storage.h"
@@ -36,7 +37,7 @@
 #include "usb_serial.h"
 #endif
 
-/* TODO: Move this target-specific stuff somewhere else (serial number reading) */
+/* TODO: Move target-specific stuff somewhere else (serial number reading) */
 
 #ifdef HAVE_AS3514
 #include "i2c-pp.h"
@@ -54,7 +55,9 @@
 #define USB_SC_SCSI      0x06            /* Transparent */
 #define USB_PROT_BULK    0x50            /* bulk only */
 
-static const struct usb_device_descriptor __attribute__((aligned(2))) device_descriptor= {
+static const struct usb_device_descriptor __attribute__((aligned(2)))
+                                          device_descriptor=
+{
     .bLength            = sizeof(struct usb_device_descriptor),
     .bDescriptorType    = USB_DT_DEVICE,
 #ifdef USE_HIGH_SPEED
@@ -75,7 +78,8 @@ static const struct usb_device_descriptor __attribute__((aligned(2))) device_des
     .bNumConfigurations = 1
 } ;
 
-struct usb_config_descriptor __attribute__((aligned(2))) config_descriptor = 
+static struct usb_config_descriptor __attribute__((aligned(2)))
+                                    config_descriptor = 
 {
     .bLength             = sizeof(struct usb_config_descriptor),
     .bDescriptorType     = USB_DT_CONFIG,
@@ -89,7 +93,8 @@ struct usb_config_descriptor __attribute__((aligned(2))) config_descriptor =
 
 #ifdef USB_CHARGING_ONLY
 /* dummy interface for charging-only */
-struct usb_interface_descriptor __attribute__((aligned(2))) charging_interface_descriptor =
+static struct usb_interface_descriptor __attribute__((aligned(2)))
+                                       charging_interface_descriptor =
 {
     .bLength            = sizeof(struct usb_interface_descriptor),
     .bDescriptorType    = USB_DT_INTERFACE,
@@ -103,77 +108,8 @@ struct usb_interface_descriptor __attribute__((aligned(2))) charging_interface_d
 };
 #endif
 
-#ifdef USB_STORAGE
-/* storage interface */
-struct usb_interface_descriptor __attribute__((aligned(2))) mass_storage_interface_descriptor = 
-{
-    .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
-};
-
-struct usb_endpoint_descriptor __attribute__((aligned(2))) mass_storage_ep_in_descriptor =
-{
-    .bLength          = sizeof(struct usb_endpoint_descriptor),
-    .bDescriptorType  = USB_DT_ENDPOINT,
-    .bEndpointAddress = EP_MASS_STORAGE | USB_DIR_IN,
-    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-    .wMaxPacketSize   = 16,
-    .bInterval        = 0
-};
-struct usb_endpoint_descriptor __attribute__((aligned(2))) mass_storage_ep_out_descriptor =
-{
-    .bLength          = sizeof(struct usb_endpoint_descriptor),
-    .bDescriptorType  = USB_DT_ENDPOINT,
-    .bEndpointAddress = EP_MASS_STORAGE | USB_DIR_OUT,
-    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-    .wMaxPacketSize   = 16,
-    .bInterval        = 0
-};
-#endif
-
-#ifdef USB_SERIAL
-/* serial interface */
-struct usb_interface_descriptor __attribute__((aligned(2))) serial_interface_descriptor =
-{
-    .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
-};
-
-struct usb_endpoint_descriptor __attribute__((aligned(2))) serial_ep_in_descriptor =
-{
-    .bLength          = sizeof(struct usb_endpoint_descriptor),
-    .bDescriptorType  = USB_DT_ENDPOINT,
-    .bEndpointAddress = EP_SERIAL | USB_DIR_IN,
-    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-    .wMaxPacketSize   = 16,
-    .bInterval        = 0
-};
-struct usb_endpoint_descriptor __attribute__((aligned(2))) serial_ep_out_descriptor =
-{
-    .bLength          = sizeof(struct usb_endpoint_descriptor),
-    .bDescriptorType  = USB_DT_ENDPOINT,
-    .bEndpointAddress = EP_SERIAL | USB_DIR_OUT,
-    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-    .wMaxPacketSize   = 16,
-    .bInterval        = 0
-};
-#endif
-
-static const struct usb_qualifier_descriptor __attribute__((aligned(2))) qualifier_descriptor =
+static const struct usb_qualifier_descriptor __attribute__((aligned(2)))
+                                             qualifier_descriptor =
 {
     .bLength            = sizeof(struct usb_qualifier_descriptor),
     .bDescriptorType    = USB_DT_DEVICE_QUALIFIER,
@@ -185,21 +121,26 @@ static const struct usb_qualifier_descriptor __attribute__((aligned(2))) qualifi
     .bNumConfigurations = 1
 };
 
-static struct usb_string_descriptor __attribute__((aligned(2))) usb_string_iManufacturer =
+static struct usb_string_descriptor __attribute__((aligned(2)))
+                                    usb_string_iManufacturer =
 {
     24,
     USB_DT_STRING,
     {'R','o','c','k','b','o','x','.','o','r','g'}
 };
 
-static struct usb_string_descriptor __attribute__((aligned(2))) usb_string_iProduct =
+static struct usb_string_descriptor __attribute__((aligned(2)))
+                                    usb_string_iProduct =
 {
     42,
     USB_DT_STRING,
-    {'R','o','c','k','b','o','x',' ','m','e','d','i','a',' ','p','l','a','y','e','r'}
+    {'R','o','c','k','b','o','x',' ',
+     'm','e','d','i','a',' ',
+     'p','l','a','y','e','r'}
 };
 
-static struct usb_string_descriptor __attribute__((aligned(2))) usb_string_iSerial =
+static struct usb_string_descriptor __attribute__((aligned(2)))
+                                    usb_string_iSerial =
 {
     84,
     USB_DT_STRING,
@@ -211,14 +152,16 @@ static struct usb_string_descriptor __attribute__((aligned(2))) usb_string_iSeri
 /* Generic for all targets */
 
 /* this is stringid #0: languages supported */
-static struct usb_string_descriptor __attribute__((aligned(2))) lang_descriptor =
+static struct usb_string_descriptor __attribute__((aligned(2)))
+                                    lang_descriptor =
 {
     4,
     USB_DT_STRING,
     {0x0409} /* LANGID US English */
 };
 
-static struct usb_string_descriptor __attribute__((aligned(2))) usb_string_charging_only =
+static struct usb_string_descriptor __attribute__((aligned(2)))
+                                    usb_string_charging_only =
 {
     28,
     USB_DT_STRING,
@@ -238,14 +181,56 @@ static int usb_address = 0;
 static bool initialized = false;
 static enum { DEFAULT, ADDRESS, CONFIGURED } usb_state;
 
+static int usb_core_num_interfaces;
+
+int usb_charging_get_config_descriptor(unsigned char *dest,int max_packet_size,
+                                            int interface_number,int endpoint);
+
+struct usb_class_driver drivers[] =
+{
 #ifdef USB_STORAGE
-static bool usb_core_storage_enabled = false;
+    [USB_DRIVER_MASS_STORAGE] = {
+        .enabled = false,
+        .needs_exclusive_ata = true,
+        .usb_endpoint = 0,
+        .usb_interface = 0,
+        .get_config_descriptor = usb_storage_get_config_descriptor,
+        .init_connection = usb_storage_init_connection,
+        .init = usb_storage_init,
+        .disconnect = NULL,
+        .transfer_complete = usb_storage_transfer_complete,
+        .control_request = usb_storage_control_request
+    },
+#endif
+#ifdef USB_SERIAL
+    [USB_DRIVER_SERIAL] = {
+        .enabled = false,
+        .needs_exclusive_ata = false,
+        .usb_endpoint = 0,
+        .usb_interface = 0,
+        .get_config_descriptor = usb_serial_get_config_descriptor,
+        .init_connection = usb_serial_init_connection,
+        .init = usb_serial_init,
+        .disconnect = usb_serial_disconnect,
+        .transfer_complete = usb_serial_transfer_complete,
+        .control_request = usb_serial_control_request
+    },
 #endif
-/* Next one is non-static, to enable setting it from the debug menu */
-bool usb_core_serial_enabled = false;
 #ifdef USB_CHARGING_ONLY
-static bool usb_core_charging_enabled = false;
+    [USB_DRIVER_CHARGING_ONLY] = {
+        .enabled = false,
+        .needs_exclusive_ata = false,
+        .usb_endpoint = 0,
+        .usb_interface = 0,
+        .get_config_descriptor = usb_charging_get_config_descriptor,
+        .init_connection = NULL,
+        .init = NULL,
+        .disconnect = NULL,
+        .transfer_complete = NULL,
+        .control_request = NULL 
+    },
 #endif
+};
 
 static void usb_core_control_request_handler(struct usb_ctrlrequest* req);
 static int ack_control(struct usb_ctrlrequest* req);
@@ -272,11 +257,9 @@ static void set_serial_descriptor(void)
     uint32_t x;
     int i,j;
 
-    for (i = 0; i < 2; i++)
-    {
+    for (i = 0; i < 2; i++) {
        x = serial[i];
-       for (j=0;j<8;j++)
-       {
+       for (j=0;j<8;j++) {
           *p-- = hex[x & 0xf];
           x >>= 4;
        }
@@ -292,8 +275,7 @@ static void set_serial_descriptor(void)
     int i;
 
     i2c_readbytes(AS3514_I2C_ADDR, 0x30, 0x10, serial);
-    for (i = 0; i < 16; i++)
-    {
+    for (i = 0; i < 16; i++) {
         *p++ = hex[(serial[i] >> 4) & 0xF];
         *p++ = hex[(serial[i] >> 0) & 0xF];
     }
@@ -309,8 +291,7 @@ static void set_serial_descriptor(void)
     unsigned short x;
     int i;
 
-    for (i = 10; i < 20; i++)
-    {
+    for (i = 10; i < 20; i++) {
        x = identify[i];
        *p++ = hex[(x >> 12) & 0xF];
        *p++ = hex[(x >> 8) & 0xF];
@@ -323,6 +304,7 @@ static void set_serial_descriptor(void)
 
 void usb_core_init(void)
 {
+    int i;
     if (initialized)
         return;
 
@@ -333,27 +315,34 @@ void usb_core_init(void)
     /* class driver init functions should be safe to call even if the driver
      * won't be used. This simplifies other logic (i.e. we don't need to know
      * yet which drivers will be enabled */
-#ifdef USB_STORAGE
-    if(usb_core_storage_enabled)
-        usb_storage_init();
-#endif
-
-#ifdef USB_SERIAL
-    if(usb_core_serial_enabled)
-        usb_serial_init();
-#endif
+    for(i=0;i<USB_NUM_DRIVERS;i++) {
+        if(drivers[i].enabled && drivers[i].init != NULL)
+            drivers[i].init();
+    }
 
     initialized = true;
     usb_state = DEFAULT;
     logf("usb_core_init() finished");
 }
 
+int usb_charging_get_config_descriptor(unsigned char *dest,int max_packet_size,
+                                            int interface_number,int endpoint)
+{
+    (void) max_packet_size;
+    (void) endpoint;
+    charging_interface_descriptor.bInterfaceNumber=interface_number;
+    memcpy(dest,&charging_interface_descriptor,
+           sizeof(struct usb_interface_descriptor));
+    return sizeof(struct usb_interface_descriptor);
+}
+
 void usb_core_exit(void)
 {
-#ifdef USB_SERIAL
-    if(usb_core_serial_enabled)
-        usb_serial_exit();
-#endif
+    int i;
+    for(i=0;i<USB_NUM_DRIVERS;i++) {
+        if(drivers[i].enabled && drivers[i].disconnect != NULL)
+            drivers[i].disconnect ();
+    }
 
     if (initialized) {
         usb_drv_exit();
@@ -362,321 +351,332 @@ void usb_core_exit(void)
     logf("usb_core_exit() finished");
 }
 
-void usb_core_handle_transfer_completion(struct usb_transfer_completion_event_data* event)
+void usb_core_handle_transfer_completion(
+              struct usb_transfer_completion_event_data* event)
 {
+    int i;
     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
-        case EP_MASS_STORAGE:
-            usb_storage_transfer_complete(event->in,event->status,event->length);
+            usb_core_control_request_handler(
+                                    (struct usb_ctrlrequest*)event->data);
             break;
-#endif
-#ifdef USB_SERIAL
-        case EP_SERIAL:
-            usb_serial_transfer_complete(event->in,event->status,event->length);
-            break;
-#endif
-#ifdef USB_CHARGING_ONLY
-        case EP_CHARGING_ONLY:
+        default:
+            for(i=0;i<USB_NUM_DRIVERS;i++) {
+                if(drivers[i].enabled &&
+                   drivers[i].usb_endpoint == event->endpoint &&
+                   drivers[i].transfer_complete != NULL)
+                {
+                    drivers[i].transfer_complete(event->in,
+                                              event->status,event->length);
+                    break;
+                }
+            }
             break;
-#endif
     }
 }
 
-void usb_core_enable_protocol(int driver,bool enabled)
+void usb_core_enable_driver(int driver,bool enabled)
 {
-    switch(driver) {
-#ifdef USB_STORAGE
-	case USB_DRIVER_MASS_STORAGE:
-            usb_core_storage_enabled = enabled;
-            break;
-#endif
-#ifdef USB_SERIAL
-	case USB_DRIVER_SERIAL:
-            usb_core_serial_enabled = enabled;
-            break;
-#endif
-#ifdef USB_CHARGING_ONLY
-	case USB_DRIVER_CHARGING_ONLY:
-            usb_core_charging_enabled = enabled;
-            break;
-#endif
+    drivers[driver].enabled = enabled;
+}
+
+bool usb_core_driver_enabled(int driver)
+{
+    return drivers[driver].enabled;
+}
+
+static void usb_core_set_serial_function_id(void)
+{
+    int id = 0;
+    int i;
+    for(i=0;i<USB_NUM_DRIVERS;i++) {
+        if(drivers[i].enabled)
+            id |= 1<<i;
     }
+    usb_string_iSerial.wString[0] = hex[id];
 }
 
-static void usb_core_control_request_handler(struct usb_ctrlrequest* req)
+static void allocate_interfaces_and_endpoints(void)
 {
-    if(usb_state == DEFAULT) {
-        set_serial_descriptor();
-        
-        int serial_function_id = 0;
-#ifdef USB_STORAGE
-        if(usb_core_storage_enabled) {
-            usb_request_exclusive_ata();
-            serial_function_id |= 1;
+    int i;
+    int interface=0;
+    int endpoint=1;
+    for(i=0;i<USB_NUM_DRIVERS;i++) {
+        if(drivers[i].enabled) {
+            drivers[i].usb_endpoint = endpoint++;
+            drivers[i].usb_interface = interface++;
+        }
+        if(endpoint>NUM_ENDPOINTS) {
+            drivers[i].enabled = false;
         }
-#endif
-#ifdef USB_SERIAL
-        if(usb_core_serial_enabled)
-            serial_function_id |= 2;
-#endif
-        usb_string_iSerial.wString[0] = hex[serial_function_id];
     }
+    usb_core_num_interfaces = interface;
+}
 
-    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
-                if(usb_core_storage_enabled)
-                    usb_storage_control_request(req);
-#endif
+static void usb_core_control_request_handler(struct usb_ctrlrequest* req)
+{
+    int i;
+    if(usb_state == DEFAULT) {
+        set_serial_descriptor();
+        usb_core_set_serial_function_id();
 
-#ifdef USB_SERIAL
-                if(usb_core_serial_enabled)
-                    usb_serial_control_request(req);
-#endif
-            }
-            else {
-                usb_state = ADDRESS;
-            }
-            ack_control(req);
-            break;
+        allocate_interfaces_and_endpoints();
 
-        case USB_REQ_GET_CONFIGURATION: {
-            logf("usb_core: GET_CONFIG");
-            if (usb_state == ADDRESS)
-                response_data[0] = 0;
-            else
-                response_data[0] = 1;
-            if(usb_drv_send(EP_CONTROL, response_data, 1)!= 0)
+        for(i=0;i<USB_NUM_DRIVERS;i++) {
+            if(drivers[i].enabled &&
+               drivers[i].needs_exclusive_ata) {
+                usb_request_exclusive_ata();
                 break;
-            ack_control(req);
-            break;
+            }
         }
+    }
 
-        case USB_REQ_SET_INTERFACE:
-            logf("usb_core: SET_INTERFACE");
-            ack_control(req);
-            break;
+    switch(req->bRequestType & 0x1f) {
+        case 0: /* Device */
+            switch (req->bRequest) {
+                case USB_REQ_GET_CONFIGURATION: {
+                    logf("usb_core: GET_CONFIG");
+                    if (usb_state == ADDRESS)
+                        response_data[0] = 0;
+                    else
+                        response_data[0] = 1;
+                    if(usb_drv_send(EP_CONTROL, response_data, 1)!= 0)
+                        break;
+                    ack_control(req);
+                    break;
+                case USB_REQ_SET_CONFIGURATION:
+                    logf("usb_core: SET_CONFIG");
+                    usb_drv_cancel_all_transfers();
+                    if (req->wValue) {
+                        usb_state = CONFIGURED;
+                        for(i=0;i<USB_NUM_DRIVERS;i++) {
+                            if(drivers[i].enabled &&
+                               drivers[i].init_connection!=NULL)
+                            {
+                                drivers[i].init_connection(
+                                            drivers[i].usb_interface,
+                                            drivers[i].usb_endpoint);
+                            }
+                        }
+                    }
+                    else {
+                        usb_state = ADDRESS;
+                    }
+                    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_DESCRIPTOR: {
+                    int index = req->wValue & 0xff;
+                    int length = req->wLength;
+                    int size;
+                    const void* ptr = NULL;
+                    logf("usb_core: GET_DESC %d", req->wValue >> 8);
+
+                    switch (req->wValue >> 8) { /* type */
+                        case USB_DT_DEVICE:
+                            ptr = &device_descriptor;
+                            size = sizeof(struct usb_device_descriptor);
+                            break;
+
+                        case USB_DT_OTHER_SPEED_CONFIG:
+                        case USB_DT_CONFIG: {
+                            int max_packet_size;
+
+                            if(req->wValue >> 8 == USB_DT_CONFIG) {
+                                if(usb_drv_port_speed())
+                                    max_packet_size=512;
+                                else
+                                    max_packet_size=64;
+                                config_descriptor.bDescriptorType=USB_DT_CONFIG;
+                            }
+                            else {
+                                if(usb_drv_port_speed())
+                                    max_packet_size=64;
+                                else
+                                    max_packet_size=512;
+                                config_descriptor.bDescriptorType =
+                                                 USB_DT_OTHER_SPEED_CONFIG;
+                            }
+                            size = sizeof(struct usb_config_descriptor);
+
+                            for(i=0;i<USB_NUM_DRIVERS;i++) {
+                                if(drivers[i].enabled &&
+                                   drivers[i].get_config_descriptor)
+                                {
+                                    size+=drivers[i].get_config_descriptor(
+                                             &response_data[size],
+                                             max_packet_size,
+                                             drivers[i].usb_interface,
+                                             drivers[i].usb_endpoint);
+                                }
+                            }
+                            config_descriptor.bNumInterfaces =
+                                                     usb_core_num_interfaces;
+                            config_descriptor.wTotalLength = size;
+                            memcpy(&response_data[0],&config_descriptor,
+                                   sizeof(struct usb_config_descriptor));
+
+                            ptr = response_data;
+                            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 & 0xf, false,(req->wIndex & 0x80) !=0);
-            else
-                usb_drv_stall(req->wIndex & 0xf, false,(req->wIndex & 0x80) !=0);
-            ack_control(req);
-            break;
+                        case USB_DT_STRING:
+                            logf("STRING %d",index);
+                            if ((unsigned)index < (sizeof(usb_strings)/
+                                         sizeof(struct usb_string_descriptor*)))
+                            {
+                                size = usb_strings[index]->bLength;
+                                memcpy(&response_data[0],usb_strings[index],
+                                       size);
+                                ptr = response_data;
+                            }
+                            else {
+                                logf("bad string id %d", index);
+                                usb_drv_stall(EP_CONTROL, true,true);
+                            }
+                            break;
+
+                        case USB_DT_DEVICE_QUALIFIER:
+                            ptr = &qualifier_descriptor;
+                            size = sizeof (struct usb_qualifier_descriptor);
+                            break;
+
+                        default:
+                            logf("bad desc %d", req->wValue >> 8);
+                            usb_drv_stall(EP_CONTROL, true,true);
+                            break;
+                    }
 
-        case USB_REQ_SET_FEATURE:
-            logf("usb_core: SET_FEATURE");
-            switch(req->bRequestType & 0x0f){
-                case 0: /* Device */
+                    if (ptr) {
+                        unsigned char *uncached = (void*)UNCACHED_ADDR(ptr);
+                        length = MIN(size, length);
+                        if(usb_drv_send(EP_CONTROL, uncached, length)!=0)
+                            break;
+                    }
+                    ack_control(req);
+                    break;
+                } /* USB_REQ_GET_DESCRIPTOR */
+                case USB_REQ_CLEAR_FEATURE:
+                    break;
+                case USB_REQ_SET_FEATURE:
                     if(req->wValue == 2) { /* TEST_MODE */
                         int mode=req->wIndex>>8;
                         ack_control(req);
                         usb_drv_set_test_mode(mode);
                     }
                     break;
-                case 2: /* Endpoint */
-                    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);
+                case USB_REQ_GET_STATUS:
+                    response_data[0]= 0;
+                    response_data[1]= 0;
+                    if(usb_drv_send(EP_CONTROL, response_data, 2)!=0)
+                        break;
                     ack_control(req);
                     break;
                 default:
                     break;
             }
             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: {
-            response_data[0]= 0;
-            response_data[1]= 0;
-            logf("usb_core: GET_STATUS"); 
-            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;
-        }
-
-        case USB_REQ_GET_DESCRIPTOR: {
-            int index = req->wValue & 0xff;
-            int length = req->wLength;
-            int size;
-            const void* ptr = NULL;
-            logf("usb_core: GET_DESC %d", req->wValue >> 8);
-
-            switch (req->wValue >> 8) { /* type */
-                case USB_DT_DEVICE:
-                    ptr = &device_descriptor;
-                    size = sizeof(struct usb_device_descriptor);
+        case 1: /* Interface */
+            switch (req->bRequest) {
+                case USB_REQ_SET_INTERFACE:
+                    logf("usb_core: SET_INTERFACE");
+                    ack_control(req);
                     break;
 
-                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_descriptor.bDescriptorType=USB_DT_CONFIG;
-                    }
-                    else {
-                        if(usb_drv_port_speed()) {
-                            max_packet_size=64;
-                        }
-                        else {
-                            max_packet_size=512;
-                        }
-                        config_descriptor.bDescriptorType=USB_DT_OTHER_SPEED_CONFIG;
-                    }
-                    size = sizeof(struct usb_config_descriptor);
-
-#ifdef USB_STORAGE
-                    if(usb_core_storage_enabled){
-                        mass_storage_ep_in_descriptor.wMaxPacketSize=max_packet_size;
-                        mass_storage_ep_out_descriptor.wMaxPacketSize=max_packet_size;
-                        mass_storage_interface_descriptor.bInterfaceNumber=interface_number;
-                        interface_number++;
-
-                        memcpy(&response_data[size],&mass_storage_interface_descriptor,sizeof(struct usb_interface_descriptor));
-                        size += sizeof(struct usb_interface_descriptor);
-                        memcpy(&response_data[size],&mass_storage_ep_in_descriptor,sizeof(struct usb_endpoint_descriptor));
-                        size += sizeof(struct usb_endpoint_descriptor);
-                        memcpy(&response_data[size],&mass_storage_ep_out_descriptor,sizeof(struct usb_endpoint_descriptor));
-                        size += sizeof(struct usb_endpoint_descriptor);
-                    }
-#endif
-#ifdef USB_SERIAL
-                    if(usb_core_serial_enabled){
-                        serial_ep_in_descriptor.wMaxPacketSize=max_packet_size;
-                        serial_ep_out_descriptor.wMaxPacketSize=max_packet_size;
-                        serial_interface_descriptor.bInterfaceNumber=interface_number;
-                        interface_number++;
-
-                        memcpy(&response_data[size],&serial_interface_descriptor,sizeof(struct usb_interface_descriptor));
-                        size += sizeof(struct usb_interface_descriptor);
-                        memcpy(&response_data[size],&serial_ep_in_descriptor,sizeof(struct usb_endpoint_descriptor));
-                        size += sizeof(struct usb_endpoint_descriptor);
-                        memcpy(&response_data[size],&serial_ep_out_descriptor,sizeof(struct usb_endpoint_descriptor));
-                        size += sizeof(struct usb_endpoint_descriptor);
-                    }
-#endif
-#ifdef USB_CHARGING_ONLY
-                    if(usb_core_charging_enabled && interface_number == 0){
-                        charging_interface_descriptor.bInterfaceNumber=interface_number;
-                        interface_number++;
-                        memcpy(&response_data[size],&charging_interface_descriptor,sizeof(struct usb_interface_descriptor));
-                        size += sizeof(struct usb_interface_descriptor);
-                    }
-#endif
-                    config_descriptor.bNumInterfaces=interface_number;
-                    config_descriptor.wTotalLength = size;
-                    memcpy(&response_data[0],&config_descriptor,sizeof(struct usb_config_descriptor));
-
-                    ptr = response_data;
+                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:
+                    break;
+                case USB_REQ_SET_FEATURE:
+                    break;
+                case USB_REQ_GET_STATUS:
+                    response_data[0]= 0;
+                    response_data[1]= 0;
+                    if(usb_drv_send(EP_CONTROL, response_data, 2)!=0)
+                        break;
+                    ack_control(req);
+                    break;
+                default: {
+                    bool handled=false;
+                    for(i=0;i<USB_NUM_DRIVERS;i++) {
+                        if(drivers[i].enabled &&
+                           drivers[i].control_request &&
+                           drivers[i].usb_interface == (req->wIndex))
+                        {
+                            handled = drivers[i].control_request(req);
+                        }
                     }
-
-                case USB_DT_STRING:
-                    logf("STRING %d",index);
-                    if ((unsigned)index < (sizeof(usb_strings)/sizeof(struct usb_string_descriptor*))) {
-                        size = usb_strings[index]->bLength;
-                        memcpy(&response_data[0],usb_strings[index],size);
-                        ptr = response_data;
-                    }
-                    else {
-                        logf("bad string id %d", index);
+                    if(!handled) {
+                        /* nope. flag error */
+                        logf("usb bad req %d", req->bRequest);
                         usb_drv_stall(EP_CONTROL, true,true);
+                        ack_control(req);
                     }
                     break;
-
-                case USB_DT_DEVICE_QUALIFIER:
-                    ptr = &qualifier_descriptor;
-                    size = sizeof (struct usb_qualifier_descriptor);
+                }
+            }
+            break;
+        case 2: /* Endpoint */
+            switch (req->bRequest) {
+                case USB_REQ_CLEAR_FEATURE:
+                    if (req->wValue == 0 ) /* ENDPOINT_HALT */
+                        usb_drv_stall(req->wIndex & 0xf, false,
+                                      (req->wIndex & 0x80) !=0);
+                    ack_control(req);
                     break;
-
-                default:
-                    logf("bad desc %d", req->wValue >> 8);
-                    usb_drv_stall(EP_CONTROL, true,true);
+                case USB_REQ_SET_FEATURE:
+                    if (req->wValue == 0 ) /* ENDPOINT_HALT */
+                        usb_drv_stall(req->wIndex & 0xf, true,
+                                      (req->wIndex & 0x80) !=0);
+                    ack_control(req);
                     break;
-            }
-
-            if (ptr) {
-                length = MIN(size, length);
-                if(usb_drv_send(EP_CONTROL, (void*)UNCACHED_ADDR(ptr), length)!=0)
+                case USB_REQ_GET_STATUS:
+                    response_data[0]= 0;
+                    response_data[1]= 0;
+                    logf("usb_core: GET_STATUS"); 
+                    if(req->wIndex>0)
+                        response_data[0] = usb_drv_stalled(req->wIndex&0xf,
+                                                       (req->wIndex&0x80)!=0);
+                    if(usb_drv_send(EP_CONTROL, response_data, 2)!=0)
+                        break;
+                    ack_control(req);
+                    break;
+                default: {
+                    bool handled=false;
+                    for(i=0;i<USB_NUM_DRIVERS;i++) {
+                        if(drivers[i].enabled &&
+                           drivers[i].control_request &&
+                           drivers[i].usb_endpoint == (req->wIndex & 0xf))
+                        {
+                            handled = drivers[i].control_request(req);
+                        }
+                    }
+                    if(!handled) {
+                        /* nope. flag error */
+                        logf("usb bad req %d", req->bRequest);
+                        usb_drv_stall(EP_CONTROL, true,true);
+                        ack_control(req);
+                    }
                     break;
-            }
-            ack_control(req);
-            break;
-        } /* USB_REQ_GET_DESCRIPTOR */
-
-        default: {
-            bool handled=false;
-            if((req->bRequestType & 0x1f) == 1) /* Interface */
-            {
-#ifdef USB_STORAGE
-                /* does usb_storage know this request? */
-                if(req->wIndex == mass_storage_interface_descriptor.bInterfaceNumber)
-                {
-                    handled = usb_storage_control_request(req);
-                }
-#endif
-
-#ifdef USB_SERIAL
-                /* does usb_serial know this request? */
-                if(req->wIndex == serial_interface_descriptor.bInterfaceNumber)
-                {
-                    handled = usb_serial_control_request(req);
                 }
-                    
-#endif
-            }
-            if(!handled)
-            {
-                /* nope. flag error */
-                logf("usb bad req %d", req->bRequest);
-                usb_drv_stall(EP_CONTROL, true,true);
-                ack_control(req);
             }
-            break;
-        }
     }
     logf("control handled");
 }
@@ -691,10 +691,6 @@ void usb_core_bus_reset(void)
 /* called by usb_drv_transfer_completed() */
 void usb_core_transfer_complete(int endpoint, bool in, int status,int length)
 {
-#if defined(USB_CHARGING_ONLY) || defined(USB_STORAGE)
-    (void)in;
-#endif
-
     switch (endpoint) {
         case EP_CONTROL:
             /* already handled */
diff --git a/firmware/usbstack/usb_serial.c b/firmware/usbstack/usb_serial.c
index 8c86932a31..55b76adc69 100644
--- a/firmware/usbstack/usb_serial.c
+++ b/firmware/usbstack/usb_serial.c
@@ -27,6 +27,31 @@
 
 #ifdef USB_SERIAL
 
+/* serial interface */
+struct usb_interface_descriptor __attribute__((aligned(2)))
+                                interface_descriptor =
+{
+    .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
+};
+
+struct usb_endpoint_descriptor __attribute__((aligned(2))) endpoint_descriptor =
+{
+    .bLength          = sizeof(struct usb_endpoint_descriptor),
+    .bDescriptorType  = USB_DT_ENDPOINT,
+    .bEndpointAddress = 0,
+    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
+    .wMaxPacketSize   = 0,
+    .bInterval        = 0
+};
+
 #define BUFFER_SIZE 512 /* Max 16k because of controller limitations */
 static unsigned char _send_buffer[BUFFER_SIZE] __attribute__((aligned(32)));
 static unsigned char* send_buffer;
@@ -38,8 +63,66 @@ static int buffer_start;
 static int buffer_length;
 static bool active = false;
 
+static int usb_endpoint;
+static int usb_interface;
+
 static struct mutex sendlock;
 
+static void sendout(void)
+{
+    if(buffer_start+buffer_length > BUFFER_SIZE)
+    {
+        /* Buffer wraps. Only send the first part */
+        usb_drv_send_nonblocking(usb_endpoint, &send_buffer[buffer_start],
+                                 (BUFFER_SIZE - buffer_start));
+    }
+    else
+    {
+        /* Send everything */
+        usb_drv_send_nonblocking(usb_endpoint, &send_buffer[buffer_start],
+                                 buffer_length);
+    }
+    busy_sending=true;
+}
+
+int usb_serial_get_config_descriptor(unsigned char *dest,int max_packet_size,
+                                     int interface_number,int endpoint)
+{
+    endpoint_descriptor.wMaxPacketSize=max_packet_size;
+    interface_descriptor.bInterfaceNumber=interface_number;
+
+
+    memcpy(dest,&interface_descriptor,sizeof(struct usb_interface_descriptor));
+    dest+=sizeof(struct usb_interface_descriptor);
+
+    endpoint_descriptor.bEndpointAddress = endpoint | USB_DIR_IN,
+    memcpy(dest,&endpoint_descriptor,sizeof(struct usb_endpoint_descriptor));
+    dest+=sizeof(struct usb_endpoint_descriptor);
+
+    endpoint_descriptor.bEndpointAddress = endpoint | USB_DIR_OUT,
+    memcpy(dest,&endpoint_descriptor,sizeof(struct usb_endpoint_descriptor));
+    return sizeof(struct usb_interface_descriptor) +
+           2 * sizeof(struct usb_endpoint_descriptor);
+}
+
+void usb_serial_init_connection(int interface,int endpoint)
+{
+    usb_interface = interface;
+    usb_endpoint = endpoint;
+
+    /* prime rx endpoint */
+    usb_drv_recv(usb_endpoint, receive_buffer, sizeof _receive_buffer);
+
+    /* we come here too after a bus reset, so reset some data */
+    mutex_lock(&sendlock);
+    busy_sending = false;
+    if(buffer_length>0)
+    {
+        sendout();
+    }
+    mutex_unlock(&sendlock);
+}
+
 /* called by usb_code_init() */
 void usb_serial_init(void)
 {
@@ -53,26 +136,11 @@ void usb_serial_init(void)
     mutex_init(&sendlock);
 }
 
-void usb_serial_exit(void)
+void usb_serial_disconnect(void)
 {
     active = false;
 }
 
-static void sendout(void)
-{
-    if(buffer_start+buffer_length > BUFFER_SIZE)
-    {
-        /* Buffer wraps. Only send the first part */
-        usb_drv_send_nonblocking(EP_SERIAL, &send_buffer[buffer_start],(BUFFER_SIZE - buffer_start));
-    }
-    else
-    {
-        /* Send everything */
-        usb_drv_send_nonblocking(EP_SERIAL, &send_buffer[buffer_start],buffer_length);
-    }
-    busy_sending=true;
-}
-
 void usb_serial_send(unsigned char *data,int length)
 {
     if(!active)
@@ -85,13 +153,14 @@ void usb_serial_send(unsigned char *data,int length)
         /* current buffer wraps, so new data can't */
         int available_space = BUFFER_SIZE - buffer_length;
         length=MIN(length,available_space);
-        memcpy(&send_buffer[(buffer_start+buffer_length)%BUFFER_SIZE],data,length);
+        memcpy(&send_buffer[(buffer_start+buffer_length)%BUFFER_SIZE],
+               data,length);
         buffer_length+=length;
     }
     else
     {
         /* current buffer doesn't wrap, so new data might */
-        int available_end_space = (BUFFER_SIZE - (buffer_start + buffer_length));
+        int available_end_space = (BUFFER_SIZE - (buffer_start+buffer_length));
         int first_chunk = MIN(length,available_end_space);
         memcpy(&send_buffer[buffer_start + buffer_length],data,first_chunk);
         length-=first_chunk;
@@ -121,7 +190,7 @@ void usb_serial_transfer_complete(bool in, int status, int length)
         case false:
             logf("serial: %s", receive_buffer);
             /* Data received. TODO : Do something with it ? */
-            usb_drv_recv(EP_SERIAL, receive_buffer, sizeof _receive_buffer);
+            usb_drv_recv(usb_endpoint, receive_buffer, sizeof _receive_buffer);
             break;
 
         case true:
@@ -148,26 +217,9 @@ bool usb_serial_control_request(struct usb_ctrlrequest* req)
 {
     bool handled = false;
     switch (req->bRequest) {
-        case USB_REQ_SET_CONFIGURATION:
-            logf("serial: set config");
-            /* prime rx endpoint */
-            usb_drv_recv(EP_SERIAL, receive_buffer, sizeof _receive_buffer);
-            handled = true;
-
-            /* we come here too after a bus reset, so reset some data */
-            mutex_lock(&sendlock);
-            busy_sending = false;
-            if(buffer_length>0)
-            {
-                sendout();
-            }
-            mutex_unlock(&sendlock);
-            break;
-
         default:
             logf("serial: unhandeld req %d", req->bRequest);
     }
-
     return handled;
 }
 
diff --git a/firmware/usbstack/usb_serial.h b/firmware/usbstack/usb_serial.h
index bcb0648af5..5d624137c3 100644
--- a/firmware/usbstack/usb_serial.h
+++ b/firmware/usbstack/usb_serial.h
@@ -21,8 +21,11 @@
 
 #include "usb_ch9.h"
 
+int usb_serial_get_config_descriptor(unsigned char *dest,int max_packet_size,
+                                     int interface_number, int endpoint);
+void usb_serial_init_connection(int interface,int endpoint);
 void usb_serial_init(void);
-void usb_serial_exit(void);
+void usb_serial_disconnect(void);
 void usb_serial_transfer_complete(bool in, int status, int length);
 bool usb_serial_control_request(struct usb_ctrlrequest* req);
 
diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c
index f1029c3c93..58578d958b 100644
--- a/firmware/usbstack/usb_storage.c
+++ b/firmware/usbstack/usb_storage.c
@@ -28,6 +28,7 @@
 /* Needed to get at the audio buffer */
 #include "audio.h"
 
+
 #ifdef USB_STORAGE
 
 /* Enable the following define to export only the SD card slot. This
@@ -85,6 +86,35 @@
 
 #define SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA 0x02000000
 
+/* storage interface */
+
+#define USB_SC_SCSI      0x06            /* Transparent */
+#define USB_PROT_BULK    0x50            /* bulk only */
+
+static struct usb_interface_descriptor __attribute__((aligned(2)))
+                                       interface_descriptor = 
+{
+    .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
+};
+
+static struct usb_endpoint_descriptor __attribute__((aligned(2)))
+                                      endpoint_descriptor =
+{
+    .bLength          = sizeof(struct usb_endpoint_descriptor),
+    .bDescriptorType  = USB_DT_ENDPOINT,
+    .bEndpointAddress = 0,
+    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
+    .wMaxPacketSize   = 0,
+    .bInterval        = 0
+};
 
 struct inquiry_data {
     unsigned char DeviceType;
@@ -111,7 +141,7 @@ struct report_lun_data {
 struct sense_data {
     unsigned char ResponseCode;
     unsigned char Obsolete;
-    unsigned char filemark_eom_ili_sensekey;
+    unsigned char fei_sensekey;
     unsigned int Information;
     unsigned char AdditionalSenseLength;
     unsigned int  CommandSpecificInformation;
@@ -122,15 +152,15 @@ struct sense_data {
     unsigned short SenseKeySpecific;
 } __attribute__ ((packed));
 
-struct mode_sense_block_descriptor_longlba {
-    unsigned char number_of_blocks[8];
+struct mode_sense_bdesc_longlba {
+    unsigned char num_blocks[8];
     unsigned char reserved[4];
     unsigned char block_size[4];
 } __attribute__ ((packed));
 
-struct mode_sense_block_descriptor_shortlba {
+struct mode_sense_bdesc_shortlba {
     unsigned char density_code;
-    unsigned char number_of_blocks[3];
+    unsigned char num_blocks[3];
     unsigned char reserved;
     unsigned char block_size[3];
 } __attribute__ ((packed));
@@ -142,7 +172,7 @@ struct mode_sense_data_10 {
     unsigned char longlba;
     unsigned char reserved;
     unsigned short block_descriptor_length;
-    struct mode_sense_block_descriptor_longlba block_descriptor;
+    struct mode_sense_bdesc_longlba block_descriptor;
 } __attribute__ ((packed));
 
 struct mode_sense_data_6 {
@@ -150,7 +180,7 @@ struct mode_sense_data_6 {
     unsigned char medium_type;
     unsigned char device_specific;
     unsigned char block_descriptor_length;
-    struct mode_sense_block_descriptor_shortlba block_descriptor;
+    struct mode_sense_bdesc_shortlba block_descriptor;
 } __attribute__ ((packed));
 
 struct command_block_wrapper {
@@ -188,8 +218,8 @@ static union {
     struct capacity* capacity_data;
     struct format_capacity* format_capacity_data;
     struct sense_data *sense_data;
-    struct mode_sense_data_6 *mode_sense_data_6;
-    struct mode_sense_data_10 *mode_sense_data_10;
+    struct mode_sense_data_6 *ms_data_6;
+    struct mode_sense_data_10 *ms_data_10;
     struct report_lun_data *lun_data;
     struct command_status_wrapper* csw;
     char *max_lun;
@@ -203,7 +233,7 @@ static struct {
     unsigned char *data[2];
     unsigned char data_select;
     unsigned int last_result;
-} current_cmd;
+} cur_cmd;
 
 static struct {
     unsigned char sense_key;
@@ -221,6 +251,9 @@ static void identify2inquiry(int lun);
 static void send_and_read_next(void);
 static bool ejected[NUM_VOLUMES];
 
+static int usb_endpoint;
+static int usb_interface;
+
 static enum {
     WAITING_FOR_COMMAND,
     SENDING_BLOCKS,
@@ -238,6 +271,49 @@ void usb_storage_init(void)
     logf("usb_storage_init done");
 }
 
+int usb_storage_get_config_descriptor(unsigned char *dest,int max_packet_size,
+                                      int interface_number,int endpoint)
+{
+    endpoint_descriptor.wMaxPacketSize=max_packet_size;
+    interface_descriptor.bInterfaceNumber=interface_number;
+
+
+    memcpy(dest,&interface_descriptor,
+           sizeof(struct usb_interface_descriptor));
+    dest+=sizeof(struct usb_interface_descriptor);
+
+    endpoint_descriptor.bEndpointAddress = endpoint | USB_DIR_IN,
+    memcpy(dest,&endpoint_descriptor,
+           sizeof(struct usb_endpoint_descriptor));
+    dest+=sizeof(struct usb_endpoint_descriptor);
+
+    endpoint_descriptor.bEndpointAddress = endpoint | USB_DIR_OUT,
+    memcpy(dest,&endpoint_descriptor,
+           sizeof(struct usb_endpoint_descriptor));
+
+    return sizeof(struct usb_interface_descriptor) + 
+           2*sizeof(struct usb_endpoint_descriptor);
+}
+
+void usb_storage_init_connection(int interface,int endpoint)
+{
+    size_t bufsize;
+    unsigned char * audio_buffer;
+
+    usb_interface = interface;
+    usb_endpoint = endpoint;
+
+    logf("ums: set config");
+    /* prime rx endpoint. We only need room for commands */
+    state = WAITING_FOR_COMMAND;
+
+    /* TODO : check if bufsize is at least 32K ? */
+    audio_buffer = audio_get_buffer(false,&bufsize);
+    tb.transfer_buffer =
+        (void *)UNCACHED_ADDR((unsigned int)(audio_buffer + 31) & 0xffffffe0);
+    usb_drv_recv(usb_endpoint, tb.transfer_buffer, 1024);
+}
+
 /* called by usb_core_transfer_complete() */
 void usb_storage_transfer_complete(bool in,int status,int length)
 {
@@ -249,26 +325,31 @@ void usb_storage_transfer_complete(bool in,int status,int length)
             if(in==true) {
                 logf("IN received in RECEIVING");
             }
-            logf("scsi write %d %d", current_cmd.sector, current_cmd.count);
+            logf("scsi write %d %d", cur_cmd.sector, cur_cmd.count);
             if(status==0) {
-                if((unsigned int)length!=(SECTOR_SIZE*current_cmd.count)
+                if((unsigned int)length!=(SECTOR_SIZE*cur_cmd.count)
                   && (unsigned int)length!=BUFFER_SIZE) {
                     logf("unexpected length :%d",length);
                 }
 
-                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);
+                unsigned int next_sector = cur_cmd.sector +
+                             (BUFFER_SIZE/SECTOR_SIZE);
+                unsigned int next_count = cur_cmd.count -
+                             MIN(cur_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],
+                    receive_block_data(cur_cmd.data[!cur_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]);
+                /* Now write the data that just came in, while the host is
+                   sending the next bit */
+                int result = ata_write_sectors(IF_MV2(cur_cmd.lun,)
+                                         cur_cmd.sector,
+                                         MIN(BUFFER_SIZE/SECTOR_SIZE,
+                                         cur_cmd.count),
+                                         cur_cmd.data[cur_cmd.data_select]);
                 if(result != 0) {
                     send_csw(UMS_STATUS_FAIL);
                     cur_sense_data.sense_key=SENSE_MEDIUM_ERROR;
@@ -282,10 +363,10 @@ void usb_storage_transfer_complete(bool in,int status,int length)
                 }
 
                 /* Switch buffers for the next one */
-                current_cmd.data_select=!current_cmd.data_select;
+                cur_cmd.data_select=!cur_cmd.data_select;
 
-                current_cmd.sector = next_sector;
-                current_cmd.count = next_count;
+                cur_cmd.sector = next_sector;
+                cur_cmd.count = next_count;
 
             }
             else {
@@ -307,8 +388,8 @@ void usb_storage_transfer_complete(bool in,int status,int length)
                 handle_scsi(cbw);
             }
             else {
-                usb_drv_stall(EP_MASS_STORAGE, true,true);
-                usb_drv_stall(EP_MASS_STORAGE, true,false);
+                usb_drv_stall(usb_endpoint, true,true);
+                usb_drv_stall(usb_endpoint, true,false);
             }
             break;
         case SENDING_CSW:
@@ -317,7 +398,7 @@ void usb_storage_transfer_complete(bool in,int status,int length)
             }
             //logf("csw sent, now go back to idle");
             state = WAITING_FOR_COMMAND;
-            usb_drv_recv(EP_MASS_STORAGE, tb.transfer_buffer, 1024);
+            usb_drv_recv(usb_endpoint, tb.transfer_buffer, 1024);
             break;
         case SENDING_RESULT:
             if(in==false) {
@@ -342,7 +423,7 @@ void usb_storage_transfer_complete(bool in,int status,int length)
                 logf("OUT received in SENDING");
             }
             if(status==0) {
-                if(current_cmd.count==0) {
+                if(cur_cmd.count==0) {
                     //logf("data sent, now send csw");
                     send_csw(UMS_STATUS_GOOD);
                 }
@@ -368,6 +449,7 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
 {
     bool handled = false;
 
+
     switch (req->bRequest) {
         case USB_BULK_GET_MAX_LUN: {
 #ifdef ONLY_EXPOSE_CARD_SLOT
@@ -386,31 +468,16 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
             logf("ums: bulk reset");
             state = WAITING_FOR_COMMAND;
             /* UMS BOT 3.1 says The device shall preserve the value of its bulk
-             * data toggle bits and endpoint STALL conditions despite the Bulk-Only
-             * Mass Storage Reset. */
+               data toggle bits and endpoint STALL conditions despite
+               the Bulk-Only Mass Storage Reset. */
 #if 0
-            usb_drv_reset_endpoint(EP_MASS_STORAGE, false);
-            usb_drv_reset_endpoint(EP_MASS_STORAGE, true);
+            usb_drv_reset_endpoint(usb_endpoint, false);
+            usb_drv_reset_endpoint(usb_endpoint, true);
 #endif
             
             usb_drv_send(EP_CONTROL, NULL, 0);  /* ack */
             handled = true;
             break;
-
-        case USB_REQ_SET_CONFIGURATION: {
-            size_t bufsize;
-            unsigned char * audio_buffer;
-            logf("ums: set config");
-            /* prime rx endpoint. We only need room for commands */
-            state = WAITING_FOR_COMMAND;
-
-            /* TODO : check if bufsize is at least 32K ? */
-            audio_buffer = audio_get_buffer(false,&bufsize);
-            tb.transfer_buffer = (void *)UNCACHED_ADDR((unsigned int)(audio_buffer + 31) & 0xffffffe0);
-            usb_drv_recv(EP_MASS_STORAGE, tb.transfer_buffer, 1024);
-            handled = true;
-            break;
-        }
     }
 
     return handled;
@@ -418,7 +485,7 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
 
 static void send_and_read_next(void)
 {
-    if(current_cmd.last_result!=0) {
+    if(cur_cmd.last_result!=0) {
         /* The last read failed. */
         send_csw(UMS_STATUS_FAIL);
         cur_sense_data.sense_key=SENSE_MEDIUM_ERROR;
@@ -426,21 +493,23 @@ static void send_and_read_next(void)
         cur_sense_data.ascq=0;
         return;
     }
-    send_block_data(current_cmd.data[current_cmd.data_select],
-                    MIN(BUFFER_SIZE,current_cmd.count*SECTOR_SIZE));
+    send_block_data(cur_cmd.data[cur_cmd.data_select],
+                    MIN(BUFFER_SIZE,cur_cmd.count*SECTOR_SIZE));
 
     /* Switch buffers for the next one */
-    current_cmd.data_select=!current_cmd.data_select;
+    cur_cmd.data_select=!cur_cmd.data_select;
 
-    current_cmd.sector+=(BUFFER_SIZE/SECTOR_SIZE);
-    current_cmd.count-=MIN(current_cmd.count,BUFFER_SIZE/SECTOR_SIZE);
+    cur_cmd.sector+=(BUFFER_SIZE/SECTOR_SIZE);
+    cur_cmd.count-=MIN(cur_cmd.count,BUFFER_SIZE/SECTOR_SIZE);
 
-    if(current_cmd.count!=0){
+    if(cur_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]);
+        cur_cmd.last_result = ata_read_sectors(IF_MV2(cur_cmd.lun,)
+                                           cur_cmd.sector,
+                                           MIN(BUFFER_SIZE/SECTOR_SIZE,
+                                           cur_cmd.count),
+                                           cur_cmd.data[cur_cmd.data_select]);
     }
 }
 /****************************************************************************/
@@ -484,8 +553,8 @@ static void handle_scsi(struct command_block_wrapper* cbw)
     block_size_mult = disk_sector_multiplier;
 #endif
 
-    current_cmd.tag = cbw->tag;
-    current_cmd.lun = lun;
+    cur_cmd.tag = cbw->tag;
+    cur_cmd.lun = lun;
 
     switch (cbw->command_block[0]) {
         case SCSI_TEST_UNIT_READY:
@@ -524,7 +593,8 @@ static void handle_scsi(struct command_block_wrapper* cbw)
 #endif
             tb.lun_data->lun0[1]=0;
             
-            send_command_result(tb.lun_data, MIN(sizeof(struct report_lun_data), length));
+            send_command_result(tb.lun_data,
+                                MIN(sizeof(struct report_lun_data), length));
             break;
         }
 
@@ -532,12 +602,13 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             logf("scsi inquiry %d",lun);
             identify2inquiry(lun);
             length = MIN(length, cbw->command_block[4]);
-            send_command_result(tb.inquiry, MIN(sizeof(struct inquiry_data), length));
+            send_command_result(tb.inquiry,
+                                MIN(sizeof(struct inquiry_data), length));
             break;
 
         case SCSI_REQUEST_SENSE: {
             tb.sense_data->ResponseCode=0x70;/*current error*/
-            tb.sense_data->filemark_eom_ili_sensekey=cur_sense_data.sense_key&0x0f;
+            tb.sense_data->fei_sensekey=cur_sense_data.sense_key&0x0f;
             tb.sense_data->Information=cur_sense_data.information;
             tb.sense_data->AdditionalSenseLength=10;
             tb.sense_data->CommandSpecificInformation=0;
@@ -564,24 +635,36 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             logf("scsi mode_sense_10 %d %X",lun,page_code);
             switch(page_code) {
                 case 0x3f:
-                    tb.mode_sense_data_10->mode_data_length=htobe16(sizeof(struct mode_sense_data_10)-2);
-                    tb.mode_sense_data_10->medium_type=0;
-                    tb.mode_sense_data_10->device_specific=0;
-                    tb.mode_sense_data_10->reserved=0;
-                    tb.mode_sense_data_10->longlba=1;
-                    tb.mode_sense_data_10->block_descriptor_length=htobe16(sizeof(struct mode_sense_block_descriptor_longlba));
-                    memset(tb.mode_sense_data_10->block_descriptor.reserved,0,4);
-                    memset(tb.mode_sense_data_10->block_descriptor.number_of_blocks,0,8);
-                    tb.mode_sense_data_10->block_descriptor.number_of_blocks[4]=((block_count/block_size_mult) & 0xff000000)>>24;
-                    tb.mode_sense_data_10->block_descriptor.number_of_blocks[5]=((block_count/block_size_mult) & 0x00ff0000)>>16;
-                    tb.mode_sense_data_10->block_descriptor.number_of_blocks[6]=((block_count/block_size_mult) & 0x0000ff00)>>8;
-                    tb.mode_sense_data_10->block_descriptor.number_of_blocks[7]=((block_count/block_size_mult) & 0x000000ff);
-
-                    tb.mode_sense_data_10->block_descriptor.block_size[0]=((block_size*block_size_mult) & 0xff000000)>>24;
-                    tb.mode_sense_data_10->block_descriptor.block_size[1]=((block_size*block_size_mult) & 0x00ff0000)>>16;
-                    tb.mode_sense_data_10->block_descriptor.block_size[2]=((block_size*block_size_mult) & 0x0000ff00)>>8;
-                    tb.mode_sense_data_10->block_descriptor.block_size[3]=((block_size*block_size_mult) & 0x000000ff);
-                    send_command_result(tb.mode_sense_data_10,
+                    tb.ms_data_10->mode_data_length =
+                                   htobe16(sizeof(struct mode_sense_data_10)-2);
+                    tb.ms_data_10->medium_type = 0;
+                    tb.ms_data_10->device_specific = 0;
+                    tb.ms_data_10->reserved = 0;
+                    tb.ms_data_10->longlba = 1;
+                    tb.ms_data_10->block_descriptor_length =
+                        htobe16(sizeof(struct mode_sense_bdesc_longlba));
+
+                    memset(tb.ms_data_10->block_descriptor.reserved,0,4);
+                    memset(tb.ms_data_10->block_descriptor.num_blocks,0,8);
+
+                    tb.ms_data_10->block_descriptor.num_blocks[4] =
+                               ((block_count/block_size_mult) & 0xff000000)>>24;
+                    tb.ms_data_10->block_descriptor.num_blocks[5] =
+                               ((block_count/block_size_mult) & 0x00ff0000)>>16;
+                    tb.ms_data_10->block_descriptor.num_blocks[6] =
+                               ((block_count/block_size_mult) & 0x0000ff00)>>8;
+                    tb.ms_data_10->block_descriptor.num_blocks[7] =
+                               ((block_count/block_size_mult) & 0x000000ff);
+
+                    tb.ms_data_10->block_descriptor.block_size[0] =
+                                ((block_size*block_size_mult) & 0xff000000)>>24;
+                    tb.ms_data_10->block_descriptor.block_size[1] =
+                                ((block_size*block_size_mult) & 0x00ff0000)>>16;
+                    tb.ms_data_10->block_descriptor.block_size[2] =
+                                ((block_size*block_size_mult) & 0x0000ff00)>>8;
+                    tb.ms_data_10->block_descriptor.block_size[3] =
+                                ((block_size*block_size_mult) & 0x000000ff);
+                    send_command_result(tb.ms_data_10,
                               MIN(sizeof(struct mode_sense_data_10), length));
                     break;
                 default:
@@ -606,27 +689,35 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             logf("scsi mode_sense_6 %d %X",lun,page_code);
             switch(page_code) {
                 case 0x3f:
-                    /* All supported pages Since we support only one this is easy*/
-                    tb.mode_sense_data_6->mode_data_length=sizeof(struct mode_sense_data_6)-1;
-                    tb.mode_sense_data_6->medium_type=0;
-                    tb.mode_sense_data_6->device_specific=0;
-                    tb.mode_sense_data_6->block_descriptor_length=sizeof(struct mode_sense_block_descriptor_shortlba);
-                    tb.mode_sense_data_6->block_descriptor.density_code=0;
-                    tb.mode_sense_data_6->block_descriptor.reserved=0;
+                    /* All supported pages. */
+                    tb.ms_data_6->mode_data_length =
+                         sizeof(struct mode_sense_data_6)-1;
+                    tb.ms_data_6->medium_type = 0;
+                    tb.ms_data_6->device_specific = 0;
+                    tb.ms_data_6->block_descriptor_length =
+                            sizeof(struct mode_sense_bdesc_shortlba);
+                    tb.ms_data_6->block_descriptor.density_code = 0;
+                    tb.ms_data_6->block_descriptor.reserved = 0;
                     if(block_count/block_size_mult > 0xffffff){
-                        tb.mode_sense_data_6->block_descriptor.number_of_blocks[0]=0xff;
-                        tb.mode_sense_data_6->block_descriptor.number_of_blocks[1]=0xff;
-                        tb.mode_sense_data_6->block_descriptor.number_of_blocks[2]=0xff;
+                        tb.ms_data_6->block_descriptor.num_blocks[0] = 0xff;
+                        tb.ms_data_6->block_descriptor.num_blocks[1] = 0xff;
+                        tb.ms_data_6->block_descriptor.num_blocks[2] = 0xff;
                     }
                     else {
-                        tb.mode_sense_data_6->block_descriptor.number_of_blocks[0]=((block_count/block_size_mult) & 0xff0000)>>16;
-                        tb.mode_sense_data_6->block_descriptor.number_of_blocks[1]=((block_count/block_size_mult) & 0x00ff00)>>8;
-                        tb.mode_sense_data_6->block_descriptor.number_of_blocks[2]=((block_count/block_size_mult) & 0x0000ff);
+                        tb.ms_data_6->block_descriptor.num_blocks[0] =
+                             ((block_count/block_size_mult) & 0xff0000)>>16;
+                        tb.ms_data_6->block_descriptor.num_blocks[1] =
+                             ((block_count/block_size_mult) & 0x00ff00)>>8;
+                        tb.ms_data_6->block_descriptor.num_blocks[2] =
+                             ((block_count/block_size_mult) & 0x0000ff);
                     }
-                    tb.mode_sense_data_6->block_descriptor.block_size[0]=((block_size*block_size_mult) & 0xff0000)>>16;
-                    tb.mode_sense_data_6->block_descriptor.block_size[1]=((block_size*block_size_mult) & 0x00ff00)>>8;
-                    tb.mode_sense_data_6->block_descriptor.block_size[2]=((block_size*block_size_mult) & 0x0000ff);
-                    send_command_result(tb.mode_sense_data_6,
+                    tb.ms_data_6->block_descriptor.block_size[0] =
+                         ((block_size*block_size_mult) & 0xff0000)>>16;
+                    tb.ms_data_6->block_descriptor.block_size[1] =
+                         ((block_size*block_size_mult) & 0x00ff00)>>8;
+                    tb.ms_data_6->block_descriptor.block_size[2] =
+                         ((block_size*block_size_mult) & 0x0000ff);
+                    send_command_result(tb.ms_data_6,
                               MIN(sizeof(struct mode_sense_data_6), length));
                     break;
                 default:
@@ -641,8 +732,8 @@ static void handle_scsi(struct command_block_wrapper* cbw)
 
         case SCSI_START_STOP_UNIT:
             logf("scsi start_stop unit %d",lun);
-            if((cbw->command_block[4] & 0xf0) == 0) /* Process start and eject bits */
-            {
+            if((cbw->command_block[4] & 0xf0) == 0)
+            { /* Process start and eject bits */
                 if((cbw->command_block[4] & 0x01) == 0 && 
                    (cbw->command_block[4] & 0x02) != 0) /* Stop and eject */
                 {
@@ -661,10 +752,13 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             logf("scsi read_format_capacity %d",lun);
             if(lun_present) {
                 tb.format_capacity_data->following_length=htobe32(8);
-                /* Careful: "block count" actually means "number of last block" */
-                tb.format_capacity_data->block_count = htobe32(block_count/block_size_mult - 1);
-                tb.format_capacity_data->block_size = htobe32(block_size*block_size_mult);
-                tb.format_capacity_data->block_size |= SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA;
+                /* "block count" actually means "number of last block" */
+                tb.format_capacity_data->block_count =
+                     htobe32(block_count/block_size_mult - 1);
+                tb.format_capacity_data->block_size =
+                     htobe32(block_size*block_size_mult);
+                tb.format_capacity_data->block_size |=
+                     SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA;
 
                 send_command_result(tb.format_capacity_data,
                           MIN(sizeof(struct format_capacity), length));
@@ -682,11 +776,14 @@ static void handle_scsi(struct command_block_wrapper* cbw)
             logf("scsi read_capacity %d",lun);
 
             if(lun_present) {
-                /* Careful: "block count" actually means "number of last block" */
-                tb.capacity_data->block_count = htobe32(block_count/block_size_mult - 1);
-                tb.capacity_data->block_size = htobe32(block_size*block_size_mult);
-
-                send_command_result(tb.capacity_data, MIN(sizeof(struct capacity), length));
+                /* "block count" actually means "number of last block" */
+                tb.capacity_data->block_count =
+                     htobe32(block_count/block_size_mult - 1);
+                tb.capacity_data->block_size =
+                     htobe32(block_size*block_size_mult);
+
+                send_command_result(tb.capacity_data,
+                     MIN(sizeof(struct capacity), length));
             }
             else
             {
@@ -707,30 +804,32 @@ static void handle_scsi(struct command_block_wrapper* cbw)
                 cur_sense_data.ascq=0;
                 break;
             }
-            current_cmd.data[0] = tb.transfer_buffer;
-            current_cmd.data[1] = &tb.transfer_buffer[BUFFER_SIZE];
-            current_cmd.data_select=0;
-            current_cmd.sector = block_size_mult *
+            cur_cmd.data[0] = tb.transfer_buffer;
+            cur_cmd.data[1] = &tb.transfer_buffer[BUFFER_SIZE];
+            cur_cmd.data_select=0;
+            cur_cmd.sector = block_size_mult *
                (cbw->command_block[2] << 24 |
                 cbw->command_block[3] << 16 |
                 cbw->command_block[4] << 8  |
                 cbw->command_block[5] );
-            current_cmd.count = block_size_mult *
+            cur_cmd.count = block_size_mult *
                (cbw->command_block[7] << 8 |
                 cbw->command_block[8]);
 
-            //logf("scsi read %d %d", current_cmd.sector, current_cmd.count);
+            //logf("scsi read %d %d", cur_cmd.sector, cur_cmd.count);
 
-            if((current_cmd.sector + current_cmd.count) > block_count) {
+            if((cur_cmd.sector + cur_cmd.count) > block_count) {
                 send_csw(UMS_STATUS_FAIL);
                 cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
                 cur_sense_data.asc=ASC_LBA_OUT_OF_RANGE;
                 cur_sense_data.ascq=0;
             }
             else {
-                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]);
+                cur_cmd.last_result = ata_read_sectors(IF_MV2(cur_cmd.lun,)
+                                             cur_cmd.sector,
+                                             MIN(BUFFER_SIZE/SECTOR_SIZE,
+                                             cur_cmd.count),
+                                             cur_cmd.data[cur_cmd.data_select]);
                 send_and_read_next();
             }
             break;
@@ -744,34 +843,35 @@ static void handle_scsi(struct command_block_wrapper* cbw)
                 cur_sense_data.ascq=0;
                 break;
             }
-            current_cmd.data[0] = tb.transfer_buffer;
-            current_cmd.data[1] = &tb.transfer_buffer[BUFFER_SIZE];
-            current_cmd.data_select=0;
-            current_cmd.sector = block_size_mult *
+            cur_cmd.data[0] = tb.transfer_buffer;
+            cur_cmd.data[1] = &tb.transfer_buffer[BUFFER_SIZE];
+            cur_cmd.data_select=0;
+            cur_cmd.sector = block_size_mult *
                (cbw->command_block[2] << 24 |
                 cbw->command_block[3] << 16 |
                 cbw->command_block[4] << 8  |
                 cbw->command_block[5] );
-            current_cmd.count = block_size_mult *
+            cur_cmd.count = block_size_mult *
                (cbw->command_block[7] << 8 |
                 cbw->command_block[8]);
             /* expect data */
-            if((current_cmd.sector + current_cmd.count) > block_count) {
+            if((cur_cmd.sector + cur_cmd.count) > block_count) {
                 send_csw(UMS_STATUS_FAIL);
                 cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
                 cur_sense_data.asc=ASC_LBA_OUT_OF_RANGE;
                 cur_sense_data.ascq=0;
             }
             else {
-                receive_block_data(current_cmd.data[0],
-                                   MIN(BUFFER_SIZE,current_cmd.count*SECTOR_SIZE));
+                receive_block_data(cur_cmd.data[0],
+                                   MIN(BUFFER_SIZE,
+                                   cur_cmd.count*SECTOR_SIZE));
             }
 
             break;
 
         default:
             logf("scsi unknown cmd %x",cbw->command_block[0x0]);
-            usb_drv_stall(EP_MASS_STORAGE, true,true);
+            usb_drv_stall(usb_endpoint, true,true);
             send_csw(UMS_STATUS_FAIL);
             break;
     }
@@ -779,30 +879,31 @@ static void handle_scsi(struct command_block_wrapper* cbw)
 
 static void send_block_data(void *data,int size)
 {
-    usb_drv_send_nonblocking(EP_MASS_STORAGE, data,size);
+    usb_drv_send_nonblocking(usb_endpoint, data,size);
     state = SENDING_BLOCKS;
 }
 
 static void send_command_result(void *data,int size)
 {
-    usb_drv_send_nonblocking(EP_MASS_STORAGE, data,size);
+    usb_drv_send_nonblocking(usb_endpoint, data,size);
     state = SENDING_RESULT;
 }
 
 static void receive_block_data(void *data,int size)
 {
-    usb_drv_recv(EP_MASS_STORAGE, data, size);
+    usb_drv_recv(usb_endpoint, data, size);
     state = RECEIVING_BLOCKS;
 }
 
 static void send_csw(int status)
 {
     tb.csw->signature = htole32(CSW_SIGNATURE);
-    tb.csw->tag = current_cmd.tag;
+    tb.csw->tag = cur_cmd.tag;
     tb.csw->data_residue = 0;
     tb.csw->status = status;
 
-    usb_drv_send_nonblocking(EP_MASS_STORAGE, tb.csw, sizeof(struct command_status_wrapper));
+    usb_drv_send_nonblocking(usb_endpoint, tb.csw,
+                             sizeof(struct command_status_wrapper));
     state = SENDING_CSW;
     //logf("CSW: %X",status);
 
@@ -866,7 +967,8 @@ static void identify2inquiry(int lun)
         tb.inquiry->DeviceTypeModifier = DEVICE_REMOVABLE;
 #endif
 #endif
-    /* Mac OSX 10.5 doesn't like this driver if DEVICE_REMOVABLE is not set */
+    /* Mac OSX 10.5 doesn't like this driver if DEVICE_REMOVABLE is not set.
+       TODO : this can probably be solved by providing caching mode page */
     tb.inquiry->DeviceTypeModifier = DEVICE_REMOVABLE;
 }
 
diff --git a/firmware/usbstack/usb_storage.h b/firmware/usbstack/usb_storage.h
index 23903a855a..34bc0144dd 100644
--- a/firmware/usbstack/usb_storage.h
+++ b/firmware/usbstack/usb_storage.h
@@ -21,6 +21,9 @@
 
 #include "usb_ch9.h"
 
+int usb_storage_get_config_descriptor(unsigned char *dest,int max_packet_size,
+                                      int interface_number,int endpoint);
+void usb_storage_init_connection(int interface,int endpoint);
 void usb_storage_init(void);
 void usb_storage_transfer_complete(bool in,int state,int length);
 bool usb_storage_control_request(struct usb_ctrlrequest* req);