fix(threads/smp): sched on other core if sched is already pending

When a thread becomes ready, the scheduler on the core with the lowest
prio running thread will be triggered. If now another thread becomes
ready as well and the scheduler didn't have a chance to run yet (e.g.
because interrupts are still disabled), the same scheduler will be
triggered again, but only one thread is then selected and can run.
The other thread is "skipped".
To solve this, the scheduler on the other core should be triggered as
well in this scenario so that both schedulers get the most recent state
and the two highest prio threads are run.

src/riot-rs-threads/src/smp/rp2040.rs

@@ -1,5 +1,6 @@
 use crate::arch::{Arch as _, Cpu};
 
+use cortex_m::peripheral::SCB;
 use embassy_rp::{
     interrupt,
     multicore::{spawn_core1, Stack},
@@ -34,19 +35,33 @@ impl Multicore for Chip {
 
     fn schedule_on_core(id: CoreId) {
         if id == Self::core_id() {
-            crate::schedule();
-            return;
+            schedule();
+        } else {
+            schedule_other_core();
         }
+    }
+}
 
-        // Use the FIFO queue between the cores to trigger the scheduler
-        // on the other core.
-        let sio = SIO;
-        // If its already full, no need to send another `SCHEDULE_TOKEN`.
-        if !sio.fifo().st().read().rdy() {
-            return;
-        }
-        sio.fifo().wr().write_value(SCHEDULE_TOKEN);
+fn schedule() {
+    if SCB::is_pendsv_pending() {
+        // If a scheduling attempt is already pending, there must have been multiple
+        // changes in the runqueue at the same time.
+        // Trigger the scheduler on the other core as well to make sure that both schedulers
+        // have the most recent runqueue state.
+        return schedule_other_core();
+    }
+    crate::schedule()
+}
+
+fn schedule_other_core() {
+    // Use the FIFO queue between the cores to trigger the scheduler
+    // on the other core.
+    let sio = SIO;
+    // If its already full, no need to send another `SCHEDULE_TOKEN`.
+    if !sio.fifo().st().read().rdy() {
+        return;
     }
+    sio.fifo().wr().write_value(SCHEDULE_TOKEN);
 }
 
 const SCHEDULE_TOKEN: u32 = 0x11;
@@ -72,7 +87,7 @@ fn handle_fifo_msg() {
 
     while sio.fifo().st().read().vld() {
         if sio.fifo().rd().read() == SCHEDULE_TOKEN {
-            crate::schedule();
+            schedule();
         }
     }
 }