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         return retval;
 268 }
 269
 270 static s32 __read_csw(usbstorage_handle *dev, u8 *status, u32 *dataResidue)
 271 {
 272         u8 *csw = NULL;
 273         s32 retval;
 274         u32 signature, tag, _dataResidue, _status;
 275
 276         csw = iosAlloc(hId, CSW_SIZE);
 277         if(csw == NULL)
 278         {
 279                 TRACE
 280                 retval = IPC_ENOHEAP;
 281                 goto free_and_return;
 282         }
 283
 284         memset(csw, 0, CSW_SIZE);
 285
 286         retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, CSW_SIZE, csw);
 287         if(retval != CSW_SIZE)
 288         {
 289                 TRACE
 290                 if(retval > 0)
 291                         retval = IPC_EINVAL;
 292                 goto free_and_return;
 293         }
 294
 295         signature = __read32(csw);
 296         tag = __read32(csw + 4);
 297         _dataResidue = __read32(csw + 8);
 298         _status = csw[12];
 299
 300         if(signature != CSW_SIGNATURE)
 301         {
 302                 TRACE
 303                 retval = IPC_EINVAL;
 304                 goto free_and_return;
 305         }
 306
 307         if(dataResidue != NULL)
 308                 *dataResidue = _dataResidue;
 309         if(status != NULL)
 310                 *status = _status;
 311
 312         if(tag != dev->tag)
 313         {
 314                 TRACE
 315                 retval = IPC_EINVAL;
 316                 goto free_and_return;
 317         }
 318
 319         dev->tag++;
 320
 321         LWP_MutexUnlock(dev->lock);
 322
 323         free_and_return:
 324         if(csw != NULL)
 325                 iosFree(hId, csw);
 326         return retval;
 327 }
 328
 329 static s32 __read_cycle(usbstorage_handle *dev, u8 *buffer, u32 len, u8 *cb, u8 cbLen)
 330 {
 331         s32 retval;
 332         u8 *bfr;
 333
 334         u8 status;
 335         u32 dataResidue;
 336
 337         bfr = iosAlloc(hId, dev->ep_in_size);
 338         if(bfr == NULL)
 339         {
 340                 TRACE
 341                 retval = IPC_ENOHEAP;
 342                 goto free_and_error;
 343         }
 344
 345         retval = __send_cbw(dev, len, CBW_IN, cb, cbLen);
 346         if(retval < 0)
 347                 goto free_and_error;
 348
 349         while(len > 0)
 350         {
 351                 TRACE
 352                 retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, dev->ep_in_size, bfr);
 353                 if(retval < 0)
 354                 {
 355                         TRACE
 356                         goto free_and_error;
 357                 }
 358
 359                 memcpy(buffer, bfr, retval);
 360                 len -= retval;
 361                 buffer += retval;
 362
 363                 if(retval != dev->ep_in_size)
 364                         break;
 365         }
 366
 367         retval = __read_csw(dev, &status, &dataResidue);
 368         if(retval < 0)
 369         {
 370                 TRACE
 371                 goto free_and_error;
 372         }
 373
 374         if(status || dataResidue)
 375         {
 376                 TRACE
 377                 // TODO error handling
 378                 retval = -1;
 379                 goto free_and_error;
 380         }
 381
 382         free_and_error:
 383         TRACE
 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;
 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                 TRACE
 402                 retval = IPC_ENOHEAP;
 403                 goto free_and_error;
 404         }
 405
 406         retval = __send_cbw(dev, len, CBW_OUT, cb, cbLen);
 407         if(retval < 0)
 408                 goto free_and_error;
 409
 410         while(len > 0)
 411         {
 412                 TRACE
 413                 thisLen = len > dev->ep_out_size ? dev->ep_out_size : len;
 414                 memset(bfr, 0, dev->ep_out_size);
 415                 memcpy(bfr, buffer, thisLen);
 416                 retval = USB_WriteBlkMsg(dev->usb_fd, dev->ep_out, thisLen, bfr);
 417                 if(retval < 0)
 418                 {
 419                         TRACE
 420                         goto free_and_error;
 421                 }
 422
 423                 len -= retval;
 424                 buffer += retval;
 425
 426                 if(retval != thisLen && len > 0)
 427                 {
 428                         retval = -1;
 429                         goto free_and_error;
 430                 }
 431         }
 432
 433         retval = __read_csw(dev, &status, &dataResidue);
 434         if(retval < 0)
 435         {
 436                 TRACE
 437                 goto free_and_error;
 438         }
 439
 440         if(status || dataResidue)
 441         {
 442                 TRACE
 443                 // TODO error handling
 444                 retval = -1;
 445                 goto free_and_error;
 446         }
 447
 448         free_and_error:
 449         TRACE
 450         if(buffer != NULL)
 451                 iosFree(hId, bfr);
 452         return retval;
 453 }
 454
 455 usbstorage_handle *USBStorage_Open(const char *bus, u16 vid, u16 pid)
 456 {
 457         usbstorage_handle *dev = NULL;
 458         u8 cmd[16];
 459         u8 *sense = NULL;
 460         u8 status = 0;
 461         s32 retval = -1;
 462
 463         u32 iConf, iInterface, iEp;
 464         _usb_devdesc udd;
 465         usb_configurationdesc *ucd;
 466         usb_interfacedesc *uid;
 467         usb_endpointdesc *ued;
 468
 469         dev = calloc(1, sizeof(*dev));
 470         if(dev == NULL)
 471         {
 472                 retval = IPC_ENOMEM;
 473                 goto free_and_return;
 474         }
 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         goto free_and_return;
 537
 538         found:
 539         USB_FreeDescriptors(&udd);
 540
 541         /* some devices needs this (TEST_UNIT_READY -> REQUEST_SENSE
 542          * to be working... */
 543         memset(cmd, 0, sizeof(cmd));
 544         cmd[0] = SCSI_TEST_UNIT_READY;
 545
 546         TRACE
 547         retval = __send_cbw(dev, 0, CBW_IN, cmd, 1);
 548
 549         if(retval < 0)
 550         {
 551                 /* fatal error */
 552                 TRACE
 553                 retval = -1;
 554                 goto free_and_return;
 555         }
 556
 557         retval = __read_csw(dev, &status, NULL);
 558         if(status != 0)
 559         {
 560                 TRACE
 561                 cmd[0] = SCSI_REQUEST_SENSE;
 562                 cmd[4] = SCSI_SENSE_REPLY_SIZE;
 563                 cmd[5] = 0;
 564                 retval = __send_cbw(dev, 0, CBW_IN, cmd, 6);
 565                 if(retval < 0)
 566                 {
 567                         /* fatal error */
 568                         TRACE
 569                         retval = -1;
 570                         goto free_and_return;
 571                 }
 572
 573                 memset(sense, 0, SCSI_SENSE_REPLY_SIZE);
 574                 retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, SCSI_SENSE_REPLY_SIZE, sense);
 575 #if 0
 576                 if(retval < SCSI_SENSE_REPLY_SIZE)
 577 #else
 578                 if(retval < 0) /* this retruns 0 on one of my sticks.. :/ */
 579 #endif
 580                 {
 581                         /* fatal error again */
 582                         TRACE
 583                         retval = -1;
 584                         goto free_and_return;
 585                 }
 586
 587                 retval = __read_csw(dev, NULL, NULL);
 588                 if(retval < 0)
 589                 {
 590                         /* fatal error again */
 591                         TRACE
 592                         retval = -1;
 593                         goto free_and_return;
 594                 }
 595         }
 596
 597         memset(cmd, 0, sizeof(cmd));
 598         memset(sense, 0, 8);
 599         cmd[0] = SCSI_READ_CAPACITY;
 600         retval = __read_cycle(dev, sense, 8, cmd, 1);
 601         if(retval < 0)
 602         {
 603                 TRACE
 604                 goto free_and_return;
 605         }
 606         memcpy(&dev->n_sectors, sense, 4);
 607         memcpy(&dev->sector_size, sense + 4, 4);
 608
 609         free_and_return:
 610         LWP_MutexUnlock(dev->lock);
 611         if(sense != NULL)
 612                 iosFree(hId, sense);
 613         if(retval < 0)
 614         {
 615                 TRACE
 616                 LWP_MutexDestroy(dev->lock);
 617                 free(dev);
 618                 return NULL;
 619         }
 620         return dev;
 621 }
 622
 623 s32 USBStorage_Close(usbstorage_handle *dev)
 624 {
 625         USB_CloseDevice(&dev->usb_fd);
 626         LWP_MutexDestroy(dev->lock);
 627         free(dev);
 628         return 0;
 629 }
 630
 631 s32 USBStorage_Read(usbstorage_handle *dev, u32 sector, u8 *buffer, u16 n_sectors)
 632 {
 633         u8 cmd[] = {
 634                 SCSI_READ_10,
 635                 0,
 636                 sector >> 24,
 637                 sector >> 16,
 638                 sector >>  8,
 639                 sector,
 640                 0,
 641                 n_sectors >> 8,
 642                 n_sectors,
 643                 0
 644                 };
 645         return __read_cycle(dev, buffer, n_sectors * dev->sector_size, cmd, sizeof(cmd));
 646 }
 647
 648 // not tested yet
 649 s32 USBStorage_Write(usbstorage_handle *dev, u32 sector, const u8 *buffer, u16 n_sectors)
 650 {
 651         u8 cmd[] = {
 652                 SCSI_WRITE_10,
 653                 sector >> 24,
 654                 sector >> 16,
 655                 sector >> 8,
 656                 sector,
 657                 0,
 658                 n_sectors >> 8,
 659                 n_sectors,
 660                 0
 661                 };
 662
 663         return __write_cycle(dev, buffer, n_sectors * dev->sector_size, cmd, sizeof(cmd));
 664
 665 }