Add wait queues to semaphore/queue (#4086)

* Add wait queue to semaphores

* Spawn idle task, prevent re-waking a ready task

* Add wait queues to queues

* Clean up wait queues

* Remove task from wait queue on timeout/resume

* Do not re-schedule deleted task

* Implement simple stack overflow detection

* Task deletion: remove task from queues in one place

* Fix storing context into freed memory

* Restore idle task stack size

* Undo alloc changes

* Don't let task sleep if wakeup time is in the past

* Improve codegen of esp_preempt_now

Author: bugadani

esp-hal/src/timer/systimer.rs

@@ -111,6 +111,7 @@ impl<'d> SystemTimer<'d> {
     }
 
     /// Get the current count of the given unit in the System Timer.
+    #[inline]
     pub fn unit_value(unit: Unit) -> u64 {
         // This should be safe to access from multiple contexts
         // worst case scenario the second accessor ends up reading
@@ -233,6 +234,7 @@ impl Unit {
         }
     }
 
+    #[inline]
     fn read_count(&self) -> u64 {
         // This can be a shared reference as long as this type isn't Sync.
 

esp-preempt/src/lib.rs

@@ -30,6 +30,7 @@ mod semaphore;
 mod task;
 mod timer;
 mod timer_queue;
+mod wait_queue;
 
 pub(crate) use esp_alloc::InternalMemory;
 use esp_hal::{

esp-preempt/src/queue.rs

@@ -5,16 +5,19 @@ use esp_hal::time::{Duration, Instant};
 use esp_radio_preempt_driver::{
     queue::{QueueImplementation, QueuePtr},
     register_queue_implementation,
-    yield_task,
 };
 use esp_sync::NonReentrantMutex;
 
+use crate::wait_queue::WaitQueue;
+
 struct QueueInner {
     storage: Box<[u8]>,
     item_size: usize,
     capacity: usize,
     current_read: usize,
     current_write: usize,
+    waiting_for_space: WaitQueue,
+    waiting_for_item: WaitQueue,
 }
 
 impl QueueInner {
@@ -25,6 +28,8 @@ impl QueueInner {
             current_read: 0,
             current_write: 0,
             storage: vec![0; capacity * item_size].into_boxed_slice(),
+            waiting_for_space: WaitQueue::new(),
+            waiting_for_item: WaitQueue::new(),
         }
     }
 
@@ -39,7 +44,7 @@ impl QueueInner {
     }
 
     unsafe fn try_enqueue(&mut self, item: *const u8) -> bool {
-        if self.len() == self.capacity {
+        if self.full() {
             return false;
         }
 
@@ -54,7 +59,7 @@ impl QueueInner {
     }
 
     unsafe fn try_dequeue(&mut self, dst: *mut u8) -> bool {
-        if self.len() == 0 {
+        if self.empty() {
             return false;
         }
 
@@ -69,15 +74,15 @@ impl QueueInner {
     }
 
     unsafe fn remove(&mut self, item: *const u8) {
+        if self.empty() {
+            return;
+        }
+
         // do what the ESP-IDF implementations does...
         // just remove all elements and add them back except the one we need to remove -
         // good enough for now
         let count = self.len();
 
-        if count == 0 {
-            return;
-        }
-
         let mut tmp_item = vec![0; self.item_size];
 
         let item_slice = unsafe { core::slice::from_raw_parts(item, self.item_size) };
@@ -100,6 +105,14 @@ impl QueueInner {
             self.capacity - self.current_read + self.current_write
         }
     }
+
+    fn empty(&self) -> bool {
+        self.len() == 0
+    }
+
+    fn full(&self) -> bool {
+        self.len() == self.capacity
+    }
 }
 
 pub struct Queue {
@@ -117,48 +130,109 @@ impl Queue {
         unsafe { ptr.cast::<Self>().as_ref() }
     }
 
-    fn yield_loop_with_timeout(timeout_us: Option<u32>, mut cb: impl FnMut() -> bool) -> bool {
-        let start = if timeout_us.is_some() {
-            Instant::now()
-        } else {
-            Instant::EPOCH
-        };
-
-        let timeout = timeout_us
-            .map(|us| Duration::from_micros(us as u64))
-            .unwrap_or(Duration::MAX);
+    unsafe fn send_to_back(&self, item: *const u8, timeout_us: Option<u32>) -> bool {
+        let deadline = timeout_us.map(|us| Instant::now() + Duration::from_micros(us as u64));
 
         loop {
-            if cb() {
+            let enqueued = self.inner.with(|queue| {
+                if unsafe { queue.try_enqueue(item) } {
+                    queue.waiting_for_item.notify();
+                    true
+                } else {
+                    // The task will go to sleep when the above critical section is released.
+                    queue.waiting_for_space.wait_with_deadline(deadline);
+                    false
+                }
+            });
+
+            if enqueued {
                 return true;
             }
 
-            if timeout_us.is_some() && start.elapsed() > timeout {
+            // We are here because the queue was full. Now we've either timed out, or an item has
+            // been removed from the queue. However, any higher priority task can wake up
+            // and preempt us still. Let's just check for the timeout, and try the whole process
+            // again.
+
+            if let Some(deadline) = deadline
+                && deadline < Instant::now()
+            {
+                // We have a deadline and we've timed out.
                 return false;
             }
-
-            yield_task();
+            // We can block more, so let's attempt to enqueue again.
         }
     }
 
-    unsafe fn send_to_back(&self, item: *const u8, timeout_us: Option<u32>) -> bool {
-        Self::yield_loop_with_timeout(timeout_us, || unsafe { self.try_send_to_back(item) })
-    }
-
     unsafe fn try_send_to_back(&self, item: *const u8) -> bool {
-        self.inner.with(|queue| unsafe { queue.try_enqueue(item) })
+        self.inner.with(|queue| {
+            if unsafe { queue.try_enqueue(item) } {
+                queue.waiting_for_item.notify();
+                true
+            } else {
+                false
+            }
+        })
     }
 
     unsafe fn receive(&self, item: *mut u8, timeout_us: Option<u32>) -> bool {
-        Self::yield_loop_with_timeout(timeout_us, || unsafe { self.try_receive(item) })
+        let deadline = timeout_us.map(|us| Instant::now() + Duration::from_micros(us as u64));
+
+        loop {
+            // Attempt to dequeue an item from the queue
+            let dequeued = self.inner.with(|queue| {
+                if unsafe { queue.try_dequeue(item) } {
+                    queue.waiting_for_space.notify();
+                    true
+                } else {
+                    // The task will go to sleep when the above critical section is released.
+                    queue.waiting_for_item.wait_with_deadline(deadline);
+                    false
+                }
+            });
+
+            if dequeued {
+                return true;
+            }
+
+            // We are here because we weren't able to dequeue from the queue previously. We've
+            // either timed out, or the queue has an item. However, any higher priority
+            // task can wake up and preempt us still. Let's just check for the timeout,
+            // and try the whole process again.
+
+            if let Some(deadline) = deadline
+                && deadline < Instant::now()
+            {
+                // We have a deadline and we've timed out.
+                return false;
+            }
+            // We can block more, so let's attempt to dequeue again.
+        }
     }
 
     unsafe fn try_receive(&self, item: *mut u8) -> bool {
-        self.inner.with(|queue| unsafe { queue.try_dequeue(item) })
+        self.inner.with(|queue| {
+            if unsafe { queue.try_dequeue(item) } {
+                queue.waiting_for_space.notify();
+                true
+            } else {
+                false
+            }
+        })
     }
 
     unsafe fn remove(&self, item: *const u8) {
-        self.inner.with(|queue| unsafe { queue.remove(item) })
+        self.inner.with(|queue| {
+            let was_full = queue.full();
+
+            unsafe {
+                queue.remove(item);
+            }
+
+            if was_full && !queue.full() {
+                queue.waiting_for_space.notify();
+            }
+        })
     }
 
     fn messages_waiting(&self) -> usize {

esp-preempt/src/run_queue.rs

@@ -21,6 +21,11 @@ impl RunQueue {
     pub(crate) fn mark_task_ready(&mut self, mut ready_task: TaskPtr) {
         // TODO: this will need to track max ready priority.
         unsafe { ready_task.as_mut().state = TaskState::Ready };
+        if let Some(mut containing_queue) = unsafe { ready_task.as_mut().current_queue.take() } {
+            unsafe {
+                containing_queue.as_mut().remove(ready_task);
+            }
+        }
         self.ready_tasks.push(ready_task);
     }