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