1 files changed, 20 insertions, 0 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