workbench/devs/USB/classes/MassStorage/storageclass.c

   1 /*
   2     Copyright (C) 2006 by Michal Schulz
   3     $Id$
   4
   5     This program is free software; you can redistribute it and/or modify
   6     it under the terms of the GNU Library General Public License as
   7     published by the Free Software Foundation; either version 2 of the
   8     License, or (at your option) any later version.
   9
  10     This program is distributed in the hope that it will be useful,
  11     but WITHOUT ANY WARRANTY; without even the implied warranty of
  12     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13     GNU General Public License for more details.
  14
  15     You should have received a copy of the GNU Library General Public
  16     License along with this program; if not, write to the
  17     Free Software Foundation, Inc.,
  18     59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  19  */
  20
  21 #define DEBUG 1
  22
  23 #include <aros/debug.h>
  24 #include <aros/libcall.h>
  25 #include <aros/asmcall.h>
  26
  27 #include <dos/dos.h>
  28 #include <dos/dosextens.h>
  29
  30 #include <usb/usb.h>
  31 #include <usb/usb_core.h>
  32 #include <usb/hid.h>
  33 #include <stdio.h>
  34
  35 #include "storage.h"
  36
  37 #include <proto/oop.h>
  38 #include <proto/dos.h>
  39
  40 BOOL USBMSS_AddVolume(mss_unit_t *unit);
  41
  42 static unit_cache_t *GetUnitFromCache(OOP_Class *cl, OOP_Object *o, uint8_t create)
  43 {
  44         StorageData *mss = OOP_INST_DATA(cl, o);
  45         struct mss_staticdata *sd = SD(cl);
  46         unit_cache_t *unit, *u = NULL;
  47
  48         intptr_t tmp;
  49         uint16_t pid, vid;
  50         char *product, *manufacturer, *serial;
  51
  52         /* Get some basic info about the device */
  53         OOP_GetAttr(o, aHidd_USBDevice_ProductID, &tmp);
  54         pid = tmp;
  55         OOP_GetAttr(o, aHidd_USBDevice_VendorID, &tmp);
  56         vid = tmp;
  57         OOP_GetAttr(o, aHidd_USBDevice_ProductName, &tmp);
  58         product = (char *)tmp;
  59         OOP_GetAttr(o, aHidd_USBDevice_ManufacturerName, &tmp);
  60         manufacturer = (char *)tmp;
  61         OOP_GetAttr(o, aHidd_USBDevice_SerialNumber, &tmp);
  62         serial = (char *)tmp;
  63
  64         D(bug("[MSS] GetUnitFromCache(%04x:%04x, '%s', '%s', '%s')\n", pid, vid, manufacturer, product, serial));
  65
  66         /* Check the cache list. If unit was here already, use it */
  67         ObtainSemaphore(&sd->Lock);
  68         ForeachNode(&sd->unitCache, unit)
  69         {
  70                 if (pid == unit->productID && vid == unit->vendorID)
  71                 {
  72                         if (strcmp(serial, unit->serialNumber) == 0 &&
  73                                 strcmp(product, unit->productName) == 0 &&
  74                                 strcmp(manufacturer, unit->manufacturerName) == 0)
  75                         {
  76                                 u = unit;
  77                                 break;
  78                         }
  79                 }
  80         }
  81         ReleaseSemaphore(&sd->Lock);
  82
  83         if (u)
  84         {
  85                 D(bug("[MSS] Found the device in cache already. Reassigning the old unit number %d\n",
  86                                 u->unitNumber));
  87         }
  88         else if (create)
  89         {
  90                 D(bug("[MSS] Unit is new to the system. Creating new cache object\n"));
  91
  92                 u = AllocVecPooled(sd->MemPool, sizeof(unit_cache_t));
  93                 NEWLIST(&u->units);
  94                 u->productID = pid;
  95                 u->vendorID = vid;
  96
  97                 if (product)
  98                 {
  99                         u->productName = AllocVecPooled(sd->MemPool, strlen(product) + 1);
 100                         if (u->productName)
 101                                 strcpy(u->productName, product);
 102                 }
 103                 else
 104                         u->productName = NULL;
 105
 106                 if (manufacturer)
 107                 {
 108                         u->manufacturerName = AllocVecPooled(sd->MemPool, strlen(manufacturer) + 1);
 109                         if (u->manufacturerName)
 110                                 strcpy(u->manufacturerName, manufacturer);
 111                 }
 112                 else
 113                         u->manufacturerName = NULL;
 114
 115                 if (serial)
 116                 {
 117                         u->serialNumber = AllocVecPooled(sd->MemPool, strlen(serial) + 1);
 118                         if (u->serialNumber)
 119                                 strcpy(u->serialNumber, serial);
 120                 }
 121                 else
 122                         u->serialNumber = NULL;
 123
 124                 ObtainSemaphore(&sd->Lock);
 125                 u->unitNumber = sd->unitNum;
 126                 sd->unitNum += mss->maxLUN + 1;
 127                 AddTail((struct List *)&sd->unitCache, (struct Node *)u);
 128                 ReleaseSemaphore(&sd->Lock);
 129         }
 130
 131         return u;
 132 }
 133
 134 /*
 135  * Initialize the USBStorage instance
 136  */
 137 OOP_Object *METHOD(Storage, Root, New)
 138 {
 139     D(bug("[MSS] Storage::New()\n"));
 140
 141     /* Prevent the class from being expunged */
 142     BASE(cl->UserData)->LibNode.lib_OpenCnt++;
 143
 144     o = (OOP_Object *)OOP_DoSuperMethod(cl, o, (OOP_Msg) msg);
 145     if (o)
 146     {
 147         StorageData *mss = OOP_INST_DATA(cl, o);
 148         usb_config_descriptor_t cdesc;
 149         intptr_t iface;
 150         int i;
 151
 152         InitSemaphore(&mss->lock);
 153         mss->sd = SD(cl);
 154         mss->o = o;
 155
 156         /* Get the interface number */
 157         OOP_GetAttr(o, aHidd_USBDevice_Interface, &iface);
 158
 159         /* Configure MSS device */
 160         HIDD_USBDevice_Configure(o, 0);
 161
 162         /* Get device and config descriptors */
 163         D(bug("[MSS] Getting device descriptor\n"));
 164         HIDD_USBDevice_GetDeviceDescriptor(o, &mss->ddesc);
 165         D(bug("[MSS] Getting initial config descriptor\n"));
 166         HIDD_USBDevice_GetConfigDescriptor(o, 0, &cdesc);
 167         /* How many LUNs this device supports? */
 168         mss->maxLUN = HIDD_USBStorage_GetMaxLUN(o);
 169         if (mss->maxLUN > 15)
 170         {
 171                 D(bug("[MSS] GetMaxLUN FAILED.\n"));
 172                 mss->maxLUN = 0;
 173         }
 174         D(bug("[MSS] GetMaxLUN returns %d\n", mss->maxLUN));
 175
 176         if (AROS_LE2WORD(cdesc.wTotalLength))
 177                 mss->cdesc = AllocVecPooled(SD(cl)->MemPool, AROS_LE2WORD(cdesc.wTotalLength));
 178
 179         if (mss->cdesc)
 180         {
 181                 D(bug("[MSS] Getting config descriptor of size %d\n", AROS_LE2WORD(cdesc.wTotalLength)));
 182                         HIDD_USBDevice_GetDescriptor(o, UDESC_CONFIG, 0, AROS_LE2WORD(cdesc.wTotalLength), mss->cdesc);
 183
 184                         D(bug("[MSS] Getting descriptor of interface %d\n", iface));
 185                         mss->iface = HIDD_USBDevice_GetInterface(o, iface);
 186
 187                         D(bug("[MSS] Interface has %d endpoints\n", mss->iface->bNumEndpoints));
 188
 189                         /*
 190                          * Iterate through the possible endpoints and look for IN and OUT endpoints for BULK
 191                          * transfers.
 192                          */
 193                         for (i=0; i < mss->iface->bNumEndpoints; i++)
 194                         {
 195                                 usb_endpoint_descriptor_t *ep;
 196                                 ep = HIDD_USBDevice_GetEndpoint(o, iface, i);
 197
 198                                 D(bug("[MSS] endpoint %d: addr %02x, interval %02x, length %02x attr %02x maxpacket %04x\n", i,
 199                                                 ep->bEndpointAddress,
 200                                                 ep->bInterval,
 201                                                 ep->bLength,
 202                                                 ep->bmAttributes,
 203                                                 AROS_LE2WORD(ep->wMaxPacketSize)));
 204
 205                                 /* Use only BULK endpoints */
 206                                 if (UE_GET_XFERTYPE(ep->bmAttributes) == UE_BULK)
 207                                 {
 208                                         if (!mss->pipe_in && UE_GET_DIR(ep->bEndpointAddress) == UE_DIR_IN)
 209                                         {
 210                                                 D(bug("[MSS] IN endpoint found\n"));
 211                                                 mss->ep_in = ep;
 212                                                 mss->pipe_in = HIDD_USBDevice_CreatePipe(o, PIPE_Bulk, ep->bEndpointAddress, 0, AROS_LE2WORD(ep->wMaxPacketSize), 10000);
 213                                         }
 214                                         else if (!mss->pipe_out && UE_GET_DIR(ep->bEndpointAddress) == UE_DIR_OUT)
 215                                         {
 216                                                 D(bug("[MSS] OUT endpoint found\n"));
 217                                                 mss->ep_out = ep;
 218                                                 mss->pipe_out = HIDD_USBDevice_CreatePipe(o, PIPE_Bulk, ep->bEndpointAddress, 0, AROS_LE2WORD(ep->wMaxPacketSize), 10000);
 219                                         }
 220                                 }
 221                         }
 222
 223                         /* Pipes are there. Let's start the handler task */
 224                         if (mss->pipe_in && mss->pipe_out)
 225                         {
 226                                 struct Task *t;
 227                                 struct MemList *ml;
 228
 229                                 HIDD_USBStorage_Reset(o);
 230
 231                                 /* Get the unit from cache, in case it was used before */
 232                                 mss->cache = GetUnitFromCache(cl, o, 0);
 233
 234                                 /*
 235                                  * Reuse the unit, i.e. do not create the tasks, just signal them and update
 236                                  * unit structures
 237                                  */
 238                                 if (mss->cache)
 239                                 {
 240                                         mss->unitNum = mss->cache->unitNumber;
 241
 242                                         for (i=0; i <= mss->maxLUN; i++)
 243                                         {
 244                                                 mss->unit[i] = (mss_unit_t *)REMHEAD(&mss->cache->units);
 245                                                 if (mss->unit[i])
 246                                                 {
 247                                                         mss->unit[i]->msu_object = o;
 248
 249                                                         if (mss->unit[i])
 250                                                         {
 251                                                                 mss->handler[i] = mss->unit[i]->msu_handler;
 252
 253                                                                 AddTail((struct List *)&SD(cl)->unitList, (struct Node *)mss->unit[i]);
 254                                                         }
 255                                                 }
 256                                                 if (mss->handler[i])
 257                                                         Signal(mss->handler[i], SIGF_SINGLE);
 258                                         }
 259                                 }
 260                                 else
 261                                 {
 262                                         /* There was no such device used before. Create the cache element and tasks */
 263
 264                                         mss->cache = GetUnitFromCache(cl, o, 1);
 265                                         mss->unitNum = mss->cache->unitNumber;
 266
 267                                         /*
 268                                          * For each LUN create a separate task. The unit number will be created as combination of
 269                                          * unitNum and the LUN: unit 0x00 is Unit 0 LUN 0, 0x12 is Unit 1 LUN 2.
 270                                          */
 271                                         for (i=0; i <= mss->maxLUN; i++)
 272                                         {
 273                                                 struct TagItem tags[] = {
 274                                                                                         { TASKTAG_ARG1,   (IPTR)cl },
 275                                                                                         { TASKTAG_ARG2,   (IPTR)o },
 276                                                                                         { TASKTAG_ARG3,   i },
 277                                                                                         { TASKTAG_ARG4,   (IPTR)FindTask(NULL) },
 278                                                                                         { TAG_DONE,       0UL }};
 279
 280                                                 t = AllocMem(sizeof(struct Task), MEMF_PUBLIC|MEMF_CLEAR);
 281                                                 ml = AllocMem(sizeof(struct MemList) + 2*sizeof(struct MemEntry), MEMF_PUBLIC|MEMF_CLEAR);
 282
 283                                                 if (t && ml)
 284                                                 {
 285                                                         uint8_t *sp = AllocMem(10240, MEMF_PUBLIC|MEMF_CLEAR);
 286                                                         uint8_t *name = AllocMem(16, MEMF_PUBLIC|MEMF_CLEAR);
 287
 288                                                         /* Create individual name */
 289                                                         snprintf(name, 15, "USB MSS %02x.%x",mss->unitNum, i);
 290
 291                                                         t->tc_SPLower = sp;
 292                                                         t->tc_SPUpper = sp + 10240;
 293 #if AROS_STACK_GROWS_DOWNWARDS
 294                                                         t->tc_SPReg = (char *)t->tc_SPUpper - SP_OFFSET;
 295 #else
 296                                                         t->tc_SPReg = (char *)t->tc_SPLower + SP_OFFSET;
 297 #endif
 298
 299                                                         ml->ml_NumEntries = 3;
 300                                                         ml->ml_ME[0].me_Addr = t;
 301                                                         ml->ml_ME[0].me_Length = sizeof(struct Task);
 302                                                         ml->ml_ME[1].me_Addr = sp;
 303                                                         ml->ml_ME[1].me_Length = 10240;
 304                                                         ml->ml_ME[2].me_Addr = name;
 305                                                         ml->ml_ME[2].me_Length = 16;
 306
 307                                                         NEWLIST(&t->tc_MemEntry);
 308                                                         ADDHEAD(&t->tc_MemEntry, &ml->ml_Node);
 309
 310                                                         t->tc_Node.ln_Name = name;
 311                                                         t->tc_Node.ln_Type = NT_TASK;
 312                                                         t->tc_Node.ln_Pri = 1;     /* same priority as input.device */
 313
 314                                                         /* Add task. It will get back in touch soon */
 315                                                         NewAddTask(t, StorageTask, NULL, &tags[0]);
 316                                                         /* Keep the initialization synchronous */
 317                                                         Wait(SIGF_SINGLE);
 318                                                         mss->handler[i] = t;
 319
 320                                                         /* Detect partitions here? */
 321                                                         USBMSS_AddVolume(mss->unit[i]);
 322                                                 }
 323                                         }
 324                                 }
 325                         }
 326         }
 327     }
 328
 329     D(bug("[MSS] Storage::New() = %p\n", o));
 330
 331     if (!o)
 332         BASE(cl->UserData)->LibNode.lib_OpenCnt--;
 333
 334     return o;
 335 }
 336
 337 void METHOD(Storage, Root, Dispose)
 338 {
 339     StorageData *mss = OOP_INST_DATA(cl, o);
 340     OOP_Object *drv = NULL;
 341
 342     int i;
 343     struct Library *base = &BASE(cl->UserData)->LibNode;
 344
 345     D(bug("[MSS] ::Dispose()\n"));
 346
 347     if (mss)
 348     {
 349         for (i=0; i <= mss->maxLUN; i++)
 350         {
 351                 if (mss->unit[i])
 352                 {
 353                         REMOVE(mss->unit[i]);
 354                         ADDTAIL(&mss->cache->units, mss->unit[i]);
 355                 }
 356                 if (mss->handler[i])
 357                 {
 358                 Signal(mss->handler[i], SIGF_SINGLE);
 359                 }
 360         }
 361
 362         HIDD_USBDevice_DeletePipe(o, mss->pipe_in);
 363         HIDD_USBDevice_DeletePipe(o, mss->pipe_out);
 364     }
 365
 366     OOP_GetAttr(o, aHidd_USBDevice_Bus, (intptr_t *)&drv);
 367
 368     FreeVecPooled(SD(cl)->MemPool, mss->cdesc);
 369
 370     OOP_DoSuperMethod(cl, o, (OOP_Msg)msg);
 371
 372     base->lib_OpenCnt--;
 373 }
 374
 375 BOOL METHOD(Storage, Hidd_USBStorage, Reset)
 376 {
 377     StorageData *mss = OOP_INST_DATA(cl, o);
 378         USBDevice_Request req;
 379         intptr_t ifnr;
 380
 381         ObtainSemaphore(&mss->lock);
 382         OOP_GetAttr(o, aHidd_USBDevice_InterfaceNumber, &ifnr);
 383
 384         req.bmRequestType = UT_WRITE_CLASS_INTERFACE;
 385         req.bRequest = 0xff;
 386         req.wValue = 0;
 387         req.wIndex = AROS_WORD2LE(ifnr);
 388         req.wLength = 0;
 389
 390         HIDD_USBDevice_ControlMessage(o, NULL, &req, NULL, 0);
 391         ReleaseSemaphore(&mss->lock);
 392
 393         return TRUE;
 394 }
 395
 396 uint8_t METHOD(Storage, Hidd_USBStorage, GetMaxLUN)
 397 {
 398         USBDevice_Request req;
 399         intptr_t ifnr;
 400         uint8_t maxlun;
 401
 402         OOP_GetAttr(o, aHidd_USBDevice_InterfaceNumber, &ifnr);
 403
 404         req.bmRequestType = UT_READ_CLASS_INTERFACE;
 405         req.bRequest = 0xfe;
 406         req.wValue = 0;
 407         req.wIndex = AROS_WORD2LE(ifnr);
 408         req.wLength = 1;
 409
 410         if (HIDD_USBDevice_ControlMessage(o, NULL, &req, &maxlun, 1))
 411                 return maxlun;
 412         else
 413                 return 0xff;
 414 }
 415
 416 uint32_t METHOD(Storage, Hidd_USBStorage, DirectSCSI)
 417 {
 418         StorageData *mss = OOP_INST_DATA(cl, o);
 419         cbw_t cbw;
 420         csw_t csw;
 421         int i;
 422         uint32_t retval = 0;
 423
 424         for (i=0; i < msg->cmdLen; i++)
 425                 cbw.CBWCB[i] = msg->cmd[i];
 426
 427         cbw.bCBWLUN = msg->lun;
 428         cbw.dCBWSignature = AROS_LONG2LE(CBW_SIGNATURE);
 429         cbw.dCBWTag = getTID(SD(cl));
 430         cbw.dCBWDataTransferLength = AROS_LONG2LE(msg->dataLen);
 431         cbw.bmCBWFlags = msg->dataLen ? (msg->read ? CBW_FLAGS_IN : CBW_FLAGS_OUT) : 0;
 432         cbw.bCBWCBLength = msg->cmdLen;
 433
 434         ObtainSemaphore(&mss->lock);
 435
 436 //      D(bug("[MSS] DirectSCSI -> (%08x,%08x,%08x,%02x,%02x,%02x) @ %08x ",
 437 //                      AROS_LONG2LE(cbw.dCBWSignature),
 438 //                      cbw.dCBWTag,
 439 //                      AROS_LONG2LE(cbw.dCBWDataTransferLength),
 440 //                      cbw.bCBWLUN, cbw.bmCBWFlags, cbw.bCBWCBLength,
 441 //                      msg->dataLen? msg->data:0));
 442 //
 443 //      for (i=0; i < msg->cmdLen; i++)
 444 //      {
 445 //              D(bug("%02x%c", msg->cmd[i], i < (msg->cmdLen) ? ',':')'));
 446 //      }
 447 //      D(bug("\n"));
 448
 449         if (HIDD_USBDevice_BulkTransfer(o, mss->pipe_out, &cbw, 31))
 450         {
 451                 if (msg->dataLen)
 452                 {
 453                         if (msg->read)
 454                                 HIDD_USBDevice_BulkTransfer(o, mss->pipe_in, msg->data, msg->dataLen);
 455                         else
 456                                 HIDD_USBDevice_BulkTransfer(o, mss->pipe_out, msg->data, msg->dataLen);
 457                 }
 458
 459                 if (HIDD_USBDevice_BulkTransfer(o, mss->pipe_in, &csw, 13))
 460                 {
 461 //                      D(bug("[MSS] DirectSCSI <- (%08x,%08x,%08x,%02x)\n",
 462 //                                      AROS_LONG2LE(csw.dCSWSignature),
 463 //                                      csw.dCSWTag,
 464 //                                      AROS_LONG2LE(csw.dCSWDataResidue),
 465 //                                      csw.bCSWStatus));
 466
 467                         if (csw.dCSWSignature == AROS_LONG2LE(CSW_SIGNATURE))
 468                         {
 469                                 if (csw.dCSWTag == cbw.dCBWTag)
 470                                 {
 471                                         if (!csw.bCSWStatus)
 472                                                 retval = 1;
 473                                 }
 474                         }
 475                 }
 476         }
 477
 478         ReleaseSemaphore(&mss->lock);
 479
 480         return retval;
 481 }
 482
 483 BOOL METHOD(Storage, Hidd_USBStorage, TestUnitReady)
 484 {
 485         StorageData *mss = OOP_INST_DATA(cl, o);
 486         uint8_t cmd[12] = {0};
 487
 488         if (msg->lun > mss->maxLUN)
 489                 return FALSE;
 490
 491         if (HIDD_USBStorage_DirectSCSI(o, msg->lun, cmd, 12, NULL, 0, 1))
 492                 return TRUE;
 493         else
 494                 return FALSE;
 495 }
 496
 497 BOOL METHOD(Storage, Hidd_USBStorage, Inquiry)
 498 {
 499         StorageData *mss = OOP_INST_DATA(cl, o);
 500         uint8_t cmd[6] = {0x12, 0, 0, 0, 0, 0};
 501
 502         if (msg->lun > mss->maxLUN)
 503                 return FALSE;
 504
 505         cmd[4] = msg->bufferLength;
 506
 507         if (msg->buffer)
 508                 return HIDD_USBStorage_DirectSCSI(o, msg->lun, cmd, 6, msg->buffer, msg->bufferLength, 1);
 509         else
 510                 return FALSE;
 511 }
 512
 513 BOOL METHOD(Storage, Hidd_USBStorage, RequestSense)
 514 {
 515         StorageData *mss = OOP_INST_DATA(cl, o);
 516         uint8_t cmd[6] = {0x03, 0, 0, 0, 0, 0};
 517
 518         if (msg->lun > mss->maxLUN)
 519                 return FALSE;
 520
 521         cmd[4] = msg->bufferLength;
 522
 523         if (msg->buffer)
 524                 return HIDD_USBStorage_DirectSCSI(o, msg->lun, cmd, 6, msg->buffer, msg->bufferLength, 1);
 525         else
 526                 return FALSE;
 527 }
 528
 529 BOOL METHOD(Storage, Hidd_USBStorage, Read)
 530 {
 531         StorageData *mss = OOP_INST_DATA(cl, o);
 532         uint8_t cmd[10] = {0x28, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 533
 534         ULONG count = msg->count;
 535         ULONG block = msg->block;
 536         UBYTE *buf = msg->buffer;
 537
 538         if (msg->lun > mss->maxLUN)
 539                 return FALSE;
 540
 541 //      D(bug("[MSS] ::Read(%08x, %04x) => %p\n", msg->block, msg->count, msg->buffer));
 542
 543         if (buf)
 544         {
 545                 while(count)
 546                 {
 547                         ULONG cnt = count > 1024 ? 1024 : count;
 548
 549                         cmd[2] = block >>24;
 550                         cmd[3] = block >>16;
 551                         cmd[4] = block >>8;
 552                         cmd[5] = block;
 553
 554                         cmd[7] = cnt >> 8;
 555                         cmd[8] = cnt;
 556
 557                         if (!HIDD_USBStorage_DirectSCSI(o, msg->lun, cmd, 10, buf, cnt * mss->blocksize[msg->lun], 1))
 558                                 return FALSE;
 559
 560                         block += cnt;
 561                         buf += cnt * mss->blocksize[msg->lun];
 562                         count -= cnt;
 563                 }
 564
 565                 return TRUE;
 566         }
 567         else
 568                 return FALSE;
 569 }
 570
 571 BOOL METHOD(Storage, Hidd_USBStorage, Write)
 572 {
 573         StorageData *mss = OOP_INST_DATA(cl, o);
 574         uint8_t cmd[10] = {0x2a, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 575
 576         ULONG count = msg->count;
 577         ULONG block = msg->block;
 578         UBYTE *buf = msg->buffer;
 579
 580         if (msg->lun > mss->maxLUN)
 581                 return FALSE;
 582
 583 //      D(bug("[MSS] ::Write(%08x, %04x) <= %p\n", msg->block, msg->count, msg->buffer));
 584
 585         if (buf)
 586         {
 587                 while(count)
 588                 {
 589                         ULONG cnt = count > 1024 ? 1024 : count;
 590
 591                         cmd[2] = block >>24;
 592                         cmd[3] = block >>16;
 593                         cmd[4] = block >>8;
 594                         cmd[5] = block;
 595
 596                         cmd[7] = cnt >> 8;
 597                         cmd[8] = cnt;
 598
 599                         if (!HIDD_USBStorage_DirectSCSI(o, msg->lun, cmd, 10, msg->buffer, msg->count * mss->blocksize[msg->lun], 0))
 600                                 return FALSE;
 601
 602                         block += cnt;
 603                         buf += cnt * mss->blocksize[msg->lun];
 604                         count -= cnt;
 605                 }
 606
 607                 return TRUE;
 608         }
 609         else
 610                 return FALSE;
 611 }
 612
 613 BOOL METHOD(Storage, Hidd_USBStorage, ReadCapacity)
 614 {
 615         StorageData *mss = OOP_INST_DATA(cl, o);
 616         uint8_t cmd[10] = {0x25, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 617         uint8_t capacity[8];
 618         BOOL retval = FALSE;
 619
 620         if (msg->lun > mss->maxLUN)
 621                 return FALSE;
 622
 623         D(bug("[MSS] ReadCapacity(%d, %x, %x)\n", msg->lun, msg->blockTotal, msg->blockSize));
 624
 625         if (HIDD_USBStorage_DirectSCSI(o, msg->lun, cmd, 10, capacity, 8, 1))
 626         {
 627                 mss->blocksize[msg->lun] = (capacity[4] << 24) | (capacity[5] << 16) | (capacity[6] << 8) | capacity[7];
 628
 629                 if (msg->blockTotal)
 630                         *msg->blockTotal = (capacity[0] << 24) | (capacity[1] << 16) | (capacity[2] << 8) | capacity[3];
 631                 if (msg->blockSize)
 632                         *msg->blockSize = mss->blocksize[msg->lun];
 633
 634                 retval = TRUE;
 635         }
 636
 637         D(bug("[MSS] == "));
 638
 639         int i;
 640         for (i=0; i < 8; i++)
 641                 D(bug("%02x ", capacity[i]));
 642
 643         D(bug("\n"));
 644
 645         return retval;
 646 }
 647
 648 static usb_interface_descriptor_t *find_idesc(usb_config_descriptor_t *cd, int ifaceidx, int altidx)
 649 {
 650     char *p = (char *)cd;
 651     char *end = p + AROS_LE2WORD(cd->wTotalLength);
 652     usb_interface_descriptor_t *d;
 653     int curidx, lastidx, curaidx = 0;
 654
 655     for (curidx = lastidx = -1; p < end; ) {
 656         d = (usb_interface_descriptor_t *)p;
 657
 658         if (d->bLength == 0) /* bad descriptor */
 659             break;
 660         p += d->bLength;
 661         if (p <= end && d->bDescriptorType == UDESC_INTERFACE) {
 662             if (d->bInterfaceNumber != lastidx) {
 663                 lastidx = d->bInterfaceNumber;
 664                 curidx++;
 665                 curaidx = 0;
 666             } else
 667                 curaidx++;
 668             if (ifaceidx == curidx && altidx == curaidx)
 669                 return (d);
 670         }
 671     }
 672     return (NULL);
 673 }
 674
 675 static const char *subclass[] = {
 676                 NULL, "SCSI subset", "SFF-8020i", "QIC-157", "UFI", "SFF-8070i", "SCSI complete"
 677 };
 678
 679 /*
 680  * The MatchCLID library call gets the device descriptor and full config
 681  * descriptor (including the interface and endpoint descriptors). It will
 682  * return CLID_Hidd_USBStorage if the requested interface conforms to Mass
 683  * Storage.
 684  */
 685 AROS_LH3(void *, MatchCLID,
 686          AROS_LHA(usb_device_descriptor_t *, dev,       A0),
 687          AROS_LHA(usb_config_descriptor_t *, cfg,       A1),
 688          AROS_LHA(int,                       i,         D0),
 689          LIBBASETYPEPTR, LIBBASE, 5, Storage)
 690 {
 691     AROS_LIBFUNC_INIT
 692
 693     void *clid = NULL;
 694
 695     D(bug("[MSS] MatchCLID(%p, %p)\n", dev, cfg));
 696
 697     if (dev->bDeviceClass == UDCLASS_IN_INTERFACE)
 698     {
 699         usb_interface_descriptor_t *iface = find_idesc(cfg, i, 0);
 700
 701         D(bug("[MSS] UDCLASS_IN_INTERFACE OK. Checking interface %d\n", i));
 702         D(bug("[MSS] iface %d @ %p class %d subclass %d protocol %d\n", i, iface,
 703                         iface->bInterfaceClass, iface->bInterfaceSubClass, iface->bInterfaceProtocol));
 704
 705         if (iface->bInterfaceClass == 0x08)
 706         {
 707                 D(bug("[MSS] Interface may be handled by Mass Storage Class\n"));
 708
 709                 if (iface->bInterfaceProtocol == 0x50)
 710                 {
 711                         D(bug("[MSS] Lucky you. The device supports Bulk Only transport\n"));
 712
 713                         if (iface->bInterfaceSubClass > 0 && iface->bInterfaceSubClass < 7)
 714                         {
 715                                 D(bug("[MSS] Protocol used: %s\n", subclass[iface->bInterfaceSubClass]));
 716
 717                                 if (iface->bInterfaceSubClass == 1 ||
 718                                                 iface->bInterfaceSubClass == 2 ||
 719                                                 iface->bInterfaceSubClass == 6)
 720                                         clid = CLID_Hidd_USBStorage;
 721                         }
 722                 }
 723         }
 724     }
 725
 726     if (clid)
 727         D(bug("[MSS] Pick me! Pick me! Pick me! I can handle it!\n"));
 728
 729     return clid;
 730
 731     AROS_LIBFUNC_EXIT
 732 }
 733
 734