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, 21 insertions, 5 deletions

diff --git a/stmhal/pybthread.h b/stmhal/pybthread.h
index d4310c66a..6edb2400e 100644
--- a/stmhal/pybthread.h
+++ b/stmhal/pybthread.h
@@ -33,15 +33,23 @@ typedef struct _pyb_thread_t {
     void *arg;                  // thread Python args, a GC root pointer
     void *stack;                // pointer to the stack
     size_t stack_len;           // number of words in the stack
-    struct _pyb_thread_t *prev;
-    struct _pyb_thread_t *next;
+    uint32_t timeslice;
+    struct _pyb_thread_t *all_next;
+    struct _pyb_thread_t *run_prev;
+    struct _pyb_thread_t *run_next;
+    struct _pyb_thread_t *queue_next;
 } pyb_thread_t;
 
-extern int pyb_thread_enabled;
-extern pyb_thread_t *pyb_thread_cur;
+typedef pyb_thread_t *pyb_mutex_t;
+
+extern volatile int pyb_thread_enabled;
+extern pyb_thread_t *volatile pyb_thread_all;
+extern pyb_thread_t *volatile pyb_thread_cur;
 
 void pyb_thread_init(pyb_thread_t *th);
+void pyb_thread_deinit();
 uint32_t pyb_thread_new(pyb_thread_t *th, void *stack, size_t stack_len, void *entry, void *arg);
+void pyb_thread_dump(void);
 
 static inline uint32_t pyb_thread_get_id(void) {
     return (uint32_t)pyb_thread_cur;
@@ -56,7 +64,15 @@ static inline void *pyb_thread_get_local(void) {
 }
 
 static inline void pyb_thread_yield(void) {
-    SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
+    if (pyb_thread_cur->run_next == pyb_thread_cur) {
+        __WFI();
+    } else {
+        SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
+    }
 }
 
+void pyb_mutex_init(pyb_mutex_t *m);
+int pyb_mutex_lock(pyb_mutex_t *m, int wait);
+void pyb_mutex_unlock(pyb_mutex_t *m);
+
 #endif // MICROPY_INCLUDED_STMHAL_PYBTHREAD_H