Fix terrible performance

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

diff --git a/src/firmware/main.c b/src/firmware/main.c
index 6e4cd3f..67f7956 100755 (executable)
--- a/src/firmware/main.c
+++ b/src/firmware/main.c
@@ -106,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