rusty_rtos_kernel_core/queue.rs
1//! `queue.c`: the queue, and the semaphore and mutex built on it.
2//!
3//! In FreeRTOS a semaphore *is* a queue with a zero-size item, and a mutex
4//! is that plus a holder and priority inheritance. Kairos keeps the same
5//! shape, because the trace does: a semaphore give fires `QUEUE_SEND` and a
6//! mutex take fires `QUEUE_RECEIVE`, and a conformance diff would notice
7//! any tidier arrangement.
8//!
9//! # A blocking call with no stack to block on
10//!
11//! `xQueueReceive( q, &v, 100 )` blocks: the C task stops inside the call,
12//! and when it is woken it loops back to the top, re-checks the queue, and
13//! either returns the item or blocks again. This kernel has no stack to
14//! suspend, so the loop is turned inside out — a blocking call returns
15//! [`Blocked`] and the caller invokes it again, unchanged, when it next
16//! runs:
17//!
18//! ```ignore
19//! // in a task body, at some `pc`:
20//! match k.queue_receive(q, 100)? {
21//! Blocked => {} // stay at this pc; the kernel parked us
22//! Ready(value) => { ...; self.pc = next; }
23//! }
24//! ```
25//!
26//! The task is off the ready list when `Blocked` comes back, so the runner
27//! will not step it again until the kernel wakes it — which is exactly when
28//! the C thread would have resumed. What the C keeps on its stack across
29//! the block (the timeout, the remaining ticks, whether inheritance
30//! happened) this kernel keeps in the TCB, in [`Wait`], because that *is*
31//! the stack frame.
32
33use rusty_rtos_core::config::Config;
34use rusty_rtos_core::error::{Error, Result};
35use rusty_rtos_core::handle::{QueueHandle, TaskHandle};
36use rusty_rtos_core::hooks::TickHook;
37use rusty_rtos_core::isr::Woken;
38use rusty_rtos_core::port::Port;
39use rusty_rtos_core::trace::{Event, Trace};
40
41use crate::kernel::{Kernel, OwedTrace, QueueResume};
42
43/// What a blocking call answers.
44#[derive(Debug, Clone, Copy, PartialEq, Eq)]
45pub enum Wait<T> {
46 /// The kernel parked the calling task. Leave the program counter where
47 /// it is and make the same call again when the task next runs.
48 Blocked,
49 /// The call completed.
50 Ready(T),
51}
52
53pub use Wait::{Blocked, Ready};
54
55/// Where an item goes (`xCopyPosition`).
56#[derive(Debug, Clone, Copy, PartialEq, Eq)]
57pub enum Position {
58 /// `queueSEND_TO_BACK`.
59 Back,
60 /// `queueSEND_TO_FRONT`.
61 Front,
62 /// `queueOVERWRITE`: write to the front of a length-one queue whether
63 /// or not it already holds something, and leave the message count
64 /// where it was rather than raising it.
65 Overwrite,
66}
67
68/// What a queue is underneath (`ucQueueType`).
69#[derive(Debug, Clone, Copy, PartialEq, Eq)]
70pub enum Kind {
71 // ORDER IS LOAD-BEARING, and only for the two predicates below. The kinds
72 // that carry data are first and the two mutex kinds are last, so
73 // `carries_data` and `is_mutex` are each a range the compiler can test
74 // with one comparison instead of two. Nothing reads a discriminant, so
75 // the order carries no other meaning.
76 /// A queue of values.
77 Queue,
78 /// A queue set: a queue whose items are the handles of the queues in
79 /// it. `xQueueSelectFromSet` is a receive from this one.
80 Set,
81 /// A binary or counting semaphore: a queue with a zero-size item, so
82 /// only the count matters.
83 Semaphore,
84 /// A mutex: a one-deep semaphore with a holder and priority
85 /// inheritance.
86 Mutex,
87 /// A recursive mutex: as above, and the holder may take it again.
88 RecursiveMutex,
89}
90
91impl Kind {
92 /// `uxItemSize == 0`: semaphores and mutexes carry a count, not data.
93 pub(crate) const fn carries_data(self) -> bool {
94 matches!(self, Self::Queue | Self::Set)
95 }
96
97 /// The two mutex kinds, which inherit priority.
98 pub(crate) const fn is_mutex(self) -> bool {
99 matches!(self, Self::Mutex | Self::RecursiveMutex)
100 }
101}
102
103/// One queue: the C `Queue_t` minus the byte pointers. Storage is a range
104/// of the kernel's shared slot pool, and the read and write cursors follow
105/// C's `pcReadFrom` / `pcWriteTo` exactly — `pcReadFrom` points at the item
106/// last read, which is what makes `queueSEND_TO_FRONT` a single step back.
107#[derive(Debug, Clone, Copy)]
108pub(crate) struct Queue {
109 pub(crate) base: usize,
110 /// `uxLength`.
111 pub(crate) length: usize,
112 /// `uxMessagesWaiting`.
113 pub(crate) waiting: usize,
114 /// `pcReadFrom`, as an index: the item last read.
115 pub(crate) read_from: usize,
116 /// `pcWriteTo`, as an index: where the next back-send writes.
117 pub(crate) write_to: usize,
118 pub(crate) kind: Kind,
119 /// `u.xSemaphore.xMutexHolder`.
120 pub(crate) holder: TaskHandle,
121 /// `u.xSemaphore.uxRecursiveCallCount`.
122 pub(crate) recursions: u32,
123 /// `cTxLock`: [`UNLOCKED`] when no task holds the queue locked;
124 /// otherwise how many sends an interrupt made while it was locked, and
125 /// therefore how many receivers `prvUnlockQueue` still owes a wake-up.
126 pub(crate) tx_lock: i8,
127 /// `cRxLock`, the same for receives and the tasks waiting to send.
128 pub(crate) rx_lock: i8,
129 /// `pxQueueSetContainer`: the set this queue belongs to, if any. An
130 /// item arriving here is announced there.
131 pub(crate) set_container: QueueHandle,
132}
133
134/// `queueUNLOCKED`.
135pub(crate) const UNLOCKED: i8 = -1;
136/// `queueLOCKED_UNMODIFIED`.
137pub(crate) const LOCKED_UNMODIFIED: i8 = 0;
138
139impl Queue {
140 pub(crate) const fn new(base: usize, length: usize, kind: Kind) -> Self {
141 Self {
142 base,
143 length,
144 waiting: 0,
145 // `xQueueGenericReset`: pcReadFrom = pcHead + (length - 1) * itemSize.
146 read_from: length.saturating_sub(1),
147 write_to: 0,
148 kind,
149 holder: TaskHandle::NULL,
150 recursions: 0,
151 tx_lock: UNLOCKED,
152 rx_lock: UNLOCKED,
153 set_container: QueueHandle::NULL,
154 }
155 }
156}
157
158impl<
159 C: Config,
160 P: Port,
161 T: Trace,
162 H,
163 const TASKS: usize,
164 const ITEMS: usize,
165 const LISTS: usize,
166 const QUEUES: usize,
167 const SLOTS: usize,
168 const BUFFERS: usize,
169 const BYTES: usize,
170 const TIMERS: usize,
171 const GROUPS: usize,
172> Kernel<C, P, T, H, TASKS, ITEMS, LISTS, QUEUES, SLOTS, BUFFERS, BYTES, TIMERS, GROUPS>
173where
174 H: TickHook<Self>,
175{
176 // ------------------------------------------------------------ create --
177
178 /// `xQueueCreate`.
179 ///
180 /// # Errors
181 /// [`Error::Full`] when the queue arena or the shared slot pool is
182 /// exhausted; [`Error::InvalidArgument`] for a zero length.
183 pub fn queue_create(&mut self, length: usize) -> Result<QueueHandle> {
184 self.new_queue(length, Kind::Queue)
185 }
186
187 /// `vQueueDelete`, and `vSemaphoreDelete`, which the C defines as the
188 /// same function.
189 ///
190 /// # It traces nothing and still costs time
191 ///
192 /// `vQueueDelete` takes no critical section of its own — but it ends in
193 /// `vPortFree( pxQueue )`, and every `heap_N.c` wraps free in
194 /// `vTaskSuspendAll` / `xTaskResumeAll` exactly as it wraps malloc. So
195 /// a delete costs **one outermost critical-section exit**, which under
196 /// the sim contract is one unit of time, while emitting no trace line
197 /// at all (this harness defines no `traceQUEUE_DELETE`).
198 ///
199 /// `AbortDelay`'s remake left the deletes out on the reasoning that an
200 /// event-less call cannot move the trace. Every event still agreed and
201 /// the exit column was one short from the first delete onwards. The
202 /// sibling `event_group_delete` already carried this note; the queue
203 /// had no delete at all.
204 ///
205 /// # Errors
206 /// [`Error::Gone`] for a stale handle.
207 pub fn queue_delete(&mut self, queue: QueueHandle) -> Result<()> {
208 self.queues.resolve(queue)?;
209 let _ = self.queues.remove(queue);
210 // `vPortFree( pxQueue )`.
211 self.account_for_allocation();
212 Ok(())
213 }
214
215 /// `xSemaphoreCreateBinary`: a one-deep semaphore, created empty.
216 ///
217 /// # Errors
218 /// As [`Kernel::queue_create`].
219 pub fn semaphore_create_binary(&mut self) -> Result<QueueHandle> {
220 self.new_queue(1, Kind::Semaphore)
221 }
222
223 /// `xSemaphoreCreateCounting`.
224 ///
225 /// # Errors
226 /// As [`Kernel::queue_create`]; [`Error::InvalidArgument`] if the
227 /// initial count exceeds the maximum.
228 pub fn semaphore_create_counting(&mut self, max: usize, initial: usize) -> Result<QueueHandle> {
229 if initial > max {
230 return Err(Error::InvalidArgument);
231 }
232 let handle = self.new_queue(max, Kind::Semaphore)?;
233 if let Ok(q) = self.queues.resolve_mut(handle) {
234 q.waiting = initial;
235 }
236 Ok(handle)
237 }
238
239 /// `xSemaphoreCreateMutex`: created **available**, unlike a semaphore.
240 ///
241 /// # Errors
242 /// As [`Kernel::queue_create`].
243 pub fn mutex_create(&mut self) -> Result<QueueHandle> {
244 self.new_mutex(Kind::Mutex)
245 }
246
247 /// `xSemaphoreCreateRecursiveMutex`.
248 ///
249 /// # Errors
250 /// As [`Kernel::queue_create`].
251 pub fn mutex_create_recursive(&mut self) -> Result<QueueHandle> {
252 self.new_mutex(Kind::RecursiveMutex)
253 }
254
255 fn new_mutex(&mut self, kind: Kind) -> Result<QueueHandle> {
256 let handle = self.new_queue(1, kind)?;
257 // `prvInitialiseMutex` gives the mutex once so it starts available,
258 // and it does so through `xQueueGenericSend` — which fires
259 // `traceQUEUE_SEND`. A mutex therefore has a `QUEUE_SEND` line
260 // immediately after its `QUEUE_CREATE`, before any task exists.
261 let _ = self.queue_send_generic(handle, 0, 0, Position::Back)?;
262 Ok(handle)
263 }
264
265 fn new_queue(&mut self, length: usize, kind: Kind) -> Result<QueueHandle> {
266 if length == 0 {
267 return Err(Error::InvalidArgument);
268 }
269 // A zero-item-size queue needs no storage; only real queues do.
270 let slots = if kind.carries_data() { length } else { 0 };
271 let base = self.slots_used;
272 let end = base.checked_add(slots).ok_or(Error::Full)?;
273 if end > SLOTS {
274 return Err(Error::Full);
275 }
276 let handle = self
277 .queues
278 .try_insert(Queue::new(base, length, kind))
279 .map_err(|_| Error::Full)?;
280 self.slots_used = end;
281 // `xQueueGenericCreate` takes the queue and its storage from the
282 // heap before it initialises anything.
283 self.account_for_allocation();
284 // `xQueueGenericReset` runs in a critical section before
285 // `traceQUEUE_CREATE` fires at the end of `prvInitialiseNewQueue`.
286 self.enter_critical();
287 self.exit_critical();
288 let tick = self.tick;
289 self.trace.note_exits(self.port.exits());
290 self.trace.event(
291 tick,
292 Event::QueueCreate {
293 queue: handle,
294 name: "",
295 length,
296 },
297 );
298 Ok(handle)
299 }
300
301 // ----------------------------------------------------------- getters --
302
303 /// `uxQueueMessagesWaiting`, which takes a critical section.
304 ///
305 /// # Errors
306 /// [`Error::Gone`] for a stale handle.
307 pub fn queue_messages_waiting(&mut self, queue: QueueHandle) -> Result<usize> {
308 self.enter_critical();
309 let n = self.queues.resolve(queue).map(|q| q.waiting);
310 self.exit_critical();
311 n
312 }
313
314 /// `xQueueIsQueueFullFromISR`, which takes NO critical section.
315 ///
316 /// The C compares `uxMessagesWaiting` against `uxLength` and returns,
317 /// with nothing around it — it is meant to be called from an interrupt,
318 /// where a critical section would be the wrong thing. That is why this
319 /// takes `&self` and [`Kernel::queue_messages_waiting`] takes `&mut
320 /// self`: the borrow is the proof that this one cannot charge the
321 /// clock, and on the sim a charged section is a sixteenth of a tick.
322 ///
323 /// # Errors
324 /// [`Error::Gone`] for a stale handle.
325 pub fn queue_is_full_from_isr(&self, queue: QueueHandle) -> Result<bool> {
326 self.queues.resolve(queue).map(|q| q.waiting == q.length)
327 }
328
329 /// `uxSemaphoreGetCount`, which is the same thing.
330 ///
331 /// # Errors
332 /// [`Error::Gone`] for a stale handle.
333 pub fn semaphore_count(&mut self, semaphore: QueueHandle) -> Result<usize> {
334 self.queue_messages_waiting(semaphore)
335 }
336
337 /// `xSemaphoreGetMutexHolderFromISR`, which is
338 /// `xQueueGetMutexHolderFromISR`: the same answer, read without a
339 /// critical section because an ISR is already inside one.
340 ///
341 /// # Errors
342 /// [`Error::Gone`] for a stale handle.
343 pub fn mutex_holder_from_isr(&mut self, mutex: QueueHandle) -> Result<TaskHandle> {
344 self.queues.resolve(mutex).map(|q| q.holder)
345 }
346
347 /// `xSemaphoreGetMutexHolder`, which takes a critical section.
348 ///
349 /// # Errors
350 /// [`Error::Gone`] for a stale handle.
351 pub fn mutex_holder(&mut self, mutex: QueueHandle) -> Result<TaskHandle> {
352 self.enter_critical();
353 let holder = self.queues.resolve(mutex).map(|q| q.holder);
354 self.exit_critical();
355 holder
356 }
357
358 // -------------------------------------------------------------- send --
359
360 /// `xQueueSend` / `xQueueSendToBack`.
361 ///
362 /// # Errors
363 /// [`Error::Full`] when the block time expires with the queue still
364 /// full; [`Error::Gone`] for a stale handle.
365 pub fn queue_send(&mut self, queue: QueueHandle, value: u64, ticks: u64) -> Result<Wait<()>> {
366 self.queue_send_generic(queue, value, ticks, Position::Back)
367 }
368
369 /// `xQueueReset`, which is `xQueueGenericReset( q, pdFALSE )`: empty
370 /// the queue and unlock it, waking one waiting sender if there is one.
371 ///
372 /// The event lists survive — only a brand-new queue re-initialises
373 /// those — so a task blocked on this queue stays blocked, and the one
374 /// woken here is woken because the reset made room.
375 ///
376 /// # Errors
377 /// [`Error::Gone`] for a stale handle.
378 pub fn queue_reset(&mut self, queue: QueueHandle) -> Result<()> {
379 self.enter_critical();
380 let result = (|| {
381 let length = self.queues.resolve(queue)?.length;
382 {
383 let q = self.queues.resolve_mut(queue)?;
384 q.waiting = 0;
385 q.write_to = 0;
386 q.read_from = length.saturating_sub(1);
387 q.rx_lock = UNLOCKED;
388 q.tx_lock = UNLOCKED;
389 }
390 let senders = Self::queue_send_list(queue);
391 if self.lists.is_empty(senders) == Ok(false) && self.remove_from_event_list(senders)? {
392 // queueYIELD_IF_USING_PREEMPTION(), inside the section.
393 self.port_yield();
394 }
395 Ok(())
396 })();
397 self.exit_critical();
398 result
399 }
400
401 /// `uxQueueSpacesAvailable`: how many more items would fit.
402 ///
403 /// # Errors
404 /// [`Error::Gone`] for a stale handle.
405 pub fn queue_spaces_available(&mut self, queue: QueueHandle) -> Result<usize> {
406 self.enter_critical();
407 let spaces = self
408 .queues
409 .resolve(queue)
410 .map(|q| q.length.wrapping_sub(q.waiting));
411 self.exit_critical();
412 spaces
413 }
414
415 // --------------------------------------------------- queue sets --
416 //
417 // A queue set is a queue whose items are the handles of the queues in
418 // it. Nothing is *moved* into the set: an item arriving on a member
419 // queue puts that queue's handle on the set, and the task that wakes on
420 // the set reads the handle and then reads the member queue itself. So a
421 // set holds an announcement, not the data — which is why adding a queue
422 // that already has items in it is refused, and why the two counts would
423 // otherwise drift apart forever.
424
425 /// `xQueueCreateSet`: a set that can hold `length` announcements.
426 ///
427 /// # Errors
428 /// As [`Kernel::queue_create`].
429 pub fn queue_create_set(&mut self, length: usize) -> Result<QueueHandle> {
430 self.new_queue(length, Kind::Set)
431 }
432
433 /// `xQueueAddToSet`.
434 ///
435 /// `false` is the C's `pdFAIL`: the target is not a set, the queue is
436 /// already in one, or it has items waiting that the set never saw.
437 ///
438 /// # Errors
439 /// [`Error::Gone`] for a stale handle.
440 pub fn queue_add_to_set(&mut self, queue: QueueHandle, set: QueueHandle) -> Result<bool> {
441 self.enter_critical();
442 let result = (|| {
443 if self.queues.resolve(set)?.kind != Kind::Set {
444 return Ok(false);
445 }
446 let member = self.queues.resolve(queue)?;
447 if member.set_container != QueueHandle::NULL || member.waiting != 0 {
448 return Ok(false);
449 }
450 self.queues.resolve_mut(queue)?.set_container = set;
451 Ok(true)
452 })();
453 self.exit_critical();
454 result
455 }
456
457 /// `xQueueRemoveFromSet`.
458 ///
459 /// # Errors
460 /// [`Error::Gone`] for a stale handle.
461 pub fn queue_remove_from_set(&mut self, queue: QueueHandle, set: QueueHandle) -> Result<bool> {
462 // Both refusals are tested OUTSIDE the section and only the removal
463 // itself is inside it, which is what the C does -- and it is NOT
464 // what `xQueueAddToSet` does, three functions up, where the whole
465 // body including both refusals is inside one section. The asymmetry
466 // is upstream's and it is load-bearing here: a failing remove costs
467 // the clock nothing, and `QueueSet`'s setup makes one deliberately,
468 // to prove a queue cannot be removed from a set it is not in.
469 // Charging for it put a tick one event early, 30 lines in.
470 let member = *self.queues.resolve(queue)?;
471 if member.set_container != set || member.waiting != 0 {
472 return Ok(false);
473 }
474 self.enter_critical();
475 let result = self
476 .queues
477 .resolve_mut(queue)
478 .map(|q| q.set_container = QueueHandle::NULL);
479 self.exit_critical();
480 result?;
481 Ok(true)
482 }
483
484 /// `xQueueSelectFromSet`: which member queue has something on it.
485 ///
486 /// The C is one line — `xQueueReceive( xQueueSet, &xReturn, xTicksToWait )`
487 /// — so this blocks, traces and costs exactly what a receive does, and
488 /// `None` is the C's `NULL`.
489 ///
490 /// # Errors
491 /// As [`Kernel::queue_receive`].
492 pub fn queue_select_from_set(
493 &mut self,
494 set: QueueHandle,
495 ticks: u64,
496 ) -> Result<Wait<Option<QueueHandle>>> {
497 match self.queue_receive(set, ticks) {
498 Ok(Blocked) => Ok(Blocked),
499 Ok(Ready(raw)) => {
500 let handle = QueueHandle::from_raw(u32::try_from(raw).unwrap_or(0));
501 Ok(Ready((handle != QueueHandle::NULL).then_some(handle)))
502 }
503 Err(Error::Empty) => Ok(Ready(None)),
504 Err(e) => Err(e),
505 }
506 }
507
508 /// `xQueueSelectFromSetFromISR`: which queue in `set` has data, from
509 /// an interrupt.
510 ///
511 /// The C is three lines -- `xQueueReceiveFromISR` on the set itself,
512 /// with a NULL handle meaning "none" -- because a set IS a queue of
513 /// member handles. This is that, and it takes no critical section for
514 /// the same reason the rest of the FromISR surface does not.
515 ///
516 /// # Errors
517 /// [`Error::Gone`] for a stale handle.
518 pub fn queue_select_from_set_from_isr(
519 &mut self,
520 set: QueueHandle,
521 ) -> Result<Option<QueueHandle>> {
522 match self.queue_receive_from_isr(set) {
523 Ok((raw, _woken)) => {
524 let handle = QueueHandle::from_raw(u32::try_from(raw).unwrap_or(0));
525 Ok((handle != QueueHandle::NULL).then_some(handle))
526 }
527 // An empty set is "no queue has data", which is what the C's
528 // NULL means; it is not a failure.
529 Err(Error::Empty) => Ok(None),
530 Err(e) => Err(e),
531 }
532 }
533
534 /// `prvNotifyQueueSetContainer`: put `queue`'s handle on the set it
535 /// belongs to. `true` when that woke a task that outranks the current
536 /// one.
537 fn notify_queue_set_container(&mut self, queue: QueueHandle) -> Result<bool> {
538 let set = self.queues.resolve(queue)?.set_container;
539 let container = *self.queues.resolve(set)?;
540 if container.waiting >= container.length {
541 // The C asserts this cannot happen and does nothing if it does.
542 return Ok(false);
543 }
544 let tx_lock = container.tx_lock;
545 let tick = self.tick;
546 self.trace.note_exits(self.port.exits());
547 // `traceQUEUE_SET_SEND` is `traceQUEUE_SEND` unless a port says
548 // otherwise, and this port does not.
549 self.trace.event(
550 tick,
551 Event::QueueSend {
552 queue: set,
553 name: "",
554 },
555 );
556 let mut woke =
557 self.copy_data_to_queue(set, &container, u64::from(queue.to_raw()), Position::Back)?;
558 if tx_lock == UNLOCKED {
559 let receivers = Self::queue_receive_list(set);
560 if self.lists.is_empty(receivers) == Ok(false)
561 && self.remove_from_event_list(receivers)?
562 {
563 woke = true;
564 }
565 } else {
566 self.increment_tx_lock(set, tx_lock);
567 }
568 Ok(woke)
569 }
570
571 /// `xQueueOverwrite`: write to a length-one queue whether or not it
572 /// already holds something.
573 ///
574 /// The C is `xQueueGenericSend( q, item, 0, queueOVERWRITE )` — a send
575 /// with no block time, because a queue that is always writable can
576 /// never make the caller wait.
577 ///
578 /// # Errors
579 /// As [`Kernel::queue_send`].
580 pub fn queue_overwrite(&mut self, queue: QueueHandle, value: u64) -> Result<Wait<()>> {
581 self.queue_send_generic(queue, value, 0, Position::Overwrite)
582 }
583
584 // ------------------------------------------------------- from an ISR --
585 //
586 // The `FromISR` half of the API. Three things make it a different
587 // animal from the task half, and all three are visible in a trace:
588 //
589 // * It never blocks and never yields. It reports that a higher
590 // priority task woke, by returning a [`Woken`], and the interrupt
591 // decides what to do about it on the way out.
592 // * Its critical section is `portSET_INTERRUPT_MASK_FROM_ISR`, not
593 // `portENTER_CRITICAL`. On the Posix port both are empty, so a call
594 // here costs no sim time at all — which is exactly what the C does
595 // and therefore what a matching trace requires.
596 // * If a task has the queue locked it may not touch the event lists.
597 // It counts what it did in `cTxLock` / `cRxLock` instead, and
598 // `prvUnlockQueue` pays the wake-ups back when the task is done.
599
600 /// `xQueueGenericSendFromISR`.
601 ///
602 /// # Errors
603 /// [`Error::Full`] when the queue is full and this is not an overwrite;
604 /// [`Error::Gone`] for a stale handle.
605 pub fn queue_send_generic_from_isr(
606 &mut self,
607 queue: QueueHandle,
608 value: u64,
609 position: Position,
610 ) -> Result<Woken> {
611 let mask = self.port.enter_critical_from_isr();
612 let result = self.send_from_isr_locked(queue, value, position);
613 self.port.exit_critical_from_isr(mask);
614 result
615 }
616
617 fn send_from_isr_locked(
618 &mut self,
619 queue: QueueHandle,
620 value: u64,
621 position: Position,
622 ) -> Result<Woken> {
623 let snapshot = *self.queues.resolve(queue)?;
624 if snapshot.waiting >= snapshot.length && position != Position::Overwrite {
625 // `traceQUEUE_SEND_FROM_ISR_FAILED` is not one of the harness's
626 // hooks, so a full queue says nothing on either side.
627 return Err(Error::Full);
628 }
629 let tx_lock = snapshot.tx_lock;
630 let tick = self.tick;
631 self.trace.note_exits(self.port.exits());
632 self.trace
633 .event(tick, Event::QueueSendFromIsr { queue, name: "" });
634 let previously_waiting = snapshot.waiting;
635 let _ = self.copy_data_to_queue(queue, &snapshot, value, position)?;
636 let mut woken = Woken::NO;
637 if tx_lock == UNLOCKED {
638 if snapshot.set_container != QueueHandle::NULL {
639 let overwrote = position == Position::Overwrite && previously_waiting > 0;
640 if !overwrote && self.notify_queue_set_container(queue)? {
641 woken = Woken::YES;
642 }
643 return Ok(woken);
644 }
645 let receivers = Self::queue_receive_list(queue);
646 if self.lists.is_empty(receivers) == Ok(false)
647 && self.remove_from_event_list(receivers)?
648 {
649 woken = Woken::YES;
650 }
651 } else {
652 self.increment_tx_lock(queue, tx_lock);
653 }
654 Ok(woken)
655 }
656
657 /// `xQueueSendToBackFromISR`.
658 ///
659 /// # Errors
660 /// As [`Kernel::queue_send_generic_from_isr`].
661 pub fn queue_send_from_isr(&mut self, queue: QueueHandle, value: u64) -> Result<Woken> {
662 self.queue_send_generic_from_isr(queue, value, Position::Back)
663 }
664
665 /// `xQueueSendToFrontFromISR`.
666 ///
667 /// # Errors
668 /// As [`Kernel::queue_send_generic_from_isr`].
669 pub fn queue_send_to_front_from_isr(
670 &mut self,
671 queue: QueueHandle,
672 value: u64,
673 ) -> Result<Woken> {
674 self.queue_send_generic_from_isr(queue, value, Position::Front)
675 }
676
677 /// `xQueueOverwriteFromISR`. Only for a queue of length one.
678 ///
679 /// # Errors
680 /// As [`Kernel::queue_send_generic_from_isr`].
681 pub fn queue_overwrite_from_isr(&mut self, queue: QueueHandle, value: u64) -> Result<Woken> {
682 self.queue_send_generic_from_isr(queue, value, Position::Overwrite)
683 }
684
685 /// `xSemaphoreGiveFromISR`.
686 ///
687 /// # Errors
688 /// As [`Kernel::queue_send_generic_from_isr`].
689 pub fn semaphore_give_from_isr(&mut self, semaphore: QueueHandle) -> Result<Woken> {
690 self.queue_send_generic_from_isr(semaphore, 0, Position::Back)
691 }
692
693 /// `xQueueReceiveFromISR`: the value, and whether a waiting sender that
694 /// this made room for outranks the interrupted task.
695 ///
696 /// # Errors
697 /// [`Error::Empty`] when the queue is empty; [`Error::Gone`] for a
698 /// stale handle.
699 pub fn queue_receive_from_isr(&mut self, queue: QueueHandle) -> Result<(u64, Woken)> {
700 let mask = self.port.enter_critical_from_isr();
701 let result = self.receive_from_isr_locked(queue);
702 self.port.exit_critical_from_isr(mask);
703 result
704 }
705
706 fn receive_from_isr_locked(&mut self, queue: QueueHandle) -> Result<(u64, Woken)> {
707 let snapshot = *self.queues.resolve(queue)?;
708 if snapshot.waiting == 0 {
709 return Err(Error::Empty);
710 }
711 let rx_lock = snapshot.rx_lock;
712 let tick = self.tick;
713 self.trace.note_exits(self.port.exits());
714 self.trace
715 .event(tick, Event::QueueReceiveFromIsr { queue, name: "" });
716 let value = self.copy_data_from_queue(queue, &snapshot, false)?;
717 let mut woken = Woken::NO;
718 if rx_lock == UNLOCKED {
719 let senders = Self::queue_send_list(queue);
720 if self.lists.is_empty(senders) == Ok(false) && self.remove_from_event_list(senders)? {
721 woken = Woken::YES;
722 }
723 } else {
724 self.increment_rx_lock(queue, rx_lock);
725 }
726 Ok((value, woken))
727 }
728
729 /// `xQueuePeekFromISR`: the head without removing it.
730 ///
731 /// It wakes nobody and traces nothing — the C has no hook for it —
732 /// which is why it returns a bare value rather than a [`Woken`].
733 ///
734 /// # Errors
735 /// [`Error::Empty`] when the queue is empty; [`Error::Gone`] for a
736 /// stale handle.
737 pub fn queue_peek_from_isr(&mut self, queue: QueueHandle) -> Result<u64> {
738 let mask = self.port.enter_critical_from_isr();
739 let result = match self.queues.resolve(queue) {
740 Ok(q) if q.waiting > 0 => {
741 let snapshot = *q;
742 self.copy_data_from_queue(queue, &snapshot, true)
743 }
744 Ok(_) => Err(Error::Empty),
745 Err(e) => Err(e),
746 };
747 self.port.exit_critical_from_isr(mask);
748 result
749 }
750
751 /// `prvIncrementQueueTxLock`, capped at the task count as the C caps it.
752 fn increment_tx_lock(&mut self, queue: QueueHandle, tx_lock: i8) {
753 let tasks = self.task_count();
754 if let Ok(q) = self.queues.resolve_mut(queue) {
755 if i64::from(tx_lock) < tasks as i64 {
756 q.tx_lock = tx_lock.saturating_add(1);
757 }
758 }
759 }
760
761 /// `prvIncrementQueueRxLock`.
762 fn increment_rx_lock(&mut self, queue: QueueHandle, rx_lock: i8) {
763 let tasks = self.task_count();
764 if let Ok(q) = self.queues.resolve_mut(queue) {
765 if i64::from(rx_lock) < tasks as i64 {
766 q.rx_lock = rx_lock.saturating_add(1);
767 }
768 }
769 }
770
771 /// `xQueueSendToFront`.
772 ///
773 /// # Errors
774 /// As [`Kernel::queue_send`].
775 pub fn queue_send_to_front(
776 &mut self,
777 queue: QueueHandle,
778 value: u64,
779 ticks: u64,
780 ) -> Result<Wait<()>> {
781 self.queue_send_generic(queue, value, ticks, Position::Front)
782 }
783
784 /// `xSemaphoreGive`: a send with no data.
785 ///
786 /// # Errors
787 /// [`Error::Full`] when the semaphore is already at its maximum count.
788 pub fn semaphore_give(&mut self, semaphore: QueueHandle) -> Result<Wait<()>> {
789 self.queue_send_generic(semaphore, 0, 0, Position::Back)
790 }
791
792 /// `xQueueGenericSend`, one pass of its `for(;;)`.
793 ///
794 /// # Errors
795 /// [`Error::Full`] on timeout; [`Error::Gone`] for a stale handle.
796 /// In line on purpose, for the same reason as
797 /// [`Kernel::queue_take`]: seven instructions of prologue and ten of
798 /// epilogue on every one of khot-ir's 48,000 sends, so that `xQueueSend`
799 /// and its siblings could pass four arguments through unchanged. This
800 /// one pays on every instrument, `kernel-ir` included.
801 #[inline(always)]
802 pub fn queue_send_generic(
803 &mut self,
804 queue: QueueHandle,
805 value: u64,
806 ticks: u64,
807 position: Position,
808 ) -> Result<Wait<()>> {
809 let caller = self.current;
810 // Resuming below the sampling exit, as the C's thread does. See
811 // `Kernel::take_queue_resume`: the prologue has already run, and
812 // repeating it would spend a second critical-section exit that the
813 // C never spends.
814 if self.take_queue_resume(caller) != QueueResume::No {
815 // The send's only resume point is below its sampling exit: its
816 // timed-out branch has nothing after `resume_all` that costs an
817 // exit, so being switched away there changes no count.
818 return self.queue_send_blocking(caller, queue);
819 }
820 self.enter_critical();
821 let snapshot = match self.queues.resolve(queue) {
822 Ok(q) => *q,
823 Err(e) => {
824 self.exit_critical();
825 return Err(e);
826 }
827 };
828 // The wait frame is set where the C sets it: `xQueueReceive` and
829 // `xQueueGenericSend` call `vTaskInternalSetTimeOutState` only
830 // after finding the queue unusable AND the block time non-zero,
831 // guarded by `xEntryTimeSet`. Setting it up here instead meant
832 // every *successful* call wrote a six-field frame and then had
833 // `end_wait` wipe it on the way out.
834 //
835 // It also has to be after the resolve, so that a handle naming
836 // nothing leaves the caller exactly as it found it -- which is
837 // what `configASSERT( pxQueue )` means in the C.
838 // `( uxMessagesWaiting < uxLength ) || ( xCopyPosition == queueOVERWRITE )`
839 if snapshot.waiting < snapshot.length || position == Position::Overwrite {
840 self.trace.note_exits(self.port.exits());
841 let tick = self.tick;
842 self.trace.event(tick, Event::QueueSend { queue, name: "" });
843 let previously_waiting = snapshot.waiting;
844 let yield_required = self.copy_data_to_queue(queue, &snapshot, value, position)?;
845 if snapshot.set_container != QueueHandle::NULL {
846 // A queue in a set announces the arrival there, not here.
847 // An overwrite of an item that was already present is not an
848 // arrival: the count did not change, so the set is not told.
849 let overwrote = position == Position::Overwrite && previously_waiting > 0;
850 if !overwrote && self.notify_queue_set_container(queue)? {
851 self.port_yield();
852 }
853 self.exit_critical();
854 self.end_wait(caller);
855 return Ok(Ready(()));
856 }
857 let receivers = Self::queue_receive_list(queue);
858 let woke_higher = if self.lists.is_empty(receivers) == Ok(false) {
859 self.remove_from_event_list(receivers)?
860 } else {
861 false
862 };
863 // queueYIELD_IF_USING_PREEMPTION(), inside the section.
864 // `yield_required` first: it is false on every send that is
865 // not a mutex give, and it is a local, so on the common path
866 // the list is not read a second time at all. Both operands are
867 // pure, so the order is free to choose. (The read cannot be
868 // hoisted above the removal above it -- removing the last
869 // waiter is exactly what changes the answer.)
870 if woke_higher || (yield_required && self.lists.is_empty(receivers) != Ok(false)) {
871 self.port_yield();
872 }
873 self.exit_critical();
874 self.end_wait(caller);
875 return Ok(Ready(()));
876 }
877 if let Err(e) = self.begin_wait(caller, queue, ticks) {
878 self.exit_critical();
879 return Err(e);
880 }
881 if self.remaining_ticks(caller) == 0 {
882 self.exit_critical();
883 self.trace_failure_or_owe(caller, OwedTrace::SendFailed(queue));
884 self.end_wait(caller);
885 return Err(Error::Full);
886 }
887 self.exit_critical();
888 // That exit can release a tick, and a tick can switch this task
889 // away. The C's thread stops INSIDE the exit, so everything below
890 // runs when the task is scheduled again -- and names that task.
891 if self.current != caller {
892 self.set_queue_resume(caller, QueueResume::BelowSample);
893 return Ok(Blocked);
894 }
895 self.queue_send_blocking(caller, queue)
896 }
897
898 /// Everything `xQueueGenericSend` does below its sampling exit.
899 ///
900 /// Split out so a call preempted at that exit re-enters here rather
901 /// than at the top. See [`Kernel::take_queue_resume`].
902 fn queue_send_blocking(&mut self, caller: TaskHandle, queue: QueueHandle) -> Result<Wait<()>> {
903 self.suspend_all();
904 self.lock_queue(queue);
905 if self.check_for_timeout(caller) {
906 self.unlock_queue(queue)?;
907 let _ = self.resume_all();
908 self.trace_failure_or_owe(caller, OwedTrace::SendFailed(queue));
909 self.end_wait(caller);
910 return Err(Error::Full);
911 }
912 if self.is_queue_full(queue) {
913 self.trace.note_exits(self.port.exits());
914 let tick = self.tick;
915 self.trace
916 .event(tick, Event::BlockingOnQueueSend { queue, name: "" });
917 let ticks_left = self.remaining_ticks(caller);
918 self.place_on_event_list(Self::queue_send_list(queue), ticks_left)?;
919 self.unlock_queue(queue)?;
920 if !self.resume_all() {
921 self.yield_or_owe(caller);
922 }
923 } else {
924 self.unlock_queue(queue)?;
925 let _ = self.resume_all();
926 }
927 Ok(Blocked)
928 }
929
930 /// `prvCopyDataToQueue`; `true` when giving a mutex back lowered the
931 /// giver's priority and a yield is therefore wanted.
932 ///
933 /// This resolves rather than taking the caller's snapshot, and
934 /// [`Kernel::copy_data_from_queue`] does the opposite. That is not an
935 /// inconsistency: both forms were measured on both helpers and they
936 /// disagree, because the writer's caller does not keep its snapshot
937 /// live across the call and the reader's does.
938 ///
939 /// In line on purpose. `queue_send_generic` is its only caller, and out
940 /// of line it paid a call and a frame on all 48,000 sends to move one
941 /// value into a slot the caller had already resolved. Its reader twin
942 /// was already being inlined by LLVM; this one was over the size
943 /// threshold and was not.
944 #[inline(always)]
945 fn copy_data_to_queue(
946 &mut self,
947 queue: QueueHandle,
948 snapshot: &Queue,
949 value: u64,
950 position: Position,
951 ) -> Result<bool> {
952 // The snapshot comes from the caller, which has already resolved this
953 // handle -- and nothing between that resolve and this call can reach
954 // the arena. Resolving again here made every send pay for the same
955 // four checks twice. `copy_data_from_queue` has always taken its
956 // snapshot this way.
957 let mut yield_required = false;
958 // `prvCopyDataToQueue` counts the item in — except on an overwrite
959 // of a queue that already held one, where it decrements first so
960 // the increment cancels and the count stays where it was.
961 //
962 // Decided once, up here, so that each arm below can fold the count
963 // into the resolve it is already making. Deciding it after the arms
964 // is what forced a further resolve for one field.
965 let counted = !(position == Position::Overwrite && snapshot.waiting > 0);
966 if snapshot.kind.carries_data() {
967 let (index, next_read, next_write) = match position {
968 Position::Back => (
969 snapshot.write_to,
970 snapshot.read_from,
971 wrap_next(snapshot.write_to, snapshot.length),
972 ),
973 // Write at the item last read, then step back, so the next
974 // read's pre-increment lands on it. An overwrite takes the
975 // same path; only the message count differs.
976 Position::Front | Position::Overwrite => (
977 snapshot.read_from,
978 wrap_prev(snapshot.read_from, snapshot.length),
979 snapshot.write_to,
980 ),
981 };
982 // `base + index` is below `base + length`, which the geometry
983 // put inside `SLOTS`; and the `get_mut` refuses anything it is
984 // not, so nothing rests on the arithmetic either way.
985 if let Some(slot) = self.slots.get_mut(snapshot.base.wrapping_add(index)) {
986 *slot = value;
987 }
988 let q = self.queues.resolve_mut(queue)?;
989 q.read_from = next_read;
990 q.write_to = next_write;
991 if counted {
992 q.waiting = q.waiting.wrapping_add(1);
993 }
994 } else if snapshot.kind.is_mutex() {
995 // Giving a mutex back: the holder drops any inherited
996 // priority. The disinherit touches the holder's TCB, not this
997 // queue, so the resolve after it is still the first one.
998 yield_required = self.priority_disinherit(snapshot.holder)?;
999 let q = self.queues.resolve_mut(queue)?;
1000 q.holder = TaskHandle::NULL;
1001 if counted {
1002 q.waiting = q.waiting.wrapping_add(1);
1003 }
1004 } else if counted {
1005 // A counting or binary semaphore: only the count moves.
1006 let q = self.queues.resolve_mut(queue)?;
1007 q.waiting = q.waiting.wrapping_add(1);
1008 }
1009 Ok(yield_required)
1010 }
1011
1012 // ----------------------------------------------------------- receive --
1013
1014 /// `xQueueReceive`.
1015 ///
1016 /// # Errors
1017 /// [`Error::Empty`] when the block time expires with the queue still
1018 /// empty; [`Error::Gone`] for a stale handle.
1019 pub fn queue_receive(&mut self, queue: QueueHandle, ticks: u64) -> Result<Wait<u64>> {
1020 self.queue_take::<false>(queue, ticks)
1021 }
1022
1023 /// `xQueuePeek`: read the head without removing it.
1024 ///
1025 /// # Errors
1026 /// As [`Kernel::queue_receive`].
1027 pub fn queue_peek(&mut self, queue: QueueHandle, ticks: u64) -> Result<Wait<u64>> {
1028 self.queue_take::<true>(queue, ticks)
1029 }
1030
1031 /// `xSemaphoreTake`, which is `xQueueSemaphoreTake`: a receive with no
1032 /// data, and — on a mutex — with priority inheritance while it waits.
1033 ///
1034 /// # Errors
1035 /// [`Error::Empty`] on timeout; [`Error::Gone`] for a stale handle.
1036 pub fn semaphore_take(&mut self, semaphore: QueueHandle, ticks: u64) -> Result<Wait<()>> {
1037 match self.queue_take::<false>(semaphore, ticks)? {
1038 Blocked => Ok(Blocked),
1039 Ready(_) => Ok(Ready(())),
1040 }
1041 }
1042
1043 /// The shared body of `xQueueReceive`, `xQueuePeek` and
1044 /// `xQueueSemaphoreTake` — one pass of the C `for(;;)`.
1045 /// `PEEK` is a const because it is one at every call site: a receive
1046 /// passes `false`, a peek `true`, and a semaphore take `false`. As a
1047 /// parameter it cost an argument to set up and a branch at each of the
1048 /// ten places this function and its copy helper consult it.
1049 /// In line on purpose, into all three of `xQueueReceive`, `xQueuePeek`
1050 /// and `xSemaphoreTake`.
1051 ///
1052 /// Out of line it charged eight instructions of prologue and ten of
1053 /// epilogue to every take -- 48,000 of them in `khot-ir`, which is 7.6%
1054 /// of that instrument -- so that three wrappers could each hand it two
1055 /// arguments and return its result unchanged. LLVM declined on size; the
1056 /// frame is the reason to overrule that.
1057 ///
1058 /// The cost is stated rather than hidden: `kernel-ir` pays 214,804 for
1059 /// the larger wrappers. Splitting the body from the symbol does not
1060 /// recover it -- outlining at the semaphore site alone leaves `kernel-ir`
1061 /// 262,828 up and gives back 427,983 of `khot-ir`'s win, and outlining at
1062 /// the receive site leaves it 190,238 up and gives back 1,104,183 -- so
1063 /// the regression is the wrappers growing, not any one call site.
1064 #[inline(always)]
1065 fn queue_take<const PEEK: bool>(
1066 &mut self,
1067 queue: QueueHandle,
1068 ticks: u64,
1069 ) -> Result<Wait<u64>> {
1070 let caller = self.current;
1071 // Resuming below the sampling exit, as `queue_send_generic` does.
1072 // The snapshot is re-read rather than carried: nothing below uses
1073 // anything of it that a concurrent access could change, and the
1074 // read takes no critical section, so it costs the clock nothing.
1075 match self.take_queue_resume(caller) {
1076 QueueResume::No => {}
1077 QueueResume::BelowSample => {
1078 let snapshot = *self.queues.resolve(queue)?;
1079 return self.queue_take_blocking::<PEEK>(caller, queue, snapshot);
1080 }
1081 QueueResume::BelowTimedOutResume => {
1082 let snapshot = *self.queues.resolve(queue)?;
1083 return self.queue_take_timed_out::<PEEK>(caller, queue, snapshot);
1084 }
1085 }
1086 self.enter_critical();
1087 let snapshot = match self.queues.resolve(queue) {
1088 Ok(q) => *q,
1089 Err(e) => {
1090 self.exit_critical();
1091 return Err(e);
1092 }
1093 };
1094 // The wait frame is set where the C sets it: `xQueueReceive` and
1095 // `xQueueGenericSend` call `vTaskInternalSetTimeOutState` only
1096 // after finding the queue unusable AND the block time non-zero,
1097 // guarded by `xEntryTimeSet`. Setting it up here instead meant
1098 // every *successful* call wrote a six-field frame and then had
1099 // `end_wait` wipe it on the way out.
1100 //
1101 // It also has to be after the resolve, so that a handle naming
1102 // nothing leaves the caller exactly as it found it -- which is
1103 // what `configASSERT( pxQueue )` means in the C.
1104 if snapshot.waiting > 0 {
1105 let value = self.copy_data_from_queue(queue, &snapshot, PEEK)?;
1106 self.trace.note_exits(self.port.exits());
1107 let tick = self.tick;
1108 // `xQueuePeek` is its own function in the C with its own trace
1109 // macro; `xQueueSemaphoreTake` shares `xQueueReceive`'s.
1110 let event = if PEEK {
1111 Event::QueuePeek { queue, name: "" }
1112 } else {
1113 Event::QueueReceive { queue, name: "" }
1114 };
1115 self.trace.event(tick, event);
1116 if PEEK {
1117 // A PEEK wakes another *receiver*, not a sender: the item
1118 // is still there.
1119 let receivers = Self::queue_receive_list(queue);
1120 if self.lists.is_empty(receivers) == Ok(false)
1121 && self.remove_from_event_list(receivers)?
1122 {
1123 self.port_yield();
1124 }
1125 } else {
1126 if snapshot.kind.is_mutex() {
1127 self.queues.resolve_mut(queue)?.holder = caller;
1128 self.increment_mutexes_held(caller);
1129 }
1130 let senders = Self::queue_send_list(queue);
1131 if self.lists.is_empty(senders) == Ok(false)
1132 && self.remove_from_event_list(senders)?
1133 {
1134 self.port_yield();
1135 }
1136 }
1137 self.exit_critical();
1138 self.end_wait(caller);
1139 return Ok(Ready(value));
1140 }
1141 if let Err(e) = self.begin_wait(caller, queue, ticks) {
1142 self.exit_critical();
1143 return Err(e);
1144 }
1145 if self.remaining_ticks(caller) == 0 {
1146 self.exit_critical();
1147 // `traceQUEUE_PEEK_FAILED` is not one of the harness's hooks,
1148 // so a failed PEEK says nothing on either side.
1149 if !PEEK {
1150 self.trace_failure_or_owe(caller, OwedTrace::ReceiveFailed(queue));
1151 }
1152 self.end_wait(caller);
1153 return Err(Error::Empty);
1154 }
1155 self.exit_critical();
1156 // That exit can release a tick, and a tick can switch this task
1157 // away. The C's thread stops INSIDE the exit, so everything below
1158 // runs when the task is scheduled again -- and names that task.
1159 if self.current != caller {
1160 self.set_queue_resume(caller, QueueResume::BelowSample);
1161 return Ok(Blocked);
1162 }
1163 self.queue_take_blocking::<PEEK>(caller, queue, snapshot)
1164 }
1165
1166 /// Everything `xQueueReceive` and `xQueuePeek` do below their sampling
1167 /// exit. See [`Kernel::take_queue_resume`].
1168 fn queue_take_blocking<const PEEK: bool>(
1169 &mut self,
1170 caller: TaskHandle,
1171 queue: QueueHandle,
1172 snapshot: Queue,
1173 ) -> Result<Wait<u64>> {
1174 self.suspend_all();
1175 self.lock_queue(queue);
1176 if self.check_for_timeout(caller) {
1177 self.unlock_queue(queue)?;
1178 let _ = self.resume_all();
1179 // `xTaskResumeAll` replays any tick that pended while the
1180 // scheduler was suspended, and replaying it can switch this task
1181 // away with `prvIsQueueEmpty` still to run. The C's thread stops
1182 // inside it; everything below runs when the task is next
1183 // scheduled. Without this, `prvIsQueueEmpty`'s section is spent
1184 // in the wrong window -- one exit, and IntQueue's 100,000-tick
1185 // run diverges at tick 16,260.
1186 if self.current != caller {
1187 self.set_queue_resume(caller, QueueResume::BelowTimedOutResume);
1188 return Ok(Blocked);
1189 }
1190 return self.queue_take_timed_out::<PEEK>(caller, queue, snapshot);
1191 }
1192 self.queue_take_locked::<PEEK>(caller, queue, snapshot)
1193 }
1194
1195 /// `xQueueReceive` below the `xTaskResumeAll` of its timed-out branch.
1196 fn queue_take_timed_out<const PEEK: bool>(
1197 &mut self,
1198 caller: TaskHandle,
1199 queue: QueueHandle,
1200 snapshot: Queue,
1201 ) -> Result<Wait<u64>> {
1202 {
1203 if self.is_queue_empty(queue) {
1204 if snapshot.kind.is_mutex() && self.wait_inherited(caller) {
1205 // `vTaskPriorityDisinheritAfterTimeout`: the holder
1206 // keeps only what the still-waiting tasks justify.
1207 self.enter_critical();
1208 let highest = self.highest_waiting_priority(queue)?;
1209 let holder = self.queues.resolve(queue).map(|q| q.holder)?;
1210 self.priority_disinherit_after_timeout(holder, highest)?;
1211 self.exit_critical();
1212 }
1213 if !PEEK {
1214 self.trace_failure_or_owe(caller, OwedTrace::ReceiveFailed(queue));
1215 }
1216 self.end_wait(caller);
1217 return Err(Error::Empty);
1218 }
1219 Ok(Blocked)
1220 }
1221 }
1222
1223 /// `xQueueReceive` below its timeout test, with the queue still locked.
1224 fn queue_take_locked<const PEEK: bool>(
1225 &mut self,
1226 caller: TaskHandle,
1227 queue: QueueHandle,
1228 snapshot: Queue,
1229 ) -> Result<Wait<u64>> {
1230 if self.is_queue_empty(queue) {
1231 let event = if PEEK {
1232 Event::BlockingOnQueuePeek { queue, name: "" }
1233 } else {
1234 Event::BlockingOnQueueReceive { queue, name: "" }
1235 };
1236 self.trace.note_exits(self.port.exits());
1237 let tick = self.tick;
1238 self.trace.event(tick, event);
1239 if snapshot.kind.is_mutex() {
1240 self.enter_critical();
1241 let holder = self.queues.resolve(queue).map(|q| q.holder)?;
1242 let inherited = self.priority_inherit(holder)?;
1243 self.exit_critical();
1244 if inherited {
1245 self.set_wait_inherited(caller);
1246 }
1247 }
1248 let ticks_left = self.remaining_ticks(caller);
1249 self.place_on_event_list(Self::queue_receive_list(queue), ticks_left)?;
1250 self.unlock_queue(queue)?;
1251 if !self.resume_all() {
1252 self.yield_or_owe(caller);
1253 }
1254 } else {
1255 self.unlock_queue(queue)?;
1256 let _ = self.resume_all();
1257 }
1258 Ok(Blocked)
1259 }
1260
1261 /// `prvCopyDataFromQueue`, with the peek variant putting the cursor
1262 /// back as `xQueuePeek` does.
1263 /// `prvCopyDataFromQueue`, over a snapshot its caller already took.
1264 ///
1265 /// Every caller resolves this queue to decide whether there is
1266 /// anything to read; this used to resolve it again to do the reading.
1267 /// The snapshot cannot be stale — nothing between the two touches this
1268 /// queue, and both are inside the same critical section.
1269 ///
1270 /// The same change to [`Kernel::copy_data_to_queue`] is **refused**,
1271 /// and the pair is why this bench exists: threading the snapshot into
1272 /// the writer is worth -245,389 Ir inside it and +211,717 in
1273 /// `queue_send_generic`, because a `&Queue` argument forces the
1274 /// caller's local to be addressable and it stops living in registers.
1275 /// Here the caller is `queue_take`, which keeps its snapshot alive
1276 /// across the call anyway, so there is no spill to pay for.
1277 fn copy_data_from_queue(
1278 &mut self,
1279 queue: QueueHandle,
1280 snapshot: &Queue,
1281 peek: bool,
1282 ) -> Result<u64> {
1283 if !snapshot.kind.carries_data() {
1284 if !peek {
1285 let q = self.queues.resolve_mut(queue)?;
1286 // Wrapping: all three callers reach here under
1287 // `waiting > 0`, and the doc above says the snapshot they
1288 // tested cannot be stale, so there is an item to take.
1289 q.waiting = q.waiting.wrapping_sub(1);
1290 }
1291 return Ok(0);
1292 }
1293 let next = wrap_next(snapshot.read_from, snapshot.length);
1294 let value = self
1295 .slots
1296 .get(snapshot.base.wrapping_add(next))
1297 .copied()
1298 .unwrap_or(0);
1299 // `xQueuePeek` saves and restores `pcReadFrom` — which is to
1300 // say it leaves it exactly as it found it. Writing it back stored
1301 // the value already there, and the resolve that reached it was a
1302 // whole handle validation for a no-op.
1303 if !peek {
1304 let q = self.queues.resolve_mut(queue)?;
1305 q.read_from = next;
1306 // Wrapping, for the same reason as the branch above.
1307 q.waiting = q.waiting.wrapping_sub(1);
1308 }
1309 Ok(value)
1310 }
1311
1312 // ------------------------------------------------------ recursive mutex --
1313
1314 /// `xSemaphoreTakeRecursive`.
1315 ///
1316 /// # Errors
1317 /// [`Error::Empty`] on timeout; [`Error::Gone`] for a stale handle.
1318 pub fn mutex_take_recursive(&mut self, mutex: QueueHandle, ticks: u64) -> Result<Wait<()>> {
1319 let caller = self.current;
1320 // One resolve reads the holder and counts the re-entry.
1321 // `resolve_mut` runs the same four checks and answers the same
1322 // errors, so asking twice only asked twice.
1323 {
1324 let q = self.queues.resolve_mut(mutex)?;
1325 if q.holder == caller {
1326 q.recursions = q.recursions.wrapping_add(1);
1327 return Ok(Ready(()));
1328 }
1329 }
1330 match self.semaphore_take(mutex, ticks)? {
1331 Blocked => Ok(Blocked),
1332 Ready(()) => {
1333 let q = self.queues.resolve_mut(mutex)?;
1334 q.recursions = q.recursions.wrapping_add(1);
1335 Ok(Ready(()))
1336 }
1337 }
1338 }
1339
1340 /// `xSemaphoreGiveRecursive`.
1341 ///
1342 /// # Errors
1343 /// [`Error::NotActive`] when the caller does not hold the mutex.
1344 pub fn mutex_give_recursive(&mut self, mutex: QueueHandle) -> Result<()> {
1345 let caller = self.current;
1346 // One resolve, as in the take above.
1347 let remaining = {
1348 let q = self.queues.resolve_mut(mutex)?;
1349 if q.holder != caller {
1350 return Err(Error::NotActive);
1351 }
1352 q.recursions = q.recursions.saturating_sub(1);
1353 q.recursions
1354 };
1355 if remaining == 0 {
1356 let _ = self.semaphore_give(mutex)?;
1357 }
1358 Ok(())
1359 }
1360
1361 // -------------------------------------------------------- queue locks --
1362
1363 /// `prvLockQueue`: one critical section. The lock counters themselves
1364 /// only matter to an ISR, and the sim has none — but the section is
1365 /// where sim time passes, so it is here.
1366 /// `prvLockQueue`: stop interrupts taking tasks off this queue's event
1367 /// lists while a task walks them. An interrupt that finds the queue
1368 /// locked counts what it did instead, and [`Kernel::unlock_queue`] pays
1369 /// it back.
1370 fn lock_queue(&mut self, queue: QueueHandle) {
1371 self.enter_critical();
1372 if let Ok(q) = self.queues.resolve_mut(queue) {
1373 if q.rx_lock == UNLOCKED {
1374 q.rx_lock = LOCKED_UNMODIFIED;
1375 }
1376 if q.tx_lock == UNLOCKED {
1377 q.tx_lock = LOCKED_UNMODIFIED;
1378 }
1379 }
1380 self.exit_critical();
1381 }
1382
1383 /// `prvUnlockQueue`: two critical sections, one per lock counter. The
1384 /// loops inside them wake tasks that an ISR queued while the lock was
1385 /// held, and on the sim there are none.
1386 /// `prvUnlockQueue`: two critical sections, one per lock count, each
1387 /// waking one task per send or receive an interrupt made while the
1388 /// queue was locked. The yields are *missed* yields — pended, not
1389 /// taken — because a task is walking the list.
1390 fn unlock_queue(&mut self, queue: QueueHandle) -> Result<()> {
1391 self.enter_critical();
1392 {
1393 // One resolve for both fields. They were two, and a resolve
1394 // is a null test, a bounds check, a generation compare and an
1395 // `Option` unwrap -- all of it repeated to reach the same slot.
1396 let (mut tx_lock, container) = self
1397 .queues
1398 .resolve(queue)
1399 .map_or((UNLOCKED, QueueHandle::NULL), |q| {
1400 (q.tx_lock, q.set_container)
1401 });
1402 while tx_lock > LOCKED_UNMODIFIED {
1403 if container != QueueHandle::NULL {
1404 if self.notify_queue_set_container(queue)? {
1405 self.missed_yield();
1406 }
1407 // Wrapping: the loop runs only while this is above
1408 // `LOCKED_UNMODIFIED`, so one off it stays in range.
1409 tx_lock = tx_lock.wrapping_sub(1);
1410 continue;
1411 }
1412 let receivers = Self::queue_receive_list(queue);
1413 if self.lists.is_empty(receivers) == Ok(true) {
1414 break;
1415 }
1416 if self.remove_from_event_list(receivers)? {
1417 self.missed_yield();
1418 }
1419 // Wrapping: bounded by the loop, as above.
1420 tx_lock = tx_lock.wrapping_sub(1);
1421 }
1422 if let Ok(q) = self.queues.resolve_mut(queue) {
1423 q.tx_lock = UNLOCKED;
1424 }
1425 }
1426 self.exit_critical();
1427 self.enter_critical();
1428 {
1429 let mut rx_lock = self
1430 .queues
1431 .resolve(queue)
1432 .map(|q| q.rx_lock)
1433 .unwrap_or(UNLOCKED);
1434 while rx_lock > LOCKED_UNMODIFIED {
1435 let senders = Self::queue_send_list(queue);
1436 if self.lists.is_empty(senders) == Ok(true) {
1437 break;
1438 }
1439 if self.remove_from_event_list(senders)? {
1440 self.missed_yield();
1441 }
1442 // Wrapping: bounded by the loop, as above.
1443 rx_lock = rx_lock.wrapping_sub(1);
1444 }
1445 if let Ok(q) = self.queues.resolve_mut(queue) {
1446 q.rx_lock = UNLOCKED;
1447 }
1448 }
1449 self.exit_critical();
1450 Ok(())
1451 }
1452
1453 /// `vQueueWaitForMessageRestricted`: what the timer service task does
1454 /// when it has no timer to run — block on the command queue without
1455 /// suspending the scheduler.
1456 ///
1457 /// `wait_indefinitely` is the C third argument: when set the wait
1458 /// becomes `portMAX_DELAY` and the task goes to the suspended list
1459 /// rather than a delayed one. The queue locks around it are C's
1460 /// `prvLockQueue` and `prvUnlockQueue` — three critical sections, and
1461 /// therefore three sixteenths of a tick's worth of sim time.
1462 ///
1463 /// # Errors
1464 /// A list error surfaces as itself.
1465 pub fn wait_for_message_restricted(
1466 &mut self,
1467 queue: QueueHandle,
1468 ticks: u64,
1469 wait_indefinitely: bool,
1470 ) -> Result<()> {
1471 self.lock_queue(queue);
1472 let empty = self
1473 .queues
1474 .resolve(queue)
1475 .map(|q| q.waiting == 0)
1476 .unwrap_or(false);
1477 if empty {
1478 // vTaskPlaceOnEventListRestricted
1479 let current = self.current;
1480 let event = Self::event_item(current);
1481 self.lists
1482 .insert_end(Self::queue_receive_list(queue), event)?;
1483 let ticks = if wait_indefinitely {
1484 Self::MAX_DELAY
1485 } else {
1486 ticks
1487 };
1488 let wake_at = self.tick.wrapping_add(ticks) & Self::MAX_DELAY;
1489 self.trace_task(current, |task, name| Event::TaskDelayUntil {
1490 task,
1491 name,
1492 wake_at,
1493 });
1494 self.add_current_task_to_delayed_list(ticks, wait_indefinitely)?;
1495 }
1496 self.unlock_queue(queue)?;
1497 Ok(())
1498 }
1499
1500 /// `prvIsQueueEmpty`, critical section and all — it is one of the
1501 /// four the blocking path spends, and therefore a sixteenth of a tick
1502 /// every four calls.
1503 fn is_queue_empty(&mut self, queue: QueueHandle) -> bool {
1504 self.enter_critical();
1505 let empty = self
1506 .queues
1507 .resolve(queue)
1508 .map(|q| q.waiting == 0)
1509 .unwrap_or(true);
1510 self.exit_critical();
1511 empty
1512 }
1513
1514 /// `prvIsQueueFull`, likewise.
1515 fn is_queue_full(&mut self, queue: QueueHandle) -> bool {
1516 self.enter_critical();
1517 let full = self
1518 .queues
1519 .resolve(queue)
1520 .map(|q| q.waiting >= q.length)
1521 .unwrap_or(false);
1522 self.exit_critical();
1523 full
1524 }
1525
1526 /// `prvGetHighestPriorityOfWaitToReceiveList`.
1527 fn highest_waiting_priority(&self, queue: QueueHandle) -> Result<u8> {
1528 let list = Self::queue_receive_list(queue);
1529 match self.lists.head(list)? {
1530 // The event item's value is `configMAX_PRIORITIES - priority`.
1531 Some(item) => {
1532 let value = self.lists.value(item)?;
1533 let max = u64::from(C::MAX_PRIORITIES);
1534 Ok(u8::try_from(max.saturating_sub(value)).unwrap_or(0))
1535 }
1536 None => Ok(0),
1537 }
1538 }
1539}
1540
1541/// The next index in a ring of `length`.
1542const fn wrap_next(index: usize, length: usize) -> usize {
1543 // `wrapping_add`, and it is the SAME function, not an approximation
1544 // of it: at `usize::MAX` saturating yields `usize::MAX`, which is
1545 // `>= length`, so the caller gets 0 -- and wrapping yields 0, which is
1546 // `< length`, so the caller gets 0 as well. Every other index agrees
1547 // without argument.
1548 let next = index.wrapping_add(1);
1549 if next >= length { 0 } else { next }
1550}
1551
1552/// The previous index in a ring of `length`.
1553const fn wrap_prev(index: usize, length: usize) -> usize {
1554 match index.checked_sub(1) {
1555 Some(prev) => prev,
1556 None => length.saturating_sub(1),
1557 }
1558}
1559
1560/// The Rust face's raw surface: see [`crate::typed`].
1561///
1562/// Every method here is one of the kernel's own calls under another name.
1563/// The one that is not — [`Raw::raw_queue_has_room`] — is the kernel
1564/// reading its own queue rather than a task asking, so it takes no
1565/// critical section and costs no sim time, which is what lets the typed
1566/// face be free.
1567impl<
1568 C: Config,
1569 P: Port,
1570 T: Trace,
1571 H,
1572 const TASKS: usize,
1573 const ITEMS: usize,
1574 const LISTS: usize,
1575 const QUEUES: usize,
1576 const SLOTS: usize,
1577 const BUFFERS: usize,
1578 const BYTES: usize,
1579 const TIMERS: usize,
1580 const GROUPS: usize,
1581> crate::typed::Raw
1582 for Kernel<C, P, T, H, TASKS, ITEMS, LISTS, QUEUES, SLOTS, BUFFERS, BYTES, TIMERS, GROUPS>
1583where
1584 H: TickHook<Self>,
1585{
1586 fn raw_queue_create(&mut self, length: usize) -> Result<QueueHandle> {
1587 self.queue_create(length)
1588 }
1589
1590 fn raw_queue_send(&mut self, queue: QueueHandle, value: u64, ticks: u64) -> Result<Wait<()>> {
1591 self.queue_send(queue, value, ticks)
1592 }
1593
1594 fn raw_queue_receive(&mut self, queue: QueueHandle, ticks: u64) -> Result<Wait<u64>> {
1595 self.queue_receive(queue, ticks)
1596 }
1597
1598 fn raw_queue_messages_waiting(&mut self, queue: QueueHandle) -> Result<usize> {
1599 self.queue_messages_waiting(queue)
1600 }
1601
1602 fn raw_queue_has_room(&self, queue: QueueHandle) -> bool {
1603 self.queues
1604 .resolve(queue)
1605 .is_ok_and(|q| q.waiting < q.length)
1606 }
1607
1608 fn raw_in_isr(&self) -> bool {
1609 self.port.in_isr()
1610 }
1611
1612 fn raw_queue_send_from_isr(&mut self, queue: QueueHandle, value: u64) -> Result<Woken> {
1613 self.queue_send_from_isr(queue, value)
1614 }
1615
1616 fn raw_mutex_create(&mut self) -> Result<QueueHandle> {
1617 self.mutex_create()
1618 }
1619
1620 fn raw_mutex_take(&mut self, mutex: QueueHandle, ticks: u64) -> Result<Wait<()>> {
1621 self.semaphore_take(mutex, ticks)
1622 }
1623
1624 fn raw_mutex_give(&mut self, mutex: QueueHandle) -> Result<()> {
1625 self.semaphore_give(mutex).map(|_| ())
1626 }
1627}