feat(threads): sort ThreadList by priority (#381)

Cargo.toml

@@ -23,6 +23,7 @@ members = [
   "tests/gpio",
   "tests/gpio-interrupt-nrf",
   "tests/gpio-interrupt-stm32",
+  "tests/threading-lock",
 ]
 
 exclude = ["src/lib"]

src/riot-rs-threads/src/threadlist.rs

@@ -1,6 +1,6 @@
 use critical_section::CriticalSection;
 
-use crate::{ThreadId, ThreadState, THREADS};
+use crate::{thread::Thread, ThreadId, ThreadState, THREADS};
 
 /// Manages blocked [`super::Thread`]s for a resource, and triggering the scheduler when needed.
 #[derive(Debug, Default)]
@@ -18,10 +18,22 @@ impl ThreadList {
     /// Puts the current (blocked) thread into this [`ThreadList`] and triggers the scheduler.
     pub fn put_current(&mut self, cs: CriticalSection, state: ThreadState) {
         THREADS.with_mut_cs(cs, |mut threads| {
-            let thread_id = threads.current_thread.unwrap();
-            threads.thread_blocklist[usize::from(thread_id)] = self.head;
-            self.head = Some(thread_id);
-            threads.set_state(thread_id, state);
+            let &mut Thread { pid, prio, .. } = threads.current().unwrap();
+            let mut curr = None;
+            let mut next = self.head;
+            while let Some(n) = next {
+                if threads.get_unchecked_mut(n).prio < prio {
+                    break;
+                }
+                curr = next;
+                next = threads.thread_blocklist[usize::from(n)];
+            }
+            threads.thread_blocklist[usize::from(pid)] = next;
+            match curr {
+                Some(curr) => threads.thread_blocklist[usize::from(curr)] = Some(pid),
+                _ => self.head = Some(pid),
+            }
+            threads.set_state(pid, state);
             crate::schedule();
         });
     }

tests/laze.yml

@@ -3,3 +3,4 @@ subdirs:
   - gpio
   - gpio-interrupt-nrf
   - gpio-interrupt-stm32
+  - threading-lock

tests/threading-lock/Cargo.toml

@@ -0,0 +1,13 @@
+[package]
+name = "threading-lock"
+version = "0.1.0"
+authors = ["Elena Frank <[email protected]>"]
+edition.workspace = true
+license.workspace = true
+publish = false
+
+[dependencies]
+embassy-executor = { workspace = true }
+riot-rs = { path = "../../src/riot-rs", features = ["threading", "time"] }
+riot-rs-boards = { path = "../../src/riot-rs-boards" }
+portable-atomic = "1.6.0"

tests/threading-lock/laze.yml

@@ -0,0 +1,6 @@
+apps:
+  - name: threading-lock
+    selects:
+      - ?release
+      - executor-thread
+      - sw/threading

tests/threading-lock/src/main.rs

@@ -0,0 +1,72 @@
+#![no_main]
+#![no_std]
+#![feature(type_alias_impl_trait)]
+#![feature(used_with_arg)]
+
+use portable_atomic::{AtomicUsize, Ordering};
+use riot_rs::thread::{lock::Lock, thread_flags, ThreadId};
+
+static LOCK: Lock = Lock::new();
+static RUN_ORDER: AtomicUsize = AtomicUsize::new(0);
+static LOCK_ORDER: AtomicUsize = AtomicUsize::new(0);
+
+#[riot_rs::thread(autostart, priority = 1)]
+fn thread0() {
+    assert_eq!(RUN_ORDER.fetch_add(1, Ordering::AcqRel), 0);
+
+    LOCK.acquire();
+
+    // Unblock other threads in the order of their IDs.
+    //
+    // Because all other threads have higher priorities, setting
+    // a flag will each time cause a context switch and give each
+    // thread the chance to run and try acquire the lock.
+    thread_flags::set(ThreadId::new(1), 0b1);
+    thread_flags::set(ThreadId::new(2), 0b1);
+    thread_flags::set(ThreadId::new(3), 0b1);
+
+    assert_eq!(LOCK_ORDER.fetch_add(1, Ordering::AcqRel), 0);
+
+    LOCK.release();
+
+    // Wait for other threads to complete.
+    thread_flags::wait_all(0b111);
+    riot_rs::debug::log::info!("Test passed!");
+}
+
+#[riot_rs::thread(autostart, priority = 2)]
+fn thread1() {
+    thread_flags::wait_one(0b1);
+    assert_eq!(RUN_ORDER.fetch_add(1, Ordering::AcqRel), 1);
+
+    LOCK.acquire();
+    assert_eq!(LOCK_ORDER.fetch_add(1, Ordering::AcqRel), 2);
+    LOCK.release();
+
+    thread_flags::set(ThreadId::new(0), 0b1);
+}
+
+#[riot_rs::thread(autostart, priority = 3)]
+fn thread2() {
+    thread_flags::wait_one(0b1);
+    assert_eq!(RUN_ORDER.fetch_add(1, Ordering::AcqRel), 2);
+
+    LOCK.acquire();
+    // Expect to be the second thread that acquires the lock.
+    assert_eq!(LOCK_ORDER.fetch_add(1, Ordering::AcqRel), 1);
+    LOCK.release();
+
+    thread_flags::set(ThreadId::new(0), 0b10);
+}
+
+#[riot_rs::thread(autostart, priority = 2)]
+fn thread3() {
+    thread_flags::wait_one(0b1);
+    assert_eq!(RUN_ORDER.fetch_add(1, Ordering::AcqRel), 3);
+
+    LOCK.acquire();
+    assert_eq!(LOCK_ORDER.fetch_add(1, Ordering::AcqRel), 3);
+    LOCK.release();
+
+    thread_flags::set(ThreadId::new(0), 0b100);
+}