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