firmware/usbstack/usb_storage.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2007 by Björn Stenberg
  11  *
  12  * This program is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU General Public License
  14  * as published by the Free Software Foundation; either version 2
  15  * of the License, or (at your option) any later version.
  16  *
  17  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  18  * KIND, either express or implied.
  19  *
  20  ****************************************************************************/
  21 #include "string.h"
  22 #include "system.h"
  23 #include "usb_core.h"
  24 #include "usb_drv.h"
  25 #include "usb_class_driver.h"
  26 /*#define LOGF_ENABLE*/
  27 #include "logf.h"
  28 #include "storage.h"
  29 #include "disk.h"
  30 #include "fat.h"
  31 /* Needed to get at the audio buffer */
  32 #include "audio.h"
  33 #include "usb_storage.h"
  34 #if CONFIG_RTC
  35 #include "timefuncs.h"
  36 #endif
  37
  38 #ifdef USB_USE_RAMDISK
  39 #define RAMDISK_SIZE 2048
  40 #endif
  41
  42 #ifndef SECTOR_SIZE
  43 #define SECTOR_SIZE 512
  44 #endif
  45
  46 /* These defaults allow the operation */
  47 #ifndef USBSTOR_READ_SECTORS_FILTER
  48 #define USBSTOR_READ_SECTORS_FILTER() ({ 0; })
  49 #endif
  50
  51 #ifndef USBSTOR_WRITE_SECTORS_FILTER
  52 #define USBSTOR_WRITE_SECTORS_FILTER() ({ 0; })
  53 #endif
  54
  55 /* the ARC driver currently supports up to 64k USB transfers. This is
  56  * enough for efficient mass storage support, as commonly host OSes
  57  * don't do larger SCSI transfers anyway, so larger USB transfers
  58  * wouldn't buy us anything.
  59  * Due to being the double-buffering system used, using a smaller write buffer
  60  * ends up being more efficient. Measurements have shown that 24k to 28k is
  61  * optimal, except for sd devices that apparently don't gain anything from
  62  * double-buffering
  63  */
  64 #ifdef USB_READ_BUFFER_SIZE
  65 #define READ_BUFFER_SIZE USB_READ_BUFFER_SIZE
  66 #else
  67 #if CONFIG_USBOTG == USBOTG_AS3525
  68 /* We'd need to implement multidescriptor dma for sizes >65535 */
  69 #define READ_BUFFER_SIZE (1024*63)
  70 #else
  71 #define READ_BUFFER_SIZE (1024*64)
  72 #endif /* CONFIG_CPU == AS3525 */
  73 #endif /* USB_READ_BUFFER_SIZE */
  74
  75 #define MAX_CBW_SIZE 1024
  76
  77 #ifdef USB_WRITE_BUFFER_SIZE
  78 #define WRITE_BUFFER_SIZE USB_WRITE_BUFFER_SIZE
  79 #else
  80 #if (CONFIG_STORAGE & STORAGE_SD)
  81 #if CONFIG_USBOTG == USBOTG_AS3525
  82 /* We'd need to implement multidescriptor dma for sizes >65535 */
  83 #define WRITE_BUFFER_SIZE (1024*63)
  84 #else
  85 #define WRITE_BUFFER_SIZE (1024*64)
  86 #endif /* CONFIG_CPU == AS3525 */
  87 #else
  88 #define WRITE_BUFFER_SIZE (1024*24)
  89 #endif /* (CONFIG_STORAGE & STORAGE_SD) */
  90 #endif /* USB_WRITE_BUFFER_SIZE */
  91
  92 #define ALLOCATE_BUFFER_SIZE (2*MAX(READ_BUFFER_SIZE,WRITE_BUFFER_SIZE))
  93
  94 /* bulk-only class specific requests */
  95 #define USB_BULK_RESET_REQUEST   0xff
  96 #define USB_BULK_GET_MAX_LUN     0xfe
  97
  98 #define DIRECT_ACCESS_DEVICE     0x00    /* disks */
  99 #define DEVICE_REMOVABLE         0x80
 100
 101 #define CBW_SIGNATURE            0x43425355
 102 #define CSW_SIGNATURE            0x53425355
 103
 104 #define SCSI_TEST_UNIT_READY      0x00
 105 #define SCSI_INQUIRY              0x12
 106 #define SCSI_MODE_SENSE_6         0x1a
 107 #define SCSI_MODE_SENSE_10        0x5a
 108 #define SCSI_REQUEST_SENSE        0x03
 109 #define SCSI_ALLOW_MEDIUM_REMOVAL 0x1e
 110 #define SCSI_READ_CAPACITY        0x25
 111 #define SCSI_READ_FORMAT_CAPACITY 0x23
 112 #define SCSI_READ_10              0x28
 113 #define SCSI_WRITE_10             0x2a
 114 #define SCSI_START_STOP_UNIT      0x1b
 115 #define SCSI_REPORT_LUNS          0xa0
 116 #define SCSI_WRITE_BUFFER         0x3b
 117
 118 #define UMS_STATUS_GOOD            0x00
 119 #define UMS_STATUS_FAIL            0x01
 120
 121 #define SENSE_NOT_READY             0x02
 122 #define SENSE_MEDIUM_ERROR          0x03
 123 #define SENSE_ILLEGAL_REQUEST       0x05
 124 #define SENSE_UNIT_ATTENTION        0x06
 125
 126 #define ASC_MEDIUM_NOT_PRESENT      0x3a
 127 #define ASC_INVALID_FIELD_IN_CBD    0x24
 128 #define ASC_LBA_OUT_OF_RANGE        0x21
 129 #define ASC_WRITE_ERROR             0x0C
 130 #define ASC_READ_ERROR              0x11
 131 #define ASC_NOT_READY               0x04
 132 #define ASC_INVALID_COMMAND         0x20
 133
 134 #define ASCQ_BECOMING_READY         0x01
 135
 136 #define SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA 0x02000000
 137
 138 /* storage interface */
 139
 140 #define USB_SC_SCSI      0x06            /* Transparent */
 141 #define USB_PROT_BULK    0x50            /* bulk only */
 142
 143 static struct usb_interface_descriptor __attribute__((aligned(2)))
 144                                        interface_descriptor =
 145 {
 146     .bLength            = sizeof(struct usb_interface_descriptor),
 147     .bDescriptorType    = USB_DT_INTERFACE,
 148     .bInterfaceNumber   = 0,
 149     .bAlternateSetting  = 0,
 150     .bNumEndpoints      = 2,
 151     .bInterfaceClass    = USB_CLASS_MASS_STORAGE,
 152     .bInterfaceSubClass = USB_SC_SCSI,
 153     .bInterfaceProtocol = USB_PROT_BULK,
 154     .iInterface         = 0
 155 };
 156
 157 static struct usb_endpoint_descriptor __attribute__((aligned(2)))
 158                                       endpoint_descriptor =
 159 {
 160     .bLength          = sizeof(struct usb_endpoint_descriptor),
 161     .bDescriptorType  = USB_DT_ENDPOINT,
 162     .bEndpointAddress = 0,
 163     .bmAttributes     = USB_ENDPOINT_XFER_BULK,
 164     .wMaxPacketSize   = 0,
 165     .bInterval        = 0
 166 };
 167
 168 struct inquiry_data {
 169     unsigned char DeviceType;
 170     unsigned char DeviceTypeModifier;
 171     unsigned char Versions;
 172     unsigned char Format;
 173     unsigned char AdditionalLength;
 174     unsigned char Reserved[2];
 175     unsigned char Capability;
 176     unsigned char VendorId[8];
 177     unsigned char ProductId[16];
 178     unsigned char ProductRevisionLevel[4];
 179 } __attribute__ ((packed));
 180
 181 struct report_lun_data {
 182     unsigned int lun_list_length;
 183     unsigned int reserved1;
 184     unsigned char luns[NUM_DRIVES][8];
 185 } __attribute__ ((packed));
 186
 187 struct sense_data {
 188     unsigned char ResponseCode;
 189     unsigned char Obsolete;
 190     unsigned char fei_sensekey;
 191     unsigned int Information;
 192     unsigned char AdditionalSenseLength;
 193     unsigned int  CommandSpecificInformation;
 194     unsigned char AdditionalSenseCode;
 195     unsigned char AdditionalSenseCodeQualifier;
 196     unsigned char FieldReplaceableUnitCode;
 197     unsigned char SKSV;
 198     unsigned short SenseKeySpecific;
 199 } __attribute__ ((packed));
 200
 201 struct mode_sense_bdesc_longlba {
 202     unsigned char num_blocks[8];
 203     unsigned char reserved[4];
 204     unsigned char block_size[4];
 205 } __attribute__ ((packed));
 206
 207 struct mode_sense_bdesc_shortlba {
 208     unsigned char density_code;
 209     unsigned char num_blocks[3];
 210     unsigned char reserved;
 211     unsigned char block_size[3];
 212 } __attribute__ ((packed));
 213
 214 struct mode_sense_data_10 {
 215     unsigned short mode_data_length;
 216     unsigned char medium_type;
 217     unsigned char device_specific;
 218     unsigned char longlba;
 219     unsigned char reserved;
 220     unsigned short block_descriptor_length;
 221     struct mode_sense_bdesc_longlba block_descriptor;
 222 } __attribute__ ((packed));
 223
 224 struct mode_sense_data_6 {
 225     unsigned char mode_data_length;
 226     unsigned char medium_type;
 227     unsigned char device_specific;
 228     unsigned char block_descriptor_length;
 229     struct mode_sense_bdesc_shortlba block_descriptor;
 230 } __attribute__ ((packed));
 231
 232 struct command_block_wrapper {
 233     unsigned int signature;
 234     unsigned int tag;
 235     unsigned int data_transfer_length;
 236     unsigned char flags;
 237     unsigned char lun;
 238     unsigned char command_length;
 239     unsigned char command_block[16];
 240 } __attribute__ ((packed));
 241
 242 struct command_status_wrapper {
 243     unsigned int signature;
 244     unsigned int tag;
 245     unsigned int data_residue;
 246     unsigned char status;
 247 } __attribute__ ((packed));
 248
 249 struct capacity {
 250     unsigned int block_count;
 251     unsigned int block_size;
 252 } __attribute__ ((packed));
 253
 254 struct format_capacity {
 255     unsigned int following_length;
 256     unsigned int block_count;
 257     unsigned int block_size;
 258 } __attribute__ ((packed));
 259
 260
 261 static union {
 262     unsigned char* transfer_buffer;
 263     struct inquiry_data* inquiry;
 264     struct capacity* capacity_data;
 265     struct format_capacity* format_capacity_data;
 266     struct sense_data *sense_data;
 267     struct mode_sense_data_6 *ms_data_6;
 268     struct mode_sense_data_10 *ms_data_10;
 269     struct report_lun_data *lun_data;
 270     struct command_status_wrapper* csw;
 271     char *max_lun;
 272 } tb;
 273
 274 static char *cbw_buffer;
 275
 276 static struct {
 277     unsigned int sector;
 278     unsigned int count;
 279     unsigned int orig_count;
 280     unsigned int cur_cmd;
 281     unsigned int tag;
 282     unsigned int lun;
 283     unsigned char *data[2];
 284     unsigned char data_select;
 285     unsigned int last_result;
 286 } cur_cmd;
 287
 288 static struct {
 289     unsigned char sense_key;
 290     unsigned char information;
 291     unsigned char asc;
 292     unsigned char ascq;
 293 } cur_sense_data;
 294
 295 static void handle_scsi(struct command_block_wrapper* cbw);
 296 static void send_csw(int status);
 297 static void send_command_result(void *data,int size);
 298 static void send_command_failed_result(void);
 299 static void send_block_data(void *data,int size);
 300 static void receive_block_data(void *data,int size);
 301 #if CONFIG_RTC
 302 static void receive_time(void);
 303 #endif
 304 static void fill_inquiry(IF_MD_NONVOID(int lun));
 305 static void send_and_read_next(void);
 306 static bool ejected[NUM_DRIVES];
 307 static bool locked[NUM_DRIVES];
 308
 309 static int usb_interface;
 310 static int ep_in, ep_out;
 311
 312 #if defined(HAVE_MULTIDRIVE)
 313 static bool skip_first = 0;
 314 #endif
 315
 316 #ifdef USB_USE_RAMDISK
 317 static unsigned char* ramdisk_buffer;
 318 #endif
 319
 320 static enum {
 321     WAITING_FOR_COMMAND,
 322     SENDING_BLOCKS,
 323     SENDING_RESULT,
 324     SENDING_FAILED_RESULT,
 325     RECEIVING_BLOCKS,
 326 #if CONFIG_RTC
 327     RECEIVING_TIME,
 328 #endif
 329     WAITING_FOR_CSW_COMPLETION_OR_COMMAND,
 330     WAITING_FOR_CSW_COMPLETION
 331 } state = WAITING_FOR_COMMAND;
 332
 333 #if CONFIG_RTC
 334 static void yearday_to_daymonth(int yd, int y, int *d, int *m)
 335 {
 336     static const char tnl[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
 337     static const char tl[] = { 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
 338     const char *t;
 339     int i=0;
 340
 341     if((y%4 == 0 && y%100 != 0) || y%400 == 0)
 342     {
 343         t=tl;
 344     }
 345     else
 346     {
 347         t=tnl;
 348     }
 349
 350     while(i<12 && yd >= t[i])
 351     {
 352         yd-=t[i];
 353         i++;
 354     }
 355     *d = yd+1;
 356     *m = i;
 357 }
 358 #endif /* CONFIG_RTC */
 359
 360 static bool check_disk_present(IF_MD_NONVOID(int volume))
 361 {
 362 #ifdef USB_USE_RAMDISK
 363     return true;
 364 #else
 365     unsigned char* sector = fat_get_sector_buffer();
 366     bool success = storage_read_sectors(IF_MD2(volume,)0,1,sector) == 0;
 367     fat_release_sector_buffer();
 368     return success;
 369 #endif
 370 }
 371
 372 #ifdef HAVE_HOTSWAP
 373 void usb_storage_notify_hotswap(int volume,bool inserted)
 374 {
 375     logf("notify %d",inserted);
 376     if(inserted && check_disk_present(IF_MD(volume))) {
 377         ejected[volume] = false;
 378     }
 379     else {
 380         ejected[volume] = true;
 381         /* If this happens while the device is locked, weird things may happen.
 382            At least try to keep our state consistent */
 383         locked[volume]=false;
 384     }
 385 }
 386 #endif
 387
 388 #ifdef HAVE_MULTIDRIVE
 389 void usb_set_skip_first_drive(bool skip)
 390 {
 391     skip_first = skip;
 392 }
 393 #endif
 394
 395 /* called by usb_core_init() */
 396 void usb_storage_init(void)
 397 {
 398     logf("usb_storage_init done");
 399 }
 400
 401 int usb_storage_request_endpoints(struct usb_class_driver *drv)
 402 {
 403     ep_in = usb_core_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_IN, drv);
 404
 405     if(ep_in<0)
 406         return -1;
 407
 408     ep_out = usb_core_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_OUT,
 409             drv);
 410
 411     if(ep_out<0) {
 412         usb_core_release_endpoint(ep_in);
 413         return -1;
 414     }
 415
 416     return 0;
 417 }
 418
 419 int usb_storage_set_first_interface(int interface)
 420 {
 421     usb_interface = interface;
 422     return interface + 1;
 423 }
 424
 425 int usb_storage_get_config_descriptor(unsigned char *dest,int max_packet_size)
 426 {
 427     unsigned char *orig_dest = dest;
 428
 429     interface_descriptor.bInterfaceNumber = usb_interface;
 430     PACK_DATA(&dest, interface_descriptor);
 431
 432     endpoint_descriptor.wMaxPacketSize = max_packet_size;
 433
 434     endpoint_descriptor.bEndpointAddress = ep_in;
 435     PACK_DATA(&dest, endpoint_descriptor);
 436
 437     endpoint_descriptor.bEndpointAddress = ep_out;
 438     PACK_DATA(&dest, endpoint_descriptor);
 439
 440     return (dest - orig_dest);
 441 }
 442
 443 void usb_storage_init_connection(void)
 444 {
 445     logf("ums: set config");
 446     /* prime rx endpoint. We only need room for commands */
 447     state = WAITING_FOR_COMMAND;
 448
 449 #if (CONFIG_CPU == IMX31L || defined(CPU_TCC77X) || defined(CPU_TCC780X) || \
 450      defined(BOOTLOADER) || CONFIG_CPU == DM320) && !defined(CPU_PP502x)
 451     static unsigned char _cbw_buffer[MAX_CBW_SIZE]
 452         USB_DEVBSS_ATTR __attribute__((aligned(32)));
 453     cbw_buffer = (void *)_cbw_buffer;
 454
 455     static unsigned char _transfer_buffer[ALLOCATE_BUFFER_SIZE]
 456         USB_DEVBSS_ATTR __attribute__((aligned(32)));
 457     tb.transfer_buffer = (void *)_transfer_buffer;
 458 #ifdef USB_USE_RAMDISK
 459     static unsigned char _ramdisk_buffer[RAMDISK_SIZE*SECTOR_SIZE];
 460     ramdisk_buffer = _ramdisk_buffer;
 461 #endif
 462 #else
 463     /* TODO : check if bufsize is at least 32K ? */
 464     size_t bufsize;
 465     unsigned char * audio_buffer;
 466
 467     audio_buffer = audio_get_buffer(false,&bufsize);
 468 #if defined(UNCACHED_ADDR) && CONFIG_CPU != AS3525
 469     cbw_buffer = (void *)UNCACHED_ADDR((unsigned int)(audio_buffer+31) & 0xffffffe0);
 470 #else
 471     cbw_buffer = (void *)((unsigned int)(audio_buffer+31) & 0xffffffe0);
 472 #endif
 473     tb.transfer_buffer = cbw_buffer + MAX_CBW_SIZE;
 474     commit_discard_dcache();
 475 #ifdef USB_USE_RAMDISK
 476     ramdisk_buffer = tb.transfer_buffer + ALLOCATE_BUFFER_SIZE;
 477 #endif
 478 #endif
 479     usb_drv_recv(ep_out, cbw_buffer, MAX_CBW_SIZE);
 480
 481     int i;
 482     for(i=0;i<storage_num_drives();i++) {
 483         locked[i] = false;
 484         ejected[i] = !check_disk_present(IF_MD(i));
 485         queue_broadcast(SYS_USB_LUN_LOCKED, (i<<16)+0);
 486     }
 487 }
 488
 489 void usb_storage_disconnect(void)
 490 {
 491     /* Empty for now */
 492 }
 493
 494 /* called by usb_core_transfer_complete() */
 495 void usb_storage_transfer_complete(int ep,int dir,int status,int length)
 496 {
 497     (void)ep;
 498     struct command_block_wrapper* cbw = (void*)cbw_buffer;
 499 #if CONFIG_RTC
 500     struct tm tm;
 501 #endif
 502
 503     logf("transfer result for ep %d/%d %X %d", ep,dir,status, length);
 504     switch(state) {
 505         case RECEIVING_BLOCKS:
 506             if(dir==USB_DIR_IN) {
 507                 logf("IN received in RECEIVING");
 508             }
 509             logf("scsi write %d %d", cur_cmd.sector, cur_cmd.count);
 510             if(status==0) {
 511                 if((unsigned int)length!=(SECTOR_SIZE* cur_cmd.count)
 512                   && (unsigned int)length!=WRITE_BUFFER_SIZE) {
 513                     logf("unexpected length :%d",length);
 514                     break;
 515                 }
 516
 517                 unsigned int next_sector = cur_cmd.sector +
 518                              (WRITE_BUFFER_SIZE/SECTOR_SIZE);
 519                 unsigned int next_count = cur_cmd.count -
 520                              MIN(cur_cmd.count,WRITE_BUFFER_SIZE/SECTOR_SIZE);
 521                 int next_select = !cur_cmd.data_select;
 522
 523                 if(next_count!=0) {
 524                     /* Ask the host to send more, to the other buffer */
 525                     receive_block_data(cur_cmd.data[next_select],
 526                                        MIN(WRITE_BUFFER_SIZE,next_count*SECTOR_SIZE));
 527                 }
 528
 529                 /* Now write the data that just came in, while the host is
 530                    sending the next bit */
 531 #ifdef USB_USE_RAMDISK
 532                 memcpy(ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
 533                         cur_cmd.data[cur_cmd.data_select],
 534                         MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE);
 535 #else
 536                 int result = USBSTOR_WRITE_SECTORS_FILTER();
 537
 538                 if (result == 0) {
 539                     result = storage_write_sectors(IF_MD2(cur_cmd.lun,)
 540                         cur_cmd.sector,
 541                         MIN(WRITE_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
 542                         cur_cmd.data[cur_cmd.data_select]);
 543                 }
 544
 545                 if(result != 0) {
 546                     send_csw(UMS_STATUS_FAIL);
 547                     cur_sense_data.sense_key=SENSE_MEDIUM_ERROR;
 548                     cur_sense_data.asc=ASC_WRITE_ERROR;
 549                     cur_sense_data.ascq=0;
 550                     break;
 551                 }
 552 #endif
 553                 if(next_count==0) {
 554                     send_csw(UMS_STATUS_GOOD);
 555                 }
 556
 557                 /* Switch buffers for the next one */
 558                 cur_cmd.data_select=!cur_cmd.data_select;
 559
 560                 cur_cmd.sector = next_sector;
 561                 cur_cmd.count = next_count;
 562             }
 563             else {
 564                 logf("Transfer failed %X",status);
 565                 send_csw(UMS_STATUS_FAIL);
 566                 /* TODO fill in cur_sense_data */
 567                 cur_sense_data.sense_key=0;
 568                 cur_sense_data.information=0;
 569                 cur_sense_data.asc=0;
 570                 cur_sense_data.ascq=0;
 571             }
 572             break;
 573         case WAITING_FOR_CSW_COMPLETION_OR_COMMAND:
 574             if(dir==USB_DIR_IN) {
 575                 /* This was the CSW */
 576                 state = WAITING_FOR_COMMAND;
 577             }
 578             else {
 579                 /* This was the command */
 580                 state = WAITING_FOR_CSW_COMPLETION;
 581                 /* We now have the CBW, but we won't execute it yet to avoid
 582                  * issues with the still-pending CSW */
 583             }
 584             break;
 585         case WAITING_FOR_COMMAND:
 586             if(dir==USB_DIR_IN) {
 587                 logf("IN received in WAITING_FOR_COMMAND");
 588             }
 589             handle_scsi(cbw);
 590             break;
 591         case WAITING_FOR_CSW_COMPLETION:
 592             if(dir==USB_DIR_OUT) {
 593                 logf("OUT received in WAITING_FOR_CSW_COMPLETION");
 594             }
 595             handle_scsi(cbw);
 596             break;
 597 #if 0
 598             if(cur_cmd.cur_cmd == SCSI_WRITE_10)
 599             {
 600                 queue_broadcast(SYS_USB_WRITE_DATA, (cur_cmd.lun<<16)+cur_cmd.orig_count);
 601             }
 602             else if(cur_cmd.cur_cmd == SCSI_READ_10)
 603             {
 604                 queue_broadcast(SYS_USB_READ_DATA, (cur_cmd.lun<<16)+cur_cmd.orig_count);
 605             }
 606 #endif
 607             break;
 608         case SENDING_RESULT:
 609             if(dir==USB_DIR_OUT) {
 610                 logf("OUT received in SENDING");
 611             }
 612             if(status==0) {
 613                 //logf("data sent, now send csw");
 614                 send_csw(UMS_STATUS_GOOD);
 615             }
 616             else {
 617                 logf("Transfer failed %X",status);
 618                 send_csw(UMS_STATUS_FAIL);
 619                 /* TODO fill in cur_sense_data */
 620                 cur_sense_data.sense_key=0;
 621                 cur_sense_data.information=0;
 622                 cur_sense_data.asc=0;
 623                 cur_sense_data.ascq=0;
 624             }
 625             break;
 626         case SENDING_FAILED_RESULT:
 627             if(dir==USB_DIR_OUT) {
 628                 logf("OUT received in SENDING");
 629             }
 630             send_csw(UMS_STATUS_FAIL);
 631             break;
 632         case SENDING_BLOCKS:
 633             if(dir==USB_DIR_OUT) {
 634                 logf("OUT received in SENDING");
 635             }
 636             if(status==0) {
 637                 if(cur_cmd.count==0) {
 638                     //logf("data sent, now send csw");
 639                     if(cur_cmd.last_result!=0) {
 640                         /* The last read failed. */
 641                         send_csw(UMS_STATUS_FAIL);
 642                         cur_sense_data.sense_key=SENSE_MEDIUM_ERROR;
 643                         cur_sense_data.asc=ASC_READ_ERROR;
 644                         cur_sense_data.ascq=0;
 645                         return;
 646                     }
 647                     else
 648                         send_csw(UMS_STATUS_GOOD);
 649                 }
 650                 else {
 651                     send_and_read_next();
 652                 }
 653             }
 654             else {
 655                 logf("Transfer failed %X",status);
 656                 send_csw(UMS_STATUS_FAIL);
 657                 /* TODO fill in cur_sense_data */
 658                 cur_sense_data.sense_key=0;
 659                 cur_sense_data.information=0;
 660                 cur_sense_data.asc=0;
 661                 cur_sense_data.ascq=0;
 662             }
 663             break;
 664 #if CONFIG_RTC
 665         case RECEIVING_TIME:
 666             tm.tm_year=(tb.transfer_buffer[0]<<8)+tb.transfer_buffer[1] - 1900;
 667             tm.tm_yday=(tb.transfer_buffer[2]<<8)+tb.transfer_buffer[3];
 668             tm.tm_hour=tb.transfer_buffer[5];
 669             tm.tm_min=tb.transfer_buffer[6];
 670             tm.tm_sec=tb.transfer_buffer[7];
 671             yearday_to_daymonth(tm.tm_yday,tm.tm_year + 1900,&tm.tm_mday,&tm.tm_mon);
 672             set_day_of_week(&tm);
 673             set_time(&tm);
 674             send_csw(UMS_STATUS_GOOD);
 675             break;
 676 #endif /* CONFIG_RTC */
 677     }
 678 }
 679
 680 /* called by usb_core_control_request() */
 681 bool usb_storage_control_request(struct usb_ctrlrequest* req, unsigned char* dest)
 682 {
 683     bool handled = false;
 684
 685     (void)dest;
 686     switch (req->bRequest) {
 687         case USB_BULK_GET_MAX_LUN: {
 688             *tb.max_lun = storage_num_drives() - 1;
 689 #if defined(HAVE_MULTIDRIVE)
 690             if(skip_first) (*tb.max_lun) --;
 691 #endif
 692             logf("ums: getmaxlun");
 693             usb_drv_recv(EP_CONTROL, NULL, 0); /* ack */
 694             usb_drv_send(EP_CONTROL, tb.max_lun, 1);
 695             handled = true;
 696             break;
 697         }
 698
 699         case USB_BULK_RESET_REQUEST:
 700             logf("ums: bulk reset");
 701             state = WAITING_FOR_COMMAND;
 702             /* UMS BOT 3.1 says The device shall preserve the value of its bulk
 703                data toggle bits and endpoint STALL conditions despite
 704                the Bulk-Only Mass Storage Reset. */
 705 #if 0
 706             usb_drv_reset_endpoint(ep_in, false);
 707             usb_drv_reset_endpoint(ep_out, true);
 708 #endif
 709             usb_drv_send(EP_CONTROL, NULL, 0);  /* ack */
 710             handled = true;
 711             break;
 712     }
 713
 714     return handled;
 715 }
 716
 717 static void send_and_read_next(void)
 718 {
 719     int result = USBSTOR_READ_SECTORS_FILTER();
 720
 721     if(result != 0 && cur_cmd.last_result == 0)
 722         cur_cmd.last_result = result;
 723
 724     send_block_data(cur_cmd.data[cur_cmd.data_select],
 725                     MIN(READ_BUFFER_SIZE,cur_cmd.count*SECTOR_SIZE));
 726
 727     /* Switch buffers for the next one */
 728     cur_cmd.data_select=!cur_cmd.data_select;
 729
 730     cur_cmd.sector+=(READ_BUFFER_SIZE/SECTOR_SIZE);
 731     cur_cmd.count-=MIN(cur_cmd.count,READ_BUFFER_SIZE/SECTOR_SIZE);
 732
 733     if(cur_cmd.count!=0) {
 734         /* already read the next bit, so we can send it out immediately when the
 735          * current transfer completes.  */
 736 #ifdef USB_USE_RAMDISK
 737         memcpy(cur_cmd.data[cur_cmd.data_select],
 738                 ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
 739                 MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count)*SECTOR_SIZE);
 740 #else
 741         result = storage_read_sectors(IF_MD2(cur_cmd.lun,)
 742                 cur_cmd.sector,
 743                 MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
 744                 cur_cmd.data[cur_cmd.data_select]);
 745         if(cur_cmd.last_result == 0)
 746             cur_cmd.last_result = result;
 747
 748 #endif
 749     }
 750 }
 751 /****************************************************************************/
 752
 753 static void handle_scsi(struct command_block_wrapper* cbw)
 754 {
 755     /* USB Mass Storage assumes LBA capability.
 756        TODO: support 48-bit LBA */
 757
 758     struct storage_info info;
 759     unsigned int length = cbw->data_transfer_length;
 760     unsigned int block_size = 0;
 761     unsigned int block_count = 0;
 762     bool lun_present=true;
 763     unsigned char lun = cbw->lun;
 764     unsigned int block_size_mult = 1;
 765
 766     if(letoh32(cbw->signature) != CBW_SIGNATURE) {
 767         usb_drv_stall(ep_in, true,true);
 768         usb_drv_stall(ep_out, true,false);
 769         return;
 770     }
 771     /* Clear the signature to prevent possible bugs elsewhere
 772      * to trigger a second execution of the same command with
 773      * bogus data */
 774     cbw->signature=0;
 775
 776 #if defined(HAVE_MULTIDRIVE)
 777     if(skip_first) lun++;
 778 #endif
 779
 780     storage_get_info(lun,&info);
 781 #ifdef USB_USE_RAMDISK
 782     block_size = SECTOR_SIZE;
 783     block_count = RAMDISK_SIZE;
 784 #else
 785     block_size=info.sector_size;
 786     block_count=info.num_sectors;
 787 #endif
 788
 789 #ifdef HAVE_HOTSWAP
 790     if(storage_removable(lun) && !storage_present(lun)) {
 791         ejected[lun] = true;
 792     }
 793 #endif
 794
 795     if(ejected[lun])
 796         lun_present = false;
 797
 798 #ifdef MAX_LOG_SECTOR_SIZE
 799     block_size_mult = disk_get_sector_multiplier(IF_MD(lun));
 800 #endif
 801
 802     cur_cmd.tag = cbw->tag;
 803     cur_cmd.lun = lun;
 804     cur_cmd.cur_cmd = cbw->command_block[0];
 805
 806     switch (cbw->command_block[0]) {
 807         case SCSI_TEST_UNIT_READY:
 808             logf("scsi test_unit_ready %d",lun);
 809             if(!usb_exclusive_storage()) {
 810                 send_csw(UMS_STATUS_FAIL);
 811                 cur_sense_data.sense_key=SENSE_NOT_READY;
 812                 cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
 813                 cur_sense_data.ascq=0;
 814                 break;
 815             }
 816             if(lun_present) {
 817                 send_csw(UMS_STATUS_GOOD);
 818             }
 819             else {
 820                 send_csw(UMS_STATUS_FAIL);
 821                 cur_sense_data.sense_key=SENSE_NOT_READY;
 822                 cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
 823                 cur_sense_data.ascq=0;
 824             }
 825             break;
 826
 827         case SCSI_REPORT_LUNS: {
 828             logf("scsi report luns %d",lun);
 829             int allocation_length=0;
 830             int i;
 831             unsigned int response_length = 8+8*storage_num_drives();
 832             allocation_length|=(cbw->command_block[6]<<24);
 833             allocation_length|=(cbw->command_block[7]<<16);
 834             allocation_length|=(cbw->command_block[8]<<8);
 835             allocation_length|=(cbw->command_block[9]);
 836             memset(tb.lun_data,0,sizeof(struct report_lun_data));
 837             tb.lun_data->lun_list_length=htobe32(8*storage_num_drives());
 838             for(i=0;i<storage_num_drives();i++)
 839             {
 840 #ifdef HAVE_HOTSWAP
 841                 if(storage_removable(i))
 842                     tb.lun_data->luns[i][1]=1;
 843                 else
 844 #endif
 845                     tb.lun_data->luns[i][1]=0;
 846             }
 847             send_command_result(tb.lun_data,
 848                                 MIN(response_length, length));
 849             break;
 850         }
 851
 852         case SCSI_INQUIRY:
 853             logf("scsi inquiry %d",lun);
 854             fill_inquiry(IF_MD(lun));
 855             length = MIN(length, cbw->command_block[4]);
 856             send_command_result(tb.inquiry,
 857                                 MIN(sizeof(struct inquiry_data), length));
 858             break;
 859
 860         case SCSI_REQUEST_SENSE: {
 861             tb.sense_data->ResponseCode=0x70;/*current error*/
 862             tb.sense_data->Obsolete=0;
 863             tb.sense_data->fei_sensekey=cur_sense_data.sense_key&0x0f;
 864             tb.sense_data->Information=cur_sense_data.information;
 865             tb.sense_data->AdditionalSenseLength=10;
 866             tb.sense_data->CommandSpecificInformation=0;
 867             tb.sense_data->AdditionalSenseCode=cur_sense_data.asc;
 868             tb.sense_data->AdditionalSenseCodeQualifier=cur_sense_data.ascq;
 869             tb.sense_data->FieldReplaceableUnitCode=0;
 870             tb.sense_data->SKSV=0;
 871             tb.sense_data->SenseKeySpecific=0;
 872             logf("scsi request_sense %d",lun);
 873             send_command_result(tb.sense_data,
 874                                 MIN(sizeof(struct sense_data), length));
 875             break;
 876         }
 877
 878         case SCSI_MODE_SENSE_10: {
 879             if(!lun_present) {
 880                 send_command_failed_result();
 881                 cur_sense_data.sense_key=SENSE_NOT_READY;
 882                 cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
 883                 cur_sense_data.ascq=0;
 884                 break;
 885             }
 886             /*unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;*/
 887             unsigned char page_code = cbw->command_block[2] & 0x3f;
 888             logf("scsi mode_sense_10 %d %X",lun,page_code);
 889             switch(page_code) {
 890                 case 0x3f:
 891                     tb.ms_data_10->mode_data_length =
 892                         htobe16(sizeof(struct mode_sense_data_10)-2);
 893                     tb.ms_data_10->medium_type = 0;
 894                     tb.ms_data_10->device_specific = 0;
 895                     tb.ms_data_10->reserved = 0;
 896                     tb.ms_data_10->longlba = 1;
 897                     tb.ms_data_10->block_descriptor_length =
 898                         htobe16(sizeof(struct mode_sense_bdesc_longlba));
 899
 900                     memset(tb.ms_data_10->block_descriptor.reserved,0,4);
 901                     memset(tb.ms_data_10->block_descriptor.num_blocks,0,8);
 902
 903                     tb.ms_data_10->block_descriptor.num_blocks[4] =
 904                         ((block_count/block_size_mult) & 0xff000000)>>24;
 905                     tb.ms_data_10->block_descriptor.num_blocks[5] =
 906                         ((block_count/block_size_mult) & 0x00ff0000)>>16;
 907                     tb.ms_data_10->block_descriptor.num_blocks[6] =
 908                         ((block_count/block_size_mult) & 0x0000ff00)>>8;
 909                     tb.ms_data_10->block_descriptor.num_blocks[7] =
 910                         ((block_count/block_size_mult) & 0x000000ff);
 911
 912                     tb.ms_data_10->block_descriptor.block_size[0] =
 913                         ((block_size*block_size_mult) & 0xff000000)>>24;
 914                     tb.ms_data_10->block_descriptor.block_size[1] =
 915                         ((block_size*block_size_mult) & 0x00ff0000)>>16;
 916                     tb.ms_data_10->block_descriptor.block_size[2] =
 917                         ((block_size*block_size_mult) & 0x0000ff00)>>8;
 918                     tb.ms_data_10->block_descriptor.block_size[3] =
 919                         ((block_size*block_size_mult) & 0x000000ff);
 920                     send_command_result(tb.ms_data_10,
 921                             MIN(sizeof(struct mode_sense_data_10), length));
 922                     break;
 923                 default:
 924                     send_command_failed_result();
 925                     cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
 926                     cur_sense_data.asc=ASC_INVALID_FIELD_IN_CBD;
 927                     cur_sense_data.ascq=0;
 928                     break;
 929             }
 930             break;
 931         }
 932
 933         case SCSI_MODE_SENSE_6: {
 934             if(!lun_present) {
 935                 send_command_failed_result();
 936                 cur_sense_data.sense_key=SENSE_NOT_READY;
 937                 cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
 938                 cur_sense_data.ascq=0;
 939                 break;
 940             }
 941             /*unsigned char pc = (cbw->command_block[2] & 0xc0) >>6;*/
 942             unsigned char page_code = cbw->command_block[2] & 0x3f;
 943             logf("scsi mode_sense_6 %d %X",lun,page_code);
 944             switch(page_code) {
 945                 case 0x3f:
 946                     /* All supported pages. */
 947                     tb.ms_data_6->mode_data_length =
 948                         sizeof(struct mode_sense_data_6)-1;
 949                     tb.ms_data_6->medium_type = 0;
 950                     tb.ms_data_6->device_specific = 0;
 951                     tb.ms_data_6->block_descriptor_length =
 952                         sizeof(struct mode_sense_bdesc_shortlba);
 953                     tb.ms_data_6->block_descriptor.density_code = 0;
 954                     tb.ms_data_6->block_descriptor.reserved = 0;
 955                     if(block_count/block_size_mult > 0xffffff) {
 956                         tb.ms_data_6->block_descriptor.num_blocks[0] = 0xff;
 957                         tb.ms_data_6->block_descriptor.num_blocks[1] = 0xff;
 958                         tb.ms_data_6->block_descriptor.num_blocks[2] = 0xff;
 959                     }
 960                     else {
 961                         tb.ms_data_6->block_descriptor.num_blocks[0] =
 962                             ((block_count/block_size_mult) & 0xff0000)>>16;
 963                         tb.ms_data_6->block_descriptor.num_blocks[1] =
 964                             ((block_count/block_size_mult) & 0x00ff00)>>8;
 965                         tb.ms_data_6->block_descriptor.num_blocks[2] =
 966                             ((block_count/block_size_mult) & 0x0000ff);
 967                     }
 968                     tb.ms_data_6->block_descriptor.block_size[0] =
 969                         ((block_size*block_size_mult) & 0xff0000)>>16;
 970                     tb.ms_data_6->block_descriptor.block_size[1] =
 971                         ((block_size*block_size_mult) & 0x00ff00)>>8;
 972                     tb.ms_data_6->block_descriptor.block_size[2] =
 973                         ((block_size*block_size_mult) & 0x0000ff);
 974                     send_command_result(tb.ms_data_6,
 975                         MIN(sizeof(struct mode_sense_data_6), length));
 976                     break;
 977                 default:
 978                     send_command_failed_result();
 979                     cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
 980                     cur_sense_data.asc=ASC_INVALID_FIELD_IN_CBD;
 981                     cur_sense_data.ascq=0;
 982                     break;
 983             }
 984             break;
 985         }
 986
 987         case SCSI_START_STOP_UNIT:
 988             logf("scsi start_stop unit %d",lun);
 989             if((cbw->command_block[4] & 0xf0) == 0) /*load/eject bit is valid*/
 990             { /* Process start and eject bits */
 991                 logf("scsi load/eject");
 992                 if((cbw->command_block[4] & 0x01) == 0) /* Don't start */
 993                 {
 994                     if((cbw->command_block[4] & 0x02) != 0) /* eject */
 995                     {
 996                         logf("scsi eject");
 997                         ejected[lun]=true;
 998                     }
 999                 }
1000             }
1001             send_csw(UMS_STATUS_GOOD);
1002             break;
1003
1004         case SCSI_ALLOW_MEDIUM_REMOVAL:
1005             logf("scsi allow_medium_removal %d",lun);
1006             if((cbw->command_block[4] & 0x03) == 0) {
1007                 locked[lun]=false;
1008                 queue_broadcast(SYS_USB_LUN_LOCKED, (lun<<16)+0);
1009             }
1010             else {
1011                 locked[lun]=true;
1012                 queue_broadcast(SYS_USB_LUN_LOCKED, (lun<<16)+1);
1013             }
1014             send_csw(UMS_STATUS_GOOD);
1015             break;
1016
1017         case SCSI_READ_FORMAT_CAPACITY: {
1018             logf("scsi read_format_capacity %d",lun);
1019             if(lun_present) {
1020                 tb.format_capacity_data->following_length=htobe32(8);
1021                 /* "block count" actually means "number of last block" */
1022                 tb.format_capacity_data->block_count =
1023                     htobe32(block_count/block_size_mult - 1);
1024                 tb.format_capacity_data->block_size =
1025                     htobe32(block_size*block_size_mult);
1026                 tb.format_capacity_data->block_size |=
1027                     htobe32(SCSI_FORMAT_CAPACITY_FORMATTED_MEDIA);
1028
1029                 send_command_result(tb.format_capacity_data,
1030                     MIN(sizeof(struct format_capacity), length));
1031            }
1032            else {
1033                send_command_failed_result();
1034                cur_sense_data.sense_key=SENSE_NOT_READY;
1035                cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
1036                cur_sense_data.ascq=0;
1037            }
1038            break;
1039         }
1040
1041         case SCSI_READ_CAPACITY: {
1042             logf("scsi read_capacity %d",lun);
1043
1044             if(lun_present) {
1045                 /* "block count" actually means "number of last block" */
1046                 tb.capacity_data->block_count =
1047                     htobe32(block_count/block_size_mult - 1);
1048                 tb.capacity_data->block_size =
1049                     htobe32(block_size*block_size_mult);
1050
1051                 send_command_result(tb.capacity_data,
1052                     MIN(sizeof(struct capacity), length));
1053             }
1054             else {
1055                 send_command_failed_result();
1056                 cur_sense_data.sense_key=SENSE_NOT_READY;
1057                 cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
1058                 cur_sense_data.ascq=0;
1059             }
1060             break;
1061         }
1062
1063         case SCSI_READ_10:
1064             logf("scsi read10 %d",lun);
1065             if(!lun_present) {
1066                 send_command_failed_result();
1067                 cur_sense_data.sense_key=SENSE_NOT_READY;
1068                 cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
1069                 cur_sense_data.ascq=0;
1070                 break;
1071             }
1072             cur_cmd.data[0] = tb.transfer_buffer;
1073             cur_cmd.data[1] = &tb.transfer_buffer[READ_BUFFER_SIZE];
1074             cur_cmd.data_select=0;
1075             cur_cmd.sector = block_size_mult *
1076                 (cbw->command_block[2] << 24 |
1077                  cbw->command_block[3] << 16 |
1078                  cbw->command_block[4] << 8  |
1079                  cbw->command_block[5] );
1080             cur_cmd.count = block_size_mult *
1081                 (cbw->command_block[7] << 8 |
1082                  cbw->command_block[8]);
1083             cur_cmd.orig_count = cur_cmd.count;
1084
1085             //logf("scsi read %d %d", cur_cmd.sector, cur_cmd.count);
1086
1087             if((cur_cmd.sector + cur_cmd.count) > block_count) {
1088                 send_csw(UMS_STATUS_FAIL);
1089                 cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
1090                 cur_sense_data.asc=ASC_LBA_OUT_OF_RANGE;
1091                 cur_sense_data.ascq=0;
1092             }
1093             else {
1094 #ifdef USB_USE_RAMDISK
1095                 memcpy(cur_cmd.data[cur_cmd.data_select],
1096                         ramdisk_buffer + cur_cmd.sector*SECTOR_SIZE,
1097                         MIN(READ_BUFFER_SIZE/SECTOR_SIZE,cur_cmd.count)*SECTOR_SIZE);
1098 #else
1099                 cur_cmd.last_result = storage_read_sectors(IF_MD2(cur_cmd.lun,)
1100                         cur_cmd.sector,
1101                         MIN(READ_BUFFER_SIZE/SECTOR_SIZE, cur_cmd.count),
1102                         cur_cmd.data[cur_cmd.data_select]);
1103 #endif
1104                 send_and_read_next();
1105             }
1106             break;
1107
1108         case SCSI_WRITE_10:
1109             logf("scsi write10 %d",lun);
1110             if(!lun_present) {
1111                 send_csw(UMS_STATUS_FAIL);
1112                 cur_sense_data.sense_key=SENSE_NOT_READY;
1113                 cur_sense_data.asc=ASC_MEDIUM_NOT_PRESENT;
1114                 cur_sense_data.ascq=0;
1115                 break;
1116             }
1117             cur_cmd.data[0] = tb.transfer_buffer;
1118             cur_cmd.data[1] = &tb.transfer_buffer[WRITE_BUFFER_SIZE];
1119             cur_cmd.data_select=0;
1120             cur_cmd.sector = block_size_mult *
1121                 (cbw->command_block[2] << 24 |
1122                  cbw->command_block[3] << 16 |
1123                  cbw->command_block[4] << 8  |
1124                  cbw->command_block[5] );
1125             cur_cmd.count = block_size_mult *
1126                 (cbw->command_block[7] << 8 |
1127                  cbw->command_block[8]);
1128             cur_cmd.orig_count = cur_cmd.count;
1129
1130             /* expect data */
1131             if((cur_cmd.sector + cur_cmd.count) > block_count) {
1132                 send_csw(UMS_STATUS_FAIL);
1133                 cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
1134                 cur_sense_data.asc=ASC_LBA_OUT_OF_RANGE;
1135                 cur_sense_data.ascq=0;
1136             }
1137             else {
1138                 receive_block_data(cur_cmd.data[0],
1139                         MIN(WRITE_BUFFER_SIZE, cur_cmd.count*SECTOR_SIZE));
1140             }
1141             break;
1142
1143 #if CONFIG_RTC
1144         case SCSI_WRITE_BUFFER:
1145             if(cbw->command_block[1]==1 /* mode = vendor specific */
1146             && cbw->command_block[2]==0 /* buffer id = 0 */
1147
1148             && cbw->command_block[3]==0x0c /* offset (3 bytes) */
1149             && cbw->command_block[4]==0
1150             && cbw->command_block[5]==0
1151
1152             /* Some versions of itunes set the parameter list length to 0.
1153              * Technically it should be 0x0c, which is what libgpod sends */
1154             && cbw->command_block[6]==0 /* parameter list (3 bytes) */
1155             && cbw->command_block[7]==0
1156             && (cbw->command_block[8]==0 || cbw->command_block[8]==0x0c)
1157
1158             && cbw->command_block[9]==0)
1159             receive_time();
1160             break;
1161 #endif /* CONFIG_RTC */
1162
1163         default:
1164             logf("scsi unknown cmd %x",cbw->command_block[0x0]);
1165             send_csw(UMS_STATUS_FAIL);
1166             cur_sense_data.sense_key=SENSE_ILLEGAL_REQUEST;
1167             cur_sense_data.asc=ASC_INVALID_COMMAND;
1168             cur_sense_data.ascq=0;
1169             break;
1170     }
1171 }
1172
1173 static void send_block_data(void *data,int size)
1174 {
1175     usb_drv_send_nonblocking(ep_in, data,size);
1176     state = SENDING_BLOCKS;
1177 }
1178
1179 static void send_command_result(void *data,int size)
1180 {
1181     usb_drv_send_nonblocking(ep_in, data,size);
1182     state = SENDING_RESULT;
1183 }
1184
1185 static void send_command_failed_result(void)
1186 {
1187     usb_drv_send_nonblocking(ep_in, NULL, 0);
1188     state = SENDING_FAILED_RESULT;
1189 }
1190
1191 #if CONFIG_RTC
1192 static void receive_time(void)
1193 {
1194     usb_drv_recv(ep_out, tb.transfer_buffer, 12);
1195     state = RECEIVING_TIME;
1196 }
1197 #endif /* CONFIG_RTC */
1198
1199 static void receive_block_data(void *data,int size)
1200 {
1201     usb_drv_recv(ep_out, data, size);
1202     state = RECEIVING_BLOCKS;
1203 }
1204
1205 static void send_csw(int status)
1206 {
1207     tb.csw->signature = htole32(CSW_SIGNATURE);
1208     tb.csw->tag = cur_cmd.tag;
1209     tb.csw->data_residue = 0;
1210     tb.csw->status = status;
1211
1212     usb_drv_send_nonblocking(ep_in, tb.csw,
1213             sizeof(struct command_status_wrapper));
1214     state = WAITING_FOR_CSW_COMPLETION_OR_COMMAND;
1215     //logf("CSW: %X",status);
1216     /* Already start waiting for the next command */
1217     usb_drv_recv(ep_out, cbw_buffer, MAX_CBW_SIZE);
1218     /* The next completed transfer will be either the CSW one
1219      * or the new command */
1220
1221     if(status == UMS_STATUS_GOOD) {
1222         cur_sense_data.sense_key=0;
1223         cur_sense_data.information=0;
1224         cur_sense_data.asc=0;
1225         cur_sense_data.ascq=0;
1226     }
1227 }
1228
1229 static void copy_padded(char *dest, char *src, int len)
1230 {
1231    int i=0;
1232    while(i<len && src[i]!=0)
1233    {
1234       dest[i]=src[i];
1235       i++;
1236    }
1237    while(i<len)
1238    {
1239       dest[i]=' ';
1240       i++;
1241    }
1242 }
1243
1244 /* build SCSI INQUIRY */
1245 static void fill_inquiry(IF_MD_NONVOID(int lun))
1246 {
1247     struct storage_info info;
1248     memset(tb.inquiry, 0, sizeof(struct inquiry_data));
1249     storage_get_info(lun,&info);
1250     copy_padded(tb.inquiry->VendorId,info.vendor,sizeof(tb.inquiry->VendorId));
1251     copy_padded(tb.inquiry->ProductId,info.product,sizeof(tb.inquiry->ProductId));
1252     copy_padded(tb.inquiry->ProductRevisionLevel,info.revision,sizeof(tb.inquiry->ProductRevisionLevel));
1253
1254     tb.inquiry->DeviceType = DIRECT_ACCESS_DEVICE;
1255     tb.inquiry->AdditionalLength = 0x1f;
1256     memset(tb.inquiry->Reserved, 0, 3);
1257     tb.inquiry->Versions = 4; /* SPC-2 */
1258     tb.inquiry->Format = 2; /* SPC-2/3 inquiry format */
1259
1260     tb.inquiry->DeviceTypeModifier = DEVICE_REMOVABLE;
1261 }