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                 // TODO uint32_t sdLBA =\r
 222 // TODO                         SCSISector2SD(\r
 223         // TODO                         scsiDev.target->cfg->sdSectorStart,\r
 224                 // TODO                 bytesPerSector,\r
 225                         // TODO         lba);\r
 226                 // TODO uint32_t sdBlocks = blocks * SDSectorsPerSCSISector(bytesPerSector);\r
 227                 // TODO sdWriteMultiSectorPrep(sdLBA, sdBlocks);\r
 228         }\r
 229 }\r
 230 \r
 231 \r
 232 static void doRead(uint32_t lba, uint32_t blocks)\r
 233 {\r
 234         if (unlikely(scsiDev.target->cfg->deviceType == S2S_CFG_FLOPPY_14MB)) {\r
 235                 // Floppies are supposed to be slow. Some systems can't handle a floppy\r
 236                 // without an access time\r
 237                 s2s_delay_ms(10);\r
 238         }\r
 239 \r
 240         uint32_t capacity = getScsiCapacity(\r
 241                 scsiDev.target->cfg->sdSectorStart,\r
 242                 scsiDev.target->liveCfg.bytesPerSector,\r
 243                 scsiDev.target->cfg->scsiSectors);\r
 244         if (unlikely(((uint64_t) lba) + blocks > capacity))\r
 245         {\r
 246                 scsiDev.status = CHECK_CONDITION;\r
 247                 scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 248                 scsiDev.target->sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;\r
 249                 scsiDev.phase = STATUS;\r
 250         }\r
 251         else\r
 252         {\r
 253                 transfer.lba = lba;\r
 254                 transfer.blocks = blocks;\r
 255                 transfer.currentBlock = 0;\r
 256                 scsiDev.phase = DATA_IN;\r
 257                 scsiDev.dataLen = 0; // No data yet\r
 258 \r
 259                 uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
 260                 uint32_t sdSectorPerSCSISector = SDSectorsPerSCSISector(bytesPerSector);\r
 261                 uint32_t sdSectors =\r
 262                         blocks * sdSectorPerSCSISector;\r
 263 \r
 264                 if ((\r
 265                                 (sdSectors == 1) &&\r
 266                                 !(scsiDev.boardCfg.flags & S2S_CFG_ENABLE_CACHE)\r
 267                         ) ||\r
 268                         unlikely(((uint64_t) lba) + blocks == capacity)\r
 269                         )\r
 270                 {\r
 271                         // We get errors on reading the last sector using a multi-sector\r
 272                         // read :-(\r
 273                         transfer.multiBlock = 0;\r
 274                 }\r
 275                 else\r
 276                 {\r
 277                         transfer.multiBlock = 1;\r
 278 \r
 279                         // uint32_t sdLBA =\r
 280                                 // SCSISector2SD(\r
 281                                         // scsiDev.target->cfg->sdSectorStart,\r
 282                                         // bytesPerSector,\r
 283                                         // lba);\r
 284 \r
 285                         // TODO sdReadMultiSectorPrep(sdLBA, sdSectors);\r
 286                 }\r
 287         }\r
 288 }\r
 289 \r
 290 static void doSeek(uint32_t lba)\r
 291 {\r
 292         if (lba >=\r
 293                 getScsiCapacity(\r
 294                         scsiDev.target->cfg->sdSectorStart,\r
 295                         scsiDev.target->liveCfg.bytesPerSector,\r
 296                         scsiDev.target->cfg->scsiSectors)\r
 297                 )\r
 298         {\r
 299                 scsiDev.status = CHECK_CONDITION;\r
 300                 scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 301                 scsiDev.target->sense.asc = LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE;\r
 302                 scsiDev.phase = STATUS;\r
 303         }\r
 304         else\r
 305         {\r
 306                 s2s_delay_ms(10);\r
 307         }\r
 308 }\r
 309 \r
 310 static int doTestUnitReady()\r
 311 {\r
 312         int ready = 1;\r
 313         if (likely(blockDev.state == (DISK_STARTED | DISK_PRESENT | DISK_INITIALISED)))\r
 314         {\r
 315                 // nothing to do.\r
 316         }\r
 317         else if (unlikely(!(blockDev.state & DISK_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                 // Ignore load/eject requests. We can't do that.\r
 355                 //int immed = scsiDev.cdb[1] & 1;\r
 356                 int start = scsiDev.cdb[4] & 1;\r
 357 \r
 358                 if (start)\r
 359                 {\r
 360                         blockDev.state = blockDev.state | DISK_STARTED;\r
 361                         if (!(blockDev.state & DISK_INITIALISED))\r
 362                         {\r
 363                                 doSdInit();\r
 364                         }\r
 365                 }\r
 366                 else\r
 367                 {\r
 368                         blockDev.state &= ~DISK_STARTED;\r
 369                 }\r
 370         }\r
 371         else if (unlikely(command == 0x00))\r
 372         {\r
 373                 // TEST UNIT READY\r
 374                 doTestUnitReady();\r
 375         }\r
 376         else if (unlikely(!doTestUnitReady()))\r
 377         {\r
 378                 // Status and sense codes already set by doTestUnitReady\r
 379         }\r
 380         else if (likely(command == 0x08))\r
 381         {\r
 382                 // READ(6)\r
 383                 uint32_t lba =\r
 384                         (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
 385                         (((uint32_t) scsiDev.cdb[2]) << 8) +\r
 386                         scsiDev.cdb[3];\r
 387                 uint32_t blocks = scsiDev.cdb[4];\r
 388                 if (unlikely(blocks == 0)) blocks = 256;\r
 389                 doRead(lba, blocks);\r
 390         }\r
 391         else if (likely(command == 0x28))\r
 392         {\r
 393                 // READ(10)\r
 394                 // Ignore all cache control bits - we don't support a memory cache.\r
 395 \r
 396                 uint32_t lba =\r
 397                         (((uint32_t) scsiDev.cdb[2]) << 24) +\r
 398                         (((uint32_t) scsiDev.cdb[3]) << 16) +\r
 399                         (((uint32_t) scsiDev.cdb[4]) << 8) +\r
 400                         scsiDev.cdb[5];\r
 401                 uint32_t blocks =\r
 402                         (((uint32_t) scsiDev.cdb[7]) << 8) +\r
 403                         scsiDev.cdb[8];\r
 404 \r
 405                 doRead(lba, blocks);\r
 406         }\r
 407         else if (likely(command == 0x0A))\r
 408         {\r
 409                 // WRITE(6)\r
 410                 uint32_t lba =\r
 411                         (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
 412                         (((uint32_t) scsiDev.cdb[2]) << 8) +\r
 413                         scsiDev.cdb[3];\r
 414                 uint32_t blocks = scsiDev.cdb[4];\r
 415                 if (unlikely(blocks == 0)) blocks = 256;\r
 416                 doWrite(lba, blocks);\r
 417         }\r
 418         else if (likely(command == 0x2A) || // WRITE(10)\r
 419                 unlikely(command == 0x2E)) // WRITE AND VERIFY\r
 420         {\r
 421                 // Ignore all cache control bits - we don't support a memory cache.\r
 422                 // Don't bother verifying either. The SD card likely stores ECC\r
 423                 // along with each flash row.\r
 424 \r
 425                 uint32_t lba =\r
 426                         (((uint32_t) scsiDev.cdb[2]) << 24) +\r
 427                         (((uint32_t) scsiDev.cdb[3]) << 16) +\r
 428                         (((uint32_t) scsiDev.cdb[4]) << 8) +\r
 429                         scsiDev.cdb[5];\r
 430                 uint32_t blocks =\r
 431                         (((uint32_t) scsiDev.cdb[7]) << 8) +\r
 432                         scsiDev.cdb[8];\r
 433 \r
 434                 doWrite(lba, blocks);\r
 435         }\r
 436         else if (unlikely(command == 0x04))\r
 437         {\r
 438                 // FORMAT UNIT\r
 439                 // We don't really do any formatting, but we need to read the correct\r
 440                 // number of bytes in the DATA_OUT phase to make the SCSI host happy.\r
 441 \r
 442                 int fmtData = (scsiDev.cdb[1] & 0x10) ? 1 : 0;\r
 443                 if (fmtData)\r
 444                 {\r
 445                         // We need to read the parameter list, but we don't know how\r
 446                         // big it is yet. Start with the header.\r
 447                         scsiDev.dataLen = 4;\r
 448                         scsiDev.phase = DATA_OUT;\r
 449                         scsiDev.postDataOutHook = doFormatUnitHeader;\r
 450                 }\r
 451                 else\r
 452                 {\r
 453                         // No data to read, we're already finished!\r
 454                 }\r
 455         }\r
 456         else if (unlikely(command == 0x25))\r
 457         {\r
 458                 // READ CAPACITY\r
 459                 doReadCapacity();\r
 460         }\r
 461         else if (unlikely(command == 0x0B))\r
 462         {\r
 463                 // SEEK(6)\r
 464                 uint32_t lba =\r
 465                         (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
 466                         (((uint32_t) scsiDev.cdb[2]) << 8) +\r
 467                         scsiDev.cdb[3];\r
 468 \r
 469                 doSeek(lba);\r
 470         }\r
 471 \r
 472         else if (unlikely(command == 0x2B))\r
 473         {\r
 474                 // SEEK(10)\r
 475                 uint32_t lba =\r
 476                         (((uint32_t) scsiDev.cdb[2]) << 24) +\r
 477                         (((uint32_t) scsiDev.cdb[3]) << 16) +\r
 478                         (((uint32_t) scsiDev.cdb[4]) << 8) +\r
 479                         scsiDev.cdb[5];\r
 480 \r
 481                 doSeek(lba);\r
 482         }\r
 483         else if (unlikely(command == 0x36))\r
 484         {\r
 485                 // LOCK UNLOCK CACHE\r
 486                 // We don't have a cache to lock data into. do nothing.\r
 487         }\r
 488         else if (unlikely(command == 0x34))\r
 489         {\r
 490                 // PRE-FETCH.\r
 491                 // We don't have a cache to pre-fetch into. do nothing.\r
 492         }\r
 493         else if (unlikely(command == 0x1E))\r
 494         {\r
 495                 // PREVENT ALLOW MEDIUM REMOVAL\r
 496                 // Not much we can do to prevent the user removing the SD card.\r
 497                 // do nothing.\r
 498         }\r
 499         else if (unlikely(command == 0x01))\r
 500         {\r
 501                 // REZERO UNIT\r
 502                 // Set the lun to a vendor-specific state. Ignore.\r
 503         }\r
 504         else if (unlikely(command == 0x35))\r
 505         {\r
 506                 // SYNCHRONIZE CACHE\r
 507                 // We don't have a cache. do nothing.\r
 508         }\r
 509         else if (unlikely(command == 0x2F))\r
 510         {\r
 511                 // VERIFY\r
 512                 // TODO: When they supply data to verify, we should read the data and\r
 513                 // verify it. If they don't supply any data, just say success.\r
 514                 if ((scsiDev.cdb[1] & 0x02) == 0)\r
 515                 {\r
 516                         // They are asking us to do a medium verification with no data\r
 517                         // comparison. Assume success, do nothing.\r
 518                 }\r
 519                 else\r
 520                 {\r
 521                         // TODO. This means they are supplying data to verify against.\r
 522                         // Technically we should probably grab the data and compare it.\r
 523                         scsiDev.status = CHECK_CONDITION;\r
 524                         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
 525                         scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
 526                         scsiDev.phase = STATUS;\r
 527                 }\r
 528         }\r
 529         else if (unlikely(command == 0x37))\r
 530         {\r
 531                 // READ DEFECT DATA\r
 532                 uint32_t allocLength = (((uint16_t)scsiDev.cdb[7]) << 8) |\r
 533                         scsiDev.cdb[8];\r
 534 \r
 535                 scsiDev.data[0] = 0;\r
 536                 scsiDev.data[1] = scsiDev.cdb[1];\r
 537                 scsiDev.data[2] = 0;\r
 538                 scsiDev.data[3] = 0;\r
 539                 scsiDev.dataLen = 4;\r
 540 \r
 541                 if (scsiDev.dataLen > allocLength)\r
 542                 {\r
 543                         scsiDev.dataLen = allocLength;\r
 544                 }\r
 545 \r
 546                 scsiDev.phase = DATA_IN;\r
 547         }\r
 548         else\r
 549         {\r
 550                 commandHandled = 0;\r
 551         }\r
 552 \r
 553         return commandHandled;\r
 554 }\r
 555 \r
 556 static uint32_t\r
 557 calcReadahead(uint32_t totalBytes, uint32_t sdSpeedKBs, uint32_t scsiSpeedKBs)\r
 558 {\r
 559         if (!(scsiDev.boardCfg.flags6 & S2S_CFG_ENABLE_BLIND_WRITES) ||\r
 560                 (scsiSpeedKBs == 0) ||\r
 561                 (scsiDev.hostSpeedMeasured == 0))\r
 562         {\r
 563                 return totalBytes;\r
 564         }\r
 565 \r
 566         // uint32_t readAheadBytes = totalBytes * (1 - scsiSpeedKBs / sdSpeedKBs);\r
 567         // Won't overflow with 65536 max bytes, 20000 max scsi speed.\r
 568         uint32_t readAheadBytes = totalBytes - totalBytes * scsiSpeedKBs / sdSpeedKBs;\r
 569 \r
 570         // Round up to nearest FIFO size (* 4 for safety)\r
 571         readAheadBytes = ((readAheadBytes / SCSI_FIFO_DEPTH) + 4) * SCSI_FIFO_DEPTH;\r
 572 \r
 573         if (readAheadBytes > totalBytes)\r
 574         {\r
 575                 readAheadBytes = totalBytes;\r
 576         }\r
 577 \r
 578         return readAheadBytes;\r
 579 }\r
 580 \r
 581 void scsiDiskPoll()\r
 582 {\r
 583         uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
 584 \r
 585         if (scsiDev.phase == DATA_IN &&\r
 586                 transfer.currentBlock != transfer.blocks)\r
 587         {\r
 588                 // Take responsibility for waiting for the phase delays\r
 589                 uint32_t phaseChangeDelayUs = scsiEnterPhaseImmediate(DATA_IN);\r
 590 \r
 591                 int totalSDSectors =\r
 592                         transfer.blocks * SDSectorsPerSCSISector(bytesPerSector);\r
 593                 uint32_t sdLBA =\r
 594                         SCSISector2SD(\r
 595                                 scsiDev.target->cfg->sdSectorStart,\r
 596                                 bytesPerSector,\r
 597                                 transfer.lba);\r
 598 \r
 599                 const int sdPerScsi = SDSectorsPerSCSISector(bytesPerSector);\r
 600                 const int buffers = sizeof(scsiDev.data) / SD_SECTOR_SIZE;\r
 601                 int prep = 0;\r
 602                 int i = 0;\r
 603                 int scsiActive __attribute__((unused)) = 0; // unused if DMA disabled\r
 604                 int sdActive = 0;\r
 605 \r
 606                 // It's highly unlikely that someone is going to use huge transfers\r
 607                 // per scsi command, but if they do it'll be slower than usual.\r
 608                 uint32_t totalScsiBytes = transfer.blocks * bytesPerSector;\r
 609                 int useSlowDataCount = totalScsiBytes >= SCSI_XFER_MAX;\r
 610                 if (!useSlowDataCount)\r
 611                 {\r
 612                         scsiSetDataCount(totalScsiBytes);\r
 613                 }\r
 614 \r
 615                 while ((i < totalSDSectors) &&\r
 616                         likely(scsiDev.phase == DATA_IN) &&\r
 617                         likely(!scsiDev.resetFlag))\r
 618                 {\r
 619                         int completedDmaSectors;\r
 620                         if (sdActive && (completedDmaSectors = sdReadDMAPoll(sdActive)))\r
 621                         {\r
 622                                 prep += completedDmaSectors;\r
 623                                 sdActive -= completedDmaSectors;\r
 624                         } else if (sdActive > 1)\r
 625                         {\r
 626                                 if ((scsiDev.data[SD_SECTOR_SIZE * (prep % buffers) + 510] != 0xAA) ||\r
 627                                         (scsiDev.data[SD_SECTOR_SIZE * (prep % buffers) + 511] != 0x33))\r
 628                                 {\r
 629                                         prep += 1;\r
 630                                         sdActive -= 1;\r
 631                                 }\r
 632                         }\r
 633 \r
 634                         if (!sdActive &&\r
 635                                 (prep - i < buffers) &&\r
 636                                 (prep < totalSDSectors) &&\r
 637                                 ((totalSDSectors - prep) >= sdPerScsi) &&\r
 638                                 (likely(!useSlowDataCount) || scsiPhyComplete()) &&\r
 639                                 (HAL_SD_GetState(&hsd) != HAL_SD_STATE_BUSY)) // rx complete but IRQ not fired yet.\r
 640                         {\r
 641                                 // Start an SD transfer if we have space.\r
 642                                 uint32_t startBuffer = prep % buffers;\r
 643                                 uint32_t sectors = totalSDSectors - prep;\r
 644                                 uint32_t freeBuffers = buffers - (prep - i);\r
 645 \r
 646                                 uint32_t contiguousBuffers = buffers - startBuffer;\r
 647                                 freeBuffers = freeBuffers < contiguousBuffers\r
 648                                         ? freeBuffers : contiguousBuffers;\r
 649                                 sectors = sectors < freeBuffers ? sectors : freeBuffers;\r
 650 \r
 651                                 if (sectors > 128) sectors = 128; // 65536 DMA limit !!\r
 652 \r
 653                                 // Round-down when we have odd sector sizes.\r
 654                                 if (sdPerScsi != 1)\r
 655                                 {\r
 656                                         sectors = (sectors / sdPerScsi) * sdPerScsi;\r
 657                                 }\r
 658 \r
 659                                 for (int dodgy = 0; dodgy < sectors; dodgy++)\r
 660                                 {\r
 661                                         scsiDev.data[SD_SECTOR_SIZE * (startBuffer + dodgy) + 510] = 0xAA;\r
 662                                         scsiDev.data[SD_SECTOR_SIZE * (startBuffer + dodgy) + 511] = 0x33;\r
 663                                 }\r
 664 \r
 665                                 sdReadDMA(sdLBA + prep, sectors, &scsiDev.data[SD_SECTOR_SIZE * startBuffer]);\r
 666 \r
 667                                 sdActive = sectors;\r
 668 \r
 669                                 if (useSlowDataCount)\r
 670                                 {\r
 671                                         scsiSetDataCount((sectors / sdPerScsi) * bytesPerSector);\r
 672                                 }\r
 673 \r
 674                                 // Wait now that the SD card is busy\r
 675                                 // Chances are we've probably already waited sufficient time,\r
 676                                 // but it's hard to measure microseconds cheaply. So just wait\r
 677                                 // extra just-in-case. Hopefully it's in parallel with dma.\r
 678                                 if (phaseChangeDelayUs > 0)\r
 679                                 {\r
 680                                         s2s_delay_us(phaseChangeDelayUs);\r
 681                                         phaseChangeDelayUs = 0;\r
 682                                 }\r
 683                         }\r
 684 \r
 685                         if ((prep - i) > 0)\r
 686                         {\r
 687                                 int dmaBytes = SD_SECTOR_SIZE;\r
 688                                 if ((i % sdPerScsi) == (sdPerScsi - 1))\r
 689                                 {\r
 690                                         dmaBytes = bytesPerSector % SD_SECTOR_SIZE;\r
 691                                         if (dmaBytes == 0) dmaBytes = SD_SECTOR_SIZE;\r
 692                                 }\r
 693 \r
 694                                 uint8_t* scsiDmaData = &(scsiDev.data[SD_SECTOR_SIZE * (i % buffers)]);\r
 695                                 scsiWritePIO(scsiDmaData, dmaBytes);\r
 696 \r
 697                                 ++i;\r
 698                         }\r
 699                 }\r
 700 \r
 701                 if (phaseChangeDelayUs > 0 && !scsiDev.resetFlag) // zero bytes ?\r
 702                 {\r
 703                         s2s_delay_us(phaseChangeDelayUs);\r
 704                         phaseChangeDelayUs = 0;\r
 705                 }\r
 706 \r
 707                 // We've finished transferring the data to the FPGA, now wait until it's\r
 708                 // written to he SCSI bus.\r
 709                 __disable_irq();\r
 710                 while (!scsiPhyComplete() &&\r
 711                         likely(scsiDev.phase == DATA_IN) &&\r
 712                         likely(!scsiDev.resetFlag))\r
 713                 {\r
 714                         __WFI();\r
 715                 }\r
 716                 __enable_irq();\r
 717 \r
 718                 while (HAL_SD_GetState(&hsd) == HAL_SD_STATE_BUSY)\r
 719                 {\r
 720                         // Wait while keeping BSY.\r
 721                 }\r
 722 \r
 723                 if (scsiDev.phase == DATA_IN)\r
 724                 {\r
 725                         scsiDev.phase = STATUS;\r
 726                 }\r
 727                 scsiDiskReset();\r
 728         }\r
 729         else if (scsiDev.phase == DATA_OUT &&\r
 730                 transfer.currentBlock != transfer.blocks)\r
 731         {\r
 732                 scsiEnterPhase(DATA_OUT);\r
 733 \r
 734                 const int sdPerScsi = SDSectorsPerSCSISector(bytesPerSector);\r
 735                 int totalSDSectors = transfer.blocks * sdPerScsi;\r
 736                 uint32_t sdLBA =\r
 737                         SCSISector2SD(\r
 738                                 scsiDev.target->cfg->sdSectorStart,\r
 739                                 bytesPerSector,\r
 740                                 transfer.lba);\r
 741                 int i = 0;\r
 742                 int clearBSY = 0;\r
 743 \r
 744                 int parityError = 0;\r
 745                 int enableParity = scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY;\r
 746 \r
 747                 uint32_t maxSectors = sizeof(scsiDev.data) / SD_SECTOR_SIZE;\r
 748 \r
 749                 static_assert(SCSI_XFER_MAX >= sizeof(scsiDev.data), "Assumes SCSI_XFER_MAX >= sizeof(scsiDev.data)");\r
 750 \r
 751                 // Start reading and filling fifos as soon as possible.\r
 752                 // It's highly unlikely that someone is going to use huge transfers\r
 753                 // per scsi command, but if they do it'll be slower than usual.\r
 754                 // Note: Happens in Macintosh FWB HDD Toolkit benchmarks which default\r
 755                 // to 768kb\r
 756                 uint32_t totalTransferBytes = transfer.blocks * bytesPerSector;\r
 757                 int useSlowDataCount = totalTransferBytes >= SCSI_XFER_MAX;\r
 758                 if (!useSlowDataCount)\r
 759                 {\r
 760                         DWT->CYCCNT = 0; // Start counting cycles\r
 761                         scsiSetDataCount(totalTransferBytes);\r
 762                 }\r
 763 \r
 764                 while ((i < totalSDSectors) &&\r
 765                         likely(scsiDev.phase == DATA_OUT) &&\r
 766                         likely(!scsiDev.resetFlag))\r
 767                         // KEEP GOING to ensure FIFOs are in a good state.\r
 768                         // likely(!parityError || !enableParity))\r
 769                 {\r
 770                         uint32_t rem = totalSDSectors - i;\r
 771                         uint32_t sectors = rem < maxSectors ? rem : maxSectors;\r
 772 \r
 773                         if (bytesPerSector == SD_SECTOR_SIZE)\r
 774                         {\r
 775                                 // We assume the SD card is faster than the SCSI interface, but has\r
 776                                 // no flow control. This can be handled if a) the scsi interface\r
 777                                 // doesn't block and b) we read enough SCSI sectors first so that\r
 778                                 // the SD interface cannot catch up.\r
 779                                 uint32_t totalBytes = sectors * SD_SECTOR_SIZE;\r
 780 \r
 781                                 uint32_t sdSpeedKBs = s2s_getSdRateKBs() + (scsiDev.sdUnderrunCount * 256);\r
 782                                 uint32_t readAheadBytes = calcReadahead(\r
 783                                         totalBytes,\r
 784                                         sdSpeedKBs,\r
 785                                         scsiDev.hostSpeedKBs);\r
 786 \r
 787                                 if (useSlowDataCount)\r
 788                                 {\r
 789                                         DWT->CYCCNT = 0; // Start counting cycles\r
 790                                         scsiSetDataCount(totalBytes);\r
 791                                 }\r
 792 \r
 793                                 uint32_t scsiBytesRead = 0;\r
 794                                 if (readAheadBytes > 0)\r
 795                                 {\r
 796                                         scsiReadPIO(\r
 797                                                 &scsiDev.data[scsiBytesRead],\r
 798                                                 readAheadBytes,\r
 799                                                 &parityError);\r
 800                                         scsiBytesRead += readAheadBytes;\r
 801 \r
 802                                         if (i == 0 && !useSlowDataCount)\r
 803                                         {\r
 804                                                 uint32_t elapsedCycles = DWT->CYCCNT;\r
 805 \r
 806                                                 // uint32_t rateKBs = (readAheadBytes / 1000) / (elapsedCycles / HAL_RCC_GetHCLKFreq());\r
 807                                                 // Scaled by 4 to avoid overflow w/ max 65536 at 108MHz.\r
 808                                                 uint32_t rateKBs = ((readAheadBytes / 4) * (HAL_RCC_GetHCLKFreq() / 1000) / elapsedCycles) * 4;\r
 809 \r
 810                                                 scsiDev.hostSpeedKBs = (scsiDev.hostSpeedKBs + rateKBs) / 2;\r
 811                                                 scsiDev.hostSpeedMeasured = 1;\r
 812 \r
 813                                                 if (rateKBs < scsiDev.hostSpeedKBs)\r
 814                                                 {\r
 815                                                         // Our readahead was too slow; assume remaining bytes\r
 816                                                         // will be as well.\r
 817                                                         if (readAheadBytes < totalBytes)\r
 818                                                         {\r
 819                                                                 uint32_t properReadahead = calcReadahead(\r
 820                                                                         totalBytes,\r
 821                                                                         sdSpeedKBs,\r
 822                                                                         rateKBs);\r
 823 \r
 824                                                                 if (properReadahead > readAheadBytes)\r
 825                                                                 {\r
 826                                                                         uint32_t diff = properReadahead - readAheadBytes;\r
 827                                                                         readAheadBytes = properReadahead;\r
 828                                                                         scsiReadPIO(\r
 829                                                                                 &scsiDev.data[scsiBytesRead],\r
 830                                                                                 diff,\r
 831                                                                                 &parityError);\r
 832                                                                         scsiBytesRead += diff;\r
 833                                                                 }\r
 834                                                         }\r
 835                                                 }\r
 836                                         }\r
 837                                 }\r
 838 \r
 839                                 HAL_SD_WriteBlocks_DMA(&hsd, (&scsiDev.data[0]), i + sdLBA, sectors);\r
 840 \r
 841                                 int underrun = 0;\r
 842                                 if (scsiBytesRead < totalBytes && !scsiDev.resetFlag)\r
 843                                 {\r
 844                                         scsiReadPIO(\r
 845                                                 &scsiDev.data[scsiBytesRead],\r
 846                                                 totalBytes - readAheadBytes,\r
 847                                                 &parityError);\r
 848 \r
 849                                         // Oh dear, SD finished first.\r
 850                                         underrun = HAL_DMA_GetState(hsd.hdmatx) == HAL_DMA_STATE_BUSY;\r
 851 \r
 852                                         scsiBytesRead += (totalBytes - readAheadBytes);\r
 853                                 }\r
 854 \r
 855                                 uint32_t dmaFinishTime = s2s_getTime_ms();\r
 856                                 while ((HAL_SD_GetState(&hsd) == HAL_SD_STATE_BUSY) &&\r
 857                                         s2s_elapsedTime_ms(dmaFinishTime) < 180)\r
 858                                 {\r
 859                                         // Wait while keeping BSY.\r
 860                                 }\r
 861 \r
 862                 HAL_SD_CardStateTypeDef cardState = HAL_SD_GetCardState(&hsd);\r
 863                 while ((cardState == HAL_SD_CARD_PROGRAMMING || cardState == HAL_SD_CARD_RECEIVING) &&\r
 864                                         s2s_elapsedTime_ms(dmaFinishTime) < 180)\r
 865                 {\r
 866                     // Wait while the SD card is writing buffer to flash\r
 867                     // The card may remain in the RECEIVING state (even though it's programming) if\r
 868                     // it has buffer space to receive more data available.\r
 869                     cardState = HAL_SD_GetCardState(&hsd);\r
 870                 }\r
 871 \r
 872                                 if (i + sectors >= totalSDSectors &&\r
 873                                         !underrun &&\r
 874                                         (!parityError || !enableParity))\r
 875                                 {\r
 876                                         // We're transferring over the SCSI bus faster than the SD card\r
 877                                         // can write.  All data is buffered, and we're just waiting for\r
 878                                         // the SD card to complete. The host won't let us disconnect.\r
 879                                         // Some drivers set a 250ms timeout on transfers to complete.\r
 880                                         // SD card writes are supposed to complete\r
 881                                         // within 200ms, but sometimes they don't.\r
 882                                         // Just pretend we're finished.\r
 883                                         process_Status();\r
 884                                         clearBSY = process_MessageIn(0); // Will go to BUS_FREE state but keep BSY asserted.\r
 885                                 }\r
 886 \r
 887                                 while (HAL_SD_GetState(&hsd) == HAL_SD_STATE_BUSY)\r
 888                                 {\r
 889                                         // Wait while keeping BSY.\r
 890                                 }\r
 891 \r
 892                 cardState = HAL_SD_GetCardState(&hsd);\r
 893                 while (cardState == HAL_SD_CARD_PROGRAMMING || cardState == HAL_SD_CARD_RECEIVING) \r
 894                 {\r
 895                     // Wait while the SD card is writing buffer to flash\r
 896                     // The card may remain in the RECEIVING state (even though it's programming) if\r
 897                     // it has buffer space to receive more data available.\r
 898                     cardState = HAL_SD_GetCardState(&hsd);\r
 899                 }\r
 900 \r
 901                                 if (underrun && (!parityError || !enableParity))\r
 902                                 {\r
 903                                         // Try again. Data is still in memory.\r
 904                                         BSP_SD_WriteBlocks_DMA(&scsiDev.data[0], i + sdLBA, sectors);\r
 905                                         scsiDev.sdUnderrunCount++;\r
 906                                 }\r
 907 \r
 908                                 i += sectors;\r
 909                         }\r
 910                         else\r
 911                         {\r
 912                                 // Well, until we have some proper non-blocking SD code, we must\r
 913                                 // do this in a half-duplex fashion. We need to write as much as\r
 914                                 // possible in each SD card transaction.\r
 915                                 // use sg_dd from sg_utils3 tools to test.\r
 916 \r
 917                                 if (useSlowDataCount)\r
 918                                 {\r
 919                                         scsiSetDataCount(sectors * bytesPerSector);\r
 920                                 }\r
 921 \r
 922                                 for (int scsiSector = i; scsiSector < i + sectors; ++scsiSector)\r
 923                                 {\r
 924                                         int dmaBytes = SD_SECTOR_SIZE;\r
 925                                         if ((scsiSector % sdPerScsi) == (sdPerScsi - 1))\r
 926                                         {\r
 927                                                 dmaBytes = bytesPerSector % SD_SECTOR_SIZE;\r
 928                                                 if (dmaBytes == 0) dmaBytes = SD_SECTOR_SIZE;\r
 929                                         }\r
 930 \r
 931                                         scsiReadPIO(&scsiDev.data[SD_SECTOR_SIZE * (scsiSector - i)], dmaBytes, &parityError);\r
 932                                 }\r
 933                                 if (!parityError || !enableParity)\r
 934                                 {\r
 935                                         BSP_SD_WriteBlocks_DMA(&scsiDev.data[0], i + sdLBA, sectors);\r
 936                                 }\r
 937                                 i += sectors;\r
 938                         }\r
 939                 }\r
 940 \r
 941                 // Should already be complete here as we've ready the FIFOs\r
 942                 // by now. Check anyway.\r
 943                 __disable_irq();\r
 944                 while (!scsiPhyComplete() && likely(!scsiDev.resetFlag))\r
 945                 {\r
 946                         __WFI();\r
 947                 }\r
 948                 __enable_irq();\r
 949 \r
 950                 if (clearBSY)\r
 951                 {\r
 952                         enter_BusFree();\r
 953                 }\r
 954 \r
 955                 if (scsiDev.phase == DATA_OUT)\r
 956                 {\r
 957                         if (parityError &&\r
 958                                 (scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))\r
 959                         {\r
 960                                 scsiDev.target->sense.code = ABORTED_COMMAND;\r
 961                                 scsiDev.target->sense.asc = SCSI_PARITY_ERROR;\r
 962                                 scsiDev.status = CHECK_CONDITION;;\r
 963                         }\r
 964                         scsiDev.phase = STATUS;\r
 965                 }\r
 966                 scsiDiskReset();\r
 967         }\r
 968 }\r
 969 \r
 970 void scsiDiskReset()\r
 971 {\r
 972         scsiDev.dataPtr = 0;\r
 973         scsiDev.savedDataPtr = 0;\r
 974         scsiDev.dataLen = 0;\r
 975         // transfer.lba = 0; // Needed in Request Sense to determine failure\r
 976         transfer.blocks = 0;\r
 977         transfer.currentBlock = 0;\r
 978 \r
 979         // Cancel long running commands!\r
 980 #if 0\r
 981         if (\r
 982                 ((scsiDev.boardCfg.flags & S2S_CFG_ENABLE_CACHE) == 0) ||\r
 983                         (transfer.multiBlock == 0)\r
 984                 )\r
 985 #endif\r
 986         {\r
 987                 sdCompleteTransfer();\r
 988         }\r
 989 \r
 990         transfer.multiBlock = 0;\r
 991 }\r
 992 \r
 993 void scsiDiskInit()\r
 994 {\r
 995         scsiDiskReset();\r
 996 \r
 997         // Don't require the host to send us a START STOP UNIT command\r
 998         blockDev.state = DISK_STARTED;\r
 999 }\r
1000 \r