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 */
@@ -86,6 +84,8 @@
 #define SCSI_WRITE_10             0x2a
 #define SCSI_START_STOP_UNIT      0x1b
 #define SCSI_REPORT_LUNS          0xa0
+#define SCSI_SYNCHRONIZE_CACHE_10 0x35
+#define SCSI_SYNCHRONIZE_CACHE_16 0x91
 
 #define UMS_STATUS_GOOD            0x00
 #define UMS_STATUS_FAIL            0x01
@@ -202,6 +202,21 @@
     struct mode_sense_bdesc_shortlba block_descriptor;
 } __attribute__ ((packed));
 
+struct caching_page {
+    unsigned char mode_hdr;
+    unsigned char page_length;
+    unsigned char attributes;
+    unsigned char retention;
+    unsigned short disable_prefetch_length;
+    unsigned short minimum_prefetch;
+    unsigned short maximum_prefetch;
+    unsigned short maximum_prefetch_ceiling;
+    unsigned char cache_attributes;
+    unsigned char cache_segments;
+    unsigned short cache_segment_size;
+    unsigned int reserved;
+} __attribute__ ((packed));
+
 struct command_block_wrapper {
     unsigned int signature;
     unsigned int tag;
@@ -292,8 +307,125 @@
     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 bool delayed_write_error;
+static unsigned char last_error_response_code;
+
+
+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;
+    static int counter=0;
+    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)
+            {
+                delayed_write_error=true;
+                cur_sense_data.sense_key=SENSE_MEDIUM_ERROR;
+                cur_sense_data.asc=ASC_WRITE_ERROR;
+                cur_sense_data.ascq=0;
+
+            }
+            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;
+
+    delayed_write_error=false;
+    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. */
@@ -356,6 +488,7 @@
 /* called by usb_core_init() */
 void usb_storage_init(void)
 {
+    delayed_write_init();
     logf("usb_storage_init done");
 }
 
@@ -428,6 +561,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,96 +586,20 @@
 
 void usb_storage_disconnect(void)
 {
-    /* Empty for now */
+    delayed_write_flush();
 }
 
 /* called by usb_core_transfer_complete() */
 void usb_storage_transfer_complete(int ep,int dir,int status,int length)
 {
     (void)ep;
+    (void)dir;
     struct command_block_wrapper* cbw = (void*)cbw_buffer;
 
     //logf("transfer result %X %d", status, length);
-    switch(state) {
-        case RECEIVING_BLOCKS:
-            if(dir==USB_DIR_IN) {
-                logf("IN received in RECEIVING");
-            }
-            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) {
-                    logf("unexpected length :%d",length);
-                }
-
-                unsigned int next_sector = cur_cmd.sector +
-                             (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;
-
-                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));
-                }
-
-                /* 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;
-            }
-            else {
-                logf("Transfer failed %X",status);
-                send_csw(UMS_STATUS_FAIL);
-                /* TODO fill in cur_sense_data */
-                cur_sense_data.sense_key=0;
-                cur_sense_data.information=0;
-                cur_sense_data.asc=0;
-                cur_sense_data.ascq=0;
-            }
-            break;
-        case WAITING_FOR_COMMAND:
-            if(dir==USB_DIR_IN) {
-                logf("IN received in WAITING_FOR_COMMAND");
-            }
-            //logf("command received");
-            if(letoh32(cbw->signature) == CBW_SIGNATURE) {
-                handle_scsi(cbw);
-            }
-            else {
-                usb_drv_stall(ep_in, true,true);
-                usb_drv_stall(ep_out, true,false);
-            }
-            break;
+    switch(state)
+    {
         case SENDING_CSW:
-            if(dir==USB_DIR_OUT) {
-                logf("OUT received in SENDING_CSW");
-            }
             //logf("csw sent, now go back to idle");
             state = WAITING_FOR_COMMAND;
 #if 0
@@ -556,9 +614,6 @@
 #endif
             break;
         case SENDING_RESULT:
-            if(dir==USB_DIR_OUT) {
-                logf("OUT received in SENDING");
-            }
             if(status==0) {
                 //logf("data sent, now send csw");
                 send_csw(UMS_STATUS_GOOD);
@@ -574,15 +629,9 @@
             }
             break;
         case SENDING_FAILED_RESULT:
-            if(dir==USB_DIR_OUT) {
-                logf("OUT received in SENDING");
-            }
             send_csw(UMS_STATUS_FAIL);
             break;
         case SENDING_BLOCKS:
-            if(dir==USB_DIR_OUT) {
-                logf("OUT received in SENDING");
-            }
             if(status==0) {
                 if(cur_cmd.count==0) {
                     //logf("data sent, now send csw");
@@ -602,6 +651,60 @@
                 cur_sense_data.ascq=0;
             }
             break;
+        case RECEIVING_BLOCKS:
+            //logf("scsi write %d %d", cur_cmd.sector, cur_cmd.count);
+            if(status==0) {
+                if((unsigned int)length!=(current_dw->count*SECTOR_SIZE))
+                {
+                    logf("unexpected length :%d",length);
+                }
+
+                unsigned int next_sector = cur_cmd.sector +
+                             (WRITE_BUFFER_SIZE/SECTOR_SIZE);
+                unsigned int next_count = cur_cmd.count -
+                             MIN(cur_cmd.count,WRITE_BUFFER_SIZE/SECTOR_SIZE);
+
+                delayed_write_do(current_dw);
+                if(next_count!=0) {
+                    /* Ask the host to send more, to the other buffer */
+                    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);
+                }
+
+                if(next_count==0) {
+                    send_csw(UMS_STATUS_GOOD);
+                }
+
+                /* Switch buffers for the next one */
+
+                cur_cmd.sector = next_sector;
+                cur_cmd.count = next_count;
+            }
+            else {
+                logf("Transfer failed %X",status);
+                send_csw(UMS_STATUS_FAIL);
+                /* TODO fill in cur_sense_data */
+                cur_sense_data.sense_key=0;
+                cur_sense_data.information=0;
+                cur_sense_data.asc=0;
+                cur_sense_data.ascq=0;
+            }
+            break;
+        case WAITING_FOR_COMMAND:
+            //logf("command received");
+            if(letoh32(cbw->signature) == CBW_SIGNATURE) {
+                handle_scsi(cbw);
+            }
+            else {
+                usb_drv_stall(ep_in, true,true);
+                usb_drv_stall(ep_out, true,false);
+            }
+            break;
+        default:
+            break;
     }
 }
 
@@ -679,6 +782,24 @@
 }
 /****************************************************************************/
 
+static unsigned char *fill_caching_page(unsigned char *buf)
+{
+    struct caching_page *cp=(struct caching_page *)buf;
+    cp->mode_hdr=0x08;
+    cp->page_length=0x12;
+    cp->attributes=0x05; /* 0x04 when read caching is implemented */
+    cp->retention=0;
+    cp->disable_prefetch_length=0;
+    cp->minimum_prefetch=0;
+    cp->maximum_prefetch=0;
+    cp->maximum_prefetch_ceiling=0;
+    cp->cache_attributes=0x20;
+    cp->cache_segments=htobe16(1);
+    cp->cache_segment_size=0;
+    cp->reserved=0;
+    return buf+sizeof(struct caching_page);
+}
+
 static void handle_scsi(struct command_block_wrapper* cbw)
 {
     /* USB Mass Storage assumes LBA capability.
@@ -721,6 +842,12 @@
     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);
@@ -775,7 +902,7 @@
             break;
 
         case SCSI_REQUEST_SENSE: {
-            tb.sense_data->ResponseCode=0x70;/*current error*/
+            tb.sense_data->ResponseCode=last_error_response_code;
             tb.sense_data->Obsolete=0;
             tb.sense_data->fei_sensekey=cur_sense_data.sense_key&0x0f;
             tb.sense_data->Information=cur_sense_data.information;
@@ -800,13 +927,24 @@
                 cur_sense_data.ascq=0;
                 break;
             }
-            /*unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;*/
+            unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;
+            if(pc!=0)
+            {
+                /* We don't implement changeable or saved mode pages */
+                send_command_failed_result();
+                cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
+                cur_sense_data.asc=ASC_INVALID_FIELD_IN_CBD;
+                cur_sense_data.ascq=0;
+                break;
+            }
             unsigned char page_code = cbw->command_block[2] & 0x3f;
+            unsigned char *nextbit=tb.transfer_buffer;
             logf("scsi mode_sense_10 %d %X",lun,page_code);
             switch(page_code) {
                 case 0x3f:
+                case 0x08: /* cache mode page */
                     tb.ms_data_10->mode_data_length =
-                        htobe16(sizeof(struct mode_sense_data_10)-2);
+                        htobe16(sizeof(struct mode_sense_data_10)+sizeof(struct caching_page)-2);
                     tb.ms_data_10->medium_type = 0;
                     tb.ms_data_10->device_specific = 0;
                     tb.ms_data_10->reserved = 0;
@@ -834,8 +972,10 @@
                         ((block_size*block_size_mult) & 0x0000ff00)>>8;
                     tb.ms_data_10->block_descriptor.block_size[3] =
                         ((block_size*block_size_mult) & 0x000000ff);
+                    nextbit+=sizeof(struct mode_sense_data_6);
+                    nextbit=fill_caching_page(nextbit);
                     send_command_result(tb.ms_data_10,
-                            MIN(sizeof(struct mode_sense_data_10), length));
+                            MIN((unsigned int)(nextbit-tb.transfer_buffer), length));
                     break;
                 default:
                     send_command_failed_result();
@@ -855,14 +995,25 @@
                 cur_sense_data.ascq=0;
                 break;
             }
-            /*unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;*/
+            unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;
+            if(pc!=0)
+            {
+                /* We don't implement changeable or saved mode pages */
+                send_command_failed_result();
+                cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
+                cur_sense_data.asc=ASC_INVALID_FIELD_IN_CBD;
+                cur_sense_data.ascq=0;
+                break;
+            }
             unsigned char page_code = cbw->command_block[2] & 0x3f;
+            unsigned char *nextbit=tb.transfer_buffer;
             logf("scsi mode_sense_6 %d %X",lun,page_code);
             switch(page_code) {
                 case 0x3f:
+                case 0x08: /* cache mode page */
                     /* All supported pages. */
                     tb.ms_data_6->mode_data_length =
-                        sizeof(struct mode_sense_data_6)-1;
+                        sizeof(struct mode_sense_data_6)+sizeof(struct caching_page)-1;
                     tb.ms_data_6->medium_type = 0;
                     tb.ms_data_6->device_specific = 0;
                     tb.ms_data_6->block_descriptor_length =
@@ -888,8 +1039,10 @@
                         ((block_size*block_size_mult) & 0x00ff00)>>8;
                     tb.ms_data_6->block_descriptor.block_size[2] =
                         ((block_size*block_size_mult) & 0x0000ff);
+                    nextbit+=sizeof(struct mode_sense_data_6);
+                    nextbit=fill_caching_page(nextbit);
                     send_command_result(tb.ms_data_6,
-                        MIN(sizeof(struct mode_sense_data_6), length));
+                            MIN((unsigned int)(nextbit-tb.transfer_buffer), length));
                     break;
                 default:
                     send_command_failed_result();
@@ -1058,10 +1211,17 @@
                 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;
+        case SCSI_SYNCHRONIZE_CACHE_10:
+        case SCSI_SYNCHRONIZE_CACHE_16:
+            send_csw(UMS_STATUS_GOOD);
+            break;
 
         default:
             logf("scsi unknown cmd %x",cbw->command_block[0x0]);
@@ -1099,6 +1259,17 @@
 
 static void send_csw(int status)
 {
+    if(delayed_write_error)
+    {
+        last_error_response_code=0x71;
+        status = UMS_STATUS_FAIL;
+        delayed_write_error=false;
+    }
+    else
+    {
+        last_error_response_code=0x70;
+    }
+
     tb.csw->signature = htole32(CSW_SIGNATURE);
     tb.csw->tag = cur_cmd.tag;
     tb.csw->data_residue = 0;