Skip to content

Commit 05a4859

Browse files
dpgeorgedpgeorge
committed
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 parent f6c22a0 commit 05a4859

9 files changed

Lines changed: 236 additions & 63 deletions

File tree

‎stmhal/main.c‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -695,6 +695,10 @@ int main(void) {
695695
can_deinit();
696696
#endif
697697

698+
#if MICROPY_PY_THREAD
699+
pyb_thread_deinit();
700+
#endif
701+
698702
first_soft_reset = false;
699703
goto soft_reset;
700704
}

‎stmhal/modmachine.c‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
#include "extmod/vfs_fat.h"
4242
#include "gccollect.h"
4343
#include "irq.h"
44+
#include "pybthread.h"
4445
#include "rng.h"
4546
#include "storage.h"
4647
#include "pin.h"
@@ -159,6 +160,10 @@ STATIC mp_obj_t machine_info(mp_uint_t n_args, const mp_obj_t *args) {
159160
}
160161
}
161162

163+
#if MICROPY_PY_THREAD
164+
pyb_thread_dump();
165+
#endif
166+
162167
if (n_args == 1) {
163168
// arg given means dump gc allocation table
164169
gc_dump_alloc_table();

‎stmhal/mpconfigport.h‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -299,7 +299,21 @@ static inline mp_uint_t disable_irq(void) {
299299

300300
#define MICROPY_BEGIN_ATOMIC_SECTION() disable_irq()
301301
#define MICROPY_END_ATOMIC_SECTION(state) enable_irq(state)
302+
303+
#if MICROPY_PY_THREAD
304+
#define MICROPY_EVENT_POLL_HOOK \
305+
do { \
306+
if (pyb_thread_enabled) { \
307+
MP_THREAD_GIL_EXIT(); \
308+
pyb_thread_yield(); \
309+
MP_THREAD_GIL_ENTER(); \
310+
} else { \
311+
__WFI(); \
312+
} \
313+
} while (0);
314+
#else
302315
#define MICROPY_EVENT_POLL_HOOK __WFI();
316+
#endif
303317

304318
// There is no classical C heap in bare-metal ports, only Python
305319
// garbage-collected heap. For completeness, emulate C heap via

‎stmhal/mpthreadport.c‎

Lines changed: 3 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -44,15 +44,13 @@ void mp_thread_init(void) {
4444

4545
void mp_thread_gc_others(void) {
4646
mp_thread_mutex_lock(&thread_mutex, 1);
47-
gc_collect_root((void**)&pyb_thread_cur, 1);
48-
for (pyb_thread_t *th = pyb_thread_cur;; th = th->next) {
47+
for (pyb_thread_t *th = pyb_thread_all; th != NULL; th = th->all_next) {
48+
gc_collect_root((void**)&th, 1);
4949
gc_collect_root(&th->arg, 1);
50+
gc_collect_root(&th->stack, 1);
5051
if (th != pyb_thread_cur) {
5152
gc_collect_root(th->stack, th->stack_len);
5253
}
53-
if (th->next == pyb_thread_cur) {
54-
break;
55-
}
5654
}
5755
mp_thread_mutex_unlock(&thread_mutex);
5856
}
@@ -93,31 +91,4 @@ void mp_thread_start(void) {
9391
void mp_thread_finish(void) {
9492
}
9593

96-
void mp_thread_mutex_init(mp_thread_mutex_t *mutex) {
97-
*mutex = 0;
98-
}
99-
100-
int mp_thread_mutex_lock(mp_thread_mutex_t *mutex, int wait) {
101-
uint32_t irq_state = disable_irq();
102-
if (*mutex) {
103-
// mutex is locked
104-
if (!wait) {
105-
enable_irq(irq_state);
106-
return 0; // failed to lock mutex
107-
}
108-
while (*mutex) {
109-
enable_irq(irq_state);
110-
pyb_thread_yield();
111-
irq_state = disable_irq();
112-
}
113-
}
114-
*mutex = 1;
115-
enable_irq(irq_state);
116-
return 1; // have mutex
117-
}
118-
119-
void mp_thread_mutex_unlock(mp_thread_mutex_t *mutex) {
120-
*mutex = 0;
121-
}
122-
12394
#endif // MICROPY_PY_THREAD

‎stmhal/mpthreadport.h‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@
2929
#include "py/mpthread.h"
3030
#include "pybthread.h"
3131

32-
typedef uint32_t mp_thread_mutex_t;
32+
typedef pyb_mutex_t mp_thread_mutex_t;
3333

3434
void mp_thread_init(void);
3535
void mp_thread_gc_others(void);
@@ -42,4 +42,16 @@ static inline struct _mp_state_thread_t *mp_thread_get_state(void) {
4242
return pyb_thread_get_local();
4343
}
4444

45+
static inline void mp_thread_mutex_init(mp_thread_mutex_t *m) {
46+
pyb_mutex_init(m);
47+
}
48+
49+
static inline int mp_thread_mutex_lock(mp_thread_mutex_t *m, int wait) {
50+
return pyb_mutex_lock(m, wait);
51+
}
52+
53+
static inline void mp_thread_mutex_unlock(mp_thread_mutex_t *m) {
54+
pyb_mutex_unlock(m);
55+
}
56+
4557
#endif // __MICROPY_INCLUDED_STMHAL_MPTHREADPORT_H__

‎stmhal/pybthread.c‎

Lines changed: 157 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -34,29 +34,80 @@
3434

3535
#if MICROPY_PY_THREAD
3636

37-
int pyb_thread_enabled;
38-
pyb_thread_t *pyb_thread_cur;
37+
#define PYB_MUTEX_UNLOCKED ((void*)0)
38+
#define PYB_MUTEX_LOCKED ((void*)1)
39+
40+
extern void __fatal_error(const char*);
41+
42+
volatile int pyb_thread_enabled;
43+
pyb_thread_t *volatile pyb_thread_all;
44+
pyb_thread_t *volatile pyb_thread_cur;
45+
46+
static inline void pyb_thread_add_to_runable(pyb_thread_t *thread) {
47+
thread->run_prev = pyb_thread_cur->run_prev;
48+
thread->run_next = pyb_thread_cur;
49+
pyb_thread_cur->run_prev->run_next = thread;
50+
pyb_thread_cur->run_prev = thread;
51+
}
52+
53+
static inline void pyb_thread_remove_from_runable(pyb_thread_t *thread) {
54+
if (thread->run_next == thread) {
55+
__fatal_error("deadlock");
56+
}
57+
thread->run_prev->run_next = thread->run_next;
58+
thread->run_next->run_prev = thread->run_prev;
59+
}
3960

4061
void pyb_thread_init(pyb_thread_t *thread) {
62+
pyb_thread_enabled = 0;
63+
pyb_thread_all = thread;
4164
pyb_thread_cur = thread;
42-
pyb_thread_cur->sp = NULL; // will be set when this thread switches out
43-
pyb_thread_cur->local_state = 0; // will be set by mp_thread_init
44-
pyb_thread_cur->arg = NULL;
45-
pyb_thread_cur->stack = &_heap_end;
46-
pyb_thread_cur->stack_len = ((uint32_t)&_estack - (uint32_t)&_heap_end) / sizeof(uint32_t);
47-
pyb_thread_cur->prev = thread;
48-
pyb_thread_cur->next = thread;
65+
thread->sp = NULL; // will be set when this thread switches out
66+
thread->local_state = 0; // will be set by mp_thread_init
67+
thread->arg = NULL;
68+
thread->stack = &_heap_end;
69+
thread->stack_len = ((uint32_t)&_estack - (uint32_t)&_heap_end) / sizeof(uint32_t);
70+
thread->all_next = NULL;
71+
thread->run_prev = thread;
72+
thread->run_next = thread;
73+
thread->queue_next = NULL;
74+
}
75+
76+
void pyb_thread_deinit() {
77+
uint32_t irq_state = disable_irq();
78+
pyb_thread_enabled = 0;
79+
pyb_thread_all = pyb_thread_cur;
80+
pyb_thread_cur->all_next = NULL;
81+
pyb_thread_cur->run_prev = pyb_thread_cur;
82+
pyb_thread_cur->run_next = pyb_thread_cur;
83+
enable_irq(irq_state);
4984
}
5085

5186
STATIC void pyb_thread_terminate(void) {
52-
uint32_t irq_state = raise_irq_pri(IRQ_PRI_PENDSV);
53-
pyb_thread_cur->prev->next = pyb_thread_cur->next;
54-
pyb_thread_cur->next->prev = pyb_thread_cur->prev;
55-
if (pyb_thread_cur->next == pyb_thread_cur->prev) {
87+
uint32_t irq_state = disable_irq();
88+
pyb_thread_t *thread = pyb_thread_cur;
89+
// take current thread off the run list
90+
pyb_thread_remove_from_runable(thread);
91+
// take current thread off the list of all threads
92+
for (pyb_thread_t **n = (pyb_thread_t**)&pyb_thread_all;; n = &(*n)->all_next) {
93+
if (*n == thread) {
94+
*n = thread->all_next;
95+
break;
96+
}
97+
}
98+
// clean pointers as much as possible to help GC
99+
thread->all_next = NULL;
100+
thread->queue_next = NULL;
101+
thread->stack = NULL;
102+
if (pyb_thread_all->all_next == NULL) {
103+
// only 1 thread left
56104
pyb_thread_enabled = 0;
57105
}
58-
restore_irq_pri(irq_state);
59-
pyb_thread_yield(); // should not return
106+
// thread switch will occur after we enable irqs
107+
SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
108+
enable_irq(irq_state);
109+
// should not return
110+
__fatal_error("could not terminate");
60111
}
61112

62113
uint32_t pyb_thread_new(pyb_thread_t *thread, void *stack, size_t stack_len, void *entry, void *arg) {
@@ -77,21 +128,105 @@ uint32_t pyb_thread_new(pyb_thread_t *thread, void *stack, size_t stack_len, voi
77128
thread->arg = arg;
78129
thread->stack = stack;
79130
thread->stack_len = stack_len;
80-
uint32_t irq_state = raise_irq_pri(IRQ_PRI_PENDSV);
131+
thread->queue_next = NULL;
132+
uint32_t irq_state = disable_irq();
81133
pyb_thread_enabled = 1;
82-
thread->next = pyb_thread_cur->next;
83-
thread->prev = pyb_thread_cur;
84-
pyb_thread_cur->next->prev = thread;
85-
pyb_thread_cur->next = thread;
86-
restore_irq_pri(irq_state);
134+
thread->all_next = pyb_thread_all;
135+
pyb_thread_all = thread;
136+
pyb_thread_add_to_runable(thread);
137+
enable_irq(irq_state);
87138
return (uint32_t)thread; // success
88139
}
89140

141+
void pyb_thread_dump(void) {
142+
if (!pyb_thread_enabled) {
143+
printf("THREAD: only main thread\n");
144+
} else {
145+
printf("THREAD:\n");
146+
for (pyb_thread_t *th = pyb_thread_all; th != NULL; th = th->all_next) {
147+
bool runable = false;
148+
for (pyb_thread_t *th2 = pyb_thread_cur;; th2 = th2->run_next) {
149+
if (th == th2) {
150+
runable = true;
151+
break;
152+
}
153+
if (th2->run_next == pyb_thread_cur) {
154+
break;
155+
}
156+
}
157+
printf(" id=%p sp=%p sz=%u", th, th->stack, th->stack_len);
158+
if (runable) {
159+
printf(" (runable)");
160+
}
161+
printf("\n");
162+
}
163+
}
164+
}
165+
90166
// should only be called from pendsv_isr_handler
91167
void *pyb_thread_next(void *sp) {
92168
pyb_thread_cur->sp = sp;
93-
pyb_thread_cur = pyb_thread_cur->next;
169+
pyb_thread_cur = pyb_thread_cur->run_next;
170+
pyb_thread_cur->timeslice = 4; // in milliseconds
94171
return pyb_thread_cur->sp;
95172
}
96173

174+
void pyb_mutex_init(pyb_mutex_t *m) {
175+
*m = PYB_MUTEX_UNLOCKED;
176+
}
177+
178+
int pyb_mutex_lock(pyb_mutex_t *m, int wait) {
179+
uint32_t irq_state = disable_irq();
180+
if (*m == PYB_MUTEX_UNLOCKED) {
181+
// mutex is available
182+
*m = PYB_MUTEX_LOCKED;
183+
enable_irq(irq_state);
184+
} else {
185+
// mutex is locked
186+
if (!wait) {
187+
enable_irq(irq_state);
188+
return 0; // failed to lock mutex
189+
}
190+
if (*m == PYB_MUTEX_LOCKED) {
191+
*m = pyb_thread_cur;
192+
} else {
193+
for (pyb_thread_t *n = *m;; n = n->queue_next) {
194+
if (n->queue_next == NULL) {
195+
n->queue_next = pyb_thread_cur;
196+
break;
197+
}
198+
}
199+
}
200+
pyb_thread_cur->queue_next = NULL;
201+
// take current thread off the run list
202+
pyb_thread_remove_from_runable(pyb_thread_cur);
203+
// thread switch will occur after we enable irqs
204+
SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
205+
enable_irq(irq_state);
206+
// when we come back we have the mutex
207+
}
208+
return 1; // have mutex
209+
}
210+
211+
void pyb_mutex_unlock(pyb_mutex_t *m) {
212+
uint32_t irq_state = disable_irq();
213+
if (*m == PYB_MUTEX_LOCKED) {
214+
// no threads are blocked on the mutex
215+
*m = PYB_MUTEX_UNLOCKED;
216+
} else {
217+
// at least one thread is blocked on this mutex
218+
pyb_thread_t *th = *m;
219+
if (th->queue_next == NULL) {
220+
// no other threads are blocked
221+
*m = PYB_MUTEX_LOCKED;
222+
} else {
223+
// at least one other thread is still blocked
224+
*m = th->queue_next;
225+
}
226+
// put unblocked thread on runable list
227+
pyb_thread_add_to_runable(th);
228+
}
229+
enable_irq(irq_state);
230+
}
231+
97232
#endif // MICROPY_PY_THREAD

‎stmhal/pybthread.h‎

Lines changed: 21 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -33,15 +33,23 @@ typedef struct _pyb_thread_t {
3333
void *arg; // thread Python args, a GC root pointer
3434
void *stack; // pointer to the stack
3535
size_t stack_len; // number of words in the stack
36-
struct _pyb_thread_t *prev;
37-
struct _pyb_thread_t *next;
36+
uint32_t timeslice;
37+
struct _pyb_thread_t *all_next;
38+
struct _pyb_thread_t *run_prev;
39+
struct _pyb_thread_t *run_next;
40+
struct _pyb_thread_t *queue_next;
3841
} pyb_thread_t;
3942

40-
extern int pyb_thread_enabled;
41-
extern pyb_thread_t *pyb_thread_cur;
43+
typedef pyb_thread_t *pyb_mutex_t;
44+
45+
extern volatile int pyb_thread_enabled;
46+
extern pyb_thread_t *volatile pyb_thread_all;
47+
extern pyb_thread_t *volatile pyb_thread_cur;
4248

4349
void pyb_thread_init(pyb_thread_t *th);
50+
void pyb_thread_deinit();
4451
uint32_t pyb_thread_new(pyb_thread_t *th, void *stack, size_t stack_len, void *entry, void *arg);
52+
void pyb_thread_dump(void);
4553

4654
static inline uint32_t pyb_thread_get_id(void) {
4755
return (uint32_t)pyb_thread_cur;
@@ -56,7 +64,15 @@ static inline void *pyb_thread_get_local(void) {
5664
}
5765

5866
static inline void pyb_thread_yield(void) {
59-
SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
67+
if (pyb_thread_cur->run_next == pyb_thread_cur) {
68+
__WFI();
69+
} else {
70+
SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
71+
}
6072
}
6173

74+
void pyb_mutex_init(pyb_mutex_t *m);
75+
int pyb_mutex_lock(pyb_mutex_t *m, int wait);
76+
void pyb_mutex_unlock(pyb_mutex_t *m);
77+
6278
#endif // MICROPY_INCLUDED_STMHAL_PYBTHREAD_H

0 commit comments

Comments
 (0)