3 files changed, 37 insertions, 19 deletions

diff --git a/firmware/target/arm/imx31/gigabeat-s/button-imx31.c b/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
index 7c432450c2..587e66e0bc 100644
--- a/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/button-imx31.c
@@ -68,8 +68,6 @@ static __attribute__((interrupt("IRQ"))) void KPP_HANDLER(void)
 
     for (col = 0; col < 3; col++) /* Col */
     {
-        int i;
-
         /* 2. Write 1s to KPDR[10:8] setting column data to 1s */
         KPP_KPDR |= (0x7 << 8);
 
@@ -78,8 +76,7 @@ static __attribute__((interrupt("IRQ"))) void KPP_HANDLER(void)
         KPP_KPCR &= ~(0x7 << 8);
 
         /* Give the columns time to discharge */
-        for (i = 0; i < 128; i++) /* TODO: find minimum safe delay */
-            asm volatile ("");
+        udelay(2);
 
         /* 4. Configure columns as open-drain */
         KPP_KPCR |= (0x7 << 8);
@@ -94,8 +91,7 @@ static __attribute__((interrupt("IRQ"))) void KPP_HANDLER(void)
 
         /* Delay added to avoid propagating the 0 from column to row
          * when scanning. */
-        for (i = 0; i < 128; i++) /* TODO: find minimum safe delay */
-            asm volatile ("");
+        udelay(2);
 
         /* Read row input */
         button |= (~KPP_KPDR & kms[col].mask) << kms[col].shift;
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-imx31.c b/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
index c339f4fe7c..7454806d07 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/system-imx31.c
@@ -32,6 +32,8 @@
 #include "clkctl-imx31.h"
 #include "mc13783.h"
 
+/** Watchdog timer routines **/
+
 /* Initialize the watchdog timer */
 void watchdog_init(unsigned int half_seconds)
 {
@@ -57,6 +59,33 @@ void watchdog_service(void)
     WDOG_WSR = 0xaaaa;
 }
 
+/** GPT timer routines - basis for udelay **/
+
+/* Start the general-purpose timer (1MHz) */
+void gpt_start(void)
+{
+    imx31_clkctl_module_clock_gating(CG_GPT, CGM_ON_RUN_WAIT);
+    unsigned int ipg_mhz = imx31_clkctl_get_ipg_clk() / 1000000;
+
+    GPTCR &= ~GPTCR_EN; /* Disable counter */
+    GPTCR |= GPTCR_SWR; /* Reset module */
+    while (GPTCR & GPTCR_SWR);
+    /* No output
+     * No capture
+     * Enable in run mode only (doesn't tick while in WFI)
+     * Freerun mode (count to 0xFFFFFFFF and roll-over to 0x00000000)
+     */
+    GPTCR = GPTCR_FRR | GPTCR_CLKSRC_IPG_CLK;
+    GPTPR = ipg_mhz - 1;
+    GPTCR |= GPTCR_EN;
+}
+
+/* Stop the general-purpose timer */
+void gpt_stop(void)
+{
+    GPTCR &= ~GPTCR_EN;
+}
+
 int system_memory_guard(int newmode)
 {
     (void)newmode;
@@ -84,7 +113,6 @@ void system_init(void)
         /* CGR0 */
         CG_SD_MMC1,
         CG_SD_MMC2,
-        CG_GPT,
         CG_IIM,
         CG_SDMA,
         CG_CSPI3,
@@ -140,6 +168,7 @@ void system_init(void)
         imx31_clkctl_module_clock_gating(disable_clocks[i], CGM_OFF);
 
     avic_init();
+    gpt_start();
     gpio_init();
 }
 
diff --git a/firmware/target/arm/imx31/gigabeat-s/system-target.h b/firmware/target/arm/imx31/gigabeat-s/system-target.h
index b99b31d1b4..c7797e43c9 100644
--- a/firmware/target/arm/imx31/gigabeat-s/system-target.h
+++ b/firmware/target/arm/imx31/gigabeat-s/system-target.h
@@ -31,25 +31,18 @@
 #define CPUFREQ_MAX CPU_FREQ
 #endif
 
-/* For USB driver - no accuracy assurance */
 static inline void udelay(unsigned int usecs)
 {
-    unsigned int x;
-    for (x = 0; x < 300*usecs; x++)
-        asm volatile ("");
+    unsigned stop = GPTCNT + usecs;
+    while (TIME_BEFORE(GPTCNT, stop));
 }
 
-#if 0
-static inline void udelay(unsigned int usecs)
-{
-    volatile signed int stop = EPITCNT1 - usecs;
-    while ((signed int)EPITCNT1 > stop);
-}
-#endif
-
 void watchdog_init(unsigned int half_seconds);
 void watchdog_service(void);
 
+void gpt_start(void);
+void gpt_stop(void);
+
 /* Prepare for transition to firmware */
 void system_prepare_fw_start(void);
 void tick_stop(void);