src/firmware/scsiPhy.c

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