rp2: Fix power consumption when sleeping with a timeout.

Fixes a regression introduced in 3af006efb39ad0b7aa7c0401c93329b654bca617
where WFE never blocked in `mp_wfe_or_timeout()` function and would
busy-wait instead.  This increases power consumption measurably.

Root cause is that `mp_wfe_or_timeout()` calls soft timer functions that
(after the regression) call `recursive_mutex_enter()` and
`recursive_mutex_exit()`.  The exit calls
`lock_internal_spin_unlock_with_notify()` and the default pico-sdk
implementation of this macro issues a SEV which negates the WFE that
follows it, meaning the CPU never suspends.

See https://forums.raspberrypi.com/viewtopic.php?p=2233908 for more
details.

The fix in this comment adds a custom "nowait" variant mutex that doesn't
do WFE/SEV, and uses this one for PendSV.  This will use more power when
there's contention for the PendSV mutex as the other core will spin, but
this shouldn't happen very often.

This work was funded through GitHub Sponsors.

Signed-off-by: Angus Gratton <angus@redyak.com.au>

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);
+}

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

ports/rp2/pendsv.c

@@ -25,8 +25,8 @@
  */
 
 #include <assert.h>
-#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);
 }