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