Improve best_effort_wfe_or_timeout

src/common/pico_time/include/pico/time.h

@@ -284,6 +284,8 @@ void sleep_ms(uint32_t ms);
  *     return false; // timed out
  * }
  * ```
+ * NOTE: This method should always be used in a loop associated with checking another "event" variable, since
+ * processor events are a shared resource and can happen for a large number of reasons.
  *
  * @param timeout_timestamp the timeout time
  * @return true if the target time is reached, false otherwise

src/common/pico_time/time.c

@@ -136,7 +136,9 @@ static void alarm_pool_irq_handler(void);
 static void alarm_pool_irq_handler(void) {
     // This IRQ handler does the main work, as it always (assuming the IRQ hasn't been enabled on both cores
     // which is unsupported) run on the alarm pool's core, and can't be preempted by itself, meaning
-    // that it doesn't need locks except to protect against linked list access
+    // that it doesn't need locks except to protect against linked list, or other state access.
+    // This simplifies the code considerably, and makes it much faster in general, even though we are forced to take
+    // two IRQs per alarm.
     uint timer_alarm_num;
     alarm_pool_timer_t *timer = ta_from_current_irq(&timer_alarm_num);
     uint timer_num = ta_timer_num(timer);
@@ -263,9 +265,17 @@ static void alarm_pool_irq_handler(void) {
         // need to wait
         alarm_pool_entry_t *earliest_entry = &pool->entries[earliest_index];
         earliest_target = earliest_entry->target;
-        ta_set_timeout(timer, timer_alarm_num, earliest_target);
-        // check we haven't now past the target time; if not we don't want to loop again
+        // we are leaving a timeout every 2^32 microseconds anyway if there is no valid target, so we can choose any value.
+        // best_effort_wfe_or_timeout now relies on it being the last value set, and arguably this is the
+        // best value anyway, as it is the furthest away from the last fire.
+        if (earliest_target != -1) { // cancelled alarm has target of -1
+            ta_set_timeout(timer, timer_alarm_num, earliest_target);
+        }
+        // check we haven't now passed the target time; if not we don't want to loop again
     } while ((earliest_target - (int64_t)ta_time_us_64(timer)) <= 0);
+    // We always want the timer IRQ to wake a WFE so that best_effort_wfe_or_timeout() will wake up. It will wake
+    // a WFE on its own core by nature of having taken an IRQ, but we do an explicit SEV so it wakes the other core
+    __sev();
 }
 
 void alarm_pool_post_alloc_init(alarm_pool_t *pool, alarm_pool_timer_t *timer, uint hardware_alarm_num, uint max_timers) {
@@ -437,26 +447,48 @@ bool best_effort_wfe_or_timeout(absolute_time_t timeout_timestamp) {
         return time_reached(timeout_timestamp);
     } else {
         alarm_id_t id;
-        id = add_alarm_at(timeout_timestamp, sleep_until_callback, NULL, false);
-        if (id <= 0) {
-            tight_loop_contents();
+        // note that as of SDK 2.0.0 calling add_alarm_at always causes a SEV. What we really
+        // want to do is cause an IRQ at the specified time in the future if there is not
+        // an IRQ already happening before then. The problem is that the IRQ may be happening on the
+        // other core, so taking an IRQ is the only way to get the state protection.
+        //
+        // Therefore, we make a compromise; we will set the alarm, if we won't wake up before the right time
+        // already. This means that repeated calls to this function with the same timeout will work correctly
+        // after the first one! This is fine, because we ask callers to use a polling loop on another
+        // event variable when using this function.
+        //
+        // For this to work, we require that once we have set an alarm, an SEV happens no later than that, even
+        // if we cancel the alarm as we do below. Therefore, the IRQ handler (which is always enabled) will
+        // never set its wakeup time to a later value, but instead wake up once and then wake up again.
+        //
+        // This overhead when canceling alarms is a small price to pay for the much simpler/faster/cleaner
+        // implementation that relies on the IRQ handler (on a single core) being the only state accessor.
+        //
+        // Note also, that the use of software spin locks on RP2350 to access state would always cause a SEV
+        // due to use of LDREX etc., so actually using spin locks to protect the state would be worse.
+        if (ta_wakes_up_on_or_before(alarm_pool_get_default()->timer, alarm_pool_get_default()->timer_alarm_num,
+                                     (int64_t)to_us_since_boot(timeout_timestamp))) {
+            // we already are waking up at or before when we want to (possibly due to us having been called
+            // before in a loop), so we can do an actual WFE. Note we rely on the fact that the alarm pool IRQ
+            // handler always does an explicit SEV, since it may be on the other core.
+            __wfe();
             return time_reached(timeout_timestamp);
         } else {
-            // the above alarm add now may force an IRQ which will wake us up,
-            // so we want to consume one __wfe.. we do an explicit __sev
-            // just to make sure there is one
-            __sev(); // make sure there is an event sow ee don't block
-            __wfe();
-            if (!time_reached(timeout_timestamp))
-            {
-                // ^ at the point above the timer hadn't fired, so it is safe
-                // to wait; the event will happen due to IRQ at some point between
-                // then and the correct wakeup time
-                __wfe();
+            id = add_alarm_at(timeout_timestamp, sleep_until_callback, NULL, false);
+            if (id <= 0) {
+                tight_loop_contents();
+                return time_reached(timeout_timestamp);
+            } else {
+                if (!time_reached(timeout_timestamp)) {
+                    // ^ at the point above the timer hadn't fired, so it is safe
+                    // to wait; the event will happen due to IRQ at some point between
+                    // then and the correct wakeup time
+                    __wfe();
+                }
+                // we need to clean up if it wasn't us that caused the wfe; if it was this will be a noop.
+                cancel_alarm(id);
+                return time_reached(timeout_timestamp);
             }
-            // we need to clean up if it wasn't us that caused the wfe; if it was this will be a noop.
-            cancel_alarm(id);
-            return time_reached(timeout_timestamp);
         }
     }
 #else

src/host/pico_time_adapter/include/pico/time_adapter.h

@@ -19,6 +19,7 @@ void ta_clear_force_irq(alarm_pool_timer_t *timer, uint hardware_alarm_num);
 void ta_clear_irq(alarm_pool_timer_t *timer, uint hardware_alarm_num);
 void ta_force_irq(alarm_pool_timer_t *timer, uint hardware_alarm_num);
 void ta_set_timeout(alarm_pool_timer_t *timer, uint hardware_alarm_num, int64_t target);
+void ta_wakes_up_on_or_before(alarm_pool_timer_t *timer, uint alarm_num, int64_t target);
 void ta_enable_irq_handler(alarm_pool_timer_t *timer, uint hardware_alarm_num, void (*irq_handler)(void));
 void ta_disable_irq_handler(alarm_pool_timer_t *timer, uint hardware_alarm_num, void (*irq_handler)(void));
 void ta_hardware_alarm_claim(alarm_pool_timer_t *timer, uint hardware_alarm_num);

src/host/pico_time_adapter/time_adapter.c

@@ -27,6 +27,10 @@ PICO_WEAK_FUNCTION_DEF(ta_set_timeout)
 void PICO_WEAK_FUNCTION_IMPL_NAME(ta_set_timeout)(alarm_pool_timer_t *timer, uint hardware_alarm_num, int64_t target) {
     panic_unsupported();
 }
+PICO_WEAK_FUNCTION_DEF(ta_wakes_up_on_or_before)
+void PICO_WEAK_FUNCTION_IMPL_NAME(ta_wakes_up_on_or_before)(alarm_pool_timer_t *timer, uint hardware_alarm_num, int64_t target) {
+    panic_unsupported();
+}
 PICO_WEAK_FUNCTION_DEF(ta_enable_irq_handler)
 void PICO_WEAK_FUNCTION_IMPL_NAME(ta_enable_irq_handler)(alarm_pool_timer_t *timer, uint hardware_alarm_num, void (*irq_handler)(void)) {
     panic_unsupported();

src/rp2_common/pico_time_adapter/include/pico/time_adapter.h

@@ -36,7 +36,28 @@ static inline alarm_pool_timer_t *ta_from_current_irq(uint *alarm_num) {
 }
 
 static inline void ta_set_timeout(alarm_pool_timer_t *timer, uint alarm_num, int64_t target) {
-    timer_hw_from_timer(timer)->alarm[alarm_num] = (uint32_t) target;
+    // We never want to set the timeout to be later than our current one.
+    uint32_t current = timer_time_us_32(timer_hw_from_timer(timer));
+    uint32_t time_til_target = (uint32_t) target - current;
+    uint32_t time_til_alarm = timer_hw_from_timer(timer)->alarm[alarm_num] - current;
+    // Note: we are only dealing with the low 32 bits of the timer values,
+    // so there is some opportunity to make wrap-around errors.
+    //
+    // 1. If we just passed the alarm time, then time_til_alarm will be high, meaning we will
+    //    likely do the update, but this is OK since the alarm will have just fired
+    // 2. If we just passed the target time, then time_til_target will be high, meaning we will
+    //    likely not do the update, but this is OK since the caller who has the full 64 bits
+    //    must check if the target time has passed when we return anyway to avoid races.
+    if (time_til_target < time_til_alarm) {
+        timer_hw_from_timer(timer)->alarm[alarm_num] = (uint32_t) target;
+    }
+}
+
+static inline bool ta_wakes_up_on_or_before(alarm_pool_timer_t *timer, uint alarm_num, int64_t target) {
+    uint32_t current = timer_time_us_32(timer_hw_from_timer(timer));
+    uint32_t time_til_target = (uint32_t) target - current;
+    uint32_t time_til_alarm = timer_hw_from_timer(timer)->alarm[alarm_num] - current;
+    return time_til_alarm <= time_til_target;
 }
 
 static inline uint64_t ta_time_us_64(alarm_pool_timer_t *timer) {