Fix terrible performance
[SCSI2SD-V6.git] / src / firmware / scsiPhy.c
index 1a8a6ae..2231089 100755 (executable)
@@ -76,21 +76,16 @@ void EXTI4_IRQHandler()
        }\r
 }\r
 \r
-#if 0\r
-uint8_t\r
-scsiReadDBxPins()\r
+static void assertFail()\r
 {\r
-       return\r
-               (SCSI_ReadPin(SCSI_In_DBx_DB7) << 7) |\r
-               (SCSI_ReadPin(SCSI_In_DBx_DB6) << 6) |\r
-               (SCSI_ReadPin(SCSI_In_DBx_DB5) << 5) |\r
-               (SCSI_ReadPin(SCSI_In_DBx_DB4) << 4) |\r
-               (SCSI_ReadPin(SCSI_In_DBx_DB3) << 3) |\r
-               (SCSI_ReadPin(SCSI_In_DBx_DB2) << 2) |\r
-               (SCSI_ReadPin(SCSI_In_DBx_DB1) << 1) |\r
-               SCSI_ReadPin(SCSI_In_DBx_DB0);\r
+       while (1)\r
+       {\r
+               s2s_ledOn();\r
+               s2s_delay_ms(100);\r
+               s2s_ledOff();\r
+               s2s_delay_ms(100);\r
+       }\r
 }\r
-#endif\r
 \r
 static void\r
 startScsiRx(uint32_t count)\r
@@ -103,6 +98,12 @@ startScsiRx(uint32_t count)
 uint8_t\r
 scsiReadByte(void)\r
 {\r
+#if FIFODEBUG\r
+       if (!scsiPhyFifoAltEmpty()) {\r
+               // Force a lock-up.\r
+               assertFail();\r
+       }\r
+#endif\r
        startScsiRx(1);\r
 \r
        trace(trace_spinPhyRxFifo);\r
@@ -111,6 +112,16 @@ scsiReadByte(void)
        uint8_t val = scsiPhyRx();\r
        // TODO scsiDev.parityError = scsiDev.parityError || SCSI_Parity_Error_Read();\r
 \r
+#if FIFODEBUG\r
+       if (!scsiPhyFifoEmpty()) {\r
+               int j = 0;\r
+               uint8_t k __attribute((unused));\r
+               while (!scsiPhyFifoEmpty()) { k = scsiPhyRx(); ++j; }\r
+\r
+               // Force a lock-up.\r
+               assertFail();\r
+       }\r
+#endif\r
        return val;\r
 }\r
 \r
@@ -165,24 +176,33 @@ void
 scsiRead(uint8_t* data, uint32_t count)\r
 {\r
        int i = 0;\r
-       while (i < count && likely(!scsiDev.resetFlag))\r
-       {\r
-               uint32_t chunk = ((count - i) > SCSI_FIFO_DEPTH)\r
-                       ? SCSI_FIFO_DEPTH : (count - i);\r
 \r
-               if (chunk >= 16)\r
-               {\r
-                       // DMA is doing 32bit transfers.\r
-                       chunk = chunk & 0xFFFFFFF8;\r
-               }\r
 \r
-               startScsiRx(chunk);\r
-               // Wait for the next scsi interrupt (or the 1ms systick)\r
-               __WFI();\r
+       uint32_t chunk = ((count - i) > SCSI_FIFO_DEPTH)\r
+               ? SCSI_FIFO_DEPTH : (count - i);\r
+       if (chunk >= 16)\r
+       {\r
+               // DMA is doing 32bit transfers.\r
+               chunk = chunk & 0xFFFFFFF8;\r
+       }\r
+       startScsiRx(chunk);\r
 \r
+       while (i < count && likely(!scsiDev.resetFlag))\r
+       {\r
                while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
                scsiPhyFifoFlip();\r
 \r
+               uint32_t nextChunk = ((count - i - chunk) > SCSI_FIFO_DEPTH)\r
+                       ? SCSI_FIFO_DEPTH : (count - i - chunk);\r
+               if (nextChunk >= 16)\r
+               {\r
+                       nextChunk = nextChunk & 0xFFFFFFF8;\r
+               }\r
+               if (nextChunk > 0)\r
+               {\r
+                       startScsiRx(nextChunk);\r
+               }\r
+\r
                if (chunk < 16)\r
                {\r
                        scsiReadPIO(data + i, chunk);\r
@@ -191,36 +211,48 @@ scsiRead(uint8_t* data, uint32_t count)
                {\r
                        scsiReadDMA(data + i, chunk);\r
 \r
-                       // Wait for the next DMA interrupt (or the 1ms systick)\r
-                       // It's beneficial to halt the processor to\r
-                       // give the DMA controller more memory bandwidth to work with.\r
-#if 0\r
-                       __WFI();\r
-#endif\r
                        trace(trace_spinReadDMAPoll);\r
 \r
                        while (!scsiReadDMAPoll() && likely(!scsiDev.resetFlag))\r
                        {\r
-#if 0\r
-// TODO NEED SCSI DMA IRQs\r
-                       __WFI();\r
-#endif\r
                        };\r
                }\r
 \r
+#if FIFODEBUG\r
+               if (!scsiPhyFifoEmpty()) {\r
+                       int j = 0;\r
+                       while (!scsiPhyFifoEmpty()) { scsiPhyRx(); ++j; }\r
+                       // Force a lock-up.\r
+                       assertFail();\r
+               }\r
+#endif\r
                i += chunk;\r
+               chunk = nextChunk;\r
        }\r
 }\r
 \r
 void\r
 scsiWriteByte(uint8_t value)\r
 {\r
+#if FIFODEBUG\r
+       if (!scsiPhyFifoEmpty()) {\r
+               // Force a lock-up.\r
+               assertFail();\r
+       }\r
+#endif\r
        trace(trace_spinPhyTxFifo);\r
        scsiPhyTx(value);\r
        scsiPhyFifoFlip();\r
 \r
        trace(trace_spinTxComplete);\r
        while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
+\r
+#if FIFODEBUG\r
+       if (!scsiPhyFifoAltEmpty()) {\r
+               // Force a lock-up.\r
+               assertFail();\r
+       }\r
+#endif\r
 }\r
 \r
 static void\r
@@ -277,6 +309,13 @@ scsiWrite(const uint8_t* data, uint32_t count)
                uint32_t chunk = ((count - i) > SCSI_FIFO_DEPTH)\r
                        ? SCSI_FIFO_DEPTH : (count - i);\r
 \r
+#if FIFODEBUG\r
+               if (!scsiPhyFifoEmpty()) {\r
+                       // Force a lock-up.\r
+                       assertFail();\r
+               }\r
+#endif\r
+\r
                if (chunk < 16)\r
                {\r
                        scsiWritePIO(data + i, chunk);\r
@@ -287,33 +326,37 @@ scsiWrite(const uint8_t* data, uint32_t count)
                        chunk = chunk & 0xFFFFFFF8;\r
                        scsiWriteDMA(data + i, chunk);\r
 \r
-                       // Wait for the next DMA interrupt (or the 1ms systick)\r
-                       // It's beneficial to halt the processor to\r
-                       // give the DMA controller more memory bandwidth to work with.\r
-#if 0\r
-                       __WFI();\r
-#endif\r
                        trace(trace_spinReadDMAPoll);\r
 \r
                        while (!scsiWriteDMAPoll() && likely(!scsiDev.resetFlag))\r
                        {\r
-#if 0\r
-// TODO NEED SCSI DMA IRQs\r
-                       __WFI();\r
-#endif\r
-                       };\r
+                       }\r
                }\r
 \r
-               while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
-               scsiPhyFifoFlip();\r
-#if 0\r
-// TODO NEED SCSI IRQs\r
-                       __WFI();\r
+               while (!scsiPhyComplete() && likely(!scsiDev.resetFlag))\r
+               {\r
+               }\r
+\r
+#if FIFODEBUG\r
+               if (!scsiPhyFifoAltEmpty()) {\r
+                       // Force a lock-up.\r
+                       assertFail();\r
+               }\r
 #endif\r
 \r
+               scsiPhyFifoFlip();\r
                i += chunk;\r
        }\r
-       while (!scsiPhyComplete() && likely(!scsiDev.resetFlag)) {}\r
+       while (!scsiPhyComplete() && likely(!scsiDev.resetFlag))\r
+       {\r
+       }\r
+\r
+#if FIFODEBUG\r
+       if (!scsiPhyFifoAltEmpty()) {\r
+               // Force a lock-up.\r
+               assertFail();\r
+       }\r
+#endif\r
 }\r
 \r
 static inline void busSettleDelay(void)\r
@@ -325,22 +368,25 @@ static inline void busSettleDelay(void)
 \r
 void scsiEnterBusFree()\r
 {\r
-       *SCSI_CTRL_PHASE = 0;\r
        *SCSI_CTRL_BSY = 0x00;\r
+       // We now have a Bus Clear Delay of 800ns to release remaining signals.\r
+       *SCSI_CTRL_PHASE = 0;\r
 }\r
 \r
 void scsiEnterPhase(int phase)\r
 {\r
        // ANSI INCITS 362-2002 SPI-3 10.7.1:\r
        // Phase changes are not allowed while REQ or ACK is asserted.\r
-#if 0\r
-       while (likely(!scsiDev.resetFlag) &&\r
-               (SCSI_ReadPin(SCSI_In_REQ) || SCSI_ReadFilt(SCSI_Filt_ACK))\r
-               ) {}\r
-#endif\r
+       while (likely(!scsiDev.resetFlag) && scsiStatusACK()) {}\r
 \r
        int newPhase = phase > 0 ? phase : 0;\r
-       if (newPhase != *SCSI_CTRL_PHASE)\r
+       int oldPhase = *SCSI_CTRL_PHASE;\r
+\r
+       if (!scsiPhyFifoEmpty() || !scsiPhyFifoAltEmpty()) {\r
+               // Force a lock-up.\r
+               assertFail();\r
+       }\r
+       if (newPhase != oldPhase)\r
        {\r
                *SCSI_CTRL_PHASE = newPhase;\r
                busSettleDelay();\r
@@ -349,6 +395,7 @@ void scsiEnterPhase(int phase)
                {\r
                        s2s_delay_us(100);\r
                }\r
+\r
        }\r
 }\r
 \r
@@ -397,6 +444,7 @@ void scsiPhyReset()
 \r
        scsiPhyFifoSel = 0;\r
        *SCSI_FIFO_SEL = 0;\r
+       *SCSI_CTRL_DBX = 0;\r
 \r
        // DMA Benchmark code\r
        // Currently 10MB/s. Assume 20MB/s is achievable with 16 bits.\r
@@ -428,7 +476,6 @@ void scsiPhyReset()
 \r
        // FPGA comms test code\r
        #ifdef FPGA_TEST\r
-\r
        while(1)\r
        {\r
                for (int j = 0; j < SCSI_FIFO_DEPTH; ++j)\r
@@ -436,6 +483,11 @@ void scsiPhyReset()
                        scsiDev.data[j] = j;\r
                }\r
 \r
+               if (!scsiPhyFifoEmpty())\r
+               {\r
+                       assertFail();\r
+               }\r
+\r
                *SCSI_CTRL_PHASE = DATA_IN;\r
                HAL_DMA_Start(\r
                        &memToFSMC,\r
@@ -448,6 +500,11 @@ void scsiPhyReset()
                        HAL_DMA_FULL_TRANSFER,\r
                        0xffffffff);\r
 \r
+               if (!scsiPhyFifoFull())\r
+               {\r
+                       assertFail();\r
+               }\r
+\r
                memset(&scsiDev.data[0], 0, SCSI_FIFO_DEPTH);\r
 \r
                *SCSI_CTRL_PHASE = DATA_OUT;\r
@@ -462,17 +519,17 @@ void scsiPhyReset()
                        HAL_DMA_FULL_TRANSFER,\r
                        0xffffffff);\r
 \r
+               if (!scsiPhyFifoEmpty())\r
+               {\r
+                       assertFail();\r
+               }\r
+\r
+\r
                for (int j = 0; j < SCSI_FIFO_DEPTH; ++j)\r
                {\r
                        if (scsiDev.data[j] != (uint8_t) j)\r
                        {\r
-                               while (1)\r
-                               {\r
-                                       s2s_ledOn();\r
-                                       s2s_delay_ms(100);\r
-                                       s2s_ledOff();\r
-                                       s2s_delay_ms(100);\r
-                               }\r
+                               assertFail();\r
                        }\r
                }\r
 \r
@@ -550,6 +607,7 @@ void scsiPhyInit()
        *SCSI_CTRL_BSY = 0x00;\r
        scsiPhyFifoSel = 0;\r
        *SCSI_FIFO_SEL = 0;\r
+       *SCSI_CTRL_DBX = 0;\r
 \r
 }\r
 \r
@@ -586,30 +644,55 @@ void scsiPhyConfig()
 // 32 = other error\r
 int scsiSelfTest()\r
 {\r
-       return 0;\r
-#if 0\r
+       if (scsiDev.phase != BUS_FREE)\r
+       {\r
+               return 32;\r
+       }\r
+\r
+       // Acquire the SCSI bus.\r
+       for (int i = 0; i < 100; ++i)\r
+       {\r
+               if (scsiStatusBSY())\r
+               {\r
+                       s2s_delay_ms(1);\r
+               }\r
+       }\r
+       if (scsiStatusBSY())\r
+       {\r
+               // Error, couldn't acquire scsi bus\r
+               return 32;\r
+       }\r
+       *SCSI_CTRL_BSY = 1;\r
+       if (! scsiStatusBSY())\r
+       {\r
+               // Error, BSY doesn't work.\r
+               return 32;\r
+       }\r
+\r
+       // Should be safe to use the bus now.\r
+\r
        int result = 0;\r
 \r
-       // TEST DBx and DBp\r
+       // TEST DBx\r
+       // TODO test DBp\r
        int i;\r
-       SCSI_Out_Ctl_Write(1); // Write bits manually.\r
-       SCSI_CTL_PHASE_Write(__scsiphase_io); // Needed for parity generation\r
        for (i = 0; i < 256; ++i)\r
        {\r
-               SCSI_Out_Bits_Write(i);\r
-               scsiDeskewDelay();\r
-               if (scsiReadDBxPins() != (i & 0xff))\r
+               *SCSI_CTRL_DBX = i;\r
+               busSettleDelay();\r
+               if (*SCSI_STS_DBX != (i & 0xff))\r
                {\r
                        result |= 1;\r
                }\r
-               if (Lookup_OddParity[i & 0xff] != SCSI_ReadPin(SCSI_In_DBP))\r
+               /*if (Lookup_OddParity[i & 0xff] != SCSI_ReadPin(SCSI_In_DBP))\r
                {\r
                        result |= 2;\r
-               }\r
+               }*/\r
        }\r
-       SCSI_Out_Ctl_Write(0); // Write bits normally.\r
+       *SCSI_CTRL_DBX = 0;\r
 \r
        // TEST MSG, CD, IO\r
+       /* TODO\r
        for (i = 0; i < 8; ++i)\r
        {\r
                SCSI_CTL_PHASE_Write(i);\r
@@ -658,7 +741,9 @@ int scsiSelfTest()
                }\r
                SCSI_ClearPin(signalsOut[i]);\r
        }\r
+       */\r
+\r
+       *SCSI_CTRL_BSY = 0;\r
        return result;\r
-#endif\r
 }\r
 \r