stmhal: Implement a proper thread scheduler.

This patch changes the threading implementation from simple round-robin
with busy waits on mutexs, to proper scheduling whereby threads that are
waiting on a mutex are only scheduled when the mutex becomes available.

1 files changed, 10 insertions, 0 deletions

diff --git a/stmhal/systick.c b/stmhal/systick.c
index aed5b9690..ade05d74d 100644
--- a/stmhal/systick.c
+++ b/stmhal/systick.c
@@ -29,9 +29,11 @@
 #include "py/obj.h"
 #include "irq.h"
 #include "systick.h"
+#include "pybthread.h"
 
 // We provide our own version of HAL_Delay that calls __WFI while waiting, in
 // order to reduce power consumption.
+// Note: Upon entering this function we may or may not have the GIL.
 void HAL_Delay(uint32_t Delay) {
     if (query_irq() == IRQ_STATE_ENABLED) {
         // IRQs enabled, so can use systick counter to do the delay
@@ -40,7 +42,15 @@ void HAL_Delay(uint32_t Delay) {
         // 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.
+            #if MICROPY_PY_THREAD
+            if (pyb_thread_enabled) {
+                pyb_thread_yield();
+            } else {
+                __WFI();
+            }
+            #else
             __WFI();
+            #endif
         }
     } else {
         // IRQs disabled, so need to use a busy loop for the delay.