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

   1 /*
   2  * storage_device.c
   3  *
   4  *  Created on: Oct 27, 2008
   5  *      Author: misc
   6  */
   7
   8 /*
   9  * This file contains exec style device, usbmss.device, which is available as
  10  * soon as the mass storage class gets initialized. The device translates the
  11  * trackdisk commands into mass storage methods.
  12  */
  13 #include <aros/symbolsets.h>
  14 #include <aros/macros.h>
  15
  16 #include <exec/types.h>
  17 #include <exec/nodes.h>
  18 #include <exec/devices.h>
  19 #include <exec/io.h>
  20 #include <exec/errors.h>
  21 #include <devices/trackdisk.h>
  22 #include <devices/scsidisk.h>
  23 #include <devices/newstyle.h>
  24 #include <oop/oop.h>
  25 #include <dos/dos.h>
  26 #include <dos/dosextens.h>
  27
  28 #include <hidd/hidd.h>
  29 #include <usb/usb.h>
  30 #include <usb/mstorage.h>
  31
  32 #define DEBUG 1
  33
  34 #include <proto/exec.h>
  35 #include <proto/oop.h>
  36 #include <proto/dos.h>
  37 #include <aros/debug.h>
  38
  39 #include "storage.h"
  40 #include LC_LIBDEFS_FILE
  41
  42 #undef SD
  43 #define SD(x) (&LIBBASE->sd)
  44
  45 /* Invalid command - reports error */
  46 static void cmd_Invalid(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
  47 {
  48         D(bug("[MSS-dev] Invalid command %d for unit %04x\n", io->io_Command, unit->msu_unitNum));
  49         io->io_Error = IOERR_NOCMD;
  50 }
  51
  52 /* NULL commad - always success. */
  53 static void cmd_NULL(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
  54 {
  55         IOStdReq(io)->io_Actual = 0;
  56 }
  57
  58 static void cmd_Reset(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
  59 {
  60         HIDD_USBStorage_Reset(unit->msu_object);
  61         IOStdReq(io)->io_Actual = 0;
  62 }
  63
  64 static void cmd_Remove(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
  65 {
  66         if (unit->msu_removeInt)
  67                 io->io_Error = TDERR_DriveInUse;
  68         else
  69                 unit->msu_removeInt = (struct Interrupt *)IOStdReq(io)->io_Data;
  70 }
  71
  72 static void cmd_AddChangeInt(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
  73 {
  74         Forbid();
  75         AddHead(&unit->msu_diskChangeList, (struct Node *)io);
  76         Permit();
  77
  78         io->io_Flags &= ~IOF_QUICK;
  79         unit->msu_unit.unit_flags &= ~UNITF_ACTIVE;
  80 }
  81
  82 static void cmd_RemChangeInt(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
  83 {
  84         Forbid();
  85         Remove((struct Node *)io);
  86         Permit();
  87 }
  88
  89 static void cmd_ChangeNum(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
  90 {
  91         IOStdReq(io)->io_Actual = unit->msu_changeNum;
  92 }
  93
  94 static void cmd_ProtStatus(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
  95 {
  96         IOStdReq(io)->io_Actual = ((unit->msu_inquiry[0] & 0x1f) == DG_CDROM) ? -1 : 0;
  97 }
  98
  99 static void cmd_ChangeState(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
 100 {
 101         IOStdReq(io)->io_Actual = unit->msu_flags & MSF_DiskPresent ? 0:1;
 102 }
 103
 104 static void cmd_GetGeometry(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
 105 {
 106         if (!(unit->msu_flags & MSF_DiskPresent))
 107         {
 108                 io->io_Error = TDERR_DiskChanged;
 109         }
 110         else if (IOStdReq(io)->io_Length == sizeof(struct DriveGeometry))
 111         {
 112                 struct DriveGeometry *dg = (struct DriveGeometry *)IOStdReq(io)->io_Data;
 113
 114                 dg->dg_SectorSize       = unit->msu_blockSize;
 115                 dg->dg_TotalSectors             = unit->msu_blockCount;
 116                 dg->dg_Cylinders                = unit->msu_blockCount / (255*63);
 117                 dg->dg_Heads                    = 255;
 118                 dg->dg_TrackSectors             = 63;
 119                 dg->dg_CylSectors               = 255*63;
 120                 dg->dg_BufMemType               = MEMF_PUBLIC;
 121                 dg->dg_DeviceType               = unit->msu_inquiry[0] & 0x1f;
 122                 dg->dg_Flags                    = (dg->dg_DeviceType == DG_CDROM) ? DGF_REMOVABLE : 0;
 123                 dg->dg_Reserved                 = 0;
 124
 125                 IOStdReq(io)->io_Actual = sizeof(struct DriveGeometry);
 126         }
 127         else
 128                 io->io_Error = TDERR_NotSpecified;
 129 }
 130
 131 static void cmd_Read32(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
 132 {
 133         uint32_t block = IOStdReq(io)->io_Offset;
 134         uint32_t count = IOStdReq(io)->io_Length;
 135
 136         if (!(unit->msu_flags & MSF_DiskPresent))
 137         {
 138                 io->io_Error = TDERR_DiskChanged;
 139         }
 140         else
 141         {
 142 //              D(bug("[%s] Read32(%08x, %08x)\n", FindTask(NULL)->tc_Node.ln_Name, IOStdReq(io)->io_Offset, IOStdReq(io)->io_Length));
 143
 144                 if (!IOStdReq(io)->io_Data)
 145                 {
 146                         D(bug("[%s] No buffer!\n", FindTask(NULL)->tc_Node.ln_Name));
 147                         io->io_Error = IOERR_BADADDRESS;
 148                 }
 149                 else if ((block & ((1 <<  unit->msu_blockShift) - 1)) || (count & ((1 <<  unit->msu_blockShift) - 1)))
 150                 {
 151                         D(bug("[%s] offset or length not sector-aligned\n", FindTask(NULL)->tc_Node.ln_Name));
 152                         cmd_Invalid(io, dev, unit);
 153                 }
 154                 else
 155                 {
 156                         block >>= unit->msu_blockShift;
 157                         count >>= unit->msu_blockShift;
 158
 159                         if (block + count - 1> unit->msu_blockCount)
 160                         {
 161                                 D(bug("[%s] ERROR! Requested read outside the available area\n", FindTask(NULL)->tc_Node.ln_Name));
 162                                 io->io_Error = IOERR_BADADDRESS;
 163                         }
 164                         else
 165                         {
 166                                 if (!HIDD_USBStorage_Read(unit->msu_object, unit->msu_lun, IOStdReq(io)->io_Data, block, count))
 167                                 {
 168                                         D(bug("[%s] READ ERROR: block=0x%08x count=0x%08x\n", FindTask(NULL)->tc_Node.ln_Name, block, count));
 169
 170                                         HIDD_USBStorage_Reset(unit->msu_object);
 171
 172                                         if (!HIDD_USBStorage_Read(unit->msu_object, unit->msu_lun, IOStdReq(io)->io_Data, block, count))
 173                                         {
 174                                                 IOStdReq(io)->io_Error = TDERR_NotSpecified;
 175                                                 IOStdReq(io)->io_Actual = 0;
 176
 177                                                 return;
 178                                         }
 179                                 }
 180                                 IOStdReq(io)->io_Actual = IOStdReq(io)->io_Length;
 181                         }
 182                 }
 183         }
 184 }
 185
 186 static void cmd_Read64(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
 187 {
 188         uint64_t block = (uint64_t)(IOStdReq(io)->io_Offset) | ((uint64_t)(IOStdReq(io)->io_Actual) << 32);
 189         uint32_t count = IOStdReq(io)->io_Length;
 190
 191         if (!(unit->msu_flags & MSF_DiskPresent))
 192         {
 193                 io->io_Error = TDERR_DiskChanged;
 194         }
 195         else
 196         {
 197
 198 //              D(bug("[%s] Read64(%04x%08x, %08x)\n", FindTask(NULL)->tc_Node.ln_Name, IOStdReq(io)->io_Actual, IOStdReq(io)->io_Offset, IOStdReq(io)->io_Length));
 199
 200                 io->io_Error = 0;
 201
 202                 if (!IOStdReq(io)->io_Data)
 203                 {
 204                         D(bug("[%s] No buffer!\n", FindTask(NULL)->tc_Node.ln_Name));
 205                         io->io_Error = IOERR_BADADDRESS;
 206                 }
 207                 else if ((block & ((1 <<  unit->msu_blockShift) - 1)) || (count & ((1 <<  unit->msu_blockShift) - 1)))
 208                 {
 209                         D(bug("[%s] offset or length not sector-aligned\n", FindTask(NULL)->tc_Node.ln_Name));
 210                         cmd_Invalid(io, dev, unit);
 211                 }
 212                 else
 213                 {
 214                         block >>= unit->msu_blockShift;
 215                         count >>= unit->msu_blockShift;
 216
 217                         if (block + count - 1> unit->msu_blockCount)
 218                         {
 219                                 D(bug("[%s] ERROR! Requested read outside the available area\n", FindTask(NULL)->tc_Node.ln_Name));
 220                                 io->io_Error = IOERR_BADADDRESS;
 221                         }
 222                         else
 223                         {
 224                                 if (!HIDD_USBStorage_Read(unit->msu_object, unit->msu_lun, IOStdReq(io)->io_Data, block, count))
 225                                 {
 226                                         D(bug("[%s] READ ERROR: block=0x%08x count=0x%08x\n", FindTask(NULL)->tc_Node.ln_Name, (int32_t)block, count));
 227
 228                                         HIDD_USBStorage_Reset(unit->msu_object);
 229
 230                                         if (!HIDD_USBStorage_Read(unit->msu_object, unit->msu_lun, IOStdReq(io)->io_Data, block, count))
 231                                         {
 232                                                 IOStdReq(io)->io_Error = TDERR_NotSpecified;
 233                                                 IOStdReq(io)->io_Actual = 0;
 234
 235                                                 return;
 236                                         }
 237                                 }
 238                                 IOStdReq(io)->io_Actual = IOStdReq(io)->io_Length;
 239                         }
 240                 }
 241         }
 242 }
 243
 244 static void cmd_Write32(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
 245 {
 246         uint32_t block = IOStdReq(io)->io_Offset;
 247         uint32_t count = IOStdReq(io)->io_Length;
 248
 249         if (!(unit->msu_flags & MSF_DiskPresent))
 250         {
 251                 io->io_Error = TDERR_DiskChanged;
 252         }
 253         else
 254         {
 255
 256         //      D(bug("[%s] Write32(%08x, %08x)\n", FindTask(NULL)->tc_Node.ln_Name, IOStdReq(io)->io_Offset, IOStdReq(io)->io_Length));
 257
 258                 if (!IOStdReq(io)->io_Data)
 259                 {
 260                         D(bug("[%s] No buffer!\n", FindTask(NULL)->tc_Node.ln_Name));
 261                         io->io_Error = IOERR_BADADDRESS;
 262                 }
 263                 else if ((block & ((1 <<  unit->msu_blockShift) - 1)) || (count & ((1 <<  unit->msu_blockShift) - 1)))
 264                 {
 265                         D(bug("[%s] offset or length not sector-aligned\n", FindTask(NULL)->tc_Node.ln_Name));
 266                         cmd_Invalid(io, dev, unit);
 267                 }
 268                 else
 269                 {
 270                         block >>= unit->msu_blockShift;
 271                         count >>= unit->msu_blockShift;
 272
 273                         if (block + count - 1> unit->msu_blockCount)
 274                         {
 275                                 D(bug("[%s] ERROR! Requested write outside the available area\n", FindTask(NULL)->tc_Node.ln_Name));
 276                                 io->io_Error = IOERR_BADADDRESS;
 277                         }
 278                         else
 279                         {
 280                                 if (!HIDD_USBStorage_Write(unit->msu_object, unit->msu_lun, IOStdReq(io)->io_Data, block, count))
 281                                 {
 282                                         D(bug("[%s] WRITE ERROR: block=0x%08x count=0x%08x\n", FindTask(NULL)->tc_Node.ln_Name, block, count));
 283
 284                                         HIDD_USBStorage_Reset(unit->msu_object);
 285
 286                                         if (!HIDD_USBStorage_Write(unit->msu_object, unit->msu_lun, IOStdReq(io)->io_Data, block, count))
 287                                         {
 288                                                 IOStdReq(io)->io_Error = TDERR_NotSpecified;
 289                                                 IOStdReq(io)->io_Actual = 0;
 290
 291                                                 return;
 292                                         }
 293                                 }
 294                                 IOStdReq(io)->io_Actual = IOStdReq(io)->io_Length;
 295                         }
 296                 }
 297         }
 298 }
 299
 300 static void cmd_Write64(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
 301 {
 302         uint64_t block = (uint64_t)(IOStdReq(io)->io_Offset) | ((uint64_t)(IOStdReq(io)->io_Actual) << 32);
 303         uint32_t count = IOStdReq(io)->io_Length;
 304
 305         if (!(unit->msu_flags & MSF_DiskPresent))
 306         {
 307                 io->io_Error = TDERR_DiskChanged;
 308         }
 309         else
 310         {
 311 //              D(bug("[%s] Write64(%04x%08x, %08x)\n", FindTask(NULL)->tc_Node.ln_Name, IOStdReq(io)->io_Actual, IOStdReq(io)->io_Offset, IOStdReq(io)->io_Length));
 312
 313                 io->io_Error = 0;
 314
 315                 if (!IOStdReq(io)->io_Data)
 316                 {
 317                         D(bug("[%s] No buffer!\n", FindTask(NULL)->tc_Node.ln_Name));
 318                         io->io_Error = IOERR_BADADDRESS;
 319                 }
 320                 else if ((block & ((1 <<  unit->msu_blockShift) - 1)) || (count & ((1 <<  unit->msu_blockShift) - 1)))
 321                 {
 322                         D(bug("[%s] offset or length not sector-aligned\n", FindTask(NULL)->tc_Node.ln_Name));
 323                         cmd_Invalid(io, dev, unit);
 324                 }
 325                 else
 326                 {
 327                         block >>= unit->msu_blockShift;
 328                         count >>= unit->msu_blockShift;
 329
 330                         if (block + count - 1 > unit->msu_blockCount)
 331                         {
 332                                 D(bug("[%s] ERROR! Requested write outside the available area\n", FindTask(NULL)->tc_Node.ln_Name));
 333                                 io->io_Error = IOERR_BADADDRESS;
 334                         }
 335                         else
 336                         {
 337                                 if (!HIDD_USBStorage_Write(unit->msu_object, unit->msu_lun, IOStdReq(io)->io_Data, block, count))
 338                                 {
 339                                         D(bug("[%s] WRITE ERROR: block=0x%08x count=0x%08x\n", FindTask(NULL)->tc_Node.ln_Name, (int32_t)block, count));
 340
 341                                         HIDD_USBStorage_Reset(unit->msu_object);
 342
 343                                         if (!HIDD_USBStorage_Write(unit->msu_object, unit->msu_lun, IOStdReq(io)->io_Data, block, count))
 344                                         {
 345                                                 IOStdReq(io)->io_Error = TDERR_NotSpecified;
 346                                                 IOStdReq(io)->io_Actual = 0;
 347
 348                                                 return;
 349                                         }
 350                                 }
 351                                 IOStdReq(io)->io_Actual = IOStdReq(io)->io_Length;
 352                         }
 353                 }
 354         }
 355 }
 356
 357 static void cmd_DirectSCSI(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit)
 358 {
 359         struct SCSICmd *cmd = (struct SCSICmd *)IOStdReq(io)->io_Data;
 360
 361         IOStdReq(io)->io_Error = 0;
 362         int i;
 363
 364         D(bug("[%s] DirectSCSI(", FindTask(NULL)->tc_Node.ln_Name));
 365         for (i=0; i < cmd->scsi_CmdLength; i++)
 366         {
 367                 D(bug("%02x%c", cmd->scsi_Command[i], i < (cmd->scsi_CmdLength-1) ? ',':')'));
 368         }
 369         D(bug("\n"));
 370
 371         if (cmd)
 372         {
 373                 cmd->scsi_Actual = 0;
 374                 cmd->scsi_Status = 0;
 375
 376                 /*
 377                    Handle the USB SCSI quirks. The code is bad because it assumes the
 378                    cmd->scsi_Command is writable
 379                 */
 380                 switch(cmd->scsi_Command[0])
 381                 {
 382                         case 0x12:      /* INQUIRY: USB devices deliver the first 36 bytes only */
 383                                 if (cmd->scsi_Command[4] > 36)
 384                                 {
 385                                         cmd->scsi_Command[4] = 36;
 386                                         cmd->scsi_Length = 36;
 387                                 }
 388                                 break;
 389
 390                         case 0x5a:      /* MODE SENSE 10: Only 8 bytes are delivered */
 391                                 if (((cmd->scsi_Command[7] << 8) | cmd->scsi_Command[8]) > 8)
 392                                 {
 393                                         cmd->scsi_Command[7] = 0;
 394                                         cmd->scsi_Command[8] = 8;
 395                                         cmd->scsi_Length = 8;
 396                                 }
 397                                 break;
 398
 399                         case 0x03:      /* REQUEST SENSE: USB devices deliver up to 18 bytes only*/
 400                                 if (cmd->scsi_Command[4] > 18)
 401                                 {
 402                                         cmd->scsi_Command[4] = 18;
 403                                         cmd->scsi_Length = 18;
 404                                 }
 405                                 break;
 406
 407                         case 0xec:      /* Hack to issue ATA Inquiry call. Ignore it. */
 408                                 if (cmd->scsi_CmdLength == 1)
 409                                 {
 410                                         IOStdReq(io)->io_Error = IOERR_NOCMD;
 411                                         return;
 412                                 }
 413                                 break;
 414                 }
 415
 416                 if (!HIDD_USBStorage_DirectSCSI(unit->msu_object, unit->msu_lun, cmd->scsi_Command, cmd->scsi_CmdLength, cmd->scsi_Data, cmd->scsi_Length, cmd->scsi_Flags & SCSIF_READ))
 417                 {
 418                         int i;
 419                         char *c = cmd->scsi_Command;
 420                         D(bug("[MSS-dev] DirectSCSI failed: "));
 421
 422                         for (i=0; i < cmd->scsi_CmdLength; i++)
 423                                 D(bug("%02x ", c[i]));
 424
 425                         D(bug("data=%08x len=%08x\n", cmd->scsi_Data, cmd->scsi_Length));
 426
 427                         IOStdReq(io)->io_Error = HFERR_BadStatus;
 428
 429                         cmd->scsi_Status = 2;
 430
 431                         if (cmd->scsi_Flags & SCSIF_AUTOSENSE)
 432                         {
 433                                 if (cmd->scsi_SenseData && cmd->scsi_SenseLength > 0)
 434                                 {
 435                                         uint8_t sense[18];
 436
 437                                         if (HIDD_USBStorage_RequestSense(unit->msu_object, unit->msu_lun, sense, 18))
 438                                         {
 439                                                 if (cmd->scsi_SenseLength >= 18)
 440                                                 {
 441                                                         CopyMem(sense, cmd->scsi_SenseData, 18);
 442                                                         cmd->scsi_SenseActual = 18;
 443                                                 }
 444                                                 else
 445                                                 {
 446                                                         CopyMem(sense, cmd->scsi_SenseData, cmd->scsi_SenseLength);
 447                                                         cmd->scsi_SenseActual = cmd->scsi_SenseLength;
 448                                                 }
 449                                         }
 450                                 }
 451                         }
 452                 }
 453                 else
 454                 {
 455                         IOStdReq(io)->io_Error = 0;
 456                         cmd->scsi_Actual = cmd->scsi_Length;
 457                         IOStdReq(io)->io_Actual = IOStdReq(io)->io_Length;
 458                 }
 459         }
 460 }
 461
 462 static const uint16_t supported_commands[] = {
 463                 CMD_RESET, CMD_READ, CMD_WRITE, CMD_UPDATE, CMD_CLEAR, CMD_START, CMD_STOP, CMD_FLUSH,
 464                 TD_MOTOR, TD_SEEK, TD_FORMAT, TD_REMOVE, TD_CHANGENUM, TD_CHANGESTATE, TD_PROTSTATUS,
 465                 TD_GETNUMTRACKS, TD_ADDCHANGEINT, TD_REMCHANGEINT, TD_GETGEOMETRY, TD_EJECT,
 466                 TD_READ64, TD_WRITE64, TD_SEEK64, TD_FORMAT64,
 467                 HD_SCSICMD, TD_GETDRIVETYPE,
 468                 NSCMD_DEVICEQUERY, NSCMD_TD_READ64, NSCMD_TD_WRITE64, NSCMD_TD_SEEK64, NSCMD_TD_FORMAT64,
 469                 0
 470 };
 471
 472 static void (*map32[HD_SCSICMD+1])(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit) = {
 473                 [CMD_INVALID]           = cmd_Invalid,
 474                 [CMD_RESET]             = cmd_Reset,
 475                 [CMD_READ]              = cmd_Read32,
 476                 [CMD_WRITE]             = cmd_Write32,
 477                 [CMD_UPDATE]            = cmd_Reset,
 478                 [CMD_CLEAR]             = cmd_Reset,
 479                 [CMD_STOP]              = cmd_Reset,
 480                 [CMD_START]             = cmd_Reset,
 481                 [CMD_FLUSH]             = cmd_Reset, /* Implement flush! */
 482                 [TD_MOTOR]              = cmd_NULL,
 483                 [TD_SEEK]               = cmd_NULL,
 484                 [TD_FORMAT]             = cmd_Write32,
 485                 [TD_REMOVE]             = cmd_Remove,
 486                 [TD_CHANGENUM]          = cmd_ChangeNum,
 487                 [TD_CHANGESTATE]        = cmd_ChangeState,
 488                 [TD_PROTSTATUS]         = cmd_ProtStatus,
 489                 [TD_RAWREAD]            = cmd_Invalid,
 490                 [TD_RAWWRITE]           = cmd_Invalid,
 491                 [TD_GETNUMTRACKS]   = cmd_Invalid,
 492                 [TD_ADDCHANGEINT]   = cmd_AddChangeInt,
 493                 [TD_REMCHANGEINT]   = cmd_RemChangeInt,
 494                 [TD_GETGEOMETRY]        = cmd_GetGeometry,
 495                 [TD_EJECT]              = cmd_Invalid,
 496                 [TD_READ64]             = cmd_Read64,
 497                 [TD_WRITE64]            = cmd_Write64,
 498                 [TD_SEEK64]             = cmd_NULL,
 499                 [TD_FORMAT64]           = cmd_Write64,
 500                 [HD_SCSICMD]            = cmd_DirectSCSI,
 501 };
 502
 503 static void (*map64[4])(struct IORequest *io, mss_device_t *dev, mss_unit_t *unit) = {
 504                 [NSCMD_TD_READ64 - 0xc000]              = cmd_Read64,
 505                 [NSCMD_TD_WRITE64 - 0xc000]     = cmd_Write64,
 506                 [NSCMD_TD_SEEK64 - 0xc000]              = cmd_NULL,
 507                 [NSCMD_TD_FORMAT64 - 0xc000]    = cmd_Write64,
 508 };
 509
 510 /*
 511  * IO handler. It may be either called directly, or through the mass storage
 512  * unit task.
 513  */
 514 void HandleIO(struct IORequest *io, mss_device_t *device, mss_unit_t *unit)
 515 {
 516         unit->msu_unit.unit_flags |= UNITF_ACTIVE;
 517
 518         io->io_Error = 0;
 519
 520         /* Some commands will be handled directly */
 521         switch (io->io_Command)
 522         {
 523         /*
 524                         New Style Devices query. Introduce self as trackdisk and provide list of
 525                         commands supported
 526          */
 527         case NSCMD_DEVICEQUERY:
 528         {
 529                 struct NSDeviceQueryResult *nsdq = (struct NSDeviceQueryResult *)IOStdReq(io)->io_Data;
 530                 nsdq->DevQueryFormat    = 0;
 531                 nsdq->SizeAvailable     = sizeof(struct NSDeviceQueryResult);
 532                 nsdq->DeviceType        = NSDEVTYPE_TRACKDISK;
 533                 nsdq->DeviceSubType     = 0;
 534                 nsdq->SupportedCommands = (uint16_t *)supported_commands;
 535                 IOStdReq(io)->io_Actual = sizeof(struct NSDeviceQueryResult);
 536                 break;
 537         }
 538
 539         /*
 540                         New Style Devices report here the 'NSTY' - only if such value is
 541                         returned here, the NSCMD_DEVICEQUERY might be called. Otherwice it should
 542                         report error.
 543          */
 544         case TD_GETDRIVETYPE:
 545                 IOStdReq(io)->io_Actual = DRIVE_NEWSTYLE;
 546                 break;
 547
 548         default:
 549                 if (io->io_Command <= HD_SCSICMD)
 550                 {
 551                         if (map32[io->io_Command])
 552                                 map32[io->io_Command](io, device, unit);
 553                         else
 554                                 cmd_Invalid(io, device, unit);
 555                 }
 556                 else if (io->io_Command >= NSCMD_TD_READ64 && io->io_Command <= NSCMD_TD_FORMAT64)
 557                 {
 558                         if (map64[io->io_Command - NSCMD_TD_READ64])
 559                                 map64[io->io_Command - NSCMD_TD_READ64](io, device, unit);
 560                         else
 561                                 cmd_Invalid(io, device, unit);
 562                 }
 563                 break;
 564         }
 565
 566         unit->msu_unit.unit_flags &= ~(UNITF_ACTIVE);
 567 }
 568
 569
 570
 571 /*
 572  * Exec device functions
 573  */
 574
 575 static
 576 AROS_LH3 (void, dev_OpenLib,
 577                 AROS_LHA(struct IORequest *, ioreq, A1),
 578                 AROS_LHA(ULONG, unitnum, D0),
 579                 AROS_LHA(ULONG, flags, D1),
 580                 mss_device_t *, lh, 1, Storage)
 581                 {
 582         AROS_LIBFUNC_INIT
 583
 584         mss_unit_t *unit, *found = NULL;
 585
 586         ioreq->io_Error = IOERR_OPENFAIL;
 587
 588         D(bug("[MSS-dev] Open(%p, %x, %x)\n", ioreq, unitnum, flags));
 589
 590         /* Lock the whole mass storage class and search for a requested unit */
 591         ObtainSemaphore(&lh->mss_static->Lock);
 592         ForeachNode(&lh->mss_static->unitList, unit)
 593         {
 594                 if (unit->msu_unitNum == unitnum)
 595                 {
 596                         found = unit;
 597                         break;
 598                 }
 599         }
 600         if (found)
 601         {
 602                 ioreq->io_Device = &lh->mss_device;
 603                 ioreq->io_Unit = (struct Unit *)found;
 604                 ioreq->io_Error = 0;
 605
 606                 found->msu_unit.unit_OpenCnt++;
 607         }
 608         ReleaseSemaphore(&lh->mss_static->Lock);
 609
 610         D(bug("[MSS-dev] %sfound unit %d at %p\n", found?"":"not ", unitnum, found));
 611
 612         AROS_LIBFUNC_EXIT
 613                 }
 614
 615 static
 616 AROS_LH1(BPTR, dev_CloseLib,
 617                 AROS_LHA(struct IORequest *, ioreq, A1),
 618                 mss_device_t *, lh, 2, Storage)
 619                 {
 620         AROS_LIBFUNC_INIT
 621
 622         /* Release the unit */
 623         ObtainSemaphore(&lh->mss_static->Lock);
 624         mss_unit_t *unit = (mss_unit_t *)ioreq->io_Unit;
 625
 626         ioreq->io_Unit = (struct Unit *)~0;
 627         if (unit && unit->msu_unit.unit_OpenCnt)
 628                 unit->msu_unit.unit_OpenCnt--;
 629
 630         ReleaseSemaphore(&lh->mss_static->Lock);
 631
 632         return NULL;
 633
 634         AROS_LIBFUNC_EXIT
 635                 }
 636
 637 static
 638 AROS_LH1 (BPTR, dev_ExpungeLib,
 639                 AROS_LHA(mss_device_t *, extralh, D0),
 640                 mss_device_t *, lh, 3, Storage)
 641                 {
 642         AROS_LIBFUNC_INIT
 643         return NULL;
 644         AROS_LIBFUNC_EXIT
 645                 }
 646
 647 static
 648 AROS_LH0 (LIBBASETYPEPTR, dev_ExtFuncLib,
 649                 mss_device_t *, lh, 4, Storage)
 650                 {
 651         AROS_LIBFUNC_INIT
 652         return NULL;
 653         AROS_LIBFUNC_EXIT
 654                 }
 655
 656 /*
 657  * Helper function used to test, whether the specified command is slow or not.
 658  * Slow commands will be transfered to the unit task, all others will be handled
 659  * immediately.
 660  *
 661  * NOTE: Every command performing *any* USB transfer/operation is considered slow.
 662  */
 663 static const uint64_t immediate_cmds = (
 664                 (1 << CMD_INVALID) |
 665                 (1 << CMD_CLEAR) |
 666                 (1 << TD_MOTOR) |
 667                 (1 << TD_SEEK) |
 668                 (1 << TD_RAWREAD) |
 669                 (1 << TD_RAWWRITE) |
 670                 (1 << TD_CHANGENUM) |
 671                 (1 << TD_CHANGESTATE) |
 672                 (1 << TD_ADDCHANGEINT) |
 673                 (1 << TD_REMCHANGEINT) |
 674                 (1 << TD_GETGEOMETRY) |
 675                 (1 << TD_SEEK64) |
 676                 (1 << TD_GETDRIVETYPE)
 677 );
 678
 679 static BOOL isSlow(uint32_t cmd)
 680 {
 681         BOOL slow = TRUE;
 682
 683         if (cmd < 32)
 684         {
 685                 if (immediate_cmds & (1 << cmd))
 686                         slow = FALSE;
 687         }
 688         else if (cmd == NSCMD_TD_SEEK64 || cmd == NSCMD_DEVICEQUERY)
 689                 slow = FALSE;
 690
 691 //      D(bug("[MSS-dev] isSlow(%04x) = %d\n", cmd, slow));
 692
 693         return slow;
 694 }
 695
 696 static
 697 AROS_LH1(void, dev_BeginIO,
 698                 AROS_LHA(struct IORequest *, io, A1),
 699                 mss_device_t *, mss_dev, 5, Storage)
 700                 {
 701         AROS_LIBFUNC_INIT
 702
 703 //      D(bug("[MSS-dev] BeginIO. CMD=%04x\n", io->io_Command));
 704
 705         if (io)
 706         {
 707                 mss_unit_t *unit = (mss_unit_t *)io->io_Unit;
 708
 709                 if (unit && unit != (void *)0xffffffff)
 710                 {
 711                         if (isSlow(io->io_Command))
 712                         {
 713 //                              D(bug("[MSS-dev] Forwarding IO call to device task\n"));
 714                                 unit->msu_unit.unit_flags |= UNITF_INTASK;
 715                                 io->io_Flags &= ~IOF_QUICK;
 716
 717                                 PutMsg(&unit->msu_unit.unit_MsgPort, &io->io_Message);
 718
 719                                 unit->msu_unit.unit_flags &= ~UNITF_INTASK;
 720                         }
 721                         else
 722                                 HandleIO(io, mss_dev, unit);
 723                 }
 724         }
 725
 726         AROS_LIBFUNC_EXIT
 727                 }
 728
 729 static
 730 AROS_LH1(LONG, dev_AbortIO,
 731                 AROS_LHA(struct IORequest *, io, A1),
 732                 mss_device_t *, mss_dev, 6, Storage)
 733                 {
 734         AROS_LIBFUNC_INIT
 735
 736         /* Cannot Abort IO */
 737         return 0;
 738
 739         AROS_LIBFUNC_EXIT
 740                 }
 741
 742 static
 743 AROS_LH1(ULONG, dev_GetRdskLba,
 744                 AROS_LHA(struct IORequest *, io, A1),
 745                 mss_device_t *, mss_dev, 7, Storage)
 746                 {
 747         AROS_LIBFUNC_INIT
 748
 749         return 0;
 750
 751         AROS_LIBFUNC_EXIT
 752                 }
 753
 754 static
 755 AROS_LH1(ULONG, dev_GetBlkSize,
 756                 AROS_LHA(struct IORequest *, io, A1),
 757                 mss_device_t *, mss_dev, 8, Storage)
 758                 {
 759         AROS_LIBFUNC_INIT
 760
 761         mss_unit_t *unit = (mss_unit_t *)io->io_Unit;
 762
 763         if (unit->msu_blockSize)
 764                 return AROS_LEAST_BIT_POS(unit->msu_blockSize);
 765         else
 766                 return 9;
 767
 768         AROS_LIBFUNC_EXIT
 769                 }
 770
 771 static const APTR Storage_dev_FuncTable[]=
 772 {
 773                 &AROS_SLIB_ENTRY(dev_OpenLib,Storage,1),
 774                 &AROS_SLIB_ENTRY(dev_CloseLib,Storage,2),
 775                 &AROS_SLIB_ENTRY(dev_ExpungeLib,Storage,3),
 776                 &AROS_SLIB_ENTRY(dev_ExtFuncLib,Storage,4),
 777                 &AROS_SLIB_ENTRY(dev_BeginIO,Storage,5),
 778                 &AROS_SLIB_ENTRY(dev_AbortIO,Storage,6),
 779                 &AROS_SLIB_ENTRY(dev_GetRdskLba,Storage,7),
 780                 &AROS_SLIB_ENTRY(dev_GetBlkSize,Storage,8),
 781                 (void *)-1
 782 };
 783
 784 static int StorageDev_Init(LIBBASETYPEPTR LIBBASE)
 785 {
 786         D(bug("[MSS] USB Mass Storage device layer init\n"));
 787
 788         mss_device_t *dev = (mss_device_t *)
 789         ((intptr_t)AllocPooled(SD(LIBBASE)->MemPool,
 790                         sizeof(mss_device_t) + sizeof(Storage_dev_FuncTable)) + sizeof(Storage_dev_FuncTable));
 791
 792         dev->mss_static = SD(LIBBASE);
 793
 794         dev->mss_device.dd_Library.lib_Node.ln_Name = "usbmss.device";
 795         dev->mss_device.dd_Library.lib_Node.ln_Type = NT_DEVICE;
 796         dev->mss_device.dd_Library.lib_Node.ln_Pri = 0;
 797         dev->mss_device.dd_Library.lib_Version = 1;
 798         dev->mss_device.dd_Library.lib_Revision = 0;
 799
 800         MakeFunctions(dev, Storage_dev_FuncTable, NULL);
 801         AddDevice((struct Device *)dev);
 802
 803         return TRUE;
 804 }
 805
 806 static int StorageDev_Expunge(LIBBASETYPEPTR LIBBASE)
 807 {
 808         return FALSE;
 809 }
 810
 811 ADD2INITLIB(StorageDev_Init, 1)
 812 ADD2EXPUNGELIB(StorageDev_Expunge, 1)