use core::cell::UnsafeCell;
use core::mem::MaybeUninit;
use core::ptr;
use crate::arch::platform::{blocking_context_available, request_context_switch};
use crate::clock::ticks_per_ms;
use crate::critical_section;
use crate::ktimer::current_time_ticks;
use crate::sync::{
MessageWaitResult, SyncType, WaitTree, block_current_thread_until, current_thread_handle,
finish_message_wait, insert_message_waiter, message_wait_tree, take_message_wait_result,
wake_waiter,
};
use crate::thread::{ThreadHandle, ThreadState, thread_handle_from_sync_entity};
use crate::waitq::{WaitQueueError, wait_entity};
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum MessageQueueStatus {
Full,
Empty,
TimedOut,
Cancelled,
NoCurrentThread,
InvalidContext,
WaitQueue(WaitQueueError),
}
impl From<WaitQueueError> for MessageQueueStatus {
fn from(error: WaitQueueError) -> Self {
Self::WaitQueue(error)
}
}
struct QueueState<T: Copy, const N: usize> {
slots: [MaybeUninit<T>; N],
head: usize,
len: usize,
senders: WaitTree,
receivers: WaitTree,
}
pub struct MessageQueue<T: Copy, const N: usize> {
state: UnsafeCell<QueueState<T, N>>,
}
unsafe impl<T: Copy + Send, const N: usize> Sync for MessageQueue<T, N> {}
unsafe impl<T: Copy + Send, const N: usize> Send for MessageQueue<T, N> {}
impl<T: Copy, const N: usize> MessageQueue<T, N> {
pub const fn new() -> Self {
assert!(N != 0, "message queue capacity must be non-zero");
Self {
state: UnsafeCell::new(QueueState {
slots: [MaybeUninit::uninit(); N],
head: 0,
len: 0,
senders: WaitTree::new(),
receivers: WaitTree::new(),
}),
}
}
pub const fn capacity(&self) -> usize {
N
}
pub fn len(&self) -> usize {
critical_section(|| unsafe { (*self.state.get()).len })
}
pub fn is_empty(&self) -> bool {
self.len() == 0
}
pub fn is_full(&self) -> bool {
self.len() == N
}
pub fn try_send(&self, message: T) -> Result<(), MessageQueueStatus> {
critical_section(|| unsafe {
{
let state = &mut *self.state.get();
if state.len == N {
return Err(MessageQueueStatus::Full);
}
let until_end = N - state.head;
let tail = if state.len < until_end {
state.head + state.len
} else {
state.len - until_end
};
state.slots[tail].write(message);
state.len += 1;
}
self.wake_one(false);
Ok(())
})
}
pub fn try_receive(&self) -> Result<T, MessageQueueStatus> {
critical_section(|| unsafe {
let message = {
let state = &mut *self.state.get();
if state.len == 0 {
return Err(MessageQueueStatus::Empty);
}
let message = state.slots[state.head].assume_init_read();
state.head = if state.head == N - 1 {
0
} else {
state.head + 1
};
state.len -= 1;
message
};
self.wake_one(true);
Ok(message)
})
}
pub fn send(&self, message: T) -> Result<(), MessageQueueStatus> {
self.send_with_wait(message, blocking_context_available(), resume_after_block)
}
pub fn receive(&self) -> Result<T, MessageQueueStatus> {
self.receive_with_wait(blocking_context_available(), resume_after_block)
}
pub fn send_timeout(&self, message: T, timeout_ms: u32) -> Result<(), MessageQueueStatus> {
self.send_with_deadline(
message,
Some(deadline_after(timeout_ms)),
blocking_context_available(),
resume_after_block,
)
}
pub fn receive_timeout(&self, timeout_ms: u32) -> Result<T, MessageQueueStatus> {
self.receive_with_deadline(
Some(deadline_after(timeout_ms)),
blocking_context_available(),
resume_after_block,
)
}
pub fn cancel_wait(&self, thread: ThreadHandle) -> bool {
critical_section(|| unsafe {
if (*thread.as_ptr()).is_idle() || !(*thread.as_ptr()).state.is_blocked() {
return false;
}
let tree = message_wait_tree(thread);
let state = self.state.get();
if tree.is_null()
|| (tree != ptr::addr_of_mut!((*state).senders)
&& tree != ptr::addr_of_mut!((*state).receivers))
{
return false;
}
let result = wait_result(thread);
let cancelled = result != MessageWaitResult::TimedOut;
finish_message_wait(
thread,
if cancelled {
MessageWaitResult::Cancelled
} else {
result
},
);
Self::wake(thread);
cancelled
})
}
fn send_with_wait(
&self,
message: T,
can_block: bool,
wait: impl FnMut(),
) -> Result<(), MessageQueueStatus> {
self.send_with_deadline(message, None, can_block, wait)
}
fn send_with_deadline(
&self,
message: T,
deadline: Option<u64>,
can_block: bool,
mut wait: impl FnMut(),
) -> Result<(), MessageQueueStatus> {
let mut waiter = None;
loop {
let sent = critical_section(|| {
check_wait_result(waiter.take(), deadline)?;
match self.try_send(message) {
Ok(()) => return Ok(true),
Err(MessageQueueStatus::Full) => {}
Err(error) => return Err(error),
}
if deadline_expired(deadline) {
return Err(MessageQueueStatus::TimedOut);
}
waiter = Some(self.block_until(SyncType::MessageQueueSend, can_block, deadline)?);
Ok(false)
})?;
if sent {
return Ok(());
}
wait();
}
}
fn receive_with_wait(
&self,
can_block: bool,
wait: impl FnMut(),
) -> Result<T, MessageQueueStatus> {
self.receive_with_deadline(None, can_block, wait)
}
fn receive_with_deadline(
&self,
deadline: Option<u64>,
can_block: bool,
mut wait: impl FnMut(),
) -> Result<T, MessageQueueStatus> {
let mut waiter = None;
loop {
let message = critical_section(|| {
check_wait_result(waiter.take(), deadline)?;
match self.try_receive() {
Ok(message) => return Ok(Some(message)),
Err(MessageQueueStatus::Empty) => {}
Err(error) => return Err(error),
}
if deadline_expired(deadline) {
return Err(MessageQueueStatus::TimedOut);
}
waiter =
Some(self.block_until(SyncType::MessageQueueReceive, can_block, deadline)?);
Ok(None)
})?;
if let Some(message) = message {
return Ok(message);
}
wait();
}
}
#[cfg(feature = "host")]
fn block(&self, kind: SyncType, can_block: bool) -> Result<ThreadHandle, MessageQueueStatus> {
self.block_until(kind, can_block, None)
}
fn block_until(
&self,
kind: SyncType,
can_block: bool,
deadline: Option<u64>,
) -> Result<ThreadHandle, MessageQueueStatus> {
unsafe {
let thread = current_thread_handle().ok_or(MessageQueueStatus::NoCurrentThread)?;
if !can_block
|| (*thread.as_ptr()).state != ThreadState::Running
|| (*thread.as_ptr()).is_idle()
{
return Err(MessageQueueStatus::InvalidContext);
}
let deadline_at =
block_current_thread_until(thread, kind, deadline.unwrap_or(u64::MAX))?;
let available = {
let state = &mut *self.state.get();
let waiters = if kind == SyncType::MessageQueueSend {
&mut state.senders
} else {
&mut state.receivers
};
insert_message_waiter(waiters, thread, deadline_at);
if kind == SyncType::MessageQueueSend {
state.len < N
} else {
state.len != 0
}
};
let result = wait_result(thread);
if available || result == MessageWaitResult::TimedOut {
finish_message_wait(thread, result);
Self::wake(thread);
}
request_context_switch();
Ok(thread)
}
}
fn wake_one(&self, wake_senders: bool) {
unsafe {
loop {
let state = self.state.get();
let tree = if wake_senders {
ptr::addr_of_mut!((*state).senders)
} else {
ptr::addr_of_mut!((*state).receivers)
};
let entity = (*tree).first();
if entity.is_null() {
return;
}
let thread = thread_handle_from_sync_entity(entity);
let result = wait_result(thread);
finish_message_wait(thread, result);
Self::wake(thread);
if result == MessageWaitResult::Notified {
return;
}
}
}
}
unsafe fn wake(thread: ThreadHandle) {
unsafe { wake_waiter(thread) }.expect("message queue waiter must remain valid");
request_context_switch();
}
}
fn deadline_after(timeout_ms: u32) -> u64 {
current_time_ticks()
.saturating_add(u64::from(timeout_ms) * u64::from(ticks_per_ms()))
.min(u64::MAX - 1)
}
fn deadline_expired(deadline: Option<u64>) -> bool {
deadline.is_some_and(|deadline| current_time_ticks() >= deadline)
}
unsafe fn wait_result(thread: ThreadHandle) -> MessageWaitResult {
let wake_at = unsafe { (*wait_entity(thread)).wake_at };
if wake_at != u64::MAX && current_time_ticks() >= wake_at {
MessageWaitResult::TimedOut
} else {
MessageWaitResult::Notified
}
}
fn check_wait_result(
waiter: Option<ThreadHandle>,
deadline: Option<u64>,
) -> Result<(), MessageQueueStatus> {
if let Some(thread) = waiter {
match unsafe { take_message_wait_result(thread) } {
MessageWaitResult::TimedOut => return Err(MessageQueueStatus::TimedOut),
MessageWaitResult::Cancelled => return Err(MessageQueueStatus::Cancelled),
MessageWaitResult::Notified => {}
MessageWaitResult::Pending => unreachable!("a pending message wait cannot resume"),
}
if deadline_expired(deadline) {
return Err(MessageQueueStatus::TimedOut);
}
}
Ok(())
}
impl<T: Copy, const N: usize> Default for MessageQueue<T, N> {
fn default() -> Self {
Self::new()
}
}
fn resume_after_block() {
#[cfg(all(target_arch = "arm", not(feature = "host")))]
{
cortex_m::asm::dsb();
cortex_m::asm::isb();
}
}
#[cfg(feature = "host")]
#[path = "../tests/support/messageq.rs"]
pub mod test_support;