Fix terrible performance
[SCSI2SD-V6.git] / src / firmware / main.c
index a231762..67f7956 100755 (executable)
@@ -55,9 +55,6 @@ void mainInit()
        scsiPhyConfig();\r
        scsiInit();\r
 \r
-       s2s_debugInit();\r
-\r
-\r
        MX_USB_DEVICE_Init(); // USB lun config now available.\r
 \r
        // Optional bootup delay\r
@@ -109,18 +106,25 @@ void mainLoop()
                }\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
+                       }\r
+                       if (!interruptState)\r
                        {\r
-                               __WFI(); // Will wake on interrupt, regardless of mask\r
+                               __enable_irq();\r
                        }\r
-                       CyExitCriticalSection(interruptState);\r
 #endif\r
                }\r
        }\r