Fix terrible performance

[SCSI2SD-V6.git] / src / firmware / main.c

diff --git a/src/firmware/main.c b/src/firmware/main.c
index 04d2c3b..67f7956 100755 (executable)
--- a/src/firmware/main.c
+++ b/src/firmware/main.c
@@ -32,6 +32,7 @@
 \r
 \r
 const char* Notice = "Copyright (C) 2016 Michael McMaster <michael@codesrc.com>";\r
+uint32_t lastSDPoll;\r
 \r
 void mainEarlyInit()\r
 {\r
@@ -51,9 +52,7 @@ void mainInit()
        scsiDiskInit();\r
        sdInit();\r
        s2s_configInit(&scsiDev.boardCfg);\r
-\r
-       s2s_debugInit();\r
-\r
+       scsiPhyConfig();\r
        scsiInit();\r
 \r
        MX_USB_DEVICE_Init(); // USB lun config now available.\r
@@ -72,10 +71,7 @@ void mainInit()
                ++delaySeconds;\r
        }\r
 \r
-#if 0\r
-       uint32_t lastSDPoll = getTime_ms();\r
-       sdCheckPresent();\r
-#endif\r
+       lastSDPoll = s2s_getTime_ms();\r
 }\r
 \r
 void mainLoop()\r
@@ -89,34 +85,53 @@ void mainLoop()
 \r
 #if 0\r
        sdPoll();\r
+#endif\r
 \r
        if (unlikely(scsiDev.phase == BUS_FREE))\r
        {\r
-               if (unlikely(elapsedTime_ms(lastSDPoll) > 200))\r
+               if (unlikely(s2s_elapsedTime_ms(lastSDPoll) > 200))\r
                {\r
-                       lastSDPoll = getTime_ms();\r
-                       sdCheckPresent();\r
+                       lastSDPoll = s2s_getTime_ms();\r
+                       if (sdInit())\r
+                       {\r
+                               s2s_configInit(&scsiDev.boardCfg);\r
+                               scsiPhyConfig();\r
+                               scsiInit();\r
+\r
+\r
+                               USBD_Stop(&hUsbDeviceFS);\r
+                               s2s_delay_ms(128);\r
+                               USBD_Start(&hUsbDeviceFS);\r
+                       }\r
                }\r
                else\r
                {\r
+                       // TODO this hurts performance significantly! Work out why __WFI()\r
+                       // doesn't wake up immediately !\r
+#if 0\r
                        // Wait for our 1ms timer to save some power.\r
                        // There's an interrupt on the SEL signal to ensure we respond\r
                        // quickly to any SCSI commands. The selection abort time is\r
                        // only 250us, and new SCSI-3 controllers time-out very\r
                        // not long after that, so we need to ensure we wake up quickly.\r
-                       uint8_t interruptState = CyEnterCriticalSection();\r
-                       if (!SCSI_ReadFilt(SCSI_Filt_SEL))\r
+                       uint32_t interruptState = __get_PRIMASK();\r
+                       __disable_irq();\r
+\r
+                       if (!*SCSI_STS_SELECTED)\r
                        {\r
-                               __WFI(); // Will wake on interrupt, regardless of mask\r
+                               //__WFI(); // Will wake on interrupt, regardless of mask\r
                        }\r
-                       CyExitCriticalSection(interruptState);\r
+                       if (!interruptState)\r
+                       {\r
+                               __enable_irq();\r
+                       }\r
+#endif\r
                }\r
        }\r
        else if (scsiDev.phase >= 0)\r
        {\r
                // don't waste time scanning SD cards while we're doing disk IO\r
-               lastSDPoll = getTime_ms();\r
+               lastSDPoll = s2s_getTime_ms();\r
        }\r
-#endif\r
 }\r
 \r