Clean up blocking until deadline (#4217)

Author: bugadani

esp-rtos/src/esp_radio/queue.rs

@@ -1,7 +1,6 @@
 use alloc::{boxed::Box, vec};
 use core::ptr::NonNull;
 
-use esp_hal::time::{Duration, Instant};
 use esp_radio_rtos_driver::{
  queue::{QueueImplementation, QueuePtr},
  register_queue_implementation,
@@ -131,10 +130,8 @@ impl Queue {
  }
 
  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 {
- let enqueued = self.inner.with(|queue| {
+ if crate::with_deadline(timeout_us, |deadline| {
+ self.inner.with(|queue| {
  if unsafe { queue.try_enqueue(item) } {
  trace!("Queue - notify with item");
  queue.waiting_for_item.notify();
@@ -145,25 +142,13 @@ impl Queue {
  queue.waiting_for_space.wait_with_deadline(deadline);
  false
  }
- });
-
- if enqueued {
- return true;
- }
-
- // 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()
- {
- debug!("Queue - send to back - timed out");
- // We have a deadline and we've timed out.
- return false;
- }
- // We can block more, so let's attempt to enqueue again.
+ })
+ }) {
+ debug!("Queue - send to back - success");
+ true
+ } else {
+ debug!("Queue - send to back - timed out");
+ false
  }
  }
 
@@ -179,11 +164,8 @@ impl Queue {
  }
 
  unsafe fn receive(&self, item: *mut u8, timeout_us: Option<u32>) -> bool {
- 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 crate::with_deadline(timeout_us, |deadline| {
+ self.inner.with(|queue| {
  if unsafe { queue.try_dequeue(item) } {
  trace!("Queue - notify with space");
  queue.waiting_for_space.notify();
@@ -194,25 +176,13 @@ impl Queue {
  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.
- debug!("Queue - timed out waiting for item");
- return false;
- }
- // We can block more, so let's attempt to dequeue again.
+ })
+ }) {
+ debug!("Queue - dequeued item");
+ true
+ } else {
+ debug!("Queue - timed out waiting for item");
+ false
  }
  }
 

esp-rtos/src/lib.rs

@@ -98,13 +98,13 @@ use esp_hal::interrupt::software::SoftwareInterrupt;
 use esp_hal::{
  Blocking,
  system::Cpu,
+ time::{Duration, Instant},
  timer::{AnyTimer, OneShotTimer},
 };
 #[cfg(multi_core)]
 use esp_hal::{
  peripherals::CPU_CTRL,
  system::{CpuControl, Stack},
- time::{Duration, Instant},
 };
 #[cfg_attr(docsrs, doc(cfg(feature = "embassy")))]
 pub use macros::rtos_main as main;
@@ -336,10 +336,33 @@ pub fn start_second_core_with_stack_guard_offset<const STACK_SIZE: usize>(
 const TICK_RATE: u32 = esp_config::esp_config_int!(u32, "ESP_RTOS_CONFIG_TICK_RATE_HZ");
 
 pub(crate) fn now() -> u64 {
- esp_hal::time::Instant::now()
- .duration_since_epoch()
- .as_micros()
+ Instant::now().duration_since_epoch().as_micros()
 }
 
 #[cfg(feature = "embassy")]
 embassy_time_driver::time_driver_impl!(static TIMER_QUEUE: crate::timer::embassy::TimerQueue = crate::timer::embassy::TimerQueue::new());
+
+/// Waits for a condition to be met or a timeout to occur.
+///
+/// This function is meant to simplify implementation of blocking primitives. Upon failure the
+/// `attempt` function should enqueue the task in a wait queue and put the task to sleep.
+fn with_deadline(timeout_us: Option<u32>, attempt: impl Fn(Instant) -> bool) -> bool {
+ let deadline = timeout_us
+ .map(|us| Instant::now() + Duration::from_micros(us as u64))
+ .unwrap_or(Instant::EPOCH + Duration::MAX);
+
+ while !attempt(deadline) {
+ // We are here because the operation failed. We've either timed out, or the operation is
+ // ready to be attempted again. 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 timeout_us.is_some() && deadline < Instant::now() {
+ // We have a deadline and we've timed out.
+ return false;
+ }
+ // We can block more, so let's attempt the operation again.
+ }
+
+ true
+}

esp-rtos/src/scheduler.rs

@@ -359,18 +359,9 @@ impl SchedulerState {
  is_current
  }
 
- pub(crate) fn sleep_until(&mut self, at: Instant) -> bool {
- let current_cpu = Cpu::current() as usize;
- let current_task = unwrap!(self.per_cpu[current_cpu].current_task, "No current task");
+ pub(crate) fn sleep_task_until(&mut self, task: TaskPtr, at: Instant) -> bool {
  let timer_queue = unwrap!(self.time_driver.as_mut());
- if timer_queue.schedule_wakeup(current_task, at) {
- // The task has been scheduled for wakeup.
- task::yield_task();
- true
- } else {
- // The task refuses to sleep.
- false
- }
+ timer_queue.schedule_wakeup(task, at)
  }
 
  pub(crate) fn resume_task(&mut self, task: TaskPtr) {
@@ -448,7 +439,19 @@ impl Scheduler {
  }
 
  pub(crate) fn sleep_until(&self, wake_at: Instant) -> bool {
- self.with(|scheduler| scheduler.sleep_until(wake_at))
+ self.with(|scheduler| {
+ let current_cpu = Cpu::current() as usize;
+ let current_task = unwrap!(
+ scheduler.per_cpu[current_cpu].current_task,
+ "No current task"
+ );
+ if scheduler.sleep_task_until(current_task, wake_at) {
+ task::yield_task();
+ true
+ } else {
+ false
+ }
+ })
  }
 }
 

esp-rtos/src/semaphore.rs

@@ -2,10 +2,7 @@
 //!
 //! This module provides the [`Semaphore`] type, which implements counting semaphores and mutexes.
 
-use esp_hal::{
- system::Cpu,
- time::{Duration, Instant},
-};
+use esp_hal::{system::Cpu, time::Instant};
 use esp_sync::NonReentrantMutex;
 
 use crate::{
@@ -118,7 +115,7 @@ impl SemaphoreInner {
  }
  }
 
- fn wait_with_deadline(&mut self, deadline: Option<Instant>) {
+ fn wait_with_deadline(&mut self, deadline: Instant) {
  trace!("Semaphore wait_with_deadline - {:?}", deadline);
  match self {
  SemaphoreInner::Counting { waiting, .. } => waiting.wait_with_deadline(deadline),
@@ -181,36 +178,22 @@ impl Semaphore {
  /// If the semaphore is already taken, the task will be blocked until the semaphore is released.
  /// Recursive mutexes can be locked multiple times by the mutex owner task.
  pub fn take(&self, timeout_us: Option<u32>) -> bool {
- let deadline = timeout_us.map(|us| Instant::now() + Duration::from_micros(us as u64));
- loop {
- let taken = self.inner.with(|sem| {
+ if crate::with_deadline(timeout_us, |deadline| {
+ self.inner.with(|sem| {
  if sem.try_take() {
  true
  } else {
  // The task will go to sleep when the above critical section is released.
  sem.wait_with_deadline(deadline);
  false
  }
- });
-
- if taken {
- debug!("Semaphore - take - success");
- return true;
- }
-
- // We are here because we weren't able to take the semaphore previously. We've either
- // timed out, or the semaphore is ready for taking. 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.
- trace!("Semaphore - take - timed out");
- return false;
- }
- // We can block more, so let's attempt to take the semaphore again.
+ })
+ }) {
+ debug!("Semaphore - take - success");
+ true
+ } else {
+ debug!("Semaphore - take - timed out");
+ false
  }
  }
 

esp-rtos/src/task/mod.rs

@@ -233,8 +233,6 @@ impl<E: TaskListElement> TaskQueue<E> {
 
 #[cfg(feature = "embassy")]
 pub(crate) mod flags {
-
- use esp_hal::time::{Duration, Instant};
  use esp_sync::NonReentrantMutex;
 
  use crate::{
@@ -290,39 +288,21 @@ pub(crate) mod flags {
  }
 
  pub(crate) fn wait(&self, wait_flags: u32, timeout_us: Option<u32>) -> bool {
- let deadline = timeout_us.map(|us| Instant::now() + Duration::from_micros(us as u64));
- loop {
- let success = self.inner.with(|inner| {
+ if crate::with_deadline(timeout_us, |deadline| {
+ self.inner.with(|inner| {
  if inner.wait(wait_flags) {
  true
  } else {
- let wake_at = if let Some(deadline) = deadline {
- deadline
- } else {
- Instant::EPOCH + Duration::MAX
- };
- SCHEDULER.sleep_until(wake_at);
+ SCHEDULER.sleep_until(deadline);
  false
  }
- });
-
- if success {
- debug!("Flags - wait - success");
- return true;
- }
-
- // We are here because the required flags were not set previously. We've
- // either timed out, or the flags are now set. 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.
- trace!("Flags - wait - timed out");
- return false;
- }
+ })
+ }) {
+ debug!("Flags - wait - success");
+ true
+ } else {
+ trace!("Flags - wait - timed out");
+ false
  }
  }
  }

esp-rtos/src/wait_queue.rs

@@ -1,9 +1,6 @@
 use core::ptr::NonNull;
 
-use esp_hal::{
- system::Cpu,
- time::{Duration, Instant},
-};
+use esp_hal::{system::Cpu, time::Instant};
 
 use crate::{
  SCHEDULER,
@@ -33,22 +30,17 @@ impl WaitQueue {
  });
  }
 
- pub(crate) fn wait_with_deadline(&mut self, deadline: Option<Instant>) {
+ pub(crate) fn wait_with_deadline(&mut self, deadline: Instant) {
  SCHEDULER.with(|scheduler| {
  let current_cpu = Cpu::current() as usize;
  let mut task = unwrap!(scheduler.per_cpu[current_cpu].current_task);
 
- let wake_at = if let Some(deadline) = deadline {
- deadline
- } else {
- Instant::EPOCH + Duration::MAX
- };
-
- if scheduler.sleep_until(wake_at) {
+ if scheduler.sleep_task_until(task, deadline) {
  self.waiting_tasks.push(task);
  unsafe {
  task.as_mut().current_queue = Some(NonNull::from(self));
  }
+ crate::task::yield_task();
  }
  });
  }