5 files changed, 57 insertions, 4 deletions

diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 7d37d051c9..2cdd67482a 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -201,6 +201,7 @@ static int perform_soft_reset(void);
 static int set_multiple_mode(int sectors);
 static int set_features(void);
 
+#ifndef ATA_TARGET_POLLING
 STATICIRAM ICODE_ATTR int wait_for_bsy(void)
 {
     long timeout = current_tick + HZ*30;
@@ -235,6 +236,12 @@ STATICIRAM ICODE_ATTR int wait_for_rdy(void)
 
     return 0; /* timeout */
 }
+#else
+extern int ata_wait_for_bsy(void);
+extern int ata_wait_for_rdy(void);
+#define wait_for_bsy    ata_wait_for_bsy
+#define wait_for_rdy    ata_wait_for_rdy
+#endif
 
 STATICIRAM ICODE_ATTR int wait_for_start_of_transfer(void)
 {
diff --git a/firmware/target/arm/imx31/ata-imx31.c b/firmware/target/arm/imx31/ata-imx31.c
index 6ba49cada0..c3e3c51e63 100644
--- a/firmware/target/arm/imx31/ata-imx31.c
+++ b/firmware/target/arm/imx31/ata-imx31.c
@@ -26,6 +26,7 @@
 #include "power.h"
 #include "panic.h"
 #include "ata.h"
+#include "ata-defines.h"
 #include "ata-target.h"
 #include "ccm-imx31.h"
 #ifdef HAVE_ATA_DMA
@@ -459,6 +460,7 @@ bool ata_dma_setup(void *addr, unsigned long bytes, bool write)
          * shouldn't be reached based upon size. Otherwise we simply didn't
          * understand the DMA mode setup. Force PIO in both cases. */
         ATA_INTF_CONTROL = ATA_FIFO_RST | ATA_ATA_RST;
+        yield();
         return false;
     }
 
@@ -645,6 +647,43 @@ bool ata_dma_finish(void)
 }
 #endif /* HAVE_ATA_DMA */
 
+static int ata_wait_status(unsigned status, unsigned mask, int timeout)
+{
+    long busy_timeout = usec_timer() + 2;
+    long end_tick = current_tick + timeout;
+    
+    while (1)
+    {
+        if ((ATA_DRIVE_STATUS & mask) == status)
+            return 1;
+
+        if (!TIME_AFTER(usec_timer(), busy_timeout))
+            continue;
+
+        ata_keep_active();
+
+        if (TIME_AFTER(current_tick, end_tick))
+            break;
+
+        sleep(0);
+        busy_timeout = usec_timer() + 2;
+    }
+
+    return 0; /* timed out */
+}
+
+int ata_wait_for_bsy(void)
+{
+    /* BSY = 0 */
+    return ata_wait_status(0, STATUS_BSY, 30*HZ);
+}
+
+int ata_wait_for_rdy(void)
+{
+    /* RDY = 1 && BSY = 0 */
+    return ata_wait_status(STATUS_RDY, STATUS_RDY | STATUS_BSY, 40*HZ);
+}
+
 void ata_device_init(void)
 {
     /* Make sure we're not in reset mode */
diff --git a/firmware/target/arm/imx31/ata-target.h b/firmware/target/arm/imx31/ata-target.h
index da1902acea..24141c6aa7 100644
--- a/firmware/target/arm/imx31/ata-target.h
+++ b/firmware/target/arm/imx31/ata-target.h
@@ -53,4 +53,6 @@ bool ata_is_coldstart(void);
 #define ATA_SET_DEVICE_FEATURES
 void ata_set_pio_timings(int mode);
 
+#define ATA_TARGET_POLLING
+
 #endif /* ATA_TARGET_H */
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c b/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
index 978c7f77cd..1f177d4252 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
+++ b/firmware/target/arm/imx31/gigabeat-s/system-gigabeat-s.c
@@ -97,10 +97,10 @@ void gpt_start(void)
     while (GPTCR & GPTCR_SWR);
     /* No output
      * No capture
-     * Enable in run mode only (doesn't tick while in WFI)
+     * Enable in wait and run mode
      * Freerun mode (count to 0xFFFFFFFF and roll-over to 0x00000000)
      */
-    GPTCR = GPTCR_FRR | GPTCR_CLKSRC_IPG_CLK;
+    GPTCR = GPTCR_FRR | GPTCR_WAITEN | GPTCR_CLKSRC_IPG_CLK;
     GPTPR = ipg_mhz - 1;
     GPTCR |= GPTCR_EN;
 }
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-target.h b/firmware/target/arm/imx31/gigabeat-s/system-target.h
index f3ba719443..4449b570f7 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/system-target.h
@@ -31,12 +31,17 @@
 /* Overdrive mode */
 #define CPUFREQ_MAX         528000000
 
-static inline void udelay(unsigned int usecs)
+static inline void udelay(unsigned long usecs)
 {
-    unsigned stop = GPTCNT + usecs;
+    unsigned long stop = GPTCNT + usecs;
     while (TIME_BEFORE(GPTCNT, stop));
 }
 
+static inline unsigned long usec_timer(void)
+{
+    return GPTCNT;
+}
+
 void watchdog_init(unsigned int half_seconds);
 void watchdog_service(void);