2 files changed, 29 insertions, 14 deletions

diff --git a/firmware/kernel.c b/firmware/kernel.c
index dc960b6e51..730484ed51 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -245,19 +245,8 @@ void timeout_register(struct timeout *tmo, timeout_cb_type callback,
 void sleep(int ticks)
 {
 #if CONFIG_CPU == S3C2440 && defined(BOOTLOADER)
-    volatile int counter;
-    TCON &= ~(1 << 20); // stop timer 4
-    // TODO: this constant depends on dividers settings inherited from
-    // firmware. Set them explicitly somwhere.
-    TCNTB4 = 12193 * ticks / HZ;
-    TCON |= 1 << 21; // set manual bit
-    TCON &= ~(1 << 21); // reset manual bit
-    TCON &= ~(1 << 22); //autoreload Off
-    TCON |= (1 << 20); // start timer 4
-    do {
-       counter = TCNTO4;
-    } while(counter > 0);
-
+    extern void delay(int ticks);
+    delay(ticks);
 #elif defined(CPU_PP) && defined(BOOTLOADER)
     unsigned stop = USEC_TIMER + ticks * (1000000/HZ);
     while (TIME_BEFORE(USEC_TIMER, stop))
diff --git a/firmware/target/arm/s3c2440/gigabeat-fx/kernel-meg-fx.c b/firmware/target/arm/s3c2440/gigabeat-fx/kernel-meg-fx.c
index 5ef8c8023a..6a750c32e2 100644
--- a/firmware/target/arm/s3c2440/gigabeat-fx/kernel-meg-fx.c
+++ b/firmware/target/arm/s3c2440/gigabeat-fx/kernel-meg-fx.c
@@ -24,7 +24,7 @@
 #include "timer.h"
 #include "thread.h"
 
-void tick_start(unsigned int interval_in_ms)
+static inline void tick_set(unsigned int interval_in_ms)
 {
     /*
      * Based on default PCLK of 49.1568MHz - scaling chosen to give
@@ -49,6 +49,12 @@ void tick_start(unsigned int interval_in_ms)
     TCON |= 1 << 21;
     /* reset manual bit */
     TCON &= ~(1 << 21);
+}
+
+void tick_start(unsigned int interval_in_ms)
+{
+    tick_set(interval_in_ms);
+
     /* interval mode */
     TCON |= 1 << 22;
     /* start timer 4 */
@@ -58,6 +64,26 @@ void tick_start(unsigned int interval_in_ms)
     INTMSK &= ~TIMER4_MASK;
 }
 
+#ifdef BOOTLOADER
+void delay(int ticks)
+{
+    volatile unsigned long counter;
+
+    INTMSK |= TIMER4_MASK;
+
+    tick_set(1000 * ticks / HZ);
+
+    /* autoreload Off */
+    TCON &= ~(1 << 22);
+    /* start timer 4 */
+    TCON |= (1 << 20);
+
+    do {
+        counter = TCNTO4;
+    } while(counter > 0);
+}
+#endif /* BOOTLOADER */
+
 void TIMER4(void)
 {
     /* Run through the list of tick tasks */