diff --git a/ports/rp2/mutex_extra.c b/ports/rp2/mutex_extra.c index 7a70a40ac..db8419170 100644 --- a/ports/rp2/mutex_extra.c +++ b/ports/rp2/mutex_extra.c @@ -33,3 +33,22 @@ void __time_critical_func(recursive_mutex_exit_and_restore_interrupts)(recursive } lock_internal_spin_unlock_with_notify(&mtx->core, save); } + +void __time_critical_func(recursive_mutex_nowait_enter_blocking)(recursive_mutex_nowait_t * mtx) { + while (!recursive_mutex_try_enter(&mtx->mutex, NULL)) { + tight_loop_contents(); + } +} + +void __time_critical_func(recursive_mutex_nowait_exit)(recursive_mutex_nowait_t * wrapper) { + recursive_mutex_t *mtx = &wrapper->mutex; + // Rest of this function is a copy of recursive_mutex_exit(), with + // lock_internal_spin_unlock_with_notify() removed. + uint32_t save = spin_lock_blocking(mtx->core.spin_lock); + assert(lock_is_owner_id_valid(mtx->owner)); + assert(mtx->enter_count); + if (!--mtx->enter_count) { + mtx->owner = LOCK_INVALID_OWNER_ID; + } + spin_unlock(mtx->core.spin_lock, save); +} diff --git a/ports/rp2/mutex_extra.h b/ports/rp2/mutex_extra.h index 61b6b4035..5f4a2c364 100644 --- a/ports/rp2/mutex_extra.h +++ b/ports/rp2/mutex_extra.h @@ -31,4 +31,28 @@ uint32_t recursive_mutex_enter_blocking_and_disable_interrupts(recursive_mutex_t *mtx); void recursive_mutex_exit_and_restore_interrupts(recursive_mutex_t *mtx, uint32_t save); +// Alternative version of recursive_mutex_t that doesn't issue WFE and SEV +// instructions. This means it will use more power (busy-waits), but exiting +// this mutex doesn't disrupt the calling CPU's event state in the same way a +// recursive mutex does (because recurse_mutex_exit() executes SEV each time the +// mutex is released.) +// +// Implement as a wrapper type because no longer compatible with the normal +// recursive_mutex functions. + +typedef struct { + recursive_mutex_t mutex; +} recursive_mutex_nowait_t; + +inline static void recursive_mutex_nowait_init(recursive_mutex_nowait_t *mtx) { + recursive_mutex_init(&mtx->mutex); +} + +inline static bool recursive_mutex_nowait_try_enter(recursive_mutex_nowait_t *mtx, uint32_t *owner_out) { + return recursive_mutex_try_enter(&mtx->mutex, owner_out); +} + +void recursive_mutex_nowait_enter_blocking(recursive_mutex_nowait_t *mtx); +void recursive_mutex_nowait_exit(recursive_mutex_nowait_t *mtx); + #endif // MICROPY_INCLUDED_RP2_MUTEX_EXTRA_H diff --git a/ports/rp2/pendsv.c b/ports/rp2/pendsv.c index 68bb65f04..6cfe624c3 100644 --- a/ports/rp2/pendsv.c +++ b/ports/rp2/pendsv.c @@ -25,8 +25,8 @@ */ #include -#include "pico/mutex.h" #include "py/mpconfig.h" +#include "mutex_extra.h" #include "pendsv.h" #include "RP2040.h" @@ -35,21 +35,25 @@ #endif static pendsv_dispatch_t pendsv_dispatch_table[PENDSV_DISPATCH_NUM_SLOTS]; -static recursive_mutex_t pendsv_mutex; + +// Using the nowait variant here as softtimer updates PendSV from the loop of mp_wfe_or_timeout(), +// where we don't want the CPU event bit to be set. +static recursive_mutex_nowait_t pendsv_mutex; void pendsv_init(void) { - recursive_mutex_init(&pendsv_mutex); + recursive_mutex_nowait_init(&pendsv_mutex); } void pendsv_suspend(void) { // Recursive Mutex here as either core may call pendsv_suspend() and expect // both mutual exclusion (other core can't enter pendsv_suspend() at the // same time), and that no PendSV handler will run. - recursive_mutex_enter_blocking(&pendsv_mutex); + recursive_mutex_nowait_enter_blocking(&pendsv_mutex); } void pendsv_resume(void) { - recursive_mutex_exit(&pendsv_mutex); + recursive_mutex_nowait_exit(&pendsv_mutex); + // Run pendsv if needed. Find an entry with a dispatch and call pendsv dispatch // with it. If pendsv runs it will service all slots. int count = PENDSV_DISPATCH_NUM_SLOTS; @@ -63,7 +67,7 @@ void pendsv_resume(void) { void pendsv_schedule_dispatch(size_t slot, pendsv_dispatch_t f) { pendsv_dispatch_table[slot] = f; - if (pendsv_mutex.enter_count == 0) { + if (pendsv_mutex.mutex.enter_count == 0) { // There is a race here where other core calls pendsv_suspend() before // ISR can execute, but dispatch will happen later when other core // calls pendsv_resume(). @@ -78,13 +82,13 @@ void pendsv_schedule_dispatch(size_t slot, pendsv_dispatch_t f) { // PendSV interrupt handler to perform background processing. void PendSV_Handler(void) { - if (!recursive_mutex_try_enter(&pendsv_mutex, NULL)) { + if (!recursive_mutex_nowait_try_enter(&pendsv_mutex, NULL)) { // Failure here means core 1 holds pendsv_mutex. ISR will // run again after core 1 calls pendsv_resume(). return; } // Core 0 should not already have locked pendsv_mutex - assert(pendsv_mutex.enter_count == 1); + assert(pendsv_mutex.mutex.enter_count == 1); #if MICROPY_PY_NETWORK_CYW43 CYW43_STAT_INC(PENDSV_RUN_COUNT); @@ -98,5 +102,5 @@ void PendSV_Handler(void) { } } - recursive_mutex_exit(&pendsv_mutex); + recursive_mutex_nowait_exit(&pendsv_mutex); }