Major USB fixes by Frank Gevaerts. Still disabled in builds, #define USE_ROCKBOX_USB to test.

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

10 files changed, 662 insertions, 203 deletions

diff --git a/bootloader/main-pp.c b/bootloader/main-pp.c
index 74c6623..5af6e57 100644
--- a/bootloader/main-pp.c
+++ b/bootloader/main-pp.c
@@ -465,6 +465,7 @@ void* main(void)
         
     btn = button_read_device();
 #if defined(SANSA_E200) || defined(SANSA_C200)
+#if !defined(USE_ROCKBOX_USB)
     usb_init();
     while (usb_drv_powered() && usb_retry < 5 && !usb)
     {
@@ -474,6 +475,7 @@ void* main(void)
     }
     if (usb)
         btn |= BOOTLOADER_BOOT_OF;
+#endif /* USE_ROCKBOX_USB */
 #endif
     /* Enable bootloader messages if any button is pressed */
     if (btn)
diff --git a/docs/CREDITS b/docs/CREDITS
index 7c95ab8..955b811 100644
--- a/docs/CREDITS
+++ b/docs/CREDITS
@@ -368,6 +368,7 @@ George Tamplaru
 Apoorva Mahajan
 Vuong Minh Hiep
 Mateusz Kubica
+Frank Gevaerts
 
 The libmad team
 The wavpack team
diff --git a/firmware/common/disk.c b/firmware/common/disk.c
index 9fb73f0..c26fdb3 100644
--- a/firmware/common/disk.c
+++ b/firmware/common/disk.c
@@ -48,6 +48,9 @@
 static struct partinfo part[8]; /* space for 4 partitions on 2 drives */
 static int vol_drive[NUM_VOLUMES]; /* mounted to which drive (-1 if none) */
 
+#ifdef MAX_LOG_SECTOR_SIZE
+int disk_sector_multiplier = 1;
+#endif
 struct partinfo* disk_init(IF_MV_NONVOID(int drive))
 {
     int i;
@@ -168,6 +171,8 @@ int disk_mount(int drive)
                 mounted++;
                 vol_drive[volume] = drive; /* remember the drive for this volume */
                 volume = get_free_volume(); /* prepare next entry */
+                if (drive == 0)
+                    disk_sector_multiplier = j;
                 break;
             }
         }
diff --git a/firmware/export/disk.h b/firmware/export/disk.h
index 9672a6d..d6d6796 100644
--- a/firmware/export/disk.h
+++ b/firmware/export/disk.h
@@ -39,4 +39,9 @@ int disk_mount_all(void); /* returns the # of successful mounts */
 int disk_mount(int drive);
 int disk_unmount(int drive);
 
+/* The number of 512-byte sectors in a "logical" sector. Needed for ipod 5.5G */
+#ifdef MAX_LOG_SECTOR_SIZE
+extern int disk_sector_multiplier;
+#endif
+
 #endif
diff --git a/firmware/export/usb_ch9.h b/firmware/export/usb_ch9.h
index b8fe181..1bfc152 100644
--- a/firmware/export/usb_ch9.h
+++ b/firmware/export/usb_ch9.h
@@ -244,7 +244,7 @@ struct usb_string_descriptor {
     uint8_t  bLength;
     uint8_t  bDescriptorType;
 
-    uint16_t wData[1]; /* UTF-16LE encoded */
+    uint16_t wString[]; /* UTF-16LE encoded */
 } __attribute__ ((packed));
 
 /* note that "string" zero is special, it holds language codes that
diff --git a/firmware/export/usb_drv.h b/firmware/export/usb_drv.h
index c503a84..6a37144 100644
--- a/firmware/export/usb_drv.h
+++ b/firmware/export/usb_drv.h
@@ -32,5 +32,8 @@ void usb_drv_set_address(int address);
 void usb_drv_reset_endpoint(int endpoint, bool send);
 void usb_drv_wait(int endpoint, bool send);
 bool usb_drv_powered(void);
+int usb_drv_get_last_transfer_status(void);
+int usb_drv_get_last_transfer_length(void);
+int usb_drv_port_speed(void);
 
 #endif
diff --git a/firmware/target/arm/usb-drv-pp502x.c b/firmware/target/arm/usb-drv-pp502x.c
index 1db3ebd..413d905 100644
--- a/firmware/target/arm/usb-drv-pp502x.c
+++ b/firmware/target/arm/usb-drv-pp502x.c
@@ -294,7 +294,7 @@ struct transfer_descriptor {
     unsigned int reserved;
 } __attribute__ ((packed));
 
-static struct transfer_descriptor _td_array[NUM_ENDPOINTS*2] __attribute((aligned (32)));
+static struct transfer_descriptor _td_array[32] __attribute((aligned (32)));
 static struct transfer_descriptor* td_array;
 
 /* manual: 32.13.1 Endpoint Queue Head (dQH) */
@@ -317,9 +317,15 @@ static const unsigned int pipe2mask[NUM_ENDPOINTS*2] = {
     0x04, 0x040000,
 };
 
+static struct transfer_descriptor* first_td;
+static struct transfer_descriptor* last_td;
+
 /*-------------------------------------------------------------------------*/
 static void transfer_completed(void);
 static int prime_transfer(int endpoint, void* ptr, int len, bool send);
+static void prepare_td(struct transfer_descriptor* td,
+                       struct transfer_descriptor* previous_td,
+                       void *ptr, int len);
 static void bus_reset(void);
 static void init_queue_heads(void);
 static void init_endpoints(void);
@@ -340,6 +346,10 @@ void usb_drv_init(void)
 
     REG_USBMODE = USBMODE_CTRL_MODE_DEVICE;
 
+    /* Force device to full speed */
+    /* See 32.9.5.9.2 */
+    REG_PORTSC1 |= PORTSCX_PORT_FORCE_FULL_SPEED;
+
     td_array = (struct transfer_descriptor*)UNCACHED_ADDR(&_td_array);
     qh_array = (struct queue_head*)UNCACHED_ADDR(&_qh_array);
     init_queue_heads();
@@ -467,6 +477,10 @@ void usb_drv_wait(int endpoint, bool send)
     }
 }
 
+int usb_drv_port_speed(void)
+{
+    return (REG_PORTSC1 & 0x08000000) ? 1 : 0;
+}
 
 void usb_drv_set_address(int address)
 {
@@ -482,44 +496,87 @@ void usb_drv_reset_endpoint(int endpoint, bool send)
     while (REG_ENDPTFLUSH & mask);
 }
 
+int usb_drv_get_last_transfer_length(void)
+{
+    struct transfer_descriptor* current_td = first_td;
+    int length = 0;
+
+    while (!((unsigned int)current_td & DTD_NEXT_TERMINATE)) {
+        if ((current_td->size_ioc_sts & 0xff) != 0)
+            return -1;
+
+        length += current_td->reserved -
+            ((current_td->size_ioc_sts & DTD_PACKET_SIZE) >> DTD_LENGTH_BIT_POS);
+        current_td = (struct transfer_descriptor*)current_td->next_td_ptr;
+    }
+    return length;
+}
+int usb_drv_get_last_transfer_status(void)
+{
+    struct transfer_descriptor* current_td = first_td;
+
+    while (!((unsigned int)current_td & DTD_NEXT_TERMINATE)) {
+        if ((current_td->size_ioc_sts & 0xff) != 0)
+            return current_td->size_ioc_sts & 0xff;
+
+        current_td = (struct transfer_descriptor*)current_td->next_td_ptr;
+    }
+    return 0;
+}
+
 /*-------------------------------------------------------------------------*/
 
 /* manual: 32.14.5.2 */
 static int prime_transfer(int endpoint, void* ptr, int len, bool send)
 {
-    int timeout;
     int pipe = endpoint * 2 + (send ? 1 : 0);
     unsigned int mask = pipe2mask[pipe];
-    struct transfer_descriptor* td = &td_array[pipe];
+    last_td = 0;
     struct queue_head* qh = &qh_array[pipe];
+    static long last_tick;
 
+/*
     if (send && endpoint > EP_CONTROL) {
         logf("usb: sent %d bytes", len);
     }
+*/
 
-    memset(td, 0, sizeof(struct transfer_descriptor));
-    td->next_td_ptr = DTD_NEXT_TERMINATE;
-    td->size_ioc_sts = (len << DTD_LENGTH_BIT_POS) |
-        DTD_STATUS_ACTIVE | DTD_IOC;
-    td->buff_ptr0 = (unsigned int)ptr;
-    td->buff_ptr1 = (unsigned int)ptr + 0x1000;
-    td->buff_ptr2 = (unsigned int)ptr + 0x2000;
-    td->buff_ptr3 = (unsigned int)ptr + 0x3000;
-    td->buff_ptr4 = (unsigned int)ptr + 0x4000;
-    td->reserved = len;
-    qh->dtd.next_td_ptr = (unsigned int)td;
+    if (len==0) {
+        struct transfer_descriptor* new_td = &td_array[0];
+        prepare_td(new_td, 0, ptr, 0);
+
+        last_td = new_td;
+        first_td = new_td;
+    }
+    else {
+        int td_idx = 0;
+        while (len > 0) {
+            int current_transfer_length = MIN(16384,len);
+            struct transfer_descriptor* new_td = &td_array[td_idx];
+            prepare_td(new_td, last_td, ptr, current_transfer_length);
+
+            last_td = new_td;
+            len -= current_transfer_length;
+            td_idx++;
+            ptr += current_transfer_length;
+        }
+        first_td = &td_array[0];
+    }
+
+    qh->dtd.next_td_ptr = (unsigned int)first_td;
     qh->dtd.size_ioc_sts &= ~(QH_STATUS_HALT | QH_STATUS_ACTIVE);
 
     REG_ENDPTPRIME |= mask;
 
-    timeout = 10000;
-    while ((REG_ENDPTPRIME & mask) && --timeout) {
+    last_tick = current_tick;
+    while ((REG_ENDPTPRIME & mask)) {
         if (REG_USBSTS & USBSTS_RESET)
             return -1;
-    }
-    if (!timeout) {
-        logf("prime timeout");
-        return -2;
+
+        if (TIME_AFTER(current_tick, last_tick + HZ/4)) {
+            logf("prime timeout");
+            return -2;
+        }
     }
 
     if (!(REG_ENDPTSTATUS & mask)) {
@@ -529,23 +586,54 @@ static int prime_transfer(int endpoint, void* ptr, int len, bool send)
 
     if (send) {
         /* wait for transfer to finish */
-        timeout = 100000;
-        while ((td->size_ioc_sts & DTD_STATUS_ACTIVE) && --timeout) {
-            if (REG_ENDPTCOMPLETE & mask)
-                REG_ENDPTCOMPLETE |= mask;
-
-            if (REG_USBSTS & USBSTS_RESET)
-                return -4;
-        }
-        if (!timeout) {
-            logf("td never finished");
-            return -5;
+        struct transfer_descriptor* current_td = first_td;
+
+        while (!((unsigned int)current_td & DTD_NEXT_TERMINATE)) {
+            while ((current_td->size_ioc_sts & 0xff) == DTD_STATUS_ACTIVE) {
+                if (REG_ENDPTCOMPLETE & mask)
+                    REG_ENDPTCOMPLETE |= mask;
+
+                /* let the host handle timeouts */
+                if (REG_USBSTS & USBSTS_RESET) {
+                    logf("td interrupted by reset");
+                    return -4;
+                }
+            }
+            if ((current_td->size_ioc_sts & 0xff) != 0) {
+                logf("td failed with error %X",(current_td->size_ioc_sts & 0xff));
+                return -6;
+            }
+            //logf("td finished : %X",current_td->size_ioc_sts & 0xff);
+            current_td=(struct transfer_descriptor*)current_td->next_td_ptr;
         }
+        //logf("all tds done");
     }
 
     return 0;
 }
 
+static void prepare_td(struct transfer_descriptor* td,
+                       struct transfer_descriptor* previous_td,
+                       void *ptr, int len)
+{
+    //logf("adding a td : %d",len);
+    memset(td, 0, sizeof(struct transfer_descriptor));
+    td->next_td_ptr = DTD_NEXT_TERMINATE;
+    td->size_ioc_sts = (len<< DTD_LENGTH_BIT_POS) |
+        DTD_STATUS_ACTIVE | DTD_IOC;
+    td->buff_ptr0 = (unsigned int)ptr;
+    td->buff_ptr1 = ((unsigned int)ptr & 0xfffff000) + 0x1000;
+    td->buff_ptr2 = ((unsigned int)ptr & 0xfffff000) + 0x2000;
+    td->buff_ptr3 = ((unsigned int)ptr & 0xfffff000) + 0x3000;
+    td->buff_ptr4 = ((unsigned int)ptr & 0xfffff000) + 0x4000;
+    td->reserved = len;
+
+    if (previous_td != 0) {
+       previous_td->next_td_ptr=(unsigned int)td;
+       previous_td->size_ioc_sts&=~DTD_IOC;// Only an interrupt on the last one
+    }
+}
+
 static void transfer_completed(void)
 {
     int i;
@@ -557,13 +645,16 @@ static void transfer_completed(void)
     for (i=0; i<NUM_ENDPOINTS; i++) {
         int x;
         for (x=0; x<2; x++) {
+            unsigned int status;
             int pipe = i * 2 + x;
+
             if (mask & pipe2mask[pipe])
                 usb_core_transfer_complete(i, x ? true : false);
                                            
+            status = usb_drv_get_last_transfer_status();
             if ((mask & pipe2mask[pipe]) &&
-                (td_array[pipe].size_ioc_sts & DTD_ERROR_MASK)) {
-                logf("pipe %d err %x", pipe, td_array[pipe].size_ioc_sts & DTD_ERROR_MASK);
+                status & DTD_ERROR_MASK) {
+                logf("pipe %d err %x", pipe, status & DTD_ERROR_MASK);
             }
         }
     }
@@ -600,23 +691,42 @@ static void bus_reset(void)
     if (!(REG_PORTSC1 & PORTSCX_PORT_RESET)) {
         logf("usb: slow reset!");
     }
+
+    logf("PTS : %X",(REG_PORTSC1 & 0xC0000000)>>30);
+    logf("STS : %X",(REG_PORTSC1 & 0x20000000)>>29);
+    logf("PTW : %X",(REG_PORTSC1 & 0x10000000)>>28);
+    logf("PSPD : %X",(REG_PORTSC1 & 0x0C000000)>>26);
+    logf("PFSC : %X",(REG_PORTSC1 & 0x01000000)>>24);
+    logf("PTC : %X",(REG_PORTSC1 & 0x000F0000)>>16);
+    logf("PO  : %X",(REG_PORTSC1 & 0x00002000)>>13);
 }
 
 /* manual: 32.14.4.1 Queue Head Initialization */
 static void init_queue_heads(void)
 {
+    int tx_packetsize;
+    int rx_packetsize;
+
+    if (usb_drv_port_speed()) {
+        rx_packetsize = 512;
+        tx_packetsize = 512;
+    }
+    else {
+        rx_packetsize = 16;
+        tx_packetsize = 16;
+    }
     memset(qh_array, 0, sizeof _qh_array);
 
     /*** control ***/
-    qh_array[EP_CONTROL].max_pkt_length = 512 << QH_MAX_PKT_LEN_POS | QH_IOS;
+    qh_array[EP_CONTROL].max_pkt_length = 64 << QH_MAX_PKT_LEN_POS | QH_IOS;
     qh_array[EP_CONTROL].dtd.next_td_ptr = QH_NEXT_TERMINATE;
-    qh_array[EP_CONTROL+1].max_pkt_length = 512 << QH_MAX_PKT_LEN_POS;
+    qh_array[EP_CONTROL+1].max_pkt_length = 64 << QH_MAX_PKT_LEN_POS;
     qh_array[EP_CONTROL+1].dtd.next_td_ptr = QH_NEXT_TERMINATE;
 
     /*** bulk ***/
-    qh_array[EP_RX*2].max_pkt_length = 512 << QH_MAX_PKT_LEN_POS;
+    qh_array[EP_RX*2].max_pkt_length = rx_packetsize << QH_MAX_PKT_LEN_POS | QH_ZLT_SEL;
     qh_array[EP_RX*2].dtd.next_td_ptr = QH_NEXT_TERMINATE;
-    qh_array[EP_TX*2+1].max_pkt_length = 512 << QH_MAX_PKT_LEN_POS;
+    qh_array[EP_TX*2+1].max_pkt_length = tx_packetsize << QH_MAX_PKT_LEN_POS | QH_ZLT_SEL;
     qh_array[EP_TX*2+1].dtd.next_td_ptr = QH_NEXT_TERMINATE;
 }
 
diff --git a/firmware/target/arm/usb-fw-pp502x.c b/firmware/target/arm/usb-fw-pp502x.c
index 0813ae1c5..5b71fbd 100644
--- a/firmware/target/arm/usb-fw-pp502x.c
+++ b/firmware/target/arm/usb-fw-pp502x.c
@@ -68,6 +68,9 @@ void usb_init_device(void)
 void usb_enable(bool on)
 {
     if (on) {
+#ifdef USE_ROCKBOX_USB
+        usb_core_init();
+#else
         /* until we have native mass-storage mode, we want to reboot on
            usb host connect */
 #if defined(IRIVER_H10) || defined (IRIVER_H10_5GB)
@@ -89,6 +92,7 @@ void usb_enable(bool on)
 
             system_reboot(); /* Reboot */
         }
+#endif  /* USE_ROCKBOX_USB */
     }
     else
         usb_core_exit();
@@ -195,9 +199,9 @@ int usb_detect(void)
         return status;
     }
 
-    /* Wait up to 50 ticks (500ms) before deciding there is no computer
+    /* Wait up to 100 ticks (1s) before deciding there is no computer
        attached. */
-    countdown = 50;
+    countdown = 100;
 
     return status;
 }
diff --git a/firmware/usbstack/usb_core.c b/firmware/usbstack/usb_core.c
index 7e86086..13993f9 100644
--- a/firmware/usbstack/usb_core.c
+++ b/firmware/usbstack/usb_core.c
@@ -23,10 +23,17 @@
 //#define LOGF_ENABLE
 #include "logf.h"
 
-//#define USB_STORAGE
+#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"
@@ -63,21 +70,21 @@ static const struct usb_device_descriptor device_descriptor = {
     .bcdDevice          = 0x0100,
     .iManufacturer      = 1,
     .iProduct           = 2,
-    .iSerialNumber      = 0,
+    .iSerialNumber      = 3,
     .bNumConfigurations = 1
 };
 
 static const struct {
     struct usb_config_descriptor config_descriptor;
     struct usb_interface_descriptor interface_descriptor;
-    struct usb_endpoint_descriptor ep1_hs_in_descriptor;
-    struct usb_endpoint_descriptor ep1_hs_out_descriptor;
-} config_data = 
+    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,
+        .wTotalLength        = sizeof config_data_fs,
         .bNumInterfaces      = 1,
         .bConfigurationValue = 1,
         .iConfiguration      = 0,
@@ -96,12 +103,153 @@ static const struct {
         .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
+    }
+#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
+    }
+#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
+    }
+#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
+    }
+#endif
+},
+config_data_hs = 
+{
+    {
+        .bLength             = sizeof(struct usb_config_descriptor),
+        .bDescriptorType     = USB_DT_CONFIG,
+        .wTotalLength        = sizeof config_data_hs,
+        .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,
@@ -168,7 +316,7 @@ static const struct {
         .bDescriptorType  = USB_DT_ENDPOINT,
         .bEndpointAddress = EP_TX | USB_DIR_IN,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 64,
+        .wMaxPacketSize   = 512,
         .bInterval        = 0
     },
     {
@@ -176,7 +324,7 @@ static const struct {
         .bDescriptorType  = USB_DT_ENDPOINT,
         .bEndpointAddress = EP_RX | USB_DIR_OUT,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-        .wMaxPacketSize   = 64,
+        .wMaxPacketSize   = 512,
         .bInterval        = 0
     }
 #endif
@@ -192,7 +340,7 @@ static const struct {
         .bInterfaceClass    = USB_CLASS_VENDOR_SPEC,
         .bInterfaceSubClass = 255,
         .bInterfaceProtocol = 255,
-        .iInterface         = 3
+        .iInterface         = 4
     },
 
     {
@@ -200,7 +348,6 @@ static const struct {
         .bDescriptorType  = USB_DT_ENDPOINT,
         .bEndpointAddress = EP_RX | USB_DIR_OUT,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-//        .wMaxPacketSize   = 64,
         .wMaxPacketSize   = 512,
         .bInterval        = 0
     },
@@ -209,7 +356,6 @@ static const struct {
         .bDescriptorType  = USB_DT_ENDPOINT,
         .bEndpointAddress = EP_TX | USB_DIR_IN,
         .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-//        .wMaxPacketSize   = 64,
         .wMaxPacketSize   = 512,
         .bInterval        = 0
     }
@@ -228,72 +374,61 @@ static const struct usb_qualifier_descriptor qualifier_descriptor =
     .bNumConfigurations = 1
 };
 
-/* full speed = 12 Mbit */
-static const struct usb_endpoint_descriptor ep1_fs_in_descriptor =
+static struct usb_string_descriptor usb_string_iManufacturer =
 {
-    .bLength          = sizeof(struct usb_endpoint_descriptor),
-    .bDescriptorType  = USB_DT_ENDPOINT,
-    .bEndpointAddress = 1 | USB_DIR_IN,
-    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-    .wMaxPacketSize   = 64,
-    .bInterval        = 0
+    24,
+    USB_DT_STRING,
+    {'R','o','c','k','b','o','x','.','o','r','g'}
 };
 
-static const struct usb_endpoint_descriptor ep1_fs_out_descriptor =
+static struct usb_string_descriptor usb_string_iProduct =
 {
-    .bLength          = sizeof(struct usb_endpoint_descriptor),
-    .bDescriptorType  = USB_DT_ENDPOINT,
-    .bEndpointAddress = 1 | USB_DIR_OUT,
-    .bmAttributes     = USB_ENDPOINT_XFER_BULK,
-    .wMaxPacketSize   = 64,
-    .bInterval        = 0
+    42,
+    USB_DT_STRING,
+    {'R','o','c','k','b','o','x',' ','m','e','d','i','a',' ','p','l','a','y','e','r'}
 };
 
-static const struct usb_endpoint_descriptor* ep_descriptors[4] =
+static struct usb_string_descriptor usb_string_iSerial =
 {
-    &config_data.ep1_hs_in_descriptor,
-    &config_data.ep1_hs_out_descriptor,
-    &ep1_fs_in_descriptor,
-    &ep1_fs_out_descriptor
+    34, 
+    USB_DT_STRING, 
+    {'0','0','0','0','0','0','0','0','0','0','0','0','0','0','0','0'}
 };
 
+
+/* Generic for all targets */
+
 /* this is stringid #0: languages supported */
-static const struct usb_string_descriptor lang_descriptor =
+static struct usb_string_descriptor lang_descriptor =
 {
-    sizeof(struct usb_string_descriptor),
+    4,
     USB_DT_STRING,
     {0x0409} /* LANGID US English */
 };
 
-/* this is stringid #1 and up: the actual strings */
-static const struct {
-    unsigned char size;
-    unsigned char type;
-    unsigned short string[32];
-} usb_strings[] =
+static struct usb_string_descriptor usb_string_usb_benchmark =
 {
-    {
-        24,
-        USB_DT_STRING,
-        {'R','o','c','k','b','o','x','.','o','r','g'}
-    },
-    {
-        42,
-        USB_DT_STRING,
-        {'R','o','c','k','b','o','x',' ','m','e','d','i','a',' ','p','l','a','y','e','r'}
-    },
-    {
-        40,
-        USB_DT_STRING,
-        {'B','u','l','k',' ','t','e','s','t',' ','i','n','t','e','r','f','a','c','e'}
-    },
-    {
-        28,
-        USB_DT_STRING,
-        {'C','h','a','r','g','i','n','g',' ','o','n','l','y'}
-    }
+    40,
+    USB_DT_STRING,
+    {'B','u','l','k',' ','t','e','s','t',' ','i','n','t','e','r','f','a','c','e'}
 };
 
+static struct usb_string_descriptor usb_string_charging_only =
+{
+    28,
+    USB_DT_STRING,
+    {'C','h','a','r','g','i','n','g',' ','o','n','l','y'}
+};
+
+static struct usb_string_descriptor* usb_strings[] =
+{
+   &lang_descriptor,
+   &usb_string_iManufacturer,
+   &usb_string_iProduct,
+   &usb_string_iSerial,
+   &usb_string_usb_benchmark,
+   &usb_string_charging_only
+};
 
 static int usb_address = 0;
 static bool initialized = false;
@@ -310,11 +445,45 @@ static void usb_core_thread(void);
 
 static void ack_control(struct usb_ctrlrequest* req);
 
+#ifdef IPOD_ARCH
+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'};
+#ifdef IPOD_VIDEO
+    uint32_t* serial = (uint32_t*)(0x20004034);
+#else
+    uint32_t* serial = (uint32_t*)(0x20002034);
+#endif
+
+    /* We need to convert from a little-endian 64-bit int
+       into a utf-16 string of hex characters */
+    short* p = &usb_string_iSerial.wString[15];
+    uint32_t x;
+    int i,j;
+
+    for (i = 0; i < 2; i++)
+    {
+       x = serial[i];
+       for (j=0;j<8;j++)
+       {
+          *p-- = hex[x & 0xf];
+          x >>= 4;
+       }
+    }
+
+}
+#endif
+
 void usb_core_init(void)
 {
     if (initialized)
         return;
 
+#ifdef IPOD_ARCH
+    set_serial_descriptor();
+#endif
+
     queue_init(&usbcore_queue, false);
     usb_drv_init();
 #ifdef USB_STORAGE
@@ -469,43 +638,25 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
                     break;
 
                 case USB_DT_CONFIG:
-                    ptr = &config_data;
-                    size = sizeof config_data;
-                    break;
-
-                case USB_DT_STRING:
-                    switch (index) {
-                        case 0: /* lang descriptor */
-                            ptr = &lang_descriptor;
-                            size = sizeof lang_descriptor;
-                            break;
-
-                        default:
-                            if ((unsigned)index <= (sizeof(usb_strings)/sizeof(usb_strings[0]))) {
-                                index -= 1;
-                                ptr = &usb_strings[index];
-                                size = usb_strings[index].size;
-                            }
-                            else {
-                                logf("bad string id %d", index);
-                                usb_drv_stall(EP_CONTROL, true);
-                            }
-                            break;
+                    if(usb_drv_port_speed())
+                    {
+                        ptr = &config_data_hs;
+                        size = sizeof config_data_hs;
+                    }
+                    else
+                    {
+                        ptr = &config_data_fs;
+                        size = sizeof config_data_fs;
                     }
                     break;
 
-                case USB_DT_INTERFACE:
-                    ptr = &config_data.interface_descriptor;
-                    size = sizeof config_data.interface_descriptor;
-                    break;
-
-                case USB_DT_ENDPOINT:
-                    if (index <= NUM_ENDPOINTS) {
-                        ptr = &ep_descriptors[index];
-                        size = sizeof ep_descriptors[index];
+                case USB_DT_STRING:
+                    if ((unsigned)index < (sizeof(usb_strings)/sizeof(struct usb_string_descriptor*))) {
+                        ptr = usb_strings[index];
+                        size = usb_strings[index]->bLength;
                     }
                     else {
-                        logf("bad endpoint %d", index);
+                        logf("bad string id %d", index);
                         usb_drv_stall(EP_CONTROL, true);
                     }
                     break;
@@ -515,12 +666,6 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
                     size = sizeof qualifier_descriptor;
                     break;
 
-/*
-  case USB_DT_OTHER_SPEED_CONFIG:
-  ptr = &other_speed_descriptor;
-  size = sizeof other_speed_descriptor;
-  break;
-*/
                 default:
                     logf("bad desc %d", req->wValue >> 8);
                     usb_drv_stall(EP_CONTROL, true);
diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c
index b852475..1fbfe52 100644
--- a/firmware/usbstack/usb_storage.c
+++ b/firmware/usbstack/usb_storage.c
@@ -24,6 +24,7 @@
 #include "logf.h"
 #include "ata.h"
 #include "hotswap.h"
+#include "disk.h"
 
 #define SECTOR_SIZE 512
 
@@ -40,14 +41,20 @@
 #define SCSI_TEST_UNIT_READY      0x00
 #define SCSI_INQUIRY              0x12
 #define SCSI_MODE_SENSE           0x1a
+#define SCSI_REQUEST_SENSE        0x03
 #define SCSI_ALLOW_MEDIUM_REMOVAL 0x1e
 #define SCSI_READ_CAPACITY        0x25
+#define SCSI_READ_FORMAT_CAPACITY 0x23
 #define SCSI_READ_10              0x28
 #define SCSI_WRITE_10             0x2a
+#define SCSI_START_STOP_UNIT      0x1b
 
 #define SCSI_STATUS_GOOD            0x00
+#define SCSI_STATUS_FAIL            0x01
 #define SCSI_STATUS_CHECK_CONDITION 0x02
 
+#define SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA 0x02000000
+
 
 struct inquiry_data {
     unsigned char DeviceType;
@@ -62,6 +69,20 @@ struct inquiry_data {
     unsigned char ProductRevisionLevel[4];
 } __attribute__ ((packed));
 
+struct sense_data {
+    unsigned char ResponseCode;
+    unsigned char Obsolete;
+    unsigned char filemark_eom_ili_sensekey;
+    unsigned int Information;
+    unsigned char AdditionalSenseLength;
+    unsigned int  CommandSpecificInformation;
+    unsigned char AdditionalSenseCode;
+    unsigned char AdditionalSenseCodeQualifier;
+    unsigned char FieldReplaceableUnitCode;
+    unsigned char SKSV;
+    unsigned short SenseKeySpecific;
+} __attribute__ ((packed));
+
 struct command_block_wrapper {
     unsigned int signature;
     unsigned int tag;
@@ -84,26 +105,34 @@ struct capacity {
     unsigned int block_size;
 } __attribute__ ((packed));
 
+struct format_capacity {
+    unsigned int following_length;
+    unsigned int block_count;
+    unsigned int block_size;
+} __attribute__ ((packed));
+
 /* the ARC USB controller can at most buffer 16KB unaligned data */
-static unsigned char _transfer_buffer[16384];
+static unsigned char _transfer_buffer[16384*8] __attribute((aligned (4096)));
 static unsigned char* transfer_buffer;
-static struct inquiry_data _inquiry;
+static struct inquiry_data _inquiry CACHEALIGN_ATTR;
 static struct inquiry_data* inquiry;
-static struct capacity _capacity_data;
+static struct capacity _capacity_data CACHEALIGN_ATTR;
 static struct capacity* capacity_data;
-
-//static unsigned char partial_sector[SECTOR_SIZE];
+static struct format_capacity _format_capacity_data CACHEALIGN_ATTR;
+static struct format_capacity* format_capacity_data;
+static struct sense_data _sense_data CACHEALIGN_ATTR;
+static struct sense_data *sense_data;
 
 static struct {
     unsigned int sector;
-    unsigned int offset; /* if partial sector */
     unsigned int count;
     unsigned int tag;
+    unsigned int lun;
 } current_cmd;
 
- void handle_scsi(struct command_block_wrapper* cbw);
- void send_csw(unsigned int tag, int status);
-static void identify2inquiry(void);
+static void handle_scsi(struct command_block_wrapper* cbw);
+static void send_csw(unsigned int tag, int status);
+static void identify2inquiry(int lun);
 
 static enum {
     IDLE,
@@ -117,7 +146,10 @@ void usb_storage_init(void)
     inquiry = (void*)UNCACHED_ADDR(&_inquiry);
     transfer_buffer = (void*)UNCACHED_ADDR(&_transfer_buffer);
     capacity_data = (void*)UNCACHED_ADDR(&_capacity_data);
-    identify2inquiry();
+    format_capacity_data = (void*)UNCACHED_ADDR(&_format_capacity_data);
+    sense_data = (void*)UNCACHED_ADDR(&_sense_data);
+    state = IDLE;
+    logf("usb_storage_init done");
 }
 
 /* called by usb_core_transfer_complete() */
@@ -128,21 +160,44 @@ void usb_storage_transfer_complete(int endpoint)
     switch (endpoint) {
         case EP_RX:
             //logf("ums: %d bytes in", length);
-            switch (state) {
-                case IDLE:
-                    handle_scsi(cbw);
-                    break;
-
-                default:
-                    break;
+            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);
+                }
+                else
+                {
+                    logf("Transfer failed %X",usb_drv_get_last_transfer_status());
+                    send_csw(current_cmd.tag, SCSI_STATUS_CHECK_CONDITION);
+                }
             }
-
-            /* re-prime endpoint */
-            usb_drv_recv(EP_RX, transfer_buffer, sizeof _transfer_buffer);
+            else
+            {
+                state = SENDING;
+                handle_scsi(cbw);
+            }
+            
             break;
 
         case EP_TX:
-            //logf("ums: %d bytes out", length);
+            //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);
+            }
+
             break;
     }
 }
@@ -156,7 +211,7 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
 
     switch (req->bRequest) {
         case USB_BULK_GET_MAX_LUN: {
-            static char maxlun = 0;
+            static char maxlun = NUM_VOLUMES - 1;
             logf("ums: getmaxlun");
             usb_drv_send(EP_CONTROL, UNCACHED_ADDR(&maxlun), 1);
             usb_drv_recv(EP_CONTROL, NULL, 0); /* ack */
@@ -174,8 +229,9 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
 
         case USB_REQ_SET_CONFIGURATION:
             logf("ums: set config");
-            /* prime rx endpoint */
-            usb_drv_recv(EP_RX, transfer_buffer, sizeof _transfer_buffer);
+            /* prime rx endpoint. We only need room for commands */
+            state = IDLE;
+            usb_drv_recv(EP_RX, transfer_buffer, 1024);
             handled = true;
             break;
     }
@@ -185,50 +241,138 @@ bool usb_storage_control_request(struct usb_ctrlrequest* req)
 
 /****************************************************************************/
 
-void handle_scsi(struct command_block_wrapper* cbw)
+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 char lun = cbw->lun;
+    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);
+    }
+#else
+    block_size = SECTOR_SIZE;
+    sectors_per_transfer=(sizeof _transfer_buffer/ block_size);
+#endif
+
+#ifdef MAX_LOG_SECTOR_SIZE
+    block_size_mult = disk_sector_multiplier;
+#endif
 
     switch (cbw->command_block[0]) {
         case SCSI_TEST_UNIT_READY:
-            logf("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);
+#else
             send_csw(cbw->tag, SCSI_STATUS_GOOD);
+#endif
             break;
 
         case SCSI_INQUIRY:
-            logf("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);
             break;
 
+        case SCSI_REQUEST_SENSE: {
+            sense_data->ResponseCode=0x70;
+            sense_data->filemark_eom_ili_sensekey=2;
+            sense_data->Information=2;
+            sense_data->AdditionalSenseLength=10;
+            sense_data->CommandSpecificInformation=0;
+            sense_data->AdditionalSenseCode=0x3a;
+            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);
+            break;
+        }
+
         case SCSI_MODE_SENSE: {
             static unsigned char sense_data[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-            logf("scsi mode_sense");
+            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);
             break;
         }
 
+        case SCSI_START_STOP_UNIT:
+            logf("scsi start_stop unit %d",lun);
+            send_csw(cbw->tag, SCSI_STATUS_GOOD);
+            break;
+
         case SCSI_ALLOW_MEDIUM_REMOVAL:
-            logf("scsi allow_medium_removal");
+            logf("scsi allow_medium_removal %d",lun);
+            send_csw(cbw->tag, 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_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);
             break;
+        }
 
         case SCSI_READ_CAPACITY: {
-            logf("scsi read_capacity");
-#ifdef HAVE_FLASH_STORAGE
-            tCardInfo* cinfo = card_get_info(0);
-            capacity_data->block_count = htobe32(cinfo->numblocks);
-            capacity_data->block_size = htobe32(cinfo->blocksize);
+            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();
-            capacity_data->block_count = htobe32(identify[60] << 16 | identify[61]);
-            capacity_data->block_size = htobe32(SECTOR_SIZE);
+            /* 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));
@@ -237,43 +381,71 @@ void handle_scsi(struct command_block_wrapper* cbw)
         }
 
         case SCSI_READ_10:
-            current_cmd.sector =
-                cbw->command_block[2] << 24 |
+            current_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 =
-                cbw->command_block[7] << 16 |
-                cbw->command_block[8];
-            current_cmd.offset = 0;
+                cbw->command_block[5] );
+            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);
 
-            /* too much? */
-            if (current_cmd.count > (sizeof _transfer_buffer / SECTOR_SIZE)) {
-                send_csw(current_cmd.tag, SCSI_STATUS_CHECK_CONDITION);
-                break;
+            //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);
 
-            ata_read_sectors(IF_MV2(0,) current_cmd.sector, current_cmd.count,
-                             transfer_buffer);
-            state = SENDING;
-            usb_drv_send(EP_TX, transfer_buffer,
-                         MIN(current_cmd.count * SECTOR_SIZE, length));
+            if(current_cmd.count*block_size > sizeof(_transfer_buffer)) {
+                send_csw(current_cmd.tag, SCSI_STATUS_CHECK_CONDITION);
+            }
+            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);
+            }
             break;
 
         case SCSI_WRITE_10:
-            logf("scsi write10");
+            //logf("scsi write10");
+            current_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 *
+               (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);
+            }
+            else {
+                usb_drv_recv(EP_RX, transfer_buffer,
+                             current_cmd.count*block_size);
+                state = RECEIVING;
+            }
+
             break;
 
         default:
-            logf("scsi unknown cmd %x", cbw->command_block[0]);
+            logf("scsi unknown cmd %x",cbw->command_block[0x0]);
+            usb_drv_stall(EP_TX, true);
+            send_csw(current_cmd.tag, SCSI_STATUS_GOOD);
             break;
     }
 }
 
-void send_csw(unsigned int tag, int status)
+static void send_csw(unsigned int tag, int status)
 {
     static struct command_status_wrapper _csw;
     struct command_status_wrapper* csw = UNCACHED_ADDR(&_csw);
@@ -287,16 +459,23 @@ void send_csw(unsigned int tag, int status)
 }
 
 /* convert ATA IDENTIFY to SCSI INQUIRY */
-static void identify2inquiry(void)
+static void identify2inquiry(int lun)
 {
-    unsigned int i;
 #ifdef HAVE_FLASH_STORAGE
-    for (i=0; i<8; i++)
-        inquiry->VendorId[i] = i + 'A';
-
-    for (i=0; i<8; i++)
-        inquiry->ProductId[i] = i + 'm';
+    if(lun==0)
+    {
+        memcpy(&inquiry->VendorId,"Rockbox ",8);
+        memcpy(&inquiry->ProductId,"Internal Storage",16);
+        memcpy(&inquiry->ProductRevisionLevel,"0.00",4);
+    }
+    else
+    {
+        memcpy(&inquiry->VendorId,"Rockbox ",8);
+        memcpy(&inquiry->ProductId,"SD Card Slot    ",16);
+        memcpy(&inquiry->ProductRevisionLevel,"0.00",4);
+    }
 #else
+    unsigned int i;
     unsigned short* dest;
     unsigned short* src;
     unsigned short* identify = ata_get_identify();
@@ -310,22 +489,27 @@ static void identify2inquiry(void)
     src = (unsigned short*)&identify[27];
     dest = (unsigned short*)&inquiry->VendorId;
     for (i=0;i<4;i++)
-        dest[i] = src[i];
+        dest[i] = htobe16(src[i]);
 
     src = (unsigned short*)&identify[27+8];
     dest = (unsigned short*)&inquiry->ProductId;
     for (i=0;i<8;i++)
-        dest[i] = src[i];
+        dest[i] = htobe16(src[i]);
     
     src = (unsigned short*)&identify[23];
     dest = (unsigned short*)&inquiry->ProductRevisionLevel;
     for (i=0;i<2;i++)
-        dest[i] = src[i];
+        dest[i] = htobe16(src[i]);
 #endif
 
     inquiry->DeviceType = DIRECT_ACCESS_DEVICE;
     inquiry->AdditionalLength = 0x1f;
     inquiry->Versions = 3; /* ANSI SCSI level 2 */
     inquiry->Format   = 3; /* ANSI SCSI level 2 INQUIRY format */
+
+#ifdef HAVE_HOTSWAP
+    inquiry->DeviceTypeModifier = DEVICE_REMOVABLE;
+#endif
+
 }