1 files changed, 29 insertions, 15 deletions

diff --git a/embassy-executor/src/raw/run_queue.rs b/embassy-executor/src/raw/run_queue.rs
index ed8c82a5c..f1ec19ac1 100644
--- a/embassy-executor/src/raw/run_queue.rs
+++ b/embassy-executor/src/raw/run_queue.rs
@@ -2,18 +2,18 @@ use core::ptr;
 use core::ptr::NonNull;
 
 use atomic_polyfill::{AtomicPtr, Ordering};
-use critical_section::CriticalSection;
 
-use super::TaskHeader;
+use super::{TaskHeader, TaskRef};
+use crate::raw::util::SyncUnsafeCell;
 
 pub(crate) struct RunQueueItem {
-    next: AtomicPtr<TaskHeader>,
+    next: SyncUnsafeCell<Option<TaskRef>>,
 }
 
 impl RunQueueItem {
     pub const fn new() -> Self {
         Self {
-            next: AtomicPtr::new(ptr::null_mut()),
+            next: SyncUnsafeCell::new(None),
         }
     }
 }
@@ -46,29 +46,43 @@ impl RunQueue {
     ///
     /// `item` must NOT be already enqueued in any queue.
     #[inline(always)]
-    pub(crate) unsafe fn enqueue(&self, _cs: CriticalSection, task: NonNull<TaskHeader>) -> bool {
-        let prev = self.head.load(Ordering::Relaxed);
-        task.as_ref().run_queue_item.next.store(prev, Ordering::Relaxed);
-        self.head.store(task.as_ptr(), Ordering::Relaxed);
-        prev.is_null()
+    pub(crate) unsafe fn enqueue(&self, task: TaskRef) -> bool {
+        let mut was_empty = false;
+
+        self.head
+            .fetch_update(Ordering::SeqCst, Ordering::SeqCst, |prev| {
+                was_empty = prev.is_null();
+                unsafe {
+                    // safety: the pointer is either null or valid
+                    let prev = NonNull::new(prev).map(|ptr| TaskRef::from_ptr(ptr.as_ptr()));
+                    // safety: there are no concurrent accesses to `next`
+                    task.header().run_queue_item.next.set(prev);
+                }
+                Some(task.as_ptr() as *mut _)
+            })
+            .ok();
+
+        was_empty
     }
 
     /// 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.
-    pub(crate) fn dequeue_all(&self, on_task: impl Fn(NonNull<TaskHeader>)) {
+    pub(crate) fn dequeue_all(&self, on_task: impl Fn(TaskRef)) {
         // Atomically empty the queue.
-        let mut ptr = self.head.swap(ptr::null_mut(), Ordering::AcqRel);
+        let ptr = self.head.swap(ptr::null_mut(), Ordering::AcqRel);
+
+        // safety: the pointer is either null or valid
+        let mut next = unsafe { NonNull::new(ptr).map(|ptr| TaskRef::from_ptr(ptr.as_ptr())) };
 
         // Iterate the linked list of tasks that were previously in the queue.
-        while let Some(task) = NonNull::new(ptr) {
+        while let Some(task) = next {
             // If the task re-enqueues itself, the `next` pointer will get overwritten.
             // Therefore, first read the next pointer, and only then process the task.
-            let next = unsafe { task.as_ref() }.run_queue_item.next.load(Ordering::Relaxed);
+            // safety: there are no concurrent accesses to `next`
+            next = unsafe { task.header().run_queue_item.next.get() };
 
             on_task(task);
-
-            ptr = next
         }
     }
 }