1 files changed, 15 insertions, 0 deletions
diff --git a/src/lpc176x/main.c b/src/lpc176x/main.c
index 1147faec..c417a2c7 100644
--- a/src/lpc176x/main.c
+++ b/src/lpc176x/main.c
@@ -7,6 +7,7 @@
 #include "LPC17xx.h" // NVIC_SystemReset
 #include "command.h" // DECL_CONSTANT
 #include "sched.h" // sched_main
+#include "board/misc.h" // timer_read_time
 
 DECL_CONSTANT_STR("MCU", "lpc176x");
 
@@ -66,6 +67,20 @@ command_reset(uint32_t *args)
 }
 DECL_COMMAND_FLAGS(command_reset, HF_IN_SHUTDOWN, "reset");
 
+// Implement simple early-boot delay mechanism
+void
+udelay(uint32_t usecs)
+{
+    if (!(CoreDebug->DEMCR & CoreDebug_DEMCR_TRCENA_Msk)) {
+        CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+        DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+    }
+
+    uint32_t end = timer_read_time() + timer_from_us(usecs);
+    while (timer_is_before(timer_read_time(), end))
+        ;
+}
+
 // Main entry point
 int
 main(void)