Skip to main content

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}