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 s32 USBStorage_Initialize()
  63 {
  64         hId = iosCreateHeap(HEAP_SIZE);
  65
  66         if(hId < 0)
  67                 return IPC_ENOMEM;
  68
  69         return IPC_OK;
  70
  71 }
  72
  73 s32 USBStorage_Deinitialize()
  74 {
  75         return iosDestroyHeap(hId);
  76 }
  77
  78 static inline void __write32(u8 *p, u32 v)
  79 {
  80         p[0] = v;
  81         p[1] = v >> 8;
  82         p[2] = v >> 16;
  83         p[3] = v >> 24;
  84 }
  85
  86 static inline u32 __read32(u8 *p)
  87 {
  88         return p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);
  89 }
  90
  91 static s32 __send_cbw(usbstorage_handle *dev, u32 len, u8 flags, const u8 *cb, u8 cbLen)
  92 {
  93         u8 *cbw = NULL;
  94         s32 retval;
  95
  96         if(cbLen == 0 || cbLen > 16)
  97                 return IPC_EINVAL;
  98
  99         cbw = iosAlloc(hId, CBW_SIZE);
 100         if(cbw == NULL)
 101         {
 102                 TRACE
 103                 retval = IPC_ENOHEAP;
 104                 goto free_and_return;
 105         }
 106
 107         memset(cbw, 0, CBW_SIZE);
 108
 109         __write32(cbw, CBW_SIGNATURE);
 110         __write32(cbw + 4, dev->tag);
 111         __write32(cbw + 8, len);
 112         cbw[12] = flags;
 113         cbw[13] = 0; /* TODO: LUN */
 114         cbw[14] = cbLen;
 115
 116         memcpy(cbw + 15, cb, cbLen);
 117
 118         LWP_MutexLock(dev->usb_fd);
 119         retval = USB_WriteBlkMsg(dev->usb_fd, dev->ep_out, CBW_SIZE, (void *)cbw);
 120
 121         if(retval == CBW_SIZE)
 122                 retval = 0;
 123         else if(retval > CBW_SIZE)
 124                 retval = IPC_EINVAL;
 125
 126         free_and_return:
 127         if(cbw != NULL)
 128                 iosFree(hId, cbw);
 129         return retval;
 130 }
 131
 132 static s32 __read_csw(usbstorage_handle *dev, u8 *status, u32 *dataResidue)
 133 {
 134         u8 *csw = NULL;
 135         s32 retval;
 136         u32 signature, tag, _dataResidue, _status;
 137
 138         csw = iosAlloc(hId, CSW_SIZE);
 139         if(csw == NULL)
 140         {
 141                 TRACE
 142                 retval = IPC_ENOHEAP;
 143                 goto free_and_return;
 144         }
 145
 146         memset(csw, 0, CSW_SIZE);
 147
 148         retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, CSW_SIZE, csw);
 149         if(retval != CSW_SIZE)
 150         {
 151                 TRACE
 152                 if(retval > 0)
 153                         retval = IPC_EINVAL;
 154                 goto free_and_return;
 155         }
 156
 157         signature = __read32(csw);
 158         tag = __read32(csw + 4);
 159         _dataResidue = __read32(csw + 8);
 160         _status = csw[12];
 161
 162         if(signature != CSW_SIGNATURE)
 163         {
 164                 TRACE
 165                 retval = IPC_EINVAL;
 166                 goto free_and_return;
 167         }
 168
 169         if(dataResidue != NULL)
 170                 *dataResidue = _dataResidue;
 171         if(status != NULL)
 172                 *status = _status;
 173
 174         if(tag != dev->tag)
 175         {
 176                 TRACE
 177                 retval = IPC_EINVAL;
 178                 goto free_and_return;
 179         }
 180
 181         dev->tag++;
 182
 183         LWP_MutexUnlock(dev->lock);
 184
 185         free_and_return:
 186         if(csw != NULL)
 187                 iosFree(hId, csw);
 188         return retval;
 189 }
 190
 191 static s32 __read_cycle(usbstorage_handle *dev, u8 *buffer, u32 len, u8 *cb, u8 cbLen)
 192 {
 193         s32 retval;
 194         u8 *bfr;
 195
 196         u8 status;
 197         u32 dataResidue;
 198
 199         bfr = iosAlloc(hId, dev->ep_in_size);
 200         if(bfr == NULL)
 201         {
 202                 TRACE
 203                 retval = IPC_ENOHEAP;
 204                 goto free_and_error;
 205         }
 206
 207         retval = __send_cbw(dev, len, CBW_IN, cb, cbLen);
 208         if(retval < 0)
 209                 goto free_and_error;
 210
 211         while(len > 0)
 212         {
 213                 TRACE
 214                 retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, dev->ep_in_size, bfr);
 215                 if(retval < 0)
 216                 {
 217                         TRACE
 218                         goto free_and_error;
 219                 }
 220
 221                 memcpy(buffer, bfr, retval);
 222                 len -= retval;
 223                 buffer += retval;
 224
 225                 if(retval != dev->ep_in_size)
 226                         break;
 227         }
 228
 229         retval = __read_csw(dev, &status, &dataResidue);
 230         if(retval < 0)
 231         {
 232                 TRACE
 233                 goto free_and_error;
 234         }
 235
 236         if(status || dataResidue)
 237         {
 238                 TRACE
 239                 // TODO error handling
 240                 retval = -1;
 241                 goto free_and_error;
 242         }
 243
 244         free_and_error:
 245         TRACE
 246         if(bfr != NULL)
 247                 iosFree(hId, bfr);
 248         return retval;
 249 }
 250
 251 static s32 __write_cycle(usbstorage_handle *dev, const u8 *buffer, u32 len, u8 *cb, u8 cbLen)
 252 {
 253         s32 retval;
 254         u8 *bfr;
 255         u32 thisLen;
 256
 257         u8 status;
 258         u32 dataResidue;
 259
 260         bfr = iosAlloc(hId, dev->ep_out_size);
 261         if(bfr == NULL)
 262         {
 263                 TRACE
 264                 retval = IPC_ENOHEAP;
 265                 goto free_and_error;
 266         }
 267
 268         retval = __send_cbw(dev, len, CBW_OUT, cb, cbLen);
 269         if(retval < 0)
 270                 goto free_and_error;
 271
 272         while(len > 0)
 273         {
 274                 TRACE
 275                 thisLen = len > dev->ep_out_size ? dev->ep_out_size : len;
 276                 memset(bfr, 0, dev->ep_out_size);
 277                 memcpy(bfr, buffer, thisLen);
 278                 retval = USB_WriteBlkMsg(dev->usb_fd, dev->ep_out, thisLen, bfr);
 279                 if(retval < 0)
 280                 {
 281                         TRACE
 282                         goto free_and_error;
 283                 }
 284
 285                 len -= retval;
 286                 buffer += retval;
 287
 288                 if(retval != thisLen && len > 0)
 289                 {
 290                         retval = -1;
 291                         goto free_and_error;
 292                 }
 293         }
 294
 295         retval = __read_csw(dev, &status, &dataResidue);
 296         if(retval < 0)
 297         {
 298                 TRACE
 299                 goto free_and_error;
 300         }
 301
 302         if(status || dataResidue)
 303         {
 304                 TRACE
 305                 // TODO error handling
 306                 retval = -1;
 307                 goto free_and_error;
 308         }
 309
 310         free_and_error:
 311         TRACE
 312         if(buffer != NULL)
 313                 iosFree(hId, bfr);
 314         return retval;
 315 }
 316
 317 usbstorage_handle *USBStorage_Open(const char *bus, u16 vid, u16 pid)
 318 {
 319         usbstorage_handle *dev = NULL;
 320         u8 cmd[16];
 321         u8 *sense = NULL;
 322         u8 status = 0;
 323         s32 retval = -1;
 324
 325         dev = calloc(1, sizeof(*dev));
 326         if(dev == NULL)
 327         {
 328                 retval = IPC_ENOMEM;
 329                 goto free_and_return;
 330         }
 331
 332         dev->tag = TAG_START;
 333         if(LWP_MutexInit(&dev->lock, true) < 0)
 334                 goto free_and_return;
 335         LWP_MutexLock(dev->lock);
 336
 337         retval = USB_OpenDevice(bus, vid, pid, &dev->usb_fd);
 338         if(retval < 0)
 339                 goto free_and_return;
 340
 341         sense = iosAlloc(hId, SCSI_SENSE_REPLY_SIZE);
 342         if(sense == NULL)
 343                 goto free_and_return;
 344
 345         // TODO: parse descriptors here
 346         dev->ep_in = 0x81;
 347         dev->ep_in_size = 64;
 348         dev->ep_out = 0x02;
 349         dev->ep_out_size = 64;
 350
 351         memset(cmd, 0, sizeof(cmd));
 352         cmd[0] = SCSI_TEST_UNIT_READY;
 353
 354         TRACE
 355         retval = __send_cbw(dev, 0, CBW_IN, cmd, 1);
 356
 357         if(retval < 0)
 358         {
 359                 /* fatal error */
 360                 TRACE
 361                 retval = -1;
 362                 goto free_and_return;
 363         }
 364
 365         /* some devices needs this to be fully working... */
 366         retval = __read_csw(dev, &status, NULL);
 367         if(status != 0)
 368         {
 369                 TRACE
 370                 cmd[0] = SCSI_REQUEST_SENSE;
 371                 cmd[4] = SCSI_SENSE_REPLY_SIZE;
 372                 cmd[5] = 0;
 373                 retval = __send_cbw(dev, 0, CBW_IN, cmd, 6);
 374                 if(retval < 0)
 375                 {
 376                         /* fatal error */
 377                         TRACE
 378                         retval = -1;
 379                         goto free_and_return;
 380                 }
 381
 382                 memset(sense, 0, SCSI_SENSE_REPLY_SIZE);
 383                 retval = USB_ReadBlkMsg(dev->usb_fd, dev->ep_in, SCSI_SENSE_REPLY_SIZE, sense);
 384 #if 0
 385                 if(retval < SCSI_SENSE_REPLY_SIZE)
 386 #else
 387                 if(retval < 0) /* this retruns 0 on one of my sticks.. :/ */
 388 #endif
 389                 {
 390                         /* fatal error again */
 391                         TRACE
 392                         retval = -1;
 393                         goto free_and_return;
 394                 }
 395
 396                 retval = __read_csw(dev, NULL, NULL);
 397                 if(retval < 0)
 398                 {
 399                         /* fatal error again */
 400                         TRACE
 401                         retval = -1;
 402                         goto free_and_return;
 403                 }
 404         }
 405
 406         memset(cmd, 0, sizeof(cmd));
 407         memset(sense, 0, 8);
 408         cmd[0] = SCSI_READ_CAPACITY;
 409         retval = __read_cycle(dev, sense, 8, cmd, 1);
 410         if(retval < 0)
 411         {
 412                 TRACE
 413                 goto free_and_return;
 414         }
 415         memcpy(&dev->n_sectors, sense, 4);
 416         memcpy(&dev->sector_size, sense + 4, 4);
 417
 418         free_and_return:
 419         LWP_MutexUnlock(dev->lock);
 420         if(sense != NULL)
 421                 iosFree(hId, sense);
 422         if(retval < 0)
 423         {
 424                 TRACE
 425                 LWP_MutexDestroy(dev->lock);
 426                 free(dev);
 427                 return NULL;
 428         }
 429         return dev;
 430 }
 431
 432 s32 USBStorage_Close(usbstorage_handle *dev)
 433 {
 434         USB_CloseDevice(&dev->usb_fd);
 435         LWP_MutexDestroy(dev->lock);
 436         memset(dev, 0, sizeof(*dev));
 437         return 0;
 438 }
 439
 440 s32 USBStorage_Read(usbstorage_handle *dev, u32 sector, u8 *buffer, u16 n_sectors)
 441 {
 442         u8 cmd[] = {
 443                 SCSI_READ_10,
 444                 0,
 445                 sector >> 24,
 446                 sector >> 16,
 447                 sector >>  8,
 448                 sector,
 449                 0,
 450                 n_sectors >> 8,
 451                 n_sectors,
 452                 0
 453                 };
 454         return __read_cycle(dev, buffer, n_sectors * dev->sector_size, cmd, sizeof(cmd));
 455 }
 456
 457 // not tested yet
 458 s32 USBStorage_Write(usbstorage_handle *dev, u32 sector, const u8 *buffer, u16 n_sectors)
 459 {
 460         u8 cmd[] = {
 461                 SCSI_WRITE_10,
 462                 sector >> 24,
 463                 sector >> 16,
 464                 sector >> 8,
 465                 sector,
 466                 0,
 467                 n_sectors >> 8,
 468                 n_sectors,
 469                 0
 470                 };
 471
 472         return __write_cycle(dev, buffer, n_sectors * dev->sector_size, cmd, sizeof(cmd));
 473
 474 }