1 files changed, 33 insertions, 6 deletions
diff --git a/stmhal/systick.c b/stmhal/systick.c
index b4dd68ffec..fc462943f8 100644
--- a/stmhal/systick.c
+++ b/stmhal/systick.c
@@ -36,12 +36,39 @@
 // We provide our own version of HAL_Delay that calls __WFI while waiting, in
 // order to reduce power consumption.
 void HAL_Delay(uint32_t Delay) {
-    extern __IO uint32_t uwTick;
-    uint32_t start = uwTick;
-    // Wraparound of tick is taken care of by 2's complement arithmetic.
-    while (uwTick - start < Delay) {
-        // Enter sleep mode, waiting for (at least) the SysTick interrupt.
-        __WFI();
+    if (query_irq() == IRQ_STATE_ENABLED) {
+        // IRQs enabled, so can use systick counter to do the delay
+        extern __IO uint32_t uwTick;
+        uint32_t start = uwTick;
+        // Wraparound of tick is taken care of by 2's complement arithmetic.
+        while (uwTick - start < Delay) {
+            // Enter sleep mode, waiting for (at least) the SysTick interrupt.
+            __WFI();
+        }
+    } else {
+        // IRQs disabled, so need to use a busy loop for the delay.
+        // To prevent possible overflow of the counter we use a double loop.
+        const uint32_t count_1ms = HAL_RCC_GetSysClockFreq() / 4000;
+        for (int i = 0; i < Delay; i++) {
+            for (uint32_t count = 0; ++count <= count_1ms;) {
+            }
+        }
+    }
+}
+
+// delay for given number of microseconds
+void sys_tick_udelay(uint32_t usec) {
+    if (query_irq() == IRQ_STATE_ENABLED) {
+        // IRQs enabled, so can use systick counter to do the delay
+        uint32_t start = sys_tick_get_microseconds();
+        while (sys_tick_get_microseconds() - start < usec) {
+        }
+    } else {
+        // IRQs disabled, so need to use a busy loop for the delay
+        // sys freq is always a multiple of 2MHz, so division here won't lose precision
+        const uint32_t ucount = HAL_RCC_GetSysClockFreq() / 2000000 * usec / 2;
+        for (uint32_t count = 0; ++count <= ucount;) {
+        }
     }
 }