src/firmware/disk.c

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