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