From ed48734cf611e5b9c526a96ba306656532f84db0 Mon Sep 17 00:00:00 2001
From: Elena Frank <elena.frank@protonmail.com>
Date: Wed, 31 Jul 2024 14:03:07 +0200
Subject: [PATCH] feat(threads): dynamic thread priorities

---
 src/riot-rs-runqueue/src/runqueue.rs | 13 +++++-
 src/riot-rs-threads/src/lib.rs       | 62 ++++++++++++++++++++++++++--
 2 files changed, 71 insertions(+), 4 deletions(-)

diff --git a/src/riot-rs-runqueue/src/runqueue.rs b/src/riot-rs-runqueue/src/runqueue.rs
index bd851d9f0..d571aca61 100644
--- a/src/riot-rs-runqueue/src/runqueue.rs
+++ b/src/riot-rs-runqueue/src/runqueue.rs
@@ -1,7 +1,7 @@
 // Disable indexing lints for now
 #![allow(clippy::indexing_slicing)]
 
-use core::mem;
+use core::{mem, ops::Deref};
 
 use self::clist::CList;
 
@@ -24,6 +24,13 @@ impl From<RunqueueId> for usize {
     }
 }
 
+impl Deref for RunqueueId {
+    type Target = u8;
+    fn deref(&self) -> &Self::Target {
+        &self.0
+    }
+}
+
 #[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct ThreadId(u8);
@@ -81,6 +88,10 @@ impl<const N_QUEUES: usize, const N_THREADS: usize> RunQueue<{ N_QUEUES }, { N_T
         self.queues.push(n.0, rq.0);
     }
 
+    pub fn peek_head(&self, rq: RunqueueId) -> Option<ThreadId> {
+        self.queues.peek_head(rq.0).map(ThreadId)
+    }
+
     /// Removes th head with pid `n` from runqueue number `rq`.
     ///
     /// # Panics
diff --git a/src/riot-rs-threads/src/lib.rs b/src/riot-rs-threads/src/lib.rs
index d6e32c55c..eecbd7533 100644
--- a/src/riot-rs-threads/src/lib.rs
+++ b/src/riot-rs-threads/src/lib.rs
@@ -109,9 +109,16 @@ impl Threads {
         }
     }
 
-    // fn get_unchecked(&self, thread_id: ThreadId) -> &Thread {
-    //     &self.threads[thread_id as usize]
-    // }
+    /// Returns access to any thread data.
+    ///
+    /// # Panics
+    ///
+    /// Panics if `thread_id` is >= [`THREADS_NUMOF`].
+    /// If the thread for this `thread_id` is in an invalid state, the
+    /// data in the returned [`Thread`] is undefined, i.e. empty or outdated.
+    fn get_unchecked(&self, thread_id: ThreadId) -> &Thread {
+        &self.threads[usize::from(thread_id)]
+    }
 
     /// Returns mutable access to any thread data.
     ///
@@ -172,6 +179,37 @@ impl Threads {
             None
         }
     }
+
+    fn get_priority(&self, thread_id: ThreadId) -> Option<RunqueueId> {
+        self.is_valid_pid(thread_id)
+            .then(|| self.get_unchecked(thread_id).prio)
+    }
+
+    /// Changes the priority of a thread.
+    ///
+    /// Returns true if the thread is in the runqueue and the order might have
+    /// changed.
+    fn set_priority(&mut self, thread_id: ThreadId, prio: RunqueueId) -> bool {
+        if !self.is_valid_pid(thread_id) {
+            return false;
+        }
+        let thread = self.get_unchecked_mut(thread_id);
+        let old_prio = thread.prio;
+        if old_prio == prio {
+            return false;
+        }
+        thread.prio = prio;
+        if thread.state != ThreadState::Running {
+            return false;
+        }
+        if self.runqueue.peek_head(old_prio) == Some(thread_id) {
+            self.runqueue.pop_head(thread_id, old_prio);
+        } else {
+            self.runqueue.del(thread_id);
+        }
+        self.runqueue.add(thread_id, prio);
+        true
+    }
 }
 
 /// Starts threading.
@@ -322,6 +360,24 @@ pub fn wakeup(thread_id: ThreadId) -> bool {
     })
 }
 
+/// Returns the priority of a thread.
+///
+/// Returns `None` if this is not a valid thread.
+pub fn get_priority(thread_id: ThreadId) -> Option<u8> {
+    THREADS.with_mut(|threads| threads.get_priority(thread_id).map(|rq| *rq))
+}
+
+/// Changes the priority of a thread.
+///
+/// This might trigger a context switch.
+pub fn set_priority(thread_id: ThreadId, prio: u8) {
+    THREADS.with_mut(|mut threads| {
+        if threads.set_priority(thread_id, RunqueueId::new(prio)) {
+            schedule();
+        }
+    })
+}
+
 /// Returns the size of the internal structure that holds the
 /// a thread's data.
 pub fn thread_struct_size() -> usize {