usbstorage.c

   1 /*-------------------------------------------------------------
   2
   3 usbstorage.c -- Bulk-only USB mass storage support
   4
   5 Copyright (C) 2008
   6 Sven Peter (svpe) <svpe@gmx.net>
   7
   8 This software is provided 'as-is', without any express or implied
   9 warranty.  In no event will the authors be held liable for any
  10 damages arising from the use of this software.
  11
  12 Permission is granted to anyone to use this software for any
  13 purpose, including commercial applications, and to alter it and
  14 redistribute it freely, subject to the following restrictions:
  15
  16 1.      The origin of this software must not be misrepresented; you
  17 must not claim that you wrote the original software. If you use
  18 this software in a product, an acknowledgment in the product
  19 documentation would be appreciated but is not required.
  20
  21 2.      Altered source versions must be plainly marked as such, and
  22 must not be misrepresented as being the original software.
  23
  24 3.      This notice may not be removed or altered from any source
  25 distribution.
  26
  27 -------------------------------------------------------------*/
  28 #include <gccore.h>
  29 #include <stdlib.h>
  30 #include <string.h>
  31
  32 #include "usbstorage.h"
  33
  34 #define HEAP_SIZE                       4096
  35 #define TAG_START                       0x0BADC0DE
  36
  37 #define CBW_SIZE                        31
  38 #define CBW_SIGNATURE                   0x43425355
  39 #define CBW_IN                          (1 << 7)
  40 #define CBW_OUT                         0
  41
  42 #define CSW_SIZE                        13
  43 #define CSW_SIGNATURE                   0x53425355
  44
  45 #define SCSI_TEST_UNIT_READY            0x00
  46 #define SCSI_REQUEST_SENSE              0x03
  47 #define SCSI_READ_CAPACITY              0x25
  48 #define SCSI_READ_10                    0x28
  49 #define SCSI_WRITE_10                   0x2A
  50
  51 #define SCSI_SENSE_REPLY_SIZE           18
  52
  53 #define USB_CLASS_MASS_STORAGE          0x08
  54 #define MASS_STORAGE_SCSI_COMMANDS      0x06
  55 #define MASS_STORAGE_BULK_ONLY          0x50
  56
  57 #define USB_ENDPOINT_BULK               0x02
  58
  59 static s32 hId;
  60
  61 static inline u16 bswap16(u16 s)
  62 {
  63         return (s >> 8) | ((s & 0xFF) << 8);
  64 }
  65
  66 static inline s32 __usb_getdesc(s32 fd, u8 *buffer, u8 type, u8 index, u8 size)
  67 {
  68         return USB_WriteCtrlMsg(fd, USB_ENDPOINT_IN, USB_REQ_GETDESCRIPTOR, (type << 8) | index, 0, size, buffer);
  69 }
  70
  71 s32 USB_GetDescriptors(s32 fd, _usb_devdesc *udd)
  72 {
  73         u8 *buffer = NULL;
  74         u8 *ptr = NULL;
  75         usb_configurationdesc *ucd = NULL;
  76         usb_interfacedesc *uid = NULL;
  77         usb_endpointdesc *ued = NULL;
  78         s32 retval = 0;
  79         u32 iConf, iInterface, iEndpoint;
  80
  81         buffer = iosAlloc(hId, sizeof(*udd));
  82         if(buffer == NULL)
  83         {
  84                 retval = IPC_ENOHEAP;
  85                 goto free_and_error;
  86         }
  87
  88         retval = __usb_getdesc(fd, buffer, USB_DT_DEVICE, 0, USB_DT_DEVICE_SIZE);
  89         if(retval < 0)
  90                 goto free_and_error;
  91         memcpy(udd, buffer, USB_DT_DEVICE_SIZE);
  92         iosFree(hId, buffer);
  93
  94         udd->bcdUSB = bswap16(udd->bcdUSB);
  95         udd->idVendor = bswap16(udd->idVendor);
  96         udd->idProduct = bswap16(udd->idProduct);
  97         udd->bcdDevice = bswap16(udd->bcdDevice);
  98
  99         udd->configurations = calloc(udd->bNumConfigurations, sizeof(*udd->configurations));
 100         if(udd->configurations == NULL)
 101         {
 102                 retval = IPC_ENOMEM;
 103                 goto free_and_error;
 104         }
 105         for(iConf = 0; iConf < udd->bNumConfigurations; iConf++)
 106         {
 107                 buffer = iosAlloc(hId, USB_DT_CONFIG_SIZE);
 108                 if(buffer == NULL)
 109                 {
 110                         retval = IPC_ENOHEAP;
 111                         goto free_and_error;
 112                 }
 113
 114                 retval = __usb_getdesc(fd, buffer, USB_DT_CONFIG, iConf, USB_DT_CONFIG_SIZE);
 115                 ucd = &udd->configurations[iConf];
 116                 memcpy(ucd, buffer, USB_DT_CONFIG_SIZE);
 117                 iosFree(hId, buffer);
 118
 119                 ucd->wTotalLength = bswap16(ucd->wTotalLength);
 120                 buffer = iosAlloc(hId, ucd->wTotalLength);
 121                 if(buffer == NULL)
 122                 {
 123                         retval = IPC_ENOHEAP;
 124                         goto free_and_error;
 125                 }
 126
 127                 retval = __usb_getdesc(fd, buffer, USB_DT_CONFIG, iConf, ucd->wTotalLength);
 128                 if(retval < 0)
 129                         goto free_and_error;
 130
 131                 ptr = buffer;
 132                 ptr += ucd->bLength;
 133
 134                 retval = IPC_ENOMEM;
 135                 ucd->interfaces = calloc(ucd->bNumInterfaces, sizeof(*ucd->interfaces));
 136                 if(ucd->interfaces == NULL)
 137                         goto free_and_error;
 138                 for(iInterface = 0; iInterface < ucd->bNumInterfaces; iInterface++)
 139                 {
 140                         uid = &ucd->interfaces[iInterface];
 141                         memcpy(uid, ptr, USB_DT_INTERFACE_SIZE);
 142                         ptr += uid->bLength;
 143
 144                         uid->endpoints = calloc(uid->bNumEndpoints, sizeof(*uid->endpoints));
 145                         if(uid->endpoints == NULL)
 146                                 goto free_and_error;
 147                         for(iEndpoint = 0; iEndpoint < uid->bNumEndpoints; iEndpoint++)
 148                         {
 149                                 ued = &uid->endpoints[iEndpoint];
 150                                 memcpy(ued, ptr, USB_DT_ENDPOINT_SIZE);
 151                                 ptr += ued->bLength;
 152                                 ued->wMaxPacketSize = bswap16(ued->wMaxPacketSize);
 153                         }
 154                 }
 155         }
 156
 157
 158         retval = 0;
 159         free_and_error:
 160         if(buffer != NULL)
 161                 iosFree(hId, buffer);
 162         if(retval < 0)
 163                 USB_FreeDescriptors(udd);
 164         return retval;
 165 }
 166
 167 void USB_FreeDescriptors(_usb_devdesc *udd)
 168 {
 169         int iConf, iInterface;
 170         usb_configurationdesc *ucd;
 171         usb_interfacedesc *uid;
 172         if(udd->configurations != NULL)
 173         {
 174                 for(iConf = 0; iConf < udd->bNumConfigurations; iConf++)
 175                 {
 176                         ucd = &udd->configurations[iConf];
 177                         if(ucd->interfaces != NULL)
 178                         {
 179                                 for(iInterface = 0; iInterface < ucd->bNumInterfaces; iInterface++)
 180                                 {
 181                                         uid = &ucd->interfaces[iInterface];
 182                                         if(uid->endpoints != NULL)
 183                                                 free(uid->endpoints);
 184                                 }
 185                                 free(ucd->interfaces);
 186                         }
 187                 }
 188                 free(udd->configurations);
 189         }
 190 }
 191 s32 USBStorage_Initialize()
 192 {
 193         hId = iosCreateHeap(HEAP_SIZE);
 194
 195         if(hId < 0)
 196                 return IPC_ENOMEM;
 197
 198         return IPC_OK;
 199
 200 }
 201
 202 s32 USBStorage_Deinitialize()
 203 {
 204         return iosDestroyHeap(hId);
 205 }
 206
 207 static inline void __write32(u8 *p, u32 v)
 208 {
 209         p[0] = v;
 210         p[1] = v >> 8;
 211         p[2] = v >> 16;
 212         p[3] = v >> 24;
 213 }
 214
 215 static inline u32 __read32(u8 *p)
 216 {
 217         return p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);
 218 }
 219
 220 static s32 __send_cbw(usbstorage_handle *dev, u32 len, u8 flags, const u8 *cb, u8 cbLen)
 221 {
 222         u8 *cbw = NULL;
 223         s32 retval = USBSTORAGE_OK;
 224
 225         if(cbLen == 0 || cbLen > 16)
 226                 return IPC_EINVAL;
 227
 228         cbw = iosAlloc(hId, CBW_SIZE);
 229         if(cbw == NULL)
 230         {
 231                 retval = IPC_ENOHEAP;
 232                 goto free_and_return;
 233         }
 234
 235         memset(cbw, 0, CBW_SIZE);
 236
 237         __write32(cbw, CBW_SIGNATURE);
 238         __write32(cbw + 4, dev->tag);
 239         __write32(cbw + 8, len);
 240         cbw[12] = flags;
 241         cbw[13] = 0; /* TODO: LUN */
 242         cbw[14] = cbLen;
 243
 244         memcpy(cbw + 15, cb, cbLen);
 245
 246         LWP_MutexLock(dev->usb_fd);
 247         retval = USB_WriteBlkMsg(dev->usb_fd, dev->ep_out, CBW_SIZE, (void *)cbw);
 248
 249         if(retval == CBW_SIZE)
 250                 retval = USBSTORAGE_OK;
 251         else if(retval > CBW_SIZE)
 252                 retval = USBSTORAGE_ESHORTWRITE;
 253
 254         free_and_return:
 255         if(cbw != NULL)
 256                 iosFree(hId, cbw);
 257         if(retval < 0)
 258         {
 259                 USBStorage_Reset(dev);
 260                 LWP_MutexUnlock(dev->lock);
 261         }
 262         return retval;
 263 }
 264
 265 static s32 __read_csw(usbstorage_handle *dev, u8 *status, u32 *dataResidue)
 266 {
 267         u8 *csw = NULL;
 268         s32 retval = USBSTORAGE_OK;
 269         u32 signature, tag, _dataResidue, _status;
 270
 271         csw = iosAlloc(hId, CSW_SIZE);
 272         if(csw == NULL)
 273         {
 274                 retval = IPC_ENOHEAP;
 275                 goto free_and_return;
 276         }
 277
 278         memset(csw, 0, CSW_SIZE);
 279
 280         retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, CSW_SIZE, csw);
 281         if(retval != CSW_SIZE)
 282         {
 283                 if(retval > 0)
 284                         retval = IPC_EINVAL;
 285                 else
 286                         retval = USBSTORAGE_ESHORTREAD;
 287                 goto free_and_return;
 288         }
 289         else
 290                 retval = USBSTORAGE_OK;
 291
 292         signature = __read32(csw);
 293         tag = __read32(csw + 4);
 294         _dataResidue = __read32(csw + 8);
 295         _status = csw[12];
 296
 297         if(signature != CSW_SIGNATURE)
 298         {
 299                 retval = USBSTORAGE_ESIGNATURE;
 300                 goto free_and_return;
 301         }
 302
 303         if(dataResidue != NULL)
 304                 *dataResidue = _dataResidue;
 305         if(status != NULL)
 306                 *status = _status;
 307
 308         if(tag != dev->tag)
 309         {
 310                 retval = USBSTORAGE_ETAG;
 311                 goto free_and_return;
 312         }
 313
 314         dev->tag++;
 315
 316
 317         free_and_return:
 318         if(csw != NULL)
 319                 iosFree(hId, csw);
 320         if(retval < 0)
 321                 USBStorage_Reset(dev);
 322         LWP_MutexUnlock(dev->lock);
 323         return retval;
 324 }
 325
 326 static s32 __read_cycle(usbstorage_handle *dev, u8 *buffer, u32 len, u8 *cb, u8 cbLen)
 327 {
 328         s32 retval = USBSTORAGE_OK;
 329         u8 *bfr;
 330
 331         u8 status;
 332         u32 dataResidue;
 333
 334         bfr = iosAlloc(hId, dev->ep_in_size);
 335         if(bfr == NULL)
 336         {
 337                 retval = IPC_ENOHEAP;
 338                 goto free_and_error;
 339         }
 340
 341         retval = __send_cbw(dev, len, CBW_IN, cb, cbLen);
 342         if(retval < 0)
 343                 goto free_and_error;
 344
 345         while(len > 0)
 346         {
 347                 retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, dev->ep_in_size, bfr);
 348                 if(retval < 0)
 349                 {
 350                         goto free_and_error;
 351                 }
 352
 353                 memcpy(buffer, bfr, retval);
 354                 len -= retval;
 355                 buffer += retval;
 356
 357                 if(retval != dev->ep_in_size)
 358                         break;
 359         }
 360
 361         retval = __read_csw(dev, &status, &dataResidue);
 362         if(retval < 0)
 363         {
 364                 goto free_and_error;
 365         }
 366
 367         if(status)
 368         {
 369                 retval = USBSTORAGE_ESTATUS;
 370                 USBStorage_Reset(dev);
 371                 goto free_and_error;
 372         }
 373
 374         if(dataResidue)
 375         {
 376                 retval = USBSTORAGE_EDATARESIDUE;
 377                 USBStorage_Reset(dev);
 378                 goto free_and_error;
 379         }
 380
 381         retval = USBSTORAGE_OK;
 382
 383         free_and_error:
 384         if(bfr != NULL)
 385                 iosFree(hId, bfr);
 386         return retval;
 387 }
 388
 389 static s32 __write_cycle(usbstorage_handle *dev, const u8 *buffer, u32 len, u8 *cb, u8 cbLen)
 390 {
 391         s32 retval = USBSTORAGE_OK;
 392         u8 *bfr;
 393         u32 thisLen;
 394
 395         u8 status;
 396         u32 dataResidue;
 397
 398         bfr = iosAlloc(hId, dev->ep_out_size);
 399         if(bfr == NULL)
 400         {
 401                 retval = IPC_ENOHEAP;
 402                 goto free_and_error;
 403         }
 404
 405         retval = __send_cbw(dev, len, CBW_OUT, cb, cbLen);
 406         if(retval < 0)
 407                 goto free_and_error;
 408
 409         while(len > 0)
 410         {
 411                 thisLen = len > dev->ep_out_size ? dev->ep_out_size : len;
 412                 memset(bfr, 0, dev->ep_out_size);
 413                 memcpy(bfr, buffer, thisLen);
 414                 retval = USB_WriteBlkMsg(dev->usb_fd, dev->ep_out, thisLen, bfr);
 415                 if(retval < 0)
 416                 {
 417                         retval = USBSTORAGE_EDATARESIDUE;
 418                         USBStorage_Reset(dev);
 419                         goto free_and_error;
 420                 }
 421
 422                 len -= retval;
 423                 buffer += retval;
 424
 425                 if(retval != thisLen && len > 0)
 426                 {
 427                         retval = USBSTORAGE_EDATARESIDUE;
 428                         USBStorage_Reset(dev);
 429                         goto free_and_error;
 430                 }
 431         }
 432
 433         retval = __read_csw(dev, &status, &dataResidue);
 434         if(retval < 0)
 435         {
 436                 USBStorage_Reset(dev);
 437                 goto free_and_error;
 438         }
 439
 440         if(status)
 441         {
 442                 retval = USBSTORAGE_ESTATUS;
 443                 USBStorage_Reset(dev);
 444                 goto free_and_error;
 445         }
 446
 447         if(dataResidue)
 448         {
 449                 retval = USBSTORAGE_EDATARESIDUE;
 450                 USBStorage_Reset(dev);
 451                 goto free_and_error;
 452         }
 453
 454         retval = USBSTORAGE_OK;
 455
 456         free_and_error:
 457         if(buffer != NULL)
 458                 iosFree(hId, bfr);
 459         return retval;
 460 }
 461
 462 s32 USBStorage_Open(usbstorage_handle *dev, const char *bus, u16 vid, u16 pid)
 463 {
 464         u8 cmd[16];
 465         u8 *sense = NULL;
 466         s32 retval = -1;
 467
 468         u32 iConf, iInterface, iEp;
 469         _usb_devdesc udd;
 470         usb_configurationdesc *ucd;
 471         usb_interfacedesc *uid;
 472         usb_endpointdesc *ued;
 473
 474         memset(dev, 0, sizeof(*dev));
 475
 476         dev->tag = TAG_START;
 477         if(LWP_MutexInit(&dev->lock, true) < 0)
 478                 goto free_and_return;
 479         LWP_MutexLock(dev->lock);
 480
 481         retval = USB_OpenDevice(bus, vid, pid, &dev->usb_fd);
 482         if(retval < 0)
 483                 goto free_and_return;
 484
 485         sense = iosAlloc(hId, SCSI_SENSE_REPLY_SIZE);
 486         if(sense == NULL)
 487                 goto free_and_return;
 488
 489         retval = USB_GetDescriptors(dev->usb_fd, &udd);
 490         if(retval < 0)
 491                 goto free_and_return;
 492
 493         for(iConf = 0; iConf < udd.bNumConfigurations; iConf++)
 494         {
 495                 ucd = &udd.configurations[iConf];
 496                 for(iInterface = 0; iInterface < ucd->bNumInterfaces; iInterface++)
 497                 {
 498                         uid = &ucd->interfaces[iInterface];
 499                         if(uid->bInterfaceClass    == USB_CLASS_MASS_STORAGE &&
 500                            uid->bInterfaceSubClass == MASS_STORAGE_SCSI_COMMANDS &&
 501                            uid->bInterfaceProtocol == MASS_STORAGE_BULK_ONLY)
 502                         {
 503                                 if(uid->bNumEndpoints < 2)
 504                                         continue;
 505
 506                                 dev->ep_in = dev->ep_in_size = dev->ep_out = dev->ep_out_size = 0;
 507                                 for(iEp = 0; iEp < uid->bNumEndpoints; iEp++)
 508                                 {
 509                                         ued = &uid->endpoints[iEp];
 510                                         if(ued->bmAttributes != USB_ENDPOINT_BULK)
 511                                                 continue;
 512
 513                                         if(ued->bEndpointAddress & USB_ENDPOINT_IN)
 514                                         {
 515                                                 dev->ep_in = ued->bEndpointAddress;
 516                                                 dev->ep_in_size = ued->wMaxPacketSize;
 517                                         }
 518                                         else
 519                                         {
 520                                                 dev->ep_out = ued->bEndpointAddress;
 521                                                 dev->ep_out_size = ued->wMaxPacketSize;
 522                                         }
 523                                 }
 524                                 if(dev->ep_in != 0 && dev->ep_out != 0)
 525                                 {
 526                                         dev->configuration = ucd->bConfigurationValue;
 527                                         dev->interface = uid->bInterfaceNumber;
 528                                         dev->altInterface = uid->bAlternateSetting;
 529                                         goto found;
 530                                 }
 531                         }
 532                 }
 533         }
 534
 535         USB_FreeDescriptors(&udd);
 536         retval = USBSTORAGE_ENOINTERFACE;
 537         goto free_and_return;
 538
 539         found:
 540         USB_FreeDescriptors(&udd);
 541
 542         // TODO support for non-default configurations
 543         retval = USBStorage_Reset(dev);
 544         if(retval < 0)
 545                 goto free_and_return;
 546
 547         memset(cmd, 0, sizeof(cmd));
 548         memset(sense, 0, 8);
 549         cmd[0] = SCSI_READ_CAPACITY;
 550         retval = __read_cycle(dev, sense, 8, cmd, 1);
 551         if(retval < 0)
 552         {
 553                 retval = USBSTORAGE_ECAPACITY;
 554                 goto free_and_return;
 555         }
 556         memcpy(&dev->n_sectors, sense, 4);
 557         memcpy(&dev->sector_size, sense + 4, 4);
 558
 559         free_and_return:
 560         LWP_MutexUnlock(dev->lock);
 561         if(sense != NULL)
 562                 iosFree(hId, sense);
 563         if(retval < 0)
 564         {
 565                 LWP_MutexDestroy(dev->lock);
 566                 return retval;
 567         }
 568         return 0;
 569 }
 570
 571 s32 USBStorage_Close(usbstorage_handle *dev)
 572 {
 573         USB_CloseDevice(&dev->usb_fd);
 574         LWP_MutexDestroy(dev->lock);
 575         memset(dev, 0, sizeof(*dev));
 576         return 0;
 577 }
 578
 579 s32 USBStorage_Reset(usbstorage_handle *dev)
 580 {
 581         s32 retval;
 582         u8 status = 0;
 583         u8 cmd[16];
 584         u8 *sense = NULL;
 585
 586         LWP_MutexLock(dev->lock);
 587         retval = USB_WriteCtrlMsg(dev->usb_fd, 33, 0xFF, dev->interface, 0, 0, NULL);
 588         if(retval < 0)
 589                 goto end;
 590         retval = USB_WriteCtrlMsg(dev->usb_fd, 2, 0x1, 0, dev->ep_in, 0, NULL);
 591         if(retval < 0)
 592                 goto end;
 593         retval = USB_WriteCtrlMsg(dev->usb_fd, 2, 0x1, 0, dev->ep_out, 0, NULL);
 594
 595         /* some devices needs this (TEST_UNIT_READY -> REQUEST_SENSE
 596          * to be working... */
 597         sense = iosAlloc(hId, SCSI_SENSE_REPLY_SIZE);
 598         if(sense == NULL)
 599         {
 600                 retval = IPC_ENOMEM;
 601                 goto end;
 602         }
 603
 604         memset(cmd, 0, sizeof(cmd));
 605         cmd[0] = SCSI_TEST_UNIT_READY;
 606
 607         retval = __send_cbw(dev, 0, CBW_IN, cmd, 1);
 608
 609         if(retval < 0)
 610         {
 611                 /* fatal error */
 612                 goto end;
 613         }
 614
 615         retval = __read_csw(dev, &status, NULL);
 616         if(status != 0)
 617         {
 618                 cmd[0] = SCSI_REQUEST_SENSE;
 619                 cmd[4] = SCSI_SENSE_REPLY_SIZE;
 620                 cmd[5] = 0;
 621                 retval = __send_cbw(dev, 0, CBW_IN, cmd, 6);
 622                 if(retval < 0)
 623                 {
 624                         /* fatal error */
 625                         goto end;
 626                 }
 627
 628                 memset(sense, 0, SCSI_SENSE_REPLY_SIZE);
 629                 retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, SCSI_SENSE_REPLY_SIZE, sense);
 630 #if 0
 631                 if(retval < SCSI_SENSE_REPLY_SIZE)
 632 #else
 633                 if(retval < 0) /* this retruns 0 on one of my sticks.. :/ */
 634 #endif
 635                 {
 636                         /* fatal error again */
 637                         retval = USBSTORAGE_ESENSE;
 638                         goto end;
 639                 }
 640
 641                 retval = __read_csw(dev, NULL, NULL);
 642                 if(retval < 0)
 643                 {
 644                         /* fatal error again */
 645                         goto end;
 646                 }
 647         }
 648
 649         end:
 650         iosFree(hId, sense);
 651         LWP_MutexUnlock(dev->lock);
 652         return retval;
 653 }
 654
 655 s32 USBStorage_Read(usbstorage_handle *dev, u32 sector, u8 *buffer, u16 n_sectors)
 656 {
 657         u8 cmd[] = {
 658                 SCSI_READ_10,
 659                 0,
 660                 sector >> 24,
 661                 sector >> 16,
 662                 sector >>  8,
 663                 sector,
 664                 0,
 665                 n_sectors >> 8,
 666                 n_sectors,
 667                 0
 668                 };
 669         return __read_cycle(dev, buffer, n_sectors * dev->sector_size, cmd, sizeof(cmd));
 670 }
 671
 672 // not tested yet
 673 s32 USBStorage_Write(usbstorage_handle *dev, u32 sector, const u8 *buffer, u16 n_sectors)
 674 {
 675         u8 cmd[] = {
 676                 SCSI_WRITE_10,
 677                 sector >> 24,
 678                 sector >> 16,
 679                 sector >> 8,
 680                 sector,
 681                 0,
 682                 n_sectors >> 8,
 683                 n_sectors,
 684                 0
 685                 };
 686
 687         return __write_cycle(dev, buffer, n_sectors * dev->sector_size, cmd, sizeof(cmd));
 688
 689 }