stmhal: Make pyb.[u]delay use systick with IRQs, busy loop otherwise.

pyb.delay and pyb.udelay now use systick if IRQs are enabled, otherwise
they use a busy loop.  Thus they work correctly when IRQs are disabled.
The busy loop is computed from the current CPU frequency, so works no
matter the CPU frequency.

4 files changed, 40 insertions, 11 deletions

diff --git a/stmhal/irq.h b/stmhal/irq.h
index ef595b7446..6689e995dc 100644
--- a/stmhal/irq.h
+++ b/stmhal/irq.h
@@ -24,10 +24,14 @@
  * THE SOFTWARE.
  */
 
-// these states correspond to values from enable_irq and disable_irq
+// these states correspond to values from query_irq, enable_irq and disable_irq
 #define IRQ_STATE_DISABLED (0x00000001)
 #define IRQ_STATE_ENABLED  (0x00000000)
 
+static inline mp_uint_t query_irq(void) {
+    return __get_PRIMASK();
+}
+
 // enable_irq and disable_irq are defined inline in mpconfigport.h
 
 MP_DECLARE_CONST_FUN_OBJ(pyb_wfi_obj);
diff --git a/stmhal/modpyb.c b/stmhal/modpyb.c
index 38c5499b0b..46ce0e0987 100644
--- a/stmhal/modpyb.c
+++ b/stmhal/modpyb.c
@@ -402,10 +402,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_1(pyb_delay_obj, pyb_delay);
 STATIC mp_obj_t pyb_udelay(mp_obj_t usec_in) {
     mp_int_t usec = mp_obj_get_int(usec_in);
     if (usec > 0) {
-        uint32_t count = 0;
-        const uint32_t utime = (168 * usec / 4);
-        while (++count <= utime) {
-        }
+        sys_tick_udelay(usec);
     }
     return mp_const_none;
 }
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;) {
+        }
     }
 }
 
diff --git a/stmhal/systick.h b/stmhal/systick.h
index 98045d16c4..5a4dfb3ed2 100644
--- a/stmhal/systick.h
+++ b/stmhal/systick.h
@@ -24,6 +24,7 @@
  * THE SOFTWARE.
  */
 
+void sys_tick_udelay(uint32_t usec);
 void sys_tick_wait_at_least(uint32_t stc, uint32_t delay_ms);
 bool sys_tick_has_passed(uint32_t stc, uint32_t delay_ms);
 uint32_t sys_tick_get_microseconds(void);