Index: firmware/usbstack/usb_storage.c
===================================================================
--- firmware/usbstack/usb_storage.c	(revision 21065)
+++ firmware/usbstack/usb_storage.c	(working copy)
@@ -56,12 +56,10 @@
  */
 #define READ_BUFFER_SIZE (1024*64)
 
-#if (CONFIG_STORAGE & STORAGE_SD)
 #define WRITE_BUFFER_SIZE (1024*64)
-#else
-#define WRITE_BUFFER_SIZE (1024*24)
-#endif
 
+#define DELAYED_SLOTS   10
+
 #define ALLOCATE_BUFFER_SIZE (2*MAX(READ_BUFFER_SIZE,WRITE_BUFFER_SIZE))
 
 /* bulk-only class specific requests */
@@ -292,8 +290,117 @@
     SENDING_CSW
 } state = WAITING_FOR_COMMAND;
 
+struct delayed_write
+{
+    bool available;
+    unsigned int disk;
+    unsigned int sector;
+    unsigned int count;
+    unsigned char *data;
+} delayed_write_array[DELAYED_SLOTS], *current_dw;
+
+static unsigned int delayed_write_slots;
+static struct event_queue delayed_write_queue;
+static struct event_queue delayed_write_completion_queue;
+static long delayed_write_stack[(DEFAULT_STACK_SIZE)/sizeof(long)];
+static const char delayed_write_thread_name[]="delayed_write";
+
+
+static struct delayed_write *get_delayed_write_slot(void)
+{
+    int i;
+    while(delayed_write_slots==DELAYED_SLOTS)
+    {
+        struct queue_event ev;
+        logf("waiting for free slot");
+        queue_wait(&delayed_write_completion_queue, &ev);
+    }
+    for(i=0;i<DELAYED_SLOTS;i++)
+    {
+        if(delayed_write_array[i].available)
+        {
+            delayed_write_array[i].available=false;
+            delayed_write_slots++;
+            logf("write slots in use : %d",delayed_write_slots);
+            logf("allocated : %d",i);
+            return &delayed_write_array[i];
+        }
+    }
+    return NULL;
+}
+
+static void delayed_write_flush(void)
+{
+    while(delayed_write_slots>0)
+    {
+        struct queue_event ev;
+        queue_wait(&delayed_write_completion_queue, &ev);
+    }
+}
+
+static void delayed_write_do(struct delayed_write *dw)
+{
+    logf("asking to write %d blocks",dw->count);
+    queue_post(&delayed_write_queue, 0, (intptr_t)dw);
+}
+
+static void delayed_write_thread(void)
+{
+    struct queue_event ev;
+    while(1)
+    {
+        logf("waiting for write");
+        queue_wait(&delayed_write_queue, &ev);
+        if(ev.data!=0)
+        {
+            struct delayed_write *dw=(struct delayed_write *)ev.data;
+            logf("writing %d blocks",dw->count);
+            int result = storage_write_sectors(dw->disk,dw->sector,
+                                               dw->count,dw->data);
+            if(result!=0)
+            {
+                /* TODO : handle this */
+            }
+            dw->available=true;
+            delayed_write_slots--;
+            queue_post(&delayed_write_completion_queue, 0, 0);
+            logf("write slots in use : %d",delayed_write_slots);
+        }
+    }
+}
+
+static void delayed_write_init_data(unsigned char *buffer)
+{
+    int i;
+    for(i=0;i<DELAYED_SLOTS;i++)
+    {
+        delayed_write_array[i].data=buffer+(i*1024*64);
+    }
+}
+static void delayed_write_init(void)
+{
+    static int init_done=0;
+    int i;
+
+    if(init_done!=0) return;
+
+    queue_init(&delayed_write_queue, false);
+    queue_init(&delayed_write_completion_queue, false);
+
+    for(i=0;i<DELAYED_SLOTS;i++)
+    {
+        delayed_write_array[i].available=true;
+    }
+    create_thread(delayed_write_thread, delayed_write_stack,
+                  sizeof(delayed_write_stack), 0, delayed_write_thread_name
+                  IF_PRIO(, PRIORITY_BACKGROUND) IF_COP(, CPU));
+
+    init_done=1;
+}
+
 #ifdef TOSHIBA_GIGABEAT_S
 
+init_done=1;
 /* The Gigabeat S factory partition table contains invalid values for the
    "active" flag in the MBR.  This prevents at least the Linux kernel from
    accepting the partition table, so we fix it on-the-fly. */
@@ -332,7 +439,7 @@
         }
     }
     if(canrelease) {
-        logf("scsi release ata");
+        //logf("scsi release ata");
         usb_release_exclusive_storage();
     }
 }
@@ -340,7 +447,7 @@
 #ifdef HAVE_HOTSWAP
 void usb_storage_notify_hotswap(int volume,bool inserted)
 {
-    logf("notify %d",inserted);
+    //logf("notify %d",inserted);
     if(inserted && check_disk_present(IF_MV(volume))) {
         ejected[volume] = false;
     }
@@ -356,6 +463,7 @@
 /* called by usb_core_init() */
 void usb_storage_init(void)
 {
+    delayed_write_init();
     logf("usb_storage_init done");
 }
 
@@ -403,7 +511,7 @@
 
 void usb_storage_init_connection(void)
 {
-    logf("ums: set config");
+    //logf("ums: set config");
     /* prime rx endpoint. We only need room for commands */
     state = WAITING_FOR_COMMAND;
 
@@ -428,6 +536,7 @@
     audio_buffer = audio_get_buffer(false,&bufsize);
     cbw_buffer = (void *)UNCACHED_ADDR((unsigned int)(audio_buffer+31) & 0xffffffe0);
     tb.transfer_buffer = cbw_buffer + 1024;
+    delayed_write_init_data(tb.transfer_buffer+ALLOCATE_BUFFER_SIZE);
     cpucache_invalidate();
 #ifdef USB_USE_RAMDISK
     ramdisk_buffer = tb.transfer_buffer + ALLOCATE_BUFFER_SIZE;
@@ -452,7 +561,7 @@
 
 void usb_storage_disconnect(void)
 {
-    /* Empty for now */
+    delayed_write_flush();
 }
 
 /* called by usb_core_transfer_complete() */
@@ -465,12 +574,12 @@
     switch(state) {
         case RECEIVING_BLOCKS:
             if(dir==USB_DIR_IN) {
-                logf("IN received in RECEIVING");
+                //logf("IN received in RECEIVING");
             }
-            logf("scsi write %d %d", cur_cmd.sector, cur_cmd.count);
+            //logf("scsi write %d %d", cur_cmd.sector, cur_cmd.count);
             if(status==0) {
-                if((unsigned int)length!=(SECTOR_SIZE* cur_cmd.count)
-                  && (unsigned int)length!=WRITE_BUFFER_SIZE) {
+                if((unsigned int)length!=(current_dw->count*SECTOR_SIZE))
+                {
                     logf("unexpected length :%d",length);
                 }
 
@@ -478,39 +587,22 @@
                              (WRITE_BUFFER_SIZE/SECTOR_SIZE);
                 unsigned int next_count = cur_cmd.count -
                              MIN(cur_cmd.count,WRITE_BUFFER_SIZE/SECTOR_SIZE);
-                int next_select = !cur_cmd.data_select;
 
+                delayed_write_do(current_dw);
                 if(next_count!=0) {
                     /* Ask the host to send more, to the other buffer */
-                    receive_block_data(cur_cmd.data[next_select],
-                                       MIN(WRITE_BUFFER_SIZE,next_count*SECTOR_SIZE));
+                    current_dw=get_delayed_write_slot();
+                    current_dw->disk=cur_cmd.lun;
+                    current_dw->count=MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE,next_count);
+                    current_dw->sector=next_sector;
+                    receive_block_data(current_dw->data,current_dw->count*SECTOR_SIZE);
                 }
 
-                /* Now write the data that just came in, while the host is
-                   sending the next bit */
-#ifdef USB_USE_RAMDISK
-                memcpy(ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
-                        cur_cmd.data[cur_cmd.data_select],
-                        MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE);
-#else
-                int result = storage_write_sectors(cur_cmd.lun,
-                        cur_cmd.sector,
-                        MIN(WRITE_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;
-                    cur_sense_data.asc=ASC_WRITE_ERROR;
-                    cur_sense_data.ascq=0;
-                    break;
-                }
-#endif
                 if(next_count==0) {
                     send_csw(UMS_STATUS_GOOD);
                 }
 
                 /* Switch buffers for the next one */
-                cur_cmd.data_select=!cur_cmd.data_select;
 
                 cur_cmd.sector = next_sector;
                 cur_cmd.count = next_count;
@@ -527,7 +619,7 @@
             break;
         case WAITING_FOR_COMMAND:
             if(dir==USB_DIR_IN) {
-                logf("IN received in WAITING_FOR_COMMAND");
+                //logf("IN received in WAITING_FOR_COMMAND");
             }
             //logf("command received");
             if(letoh32(cbw->signature) == CBW_SIGNATURE) {
@@ -540,7 +632,7 @@
             break;
         case SENDING_CSW:
             if(dir==USB_DIR_OUT) {
-                logf("OUT received in SENDING_CSW");
+                //logf("OUT received in SENDING_CSW");
             }
             //logf("csw sent, now go back to idle");
             state = WAITING_FOR_COMMAND;
@@ -557,7 +649,7 @@
             break;
         case SENDING_RESULT:
             if(dir==USB_DIR_OUT) {
-                logf("OUT received in SENDING");
+                //logf("OUT received in SENDING");
             }
             if(status==0) {
                 //logf("data sent, now send csw");
@@ -575,13 +667,13 @@
             break;
         case SENDING_FAILED_RESULT:
             if(dir==USB_DIR_OUT) {
-                logf("OUT received in SENDING");
+                //logf("OUT received in SENDING");
             }
             send_csw(UMS_STATUS_FAIL);
             break;
         case SENDING_BLOCKS:
             if(dir==USB_DIR_OUT) {
-                logf("OUT received in SENDING");
+                //logf("OUT received in SENDING");
             }
             if(status==0) {
                 if(cur_cmd.count==0) {
@@ -618,7 +710,7 @@
 #else
             *tb.max_lun = NUM_VOLUMES - 1;
 #endif
-            logf("ums: getmaxlun");
+            //logf("ums: getmaxlun");
             usb_drv_send(EP_CONTROL, tb.max_lun, 1);
             usb_drv_recv(EP_CONTROL, NULL, 0); /* ack */
             handled = true;
@@ -626,7 +718,7 @@
         }
 
         case USB_BULK_RESET_REQUEST:
-            logf("ums: bulk reset");
+            //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
@@ -721,9 +813,14 @@
     cur_cmd.lun = lun;
     cur_cmd.cur_cmd = cbw->command_block[0];
 
+    if(cbw->command_block[0]!=SCSI_WRITE_10)
+    {
+        delayed_write_flush();
+    }
+
     switch (cbw->command_block[0]) {
         case SCSI_TEST_UNIT_READY:
-            logf("scsi test_unit_ready %d",lun);
+            //logf("scsi test_unit_ready %d",lun);
             if(!usb_exclusive_storage()) {
                 send_csw(UMS_STATUS_FAIL);
                 cur_sense_data.sense_key=SENSE_NOT_READY;
@@ -743,7 +840,7 @@
             break;
 
         case SCSI_REPORT_LUNS: {
-            logf("scsi report luns %d",lun);
+            //logf("scsi report luns %d",lun);
             int allocation_length=0;
             int i;
             allocation_length|=(cbw->command_block[6]<<24);
@@ -767,7 +864,7 @@
         }
 
         case SCSI_INQUIRY:
-            logf("scsi inquiry %d",lun);
+            //logf("scsi inquiry %d",lun);
             fill_inquiry(IF_MV(lun));
             length = MIN(length, cbw->command_block[4]);
             send_command_result(tb.inquiry,
@@ -786,7 +883,7 @@
             tb.sense_data->FieldReplaceableUnitCode=0;
             tb.sense_data->SKSV=0;
             tb.sense_data->SenseKeySpecific=0;
-            logf("scsi request_sense %d",lun);
+            //logf("scsi request_sense %d",lun);
             send_command_result(tb.sense_data,
                                 MIN(sizeof(struct sense_data), length));
             break;
@@ -802,7 +899,7 @@
             }
             /*unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;*/
             unsigned char page_code = cbw->command_block[2] & 0x3f;
-            logf("scsi mode_sense_10 %d %X",lun,page_code);
+            //logf("scsi mode_sense_10 %d %X",lun,page_code);
             switch(page_code) {
                 case 0x3f:
                     tb.ms_data_10->mode_data_length =
@@ -857,7 +954,7 @@
             }
             /*unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;*/
             unsigned char page_code = cbw->command_block[2] & 0x3f;
-            logf("scsi mode_sense_6 %d %X",lun,page_code);
+            //logf("scsi mode_sense_6 %d %X",lun,page_code);
             switch(page_code) {
                 case 0x3f:
                     /* All supported pages. */
@@ -902,15 +999,15 @@
         }
 
         case SCSI_START_STOP_UNIT:
-            logf("scsi start_stop unit %d",lun);
+            //logf("scsi start_stop unit %d",lun);
             if((cbw->command_block[4] & 0xf0) == 0) /*load/eject bit is valid*/
             { /* Process start and eject bits */
-                logf("scsi load/eject");
+                //logf("scsi load/eject");
                 if((cbw->command_block[4] & 0x01) == 0) /* Don't start */
                 {
                     if((cbw->command_block[4] & 0x02) != 0) /* eject */
                     {
-                        logf("scsi eject");
+                        //logf("scsi eject");
                         ejected[lun]=true;
                     }
                 }
@@ -919,7 +1016,7 @@
             break;
 
         case SCSI_ALLOW_MEDIUM_REMOVAL:
-            logf("scsi allow_medium_removal %d",lun);
+            //logf("scsi allow_medium_removal %d",lun);
             if((cbw->command_block[4] & 0x03) == 0) {
                 locked[lun]=false;
                 queue_broadcast(SYS_USB_LUN_LOCKED, (lun<<16)+0);
@@ -932,7 +1029,7 @@
             break;
 
         case SCSI_READ_FORMAT_CAPACITY: {
-            logf("scsi read_format_capacity %d",lun);
+            //logf("scsi read_format_capacity %d",lun);
             if(lun_present) {
                 tb.format_capacity_data->following_length=htobe32(8);
                 /* "block count" actually means "number of last block" */
@@ -956,7 +1053,7 @@
         }
 
         case SCSI_READ_CAPACITY: {
-            logf("scsi read_capacity %d",lun);
+            //logf("scsi read_capacity %d",lun);
 
             if(lun_present) {
                 /* "block count" actually means "number of last block" */
@@ -978,7 +1075,7 @@
         }
 
         case SCSI_READ_10:
-            logf("scsi read10 %d",lun);
+            //logf("scsi read10 %d",lun);
             if(!lun_present) {
                 send_command_failed_result();
                 cur_sense_data.sense_key=SENSE_NOT_READY;
@@ -1029,7 +1126,7 @@
             break;
 
         case SCSI_WRITE_10:
-            logf("scsi write10 %d",lun);
+            //logf("scsi write10 %d",lun);
             if(!lun_present) {
                 send_csw(UMS_STATUS_FAIL);
                 cur_sense_data.sense_key=SENSE_NOT_READY;
@@ -1058,13 +1155,16 @@
                 cur_sense_data.ascq=0;
             }
             else {
-                receive_block_data(cur_cmd.data[0],
-                        MIN(WRITE_BUFFER_SIZE, cur_cmd.count*SECTOR_SIZE));
+                current_dw=get_delayed_write_slot();
+                current_dw->disk=cur_cmd.lun;
+                current_dw->count=MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE,cur_cmd.count);
+                current_dw->sector=cur_cmd.sector;
+                receive_block_data(current_dw->data,current_dw->count*SECTOR_SIZE);
             }
             break;
 
         default:
-            logf("scsi unknown cmd %x",cbw->command_block[0x0]);
+            //logf("scsi unknown cmd %x",cbw->command_block[0x0]);
             send_csw(UMS_STATUS_FAIL);
             cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
             cur_sense_data.asc=ASC_INVALID_COMMAND;