Fixed SCSI issues. It should now be stable on all systems.
[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         while (i < count && likely(!scsiDev.resetFlag))\r
180         {\r
181                 uint32_t chunk = ((count - i) > SCSI_FIFO_DEPTH)\r
182                         ? SCSI_FIFO_DEPTH : (count - i);\r
183 \r
184                 if (chunk >= 16)\r
185                 {\r
186                         // DMA is doing 32bit transfers.\r
187                         chunk = chunk & 0xFFFFFFF8;\r
188                 }\r
189 \r
190 #if FIFODEBUG\r
191                 if (!scsiPhyFifoAltEmpty()) {\r
192                         // Force a lock-up.\r
193                         assertFail();\r
194                 }\r
195 #endif\r
196 \r
197                 startScsiRx(chunk);\r
198                 // Wait for the next scsi interrupt (or the 1ms systick)\r
199                 __WFI();\r
200 \r
201                 while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
202                 scsiPhyFifoFlip();\r
203 \r
204                 if (chunk < 16)\r
205                 {\r
206                         scsiReadPIO(data + i, chunk);\r
207                 }\r
208                 else\r
209                 {\r
210                         scsiReadDMA(data + i, chunk);\r
211 \r
212                         // Wait for the next DMA interrupt (or the 1ms systick)\r
213                         // It's beneficial to halt the processor to\r
214                         // give the DMA controller more memory bandwidth to work with.\r
215 #if 0\r
216                         __WFI();\r
217 #endif\r
218                         trace(trace_spinReadDMAPoll);\r
219 \r
220                         while (!scsiReadDMAPoll() && likely(!scsiDev.resetFlag))\r
221                         {\r
222 #if 0\r
223 // TODO NEED SCSI DMA IRQs\r
224                         __WFI();\r
225 #endif\r
226                         };\r
227                 }\r
228 \r
229 #if FIFODEBUG\r
230                 if (!scsiPhyFifoEmpty()) {\r
231                         int j = 0;\r
232                         while (!scsiPhyFifoEmpty()) { scsiPhyRx(); ++j; }\r
233                         // Force a lock-up.\r
234                         assertFail();\r
235                 }\r
236 #endif\r
237                 i += chunk;\r
238         }\r
239 }\r
240 \r
241 void\r
242 scsiWriteByte(uint8_t value)\r
243 {\r
244 #if FIFODEBUG\r
245         if (!scsiPhyFifoEmpty()) {\r
246                 // Force a lock-up.\r
247                 assertFail();\r
248         }\r
249 #endif\r
250         trace(trace_spinPhyTxFifo);\r
251         scsiPhyTx(value);\r
252         scsiPhyFifoFlip();\r
253 \r
254         trace(trace_spinTxComplete);\r
255         while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
256 \r
257 #if FIFODEBUG\r
258         if (!scsiPhyFifoAltEmpty()) {\r
259                 // Force a lock-up.\r
260                 assertFail();\r
261         }\r
262 #endif\r
263 }\r
264 \r
265 static void\r
266 scsiWritePIO(const uint8_t* data, uint32_t count)\r
267 {\r
268         for (int i = 0; i < count; ++i)\r
269         {\r
270                 scsiPhyTx(data[i]);\r
271         }\r
272 }\r
273 \r
274 void\r
275 scsiWriteDMA(const uint8_t* data, uint32_t count)\r
276 {\r
277         // Prepare DMA transfer\r
278         dmaInProgress = 1;\r
279         trace(trace_doTxSingleDMA);\r
280 \r
281         scsiTxDMAComplete = 0;\r
282         scsiRxDMAComplete = 1;\r
283 \r
284         HAL_DMA_Start(\r
285                 &memToFSMC,\r
286                 (uint32_t) data,\r
287                 (uint32_t) SCSI_FIFO_DATA,\r
288                 count / 4);\r
289 }\r
290 \r
291 int\r
292 scsiWriteDMAPoll()\r
293 {\r
294         int complete = __HAL_DMA_GET_COUNTER(&memToFSMC) == 0;\r
295         complete = complete && (HAL_DMA_PollForTransfer(&memToFSMC, HAL_DMA_FULL_TRANSFER, 0xffffffff) == HAL_OK);\r
296         if (complete)\r
297         {\r
298                 scsiTxDMAComplete = 1; // TODO MM FIX IRQ\r
299                 scsiRxDMAComplete = 1;\r
300 \r
301                 dmaInProgress = 0;\r
302                 return 1;\r
303         }\r
304         else\r
305         {\r
306                 return 0;\r
307         }\r
308 }\r
309 \r
310 void\r
311 scsiWrite(const uint8_t* data, uint32_t count)\r
312 {\r
313         int i = 0;\r
314         while (i < count && likely(!scsiDev.resetFlag))\r
315         {\r
316                 uint32_t chunk = ((count - i) > SCSI_FIFO_DEPTH)\r
317                         ? SCSI_FIFO_DEPTH : (count - i);\r
318 \r
319 #if FIFODEBUG\r
320                 if (!scsiPhyFifoEmpty()) {\r
321                         // Force a lock-up.\r
322                         assertFail();\r
323                 }\r
324 #endif\r
325 \r
326                 if (chunk < 16)\r
327                 {\r
328                         scsiWritePIO(data + i, chunk);\r
329                 }\r
330                 else\r
331                 {\r
332                         // DMA is doing 32bit transfers.\r
333                         chunk = chunk & 0xFFFFFFF8;\r
334                         scsiWriteDMA(data + i, chunk);\r
335 \r
336                         // Wait for the next DMA interrupt (or the 1ms systick)\r
337                         // It's beneficial to halt the processor to\r
338                         // give the DMA controller more memory bandwidth to work with.\r
339 #if 0\r
340                         __WFI();\r
341 #endif\r
342                         trace(trace_spinReadDMAPoll);\r
343 \r
344                         while (!scsiWriteDMAPoll() && likely(!scsiDev.resetFlag))\r
345                         {\r
346 #if 0\r
347 // TODO NEED SCSI DMA IRQs\r
348                         __WFI();\r
349 #endif\r
350                         };\r
351                 }\r
352 \r
353                 while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
354 \r
355 #if FIFODEBUG\r
356                 if (!scsiPhyFifoAltEmpty()) {\r
357                         // Force a lock-up.\r
358                         assertFail();\r
359                 }\r
360 #endif\r
361 \r
362                 scsiPhyFifoFlip();\r
363 #if 0\r
364 // TODO NEED SCSI IRQs\r
365                         __WFI();\r
366 #endif\r
367 \r
368                 i += chunk;\r
369         }\r
370         while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
371 \r
372 #if FIFODEBUG\r
373         if (!scsiPhyFifoAltEmpty()) {\r
374                 // Force a lock-up.\r
375                 assertFail();\r
376         }\r
377 #endif\r
378 }\r
379 \r
380 static inline void busSettleDelay(void)\r
381 {\r
382         // Data Release time (switching IO) = 400ns\r
383         // + Bus Settle time (switching phase) = 400ns.\r
384         s2s_delay_us(1); // Close enough.\r
385 }\r
386 \r
387 void scsiEnterBusFree()\r
388 {\r
389         *SCSI_CTRL_BSY = 0x00;\r
390         // We now have a Bus Clear Delay of 800ns to release remaining signals.\r
391         *SCSI_CTRL_PHASE = 0;\r
392 }\r
393 \r
394 void scsiEnterPhase(int phase)\r
395 {\r
396         // ANSI INCITS 362-2002 SPI-3 10.7.1:\r
397         // Phase changes are not allowed while REQ or ACK is asserted.\r
398         while (likely(!scsiDev.resetFlag) && scsiStatusACK()) {}\r
399 \r
400         int newPhase = phase > 0 ? phase : 0;\r
401         int oldPhase = *SCSI_CTRL_PHASE;\r
402 \r
403         if (!scsiPhyFifoEmpty() || !scsiPhyFifoAltEmpty()) {\r
404                 // Force a lock-up.\r
405                 assertFail();\r
406         }\r
407         if (newPhase != oldPhase)\r
408         {\r
409                 *SCSI_CTRL_PHASE = newPhase;\r
410                 busSettleDelay();\r
411 \r
412                 if (scsiDev.compatMode < COMPAT_SCSI2)\r
413                 {\r
414                         s2s_delay_us(100);\r
415                 }\r
416 \r
417         }\r
418 }\r
419 \r
420 void scsiPhyReset()\r
421 {\r
422         trace(trace_scsiPhyReset);\r
423         if (dmaInProgress)\r
424         {\r
425                 trace(trace_spinDMAReset);\r
426                 HAL_DMA_Abort(&memToFSMC);\r
427                 HAL_DMA_Abort(&fsmcToMem);\r
428 \r
429                 dmaInProgress = 0;\r
430         }\r
431 #if 0\r
432 \r
433         // Set the Clear bits for both SCSI device FIFOs\r
434         scsiTarget_AUX_CTL = scsiTarget_AUX_CTL | 0x03;\r
435 \r
436         // Trigger RST outselves.  It is connected to the datapath and will\r
437         // ensure it returns to the idle state.  The datapath runs at the BUS clk\r
438         // speed (ie. same as the CPU), so we can be sure it is active for a sufficient\r
439         // duration.\r
440         SCSI_RST_ISR_Disable();\r
441         SCSI_SetPin(SCSI_Out_RST);\r
442 \r
443         SCSI_CTL_PHASE_Write(0);\r
444         SCSI_ClearPin(SCSI_Out_ATN);\r
445         SCSI_ClearPin(SCSI_Out_BSY);\r
446         SCSI_ClearPin(SCSI_Out_ACK);\r
447         SCSI_ClearPin(SCSI_Out_RST);\r
448         SCSI_ClearPin(SCSI_Out_SEL);\r
449         SCSI_ClearPin(SCSI_Out_REQ);\r
450 \r
451         // Allow the FIFOs to fill up again.\r
452         SCSI_ClearPin(SCSI_Out_RST);\r
453         SCSI_RST_ISR_Enable();\r
454         scsiTarget_AUX_CTL = scsiTarget_AUX_CTL & ~(0x03);\r
455 \r
456         SCSI_Parity_Error_Read(); // clear sticky bits\r
457 #endif\r
458 \r
459         *SCSI_CTRL_PHASE = 0x00;\r
460         *SCSI_CTRL_BSY = 0x00;\r
461         s2s_fpgaReset(); // Clears fifos etc.\r
462 \r
463         scsiPhyFifoSel = 0;\r
464         *SCSI_FIFO_SEL = 0;\r
465 \r
466         // DMA Benchmark code\r
467         // Currently 10MB/s. Assume 20MB/s is achievable with 16 bits.\r
468         #ifdef DMA_BENCHMARK\r
469         while(1)\r
470         {\r
471                 s2s_ledOn();\r
472                 // 100MB\r
473                 for (int i = 0; i < (100LL * 1024 * 1024 / SCSI_FIFO_DEPTH); ++i)\r
474                 {\r
475                         HAL_DMA_Start(\r
476                                 &memToFSMC,\r
477                                 (uint32_t) &scsiDev.data[0],\r
478                                 (uint32_t) SCSI_FIFO_DATA,\r
479                                 SCSI_FIFO_DEPTH / 4);\r
480 \r
481                         HAL_DMA_PollForTransfer(\r
482                                 &memToFSMC,\r
483                                 HAL_DMA_FULL_TRANSFER,\r
484                                 0xffffffff);\r
485 \r
486                         s2s_fpgaReset();\r
487                 }\r
488                 s2s_ledOff();\r
489 \r
490                 for(int i = 0; i < 10; ++i) s2s_delay_ms(1000);\r
491         }\r
492         #endif\r
493 \r
494         // FPGA comms test code\r
495         #ifdef FPGA_TEST\r
496 \r
497         while(1)\r
498         {\r
499                 for (int j = 0; j < SCSI_FIFO_DEPTH; ++j)\r
500                 {\r
501                         scsiDev.data[j] = j;\r
502                 }\r
503 \r
504                 *SCSI_CTRL_PHASE = DATA_IN;\r
505                 HAL_DMA_Start(\r
506                         &memToFSMC,\r
507                         (uint32_t) &scsiDev.data[0],\r
508                         (uint32_t) SCSI_FIFO_DATA,\r
509                         SCSI_FIFO_DEPTH / 4);\r
510 \r
511                 HAL_DMA_PollForTransfer(\r
512                         &memToFSMC,\r
513                         HAL_DMA_FULL_TRANSFER,\r
514                         0xffffffff);\r
515 \r
516                 memset(&scsiDev.data[0], 0, SCSI_FIFO_DEPTH);\r
517 \r
518                 *SCSI_CTRL_PHASE = DATA_OUT;\r
519                 HAL_DMA_Start(\r
520                         &fsmcToMem,\r
521                         (uint32_t) SCSI_FIFO_DATA,\r
522                         (uint32_t) &scsiDev.data[0],\r
523                         SCSI_FIFO_DEPTH);\r
524 \r
525                 HAL_DMA_PollForTransfer(\r
526                         &fsmcToMem,\r
527                         HAL_DMA_FULL_TRANSFER,\r
528                         0xffffffff);\r
529 \r
530                 for (int j = 0; j < SCSI_FIFO_DEPTH; ++j)\r
531                 {\r
532                         if (scsiDev.data[j] != (uint8_t) j)\r
533                         {\r
534                                 assertFail();\r
535                         }\r
536                 }\r
537 \r
538                 s2s_fpgaReset();\r
539 \r
540         }\r
541         #endif\r
542 \r
543 }\r
544 \r
545 static void scsiPhyInitDMA()\r
546 {\r
547         // One-time init only.\r
548         static uint8_t init = 0;\r
549         if (init == 0)\r
550         {\r
551                 init = 1;\r
552 \r
553                 // Memory to memory transfers can only be done using DMA2\r
554                 __DMA2_CLK_ENABLE();\r
555 \r
556                 // Transmit SCSI data. The source data is treated as the\r
557                 // peripheral (even though this is memory-to-memory)\r
558                 memToFSMC.Instance = DMA2_Stream0;\r
559                 memToFSMC.Init.Channel = DMA_CHANNEL_0;\r
560                 memToFSMC.Init.Direction = DMA_MEMORY_TO_MEMORY;\r
561                 memToFSMC.Init.PeriphInc = DMA_PINC_ENABLE;\r
562                 memToFSMC.Init.MemInc = DMA_MINC_DISABLE;\r
563                 memToFSMC.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;\r
564                 memToFSMC.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;\r
565                 memToFSMC.Init.Mode = DMA_NORMAL;\r
566                 memToFSMC.Init.Priority = DMA_PRIORITY_LOW;\r
567                 // FIFO mode is needed to allow conversion from 32bit words to the\r
568                 // 8bit FSMC interface.\r
569                 memToFSMC.Init.FIFOMode = DMA_FIFOMODE_ENABLE;\r
570 \r
571                 // We only use 1 word (4 bytes) in the fifo at a time. Normally it's\r
572                 // better to let the DMA fifo fill up then do burst transfers, but\r
573                 // bursting out the FSMC interface will be very slow and may starve\r
574                 // other (faster) transfers. We don't want to risk the SDIO transfers\r
575                 // from overrun/underrun conditions.\r
576                 memToFSMC.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;\r
577                 memToFSMC.Init.MemBurst = DMA_MBURST_SINGLE;\r
578                 memToFSMC.Init.PeriphBurst = DMA_PBURST_SINGLE;\r
579                 HAL_DMA_Init(&memToFSMC);\r
580 \r
581                 // Receive SCSI data. The source data (fsmc) is treated as the\r
582                 // peripheral (even though this is memory-to-memory)\r
583                 fsmcToMem.Instance = DMA2_Stream1;\r
584                 fsmcToMem.Init.Channel = DMA_CHANNEL_0;\r
585                 fsmcToMem.Init.Direction = DMA_MEMORY_TO_MEMORY;\r
586                 fsmcToMem.Init.PeriphInc = DMA_PINC_DISABLE;\r
587                 fsmcToMem.Init.MemInc = DMA_MINC_ENABLE;\r
588                 fsmcToMem.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;\r
589                 fsmcToMem.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;\r
590                 fsmcToMem.Init.Mode = DMA_NORMAL;\r
591                 fsmcToMem.Init.Priority = DMA_PRIORITY_LOW;\r
592                 fsmcToMem.Init.FIFOMode = DMA_FIFOMODE_ENABLE;\r
593                 fsmcToMem.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;\r
594                 fsmcToMem.Init.MemBurst = DMA_MBURST_SINGLE;\r
595                 fsmcToMem.Init.PeriphBurst = DMA_PBURST_SINGLE;\r
596                 HAL_DMA_Init(&fsmcToMem);\r
597 \r
598                 // TODO configure IRQs\r
599         }\r
600 }\r
601 \r
602 \r
603 void scsiPhyInit()\r
604 {\r
605         scsiPhyInitDMA();\r
606 \r
607         *SCSI_CTRL_IDMASK = 0x00; // Reset in scsiPhyConfig\r
608         *SCSI_CTRL_PHASE = 0x00;\r
609         *SCSI_CTRL_BSY = 0x00;\r
610         scsiPhyFifoSel = 0;\r
611         *SCSI_FIFO_SEL = 0;\r
612 \r
613 }\r
614 \r
615 void scsiPhyConfig()\r
616 {\r
617         if (scsiDev.boardCfg.flags6 & S2S_CFG_ENABLE_TERMINATOR)\r
618         {\r
619                 HAL_GPIO_WritePin(nTERM_EN_GPIO_Port, nTERM_EN_Pin, GPIO_PIN_RESET);\r
620         }\r
621         else\r
622         {\r
623                 HAL_GPIO_WritePin(nTERM_EN_GPIO_Port, nTERM_EN_Pin, GPIO_PIN_SET);\r
624         }\r
625 \r
626 \r
627         uint8_t idMask = 0;\r
628         for (int i = 0; i < 8; ++i)\r
629         {\r
630                 const S2S_TargetCfg* cfg = s2s_getConfigById(i);\r
631                 if (cfg && (cfg->scsiId & S2S_CFG_TARGET_ENABLED))\r
632                 {\r
633                         idMask |= (1 << i);\r
634                 }\r
635         }\r
636         *SCSI_CTRL_IDMASK = idMask;\r
637 }\r
638 \r
639 \r
640 // 1 = DBx error\r
641 // 2 = Parity error\r
642 // 4 = MSG error\r
643 // 8 = CD error\r
644 // 16 = IO error\r
645 // 32 = other error\r
646 int scsiSelfTest()\r
647 {\r
648         return 0;\r
649 #if 0\r
650         int result = 0;\r
651 \r
652         // TEST DBx and DBp\r
653         int i;\r
654         SCSI_Out_Ctl_Write(1); // Write bits manually.\r
655         SCSI_CTL_PHASE_Write(__scsiphase_io); // Needed for parity generation\r
656         for (i = 0; i < 256; ++i)\r
657         {\r
658                 SCSI_Out_Bits_Write(i);\r
659                 scsiDeskewDelay();\r
660                 if (scsiReadDBxPins() != (i & 0xff))\r
661                 {\r
662                         result |= 1;\r
663                 }\r
664                 if (Lookup_OddParity[i & 0xff] != SCSI_ReadPin(SCSI_In_DBP))\r
665                 {\r
666                         result |= 2;\r
667                 }\r
668         }\r
669         SCSI_Out_Ctl_Write(0); // Write bits normally.\r
670 \r
671         // TEST MSG, CD, IO\r
672         for (i = 0; i < 8; ++i)\r
673         {\r
674                 SCSI_CTL_PHASE_Write(i);\r
675                 scsiDeskewDelay();\r
676 \r
677                 if (SCSI_ReadPin(SCSI_In_MSG) != !!(i & __scsiphase_msg))\r
678                 {\r
679                         result |= 4;\r
680                 }\r
681                 if (SCSI_ReadPin(SCSI_In_CD) != !!(i & __scsiphase_cd))\r
682                 {\r
683                         result |= 8;\r
684                 }\r
685                 if (SCSI_ReadPin(SCSI_In_IO) != !!(i & __scsiphase_io))\r
686                 {\r
687                         result |= 16;\r
688                 }\r
689         }\r
690         SCSI_CTL_PHASE_Write(0);\r
691 \r
692         uint32_t signalsOut[] = { SCSI_Out_ATN, SCSI_Out_BSY, SCSI_Out_RST, SCSI_Out_SEL };\r
693         uint32_t signalsIn[] = { SCSI_Filt_ATN, SCSI_Filt_BSY, SCSI_Filt_RST, SCSI_Filt_SEL };\r
694 \r
695         for (i = 0; i < 4; ++i)\r
696         {\r
697                 SCSI_SetPin(signalsOut[i]);\r
698                 scsiDeskewDelay();\r
699 \r
700                 int j;\r
701                 for (j = 0; j < 4; ++j)\r
702                 {\r
703                         if (i == j)\r
704                         {\r
705                                 if (! SCSI_ReadFilt(signalsIn[j]))\r
706                                 {\r
707                                         result |= 32;\r
708                                 }\r
709                         }\r
710                         else\r
711                         {\r
712                                 if (SCSI_ReadFilt(signalsIn[j]))\r
713                                 {\r
714                                         result |= 32;\r
715                                 }\r
716                         }\r
717                 }\r
718                 SCSI_ClearPin(signalsOut[i]);\r
719         }\r
720         return result;\r
721 #endif\r
722 }\r
723 \r