fix(threads): don't re-add running thread to rq outside of sched

src/riot-rs-threads/src/arch/cortex_m.rs

@@ -185,13 +185,13 @@ unsafe extern "C" fn PendSV() {
 /// This function is called in PendSV.
 #[no_mangle]
 unsafe fn sched() -> u128 {
-    #[cfg(feature = "multicore")]
-    THREADS.with_mut(|mut threads| threads.add_current_thread_to_rq());
-
     loop {
         if let Some(res) = critical_section::with(|cs| {
             let threads = unsafe { &mut *THREADS.as_ptr(cs) };
 
+            #[cfg(feature = "multicore")]
+            threads.add_current_thread_to_rq();
+
             let next_pid = match threads.get_next_pid() {
                 Some(pid) => pid,
                 None => {

src/riot-rs-threads/src/lib.rs

@@ -217,19 +217,25 @@ impl Threads {
         let prio = thread.prio;
         match (old_state, state) {
             (old, new) if new == old => {}
-            (old, ThreadState::Running) => {
+            (ThreadState::Invalid, ThreadState::Running) if self.current_pid().is_none() => {
                 self.runqueue.add(pid, prio);
-                if old == ThreadState::Invalid && self.current_pid().is_none() {
-                    return old;
-                }
+            }
+            (_, ThreadState::Running) => {
                 #[cfg(not(feature = "multicore"))]
                 {
+                    self.runqueue.add(pid, prio);
                     if prio > self.get_unchecked_mut(pid).prio {
                         schedule()
                     }
                 }
                 #[cfg(feature = "multicore")]
                 {
+                    if self.current_threads.contains(&Some(pid)) {
+                        // If the thread is currently running, the scheduler
+                        // will re-add it to the runqueue in `sched`.
+                        return old_state;
+                    }
+                    self.runqueue.add(pid, prio);
                     let (core, lowest_prio) = self.lowest_running_prio(pid);
                     if lowest_prio <= Some(prio) {
                         schedule_on_core(core);