modules/usb/usbhdfsd/usb_driver.c

   1 /*
   2  * usb_driver.c - USB Mass Storage Driver
   3  * se  usbmass-ufi10.pdf
   4  */
   5
   6 #include <sysclib.h>
   7 #include <thsemap.h>
   8 #include <stdio.h>
   9 #include <usbd.h>
  10 #include <usbd_macro.h>
  11
  12 //#define DEBUG  //comment out this line when not debugging
  13
  14 #include "mass_debug.h"
  15 #include "usbhd_common.h"
  16 #include "mass_stor.h"
  17 #include "scache.h"
  18 #include "part_driver.h"
  19
  20 #define getBI32(__buf) ((((u8 *) (__buf))[3] << 0) | (((u8 *) (__buf))[2] << 8) | (((u8 *) (__buf))[1] << 16) | (((u8 *) (__buf))[0] << 24))
  21
  22 #define USB_SUBCLASS_MASS_RBC           0x01
  23 #define USB_SUBCLASS_MASS_ATAPI         0x02
  24 #define USB_SUBCLASS_MASS_QIC           0x03
  25 #define USB_SUBCLASS_MASS_UFI           0x04
  26 #define USB_SUBCLASS_MASS_SFF_8070I     0x05
  27 #define USB_SUBCLASS_MASS_SCSI          0x06
  28
  29 #define USB_PROTOCOL_MASS_CBI           0x00
  30 #define USB_PROTOCOL_MASS_CBI_NO_CCI    0x01
  31 #define USB_PROTOCOL_MASS_BULK_ONLY     0x50
  32
  33 #define TAG_TEST_UNIT_READY     0
  34 #define TAG_REQUEST_SENSE       3
  35 #define TAG_INQUIRY             18
  36 #define TAG_READ_CAPACITY       37
  37 #define TAG_READ                40
  38 #define TAG_START_STOP_UNIT     33
  39 #define TAG_WRITE               42
  40
  41 #define DEVICE_DETECTED         1
  42 #define DEVICE_CONFIGURED       2
  43
  44 #define CBW_TAG 0x43425355
  45 #define CSW_TAG 0x53425355
  46
  47 typedef struct _cbw_packet {
  48         unsigned int signature;
  49         unsigned int tag;
  50         unsigned int dataTransferLength;
  51         unsigned char flags;            //80->data in,  00->out
  52         unsigned char lun;
  53         unsigned char comLength;                //command data length
  54         unsigned char comData[16];              //command data
  55 } cbw_packet __attribute__((packed));
  56
  57 typedef struct _csw_packet {
  58         unsigned int signature;
  59         unsigned int tag;
  60         unsigned int dataResidue;
  61         unsigned char status;
  62 } csw_packet __attribute__((packed));
  63
  64 typedef struct _inquiry_data {
  65     u8 peripheral_device_type;  // 00h - Direct access (Floppy), 1Fh none (no FDD connected)
  66     u8 removable_media; // 80h - removeable
  67     u8 iso_ecma_ansi;
  68     u8 repsonse_data_format;
  69     u8 additional_length;
  70     u8 res[3];
  71     u8 vendor[8];
  72     u8 product[16];
  73     u8 revision[4];
  74 } inquiry_data __attribute__((packed));
  75
  76 typedef struct _sense_data {
  77     u8 error_code;
  78     u8 res1;
  79     u8 sense_key;
  80     u8 information[4];
  81     u8 add_sense_len;
  82     u8 res3[4];
  83     u8 add_sense_code;
  84     u8 add_sense_qual;
  85     u8 res4[4];
  86 } sense_data __attribute__((packed));
  87
  88 typedef struct _read_capacity_data {
  89     u8 last_lba[4];
  90     u8 block_length[4];
  91 } read_capacity_data __attribute__((packed));
  92
  93 static UsbDriver driver;
  94
  95 //volatile int wait_for_connect = 1;
  96
  97 typedef struct _usb_callback_data {
  98     int semh;
  99     int returnCode;
 100     int returnSize;
 101 } usb_callback_data;
 102
 103 #define NUM_DEVICES 5
 104 static mass_dev g_mass_device[NUM_DEVICES];
 105
 106 static void usb_callback(int resultCode, int bytes, void *arg)
 107 {
 108         usb_callback_data* data = (usb_callback_data*) arg;
 109         data->returnCode = resultCode;
 110         data->returnSize = bytes;
 111         XPRINTF("USBHDFSD: callback: res %d, bytes %d, arg %p \n", resultCode, bytes, arg);
 112         SignalSema(data->semh);
 113 }
 114
 115 void usb_set_configuration(mass_dev* dev, int configNumber) {
 116         int ret;
 117         usb_callback_data cb_data;
 118         iop_sema_t s;
 119
 120         s.initial = 0;
 121         s.max = 1;
 122         s.option = 0;
 123         s.attr = 0;
 124         cb_data.semh = CreateSema(&s);
 125
 126         XPRINTF("USBHDFSD: setting configuration controlEp=%i, confNum=%i \n", dev->controlEp, configNumber);
 127         ret = UsbSetDeviceConfiguration(dev->controlEp, configNumber, usb_callback, (void*)&cb_data);
 128
 129         if (ret != USB_RC_OK) {
 130                 printf("USBHDFSD: Error - sending set_configuration %d\n", ret);
 131         } else {
 132                 WaitSema(cb_data.semh);
 133         }
 134         DeleteSema(cb_data.semh);
 135 }
 136
 137 void usb_set_interface(mass_dev* dev, int interface, int altSetting) {
 138         int ret;
 139         usb_callback_data cb_data;
 140         iop_sema_t s;
 141
 142         s.initial = 0;
 143         s.max = 1;
 144         s.option = 0;
 145         s.attr = 0;
 146         cb_data.semh = CreateSema(&s);
 147
 148         XPRINTF("USBHDFSD: setting interface controlEp=%i, interface=%i altSetting=%i\n", dev->controlEp, interface, altSetting);
 149         ret = UsbSetInterface(dev->controlEp, interface, altSetting, usb_callback, (void*)&cb_data);
 150
 151     if (ret != USB_RC_OK) {
 152                 printf("USBHDFSD: Error - sending set_interface %d\n", ret);
 153         } else {
 154                 WaitSema(cb_data.semh);
 155         }
 156         DeleteSema(cb_data.semh);
 157 }
 158
 159 void usb_set_device_feature(mass_dev* dev, int feature) {
 160         int ret;
 161         usb_callback_data cb_data;
 162         iop_sema_t s;
 163
 164         s.initial = 0;
 165         s.max = 1;
 166         s.option = 0;
 167         s.attr = 0;
 168         cb_data.semh = CreateSema(&s);
 169
 170         XPRINTF("USBHDFSD: setting device feature controlEp=%i, feature=%i\n", dev->controlEp, feature);
 171         ret = UsbSetDeviceFeature(dev->controlEp, feature, usb_callback, (void*)&cb_data);
 172
 173         if (ret != USB_RC_OK) {
 174                 printf("USBHDFSD: Error - sending set_device feature %d\n", ret);
 175         } else {
 176                 WaitSema(cb_data.semh);
 177         }
 178         DeleteSema(cb_data.semh);
 179 }
 180
 181 void usb_bulk_clear_halt(mass_dev* dev, int direction) {
 182         int ret;
 183         usb_callback_data cb_data;
 184         iop_sema_t s;
 185         int endpoint;
 186
 187         s.initial = 0;
 188         s.max = 1;
 189         s.option = 0;
 190         s.attr = 0;
 191         cb_data.semh = CreateSema(&s);
 192
 193         if (direction == 0) {
 194                 endpoint = dev->bulkEpIAddr;
 195                 //endpoint = dev->bulkEpI;
 196         } else {
 197                 endpoint = dev->bulkEpOAddr;
 198         }
 199
 200         ret = UsbClearEndpointFeature(
 201                 dev->controlEp,                 //Config pipe
 202                 0,                      //HALT feature
 203                 endpoint,
 204                 usb_callback,
 205                 (void*)&cb_data
 206                 );
 207
 208         if (ret != USB_RC_OK) {
 209                 printf("USBHDFSD: Error - sending clear halt %d\n", ret);
 210         } else {
 211                 WaitSema(cb_data.semh);
 212         }
 213         DeleteSema(cb_data.semh);
 214 }
 215
 216 void usb_bulk_reset(mass_dev* dev, int mode) {
 217         int ret;
 218         usb_callback_data cb_data;
 219         iop_sema_t s;
 220
 221         s.initial = 0;
 222         s.max = 1;
 223         s.option = 0;
 224         s.attr = 0;
 225         cb_data.semh = CreateSema(&s);
 226
 227         //Call Bulk only mass storage reset
 228         ret = UsbControlTransfer(
 229                 dev->controlEp,                 //default pipe
 230                 0x21,                   //bulk reset
 231                 0xFF,
 232                 0,
 233                 dev->interfaceNumber, //interface number
 234                 0,                      //length
 235                 NULL,                   //data
 236                 usb_callback,
 237                 (void*) &cb_data
 238                 );
 239
 240         if (ret != USB_RC_OK) {
 241                 printf("USBHDFSD: Error - sending reset %d\n", ret);
 242         } else {
 243                 WaitSema(cb_data.semh);
 244
 245         //clear bulk-in endpoint
 246         if (mode & 0x01) {
 247                 usb_bulk_clear_halt(dev, 0);
 248         }
 249
 250         //clear bulk-out endpoint
 251         if (mode & 0x02) {
 252                 usb_bulk_clear_halt(dev, 1);
 253         }
 254         }
 255         DeleteSema(cb_data.semh);
 256 }
 257
 258 int usb_bulk_status(mass_dev* dev, csw_packet* csw, int tag) {
 259         int ret;
 260         iop_sema_t s;
 261         usb_callback_data cb_data;
 262
 263         s.initial = 0;
 264         s.max = 1;
 265         s.option = 0;
 266         s.attr = 0;
 267         cb_data.semh = CreateSema(&s);
 268
 269         csw->signature = CSW_TAG;
 270         csw->tag = tag;
 271         csw->dataResidue = 0;
 272         csw->status = 0;
 273
 274         ret = UsbBulkTransfer(
 275                 dev->bulkEpI,           //bulk input pipe
 276                 csw,                    //data ptr
 277                 13,                     //data length
 278                 usb_callback,
 279                 (void*)&cb_data
 280         );
 281
 282         if (ret != USB_RC_OK) {
 283                 printf("USBHDFSD: Error - sending bulk status %d\n", ret);
 284                 DeleteSema(cb_data.semh);
 285                 return -1;
 286         } else {
 287                 WaitSema(cb_data.semh);
 288         DeleteSema(cb_data.semh);
 289         if (cb_data.returnSize != 13)
 290             printf("USBHDFSD: bulk csw.status returnSize: %i\n", cb_data.returnSize);
 291         if (csw->dataResidue != 0)
 292             printf("USBHDFSD: bulk csw.status residue: %i\n", csw->dataResidue);
 293         XPRINTF("USBHDFSD: bulk csw.status: %i\n", csw->status);
 294         return csw->status;
 295         }
 296 }
 297
 298 /* see flow chart in the usbmassbulk_10.pdf doc (page 15) */
 299 int usb_bulk_manage_status(mass_dev* dev, int tag) {
 300         int ret;
 301         csw_packet csw;
 302
 303         //XPRINTF("USBHDFSD: usb_bulk_manage_status 1 ...\n");
 304         ret = usb_bulk_status(dev, &csw, tag); /* Attempt to read CSW from bulk in endpoint */
 305     if (ret < 0) { /* STALL bulk in  -OR- Bulk error */
 306                 usb_bulk_clear_halt(dev, 0); /* clear the stall condition for bulk in */
 307
 308                 XPRINTF("USBHDFSD: usb_bulk_manage_status stall ...\n");
 309                 ret = usb_bulk_status(dev, &csw, tag); /* Attempt to read CSW from bulk in endpoint */
 310         }
 311
 312         /* CSW not valid  or stalled or phase error */
 313         if (csw.signature != CSW_TAG || csw.tag != tag || ret == 2) {
 314                 printf("USBHDFSD: usb_bulk_manage_status call reset recovery ...\n");
 315                 usb_bulk_reset(dev, 3); /* Perform reset recovery */
 316         }
 317
 318     return ret;
 319 }
 320
 321 int usb_bulk_get_max_lun(mass_dev* dev) {
 322         int ret;
 323         usb_callback_data cb_data;
 324         iop_sema_t s;
 325         char max_lun;
 326
 327         s.initial = 0;
 328         s.max = 1;
 329         s.option = 0;
 330         s.attr = 0;
 331         cb_data.semh = CreateSema(&s);
 332
 333         //Call Bulk only mass storage reset
 334         ret = UsbControlTransfer(
 335                 dev->controlEp,                 //default pipe
 336                 0xA1,
 337                 0xFE,
 338                 0,
 339                 dev->interfaceNumber, //interface number
 340                 1,                      //length
 341                 &max_lun,                       //data
 342                 usb_callback,
 343                 (void*) &cb_data
 344                 );
 345
 346         if (ret != USB_RC_OK) {
 347                 printf("USBHDFSD: Error - get_max_lun %d\n", ret);
 348         max_lun = -ret;
 349         } else {
 350                 WaitSema(cb_data.semh);
 351         }
 352         DeleteSema(cb_data.semh);
 353         return max_lun;
 354 }
 355
 356 void usb_bulk_command(mass_dev* dev, cbw_packet* packet ) {
 357         int ret;
 358         iop_sema_t s;
 359         usb_callback_data cb_data;
 360
 361         s.initial = 0;
 362         s.max = 1;
 363         s.option = 0;
 364         s.attr = 0;
 365         cb_data.semh = CreateSema(&s);
 366
 367         ret =  UsbBulkTransfer(
 368                 dev->bulkEpO,           //bulk output pipe
 369                 packet,                 //data ptr
 370                 31,                     //data length
 371                 usb_callback,
 372                 (void*)&cb_data
 373         );
 374
 375         if (ret != USB_RC_OK) {
 376                 printf("USBHDFSD: Error - sending bulk command %d\n", ret);
 377         } else {
 378                 WaitSema(cb_data.semh);
 379         }
 380         DeleteSema(cb_data.semh);
 381 }
 382
 383 int usb_bulk_transfer(int pipe, void* buffer, int transferSize) {
 384         int ret;
 385         char* buf = (char*) buffer;
 386         int blockSize = transferSize;
 387         int offset = 0;
 388         iop_sema_t s;
 389         usb_callback_data cb_data;
 390
 391         s.initial = 0;
 392         s.max = 1;
 393         s.option = 0;
 394         s.attr = 0;
 395         cb_data.semh = CreateSema(&s);
 396
 397         while (transferSize > 0) {
 398                 if (transferSize < blockSize) {
 399                         blockSize = transferSize;
 400                 }
 401
 402                 ret = UsbBulkTransfer(
 403                         pipe,           //bulk pipe epI(Read)  epO(Write)
 404                         (buf + offset),         //data ptr
 405                         blockSize,              //data length
 406                         usb_callback,
 407                         (void*)&cb_data
 408             );
 409                 if (ret != USB_RC_OK) {
 410                         printf("USBHDFSD: Error - sending bulk data transfer %d\n", ret);
 411                         cb_data.returnCode = -1;
 412                         break;
 413                 } else {
 414                         WaitSema(cb_data.semh);
 415                         //XPRINTF("USBHDFSD: retCode=%i retSize=%i \n", cb_data.returnCode, cb_data.returnSize);
 416                         if (cb_data.returnCode > 0) {
 417                                 break;
 418                         }
 419                         offset += cb_data.returnSize;
 420                         transferSize-= cb_data.returnSize;
 421                 }
 422         }
 423         DeleteSema(cb_data.semh);
 424         return cb_data.returnCode;
 425 }
 426
 427 inline int cbw_scsi_test_unit_ready(mass_dev* dev)
 428 {
 429     int ret;
 430     cbw_packet cbw;
 431
 432     XPRINTF("USBHDFSD: cbw_scsi_test_unit_ready\n");
 433
 434     cbw.signature = CBW_TAG;
 435     cbw.lun = 0;
 436         cbw.tag = -TAG_TEST_UNIT_READY;
 437         cbw.dataTransferLength = 0;             //TUR_REPLY_LENGTH
 438         cbw.flags = 0x80;                       //data will flow In
 439         cbw.comLength = 12;
 440
 441         //scsi command packet
 442         cbw.comData[0] = 0x00;          //test unit ready operation code
 443         cbw.comData[1] = 0;                     //lun/reserved
 444         cbw.comData[2] = 0;                     //reserved
 445         cbw.comData[3] = 0;                     //reserved
 446         cbw.comData[4] = 0;                     //reserved
 447         cbw.comData[5] = 0;                     //reserved
 448         cbw.comData[6] = 0;                     //reserved
 449         cbw.comData[7] = 0;                     //reserved
 450         cbw.comData[8] = 0;                     //reserved
 451         cbw.comData[9] = 0;                     //reserved
 452         cbw.comData[10] = 0;            //reserved
 453         cbw.comData[11] = 0;            //reserved
 454
 455     usb_bulk_command(dev, &cbw);
 456
 457     ret = usb_bulk_manage_status(dev, -TAG_TEST_UNIT_READY);
 458     return ret;
 459 }
 460
 461 inline int cbw_scsi_request_sense(mass_dev* dev, void *buffer, int size)
 462 {
 463     int ret;
 464     cbw_packet cbw;
 465
 466     XPRINTF("USBHDFSD: cbw_scsi_request_sense\n");
 467
 468     cbw.signature = CBW_TAG;
 469     cbw.lun = 0;
 470         cbw.tag = -TAG_REQUEST_SENSE;
 471         cbw.dataTransferLength = size;  //REQUEST_SENSE_REPLY_LENGTH
 472         cbw.flags = 0x80;                       //sense data will flow In
 473         cbw.comLength = 12;
 474
 475         //scsi command packet
 476         cbw.comData[0] = 0x03;          //request sense operation code
 477         cbw.comData[1] = 0;                     //lun/reserved
 478         cbw.comData[2] = 0;                     //reserved
 479         cbw.comData[3] = 0;                     //reserved
 480         cbw.comData[4] = size;          //allocation length
 481         cbw.comData[5] = 0;                     //reserved
 482         cbw.comData[6] = 0;                     //reserved
 483         cbw.comData[7] = 0;                     //reserved
 484         cbw.comData[8] = 0;                     //reserved
 485         cbw.comData[9] = 0;                     //reserved
 486         cbw.comData[10] = 0;            //reserved
 487         cbw.comData[11] = 0;            //reserved
 488
 489     usb_bulk_command(dev, &cbw);
 490
 491     ret = usb_bulk_transfer(dev->bulkEpI, buffer, size);
 492     if (ret != 0)
 493         printf("USBHDFSD: cbw_scsi_request_sense error from usb_bulk_transfer %d\n", ret);
 494
 495     ret = usb_bulk_manage_status(dev, -TAG_REQUEST_SENSE);
 496     return ret;
 497 }
 498
 499 inline int cbw_scsi_inquiry(mass_dev* dev, void *buffer, int size)
 500 {
 501     int ret;
 502     cbw_packet cbw;
 503
 504     XPRINTF("USBHDFSD: cbw_scsi_inquiry\n");
 505
 506     cbw.signature = CBW_TAG;
 507     cbw.lun = 0;
 508         cbw.tag = -TAG_INQUIRY;
 509         cbw.dataTransferLength = size;  //INQUIRY_REPLY_LENGTH
 510         cbw.flags = 0x80;                       //inquiry data will flow In
 511         cbw.comLength = 12;
 512
 513         //scsi command packet
 514         cbw.comData[0] = 0x12;          //inquiry operation code
 515         cbw.comData[1] = 0;                     //lun/reserved
 516         cbw.comData[2] = 0;                     //page code
 517         cbw.comData[3] = 0;                     //reserved
 518         cbw.comData[4] = size;          //inquiry reply length
 519         cbw.comData[5] = 0;                     //reserved
 520         cbw.comData[6] = 0;                     //reserved
 521         cbw.comData[7] = 0;                     //reserved
 522         cbw.comData[8] = 0;                     //reserved
 523         cbw.comData[9] = 0;                     //reserved
 524         cbw.comData[10] = 0;            //reserved
 525         cbw.comData[11] = 0;            //reserved
 526
 527         usb_bulk_command(dev, &cbw);
 528
 529         ret = usb_bulk_transfer(dev->bulkEpI, buffer, size);
 530     if (ret != 0)
 531         printf("USBHDFSD: cbw_scsi_inquiry error from usb_bulk_transfer %d\n", ret);
 532
 533         usb_bulk_manage_status(dev, -TAG_INQUIRY);
 534
 535     return ret; // TODO What to return???
 536 }
 537
 538 inline int cbw_scsi_start_stop_unit(mass_dev* dev)
 539 {
 540     int ret;
 541     cbw_packet cbw;
 542
 543     XPRINTF("USBHDFSD: cbw_scsi_start_stop_unit\n");
 544
 545     cbw.signature = CBW_TAG;
 546     cbw.lun = 0;
 547         cbw.tag = -TAG_START_STOP_UNIT;
 548         cbw.dataTransferLength = 0;     //START_STOP_REPLY_LENGTH
 549         cbw.flags = 0x80;                       //inquiry data will flow In
 550         cbw.comLength = 12;
 551
 552         //scsi command packet
 553         cbw.comData[0] = 0x1B;          //start stop unit operation code
 554         cbw.comData[1] = 1;                     //lun/reserved/immed
 555         cbw.comData[2] = 0;                     //reserved
 556         cbw.comData[3] = 0;                     //reserved
 557         cbw.comData[4] = 1;                     //reserved/LoEj/Start "Start the media and acquire the format type"
 558         cbw.comData[5] = 0;                     //reserved
 559         cbw.comData[6] = 0;                     //reserved
 560         cbw.comData[7] = 0;                     //reserved
 561         cbw.comData[8] = 0;                     //reserved
 562         cbw.comData[9] = 0;                     //reserved
 563         cbw.comData[10] = 0;            //reserved
 564         cbw.comData[11] = 0;            //reserved
 565
 566     usb_bulk_command(dev, &cbw);
 567
 568     ret = usb_bulk_manage_status(dev, -TAG_START_STOP_UNIT);
 569     return ret;
 570 }
 571
 572 inline int cbw_scsi_read_capacity(mass_dev* dev, void *buffer, int size)
 573 {
 574     int ret;
 575     cbw_packet cbw;
 576     int retryCount;
 577
 578     XPRINTF("USBHDFSD: cbw_scsi_read_capacity\n");
 579
 580     cbw.signature = CBW_TAG;
 581     cbw.lun = 0;
 582         cbw.tag = -TAG_READ_CAPACITY;
 583         cbw.dataTransferLength = size;  //READ_CAPACITY_REPLY_LENGTH
 584         cbw.flags = 0x80;                       //inquiry data will flow In
 585         cbw.comLength = 12;
 586
 587         //scsi command packet
 588         cbw.comData[0] = 0x25;          //read capacity operation code
 589         cbw.comData[1] = 0;                     //lun/reserved/RelAdr
 590         cbw.comData[2] = 0;                     //LBA 1
 591         cbw.comData[3] = 0;                     //LBA 2
 592         cbw.comData[4] = 0;                     //LBA 3
 593         cbw.comData[5] = 0;                     //LBA 4
 594         cbw.comData[6] = 0;                     //Reserved
 595         cbw.comData[7] = 0;                     //Reserved
 596         cbw.comData[8] = 0;                     //Reserved/PMI
 597         cbw.comData[9] = 0;                     //reserved
 598         cbw.comData[10] = 0;            //reserved
 599         cbw.comData[11] = 0;            //reserved
 600
 601         ret = 1;
 602         retryCount = 6;
 603         while (ret != 0 && retryCount > 0) {
 604                 usb_bulk_command(dev, &cbw);
 605
 606                 ret = usb_bulk_transfer(dev->bulkEpI, buffer, size);
 607         if (ret != 0)
 608             printf("USBHDFSD: cbw_scsi_read_capacity error from usb_bulk_transfer %d\n", ret);
 609
 610                 //HACK HACK HACK !!!
 611                 //according to usb doc we should allways
 612                 //attempt to read the CSW packet. But in some cases
 613                 //reading of CSW packet just freeze ps2....:-(
 614         if (ret == USB_RC_STALL) {
 615             XPRINTF("USBHDFSD: call reset recovery ...\n");
 616                         usb_bulk_reset(dev, 1);
 617                 } else {
 618                         ret = usb_bulk_manage_status(dev, -TAG_READ_CAPACITY);
 619                 }
 620                 retryCount--;
 621         }
 622     return ret;
 623 }
 624
 625 inline int cbw_scsi_read_sector(mass_dev* dev, int lba, void* buffer, int sectorSize, int sectorCount)
 626 {
 627     int ret;
 628     cbw_packet cbw;
 629
 630     XPRINTF("USBHDFSD: cbw_scsi_read_sector\n");
 631
 632     cbw.signature = CBW_TAG;
 633     cbw.lun = 0;
 634         cbw.tag = -TAG_READ;
 635         cbw.dataTransferLength = sectorSize * sectorCount;
 636         cbw.flags = 0x80;                       //read data will flow In
 637         cbw.comLength = 12;
 638
 639         //scsi command packet
 640         cbw.comData[0] = 0x28;          //read operation code
 641         cbw.comData[1] = 0;                     //LUN/DPO/FUA/Reserved/Reldr
 642         cbw.comData[2] = (lba & 0xFF000000) >> 24;      //lba 1 (MSB)
 643         cbw.comData[3] = (lba & 0xFF0000) >> 16;        //lba 2
 644         cbw.comData[4] = (lba & 0xFF00) >> 8;   //lba 3
 645         cbw.comData[5] = (lba & 0xFF);          //lba 4 (LSB)
 646         cbw.comData[6] = 0;                     //Reserved
 647         cbw.comData[7] = (sectorCount & 0xFF00) >> 8;   //Transfer length MSB
 648         cbw.comData[8] = (sectorCount & 0xFF);                  //Transfer length LSB
 649         cbw.comData[9] = 0;                     //reserved
 650         cbw.comData[10] = 0;            //reserved
 651         cbw.comData[11] = 0;            //reserved
 652
 653     usb_bulk_command(dev, &cbw);
 654
 655     ret = usb_bulk_transfer(dev->bulkEpI, buffer, sectorSize * sectorCount);
 656     if (ret != 0)
 657         printf("USBHDFSD: cbw_scsi_read_sector error from usb_bulk_transfer %d\n", ret);
 658
 659     ret = usb_bulk_manage_status(dev, -TAG_READ);
 660     return ret;
 661 }
 662
 663 #ifdef WRITE_SUPPORT
 664 inline int cbw_scsi_write_sector(mass_dev* dev, int lba, void* buffer, int sectorSize, int sectorCount)
 665 {
 666     int ret;
 667     cbw_packet cbw;
 668
 669     XPRINTF("USBHDFSD: cbw_scsi_write_sector\n");
 670
 671     cbw.signature = CBW_TAG;
 672     cbw.lun = 0;
 673         cbw.tag = -TAG_WRITE;
 674         cbw.dataTransferLength = sectorSize      * sectorCount;
 675         cbw.flags = 0x00;                       //write data will flow Out
 676         cbw.comLength = 12;
 677
 678         //scsi command packet
 679         cbw.comData[0] = 0x2A;          //write operation code
 680         cbw.comData[1] = 0;                     //LUN/DPO/FUA/Reserved/Reldr
 681         cbw.comData[2] = (lba & 0xFF000000) >> 24;      //lba 1 (MSB)
 682         cbw.comData[3] = (lba & 0xFF0000) >> 16;        //lba 2
 683         cbw.comData[4] = (lba & 0xFF00) >> 8;   //lba 3
 684         cbw.comData[5] = (lba & 0xFF);          //lba 4 (LSB)
 685         cbw.comData[6] = 0;                     //Reserved
 686         cbw.comData[7] = (sectorCount & 0xFF00) >> 8;   //Transfer length MSB
 687         cbw.comData[8] = (sectorCount & 0xFF);                  //Transfer length LSB
 688         cbw.comData[9] = 0;                     //reserved
 689         cbw.comData[10] = 0;            //reserved
 690         cbw.comData[11] = 0;            //reserved
 691
 692     usb_bulk_command(dev, &cbw);
 693
 694     ret = usb_bulk_transfer(dev->bulkEpO, buffer, sectorSize * sectorCount);
 695     if (ret != 0)
 696         printf("USBHDFSD: cbw_scsi_write_sector error from usb_bulk_transfer %d\n", ret);
 697
 698     ret = usb_bulk_manage_status(dev, -TAG_WRITE);
 699     return ret;
 700 }
 701 #endif
 702
 703 mass_dev* mass_stor_findDevice(int devId, int create)
 704 {
 705     mass_dev* dev = NULL;
 706     int i;
 707     XPRINTF("USBHDFSD: mass_stor_findDevice devId %i\n", devId);
 708     for (i = 0; i < NUM_DEVICES; ++i)
 709     {
 710         if (g_mass_device[i].devId == devId)
 711         {
 712             XPRINTF("USBHDFSD: mass_stor_findDevice exists %i\n", i);
 713             return &g_mass_device[i];
 714         }
 715         else if (create && dev == NULL && g_mass_device[i].devId == -1)
 716         {
 717             dev = &g_mass_device[i];
 718         }
 719     }
 720     return dev;
 721 }
 722
 723 /* size should be a multiple of sector size */
 724 int mass_stor_readSector(mass_dev* mass_device, unsigned int sector, unsigned char* buffer, int size) {
 725     //assert(size % mass_device->sectorSize == 0);
 726     //assert(sector <= mass_device->maxLBA);
 727     int ret;
 728
 729         ret = 1;
 730         while (ret != 0) {
 731         ret = cbw_scsi_read_sector(mass_device, sector, buffer, mass_device->sectorSize, size/mass_device->sectorSize);
 732         }
 733         return (size / mass_device->sectorSize) * mass_device->sectorSize;
 734 }
 735
 736 #ifdef WRITE_SUPPORT
 737 /* size should be a multiple of sector size */
 738 int mass_stor_writeSector(mass_dev* mass_device, unsigned int sector, unsigned char* buffer, int size) {
 739     //assert(size % mass_device->sectorSize == 0);
 740     //assert(sector <= mass_device->maxLBA);
 741     int ret;
 742
 743         ret = 1;
 744         while (ret != 0) {
 745         ret = cbw_scsi_write_sector(mass_device, sector, buffer, mass_device->sectorSize, size/mass_device->sectorSize);
 746         }
 747         return (size / mass_device->sectorSize) * mass_device->sectorSize;
 748 }
 749 #endif
 750
 751 /* test that endpoint is bulk endpoint and if so, update device info */
 752 void usb_bulk_probeEndpoint(int devId, mass_dev* dev, UsbEndpointDescriptor* endpoint) {
 753         if (endpoint->bmAttributes == USB_ENDPOINT_XFER_BULK) {
 754                 /* out transfer */
 755                 if ((endpoint->bEndpointAddress & 0x80) == 0 && dev->bulkEpO < 0) {
 756                         dev->bulkEpOAddr = endpoint->bEndpointAddress;
 757                         dev->bulkEpO = UsbOpenEndpointAligned(devId, endpoint);
 758                         dev->packetSzO = endpoint->wMaxPacketSizeHB * 256 + endpoint->wMaxPacketSizeLB;
 759                         XPRINTF("USBHDFSD: register Output endpoint id =%i addr=%02X packetSize=%i\n", dev->bulkEpO,dev->bulkEpOAddr, dev->packetSzO);
 760                 } else
 761                 /* in transfer */
 762                 if ((endpoint->bEndpointAddress & 0x80) != 0 && dev->bulkEpI < 0) {
 763                         dev->bulkEpIAddr = endpoint->bEndpointAddress;
 764                         dev->bulkEpI = UsbOpenEndpointAligned(devId, endpoint);
 765                         dev->packetSzI = endpoint->wMaxPacketSizeHB * 256 + endpoint->wMaxPacketSizeLB;
 766                         XPRINTF("USBHDFSD: register Input endpoint id =%i addr=%02X packetSize=%i\n", dev->bulkEpI, dev->bulkEpIAddr, dev->packetSzI);
 767                 }
 768         }
 769 }
 770
 771
 772 int mass_stor_probe(int devId)
 773 {
 774         UsbDeviceDescriptor *device = NULL;
 775         UsbConfigDescriptor *config = NULL;
 776         UsbInterfaceDescriptor *intf = NULL;
 777
 778         XPRINTF("USBHDFSD: probe: devId=%i\n", devId);
 779
 780     mass_dev* mass_device = mass_stor_findDevice(devId, 0);
 781
 782         /* only one device supported */
 783         if ((mass_device != NULL) && (mass_device->status & DEVICE_DETECTED))
 784         {
 785                 printf("USBHDFSD: Error - only one mass storage device allowed ! \n");
 786                 return 0;
 787         }
 788
 789         /* get device descriptor */
 790         device = (UsbDeviceDescriptor*)UsbGetDeviceStaticDescriptor(devId, NULL, USB_DT_DEVICE);
 791         if (device == NULL)  {
 792                 printf("USBHDFSD: Error - Couldn't get device descriptor\n");
 793                 return 0;
 794         }
 795
 796         /* Check if the device has at least one configuration */
 797         if (device->bNumConfigurations < 1) { return 0; }
 798
 799         /* read configuration */
 800         config = (UsbConfigDescriptor*)UsbGetDeviceStaticDescriptor(devId, device, USB_DT_CONFIG);
 801         if (config == NULL) {
 802             printf("USBHDFSD: Error - Couldn't get configuration descriptor\n");
 803             return 0;
 804         }
 805         /* check that at least one interface exists */
 806     XPRINTF("USBHDFSD: bNumInterfaces %d\n", config->bNumInterfaces);
 807         if ((config->bNumInterfaces < 1) ||
 808                 (config->wTotalLength < (sizeof(UsbConfigDescriptor) + sizeof(UsbInterfaceDescriptor)))) {
 809                     printf("USBHDFSD: Error - No interfaces available\n");
 810                     return 0;
 811         }
 812         /* get interface */
 813         intf = (UsbInterfaceDescriptor *) ((char *) config + config->bLength); /* Get first interface */
 814     XPRINTF("USBHDFSD: bInterfaceClass %X bInterfaceSubClass %X bInterfaceProtocol %X\n",
 815         intf->bInterfaceClass, intf->bInterfaceSubClass, intf->bInterfaceProtocol);
 816
 817         if ((intf->bInterfaceClass              != USB_CLASS_MASS_STORAGE) ||
 818                 (intf->bInterfaceSubClass       != USB_SUBCLASS_MASS_SCSI  &&
 819                  intf->bInterfaceSubClass       != USB_SUBCLASS_MASS_SFF_8070I) ||
 820                 (intf->bInterfaceProtocol       != USB_PROTOCOL_MASS_BULK_ONLY) ||
 821                 (intf->bNumEndpoints < 2))    { //one bulk endpoint is not enough because
 822                         return 0;               //we send the CBW to te bulk out endpoint
 823         }
 824         return 1;
 825 }
 826
 827 int mass_stor_connect(int devId)
 828 {
 829         int i;
 830         int epCount;
 831         UsbDeviceDescriptor *device;
 832         UsbConfigDescriptor *config;
 833         UsbInterfaceDescriptor *interface;
 834         UsbEndpointDescriptor *endpoint;
 835         mass_dev* dev;
 836
 837         //wait_for_connect = 0;
 838
 839         printf("USBHDFSD: connect: devId=%i\n", devId);
 840         dev = mass_stor_findDevice(devId, 1);
 841
 842         if (dev == NULL) {
 843                 printf("USBHDFSD: Error - unable to allocate space!\n");
 844                 return 1;
 845         }
 846
 847         /* only one mass device allowed */
 848         if (dev->devId != -1) {
 849                 printf("USBHDFSD: Error - only one mass storage device allowed !\n");
 850                 return 1;
 851         }
 852
 853         dev->status = 0;
 854         dev->sectorSize = 0;
 855
 856         dev->bulkEpI = -1;
 857         dev->bulkEpO = -1;
 858
 859         /* open the config endpoint */
 860         dev->controlEp = UsbOpenEndpoint(devId, NULL);
 861
 862         device = (UsbDeviceDescriptor*)UsbGetDeviceStaticDescriptor(devId, NULL, USB_DT_DEVICE);
 863
 864         config = (UsbConfigDescriptor*)UsbGetDeviceStaticDescriptor(devId, device, USB_DT_CONFIG);
 865
 866         interface = (UsbInterfaceDescriptor *) ((char *) config + config->bLength); /* Get first interface */
 867
 868         // store interface numbers
 869         dev->interfaceNumber = interface->bInterfaceNumber;
 870         dev->interfaceAlt    = interface->bAlternateSetting;
 871
 872         epCount = interface->bNumEndpoints;
 873         endpoint = (UsbEndpointDescriptor*) UsbGetDeviceStaticDescriptor(devId, NULL, USB_DT_ENDPOINT);
 874         usb_bulk_probeEndpoint(devId, dev, endpoint);
 875
 876         for (i = 1; i < epCount; i++)
 877         {
 878                 endpoint = (UsbEndpointDescriptor*) ((char *) endpoint + endpoint->bLength);
 879                 usb_bulk_probeEndpoint(devId, dev, endpoint);
 880         }
 881
 882         /* we do NOT have enough bulk endpoints */
 883         if (dev->bulkEpI < 0  /* || dev->bulkEpO < 0 */ ) { /* the bulkOut is not needed now */
 884                 if (dev->bulkEpI >= 0) {
 885                         UsbCloseEndpoint(dev->bulkEpI);
 886                 }
 887                 if (dev->bulkEpO >= 0) {
 888                         UsbCloseEndpoint(dev->bulkEpO);
 889                 }
 890                 printf("USBHDFSD: Error - connect failed: not enough bulk endpoints! \n");
 891                 return -1;
 892         }
 893
 894         /*store current configuration id - can't call set_configuration here */
 895         dev->devId = devId;
 896         dev->configId = config->bConfigurationValue;
 897         dev->status = DEVICE_DETECTED;
 898         XPRINTF("USBHDFSD: connect ok: epI=%i, epO=%i \n", dev->bulkEpI, dev->bulkEpO);
 899
 900         return 0;
 901 }
 902
 903 void mass_stor_release(mass_dev *dev)
 904 {
 905         if (dev->bulkEpI >= 0)
 906         {
 907                 UsbCloseEndpoint(dev->bulkEpI);
 908         }
 909
 910         if (dev->bulkEpO >= 0)
 911         {
 912                 UsbCloseEndpoint(dev->bulkEpO);
 913         }
 914
 915         dev->bulkEpI = -1;
 916         dev->bulkEpO = -1;
 917         dev->controlEp = -1;
 918         dev->status = 0;
 919 }
 920
 921 int mass_stor_disconnect(int devId) {
 922         mass_dev* dev;
 923         dev = mass_stor_findDevice(devId, 0);
 924
 925         printf("USBHDFSD: disconnect: devId=%i\n", devId);
 926
 927     if (dev == NULL)
 928     {
 929         printf("USBHDFSD: Error - disconnect: no device storage!\n");
 930         return 0;
 931     }
 932
 933         if ((dev->status & DEVICE_DETECTED) && devId == dev->devId)
 934         {
 935                 mass_stor_release(dev);
 936                 part_disconnect(dev);
 937                 scache_kill(dev->cache);
 938                 dev->cache = NULL;
 939                 dev->devId = -1;
 940         }
 941         return 0;
 942 }
 943
 944
 945 int mass_stor_warmup(mass_dev *dev) {
 946     inquiry_data id;
 947     sense_data sd;
 948     read_capacity_data rcd;
 949     int stat;
 950
 951     XPRINTF("USBHDFSD: mass_stor_warmup\n");
 952
 953     if (!(dev->status & DEVICE_DETECTED)) {
 954         printf("USBHDFSD: Error - no mass storage device found!\n");
 955         return -1;
 956     }
 957
 958     stat = usb_bulk_get_max_lun(dev);
 959     XPRINTF("USBHDFSD: usb_bulk_get_max_lun %d\n", stat);
 960
 961     memset(&id, 0, sizeof(inquiry_data));
 962     if ((stat = cbw_scsi_inquiry(dev, &id, sizeof(inquiry_data))) < 0) {
 963         printf("USBHDFSD: Error - cbw_scsi_inquiry %d\n", stat);
 964         return -1;
 965     }
 966
 967     printf("USBHDFSD: Vendor: %.8s\n", id.vendor);
 968     printf("USBHDFSD: Product: %.16s\n", id.product);
 969     printf("USBHDFSD: Revision: %.4s\n", id.revision);
 970
 971     while((stat = cbw_scsi_test_unit_ready(dev)) != 0)
 972     {
 973         printf("USBHDFSD: Error - cbw_scsi_test_unit_ready %d\n", stat);
 974
 975         stat = cbw_scsi_request_sense(dev, &sd, sizeof(sense_data));
 976         if (stat != 0)
 977             printf("USBHDFSD: Error - cbw_scsi_request_sense %d\n", stat);
 978
 979         if ((sd.error_code == 0x70) && (sd.sense_key != 0x00))
 980         {
 981             printf("USBHDFSD: Sense Data key: %02X code: %02X qual: %02X\n", sd.sense_key, sd.add_sense_code, sd.add_sense_qual);
 982
 983             if ((sd.sense_key == 0x02) && (sd.add_sense_code == 0x04) && (sd.add_sense_qual == 0x02))
 984             {
 985                 printf("USBHDFSD: Error - Additional initalization is required for this device!\n");
 986                 if ((stat = cbw_scsi_start_stop_unit(dev)) != 0) {
 987                     printf("USBHDFSD: Error - cbw_scsi_start_stop_unit %d\n", stat);
 988                     return -1;
 989                 }
 990             }
 991         }
 992     }
 993
 994     stat = cbw_scsi_request_sense(dev, &sd, sizeof(sense_data));
 995     if (stat != 0)
 996         printf("USBHDFSD: Error - cbw_scsi_request_sense %d\n", stat);
 997
 998     if ((sd.error_code == 0x70) && (sd.sense_key != 0x00))
 999     {
1000         printf("USBHDFSD: Sense Data key: %02X code: %02X qual: %02X\n", sd.sense_key, sd.add_sense_code, sd.add_sense_qual);
1001     }
1002
1003     if ((stat = cbw_scsi_read_capacity(dev, &rcd, sizeof(read_capacity_data))) != 0) {
1004         printf("USBHDFSD: Error - cbw_scsi_read_capacity %d\n", stat);
1005         return -1;
1006     }
1007
1008     dev->sectorSize = getBI32(&rcd.block_length);
1009     dev->maxLBA     = getBI32(&rcd.last_lba);
1010     printf("USBHDFSD: sectorSize %d maxLBA %d\n", dev->sectorSize, dev->maxLBA);
1011
1012     return 0;
1013 }
1014
1015 int mass_stor_configureNextDevice()
1016 {
1017         int i;
1018
1019     // give the USB driver some time to detect the device
1020     //i = 10000;
1021     //while (wait_for_connect && (--i > 0))
1022         //DelayThread(100);
1023     //wait_for_connect = 0;
1024
1025         XPRINTF("USBHDFSD: configuring devices... \n");
1026
1027     for (i = 0; i < NUM_DEVICES; ++i)
1028     {
1029         mass_dev *dev = &g_mass_device[i];
1030         if (dev->devId != -1 && (dev->status & DEVICE_DETECTED) && !(dev->status & DEVICE_CONFIGURED))
1031         {
1032             int ret;
1033             usb_set_configuration(dev, dev->configId);
1034             usb_set_interface(dev, dev->interfaceNumber, dev->interfaceAlt);
1035             dev->status |= DEVICE_CONFIGURED;
1036
1037             ret = mass_stor_warmup(dev);
1038             if (ret < 0)
1039             {
1040                 printf("USBHDFSD: Error - failed to warmup device %d\n", dev->devId);
1041                 mass_stor_release(dev);
1042                 continue;
1043             }
1044
1045             dev->cache = scache_init(dev, dev->sectorSize); // modified by Hermes
1046             if (dev->cache == NULL) {
1047                 printf("USBHDFSD: Error - scache_init failed \n" );
1048                 continue;
1049             }
1050
1051             return part_connect(dev) >= 0;
1052         }
1053     }
1054     return 0;
1055 }
1056
1057 int mass_stor_sectorsize(mass_dev *dev)
1058 {
1059     XPRINTF("USBHDFSD: mass_stor_sectorsize\n");
1060
1061     if (!(dev->status & DEVICE_DETECTED)) {
1062         printf("USBHDFSD: Error - no mass storage device found!\n");
1063
1064         return -1;
1065     }
1066
1067     return dev->sectorSize;
1068 }
1069
1070 int InitUSB()
1071 {
1072     int i;
1073         int ret = 0;
1074     for (i = 0; i < NUM_DEVICES; ++i)
1075         g_mass_device[i].devId = -1;
1076
1077         driver.next             = NULL;
1078         driver.prev                 = NULL;
1079         driver.name             = "mass-stor";
1080         driver.probe            = mass_stor_probe;
1081         driver.connect          = mass_stor_connect;
1082         driver.disconnect       = mass_stor_disconnect;
1083
1084         ret = UsbRegisterDriver(&driver);
1085         XPRINTF("USBHDFSD: registerDriver=%i \n", ret);
1086         if (ret < 0)
1087         {
1088                 printf("USBHDFSD: register driver failed! ret=%d\n", ret);
1089                 return(-1);
1090         }
1091
1092     return(0);
1093 }