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