src/firmware/disk.c

   1 //    Copyright (C) 2013 Michael McMaster <michael@codesrc.com>\r
   2 //    Copyright (C) 2014 Doug Brown <doug@downtowndougbrown.com>\r
   3 //\r
   4 //    This file is part of SCSI2SD.\r
   5 //\r
   6 //    SCSI2SD is free software: you can redistribute it and/or modify\r
   7 //    it under the terms of the GNU General Public License as published by\r
   8 //    the Free Software Foundation, either version 3 of the License, or\r
   9 //    (at your option) any later version.\r
  10 //\r
  11 //    SCSI2SD is distributed in the hope that it will be useful,\r
  12 //    but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  13 //    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  14 //    GNU General Public License for more details.\r
  15 //\r
  16 //    You should have received a copy of the GNU General Public License\r
  17 //    along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.\r
  18 \r
  19 #ifdef STM32F2xx\r
  20 #include "stm32f2xx.h"\r
  21 #endif\r
  22 #ifdef STM32F4xx\r
  23 #include "stm32f4xx.h"\r
  24 #endif\r
  25 \r
  26 #include <assert.h>\r
  27 \r
  28 // For SD write direct routines\r
  29 #include "sdio.h"\r
  30 #include "bsp_driver_sd.h"\r
  31 \r
  32 \r
  33 #include "scsi.h"\r
  34 #include "scsiPhy.h"\r
  35 #include "config.h"\r
  36 #include "disk.h"\r
  37 #include "sd.h"\r
  38 #include "time.h"\r
  39 #include "bsp.h"\r
  40 \r
  41 #include <string.h>\r
  42 \r
  43 // Global\r
  44 BlockDevice blockDev;\r
  45 Transfer transfer;\r
  46 \r
  47 static int doSdInit()\r
  48 {\r
  49     int result = 0;\r
  50     if (blockDev.state & DISK_PRESENT)\r
  51     {\r
  52         blockDev.state = blockDev.state | DISK_INITIALISED;\r
  53     }\r
  54     return result;\r
  55 }\r
  56 \r
  57 // Callback once all data has been read in the data out phase.\r
  58 static void doFormatUnitComplete(void)\r
  59 {\r
  60     // TODO start writing the initialisation pattern to the SD\r
  61     // card\r
  62     scsiDev.phase = STATUS;\r
  63 }\r
  64 \r
  65 static void doFormatUnitSkipData(int bytes)\r
  66 {\r
  67     // We may not have enough memory to store the initialisation pattern and\r
  68     // defect list data.  Since we're not making use of it yet anyway, just\r
  69     // discard the bytes.\r
  70     scsiEnterPhase(DATA_OUT);\r
  71     int i;\r
  72     for (i = 0; i < bytes; ++i)\r
  73     {\r
  74         scsiReadByte();\r
  75     }\r
  76 }\r
  77 \r
  78 // Callback from the data out phase.\r
  79 static void doFormatUnitPatternHeader(void)\r
  80 {\r
  81     int defectLength =\r
  82         ((((uint16_t)scsiDev.data[2])) << 8) +\r
  83             scsiDev.data[3];\r
  84 \r
  85     int patternLength =\r
  86         ((((uint16_t)scsiDev.data[4 + 2])) << 8) +\r
  87         scsiDev.data[4 + 3];\r
  88 \r
  89         doFormatUnitSkipData(defectLength + patternLength);\r
  90         doFormatUnitComplete();\r
  91 }\r
  92 \r
  93 // Callback from the data out phase.\r
  94 static void doFormatUnitHeader(void)\r
  95 {\r
  96     int IP = (scsiDev.data[1] & 0x08) ? 1 : 0;\r
  97     int DSP = (scsiDev.data[1] & 0x04) ? 1 : 0;\r
  98 \r
  99     if (! DSP) // disable save parameters\r
 100     {\r
 101         // Save the "MODE SELECT savable parameters"\r
 102         s2s_configSave(\r
 103             scsiDev.target->targetId,\r
 104             scsiDev.target->liveCfg.bytesPerSector);\r
 105     }\r
 106 \r
 107     if (IP)\r
 108     {\r
 109         // We need to read the initialisation pattern header first.\r
 110         scsiDev.dataLen += 4;\r
 111         scsiDev.phase = DATA_OUT;\r
 112         scsiDev.postDataOutHook = doFormatUnitPatternHeader;\r
 113     }\r
 114     else\r
 115     {\r
 116         // Read the defect list data\r
 117         int defectLength =\r
 118             ((((uint16_t)scsiDev.data[2])) << 8) +\r
 119             scsiDev.data[3];\r
 120         doFormatUnitSkipData(defectLength);\r
 121         doFormatUnitComplete();\r
 122     }\r
 123 }\r
 124 \r
 125 static void doReadCapacity()\r
 126 {\r
 127     uint32_t lba = (((uint32_t) scsiDev.cdb[2]) << 24) +\r
 128         (((uint32_t) scsiDev.cdb[3]) << 16) +\r
 129         (((uint32_t) scsiDev.cdb[4]) << 8) +\r
 130         scsiDev.cdb[5];\r
 131     int pmi = scsiDev.cdb[8] & 1;\r
 132 \r
 133     uint32_t capacity = getScsiCapacity(\r
 134         scsiDev.target->cfg->sdSectorStart,\r
 135         scsiDev.target->liveCfg.bytesPerSector,\r
 136         scsiDev.target->cfg->scsiSectors);\r
 137 \r
 138     if (!pmi && lba)\r
 139     {\r
 140         // error.\r
 141         // We don't do anything with the "partial medium indicator", and\r
 142         // assume that delays are constant across each block. But the spec\r
 143         // says we must return this error if pmi is specified incorrectly.\r
 144         scsiDev.status = CHECK_CONDITION;\r
 145         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 146         scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
 147         scsiDev.phase = STATUS;\r
 148     }\r
 149     else if (capacity > 0)\r
 150     {\r
 151         uint32_t highestBlock = capacity - 1;\r
 152 \r
 153         scsiDev.data[0] = highestBlock >> 24;\r
 154         scsiDev.data[1] = highestBlock >> 16;\r
 155         scsiDev.data[2] = highestBlock >> 8;\r
 156         scsiDev.data[3] = highestBlock;\r
 157 \r
 158         uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
 159         scsiDev.data[4] = bytesPerSector >> 24;\r
 160         scsiDev.data[5] = bytesPerSector >> 16;\r
 161         scsiDev.data[6] = bytesPerSector >> 8;\r
 162         scsiDev.data[7] = bytesPerSector;\r
 163         scsiDev.dataLen = 8;\r
 164         scsiDev.phase = DATA_IN;\r
 165     }\r
 166     else\r
 167     {\r
 168         scsiDev.status = CHECK_CONDITION;\r
 169         scsiDev.target->sense.code = NOT_READY;\r
 170         scsiDev.target->sense.asc = MEDIUM_NOT_PRESENT;\r
 171         scsiDev.phase = STATUS;\r
 172     }\r
 173 }\r
 174 \r
 175 static void doWrite(uint32_t lba, uint32_t blocks)\r
 176 {\r
 177     if (unlikely(scsiDev.target->cfg->deviceType == S2S_CFG_FLOPPY_14MB)) {\r
 178         // Floppies are supposed to be slow. Some systems can't handle a floppy\r
 179         // without an access time\r
 180         s2s_delay_ms(10);\r
 181     }\r
 182 \r
 183     uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
 184 \r
 185     if (unlikely(blockDev.state & DISK_WP) ||\r
 186         unlikely(scsiDev.target->cfg->deviceType == S2S_CFG_OPTICAL))\r
 187 \r
 188     {\r
 189         scsiDev.status = CHECK_CONDITION;\r
 190         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 191         scsiDev.target->sense.asc = WRITE_PROTECTED;\r
 192         scsiDev.phase = STATUS;\r
 193     }\r
 194     else if (unlikely(((uint64_t) lba) + blocks >\r
 195         getScsiCapacity(\r
 196             scsiDev.target->cfg->sdSectorStart,\r
 197             bytesPerSector,\r
 198             scsiDev.target->cfg->scsiSectors\r
 199             )\r
 200         ))\r
 201     {\r
 202         scsiDev.status = CHECK_CONDITION;\r
 203         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 204         scsiDev.target->sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;\r
 205         scsiDev.phase = STATUS;\r
 206     }\r
 207     else\r
 208     {\r
 209         transfer.lba = lba;\r
 210         transfer.blocks = blocks;\r
 211         transfer.currentBlock = 0;\r
 212         scsiDev.phase = DATA_OUT;\r
 213         scsiDev.dataLen = bytesPerSector;\r
 214         scsiDev.dataPtr = bytesPerSector;\r
 215 \r
 216         // No need for single-block writes atm.  Overhead of the\r
 217         // multi-block write is minimal.\r
 218         transfer.multiBlock = 1;\r
 219     }\r
 220 }\r
 221 \r
 222 \r
 223 static void doRead(uint32_t lba, uint32_t blocks)\r
 224 {\r
 225     if (unlikely(scsiDev.target->cfg->deviceType == S2S_CFG_FLOPPY_14MB)) {\r
 226         // Floppies are supposed to be slow. Some systems can't handle a floppy\r
 227         // without an access time\r
 228         s2s_delay_ms(10);\r
 229     }\r
 230 \r
 231     uint32_t capacity = getScsiCapacity(\r
 232         scsiDev.target->cfg->sdSectorStart,\r
 233         scsiDev.target->liveCfg.bytesPerSector,\r
 234         scsiDev.target->cfg->scsiSectors);\r
 235     if (unlikely(((uint64_t) lba) + blocks > capacity))\r
 236     {\r
 237         scsiDev.status = CHECK_CONDITION;\r
 238         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 239         scsiDev.target->sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;\r
 240         scsiDev.phase = STATUS;\r
 241     }\r
 242     else\r
 243     {\r
 244         transfer.lba = lba;\r
 245         transfer.blocks = blocks;\r
 246         transfer.currentBlock = 0;\r
 247         scsiDev.phase = DATA_IN;\r
 248         scsiDev.dataLen = 0; // No data yet\r
 249 \r
 250         uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
 251         uint32_t sdSectorPerSCSISector = SDSectorsPerSCSISector(bytesPerSector);\r
 252         uint32_t sdSectors =\r
 253             blocks * sdSectorPerSCSISector;\r
 254 \r
 255         if ((\r
 256                 (sdSectors == 1) &&\r
 257                 !(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_CACHE)\r
 258             ) ||\r
 259             unlikely(((uint64_t) lba) + blocks == capacity)\r
 260             )\r
 261         {\r
 262             // We get errors on reading the last sector using a multi-sector\r
 263             // read :-(\r
 264             transfer.multiBlock = 0;\r
 265         }\r
 266         else\r
 267         {\r
 268             transfer.multiBlock = 1;\r
 269 \r
 270             // uint32_t sdLBA =\r
 271                 // SCSISector2SD(\r
 272                     // scsiDev.target->cfg->sdSectorStart,\r
 273                     // bytesPerSector,\r
 274                     // lba);\r
 275 \r
 276             // TODO sdReadMultiSectorPrep(sdLBA, sdSectors);\r
 277         }\r
 278     }\r
 279 }\r
 280 \r
 281 static void doSeek(uint32_t lba)\r
 282 {\r
 283     if (lba >=\r
 284         getScsiCapacity(\r
 285             scsiDev.target->cfg->sdSectorStart,\r
 286             scsiDev.target->liveCfg.bytesPerSector,\r
 287             scsiDev.target->cfg->scsiSectors)\r
 288         )\r
 289     {\r
 290         scsiDev.status = CHECK_CONDITION;\r
 291         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 292         scsiDev.target->sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;\r
 293         scsiDev.phase = STATUS;\r
 294     }\r
 295     else\r
 296     {\r
 297         if (unlikely(scsiDev.target->cfg->deviceType == S2S_CFG_FLOPPY_14MB) ||\r
 298             scsiDev.compatMode < COMPAT_SCSI2)\r
 299         {\r
 300             s2s_delay_ms(10);\r
 301         }\r
 302         else\r
 303         {\r
 304             s2s_delay_us(10);\r
 305         }\r
 306     }\r
 307 }\r
 308 \r
 309 static int doTestUnitReady()\r
 310 {\r
 311     int ready = 1;\r
 312     if (likely(blockDev.state == (DISK_PRESENT | DISK_INITIALISED) &&\r
 313                 scsiDev.target->started))\r
 314     {\r
 315         // nothing to do.\r
 316     }\r
 317     else if (unlikely(!scsiDev.target->started))\r
 318     {\r
 319         ready = 0;\r
 320         scsiDev.status = CHECK_CONDITION;\r
 321         scsiDev.target->sense.code = NOT_READY;\r
 322         scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_READY_INITIALIZING_COMMAND_REQUIRED;\r
 323         scsiDev.phase = STATUS;\r
 324     }\r
 325     else if (unlikely(!(blockDev.state & DISK_PRESENT)))\r
 326     {\r
 327         ready = 0;\r
 328         scsiDev.status = CHECK_CONDITION;\r
 329         scsiDev.target->sense.code = NOT_READY;\r
 330         scsiDev.target->sense.asc = MEDIUM_NOT_PRESENT;\r
 331         scsiDev.phase = STATUS;\r
 332     }\r
 333     else if (unlikely(!(blockDev.state & DISK_INITIALISED)))\r
 334     {\r
 335         ready = 0;\r
 336         scsiDev.status = CHECK_CONDITION;\r
 337         scsiDev.target->sense.code = NOT_READY;\r
 338         scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_READY_CAUSE_NOT_REPORTABLE;\r
 339         scsiDev.phase = STATUS;\r
 340     }\r
 341     return ready;\r
 342 }\r
 343 \r
 344 // Handle direct-access scsi device commands\r
 345 int scsiDiskCommand()\r
 346 {\r
 347     int commandHandled = 1;\r
 348 \r
 349     uint8_t command = scsiDev.cdb[0];\r
 350     if (unlikely(command == 0x1B))\r
 351     {\r
 352         // START STOP UNIT\r
 353         // Enable or disable media access operations.\r
 354         //int immed = scsiDev.cdb[1] & 1;\r
 355         int start = scsiDev.cdb[4] & 1;\r
 356         int loadEject = scsiDev.cdb[4] & 2;\r
 357         \r
 358         if (loadEject)\r
 359         {\r
 360             // Ignore load/eject requests. We can't do that.\r
 361         }\r
 362         else if (start)\r
 363         {\r
 364             scsiDev.target->started = 1;\r
 365             if (!(blockDev.state & DISK_INITIALISED))\r
 366             {\r
 367                 doSdInit();\r
 368             }\r
 369         }\r
 370         else\r
 371         {\r
 372             scsiDev.target->started = 0;\r
 373         }\r
 374     }\r
 375     else if (unlikely(command == 0x00))\r
 376     {\r
 377         // TEST UNIT READY\r
 378         doTestUnitReady();\r
 379     }\r
 380     else if (unlikely(!doTestUnitReady()))\r
 381     {\r
 382         // Status and sense codes already set by doTestUnitReady\r
 383     }\r
 384     else if (likely(command == 0x08))\r
 385     {\r
 386         // READ(6)\r
 387         uint32_t lba =\r
 388             (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
 389             (((uint32_t) scsiDev.cdb[2]) << 8) +\r
 390             scsiDev.cdb[3];\r
 391         uint32_t blocks = scsiDev.cdb[4];\r
 392         if (unlikely(blocks == 0)) blocks = 256;\r
 393         doRead(lba, blocks);\r
 394     }\r
 395     else if (likely(command == 0x28))\r
 396     {\r
 397         // READ(10)\r
 398         // Ignore all cache control bits - we don't support a memory cache.\r
 399 \r
 400         uint32_t lba =\r
 401             (((uint32_t) scsiDev.cdb[2]) << 24) +\r
 402             (((uint32_t) scsiDev.cdb[3]) << 16) +\r
 403             (((uint32_t) scsiDev.cdb[4]) << 8) +\r
 404             scsiDev.cdb[5];\r
 405         uint32_t blocks =\r
 406             (((uint32_t) scsiDev.cdb[7]) << 8) +\r
 407             scsiDev.cdb[8];\r
 408 \r
 409         doRead(lba, blocks);\r
 410     }\r
 411     else if (likely(command == 0x0A))\r
 412     {\r
 413         // WRITE(6)\r
 414         uint32_t lba =\r
 415             (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
 416             (((uint32_t) scsiDev.cdb[2]) << 8) +\r
 417             scsiDev.cdb[3];\r
 418         uint32_t blocks = scsiDev.cdb[4];\r
 419         if (unlikely(blocks == 0)) blocks = 256;\r
 420         doWrite(lba, blocks);\r
 421     }\r
 422     else if (likely(command == 0x2A) || // WRITE(10)\r
 423         unlikely(command == 0x2E)) // WRITE AND VERIFY\r
 424     {\r
 425         // Ignore all cache control bits - we don't support a memory cache.\r
 426         // Don't bother verifying either. The SD card likely stores ECC\r
 427         // along with each flash row.\r
 428 \r
 429         uint32_t lba =\r
 430             (((uint32_t) scsiDev.cdb[2]) << 24) +\r
 431             (((uint32_t) scsiDev.cdb[3]) << 16) +\r
 432             (((uint32_t) scsiDev.cdb[4]) << 8) +\r
 433             scsiDev.cdb[5];\r
 434         uint32_t blocks =\r
 435             (((uint32_t) scsiDev.cdb[7]) << 8) +\r
 436             scsiDev.cdb[8];\r
 437 \r
 438         doWrite(lba, blocks);\r
 439     }\r
 440     else if (unlikely(command == 0x04))\r
 441     {\r
 442         // FORMAT UNIT\r
 443         // We don't really do any formatting, but we need to read the correct\r
 444         // number of bytes in the DATA_OUT phase to make the SCSI host happy.\r
 445 \r
 446         int fmtData = (scsiDev.cdb[1] & 0x10) ? 1 : 0;\r
 447         if (fmtData)\r
 448         {\r
 449             // We need to read the parameter list, but we don't know how\r
 450             // big it is yet. Start with the header.\r
 451             scsiDev.dataLen = 4;\r
 452             scsiDev.phase = DATA_OUT;\r
 453             scsiDev.postDataOutHook = doFormatUnitHeader;\r
 454         }\r
 455         else\r
 456         {\r
 457             // No data to read, we're already finished!\r
 458         }\r
 459     }\r
 460     else if (unlikely(command == 0x25))\r
 461     {\r
 462         // READ CAPACITY\r
 463         doReadCapacity();\r
 464     }\r
 465     else if (unlikely(command == 0x0B))\r
 466     {\r
 467         // SEEK(6)\r
 468         uint32_t lba =\r
 469             (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
 470             (((uint32_t) scsiDev.cdb[2]) << 8) +\r
 471             scsiDev.cdb[3];\r
 472 \r
 473         doSeek(lba);\r
 474     }\r
 475 \r
 476     else if (unlikely(command == 0x2B))\r
 477     {\r
 478         // SEEK(10)\r
 479         uint32_t lba =\r
 480             (((uint32_t) scsiDev.cdb[2]) << 24) +\r
 481             (((uint32_t) scsiDev.cdb[3]) << 16) +\r
 482             (((uint32_t) scsiDev.cdb[4]) << 8) +\r
 483             scsiDev.cdb[5];\r
 484 \r
 485         doSeek(lba);\r
 486     }\r
 487     else if (unlikely(command == 0x36))\r
 488     {\r
 489         // LOCK UNLOCK CACHE\r
 490         // We don't have a cache to lock data into. do nothing.\r
 491     }\r
 492     else if (unlikely(command == 0x34))\r
 493     {\r
 494         // PRE-FETCH.\r
 495         // We don't have a cache to pre-fetch into. do nothing.\r
 496     }\r
 497     else if (unlikely(command == 0x1E))\r
 498     {\r
 499         // PREVENT ALLOW MEDIUM REMOVAL\r
 500         // Not much we can do to prevent the user removing the SD card.\r
 501         // do nothing.\r
 502     }\r
 503     else if (unlikely(command == 0x01))\r
 504     {\r
 505         // REZERO UNIT\r
 506         // Set the lun to a vendor-specific state. Ignore.\r
 507     }\r
 508     else if (unlikely(command == 0x35))\r
 509     {\r
 510         // SYNCHRONIZE CACHE\r
 511         // We don't have a cache. do nothing.\r
 512     }\r
 513     else if (unlikely(command == 0x2F))\r
 514     {\r
 515         // VERIFY\r
 516         // TODO: When they supply data to verify, we should read the data and\r
 517         // verify it. If they don't supply any data, just say success.\r
 518         if ((scsiDev.cdb[1] & 0x02) == 0)\r
 519         {\r
 520             // They are asking us to do a medium verification with no data\r
 521             // comparison. Assume success, do nothing.\r
 522         }\r
 523         else\r
 524         {\r
 525             // TODO. This means they are supplying data to verify against.\r
 526             // Technically we should probably grab the data and compare it.\r
 527             scsiDev.status = CHECK_CONDITION;\r
 528             scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 529             scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
 530             scsiDev.phase = STATUS;\r
 531         }\r
 532     }\r
 533     else if (unlikely(command == 0x37))\r
 534     {\r
 535         // READ DEFECT DATA\r
 536         uint32_t allocLength = (((uint16_t)scsiDev.cdb[7]) << 8) |\r
 537             scsiDev.cdb[8];\r
 538 \r
 539         scsiDev.data[0] = 0;\r
 540         scsiDev.data[1] = scsiDev.cdb[1];\r
 541         scsiDev.data[2] = 0;\r
 542         scsiDev.data[3] = 0;\r
 543         scsiDev.dataLen = 4;\r
 544 \r
 545         if (scsiDev.dataLen > allocLength)\r
 546         {\r
 547             scsiDev.dataLen = allocLength;\r
 548         }\r
 549 \r
 550         scsiDev.phase = DATA_IN;\r
 551     }\r
 552     else\r
 553     {\r
 554         commandHandled = 0;\r
 555     }\r
 556 \r
 557     return commandHandled;\r
 558 }\r
 559 \r
 560 static void diskDataInBuffered(int totalSDSectors, uint32_t sdLBA, int useSlowDataCount, uint32_t* phaseChangeDelayNs)\r
 561 {\r
 562     uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
 563 \r
 564     const int sdPerScsi = SDSectorsPerSCSISector(bytesPerSector);\r
 565     const int buffers = sizeof(scsiDev.data) / SD_SECTOR_SIZE;\r
 566     int prep = 0;\r
 567     int i = 0;\r
 568     int scsiActive __attribute__((unused)) = 0; // unused if DMA disabled\r
 569     int sdActive = 0;\r
 570 \r
 571     int gotHalf = 0;\r
 572     int sentHalf = 0;\r
 573 \r
 574     while ((i < totalSDSectors) &&\r
 575         likely(scsiDev.phase == DATA_IN) &&\r
 576         likely(!scsiDev.resetFlag))\r
 577     {\r
 578         int completedDmaSectors;\r
 579         if (sdActive && (completedDmaSectors = sdReadDMAPoll(sdActive)))\r
 580         {\r
 581             prep += completedDmaSectors;\r
 582             sdActive -= completedDmaSectors;\r
 583             gotHalf = 0;\r
 584         }\r
 585         else if (sdActive > 1)\r
 586         {\r
 587             if ((scsiDev.data[SD_SECTOR_SIZE * (prep % buffers) + 510] != 0xAA) ||\r
 588                 (scsiDev.data[SD_SECTOR_SIZE * (prep % buffers) + 511] != 0x33))\r
 589             {\r
 590                 prep += 1;\r
 591                 sdActive -= 1;\r
 592                 gotHalf = 0;\r
 593             }\r
 594             else if (scsiDev.data[SD_SECTOR_SIZE * (prep % buffers) + 127] != 0xAA)\r
 595             {\r
 596                 // Half-block\r
 597                 gotHalf = 1;\r
 598             }\r
 599         }\r
 600 \r
 601         if (!sdActive &&\r
 602             (prep - i < buffers) &&\r
 603             (prep < totalSDSectors) &&\r
 604             ((totalSDSectors - prep) >= sdPerScsi) &&\r
 605             (likely(!useSlowDataCount) || scsiPhyComplete()) &&\r
 606             (HAL_SD_GetState(&hsd) != HAL_SD_STATE_BUSY)) // rx complete but IRQ not fired yet.\r
 607         {\r
 608             // Start an SD transfer if we have space.\r
 609             uint32_t startBuffer = prep % buffers;\r
 610             uint32_t sectors = totalSDSectors - prep;\r
 611             uint32_t freeBuffers = buffers - (prep - i);\r
 612 \r
 613             uint32_t contiguousBuffers = buffers - startBuffer;\r
 614             freeBuffers = freeBuffers < contiguousBuffers\r
 615                 ? freeBuffers : contiguousBuffers;\r
 616             sectors = sectors < freeBuffers ? sectors : freeBuffers;\r
 617 \r
 618             if (sectors > 128) sectors = 128; // 65536 DMA limit !!\r
 619 \r
 620             // Round-down when we have odd sector sizes.\r
 621             if (sdPerScsi != 1)\r
 622             {\r
 623                 sectors = (sectors / sdPerScsi) * sdPerScsi;\r
 624             }\r
 625 \r
 626             for (int dodgy = 0; dodgy < sectors; dodgy++)\r
 627             {\r
 628                 scsiDev.data[SD_SECTOR_SIZE * (startBuffer + dodgy) + 127] = 0xAA;\r
 629 \r
 630                 scsiDev.data[SD_SECTOR_SIZE * (startBuffer + dodgy) + 510] = 0xAA;\r
 631                 scsiDev.data[SD_SECTOR_SIZE * (startBuffer + dodgy) + 511] = 0x33;\r
 632             }\r
 633 \r
 634             sdReadDMA(sdLBA + prep, sectors, &scsiDev.data[SD_SECTOR_SIZE * startBuffer]);\r
 635 \r
 636             sdActive = sectors;\r
 637 \r
 638             if (useSlowDataCount)\r
 639             {\r
 640                 scsiSetDataCount((sectors / sdPerScsi) * bytesPerSector);\r
 641             }\r
 642 \r
 643             // Wait now that the SD card is busy\r
 644             // Chances are we've probably already waited sufficient time,\r
 645             // but it's hard to measure microseconds cheaply. So just wait\r
 646             // extra just-in-case. Hopefully it's in parallel with dma.\r
 647             if (*phaseChangeDelayNs > 0)\r
 648             {\r
 649                 s2s_delay_ns(*phaseChangeDelayNs);\r
 650                 *phaseChangeDelayNs = 0;\r
 651             }\r
 652         }\r
 653 \r
 654         int fifoReady = scsiFifoReady();\r
 655         if (((prep - i) > 0) && fifoReady)\r
 656         {\r
 657             int dmaBytes = SD_SECTOR_SIZE;\r
 658             if ((i % sdPerScsi) == (sdPerScsi - 1))\r
 659             {\r
 660                 dmaBytes = bytesPerSector % SD_SECTOR_SIZE;\r
 661                 if (dmaBytes == 0) dmaBytes = SD_SECTOR_SIZE;\r
 662             }\r
 663 \r
 664             uint8_t* scsiDmaData = &(scsiDev.data[SD_SECTOR_SIZE * (i % buffers)]);\r
 665 \r
 666             if (sentHalf)\r
 667             {\r
 668                 scsiDmaData += SD_SECTOR_SIZE / 2;\r
 669                 dmaBytes -= (SD_SECTOR_SIZE / 2);\r
 670             }\r
 671             scsiWritePIO(scsiDmaData, dmaBytes);\r
 672 \r
 673             ++i;\r
 674             sentHalf = 0;\r
 675             gotHalf = 0;\r
 676         }\r
 677         else if (gotHalf && !sentHalf && fifoReady && bytesPerSector == SD_SECTOR_SIZE)\r
 678         {\r
 679             uint8_t* scsiDmaData = &(scsiDev.data[SD_SECTOR_SIZE * (i % buffers)]);\r
 680             scsiWritePIO(scsiDmaData, SD_SECTOR_SIZE / 2);\r
 681             sentHalf = 1;\r
 682         }\r
 683     }\r
 684 }\r
 685 \r
 686 // Transfer from the SD card straight to the SCSI Fifo without storing in memory first for lower latency\r
 687 // This requires hardware flow control on the SD device (broken on stm32f205)\r
 688 // Only functional for 512 byte sectors.\r
 689 static void diskDataInDirect(uint32_t totalSDSectors, uint32_t sdLBA, int useSlowDataCount, uint32_t* phaseChangeDelayNs)\r
 690 {\r
 691     sdReadCmd(sdLBA, totalSDSectors);\r
 692 \r
 693     // Wait while the SD card starts buffering data\r
 694     if (*phaseChangeDelayNs > 0)\r
 695     {\r
 696         s2s_delay_ns(*phaseChangeDelayNs);\r
 697         *phaseChangeDelayNs = 0;\r
 698     }\r
 699 \r
 700     for (int i = 0; i < totalSDSectors && !scsiDev.resetFlag; ++i)\r
 701     {\r
 702         if (i % 128 == 0)\r
 703         {\r
 704             // SD DPSM has 24 bit limit. Re-use 128 (DMA limit)\r
 705             uint32_t chunk = totalSDSectors - i > 128 ? 128 : totalSDSectors - i;\r
 706             sdReadPIOData(chunk);\r
 707 \r
 708             if (useSlowDataCount)\r
 709             {\r
 710                 while (!scsiDev.resetFlag && !scsiPhyComplete())\r
 711                 {}\r
 712                 scsiSetDataCount(chunk * SD_SECTOR_SIZE); // SCSI_XFER_MAX > 65536\r
 713             }\r
 714         }\r
 715 \r
 716         // The SCSI fifo is a full sector so we only need to check once.\r
 717         while (!scsiFifoReady() && !scsiDev.resetFlag)\r
 718         {}\r
 719 \r
 720         int byteCount = 0;\r
 721         while(byteCount < SD_SECTOR_SIZE &&\r
 722             likely(!scsiDev.resetFlag) &&\r
 723             likely(scsiDev.phase == DATA_IN) &&\r
 724             !__HAL_SD_GET_FLAG(&hsd, SDIO_FLAG_RXOVERR | SDIO_FLAG_DCRCFAIL | SDIO_FLAG_DTIMEOUT))\r
 725         {\r
 726             if(__HAL_SD_GET_FLAG(&hsd, SDIO_FLAG_RXFIFOHF))\r
 727             {\r
 728                 // The SDIO fifo is 32 x 32bits. As we're using the "half full" flag we must\r
 729                 // always read half the FIFO.\r
 730 \r
 731                 for (int j = 0; j < 4; ++j)\r
 732                 {\r
 733                     uint32_t data[4];\r
 734                     data[0] = SDIO_ReadFIFO(hsd.Instance);\r
 735                     data[1] = SDIO_ReadFIFO(hsd.Instance);\r
 736                     data[2] = SDIO_ReadFIFO(hsd.Instance);\r
 737                     data[3] = SDIO_ReadFIFO(hsd.Instance);\r
 738 \r
 739                     *((volatile uint32_t*)SCSI_FIFO_DATA) = data[0];\r
 740                     *((volatile uint32_t*)SCSI_FIFO_DATA) = data[1];\r
 741                     *((volatile uint32_t*)SCSI_FIFO_DATA) = data[2];\r
 742                     *((volatile uint32_t*)SCSI_FIFO_DATA) = data[3];\r
 743                 }\r
 744 \r
 745                 byteCount += 64;\r
 746             }\r
 747         }\r
 748 \r
 749         int error = 0;\r
 750         if (__HAL_SD_GET_FLAG(&hsd, SDIO_FLAG_DTIMEOUT))\r
 751         {\r
 752             __HAL_SD_CLEAR_FLAG(&hsd, SDIO_FLAG_DTIMEOUT);\r
 753             error = 1;\r
 754         }\r
 755         else if (__HAL_SD_GET_FLAG(&hsd, SDIO_FLAG_DCRCFAIL))\r
 756         {\r
 757             __HAL_SD_CLEAR_FLAG(&hsd, SDIO_FLAG_DCRCFAIL);\r
 758             error = 1;\r
 759         }\r
 760         else if (__HAL_SD_GET_FLAG(&hsd, SDIO_FLAG_RXOVERR))\r
 761         {\r
 762             __HAL_SD_CLEAR_FLAG(&hsd, SDIO_FLAG_RXOVERR);\r
 763             error = 1;\r
 764         }\r
 765 \r
 766         if (error && scsiDev.phase == DATA_IN)\r
 767         {\r
 768             __HAL_SD_CLEAR_FLAG(&hsd, SDIO_STATIC_FLAGS);\r
 769 \r
 770             scsiDiskReset();\r
 771 \r
 772             scsiDev.status = CHECK_CONDITION;\r
 773             scsiDev.target->sense.code = HARDWARE_ERROR;\r
 774             scsiDev.target->sense.asc = LOGICAL_UNIT_COMMUNICATION_FAILURE;\r
 775             scsiDev.phase = STATUS;\r
 776         }\r
 777 \r
 778         // We need the SCSI FIFO count to complete even after the SD read has failed\r
 779         while (byteCount < SD_SECTOR_SIZE &&\r
 780             likely(!scsiDev.resetFlag))\r
 781         {\r
 782             scsiPhyTx32(0, 0);\r
 783             byteCount += 4;\r
 784         }\r
 785     }\r
 786 \r
 787     /* Send stop transmission command in case of multiblock read */\r
 788     if(totalSDSectors > 1U)\r
 789     {\r
 790         SDMMC_CmdStopTransfer(hsd.Instance);\r
 791     }\r
 792 \r
 793     // Read remaining data\r
 794     uint32_t extraCount = SD_DATATIMEOUT;\r
 795     while ((__HAL_SD_GET_FLAG(&hsd, SDIO_FLAG_RXDAVL)) && (extraCount > 0))\r
 796     {\r
 797         SDIO_ReadFIFO(hsd.Instance);\r
 798         extraCount--;\r
 799     }\r
 800 \r
 801     __HAL_SD_CLEAR_FLAG(&hsd, SDIO_STATIC_DATA_FLAGS);\r
 802     hsd.State = HAL_SD_STATE_READY;\r
 803     \r
 804     sdCompleteTransfer(); // Probably overkill\r
 805 }\r
 806 \r
 807 static void diskDataIn()\r
 808 {\r
 809     uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
 810 \r
 811     // Take responsibility for waiting for the phase delays\r
 812     uint32_t phaseChangeDelayNs = scsiEnterPhaseImmediate(DATA_IN);\r
 813 \r
 814     int totalSDSectors =\r
 815         transfer.blocks * SDSectorsPerSCSISector(bytesPerSector);\r
 816     uint32_t sdLBA =\r
 817         SCSISector2SD(\r
 818             scsiDev.target->cfg->sdSectorStart,\r
 819             bytesPerSector,\r
 820             transfer.lba);\r
 821 \r
 822     // It's highly unlikely that someone is going to use huge transfers\r
 823     // per scsi command, but if they do it'll be slower than usual.\r
 824     uint32_t totalScsiBytes = transfer.blocks * bytesPerSector;\r
 825     int useSlowDataCount = totalScsiBytes >= SCSI_XFER_MAX;\r
 826     if (!useSlowDataCount)\r
 827     {\r
 828         scsiSetDataCount(totalScsiBytes);\r
 829     }\r
 830 \r
 831 #ifdef STM32F4xx\r
 832     // Direct mode requires hardware flow control to be working on the SD peripheral\r
 833     // Code isn't currently working above 128 sectors. TODO investigate\r
 834     if (totalSDSectors < 128 && bytesPerSector == SD_SECTOR_SIZE)\r
 835     {\r
 836         diskDataInDirect(totalSDSectors, sdLBA, useSlowDataCount, &phaseChangeDelayNs);\r
 837     }\r
 838     else\r
 839 #endif \r
 840     {\r
 841         diskDataInBuffered(totalSDSectors, sdLBA, useSlowDataCount, &phaseChangeDelayNs);\r
 842     }\r
 843 \r
 844     if (phaseChangeDelayNs > 0 && !scsiDev.resetFlag) // zero bytes ?\r
 845     {\r
 846         s2s_delay_ns(phaseChangeDelayNs);\r
 847         phaseChangeDelayNs = 0;\r
 848     }\r
 849 \r
 850     if (scsiDev.resetFlag)\r
 851     {\r
 852         HAL_SD_Abort(&hsd);\r
 853     }\r
 854     else\r
 855     {\r
 856         // Wait for the SD transfer to complete before we disable IRQs.\r
 857         // (Otherwise some cards will cause an error if we don't sent the\r
 858         // stop transfer command via the DMA complete handler in time)\r
 859         while (HAL_SD_GetState(&hsd) == HAL_SD_STATE_BUSY)\r
 860         {\r
 861             // Wait while keeping BSY.\r
 862         }\r
 863     }\r
 864 \r
 865     HAL_SD_CardStateTypeDef cardState = HAL_SD_GetCardState(&hsd);\r
 866     while (cardState == HAL_SD_CARD_PROGRAMMING || cardState == HAL_SD_CARD_SENDING) \r
 867     {\r
 868         cardState = HAL_SD_GetCardState(&hsd);\r
 869     }\r
 870 \r
 871     // We've finished transferring the data to the FPGA, now wait until it's\r
 872     // written to he SCSI bus.\r
 873     while (!scsiPhyComplete() &&\r
 874         likely(scsiDev.phase == DATA_IN) &&\r
 875         likely(!scsiDev.resetFlag))\r
 876     {\r
 877         // spin\r
 878     }\r
 879 \r
 880     if (scsiDev.phase == DATA_IN)\r
 881     {\r
 882         scsiDev.phase = STATUS;\r
 883     }\r
 884     scsiDiskReset();\r
 885 }\r
 886 \r
 887 void diskDataOut_512(int totalSDSectors, uint32_t sdLBA, int useSlowDataCount, int* clearBSY, int* parityError)\r
 888 {\r
 889     int i = 0;\r
 890     int disconnected = 0;\r
 891 \r
 892     int enableParity = scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY;\r
 893 \r
 894     uint32_t maxSectors = sizeof(scsiDev.data) / SD_SECTOR_SIZE;\r
 895 \r
 896     int lastWriteSize = 0;\r
 897 \r
 898     while ((i < totalSDSectors) &&\r
 899         likely(scsiDev.phase == DATA_OUT) &&\r
 900         likely(!scsiDev.resetFlag))\r
 901         // KEEP GOING to ensure FIFOs are in a good state.\r
 902         // likely(!parityError || !enableParity))\r
 903     {\r
 904 \r
 905         uint32_t maxXferSectors = SCSI_XFER_MAX / SD_SECTOR_SIZE;\r
 906         uint32_t rem = totalSDSectors - i;\r
 907         uint32_t sectors = rem < maxXferSectors ? rem : maxXferSectors;\r
 908 \r
 909         uint32_t totalBytes = sectors * SD_SECTOR_SIZE;\r
 910 \r
 911         if (useSlowDataCount)\r
 912         {\r
 913             scsiSetDataCount(totalBytes);\r
 914         }\r
 915 \r
 916         lastWriteSize = sectors;\r
 917         HAL_SD_WriteBlocks_DMA(&hsd, i + sdLBA, sectors);\r
 918         int j = 0;\r
 919         int prep = 0;\r
 920         int sdActive = 0;\r
 921         uint32_t dmaFinishTime = 0;\r
 922         while (j < sectors && !scsiDev.resetFlag)\r
 923         {\r
 924             if (sdActive &&\r
 925                 HAL_SD_GetState(&hsd) != HAL_SD_STATE_BUSY &&\r
 926                 !sdIsBusy())\r
 927             {\r
 928                 j += sdActive;\r
 929                 sdActive = 0;\r
 930             }\r
 931             if (!sdActive && ((prep - j) > 0))\r
 932             {\r
 933                 // Start an SD transfer if we have space.\r
 934                 HAL_SD_WriteBlocks_Data(&hsd, &scsiDev.data[SD_SECTOR_SIZE * (j % maxSectors)]);\r
 935 \r
 936                 sdActive = 1;\r
 937             }\r
 938 \r
 939             if (((prep - j) < maxSectors) &&\r
 940                 (prep < sectors) &&\r
 941                 scsiFifoReady())\r
 942             {\r
 943                 scsiReadPIO(\r
 944                     &scsiDev.data[(prep % maxSectors) * SD_SECTOR_SIZE],\r
 945                     SD_SECTOR_SIZE,\r
 946                     parityError);\r
 947                 prep++;\r
 948                 if (prep == sectors)\r
 949                 {\r
 950                     dmaFinishTime = s2s_getTime_ms();\r
 951                 }\r
 952             }\r
 953         \r
 954             if (i + prep >= totalSDSectors &&\r
 955                 !disconnected &&\r
 956                 (!(*parityError) || !enableParity) &&\r
 957                 s2s_elapsedTime_ms(dmaFinishTime) >= 180)\r
 958             {\r
 959                 // We're transferring over the SCSI bus faster than the SD card\r
 960                 // can write.  All data is buffered, and we're just waiting for\r
 961                 // the SD card to complete. The host won't let us disconnect.\r
 962                 // Some drivers set a 250ms timeout on transfers to complete.\r
 963                 // SD card writes are supposed to complete\r
 964                 // within 200ms, but sometimes they don't.\r
 965                 // Just pretend we're finished.\r
 966                 process_Status();\r
 967                 *clearBSY = process_MessageIn(0); // Will go to BUS_FREE state but keep BSY asserted.\r
 968                 disconnected = 1;\r
 969             }\r
 970         }\r
 971 \r
 972         if (scsiDev.resetFlag)\r
 973         {\r
 974             HAL_SD_Abort(&hsd);\r
 975         }\r
 976         else\r
 977         {\r
 978             while (HAL_SD_GetState(&hsd) == HAL_SD_STATE_BUSY) {} // Waits for DMA to complete\r
 979             if (lastWriteSize > 1)\r
 980             {\r
 981                 SDMMC_CmdStopTransfer(hsd.Instance);\r
 982             }\r
 983         }\r
 984 \r
 985         while (sdIsBusy() &&\r
 986             s2s_elapsedTime_ms(dmaFinishTime) < 180)\r
 987         {\r
 988             // Wait while the SD card is writing buffer to flash\r
 989             // The card may remain in the RECEIVING state (even though it's programming) if\r
 990             // it has buffer space to receive more data available.\r
 991         }\r
 992 \r
 993         if (!disconnected && \r
 994             i + sectors >= totalSDSectors &&\r
 995             (!parityError || !enableParity))\r
 996         {\r
 997             // We're transferring over the SCSI bus faster than the SD card\r
 998             // can write.  All data is buffered, and we're just waiting for\r
 999             // the SD card to complete. The host won't let us disconnect.\r
1000             // Some drivers set a 250ms timeout on transfers to complete.\r
1001             // SD card writes are supposed to complete\r
1002             // within 200ms, but sometimes they don't.\r
1003             // Just pretend we're finished.\r
1004             process_Status();\r
1005             *clearBSY = process_MessageIn(0); // Will go to BUS_FREE state but keep BSY asserted.\r
1006         }\r
1007 \r
1008         // Wait while the SD card is writing buffer to flash\r
1009         // The card may remain in the RECEIVING state (even though it's programming) if\r
1010         // it has buffer space to receive more data available.\r
1011         while (sdIsBusy()) {}\r
1012         HAL_SD_CardStateTypeDef cardState = HAL_SD_GetCardState(&hsd);\r
1013         while (cardState == HAL_SD_CARD_PROGRAMMING || cardState == HAL_SD_CARD_RECEIVING) \r
1014         {\r
1015             // Wait while the SD card is writing buffer to flash\r
1016             // The card may remain in the RECEIVING state (even though it's programming) if\r
1017             // it has buffer space to receive more data available.\r
1018             cardState = HAL_SD_GetCardState(&hsd);\r
1019         }\r
1020 \r
1021         i += sectors;\r
1022    \r
1023     }\r
1024 }\r
1025 \r
1026 void diskDataOut_variableSectorSize(int sdPerScsi, int totalSDSectors, uint32_t sdLBA, int useSlowDataCount, int* parityError)\r
1027 {\r
1028     uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
1029 \r
1030     int i = 0;\r
1031 \r
1032     int enableParity = scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY;\r
1033 \r
1034     uint32_t maxSectors = sizeof(scsiDev.data) / SD_SECTOR_SIZE;\r
1035 \r
1036     while ((i < totalSDSectors) &&\r
1037         likely(scsiDev.phase == DATA_OUT) &&\r
1038         likely(!scsiDev.resetFlag))\r
1039         // KEEP GOING to ensure FIFOs are in a good state.\r
1040         // likely(!parityError || !enableParity))\r
1041     {\r
1042         // Well, until we have some proper non-blocking SD code, we must\r
1043         // do this in a half-duplex fashion. We need to write as much as\r
1044         // possible in each SD card transaction.\r
1045         // use sg_dd from sg_utils3 tools to test.\r
1046 \r
1047         uint32_t rem = totalSDSectors - i;\r
1048         uint32_t sectors;\r
1049         if (rem <= maxSectors)\r
1050         {\r
1051             sectors = rem;\r
1052         }\r
1053         else\r
1054         {\r
1055             sectors = maxSectors;\r
1056             while (sectors % sdPerScsi) sectors--;\r
1057         }\r
1058         \r
1059 \r
1060         if (useSlowDataCount)\r
1061         {\r
1062             scsiSetDataCount((sectors / sdPerScsi) * bytesPerSector);\r
1063         }\r
1064 \r
1065         for (int scsiSector = i; scsiSector < i + sectors; ++scsiSector)\r
1066         {\r
1067             int dmaBytes = SD_SECTOR_SIZE;\r
1068             if ((scsiSector % sdPerScsi) == (sdPerScsi - 1))\r
1069             {\r
1070                 dmaBytes = bytesPerSector % SD_SECTOR_SIZE;\r
1071                 if (dmaBytes == 0) dmaBytes = SD_SECTOR_SIZE;\r
1072             }\r
1073 \r
1074             scsiReadPIO(&scsiDev.data[SD_SECTOR_SIZE * (scsiSector - i)], dmaBytes, parityError);\r
1075         }\r
1076         if (!(*parityError) || !enableParity)\r
1077         {\r
1078             BSP_SD_WriteBlocks_DMA(&scsiDev.data[0], i + sdLBA, sectors);\r
1079         }\r
1080         i += sectors;\r
1081     }\r
1082 }\r
1083 \r
1084 void diskDataOut()\r
1085 {\r
1086     uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
1087 \r
1088     scsiEnterPhase(DATA_OUT);\r
1089 \r
1090     const int sdPerScsi = SDSectorsPerSCSISector(bytesPerSector);\r
1091     int totalSDSectors = transfer.blocks * sdPerScsi;\r
1092     uint32_t sdLBA =\r
1093         SCSISector2SD(\r
1094             scsiDev.target->cfg->sdSectorStart,\r
1095             bytesPerSector,\r
1096             transfer.lba);\r
1097     int clearBSY = 0;\r
1098 \r
1099     int parityError = 0;\r
1100 \r
1101     static_assert(SCSI_XFER_MAX >= sizeof(scsiDev.data), "Assumes SCSI_XFER_MAX >= sizeof(scsiDev.data)");\r
1102 \r
1103     // Start reading and filling fifos as soon as possible.\r
1104     // It's highly unlikely that someone is going to use huge transfers\r
1105     // per scsi command, but if they do it'll be slower than usual.\r
1106     // Note: Happens in Macintosh FWB HDD Toolkit benchmarks which default\r
1107     // to 768kb\r
1108     uint32_t totalTransferBytes = transfer.blocks * bytesPerSector;\r
1109     int useSlowDataCount = totalTransferBytes >= SCSI_XFER_MAX;\r
1110     if (!useSlowDataCount)\r
1111     {\r
1112         DWT->CYCCNT = 0; // Start counting cycles\r
1113         scsiSetDataCount(totalTransferBytes);\r
1114     }\r
1115 \r
1116     if (bytesPerSector == SD_SECTOR_SIZE)\r
1117     {\r
1118         diskDataOut_512(totalSDSectors, sdLBA, useSlowDataCount, &clearBSY, &parityError);\r
1119     }\r
1120     else\r
1121     {\r
1122         diskDataOut_variableSectorSize(sdPerScsi, totalSDSectors, sdLBA, useSlowDataCount, &parityError);\r
1123     }\r
1124     \r
1125 \r
1126     // Should already be complete here as we've ready the FIFOs\r
1127     // by now. Check anyway.\r
1128     while (!scsiPhyComplete() && likely(!scsiDev.resetFlag))\r
1129     {\r
1130         // spin\r
1131     }\r
1132 \r
1133     if (clearBSY)\r
1134     {\r
1135         enter_BusFree();\r
1136     }\r
1137 \r
1138     if (scsiDev.phase == DATA_OUT)\r
1139     {\r
1140         if (parityError &&\r
1141             (scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))\r
1142         {\r
1143             scsiDev.target->sense.code = ABORTED_COMMAND;\r
1144             scsiDev.target->sense.asc = SCSI_PARITY_ERROR;\r
1145             scsiDev.status = CHECK_CONDITION;;\r
1146         }\r
1147         scsiDev.phase = STATUS;\r
1148     }\r
1149     scsiDiskReset();\r
1150 }\r
1151 \r
1152 \r
1153 void scsiDiskPoll()\r
1154 {\r
1155     if (scsiDev.phase == DATA_IN &&\r
1156         transfer.currentBlock != transfer.blocks)\r
1157     {\r
1158         diskDataIn();\r
1159      }\r
1160     else if (scsiDev.phase == DATA_OUT &&\r
1161         transfer.currentBlock != transfer.blocks)\r
1162     {\r
1163         diskDataOut();\r
1164     }\r
1165 }\r
1166 \r
1167 \r
1168 void scsiDiskReset()\r
1169 {\r
1170     scsiDev.dataPtr = 0;\r
1171     scsiDev.savedDataPtr = 0;\r
1172     scsiDev.dataLen = 0;\r
1173     // transfer.lba = 0; // Needed in Request Sense to determine failure\r
1174     transfer.blocks = 0;\r
1175     transfer.currentBlock = 0;\r
1176 \r
1177     // Cancel long running commands!\r
1178 #if 0\r
1179     if (\r
1180         ((scsiDev.boardCfg.flags & S2S_CFG_ENABLE_CACHE) == 0) ||\r
1181             (transfer.multiBlock == 0)\r
1182         )\r
1183 #endif\r
1184     {\r
1185         sdCompleteTransfer();\r
1186     }\r
1187 \r
1188     transfer.multiBlock = 0;\r
1189 }\r
1190 \r
1191 void scsiDiskInit()\r
1192 {\r
1193     scsiDiskReset();\r
1194 }\r
1195 \r