Add 10ms delay to seek command
[SCSI2SD-V6.git] / 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         else\r
298         {\r
299                 s2s_delay_ms(10);\r
300         }\r
301 }\r
302 \r
303 static int doTestUnitReady()\r
304 {\r
305         int ready = 1;\r
306         if (likely(blockDev.state == (DISK_STARTED | DISK_PRESENT | DISK_INITIALISED)))\r
307         {\r
308                 // nothing to do.\r
309         }\r
310         else if (unlikely(!(blockDev.state & DISK_STARTED)))\r
311         {\r
312                 ready = 0;\r
313                 scsiDev.status = CHECK_CONDITION;\r
314                 scsiDev.target->sense.code = NOT_READY;\r
315                 scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_READY_INITIALIZING_COMMAND_REQUIRED;\r
316                 scsiDev.phase = STATUS;\r
317         }\r
318         else if (unlikely(!(blockDev.state & DISK_PRESENT)))\r
319         {\r
320                 ready = 0;\r
321                 scsiDev.status = CHECK_CONDITION;\r
322                 scsiDev.target->sense.code = NOT_READY;\r
323                 scsiDev.target->sense.asc = MEDIUM_NOT_PRESENT;\r
324                 scsiDev.phase = STATUS;\r
325         }\r
326         else if (unlikely(!(blockDev.state & DISK_INITIALISED)))\r
327         {\r
328                 ready = 0;\r
329                 scsiDev.status = CHECK_CONDITION;\r
330                 scsiDev.target->sense.code = NOT_READY;\r
331                 scsiDev.target->sense.asc = LOGICAL_UNIT_NOT_READY_CAUSE_NOT_REPORTABLE;\r
332                 scsiDev.phase = STATUS;\r
333         }\r
334         return ready;\r
335 }\r
336 \r
337 // Handle direct-access scsi device commands\r
338 int scsiDiskCommand()\r
339 {\r
340         int commandHandled = 1;\r
341 \r
342         uint8_t command = scsiDev.cdb[0];\r
343         if (unlikely(command == 0x1B))\r
344         {\r
345                 // START STOP UNIT\r
346                 // Enable or disable media access operations.\r
347                 // Ignore load/eject requests. We can't do that.\r
348                 //int immed = scsiDev.cdb[1] & 1;\r
349                 int start = scsiDev.cdb[4] & 1;\r
350 \r
351                 if (start)\r
352                 {\r
353                         blockDev.state = blockDev.state | DISK_STARTED;\r
354                         if (!(blockDev.state & DISK_INITIALISED))\r
355                         {\r
356                                 doSdInit();\r
357                         }\r
358                 }\r
359                 else\r
360                 {\r
361                         blockDev.state &= ~DISK_STARTED;\r
362                 }\r
363         }\r
364         else if (unlikely(command == 0x00))\r
365         {\r
366                 // TEST UNIT READY\r
367                 doTestUnitReady();\r
368         }\r
369         else if (unlikely(!doTestUnitReady()))\r
370         {\r
371                 // Status and sense codes already set by doTestUnitReady\r
372         }\r
373         else if (likely(command == 0x08))\r
374         {\r
375                 // READ(6)\r
376                 uint32_t lba =\r
377                         (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
378                         (((uint32_t) scsiDev.cdb[2]) << 8) +\r
379                         scsiDev.cdb[3];\r
380                 uint32_t blocks = scsiDev.cdb[4];\r
381                 if (unlikely(blocks == 0)) blocks = 256;\r
382                 doRead(lba, blocks);\r
383         }\r
384         else if (likely(command == 0x28))\r
385         {\r
386                 // READ(10)\r
387                 // Ignore all cache control bits - we don't support a memory cache.\r
388 \r
389                 uint32_t lba =\r
390                         (((uint32_t) scsiDev.cdb[2]) << 24) +\r
391                         (((uint32_t) scsiDev.cdb[3]) << 16) +\r
392                         (((uint32_t) scsiDev.cdb[4]) << 8) +\r
393                         scsiDev.cdb[5];\r
394                 uint32_t blocks =\r
395                         (((uint32_t) scsiDev.cdb[7]) << 8) +\r
396                         scsiDev.cdb[8];\r
397 \r
398                 doRead(lba, blocks);\r
399         }\r
400         else if (likely(command == 0x0A))\r
401         {\r
402                 // WRITE(6)\r
403                 uint32_t lba =\r
404                         (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
405                         (((uint32_t) scsiDev.cdb[2]) << 8) +\r
406                         scsiDev.cdb[3];\r
407                 uint32_t blocks = scsiDev.cdb[4];\r
408                 if (unlikely(blocks == 0)) blocks = 256;\r
409                 doWrite(lba, blocks);\r
410         }\r
411         else if (likely(command == 0x2A) || // WRITE(10)\r
412                 unlikely(command == 0x2E)) // WRITE AND VERIFY\r
413         {\r
414                 // Ignore all cache control bits - we don't support a memory cache.\r
415                 // Don't bother verifying either. The SD card likely stores ECC\r
416                 // along with each flash row.\r
417 \r
418                 uint32_t lba =\r
419                         (((uint32_t) scsiDev.cdb[2]) << 24) +\r
420                         (((uint32_t) scsiDev.cdb[3]) << 16) +\r
421                         (((uint32_t) scsiDev.cdb[4]) << 8) +\r
422                         scsiDev.cdb[5];\r
423                 uint32_t blocks =\r
424                         (((uint32_t) scsiDev.cdb[7]) << 8) +\r
425                         scsiDev.cdb[8];\r
426 \r
427                 doWrite(lba, blocks);\r
428         }\r
429         else if (unlikely(command == 0x04))\r
430         {\r
431                 // FORMAT UNIT\r
432                 // We don't really do any formatting, but we need to read the correct\r
433                 // number of bytes in the DATA_OUT phase to make the SCSI host happy.\r
434 \r
435                 int fmtData = (scsiDev.cdb[1] & 0x10) ? 1 : 0;\r
436                 if (fmtData)\r
437                 {\r
438                         // We need to read the parameter list, but we don't know how\r
439                         // big it is yet. Start with the header.\r
440                         scsiDev.dataLen = 4;\r
441                         scsiDev.phase = DATA_OUT;\r
442                         scsiDev.postDataOutHook = doFormatUnitHeader;\r
443                 }\r
444                 else\r
445                 {\r
446                         // No data to read, we're already finished!\r
447                 }\r
448         }\r
449         else if (unlikely(command == 0x25))\r
450         {\r
451                 // READ CAPACITY\r
452                 doReadCapacity();\r
453         }\r
454         else if (unlikely(command == 0x0B))\r
455         {\r
456                 // SEEK(6)\r
457                 uint32_t lba =\r
458                         (((uint32_t) scsiDev.cdb[1] & 0x1F) << 16) +\r
459                         (((uint32_t) scsiDev.cdb[2]) << 8) +\r
460                         scsiDev.cdb[3];\r
461 \r
462                 doSeek(lba);\r
463         }\r
464 \r
465         else if (unlikely(command == 0x2B))\r
466         {\r
467                 // SEEK(10)\r
468                 uint32_t lba =\r
469                         (((uint32_t) scsiDev.cdb[2]) << 24) +\r
470                         (((uint32_t) scsiDev.cdb[3]) << 16) +\r
471                         (((uint32_t) scsiDev.cdb[4]) << 8) +\r
472                         scsiDev.cdb[5];\r
473 \r
474                 doSeek(lba);\r
475         }\r
476         else if (unlikely(command == 0x36))\r
477         {\r
478                 // LOCK UNLOCK CACHE\r
479                 // We don't have a cache to lock data into. do nothing.\r
480         }\r
481         else if (unlikely(command == 0x34))\r
482         {\r
483                 // PRE-FETCH.\r
484                 // We don't have a cache to pre-fetch into. do nothing.\r
485         }\r
486         else if (unlikely(command == 0x1E))\r
487         {\r
488                 // PREVENT ALLOW MEDIUM REMOVAL\r
489                 // Not much we can do to prevent the user removing the SD card.\r
490                 // do nothing.\r
491         }\r
492         else if (unlikely(command == 0x01))\r
493         {\r
494                 // REZERO UNIT\r
495                 // Set the lun to a vendor-specific state. Ignore.\r
496         }\r
497         else if (unlikely(command == 0x35))\r
498         {\r
499                 // SYNCHRONIZE CACHE\r
500                 // We don't have a cache. do nothing.\r
501         }\r
502         else if (unlikely(command == 0x2F))\r
503         {\r
504                 // VERIFY\r
505                 // TODO: When they supply data to verify, we should read the data and\r
506                 // verify it. If they don't supply any data, just say success.\r
507                 if ((scsiDev.cdb[1] & 0x02) == 0)\r
508                 {\r
509                         // They are asking us to do a medium verification with no data\r
510                         // comparison. Assume success, do nothing.\r
511                 }\r
512                 else\r
513                 {\r
514                         // TODO. This means they are supplying data to verify against.\r
515                         // Technically we should probably grab the data and compare it.\r
516                         scsiDev.status = CHECK_CONDITION;\r
517                         scsiDev.target->sense.code = ILLEGAL_REQUEST;\r
518                         scsiDev.target->sense.asc = INVALID_FIELD_IN_CDB;\r
519                         scsiDev.phase = STATUS;\r
520                 }\r
521         }\r
522         else if (unlikely(command == 0x37))\r
523         {\r
524                 // READ DEFECT DATA\r
525                 scsiDev.status = CHECK_CONDITION;\r
526                 scsiDev.target->sense.code = NO_SENSE;\r
527                 scsiDev.target->sense.asc = DEFECT_LIST_NOT_FOUND;\r
528                 scsiDev.phase = STATUS;\r
529 \r
530         }\r
531         else\r
532         {\r
533                 commandHandled = 0;\r
534         }\r
535 \r
536         return commandHandled;\r
537 }\r
538 \r
539 void scsiDiskPoll()\r
540 {\r
541         uint32_t bytesPerSector = scsiDev.target->liveCfg.bytesPerSector;\r
542 \r
543         if (scsiDev.phase == DATA_IN &&\r
544                 transfer.currentBlock != transfer.blocks)\r
545         {\r
546 \r
547                 int totalSDSectors =\r
548                         transfer.blocks * SDSectorsPerSCSISector(bytesPerSector);\r
549                 uint32_t sdLBA =\r
550                         SCSISector2SD(\r
551                                 scsiDev.target->cfg->sdSectorStart,\r
552                                 bytesPerSector,\r
553                                 transfer.lba);\r
554 \r
555                 const int sdPerScsi = SDSectorsPerSCSISector(bytesPerSector);\r
556                 const int buffers = sizeof(scsiDev.data) / SD_SECTOR_SIZE;\r
557                 int prep = 0;\r
558                 int i = 0;\r
559                 int scsiActive __attribute__((unused)) = 0; // unused if DMA disabled\r
560                 int sdActive = 0;\r
561 \r
562                 uint32_t partialScsiChunk = 0;\r
563 \r
564                 // Start reading from the SD card FIRST, because we change state and\r
565                 // wai for SCSI signals\r
566                 int dataInStarted = 0;\r
567 \r
568                 while ((i < totalSDSectors) &&\r
569                         (!dataInStarted || likely(scsiDev.phase == DATA_IN)) &&\r
570                         likely(!scsiDev.resetFlag))\r
571                 {\r
572                         int completedDmaSectors;\r
573                         if (sdActive && (completedDmaSectors = sdReadDMAPoll(sdActive)))\r
574                         {\r
575                                 prep += completedDmaSectors;\r
576                                 sdActive -= completedDmaSectors;\r
577                         } else if (sdActive > 1)\r
578                         {\r
579                                 if ((scsiDev.data[SD_SECTOR_SIZE * (prep % buffers) + 510] != 0xAA) ||\r
580                                         (scsiDev.data[SD_SECTOR_SIZE * (prep % buffers) + 511] != 0x33))\r
581                                 {\r
582                                         prep += 1;\r
583                                         sdActive -= 1;\r
584                                 }\r
585                         }\r
586 \r
587                         if (!sdActive &&\r
588                                 (prep - i < buffers) &&\r
589                                 (prep < totalSDSectors))\r
590                         {\r
591                                 // Start an SD transfer if we have space.\r
592                                 uint32_t startBuffer = prep % buffers;\r
593                                 uint32_t sectors = totalSDSectors - prep;\r
594 \r
595                                 uint32_t freeBuffers = buffers - (prep - i);\r
596 \r
597                                 uint32_t contiguousBuffers = buffers - startBuffer;\r
598                                 freeBuffers = freeBuffers < contiguousBuffers\r
599                                         ? freeBuffers : contiguousBuffers;\r
600                                 sectors = sectors < freeBuffers ? sectors : freeBuffers;\r
601 \r
602                                 if (sectors > 128) sectors = 128; // 65536 DMA limit !!\r
603 \r
604                                 for (int dodgy = 0; dodgy < sectors; dodgy++)\r
605                                 {\r
606                                         scsiDev.data[SD_SECTOR_SIZE * (startBuffer + dodgy) + 510] = 0xAA;\r
607                                         scsiDev.data[SD_SECTOR_SIZE * (startBuffer + dodgy) + 511] = 0x33;\r
608                                 }\r
609 \r
610                                 sdReadDMA(sdLBA + prep, sectors, &scsiDev.data[SD_SECTOR_SIZE * startBuffer]);\r
611 \r
612                                 sdActive = sectors;\r
613 \r
614                                 if (!dataInStarted)\r
615                                 {\r
616                                         dataInStarted = 1;\r
617                                         scsiEnterPhase(DATA_IN); // Will wait a few microseconds.\r
618                                 }\r
619                         }\r
620 \r
621 #ifdef SCSI_FSMC_DMA\r
622                         #error this code not updated for 256 max bytes in scsi fifo\r
623                         if (scsiActive && scsiPhyComplete() && scsiWriteDMAPoll())\r
624                         {\r
625                                 scsiActive = 0;\r
626                                 i++;\r
627                                 scsiPhyFifoFlip();\r
628                         }\r
629                         if (!scsiActive && ((prep - i) > 0))\r
630                         {\r
631                                 int dmaBytes = SD_SECTOR_SIZE;\r
632                                 if ((i % sdPerScsi) == (sdPerScsi - 1))\r
633                                 {\r
634                                         dmaBytes = bytesPerSector % SD_SECTOR_SIZE;\r
635                                         if (dmaBytes == 0) dmaBytes = SD_SECTOR_SIZE;\r
636                                 }\r
637                                 scsiWriteDMA(&scsiDev.data[SD_SECTOR_SIZE * (i % buffers)], dmaBytes);\r
638                                 scsiActive = 1;\r
639                         }\r
640 #else\r
641                         if ((prep - i) > 0)\r
642                         {\r
643                                 int dmaBytes = SD_SECTOR_SIZE;\r
644                                 if ((i % sdPerScsi) == (sdPerScsi - 1))\r
645                                 {\r
646                                         dmaBytes = bytesPerSector % SD_SECTOR_SIZE;\r
647                                         if (dmaBytes == 0) dmaBytes = SD_SECTOR_SIZE;\r
648                                 }\r
649 \r
650                                 // Manually unrolled loop for performance.\r
651                                 // -Os won't unroll this for us automatically,\r
652                                 // especially since scsiPhyTx does volatile stuff.\r
653                                 // Reduces bus utilisation by making the fsmc split\r
654                                 // 32bits into 2 16 bit writes.\r
655 \r
656                                 uint16_t* scsiDmaData = (uint16_t*) &(scsiDev.data[SD_SECTOR_SIZE * (i % buffers) + partialScsiChunk]);\r
657 \r
658                                 uint32_t chunk = ((dmaBytes - partialScsiChunk) > SCSI_FIFO_DEPTH)\r
659                                         ? SCSI_FIFO_DEPTH : (dmaBytes - partialScsiChunk);\r
660 \r
661                                 int k = 0;\r
662                                 for (; k + 4 < (chunk + 1) / 2; k += 4)\r
663                                 {\r
664                                         scsiPhyTx32(scsiDmaData[k], scsiDmaData[k+1]);\r
665                                         scsiPhyTx32(scsiDmaData[k+2], scsiDmaData[k+3]);\r
666                                 }\r
667                                 for (; k < (chunk + 1) / 2; ++k)\r
668                                 {\r
669                                         scsiPhyTx(scsiDmaData[k]);\r
670                                 }\r
671                                 while (!scsiPhyComplete() && !scsiDev.resetFlag)\r
672                                 {\r
673                                         __WFE(); // Wait for event\r
674                                 }\r
675                                 scsiPhyFifoFlip();\r
676                                 scsiSetDataCount(chunk);\r
677 \r
678                                 partialScsiChunk += chunk;\r
679                                 if (partialScsiChunk == dmaBytes)\r
680                                 {\r
681                                         partialScsiChunk = 0;\r
682                                         ++i;\r
683                                 }\r
684                         }\r
685 #endif\r
686                 }\r
687 \r
688                 if (!dataInStarted && !scsiDev.resetFlag) // zero bytes ?\r
689                 {\r
690                         scsiEnterPhase(DATA_IN); // Will wait a few microseconds.\r
691                 }\r
692 \r
693                 // We've finished transferring the data to the FPGA, now wait until it's\r
694                 // written to he SCSI bus.\r
695                 while (!scsiPhyComplete() &&\r
696                         likely(scsiDev.phase == DATA_IN) &&\r
697                         likely(!scsiDev.resetFlag))\r
698                 {\r
699                         __WFE(); // Wait for event\r
700                 }\r
701 \r
702 \r
703                 if (scsiDev.phase == DATA_IN)\r
704                 {\r
705                         scsiDev.phase = STATUS;\r
706                 }\r
707                 scsiDiskReset();\r
708         }\r
709         else if (scsiDev.phase == DATA_OUT &&\r
710                 transfer.currentBlock != transfer.blocks)\r
711         {\r
712                 scsiEnterPhase(DATA_OUT);\r
713 \r
714                 const int sdPerScsi = SDSectorsPerSCSISector(bytesPerSector);\r
715                 int totalSDSectors = transfer.blocks * sdPerScsi;\r
716                 uint32_t sdLBA =\r
717                         SCSISector2SD(\r
718                                 scsiDev.target->cfg->sdSectorStart,\r
719                                 bytesPerSector,\r
720                                 transfer.lba);\r
721                 int i = 0;\r
722                 int clearBSY = 0;\r
723 \r
724                 int parityError = 0;\r
725                 int enableParity = scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY;\r
726 \r
727                 while ((i < totalSDSectors) &&\r
728                         likely(scsiDev.phase == DATA_OUT) &&\r
729                         likely(!scsiDev.resetFlag) &&\r
730                         likely(!parityError || !enableParity))\r
731                 {\r
732                         // Well, until we have some proper non-blocking SD code, we must\r
733                         // do this in a half-duplex fashion. We need to write as much as\r
734                         // possible in each SD card transaction.\r
735                         uint32_t maxSectors = sizeof(scsiDev.data) / SD_SECTOR_SIZE;\r
736                         uint32_t rem = totalSDSectors - i;\r
737                         uint32_t sectors =\r
738                                 rem < maxSectors ? rem : maxSectors;\r
739 \r
740                         if (bytesPerSector == SD_SECTOR_SIZE)\r
741                         {\r
742                                 // We assume the SD card is faster than the SCSI interface, but has\r
743                                 // no flow control. This can be handled if a) the scsi interface\r
744                                 // doesn't block and b) we read enough SCSI sectors first so that\r
745                                 // the SD interface cannot catch up.\r
746                                 uint32_t totalBytes = sectors * SD_SECTOR_SIZE;\r
747                                 uint32_t readAheadBytes = sectors * SD_SECTOR_SIZE;\r
748                                 uint32_t sdSpeed = s2s_getSdRateMBs() + (scsiDev.sdUnderrunCount / 2);\r
749                                 uint32_t scsiSpeed = s2s_getScsiRateMBs();\r
750                                 // if (have blind writes)\r
751                                 if (scsiSpeed > 0 && scsiDev.sdUnderrunCount < 16)\r
752                                 {\r
753                                         // readAhead = sectors * (sd / scsi - 1 + 0.1);\r
754                                         readAheadBytes = totalBytes * sdSpeed / scsiSpeed - totalBytes + SCSI_FIFO_DEPTH;\r
755                                         if (readAheadBytes < SCSI_FIFO_DEPTH)\r
756                                         {\r
757                                                 readAheadBytes = SCSI_FIFO_DEPTH;\r
758                                         }\r
759 \r
760                                         if (readAheadBytes > totalBytes)\r
761                                         {\r
762                                                 readAheadBytes = totalBytes;\r
763                                         }\r
764                                 }\r
765 \r
766                                 uint32_t chunk = (readAheadBytes > SCSI_FIFO_DEPTH) ? SCSI_FIFO_DEPTH : readAheadBytes;\r
767                                 scsiSetDataCount(chunk);\r
768 \r
769                                 uint32_t scsiBytesRead = 0;\r
770                                 while (scsiBytesRead < readAheadBytes)\r
771                                 {\r
772                                         while (!scsiPhyComplete() && likely(!scsiDev.resetFlag))\r
773                                         {\r
774                                                 __WFE(); // Wait for event\r
775                                         }\r
776                                         parityError |= scsiParityError();\r
777                                         scsiPhyFifoFlip();\r
778                                         uint32_t nextChunk = ((totalBytes - scsiBytesRead - chunk) > SCSI_FIFO_DEPTH)\r
779                                                 ? SCSI_FIFO_DEPTH : (totalBytes - scsiBytesRead - chunk);\r
780 \r
781                                         if (nextChunk > 0) scsiSetDataCount(nextChunk);\r
782                                         scsiReadPIO(&scsiDev.data[scsiBytesRead], chunk);\r
783                                         scsiBytesRead += chunk;\r
784                                         chunk = nextChunk;\r
785                                 }\r
786 \r
787                                 HAL_SD_WriteBlocks_DMA(&hsd, (uint32_t*) (&scsiDev.data[0]), (i + sdLBA) * 512ll, SD_SECTOR_SIZE, sectors);\r
788 \r
789                                 while (scsiBytesRead < totalBytes)\r
790                                 {\r
791                                         while (!scsiPhyComplete() && likely(!scsiDev.resetFlag))\r
792                                         {\r
793                                                 __WFE(); // Wait for event\r
794                                         }\r
795                                         parityError |= scsiParityError();\r
796                                         scsiPhyFifoFlip();\r
797                                         uint32_t nextChunk = ((totalBytes - scsiBytesRead - chunk) > SCSI_FIFO_DEPTH)\r
798                                                 ? SCSI_FIFO_DEPTH : (totalBytes - scsiBytesRead - chunk);\r
799 \r
800                                         if (nextChunk > 0) scsiSetDataCount(nextChunk);\r
801                                         scsiReadPIO(&scsiDev.data[scsiBytesRead], chunk);\r
802                                         scsiBytesRead += chunk;\r
803                                         chunk = nextChunk;\r
804                                 }\r
805 \r
806                                 // Oh dear, SD finished first.\r
807                                 int underrun = totalBytes > readAheadBytes && hsd.DmaTransferCplt;\r
808 \r
809                                 uint32_t dmaFinishTime = s2s_getTime_ms();\r
810                                 while (!hsd.SdTransferCplt &&\r
811                                         s2s_elapsedTime_ms(dmaFinishTime) < 180)\r
812                                 {\r
813                                         // Wait while keeping BSY.\r
814                                 }\r
815                                 while((__HAL_SD_SDIO_GET_FLAG(&hsd, SDIO_FLAG_TXACT)) &&\r
816                                         s2s_elapsedTime_ms(dmaFinishTime) < 180)\r
817                                 {\r
818                                         // Wait for SD card while keeping BSY.\r
819                                 }\r
820 \r
821                                 if (i + sectors >= totalSDSectors &&\r
822                                         !underrun &&\r
823                                         (!parityError || !enableParity))\r
824                                 {\r
825                                         // We're transferring over the SCSI bus faster than the SD card\r
826                                         // can write.  All data is buffered, and we're just waiting for\r
827                                         // the SD card to complete. The host won't let us disconnect.\r
828                                         // Some drivers set a 250ms timeout on transfers to complete.\r
829                                         // SD card writes are supposed to complete\r
830                                         // within 200ms, but sometimes they don't.\r
831                                         // Just pretend we're finished.\r
832                                         process_Status();\r
833                                         clearBSY = process_MessageIn(0); // Will go to BUS_FREE state but keep BSY asserted.\r
834                                 }\r
835 \r
836                                 HAL_SD_CheckWriteOperation(&hsd, (uint32_t)SD_DATATIMEOUT);\r
837 \r
838                                 if (underrun)\r
839                                 {\r
840                                         // Try again. Data is still in memory.\r
841                                         sdTmpWrite(&scsiDev.data[0], i + sdLBA, sectors);\r
842                                         scsiDev.sdUnderrunCount++;\r
843                                 }\r
844                                 i += sectors;\r
845 \r
846                         }\r
847                         else\r
848                         {\r
849                                 // Well, until we have some proper non-blocking SD code, we must\r
850                                 // do this in a half-duplex fashion. We need to write as much as\r
851                                 // possible in each SD card transaction.\r
852                                 // use sg_dd from sg_utils3 tools to test.\r
853                                 uint32_t maxSectors = sizeof(scsiDev.data) / SD_SECTOR_SIZE;\r
854                                 uint32_t rem = totalSDSectors - i;\r
855                                 uint32_t sectors = rem < maxSectors ? rem : maxSectors;\r
856                                 int scsiSector;\r
857                                 for (scsiSector = i; scsiSector < i + sectors; ++scsiSector)\r
858                                 {\r
859                                         int dmaBytes = SD_SECTOR_SIZE;\r
860                                         if ((scsiSector % sdPerScsi) == (sdPerScsi - 1))\r
861                                         {\r
862                                                 dmaBytes = bytesPerSector % SD_SECTOR_SIZE;\r
863                                                 if (dmaBytes == 0) dmaBytes = SD_SECTOR_SIZE;\r
864                                         }\r
865                                         scsiRead(&scsiDev.data[SD_SECTOR_SIZE * (scsiSector - i)], dmaBytes, &parityError);\r
866                                 }\r
867                                 if (!parityError)\r
868                                 {\r
869                                         sdTmpWrite(&scsiDev.data[0], i + sdLBA, sectors);\r
870                                 }\r
871                                 i += sectors;\r
872                         }\r
873                 }\r
874 \r
875                 if (clearBSY)\r
876                 {\r
877                         enter_BusFree();\r
878                 }\r
879 \r
880                 if (scsiDev.phase == DATA_OUT)\r
881                 {\r
882                         if (parityError &&\r
883                                 (scsiDev.boardCfg.flags & S2S_CFG_ENABLE_PARITY))\r
884                         {\r
885                                 scsiDev.target->sense.code = ABORTED_COMMAND;\r
886                                 scsiDev.target->sense.asc = SCSI_PARITY_ERROR;\r
887                                 scsiDev.status = CHECK_CONDITION;;\r
888                         }\r
889                         scsiDev.phase = STATUS;\r
890                 }\r
891                 scsiDiskReset();\r
892         }\r
893 }\r
894 \r
895 void scsiDiskReset()\r
896 {\r
897         scsiDev.dataPtr = 0;\r
898         scsiDev.savedDataPtr = 0;\r
899         scsiDev.dataLen = 0;\r
900         // transfer.lba = 0; // Needed in Request Sense to determine failure\r
901         transfer.blocks = 0;\r
902         transfer.currentBlock = 0;\r
903 \r
904         // Cancel long running commands!\r
905 #if 0\r
906         if (\r
907                 ((scsiDev.boardCfg.flags & S2S_CFG_ENABLE_CACHE) == 0) ||\r
908                         (transfer.multiBlock == 0)\r
909                 )\r
910 #endif\r
911         {\r
912                 sdCompleteTransfer();\r
913         }\r
914 \r
915         transfer.multiBlock = 0;\r
916 }\r
917 \r
918 void scsiDiskInit()\r
919 {\r
920         scsiDiskReset();\r
921 \r
922         // Don't require the host to send us a START STOP UNIT command\r
923         blockDev.state = DISK_STARTED;\r
924 }\r
925 \r