1 files changed, 24 insertions, 5 deletions

diff --git a/embassy-executor/src/raw/run_queue.rs b/embassy-executor/src/raw/run_queue.rs
index d98c26f73..b8b052310 100644
--- a/embassy-executor/src/raw/run_queue.rs
+++ b/embassy-executor/src/raw/run_queue.rs
@@ -2,7 +2,7 @@ use core::ptr::{addr_of_mut, NonNull};
 
 use cordyceps::sorted_list::Links;
 use cordyceps::Linked;
-#[cfg(feature = "scheduler-deadline")]
+#[cfg(any(feature = "scheduler-priority", feature = "scheduler-deadline"))]
 use cordyceps::SortedList;
 
 #[cfg(target_has_atomic = "ptr")]
@@ -83,7 +83,7 @@ impl RunQueue {
     /// Empty the queue, then call `on_task` for each task that was in the queue.
     /// NOTE: It is OK for `on_task` to enqueue more tasks. In this case they're left in the queue
     /// and will be processed by the *next* call to `dequeue_all`, *not* the current one.
-    #[cfg(not(feature = "scheduler-deadline"))]
+    #[cfg(not(any(feature = "scheduler-priority", feature = "scheduler-deadline")))]
     pub(crate) fn dequeue_all(&self, on_task: impl Fn(TaskRef)) {
         let taken = self.stack.take_all();
         for taskref in taken {
@@ -106,10 +106,29 @@ impl RunQueue {
     ///
     /// This process will repeat until the local `sorted` queue AND the global
     /// runqueue are both empty, at which point this function will return.
-    #[cfg(feature = "scheduler-deadline")]
+    #[cfg(any(feature = "scheduler-priority", feature = "scheduler-deadline"))]
     pub(crate) fn dequeue_all(&self, on_task: impl Fn(TaskRef)) {
-        let mut sorted =
-            SortedList::<TaskHeader>::new_with_cmp(|lhs, rhs| lhs.metadata.deadline().cmp(&rhs.metadata.deadline()));
+        let mut sorted = SortedList::<TaskHeader>::new_with_cmp(|lhs, rhs| {
+            // compare by priority first
+            #[cfg(feature = "scheduler-priority")]
+            {
+                let lp = lhs.metadata.priority();
+                let rp = rhs.metadata.priority();
+                if lp != rp {
+                    return lp.cmp(&rp).reverse();
+                }
+            }
+            // compare deadlines in case of tie.
+            #[cfg(feature = "scheduler-deadline")]
+            {
+                let ld = lhs.metadata.deadline();
+                let rd = rhs.metadata.deadline();
+                if ld != rd {
+                    return ld.cmp(&rd);
+                }
+            }
+            core::cmp::Ordering::Equal
+        });
 
         loop {
             // For each loop, grab any newly pended items