Fix terrible performance
[SCSI2SD-V6.git] / src / firmware / scsiPhy.c
1 //      Copyright (C) 2013 Michael McMaster <michael@codesrc.com>\r
2 //\r
3 //      This file is part of SCSI2SD.\r
4 //\r
5 //      SCSI2SD is free software: you can redistribute it and/or modify\r
6 //      it under the terms of the GNU General Public License as published by\r
7 //      the Free Software Foundation, either version 3 of the License, or\r
8 //      (at your option) any later version.\r
9 //\r
10 //      SCSI2SD is distributed in the hope that it will be useful,\r
11 //      but WITHOUT ANY WARRANTY; without even the implied warranty of\r
12 //      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
13 //      GNU General Public License for more details.\r
14 //\r
15 //      You should have received a copy of the GNU General Public License\r
16 //      along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.\r
17 \r
18 #include "stm32f2xx.h"\r
19 #include "stm32f2xx_hal.h"\r
20 #include "stm32f2xx_hal_dma.h"\r
21 \r
22 #include "scsi.h"\r
23 #include "scsiPhy.h"\r
24 #include "trace.h"\r
25 #include "time.h"\r
26 #include "fpga.h"\r
27 #include "led.h"\r
28 \r
29 #include <string.h>\r
30 \r
31 // Private DMA variables.\r
32 static int dmaInProgress = 0;\r
33 \r
34 static DMA_HandleTypeDef memToFSMC;\r
35 static DMA_HandleTypeDef fsmcToMem;\r
36 \r
37 \r
38 volatile uint8_t scsiRxDMAComplete;\r
39 volatile uint8_t scsiTxDMAComplete;\r
40 \r
41 #if 0\r
42 CY_ISR_PROTO(scsiRxCompleteISR);\r
43 CY_ISR(scsiRxCompleteISR)\r
44 {\r
45         traceIrq(trace_scsiRxCompleteISR);\r
46         scsiRxDMAComplete = 1;\r
47 }\r
48 \r
49 CY_ISR_PROTO(scsiTxCompleteISR);\r
50 CY_ISR(scsiTxCompleteISR)\r
51 {\r
52         traceIrq(trace_scsiTxCompleteISR);\r
53         scsiTxDMAComplete = 1;\r
54 }\r
55 #endif\r
56 \r
57 uint8_t scsiPhyFifoSel = 0; // global\r
58 \r
59 // scsi IRQ handler is initialised by the STM32 HAL. Connected to\r
60 // PE4\r
61 // Note: naming is important to ensure this function is listed in the\r
62 // vector table.\r
63 void EXTI4_IRQHandler()\r
64 {\r
65         traceIrq(trace_scsiResetISR);\r
66 \r
67         // Make sure that interrupt flag is set\r
68         if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_4) != RESET) {\r
69 \r
70                 // Clear interrupt flag\r
71                 __HAL_GPIO_EXTI_CLEAR_IT(GPIO_PIN_4);\r
72 \r
73                 scsiDev.resetFlag = scsiDev.resetFlag || scsiStatusRST();\r
74                 // TODO grab SEL status as well\r
75 \r
76         }\r
77 }\r
78 \r
79 static void assertFail()\r
80 {\r
81         while (1)\r
82         {\r
83                 s2s_ledOn();\r
84                 s2s_delay_ms(100);\r
85                 s2s_ledOff();\r
86                 s2s_delay_ms(100);\r
87         }\r
88 }\r
89 \r
90 static void\r
91 startScsiRx(uint32_t count)\r
92 {\r
93         *SCSI_DATA_CNT_HI = count >> 8;\r
94         *SCSI_DATA_CNT_LO = count & 0xff;\r
95         *SCSI_DATA_CNT_SET = 1;\r
96 }\r
97 \r
98 uint8_t\r
99 scsiReadByte(void)\r
100 {\r
101 #if FIFODEBUG\r
102         if (!scsiPhyFifoAltEmpty()) {\r
103                 // Force a lock-up.\r
104                 assertFail();\r
105         }\r
106 #endif\r
107         startScsiRx(1);\r
108 \r
109         trace(trace_spinPhyRxFifo);\r
110         while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
111         scsiPhyFifoFlip();\r
112         uint8_t val = scsiPhyRx();\r
113         // TODO scsiDev.parityError = scsiDev.parityError || SCSI_Parity_Error_Read();\r
114 \r
115 #if FIFODEBUG\r
116         if (!scsiPhyFifoEmpty()) {\r
117                 int j = 0;\r
118                 uint8_t k __attribute((unused));\r
119                 while (!scsiPhyFifoEmpty()) { k = scsiPhyRx(); ++j; }\r
120 \r
121                 // Force a lock-up.\r
122                 assertFail();\r
123         }\r
124 #endif\r
125         return val;\r
126 }\r
127 \r
128 \r
129 static void\r
130 scsiReadPIO(uint8_t* data, uint32_t count)\r
131 {\r
132         for (int i = 0; i < count; ++i)\r
133         {\r
134                 data[i] = scsiPhyRx();\r
135         }\r
136         // TODO scsiDev.parityError = scsiDev.parityError || SCSI_Parity_Error_Read();\r
137 }\r
138 \r
139 void\r
140 scsiReadDMA(uint8_t* data, uint32_t count)\r
141 {\r
142         // Prepare DMA transfer\r
143         dmaInProgress = 1;\r
144         trace(trace_doRxSingleDMA);\r
145 \r
146         scsiTxDMAComplete = 1; // TODO not used much\r
147         scsiRxDMAComplete = 0; // TODO not used much\r
148 \r
149         HAL_DMA_Start(&fsmcToMem, (uint32_t) SCSI_FIFO_DATA, (uint32_t) data, count);\r
150 }\r
151 \r
152 int\r
153 scsiReadDMAPoll()\r
154 {\r
155         int complete = __HAL_DMA_GET_COUNTER(&fsmcToMem) == 0;\r
156         complete = complete && (HAL_DMA_PollForTransfer(&fsmcToMem, HAL_DMA_FULL_TRANSFER, 0xffffffff) == HAL_OK);\r
157         if (complete)\r
158         {\r
159                 scsiTxDMAComplete = 1; // TODO MM FIX IRQ\r
160                 scsiRxDMAComplete = 1;\r
161 \r
162                 dmaInProgress = 0;\r
163 #if 0\r
164                 // TODO MM scsiDev.parityError = scsiDev.parityError || SCSI_Parity_Error_Read();\r
165 #endif\r
166                 return 1;\r
167 \r
168         }\r
169         else\r
170         {\r
171                 return 0;\r
172         }\r
173 }\r
174 \r
175 void\r
176 scsiRead(uint8_t* data, uint32_t count)\r
177 {\r
178         int i = 0;\r
179 \r
180 \r
181         uint32_t chunk = ((count - i) > SCSI_FIFO_DEPTH)\r
182                 ? SCSI_FIFO_DEPTH : (count - i);\r
183         if (chunk >= 16)\r
184         {\r
185                 // DMA is doing 32bit transfers.\r
186                 chunk = chunk & 0xFFFFFFF8;\r
187         }\r
188         startScsiRx(chunk);\r
189 \r
190         while (i < count && likely(!scsiDev.resetFlag))\r
191         {\r
192                 while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
193                 scsiPhyFifoFlip();\r
194 \r
195                 uint32_t nextChunk = ((count - i - chunk) > SCSI_FIFO_DEPTH)\r
196                         ? SCSI_FIFO_DEPTH : (count - i - chunk);\r
197                 if (nextChunk >= 16)\r
198                 {\r
199                         nextChunk = nextChunk & 0xFFFFFFF8;\r
200                 }\r
201                 if (nextChunk > 0)\r
202                 {\r
203                         startScsiRx(nextChunk);\r
204                 }\r
205 \r
206                 if (chunk < 16)\r
207                 {\r
208                         scsiReadPIO(data + i, chunk);\r
209                 }\r
210                 else\r
211                 {\r
212                         scsiReadDMA(data + i, chunk);\r
213 \r
214                         trace(trace_spinReadDMAPoll);\r
215 \r
216                         while (!scsiReadDMAPoll() && likely(!scsiDev.resetFlag))\r
217                         {\r
218                         };\r
219                 }\r
220 \r
221 #if FIFODEBUG\r
222                 if (!scsiPhyFifoEmpty()) {\r
223                         int j = 0;\r
224                         while (!scsiPhyFifoEmpty()) { scsiPhyRx(); ++j; }\r
225                         // Force a lock-up.\r
226                         assertFail();\r
227                 }\r
228 #endif\r
229                 i += chunk;\r
230                 chunk = nextChunk;\r
231         }\r
232 }\r
233 \r
234 void\r
235 scsiWriteByte(uint8_t value)\r
236 {\r
237 #if FIFODEBUG\r
238         if (!scsiPhyFifoEmpty()) {\r
239                 // Force a lock-up.\r
240                 assertFail();\r
241         }\r
242 #endif\r
243         trace(trace_spinPhyTxFifo);\r
244         scsiPhyTx(value);\r
245         scsiPhyFifoFlip();\r
246 \r
247         trace(trace_spinTxComplete);\r
248         while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
249 \r
250 #if FIFODEBUG\r
251         if (!scsiPhyFifoAltEmpty()) {\r
252                 // Force a lock-up.\r
253                 assertFail();\r
254         }\r
255 #endif\r
256 }\r
257 \r
258 static void\r
259 scsiWritePIO(const uint8_t* data, uint32_t count)\r
260 {\r
261         for (int i = 0; i < count; ++i)\r
262         {\r
263                 scsiPhyTx(data[i]);\r
264         }\r
265 }\r
266 \r
267 void\r
268 scsiWriteDMA(const uint8_t* data, uint32_t count)\r
269 {\r
270         // Prepare DMA transfer\r
271         dmaInProgress = 1;\r
272         trace(trace_doTxSingleDMA);\r
273 \r
274         scsiTxDMAComplete = 0;\r
275         scsiRxDMAComplete = 1;\r
276 \r
277         HAL_DMA_Start(\r
278                 &memToFSMC,\r
279                 (uint32_t) data,\r
280                 (uint32_t) SCSI_FIFO_DATA,\r
281                 count / 4);\r
282 }\r
283 \r
284 int\r
285 scsiWriteDMAPoll()\r
286 {\r
287         int complete = __HAL_DMA_GET_COUNTER(&memToFSMC) == 0;\r
288         complete = complete && (HAL_DMA_PollForTransfer(&memToFSMC, HAL_DMA_FULL_TRANSFER, 0xffffffff) == HAL_OK);\r
289         if (complete)\r
290         {\r
291                 scsiTxDMAComplete = 1; // TODO MM FIX IRQ\r
292                 scsiRxDMAComplete = 1;\r
293 \r
294                 dmaInProgress = 0;\r
295                 return 1;\r
296         }\r
297         else\r
298         {\r
299                 return 0;\r
300         }\r
301 }\r
302 \r
303 void\r
304 scsiWrite(const uint8_t* data, uint32_t count)\r
305 {\r
306         int i = 0;\r
307         while (i < count && likely(!scsiDev.resetFlag))\r
308         {\r
309                 uint32_t chunk = ((count - i) > SCSI_FIFO_DEPTH)\r
310                         ? SCSI_FIFO_DEPTH : (count - i);\r
311 \r
312 #if FIFODEBUG\r
313                 if (!scsiPhyFifoEmpty()) {\r
314                         // Force a lock-up.\r
315                         assertFail();\r
316                 }\r
317 #endif\r
318 \r
319                 if (chunk < 16)\r
320                 {\r
321                         scsiWritePIO(data + i, chunk);\r
322                 }\r
323                 else\r
324                 {\r
325                         // DMA is doing 32bit transfers.\r
326                         chunk = chunk & 0xFFFFFFF8;\r
327                         scsiWriteDMA(data + i, chunk);\r
328 \r
329                         trace(trace_spinReadDMAPoll);\r
330 \r
331                         while (!scsiWriteDMAPoll() && likely(!scsiDev.resetFlag))\r
332                         {\r
333                         }\r
334                 }\r
335 \r
336                 while (!scsiPhyComplete() && likely(!scsiDev.resetFlag))\r
337                 {\r
338                 }\r
339 \r
340 #if FIFODEBUG\r
341                 if (!scsiPhyFifoAltEmpty()) {\r
342                         // Force a lock-up.\r
343                         assertFail();\r
344                 }\r
345 #endif\r
346 \r
347                 scsiPhyFifoFlip();\r
348                 i += chunk;\r
349         }\r
350         while (!scsiPhyComplete() && likely(!scsiDev.resetFlag))\r
351         {\r
352         }\r
353 \r
354 #if FIFODEBUG\r
355         if (!scsiPhyFifoAltEmpty()) {\r
356                 // Force a lock-up.\r
357                 assertFail();\r
358         }\r
359 #endif\r
360 }\r
361 \r
362 static inline void busSettleDelay(void)\r
363 {\r
364         // Data Release time (switching IO) = 400ns\r
365         // + Bus Settle time (switching phase) = 400ns.\r
366         s2s_delay_us(1); // Close enough.\r
367 }\r
368 \r
369 void scsiEnterBusFree()\r
370 {\r
371         *SCSI_CTRL_BSY = 0x00;\r
372         // We now have a Bus Clear Delay of 800ns to release remaining signals.\r
373         *SCSI_CTRL_PHASE = 0;\r
374 }\r
375 \r
376 void scsiEnterPhase(int phase)\r
377 {\r
378         // ANSI INCITS 362-2002 SPI-3 10.7.1:\r
379         // Phase changes are not allowed while REQ or ACK is asserted.\r
380         while (likely(!scsiDev.resetFlag) && scsiStatusACK()) {}\r
381 \r
382         int newPhase = phase > 0 ? phase : 0;\r
383         int oldPhase = *SCSI_CTRL_PHASE;\r
384 \r
385         if (!scsiPhyFifoEmpty() || !scsiPhyFifoAltEmpty()) {\r
386                 // Force a lock-up.\r
387                 assertFail();\r
388         }\r
389         if (newPhase != oldPhase)\r
390         {\r
391                 *SCSI_CTRL_PHASE = newPhase;\r
392                 busSettleDelay();\r
393 \r
394                 if (scsiDev.compatMode < COMPAT_SCSI2)\r
395                 {\r
396                         s2s_delay_us(100);\r
397                 }\r
398 \r
399         }\r
400 }\r
401 \r
402 void scsiPhyReset()\r
403 {\r
404         trace(trace_scsiPhyReset);\r
405         if (dmaInProgress)\r
406         {\r
407                 trace(trace_spinDMAReset);\r
408                 HAL_DMA_Abort(&memToFSMC);\r
409                 HAL_DMA_Abort(&fsmcToMem);\r
410 \r
411                 dmaInProgress = 0;\r
412         }\r
413 #if 0\r
414 \r
415         // Set the Clear bits for both SCSI device FIFOs\r
416         scsiTarget_AUX_CTL = scsiTarget_AUX_CTL | 0x03;\r
417 \r
418         // Trigger RST outselves.  It is connected to the datapath and will\r
419         // ensure it returns to the idle state.  The datapath runs at the BUS clk\r
420         // speed (ie. same as the CPU), so we can be sure it is active for a sufficient\r
421         // duration.\r
422         SCSI_RST_ISR_Disable();\r
423         SCSI_SetPin(SCSI_Out_RST);\r
424 \r
425         SCSI_CTL_PHASE_Write(0);\r
426         SCSI_ClearPin(SCSI_Out_ATN);\r
427         SCSI_ClearPin(SCSI_Out_BSY);\r
428         SCSI_ClearPin(SCSI_Out_ACK);\r
429         SCSI_ClearPin(SCSI_Out_RST);\r
430         SCSI_ClearPin(SCSI_Out_SEL);\r
431         SCSI_ClearPin(SCSI_Out_REQ);\r
432 \r
433         // Allow the FIFOs to fill up again.\r
434         SCSI_ClearPin(SCSI_Out_RST);\r
435         SCSI_RST_ISR_Enable();\r
436         scsiTarget_AUX_CTL = scsiTarget_AUX_CTL & ~(0x03);\r
437 \r
438         SCSI_Parity_Error_Read(); // clear sticky bits\r
439 #endif\r
440 \r
441         *SCSI_CTRL_PHASE = 0x00;\r
442         *SCSI_CTRL_BSY = 0x00;\r
443         s2s_fpgaReset(); // Clears fifos etc.\r
444 \r
445         scsiPhyFifoSel = 0;\r
446         *SCSI_FIFO_SEL = 0;\r
447         *SCSI_CTRL_DBX = 0;\r
448 \r
449         // DMA Benchmark code\r
450         // Currently 10MB/s. Assume 20MB/s is achievable with 16 bits.\r
451         #ifdef DMA_BENCHMARK\r
452         while(1)\r
453         {\r
454                 s2s_ledOn();\r
455                 // 100MB\r
456                 for (int i = 0; i < (100LL * 1024 * 1024 / SCSI_FIFO_DEPTH); ++i)\r
457                 {\r
458                         HAL_DMA_Start(\r
459                                 &memToFSMC,\r
460                                 (uint32_t) &scsiDev.data[0],\r
461                                 (uint32_t) SCSI_FIFO_DATA,\r
462                                 SCSI_FIFO_DEPTH / 4);\r
463 \r
464                         HAL_DMA_PollForTransfer(\r
465                                 &memToFSMC,\r
466                                 HAL_DMA_FULL_TRANSFER,\r
467                                 0xffffffff);\r
468 \r
469                         s2s_fpgaReset();\r
470                 }\r
471                 s2s_ledOff();\r
472 \r
473                 for(int i = 0; i < 10; ++i) s2s_delay_ms(1000);\r
474         }\r
475         #endif\r
476 \r
477         // FPGA comms test code\r
478         #ifdef FPGA_TEST\r
479         while(1)\r
480         {\r
481                 for (int j = 0; j < SCSI_FIFO_DEPTH; ++j)\r
482                 {\r
483                         scsiDev.data[j] = j;\r
484                 }\r
485 \r
486                 if (!scsiPhyFifoEmpty())\r
487                 {\r
488                         assertFail();\r
489                 }\r
490 \r
491                 *SCSI_CTRL_PHASE = DATA_IN;\r
492                 HAL_DMA_Start(\r
493                         &memToFSMC,\r
494                         (uint32_t) &scsiDev.data[0],\r
495                         (uint32_t) SCSI_FIFO_DATA,\r
496                         SCSI_FIFO_DEPTH / 4);\r
497 \r
498                 HAL_DMA_PollForTransfer(\r
499                         &memToFSMC,\r
500                         HAL_DMA_FULL_TRANSFER,\r
501                         0xffffffff);\r
502 \r
503                 if (!scsiPhyFifoFull())\r
504                 {\r
505                         assertFail();\r
506                 }\r
507 \r
508                 memset(&scsiDev.data[0], 0, SCSI_FIFO_DEPTH);\r
509 \r
510                 *SCSI_CTRL_PHASE = DATA_OUT;\r
511                 HAL_DMA_Start(\r
512                         &fsmcToMem,\r
513                         (uint32_t) SCSI_FIFO_DATA,\r
514                         (uint32_t) &scsiDev.data[0],\r
515                         SCSI_FIFO_DEPTH);\r
516 \r
517                 HAL_DMA_PollForTransfer(\r
518                         &fsmcToMem,\r
519                         HAL_DMA_FULL_TRANSFER,\r
520                         0xffffffff);\r
521 \r
522                 if (!scsiPhyFifoEmpty())\r
523                 {\r
524                         assertFail();\r
525                 }\r
526 \r
527 \r
528                 for (int j = 0; j < SCSI_FIFO_DEPTH; ++j)\r
529                 {\r
530                         if (scsiDev.data[j] != (uint8_t) j)\r
531                         {\r
532                                 assertFail();\r
533                         }\r
534                 }\r
535 \r
536                 s2s_fpgaReset();\r
537 \r
538         }\r
539         #endif\r
540 \r
541 }\r
542 \r
543 static void scsiPhyInitDMA()\r
544 {\r
545         // One-time init only.\r
546         static uint8_t init = 0;\r
547         if (init == 0)\r
548         {\r
549                 init = 1;\r
550 \r
551                 // Memory to memory transfers can only be done using DMA2\r
552                 __DMA2_CLK_ENABLE();\r
553 \r
554                 // Transmit SCSI data. The source data is treated as the\r
555                 // peripheral (even though this is memory-to-memory)\r
556                 memToFSMC.Instance = DMA2_Stream0;\r
557                 memToFSMC.Init.Channel = DMA_CHANNEL_0;\r
558                 memToFSMC.Init.Direction = DMA_MEMORY_TO_MEMORY;\r
559                 memToFSMC.Init.PeriphInc = DMA_PINC_ENABLE;\r
560                 memToFSMC.Init.MemInc = DMA_MINC_DISABLE;\r
561                 memToFSMC.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;\r
562                 memToFSMC.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;\r
563                 memToFSMC.Init.Mode = DMA_NORMAL;\r
564                 memToFSMC.Init.Priority = DMA_PRIORITY_LOW;\r
565                 // FIFO mode is needed to allow conversion from 32bit words to the\r
566                 // 8bit FSMC interface.\r
567                 memToFSMC.Init.FIFOMode = DMA_FIFOMODE_ENABLE;\r
568 \r
569                 // We only use 1 word (4 bytes) in the fifo at a time. Normally it's\r
570                 // better to let the DMA fifo fill up then do burst transfers, but\r
571                 // bursting out the FSMC interface will be very slow and may starve\r
572                 // other (faster) transfers. We don't want to risk the SDIO transfers\r
573                 // from overrun/underrun conditions.\r
574                 memToFSMC.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;\r
575                 memToFSMC.Init.MemBurst = DMA_MBURST_SINGLE;\r
576                 memToFSMC.Init.PeriphBurst = DMA_PBURST_SINGLE;\r
577                 HAL_DMA_Init(&memToFSMC);\r
578 \r
579                 // Receive SCSI data. The source data (fsmc) is treated as the\r
580                 // peripheral (even though this is memory-to-memory)\r
581                 fsmcToMem.Instance = DMA2_Stream1;\r
582                 fsmcToMem.Init.Channel = DMA_CHANNEL_0;\r
583                 fsmcToMem.Init.Direction = DMA_MEMORY_TO_MEMORY;\r
584                 fsmcToMem.Init.PeriphInc = DMA_PINC_DISABLE;\r
585                 fsmcToMem.Init.MemInc = DMA_MINC_ENABLE;\r
586                 fsmcToMem.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;\r
587                 fsmcToMem.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;\r
588                 fsmcToMem.Init.Mode = DMA_NORMAL;\r
589                 fsmcToMem.Init.Priority = DMA_PRIORITY_LOW;\r
590                 fsmcToMem.Init.FIFOMode = DMA_FIFOMODE_ENABLE;\r
591                 fsmcToMem.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;\r
592                 fsmcToMem.Init.MemBurst = DMA_MBURST_SINGLE;\r
593                 fsmcToMem.Init.PeriphBurst = DMA_PBURST_SINGLE;\r
594                 HAL_DMA_Init(&fsmcToMem);\r
595 \r
596                 // TODO configure IRQs\r
597         }\r
598 }\r
599 \r
600 \r
601 void scsiPhyInit()\r
602 {\r
603         scsiPhyInitDMA();\r
604 \r
605         *SCSI_CTRL_IDMASK = 0x00; // Reset in scsiPhyConfig\r
606         *SCSI_CTRL_PHASE = 0x00;\r
607         *SCSI_CTRL_BSY = 0x00;\r
608         scsiPhyFifoSel = 0;\r
609         *SCSI_FIFO_SEL = 0;\r
610         *SCSI_CTRL_DBX = 0;\r
611 \r
612 }\r
613 \r
614 void scsiPhyConfig()\r
615 {\r
616         if (scsiDev.boardCfg.flags6 & S2S_CFG_ENABLE_TERMINATOR)\r
617         {\r
618                 HAL_GPIO_WritePin(nTERM_EN_GPIO_Port, nTERM_EN_Pin, GPIO_PIN_RESET);\r
619         }\r
620         else\r
621         {\r
622                 HAL_GPIO_WritePin(nTERM_EN_GPIO_Port, nTERM_EN_Pin, GPIO_PIN_SET);\r
623         }\r
624 \r
625 \r
626         uint8_t idMask = 0;\r
627         for (int i = 0; i < 8; ++i)\r
628         {\r
629                 const S2S_TargetCfg* cfg = s2s_getConfigById(i);\r
630                 if (cfg && (cfg->scsiId & S2S_CFG_TARGET_ENABLED))\r
631                 {\r
632                         idMask |= (1 << i);\r
633                 }\r
634         }\r
635         *SCSI_CTRL_IDMASK = idMask;\r
636 }\r
637 \r
638 \r
639 // 1 = DBx error\r
640 // 2 = Parity error\r
641 // 4 = MSG error\r
642 // 8 = CD error\r
643 // 16 = IO error\r
644 // 32 = other error\r
645 int scsiSelfTest()\r
646 {\r
647         if (scsiDev.phase != BUS_FREE)\r
648         {\r
649                 return 32;\r
650         }\r
651 \r
652         // Acquire the SCSI bus.\r
653         for (int i = 0; i < 100; ++i)\r
654         {\r
655                 if (scsiStatusBSY())\r
656                 {\r
657                         s2s_delay_ms(1);\r
658                 }\r
659         }\r
660         if (scsiStatusBSY())\r
661         {\r
662                 // Error, couldn't acquire scsi bus\r
663                 return 32;\r
664         }\r
665         *SCSI_CTRL_BSY = 1;\r
666         if (! scsiStatusBSY())\r
667         {\r
668                 // Error, BSY doesn't work.\r
669                 return 32;\r
670         }\r
671 \r
672         // Should be safe to use the bus now.\r
673 \r
674         int result = 0;\r
675 \r
676         // TEST DBx\r
677         // TODO test DBp\r
678         int i;\r
679         for (i = 0; i < 256; ++i)\r
680         {\r
681                 *SCSI_CTRL_DBX = i;\r
682                 busSettleDelay();\r
683                 if (*SCSI_STS_DBX != (i & 0xff))\r
684                 {\r
685                         result |= 1;\r
686                 }\r
687                 /*if (Lookup_OddParity[i & 0xff] != SCSI_ReadPin(SCSI_In_DBP))\r
688                 {\r
689                         result |= 2;\r
690                 }*/\r
691         }\r
692         *SCSI_CTRL_DBX = 0;\r
693 \r
694         // TEST MSG, CD, IO\r
695         /* TODO\r
696         for (i = 0; i < 8; ++i)\r
697         {\r
698                 SCSI_CTL_PHASE_Write(i);\r
699                 scsiDeskewDelay();\r
700 \r
701                 if (SCSI_ReadPin(SCSI_In_MSG) != !!(i & __scsiphase_msg))\r
702                 {\r
703                         result |= 4;\r
704                 }\r
705                 if (SCSI_ReadPin(SCSI_In_CD) != !!(i & __scsiphase_cd))\r
706                 {\r
707                         result |= 8;\r
708                 }\r
709                 if (SCSI_ReadPin(SCSI_In_IO) != !!(i & __scsiphase_io))\r
710                 {\r
711                         result |= 16;\r
712                 }\r
713         }\r
714         SCSI_CTL_PHASE_Write(0);\r
715 \r
716         uint32_t signalsOut[] = { SCSI_Out_ATN, SCSI_Out_BSY, SCSI_Out_RST, SCSI_Out_SEL };\r
717         uint32_t signalsIn[] = { SCSI_Filt_ATN, SCSI_Filt_BSY, SCSI_Filt_RST, SCSI_Filt_SEL };\r
718 \r
719         for (i = 0; i < 4; ++i)\r
720         {\r
721                 SCSI_SetPin(signalsOut[i]);\r
722                 scsiDeskewDelay();\r
723 \r
724                 int j;\r
725                 for (j = 0; j < 4; ++j)\r
726                 {\r
727                         if (i == j)\r
728                         {\r
729                                 if (! SCSI_ReadFilt(signalsIn[j]))\r
730                                 {\r
731                                         result |= 32;\r
732                                 }\r
733                         }\r
734                         else\r
735                         {\r
736                                 if (SCSI_ReadFilt(signalsIn[j]))\r
737                                 {\r
738                                         result |= 32;\r
739                                 }\r
740                         }\r
741                 }\r
742                 SCSI_ClearPin(signalsOut[i]);\r
743         }\r
744         */\r
745 \r
746         *SCSI_CTRL_BSY = 0;\r
747         return result;\r
748 }\r
749 \r