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