nros_node/executor/

node_wake.rs

//! Phase 130.3 / 130.5 — `NodeWake`: heap-backed wake primitive
//! used by `Executor::spin_once` on builds where the
//! `nros_platform_wake_*` ABI is wired (Phase 130.1 + 130.5).
//!
//! Originally added for Zephyr+std because Zephyr's libc
//! `pthread_cond_timedwait` hangs past its deadline (Phase
//! 127.C.4). Now also active on FreeRTOS, NuttX, and ThreadX
//! when those platforms are linked — each owns a kernel-native
//! binary semaphore (`k_sem` / `xSemaphoreBinary` / `sem_t` /
//! `tx_semaphore`) that honors its timeout. POSIX-only hosts
//! still use the existing `std::sync::Condvar` path.
//!
//! Construction is fallible — if the platform provider hasn't
//! linked a wake primitive (`wake_storage_size() == 0`) or
//! `wake_init` returns non-zero, the caller falls back to
//! driving the transport for the full timeout (matches the
//! Phase 127.C.4 expedient gate behaviour but without skipping
//! reliable RTOS stream retransmission).
//!
//! Phase 141.A.2 — cfg relaxed from `std` to `alloc` so the same
//! `NodeWake` type works for the embedded no_std FreeRTOS wake-cb
//! path (target for the Cortex-M3 P99 acceptance). The
//! `Box`/`Vec`/`Arc` types switch to the `alloc` crate; the
//! kernel-side semantics are unchanged.

// Phase 248 (C2) — platform-agnostic: no `platform-*` feature gate.
// `NodeWake` calls the `nros_platform_wake_*` C ABI (the platform
// vtable) generically; availability is decided at runtime by the
// `wake_storage_size() == 0` probe in [`NodeWake::new`], not by a
// compile-time per-RTOS cfg. Compiled for any `alloc + rmw-cffi` build;
// platforms without a wake primitive simply report size 0 and the
// caller falls back to driving the transport for the full timeout.
#![cfg(all(feature = "alloc", feature = "rmw-cffi"))]
// Phase 141.A.2 — `NodeWake` is callable from the std-gated
// `install_wake_signal_on_*` path today; the matching no_std
// caller for the FreeRTOS-embedded wake-cb path is the
// follow-on 141.A.3 work. Until that lands, the no_std build of
// this module has no consumer — `#[allow(dead_code)]` keeps the
// type compilable so 141.A.3 only has to add the caller, not
// re-introduce the type.
#![cfg_attr(not(feature = "alloc"), allow(dead_code))]

use core::ffi::c_void;

unsafe extern "C" {
    fn nros_platform_wake_init(w: *mut c_void) -> i8;
    fn nros_platform_wake_drop(w: *mut c_void) -> i8;
    fn nros_platform_wake_wait_ms(w: *mut c_void, timeout_ms: u32) -> i8;
    fn nros_platform_wake_signal(w: *mut c_void) -> i8;
    #[allow(dead_code)] // Wired by 124.B.7 ISR callers in a follow-up.
    fn nros_platform_wake_signal_from_isr(w: *mut c_void) -> i8;
    fn nros_platform_wake_park_handles_us(
        w: *mut c_void,
        handles: *const i32,
        n: usize,
        deadline_us: u64,
    ) -> i8;
    fn nros_platform_wake_storage_size() -> usize;
    fn nros_platform_wake_storage_align() -> usize;
}

/// Heap-backed wake primitive. Sized at runtime from the platform
/// probe (`nros_platform_wake_storage_size`).
pub(crate) struct NodeWake {
    storage: alloc::boxed::Box<[u8]>,
    /// Issue 2225 (RFC-0113 D9) -- a framework scheduler's wake, called after
    /// every [`signal`](Self::signal). The other half of the yielding park
    /// slot: a PX4 tier is a work item that never waits on this object, so an
    /// arrival that only signals it would be dispatched at the tier's next
    /// deadline. `hook_fn` is the `WakeHookFn` as a `usize`, 0 when unset;
    /// `hook_ctx` is stored before it and cleared after it.
    hook_fn: portable_atomic::AtomicUsize,
    hook_ctx: portable_atomic::AtomicPtr<c_void>,
}

/// Issue 2225 -- the hook [`NodeWake::set_hook`] installs. Called from the
/// signalling context (a transport thread, a publisher's context, an ISR
/// forward), so it must be thread-safe, non-blocking and O(1).
pub(crate) type WakeHookFn = unsafe extern "C" fn(ctx: *mut c_void);

// SAFETY: per `<nros/platform.h>`'s wake contract, signal/wait are
// callable from any thread (k_sem on Zephyr is kernel-managed).
unsafe impl Send for NodeWake {}
unsafe impl Sync for NodeWake {}

impl NodeWake {
    /// Allocate + init. Returns `None` if the platform provider
    /// reports the primitive unavailable (`nros_platform_wake_storage_size() == 0`)
    /// or if `wake_init` returns non-zero.
    pub(crate) fn new() -> Option<Self> {
        // SAFETY: probe functions are documented pure (no global
        // state, may be called before init).
        let size = unsafe { nros_platform_wake_storage_size() };
        let align = unsafe { nros_platform_wake_storage_align() };
        if size == 0 {
            return None;
        }
        // Round capacity up so the boxed slice's data ptr is
        // aligned to the requested boundary. `Box::<[u8]>` only
        // guarantees byte alignment; layout-allocate via Vec
        // capacity + a manual alignment check, or fall back to
        // `vec![0; size + align]` and offset. Simplest: use a
        // `Vec<u64>` for >=8B alignment (covers every platform we
        // ship today; `k_sem` and `sem_t` are <= 8B-aligned).
        if align > core::mem::align_of::<u64>() {
            return None;
        }
        let u64s = size.div_ceil(8);
        let boxed: alloc::boxed::Box<[u64]> = alloc::vec![0u64; u64s].into_boxed_slice();
        // Reinterpret the boxed [u64] as a boxed [u8]. The
        // capacity in bytes is `u64s * 8 >= size`; the data
        // pointer inherits 8-byte alignment from `Vec<u64>`'s
        // allocator request.
        // Take the data pointer as a THIN `*mut u8` (not `as *mut [u8]`, which
        // would keep the `u64s` element count for a u8 slice — wrong length, and
        // trips `clippy::cast_slice_different_sizes`). The fat slice is rebuilt
        // below with the correct byte length.
        let raw = alloc::boxed::Box::into_raw(boxed) as *mut u8;
        // SAFETY: `raw` came from `Box::<[u64]>::into_raw`; we
        // re-box as `Box<[u8]>` with the same total byte length.
        // The allocator only cares about the total size + the
        // pointer originally returned, both preserved.
        let storage: alloc::boxed::Box<[u8]> = unsafe {
            alloc::boxed::Box::from_raw(core::ptr::slice_from_raw_parts_mut(raw, u64s * 8))
        };
        let ptr = storage.as_ptr() as *mut c_void;
        // SAFETY: `ptr` references at least `size` bytes aligned
        // to 8 (verified above). `wake_init` initialises in place
        // or returns non-zero — in which case we drop `storage`
        // without calling `wake_drop`, matching the "init failed,
        // no teardown" contract.
        let rc = unsafe { nros_platform_wake_init(ptr) };
        if rc != 0 {
            return None;
        }
        Some(Self {
            storage,
            hook_fn: portable_atomic::AtomicUsize::new(0),
            hook_ctx: portable_atomic::AtomicPtr::new(core::ptr::null_mut()),
        })
    }

    /// Block until signaled or `timeout_ms` elapses. Returns
    /// `true` on signal, `false` on timeout or error.
    /// The raw primitive a PORT must wait on if it installs its own park.
    ///
    /// phase-436 W7 — a park primitive that waits on its own fresh object
    /// cannot be broken by the backend's listener, because the listener
    /// signals THIS one. Installing such a park makes latency worse, not
    /// better: `spin_once` drops the transport drain to non-blocking once a
    /// port has parked, so an unbreakable park sleeps out its whole deadline
    /// with data already waiting.
    ///
    /// A port's `ParkUntilFn` therefore takes this pointer as its `ctx`.
    pub(crate) fn raw_ptr(&self) -> *mut c_void {
        self.storage.as_ptr() as *mut c_void
    }

    pub(crate) fn wait_ms(&self, timeout_ms: u32) -> bool {
        let ptr = self.storage.as_ptr() as *mut c_void;
        // SAFETY: ptr was init'd in `new`; the underlying
        // primitive supports concurrent calls per platform spec.
        unsafe { nros_platform_wake_wait_ms(ptr, timeout_ms) == 0 }
    }

    /// phase-521 W3 (RFC-0113 D3) — park on this object AND on the OS
    /// `handles` (the sessions' `ReadinessSource::Waitable` descriptors) for
    /// at most `deadline_us`.
    ///
    /// `Some(true)` when signalled or a handle became readable, `Some(false)`
    /// when the deadline expired, `None` when the port cannot wait on handles —
    /// the caller then keeps the wait it had.
    pub(crate) fn park_handles_us(&self, handles: &[i32], deadline_us: u64) -> Option<bool> {
        let ptr = self.storage.as_ptr() as *mut c_void;
        // SAFETY: ptr was init'd in `new`; `handles` is a live slice for the
        // call, and the port reads it only during the call.
        match unsafe {
            nros_platform_wake_park_handles_us(ptr, handles.as_ptr(), handles.len(), deadline_us)
        } {
            0 => Some(true),
            1 => Some(false),
            _ => None,
        }
    }

    /// Call the hook if one is installed, then wake one waiter. Safe from
    /// any thread.
    ///
    /// The hook goes FIRST: a framework that installs one (a PX4 work item)
    /// has nothing waiting on this object, so its reschedule must not queue
    /// behind the platform signal's syscall.
    pub(crate) fn signal(&self) {
        let f = self.hook_fn.load(portable_atomic::Ordering::Acquire);
        if f != 0 {
            let ctx = self.hook_ctx.load(portable_atomic::Ordering::Acquire);
            // SAFETY: `f` was stored from a `WakeHookFn` by `set_hook`, and
            // its caller keeps `ctx` alive until it clears the hook.
            let hook: WakeHookFn = unsafe { core::mem::transmute::<usize, WakeHookFn>(f) };
            unsafe { hook(ctx) };
        }
        let ptr = self.storage.as_ptr() as *mut c_void;
        // SAFETY: see wait_ms.
        let _ = unsafe { nros_platform_wake_signal(ptr) };
    }

    /// Issue 2225 -- install (`Some`) or clear (`None`) the hook
    /// [`signal`](Self::signal) calls. `ctx` must outlive the hook: clear it
    /// before `ctx` goes away.
    pub(crate) fn set_hook(&self, hook: Option<WakeHookFn>, ctx: *mut c_void) {
        match hook {
            Some(f) => {
                self.hook_ctx.store(ctx, portable_atomic::Ordering::Release);
                self.hook_fn
                    .store(f as usize, portable_atomic::Ordering::Release);
            }
            None => {
                self.hook_fn.store(0, portable_atomic::Ordering::Release);
                self.hook_ctx
                    .store(core::ptr::null_mut(), portable_atomic::Ordering::Release);
            }
        }
    }
}

impl Drop for NodeWake {
    fn drop(&mut self) {
        let ptr = self.storage.as_ptr() as *mut c_void;
        // SAFETY: ptr was init'd in `new`; this is the matching
        // teardown call. NodeWake is owned exclusively by the
        // Executor (held inside Arc), so no in-flight wait can
        // overlap Drop.
        let _ = unsafe { nros_platform_wake_drop(ptr) };
    }
}

/// phase-509 P9b item 4 (RFC-0108 "As built in P9b") -- a band worker's wake
/// object over the platform's (`nros_platform_wake_*`): what a level runner
/// gives each spawned level.
impl super::scheduler::BandWaiter {
    /// A waiter over a new platform wake object, leaked with it: one per
    /// level, for the image's life. `None` when the platform links no wake
    /// primitive; the level then polls.
    pub fn platform() -> Option<&'static Self> {
        unsafe extern "C" fn wake(ctx: *mut c_void) {
            // SAFETY: `ctx` is the leaked `NodeWake` below.
            unsafe { &*(ctx as *const NodeWake) }.signal();
        }
        unsafe extern "C" fn wait(ctx: *mut c_void, us: u64) -> i32 {
            // The wake primitive's own resolution is the millisecond; a
            // band's park rounds UP, so it never wakes before its timer.
            let ms = us.div_ceil(1000).min(u64::from(u32::MAX)) as u32;
            // SAFETY: as `wake`.
            if unsafe { &*(ctx as *const NodeWake) }.wait_ms(ms) {
                0
            } else {
                -1
            }
        }
        let w: &'static NodeWake =
            alloc::boxed::Box::leak(alloc::boxed::Box::new(NodeWake::new()?));
        let ctx = w as *const NodeWake as *mut c_void;
        // SAFETY: the wake object is leaked, so it outlives every executor;
        // its signal and wait are callable from any thread (the platform's
        // wake contract).
        let waiter = unsafe { Self::new(wake, wait, ctx) };
        Some(alloc::boxed::Box::leak(alloc::boxed::Box::new(waiter)))
    }
}