3 files changed, 22 insertions, 64 deletions

diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index 940f183c63..aa98aff852 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -326,6 +326,26 @@ int system_memory_guard(int newmode)
     return 0;
 }
 
+void udelay(unsigned short usecs)
+{
+    unsigned cycles_per_usec;
+    unsigned delay;
+
+    if (cpu_frequency == CPUFREQ_MAX) {
+        cycles_per_usec = (CPUFREQ_MAX + 999999) / 1000000;
+    } else {
+        cycles_per_usec = (CPUFREQ_NORMAL + 999999) / 1000000;
+    }
+
+    delay = (usecs * cycles_per_usec + 3) / 4;
+
+    asm volatile(
+        "1: subs %0, %0, #1  \n"    /* 1 cycle  */
+        "   bne  1b          \n"    /* 3 cycles */
+        : : "r"(delay)
+    );
+}
+
 #ifndef BOOTLOADER
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 
diff --git a/firmware/target/arm/as3525/system-target.h b/firmware/target/arm/as3525/system-target.h
index 523237ffcd..90e5e3a882 100644
--- a/firmware/target/arm/as3525/system-target.h
+++ b/firmware/target/arm/as3525/system-target.h
@@ -49,66 +49,5 @@ extern int c200v2_variant;
 #define TIMER_PERIOD (KERNEL_TIMER_FREQ/HZ)
 #endif
 
-/*
- * This function is not overly accurate, so rather call it with an usec more
- * than less (see below comment)
- * 
- * if inlined it expands to a really small and fast function if it's called
- * with compile time constants */
-static inline void udelay(unsigned usecs) __attribute__((always_inline));
-static inline void udelay(unsigned usecs)
-{
-    unsigned now;
-    int end;
-    
-    /**
-     * we're limited to 0.666us multiplies due to the odd timer frequency (1.5MHz),
-     * to avoid calculating which is safer (need to round up for small values)
-     * and saves spending time in the divider we have a lut for
-     * small us values, it should be roughly us*3/2
-     **/
-    static const unsigned char udelay_lut[] =
-    {
-         0,  2,  3,  5,  6,  8,  9, 11, 12, 14,
-        15, 17, 18, 20, 21, 23, 24, 26, 27, 29,
-    };
-
-
-    now = TIMER2_VALUE;
-    /* we don't want to handle multiple overflows, so limit the numbers
-     * (if you want to wait more than a tick just poll current_tick, or
-     * call sleep()) */
-    if (UNLIKELY(usecs >= (TIMER_PERIOD*2/3)))
-        panicf("%s(): %d too high!", __func__, usecs);
-    if (UNLIKELY(usecs <= 0))
-        return;
-    if (usecs < ARRAYLEN(udelay_lut))
-    {   /* the timer decrements */
-        end = now - udelay_lut[usecs];
-    }
-    else
-    {   /* to usecs */
-        int delay = usecs * 3 / 2; /* us * 0.666 = us*timer_period */
-        end = now - delay;
-    }
-
-    unsigned old;
-
-    /* underrun ? */
-    if (end < 0)
-    {
-        do {
-            old = now;
-            now = TIMER2_VALUE;
-        } while(now <= old);  /* if the new value is higher then we wrapped */
-
-        end += TIMER_PERIOD;
-    }
-
-    do {
-        /* if timer wraps then we missed our end value */
-        old = now;
-        now = TIMER2_VALUE;
-    } while(now > (unsigned)end && now <= old);
-}
+void udelay(unsigned short usecs);
 #endif /* SYSTEM_TARGET_H */
diff --git a/firmware/target/arm/as3525/usb-drv-as3525.c b/firmware/target/arm/as3525/usb-drv-as3525.c
index 0b73713c51..0b69e8953b 100644
--- a/firmware/target/arm/as3525/usb-drv-as3525.c
+++ b/firmware/target/arm/as3525/usb-drv-as3525.c
@@ -80,8 +80,7 @@ void usb_attach(void)
 /* delay is in milliseconds */
 static inline void usb_delay(int delay)
 {
-    while(delay--)
-        udelay(1000);
+    udelay(1000 * delay);
 }
 
 static void usb_phy_on(void)