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