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