nros_node/executor/spin.rs
1//! Executor struct and core spin methods.
2
3use core::{marker::PhantomData, mem::MaybeUninit};
4
5use nros_core::{BorrowedMessage, RosMessage, RosService};
6use nros_rmw::{QosSettings, ServiceInfo, Session, TopicInfo, TransportError};
7
8use crate::{session, timer::TimerDuration};
9
10#[cfg(feature = "safety-e2e")]
11use super::arena::{
12 SubSafetyEntry, sub_safety_has_data, sub_safety_pre_sample, sub_safety_try_process,
13};
14#[cfg(feature = "rmw-cffi")]
15use super::types::ExecutorConfig;
16#[cfg(feature = "std")]
17use super::types::SpinOptions;
18use super::{
19 arena::{
20 BufferStrategy, CallbackMeta, EntryKind, GuardConditionEntry, ServiceClientCallbackEntry,
21 ServiceClientRawArenaEntry, ServiceClientSendHeader, SrvEntry, SrvRawEntry,
22 SubBufferedBorrowedEntry, SubBufferedEntry, SubBufferedRawCEntry, SubBufferedRawEntry,
23 SubBufferedRawInfoCEntry, SubBufferedRawInfoEntry, SubInfoEntry, SubInplaceEntry,
24 TimerEntry, TimerHeader, always_ready, buffered_region_size, drop_entry, guard_has_data,
25 guard_try_process, no_pre_sample, service_client_callback_try_process,
26 service_client_raw_try_process, srv_has_data, srv_raw_has_data, srv_raw_try_process,
27 srv_try_process, sub_buffered_borrowed_has_data, sub_buffered_borrowed_try_process,
28 sub_buffered_has_data, sub_buffered_raw_c_has_data, sub_buffered_raw_c_try_process,
29 sub_buffered_raw_has_data, sub_buffered_raw_info_c_has_data,
30 sub_buffered_raw_info_c_try_process, sub_buffered_raw_info_has_data,
31 sub_buffered_raw_info_try_process, sub_buffered_raw_try_process, sub_buffered_try_process,
32 sub_info_has_data, sub_info_pre_sample, sub_info_try_process, sub_inplace_has_data,
33 sub_inplace_try_process, timer_try_process,
34 },
35 node::NodeHandle,
36 spsc_ring::SpscRing,
37 triple_buffer::TripleBuffer,
38 types::{
39 ExecutorSemantics, GuardConditionHandle, HandleId, InvocationMode, NodeError,
40 RawResponseCallback, RawServiceCallback, RawSubscriptionCallback,
41 RawSubscriptionInfoCallback, ReadinessSnapshot, SpinOnceResult, SpinPeriodPollingResult,
42 Trigger,
43 },
44};
45
46// ============================================================================
47// Executor::open() factory method
48// ============================================================================
49
50/// phase-271 — leak a default-sized (`ExecutorSizing::DEFAULT`) `u64` backing,
51/// yielding the `'static` storage the `alloc` convenience constructors borrow.
52/// One-time, executor-lifetime allocation (the executor lives for the program);
53/// intentionally not freed. `alloc`-only — no_std-no-alloc entries supply their
54/// own `static`/stack backing via `from_session_in` / the `nros::main!` macro.
55#[cfg(feature = "alloc")]
56fn leak_default_backing(sizing: super::storage::ExecutorSizing) -> &'static mut [MaybeUninit<u64>] {
57 alloc::boxed::Box::leak(alloc::boxed::Box::new_uninit_slice(sizing.u64_len()))
58}
59
60#[cfg(feature = "rmw-cffi")]
61impl<'s> Executor<'s> {
62 /// phase-271 — open a new executor session over caller-supplied `backing`,
63 /// sized by `sizing` (per-entry sizing). The core, non-generic sized entry
64 /// point: the `alloc` [`open`](Self::open) convenience leaks a default
65 /// backing and delegates here, and the `nros::main!` macro emits a backing
66 /// sized to the entry's own entity count.
67 ///
68 /// Phase 115.M.4 — auto-registers the cffi vtable for whichever
69 /// backend the build was configured for, mirroring the C++ side's
70 /// `#ifdef NROS_RMW_<NAME>` fan-out in `<nros/node.hpp>`. The
71 /// runtime's atomic vtable slot is idempotent: a re-call of any
72 /// backend's `register()` is a no-op, so the fan-out below is safe
73 /// to invoke on every `Executor::open` (cheaper than a `Once` and
74 /// doesn't pull in `std::sync` for no_std targets).
75 ///
76 /// Connects to the middleware at the locator specified in `config`.
77 ///
78 /// # Safety
79 /// `backing` must be ≥ `sizing.u64_len()` words, live for `'s`, and be
80 /// otherwise untouched while the executor lives (see
81 /// [`from_session_in`](Self::from_session_in)).
82 pub unsafe fn open_in(
83 config: &ExecutorConfig<'_>,
84 backing: &'s mut [MaybeUninit<u64>],
85 sizing: super::storage::ExecutorSizing,
86 ) -> Result<Self, NodeError> {
87 use nros_rmw::Rmw;
88
89 // Phase 128.A.3 / 249 P4b.1 — manifest-driven backend selection.
90 //
91 // Every linked backend self-registered via its `.init_array`
92 // ctor before `main` (RFC-0042 §D3.3), so the registry is
93 // already populated — no runtime section walk.
94 //
95 // 1. Consult `$NROS_RMW` (when std/env is available) for
96 // explicit override, mirroring ROS 2's `RMW_IMPLEMENTATION`.
97 // 2. With no selector, pick the unique registered backend.
98 // Zero registered → `NoBackend`; more than one →
99 // `Ambiguous` (user must set `$NROS_RMW` or use
100 // `Executor::open_multi`).
101 let selector = read_rmw_selector_env();
102 // `as_deref()` on `Option<Vec<u8>>` yields `Option<&[u8]>`;
103 // on the no_std `Option<&'static [u8]>` variant it's a
104 // no-op the lint catches but the std signature still
105 // requires the call. Allowed locally.
106 #[allow(clippy::needless_option_as_deref)]
107 let sel_ref = selector.as_deref();
108 match nros_rmw_cffi::resolve_backend(sel_ref) {
109 nros_rmw_cffi::BackendResolution::Single(_) => {}
110 // Map every non-`Single` outcome to a transport
111 // ConnectionFailed for now; the more granular ret codes
112 // (NO_BACKEND / AMBIGUOUS / UNKNOWN) are exposed to C
113 // callers via `nros_init`'s return value (Phase 128.C.2).
114 _ => return Err(NodeError::Transport(TransportError::ConnectionFailed)),
115 }
116
117 let rmw_config = nros_rmw::RmwConfig {
118 locator: config.locator,
119 mode: config.mode,
120 domain_id: config.domain_id,
121 node_name: config.node_name,
122 namespace: config.namespace,
123 properties: &[],
124 };
125 let session = if let Some(name) = sel_ref {
126 // Selector path: route to the specific named backend so
127 // the env-var-disambiguated outcome matches what the
128 // resolver above identified.
129 nros_rmw_cffi::CffiRmw::open_with_rmw(
130 core::str::from_utf8(name).unwrap_or(""),
131 &rmw_config,
132 )
133 } else {
134 nros_rmw_cffi::CffiRmw.open(&rmw_config)
135 }
136 .map_err(|_| NodeError::Transport(TransportError::ConnectionFailed))?;
137 // SAFETY: forwarded from this fn's contract — `backing`/`sizing` sized
138 // + alive for `'s`.
139 let mut executor = unsafe { Self::from_session_in(session, backing, sizing) };
140 #[cfg(not(feature = "std"))]
141 {
142 executor.clock_us_fn = config.clock_us;
143 executor.epoch_us_fn = config.epoch_us;
144 executor.last_spin_end_us = config.clock_us.map(|clock| clock());
145 }
146 executor.set_node_identity(config.node_name, config.namespace);
147 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
148 executor.install_wake_signal_on_primary();
149 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
150 executor.install_wake_signal_on_primary_alloc();
151 // Phase 277 W2.c — readiness marker for E2E harnesses. This is the
152 // single call-through `open()`/`open_sized()` share, so it fires on
153 // every platform that reaches here (native, freertos, zephyr,
154 // threadx, …) regardless of which RMW backend or board owns the
155 // boot path. It replaces the per-example synthetic
156 // `log::info!("Publishing messages")` markers W4 removes — those
157 // only proved a callback had fired at least once; this line proves
158 // the session itself is up, before any node/callback exists.
159 //
160 // STABILITY CONTRACT: the leading `"nros: session open"` text is
161 // load-bearing — test harnesses grep for it verbatim. Keep it
162 // stable even if the trailing `(rmw=...)` detail changes.
163 #[cfg(feature = "log")]
164 {
165 if let Some(name) = sel_ref {
166 log::info!(
167 "nros: session open (rmw={})",
168 core::str::from_utf8(name).unwrap_or("?")
169 );
170 } else {
171 log::info!("nros: session open");
172 }
173 }
174 Ok(executor)
175 }
176}
177
178#[cfg(all(feature = "rmw-cffi", feature = "alloc"))]
179impl Executor<'static> {
180 /// Open a new executor session using the active RMW backend, at the
181 /// build-time default sizing. Convenience over
182 /// [`open_in`](Self::open_in): leaks a default-sized backing (executor-
183 /// lifetime) so existing callers keep the zero-storage-arg signature.
184 /// Per-entry sizing goes through `open_in` / the `nros::main!` macro.
185 ///
186 /// # Example
187 ///
188 /// ```ignore
189 /// let config = ExecutorConfig::from_env().node_name("my_node");
190 /// let mut executor = Executor::open(&config)?;
191 /// ```
192 pub fn open(config: &ExecutorConfig<'_>) -> Result<Self, NodeError> {
193 Self::open_sized(config, super::storage::ExecutorSizing::DEFAULT)
194 }
195
196 /// phase-271 — like [`open`](Self::open) but sized to a caller-supplied
197 /// `sizing` (its own declared topology) instead of the build-time default.
198 /// The `alloc` entry point the `nros::main!` macro's native board path uses
199 /// to size a fat entry (>default `MAX_CBS` callbacks) without a
200 /// workspace-global `NROS_EXECUTOR_MAX_CBS`. Leaks a `sizing`-sized backing
201 /// (executor-lifetime); no-alloc entries use `open_in` with their own static.
202 pub fn open_sized(
203 config: &ExecutorConfig<'_>,
204 sizing: super::storage::ExecutorSizing,
205 ) -> Result<Self, NodeError> {
206 // SAFETY: leaked backing is exactly `sizing.u64_len()` words, `'static`,
207 // uniquely owned by the returned executor.
208 unsafe { Self::open_in(config, leak_default_backing(sizing), sizing) }
209 }
210
211 /// Phase 128.F.1 — explicit per-backend session declaration for
212 /// bridge mode. `specs[0]` becomes the primary session; `specs[1..]`
213 /// open as extras keyed by RMW name. After construction, every
214 /// `create_node_on(name, rmw)` call dispatches to whichever
215 /// session was opened under that RMW name (or, when the rmw name
216 /// matches the primary, the primary session itself).
217 ///
218 /// Single-backend callers should keep using
219 /// [`open`](Self::open) — this entry costs an extra
220 /// `open_with_rmw` per spec and adds no value when only one
221 /// backend is linked.
222 ///
223 /// `$NROS_RMW` env is ignored: bridge mode wants explicit names.
224 ///
225 /// Default-sized `alloc` convenience over
226 /// [`open_multi_in`](Self::open_multi_in) (leaks a default backing).
227 #[cfg(feature = "rmw-cffi")]
228 pub fn open_multi(specs: &[SessionSpec<'_>]) -> Result<Self, NodeError> {
229 let sizing = super::storage::ExecutorSizing::DEFAULT;
230 // SAFETY: leaked backing is exactly `sizing.u64_len()` words, `'static`.
231 unsafe { Self::open_multi_in(specs, leak_default_backing(sizing), sizing) }
232 }
233
234 /// Phase 104.C.1 — open the Executor against a specific RMW
235 /// backend by name. Selects from the named registry (Phase
236 /// 104.B.2). `rmw_name` must match one of the names a backend
237 /// registered under (`"zenoh"`, `"cyclonedds"`, `"xrce"`, …).
238 ///
239 /// Equivalent to [`Executor::open`] when the registry has exactly
240 /// one backend (the default-backend fast path). Use this entry
241 /// point in multi-backend builds where `Executor::open` would
242 /// pick the first-registered slot.
243 ///
244 /// Single-Executor multi-Node multi-RMW (the long-term Design X
245 /// from `docs/roadmap/phase-104-multi-backend-bridges.md`) is
246 /// follow-up work — Phase 104.C.2 + C.3.
247 ///
248 /// Default-sized `alloc` convenience over
249 /// [`open_with_rmw_in`](Self::open_with_rmw_in) (leaks a default backing).
250 #[cfg(feature = "rmw-cffi")]
251 pub fn open_with_rmw(rmw_name: &str, config: &ExecutorConfig<'_>) -> Result<Self, NodeError> {
252 let sizing = super::storage::ExecutorSizing::DEFAULT;
253 // SAFETY: leaked backing is exactly `sizing.u64_len()` words, `'static`.
254 unsafe { Self::open_with_rmw_in(rmw_name, config, leak_default_backing(sizing), sizing) }
255 }
256}
257
258// phase-271 — no-alloc sized cores for the bridge/named open paths. In
259// `impl<'s>` (not the `'static` alloc block) so they stay available in
260// `rmw-cffi`-without-`alloc` builds (e.g. the `nros-bridge` no_std default),
261// which is where `open_multi`/`open_with_rmw` lived before.
262#[cfg(feature = "rmw-cffi")]
263impl<'s> Executor<'s> {
264 /// Per-entry-sized [`open_multi`](Self::open_multi): carves `backing` for
265 /// the executor's tables instead of leaking a default one.
266 ///
267 /// # Safety
268 /// `backing`/`sizing` as in [`from_session_in`](Self::from_session_in).
269 pub unsafe fn open_multi_in(
270 specs: &[SessionSpec<'_>],
271 backing: &'s mut [MaybeUninit<u64>],
272 sizing: super::storage::ExecutorSizing,
273 ) -> Result<Self, NodeError> {
274 // Phase 249 P4b.1 — backends self-registered via their
275 // `.init_array` ctor before `main`; no runtime section walk.
276 let primary = specs
277 .first()
278 .ok_or(NodeError::Transport(TransportError::ConnectionFailed))?;
279 let primary_session =
280 nros_rmw_cffi::CffiRmw::open_with_rmw(primary.rmw, &primary.to_rmw_config())
281 .map_err(NodeError::Transport)?;
282 // SAFETY: forwarded from this fn's contract.
283 let mut executor = unsafe { Self::from_session_in(primary_session, backing, sizing) };
284 executor.set_node_identity("", "/");
285 // Phase 156 — see `Executor::open` for primary-identity
286 // recording rationale.
287 let _ = executor.primary_rmw_name.push_str(primary.rmw);
288 let _ = executor.primary_locator.push_str(primary.locator);
289 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
290 executor.install_wake_signal_on_primary();
291 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
292 executor.install_wake_signal_on_primary_alloc();
293
294 for spec in specs.iter().skip(1) {
295 let session = nros_rmw_cffi::CffiRmw::open_with_rmw(spec.rmw, &spec.to_rmw_config())
296 .map_err(NodeError::Transport)?;
297 executor
298 .extra_sessions
299 .push(session)
300 .map_err(|_| NodeError::NodeTableFull)?;
301 #[cfg(feature = "std")]
302 {
303 let idx = executor.extra_sessions.len() - 1;
304 executor.install_wake_signal_on_extra(idx);
305 }
306 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
307 {
308 let idx = executor.extra_sessions.len() - 1;
309 executor.install_wake_signal_on_extra_alloc(idx);
310 }
311 }
312
313 Ok(executor)
314 }
315
316 /// Per-entry-sized [`open_with_rmw`](Self::open_with_rmw): carves `backing`
317 /// for the executor's tables instead of leaking a default one.
318 ///
319 /// # Safety
320 /// `backing`/`sizing` as in [`from_session_in`](Self::from_session_in).
321 pub unsafe fn open_with_rmw_in(
322 rmw_name: &str,
323 config: &ExecutorConfig<'_>,
324 backing: &'s mut [MaybeUninit<u64>],
325 sizing: super::storage::ExecutorSizing,
326 ) -> Result<Self, NodeError> {
327 if !nros_rmw_cffi::backend_registered() {
328 return Err(NodeError::Transport(TransportError::ConnectionFailed));
329 }
330
331 let rmw_config = nros_rmw::RmwConfig {
332 locator: config.locator,
333 mode: config.mode,
334 domain_id: config.domain_id,
335 node_name: config.node_name,
336 namespace: config.namespace,
337 properties: &[],
338 };
339 let session = nros_rmw_cffi::CffiRmw::open_with_rmw(rmw_name, &rmw_config)
340 .map_err(|_| NodeError::Transport(TransportError::ConnectionFailed))?;
341 // SAFETY: forwarded from this fn's contract.
342 let mut executor = unsafe { Self::from_session_in(session, backing, sizing) };
343 #[cfg(not(feature = "std"))]
344 {
345 executor.clock_us_fn = config.clock_us;
346 executor.epoch_us_fn = config.epoch_us;
347 executor.last_spin_end_us = config.clock_us.map(|clock| clock());
348 }
349 executor.set_node_identity(config.node_name, config.namespace);
350 // Phase 156 — record primary identity for the session-
351 // cache hit path. See `Executor::open` for the rationale.
352 let _ = executor.primary_rmw_name.push_str(rmw_name);
353 let _ = executor.primary_locator.push_str(config.locator);
354 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
355 executor.install_wake_signal_on_primary();
356 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
357 executor.install_wake_signal_on_primary_alloc();
358 Ok(executor)
359 }
360}
361
362/// Phase 128.F.1 — per-backend session declaration for
363/// [`Executor::open_multi`]. Each spec names an RMW backend (must
364/// match one a backend registered under via
365/// `nros_rmw_cffi_register_named` / the `RMW_INIT_ENTRIES` linker
366/// section) and the locator + domain id to open against it.
367#[cfg(feature = "rmw-cffi")]
368#[derive(Clone, Copy)]
369pub struct SessionSpec<'cfg> {
370 pub rmw: &'cfg str,
371 pub locator: &'cfg str,
372 pub domain_id: u32,
373 pub node_name: &'cfg str,
374 pub namespace: &'cfg str,
375}
376
377#[cfg(feature = "rmw-cffi")]
378impl<'cfg> SessionSpec<'cfg> {
379 /// Minimal spec — just RMW name + locator. Domain id defaults to
380 /// 0; node name and namespace are empty.
381 pub const fn new(rmw: &'cfg str, locator: &'cfg str) -> Self {
382 Self {
383 rmw,
384 locator,
385 domain_id: 0,
386 node_name: "",
387 namespace: "",
388 }
389 }
390
391 pub const fn domain_id(mut self, domain_id: u32) -> Self {
392 self.domain_id = domain_id;
393 self
394 }
395
396 pub const fn node_name(mut self, name: &'cfg str) -> Self {
397 self.node_name = name;
398 self
399 }
400
401 pub const fn namespace(mut self, ns: &'cfg str) -> Self {
402 self.namespace = ns;
403 self
404 }
405
406 fn to_rmw_config(self) -> nros_rmw::RmwConfig<'cfg> {
407 nros_rmw::RmwConfig {
408 locator: self.locator,
409 mode: nros_rmw::SessionMode::Client,
410 domain_id: self.domain_id,
411 node_name: self.node_name,
412 namespace: self.namespace,
413 properties: &[],
414 }
415 }
416}
417
418// Phase 128.A.3 — selector for the single-backend resolution path.
419//
420// On hosted (`std`) builds, read `$NROS_RMW`; mirrors ROS 2's
421// `RMW_IMPLEMENTATION`. Returns the name as a byte vector so the
422// caller can pass it to `nros_rmw_cffi::resolve_backend` and (when
423// `Some`) to `CffiRmw::open_with_rmw`.
424//
425// On `no_std` / bare-metal builds, environment variables are not
426// available; resolution always falls through to the single-backend
427// or ambiguous path. Embedded users with multiple backends use the
428// bridge surface `Executor::open_multi` instead.
429#[cfg(all(feature = "std", feature = "rmw-cffi"))]
430fn read_rmw_selector_env() -> Option<alloc::vec::Vec<u8>> {
431 let raw = std::env::var_os("NROS_RMW")?;
432 let bytes = raw.as_encoded_bytes();
433 if bytes.is_empty() {
434 return None;
435 }
436 Some(bytes.to_vec())
437}
438
439#[cfg(all(not(feature = "std"), feature = "rmw-cffi"))]
440fn read_rmw_selector_env() -> Option<&'static [u8]> {
441 None
442}
443
444// ============================================================================
445// SessionStore — owned or borrowed session
446// ============================================================================
447
448/// Session storage: owned or borrowed via raw pointer.
449///
450/// The C API creates a session in `nros_support_init()` before the
451/// executor. `Borrowed` lets the executor use that session without owning it.
452#[allow(clippy::large_enum_variant)]
453pub(crate) enum SessionStore {
454 Owned(session::ConcreteSession),
455 Borrowed(*mut session::ConcreteSession),
456}
457
458impl core::ops::Deref for SessionStore {
459 type Target = session::ConcreteSession;
460 fn deref(&self) -> &session::ConcreteSession {
461 match self {
462 SessionStore::Owned(s) => s,
463 SessionStore::Borrowed(ptr) => unsafe { &**ptr },
464 }
465 }
466}
467
468impl core::ops::DerefMut for SessionStore {
469 fn deref_mut(&mut self) -> &mut session::ConcreteSession {
470 match self {
471 SessionStore::Owned(s) => s,
472 SessionStore::Borrowed(ptr) => unsafe { &mut **ptr },
473 }
474 }
475}
476
477/// Phase 228.E — an opaque, `Send` handle to an [`Executor`]'s RMW session.
478///
479/// In the per-tier model the boot executor opens the one session and hands each
480/// spawned tier task a handle (not a borrow) so the task opens its own
481/// [`Executor`] over that *same* session across the RTOS task boundary. Wrapping
482/// the `pub(crate)` session pointer lets board crates (`nros-board-posix`,
483/// `nros-board-freertos`, …) name + move the handle without naming the session
484/// type. Obtain via [`Executor::session_handle`]; consume via
485/// [`Executor::open_with_session_handle`].
486#[cfg(any(has_rmw, test))]
487pub struct SessionHandle(*mut session::ConcreteSession);
488
489// SAFETY: the per-tier model deliberately shares one session across RTOS tasks;
490// concurrent access is serialized by the RMW backend's internal locks (the RTOS
491// targets build zenoh-pico `Z_FEATURE_MULTI_THREAD=1` — RFC-0032 §5.0). The
492// boot executor owns the session and outlives every tier task.
493#[cfg(any(has_rmw, test))]
494unsafe impl Send for SessionHandle {}
495
496#[cfg(any(has_rmw, test))]
497impl SessionHandle {
498 /// Phase 274.W1 — convert to an opaque `*mut c_void` for C/C++ FFI.
499 ///
500 /// The returned pointer encodes the session address and is valid as long as
501 /// the owning executor lives. Reconstruct via [`Self::from_raw`].
502 pub fn into_raw(self) -> *mut core::ffi::c_void {
503 self.0 as *mut core::ffi::c_void
504 }
505
506 /// Phase 274.W1 — reconstruct a `SessionHandle` from an opaque pointer
507 /// returned by [`Self::into_raw`].
508 ///
509 /// # Safety
510 /// `ptr` must be a pointer obtained from `into_raw()` on a `SessionHandle`
511 /// whose underlying session is still live and owned by its original executor.
512 pub unsafe fn from_raw(ptr: *mut core::ffi::c_void) -> Self {
513 Self(ptr as *mut session::ConcreteSession)
514 }
515}
516
517/// Phase 228.C — pure callback-group filter decision. `None` = wildcard (accept
518/// every group); `Some` = accept only listed groups. Backs
519/// [`Executor::group_active`]; split out so the logic is unit-testable without a
520/// live session.
521pub(crate) fn group_filter_accepts<const N: usize, const M: usize>(
522 active: &Option<heapless::Vec<heapless::String<N>, M>>,
523 group: &str,
524) -> bool {
525 match active {
526 None => true,
527 Some(v) => v.iter().any(|g| g.as_str() == group),
528 }
529}
530
531#[cfg(test)]
532mod group_filter_tests {
533 use super::group_filter_accepts;
534
535 type Groups = heapless::Vec<heapless::String<32>, { crate::config::MAX_NODES }>;
536
537 #[test]
538 fn wildcard_accepts_all() {
539 let none: Option<Groups> = None;
540 assert!(group_filter_accepts(&none, "anything"));
541 }
542
543 #[test]
544 fn set_accepts_only_listed_groups() {
545 let mut v: Groups = heapless::Vec::new();
546 let mut s = heapless::String::new();
547 s.push_str("ctrl").unwrap();
548 v.push(s).unwrap();
549 let active = Some(v);
550 assert!(group_filter_accepts(&active, "ctrl"));
551 assert!(!group_filter_accepts(&active, "telem"));
552 }
553}
554
555// ============================================================================
556// Executor
557// ============================================================================
558
559/// Backend-agnostic executor that owns a session.
560///
561/// Provides `create_node()` for entity creation and `drive_io()` for polling.
562///
563/// # Callback Mode
564///
565/// The executor supports arena-based callback registration via the
566/// `node_mut(id).subscription(t)` builder and
567/// [`register_service()`](Self::register_service), with dispatch via
568/// [`spin_once()`](Self::spin_once). No heap allocation is needed.
569///
570/// The sizes are set via `NROS_EXECUTOR_MAX_CBS` (default 4) and
571/// `NROS_EXECUTOR_ARENA_SIZE` (default 4096) environment variables at build time.
572///
573/// Phase 124.B.2 — opaque context handed to the runtime wake
574/// callback. Backends store the raw pointer + invoke the callback;
575/// the callback decodes back to `&WakeCtx`.
576#[cfg(all(feature = "std", feature = "rmw-cffi"))]
577pub(crate) struct WakeCtx {
578 pub(crate) flag: std::sync::Arc<std::sync::atomic::AtomicBool>,
579 pub(crate) cv: std::sync::Arc<std::sync::Condvar>,
580 #[allow(dead_code)] // Held by spin_once's wait predicate (124.B.4).
581 pub(crate) mu: std::sync::Arc<std::sync::Mutex<()>>,
582 /// Phase 130.3 — Zephyr+std uses the k_sem wake primitive; the
583 /// runtime cb signals both this and the std cv so a future
584 /// migration to a single primitive flips one branch instead
585 /// of two.
586 pub(crate) node_wake: Option<std::sync::Arc<super::node_wake::NodeWake>>,
587}
588
589/// Phase 124.B.2 — runtime wake callback.
590///
591/// RT-context contract:
592///
593/// * **Thread-safe**: callable from any thread. The cb is lock-free
594/// on the cv path — no mutex held during `notify_all`. Lost-wakeup
595/// is prevented by the waiter checking `wake_flag` under
596/// `wake_mu` via the `wait_timeout_while` predicate.
597/// * **NOT async-signal-safe on POSIX**: `pthread_cond_signal`
598/// isn't on the POSIX async-signal-safe function list. For POSIX
599/// signal handler wake, use a `signalfd` + select pattern in a
600/// thread that owns the wake duty.
601/// * **RTOS ISR**: per-RTOS platform layer wraps the cv with an
602/// ISR-safe primitive (`xSemaphoreGiveFromISR`,
603/// `tx_event_flags_set` from ISR, `k_sem_give` from ISR on
604/// Zephyr). Backend's ISR caller routes through the platform's
605/// `signal_from_isr` API instead of this cb directly.
606/// * **Bounded execution time**: O(1) — atomic store + cv notify.
607/// No allocation, no contended lock.
608///
609/// The cb is the symbol backends invoke from their async wake path
610/// (datagram arrival, worker-thread enqueue, etc.). It does
611/// flag-write + condvar-signal in that order, lock-free.
612#[cfg(all(feature = "std", feature = "rmw-cffi"))]
613pub(crate) unsafe extern "C" fn nros_rmw_runtime_wake_cb(ctx: *mut core::ffi::c_void) {
614 if ctx.is_null() {
615 return;
616 }
617 // Phase 141.B.2 — capture T0 at cb entry. No-op when the
618 // probe feature is off or no cycle reader is installed.
619 #[cfg(feature = "wake-latency-probe")]
620 super::wake_probe::on_wake();
621 // SAFETY: ctx points at a `WakeCtx` owned by an Executor still
622 // alive at the time of the call. Executor::drop must clear the
623 // callback via `set_wake_callback(None, _)` on all sessions
624 // before dropping wake_ctx; this happens in `install_wake_*`
625 // teardown path.
626 let wake = unsafe { &*(ctx as *const WakeCtx) };
627 wake.flag.store(true, std::sync::atomic::Ordering::SeqCst);
628 // Lock-free notify. The waiter observes wake_flag under wake_mu
629 // in its wait_timeout_while predicate — flag.store with SeqCst
630 // happens-before any subsequent acquire in the waiter, so the
631 // waiter cannot miss the signal even though we don't hold mu
632 // here. Standard pthread cond-var idiom.
633 wake.cv.notify_all();
634 // Phase 130.3 — Zephyr+std waits on `NodeWake` (k_sem) instead
635 // of the std cv. Signal both so the cb keeps working whichever
636 // wait primitive spin_once is using.
637 if let Some(nw) = wake.node_wake.as_ref() {
638 nw.signal();
639 }
640}
641
642/// Phase 124.B.7.c — POSIX signalfd worker.
643///
644/// Owns a Linux `eventfd` plus a worker thread that `read()`s the
645/// fd and forwards via `wake_ctx.cv.notify_all()`. The eventfd
646/// write side is async-signal-safe per the kernel contract
647/// (`write(2)` to an eventfd is permitted from signal handlers),
648/// closing the gap that `pthread_cond_signal` leaves open on POSIX.
649///
650/// Lifecycle:
651/// * Constructed lazily in `Executor::signal_fd()` on first
652/// caller request.
653/// * `Drop` writes a shutdown sentinel + joins the worker.
654///
655/// Caller flow (signal handler):
656/// 1. Get fd via `Executor::signal_fd()` before installing the
657/// handler.
658/// 2. Handler does `eventfd_write(fd, 1)` (equivalently,
659/// `write(fd, &1u64, 8)`).
660/// 3. Worker thread reads the fd, signals wake_cv. spin_once
661/// blocked in cv.wait_timeout_while sees flag=true and exits.
662#[cfg(all(feature = "signal-fd-wake", target_os = "linux"))]
663pub struct WakeSignalFd {
664 fd: core::ffi::c_int,
665 shutdown: std::sync::Arc<std::sync::atomic::AtomicBool>,
666 worker: Option<std::thread::JoinHandle<()>>,
667}
668
669#[cfg(all(feature = "signal-fd-wake", target_os = "linux"))]
670impl WakeSignalFd {
671 /// Spawn the worker. `wake_ctx_ptr` is the `*const WakeCtx`
672 /// produced by `Executor::wake_ctx_ptr` — same value the
673 /// runtime wake cb decodes.
674 fn new(wake_ctx_ptr: *const WakeCtx) -> Result<Self, std::io::Error> {
675 let fd = unsafe { libc::eventfd(0, libc::EFD_CLOEXEC) };
676 if fd < 0 {
677 return Err(std::io::Error::last_os_error());
678 }
679
680 let shutdown = std::sync::Arc::new(std::sync::atomic::AtomicBool::new(false));
681 let shutdown_clone = std::sync::Arc::clone(&shutdown);
682
683 // Pass wake_ctx pointer as usize so the closure is Send.
684 // SAFETY: the pointer is valid for the Executor's lifetime
685 // (WakeCtx is owned by the Executor's `wake_ctx: Arc<WakeCtx>`
686 // field which outlives this worker thread — we join in Drop).
687 let ctx_addr = wake_ctx_ptr as usize;
688 let worker = std::thread::Builder::new()
689 .name("nros-wakefd".into())
690 .spawn(move || {
691 let ctx = ctx_addr as *const WakeCtx;
692 loop {
693 let mut buf = [0u8; 8];
694 let n =
695 unsafe { libc::read(fd, buf.as_mut_ptr() as *mut core::ffi::c_void, 8) };
696 if n <= 0 {
697 // EINTR / EOF — re-check shutdown then loop.
698 if shutdown_clone.load(std::sync::atomic::Ordering::Acquire) {
699 return;
700 }
701 continue;
702 }
703 if shutdown_clone.load(std::sync::atomic::Ordering::Acquire) {
704 return;
705 }
706 // Same effect as nros_rmw_runtime_wake_cb. We
707 // can't call it directly because it dereferences
708 // ctx as &WakeCtx which would race with Executor
709 // drop unless we hold a guarantee — the
710 // shutdown_flag check above + Drop's join gives it.
711 unsafe {
712 let w = &*ctx;
713 w.flag.store(true, std::sync::atomic::Ordering::SeqCst);
714 w.cv.notify_all();
715 }
716 }
717 })
718 .map_err(|e| {
719 unsafe { libc::close(fd) };
720 std::io::Error::other(alloc::format!("spawn nros-wakefd worker: {e}"))
721 })?;
722
723 Ok(Self {
724 fd,
725 shutdown,
726 worker: Some(worker),
727 })
728 }
729
730 /// Returns the writable eventfd. The caller (typically a POSIX
731 /// signal handler) writes any non-zero 8-byte value to trigger
732 /// a wake. `write(2)` on an eventfd is async-signal-safe per
733 /// `eventfd(2)` man page.
734 pub fn fd(&self) -> core::ffi::c_int {
735 self.fd
736 }
737}
738
739#[cfg(all(feature = "signal-fd-wake", target_os = "linux"))]
740impl Drop for WakeSignalFd {
741 fn drop(&mut self) {
742 self.shutdown
743 .store(true, std::sync::atomic::Ordering::Release);
744 // Wake the worker so it re-checks shutdown.
745 let one: u64 = 1;
746 unsafe {
747 libc::write(self.fd, &one as *const u64 as *const core::ffi::c_void, 8);
748 }
749 if let Some(j) = self.worker.take() {
750 let _ = j.join();
751 }
752 unsafe { libc::close(self.fd) };
753 }
754}
755
756/// Phase 124.B.7.b — ISR / interrupt-context wake callback.
757///
758/// Same semantics as [`nros_rmw_runtime_wake_cb`] but constrained to
759/// async-signal-safe / ISR-safe primitives.
760///
761/// Per-platform routing:
762///
763/// * **POSIX (std)**: `pthread_cond_signal` is NOT on the POSIX
764/// async-signal-safe function list. Calling from a SIGUSR1
765/// handler is technically UB. Real fix (Phase 124.B.7.c) routes
766/// via `signalfd`/`eventfd` + a runtime worker thread; until that
767/// lands, signal-handler callers MUST use
768/// `nros_guard_condition_trigger` from a **separate thread** (not
769/// from the handler itself), OR set the wake_flag and rely on
770/// the next poll deadline. This cb currently aliases the regular
771/// `wake_cb` and is safe only from non-signal-handler ISR-like
772/// contexts (e.g. timer thread, kernel callback).
773///
774/// * **RTOS no_std (Zephyr/FreeRTOS/ThreadX)**: routes through the
775/// platform-cffi `condvar_signal_from_isr` slot. Each backend
776/// uses its ISR-safe variant — `xSemaphoreGiveFromISR`,
777/// `tx_semaphore_put`, `k_condvar_signal`.
778///
779/// `ctx` semantics identical to [`nros_rmw_runtime_wake_cb`].
780#[cfg(all(feature = "std", feature = "rmw-cffi"))]
781#[allow(dead_code)] // Public exposure pending B.7.c signalfd worker.
782pub(crate) unsafe extern "C" fn nros_rmw_runtime_wake_cb_from_isr(ctx: *mut core::ffi::c_void) {
783 // Today: alias regular wake_cb. POSIX signal-handler safety
784 // pending B.7.c (signalfd worker-thread forward). Documented in
785 // the contract above so callers know the boundary.
786 unsafe { nros_rmw_runtime_wake_cb(ctx) };
787}
788
789/// Phase 216 follow-up — per-Node dispatch trampoline registered with
790/// [`Executor::register_dispatch_slot`].
791///
792/// The board-side dispatch task (RTIC `__nros_run` / Embassy
793/// `__nros_run_task`) dequeues a `nros_platform::SignaledCallback`
794/// envelope and forwards `(cb_id, ctx_ptr)` into
795/// [`Executor::dispatch_callback`]; that method linear-scans this
796/// slot table and invokes every registered `on_callback` with the
797/// owning Node's per-Node `state` blob. Each Node's
798/// `__nros_node_<pkg>_on_callback` self-filters on its own
799/// `CallbackId` tag set, so a slot whose Node doesn't own this
800/// callback is a cheap no-op string compare.
801///
802/// The shape mirrors the per-pkg `__nros_node_<pkg>_on_callback`
803/// extern "C" trampoline emitted by the `nros::node!()` macro
804/// (see `packages/core/nros-macros/src/lib.rs` Phase 216.A.5).
805///
806/// # Why not `linkme`
807///
808/// `linkme::distributed_slice` hangs on bare-metal Cortex-M /
809/// RISC-V because `cortex_m_rt`'s link script doesn't provide the
810/// `__start_/__stop_` section anchors in a shape that lets the
811/// iterator terminate (see
812/// `packages/core/nros-rmw-cffi/src/section.rs` Phase 142). Since
813/// stm32f4 RTIC / Embassy boards are the whole point of Phase 216,
814/// the registry uses the explicit `register()` pattern from Phase
815/// 104.A.
816#[derive(Clone, Copy)]
817pub struct DispatchSlot {
818 /// Owning Node's `State` blob — produced by the macro-emitted
819 /// `i()` and round-tripped through
820 /// `nros::__private_node_state_into_raw`. Opaque to the
821 /// executor.
822 pub state: *mut core::ffi::c_void,
823 /// Per-Node `extern "C"` trampoline; signature matches the
824 /// `__nros_node_<pkg>_on_callback` symbol the `nros::node!()`
825 /// macro emits.
826 pub on_callback: unsafe extern "C" fn(
827 state: *mut core::ffi::c_void,
828 cb_id_ptr: *const u8,
829 cb_id_len: usize,
830 ctx: *mut core::ffi::c_void,
831 ),
832}
833
834// SAFETY: `DispatchSlot` carries two raw pointers (`state` + an
835// extern "C" fn pointer). The fn pointer is `Send`/`Sync` by
836// definition; the `state` pointer's `Send`/`Sync` story matches the
837// owning `Executor` (which is `unsafe impl Send`). Treating the
838// slot itself as `Send` keeps the existing `Executor` Send impl
839// intact — see `unsafe impl Send for Executor {}` later in this
840// file.
841unsafe impl Send for DispatchSlot {}
842unsafe impl Sync for DispatchSlot {}
843
844/// Phase 258 (Track 2, 2a) — executor-owned component tick slot.
845///
846/// The layering-clean half of the W0-B `install` seam's tick fix
847/// (phase-257 D2). A `nros`-layer `install`/`register_node_borrowed`
848/// builds an `Arc<ComponentCell>` (the typed/poll-driven component
849/// state) and enrolls it here via [`Executor::enroll_component`]; the
850/// executor then drives `tick` on every enrolled slot at the tail of
851/// each [`spin_once`](Executor::spin_once) — so `install`'d nodes
852/// (C, C++, **and Rust owned-spin**) tick, closing the
853/// service-client/action poll gap that the callback-`Arc`-only
854/// lifetime left open.
855///
856/// Like [`DispatchSlot`] the executor only sees raw pointers + `extern
857/// "C"` fn pointers (no `nros` dep — `nros-node` is the lower layer):
858///
859/// * `state` — a *leaked* `Arc<ComponentCell>` (via `Arc::into_raw`),
860/// re-borrowed by the `nros`-side `tick`/`drop` fns. Unlike a
861/// pub/sub/timer component (kept alive by the executor's per-entity
862/// callback `Arc` clones), a poll-only component has no callbacks, so
863/// the slot must own a clone of the cell — hence the paired `drop`.
864/// * `tick` — `nros`-side `extern "C"` fn that casts `state` back to
865/// `&ComponentCell`, casts `exec_ctx` back to `*mut Executor`, and
866/// runs that one cell's tick (mirrors `ExecutorNodeRuntime::run_ticks`).
867/// * `drop` — `nros`-side `extern "C"` fn run on `Executor::drop` that
868/// reconstitutes + drops the leaked `Arc`, so the executor owns the
869/// cell's lifetime.
870///
871/// Kept a SEPARATE registry from [`DispatchSlot`] on purpose: framework
872/// dispatch (RTIC / Embassy) is interrupt-driven, name-keyed, and has no
873/// tick/own concern — mixing the two risks that path.
874#[derive(Clone, Copy)]
875pub struct ComponentSlot {
876 /// Leaked `Arc<ComponentCell>` (opaque to the executor). Owned by
877 /// this slot — dropped via `drop` on `Executor::drop`.
878 pub state: *mut core::ffi::c_void,
879 /// `nros`-side tick trampoline: `(state, exec_ctx)` where `exec_ctx`
880 /// is `*mut Executor`. Drives one component's `tick`.
881 pub tick: unsafe extern "C" fn(state: *mut core::ffi::c_void, exec_ctx: *mut core::ffi::c_void),
882 /// `nros`-side drop trampoline: reconstitutes + drops the leaked
883 /// `Arc<ComponentCell>` at `state`. Run once on `Executor::drop`.
884 pub drop: unsafe extern "C" fn(state: *mut core::ffi::c_void),
885}
886
887// SAFETY: same story as `DispatchSlot` — two raw pointers + two extern
888// "C" fn pointers. The `state` pointer's Send/Sync matches the owning
889// `Executor` (`unsafe impl Send for Executor`); the fn pointers are
890// Send/Sync by definition.
891unsafe impl Send for ComponentSlot {}
892unsafe impl Sync for ComponentSlot {}
893
894/// phase-271 — fixed capacity of the per-spin ready-sets (FIFO bitmap + EDF
895/// heap) and the dispatch loop's upper bound. The executor's callback index is
896/// carried in a `u64` active-mask (`1u64 << i`), so a callback table can hold at
897/// most 64 entries regardless of per-entry sizing; the ready-sets are sized to
898/// this ceiling (stack-transient) so any entry slice up to 64 dispatches
899/// correctly. `EdfReadySet`'s presence bitmap independently asserts `N <= 64`.
900pub(crate) const MAX_CALLBACK_SLOTS: usize = 64;
901
902pub struct Executor<'s> {
903 pub(crate) session: SessionStore,
904 /// phase-271 (issue 0110) — the six sized tables are no longer inline
905 /// arrays baked to `nros-node`'s build-time consts; they borrow
906 /// caller-owned, per-entry-sized storage (`&'s mut` slices carved from a
907 /// raw `[MaybeUninit<u64>]` backing by [`super::storage::carve`]). Lets a
908 /// fat native entry and a lean embedded entry in one shared-target
909 /// workspace each size to its own topology. `Executor` stays non-generic
910 /// (lifetime only) so the C/C++ FFI keeps wrapping one concrete type.
911 pub(crate) arena: &'s mut [MaybeUninit<u8>],
912 pub(crate) arena_used: usize,
913 pub(crate) entries: &'s mut [Option<CallbackMeta>],
914 /// Phase 110.B — registered scheduling contexts. Slot 0 is
915 /// auto-populated with a `Fifo` SC at construction; every entry
916 /// without an explicit binding maps to it via
917 /// `sched_context_bindings`.
918 pub(crate) sched_contexts: &'s mut [Option<super::sched_context::SchedContext>],
919 /// Per-entry SC binding parallel to `entries`. Defaults to
920 /// `SchedContextId(0)` (the auto-created Fifo SC).
921 pub(crate) sched_context_bindings: &'s mut [super::sched_context::SchedContextId],
922 /// Phase 110.E — user-space sporadic-server budget state per
923 /// Sporadic-class SC. Slot indices match `sched_contexts`; non-
924 /// Sporadic slots stay `None`.
925 pub(crate) sporadic_states: &'s mut [Option<super::sched_context::SporadicState>],
926 /// Phase 110.E.b — atomic sporadic state + opaque platform-timer
927 /// handle for ISR-driven refill. Populated by
928 /// `register_sporadic_timer`; dropped on Executor `Drop` via the
929 /// stored `destroy_fn`.
930 #[cfg(feature = "alloc")]
931 pub(crate) sporadic_atomic_states: &'s mut [Option<(
932 portable_atomic_util::Arc<super::sched_context::AtomicSporadicState>,
933 OpaqueTimerHandle,
934 )>],
935 /// Phase 110.G — major-frame length for time-triggered dispatch.
936 /// `0` (default) disables the TT gate entirely; non-zero enables
937 /// gating per
938 /// `SchedContext.tt_window_offset_us / tt_window_duration_us`.
939 pub(crate) major_frame_us: u32,
940 /// Phase 110.F — per-OS-priority worker pool. Lazily populated
941 /// on first dispatch routing to a non-zero `os_pri`. Lives
942 /// behind `feature = "scheduler-os-priority"` + `feature =
943 /// "std"` because workers need `std::thread` + `mpsc`.
944 #[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
945 pub(crate) os_priority_workers: std::collections::HashMap<u8, OsPriorityWorker>,
946 /// Phase 110.F — caller-supplied `apply_policy` function pointer
947 /// each worker invokes at startup to elevate its OS priority.
948 /// `None` = the worker pool is disabled; entries bound to non-
949 /// zero `os_pri` SCs fall back to the cooperative path.
950 /// Mirrors `Executor::open_threaded`'s `apply_policy: fn(...)`
951 /// shape — keeps Executor non-generic over Platform.
952 #[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
953 pub(crate) os_priority_apply_policy:
954 Option<fn(nros_platform_api::SchedPolicy) -> Result<(), nros_platform_api::SchedError>>,
955 pub(crate) trigger: Trigger,
956 pub(crate) semantics: ExecutorSemantics,
957 /// Node name for entities created via `register_subscription`/`register_service`.
958 /// Empty means unset — no liveliness tokens will be declared.
959 pub(crate) node_name: heapless::String<64>,
960 /// Phase 228.C — per-tier callback-group filter. `None` = wildcard (register
961 /// every callback — the single-tier degenerate case + today's behaviour).
962 /// `Some(groups)` = this tier's executor accepts only callbacks whose
963 /// `.callback_group()` is in the set; others are skipped at registration.
964 pub(crate) active_groups:
965 Option<heapless::Vec<heapless::String<32>, { crate::config::MAX_NODES }>>,
966 /// Node namespace (default: "/").
967 pub(crate) namespace: heapless::String<64>,
968 /// Phase 104.C.2 — rclcpp-style `add_node` table. Holds the
969 /// per-Node metadata (name, namespace, rmw, locator, default
970 /// SchedContext) for every Node attached to this Executor. The
971 /// implicit "primary" Node (NodeId(0)) mirrors `node_name` +
972 /// `namespace` above and is auto-populated on first use.
973 pub(crate) nodes: heapless::Vec<super::node_record::NodeRecord, { crate::config::MAX_NODES }>,
974 /// Phase 272 (RFC-0047) — config-seeded node → sched-context bindings, keyed by the node's
975 /// fully-qualified `(name, namespace)` pair. `NodeBuilder::build` consults this table to set a
976 /// node's `default_sched` when no explicit `.sched()` was given. Empty ⇒ every node stays
977 /// `SchedContextId(0)` (byte-identical to pre-272 behaviour). Sized by `MAX_NODES` (at most
978 /// one tier per node — RFC-0047 OQ1).
979 pub(crate) node_sched_table: heapless::Vec<
980 (
981 heapless::String<64>,
982 heapless::String<64>,
983 super::sched_context::SchedContextId,
984 ),
985 { crate::config::MAX_NODES },
986 >,
987 /// Phase 273 (RFC-0047) — config-seeded per-callback-group sched bindings, keyed by the node's
988 /// fully-qualified `(name, namespace)` pair PLUS the callback-group name. Overrides the node
989 /// default for a callback created in that group. Empty ⇒ no per-group binding (node default
990 /// stands). Sized by `MAX_CBS` — an upper bound on distinct callback-group bindings (you can
991 /// never have more distinct group bindings than max callbacks).
992 pub(crate) group_sched_table: heapless::Vec<
993 (
994 heapless::String<64>,
995 heapless::String<64>,
996 heapless::String<32>,
997 super::sched_context::SchedContextId,
998 ),
999 { crate::config::MAX_CBS },
1000 >,
1001 /// Phase 216 follow-up — per-Node dispatch trampoline registry.
1002 ///
1003 /// Populated by [`Executor::register_dispatch_slot`]; walked by
1004 /// [`Executor::dispatch_callback`] each time the board-side
1005 /// dispatch task hands off a `SignaledCallback` envelope.
1006 /// Sized by `MAX_NODES` because the upper-bound is one slot per
1007 /// Node pkg deployed on this executor (the same upper bound used
1008 /// by `nodes` and `extra_sessions`). `MAX_NODES` is driven by the
1009 /// `NROS_EXECUTOR_MAX_NODES` build-script env var (default 4);
1010 /// boards that deploy more Node pkgs raise it at build time.
1011 ///
1012 /// Default is `heapless::Vec::new()` (empty) — Nodes register
1013 /// themselves explicitly via the `register_dispatch_slot` API.
1014 /// The fallback shape avoids the `linkme` hazard on bare-metal
1015 /// Cortex-M / RISC-V (see `DispatchSlot` doc).
1016 pub(crate) dispatch_slots: heapless::Vec<DispatchSlot, { crate::config::MAX_NODES }>,
1017 /// Phase 258 (Track 2, 2a) — executor-owned component tick registry.
1018 /// Enrolled by [`Executor::enroll_component`] (from `nros`'s
1019 /// `install`/`register_node_borrowed`); each slot's `tick` runs at the
1020 /// tail of [`spin_once`](Self::spin_once); each slot's `drop` runs on
1021 /// `Executor::drop`. Bounded `MAX_NODES` (matches `dispatch_slots` /
1022 /// `nodes`). See [`ComponentSlot`] for why it's separate from
1023 /// `dispatch_slots`.
1024 pub(crate) component_slots: heapless::Vec<ComponentSlot, { crate::config::MAX_NODES }>,
1025 /// Phase 104.C.3 — extra sessions opened by `node_builder.rmw()`
1026 /// calls that named a backend different from the Executor's
1027 /// primary session. Indexed by `NodeRecord.session_idx`
1028 /// (1..=N maps to `extra_sessions[N-1]`; idx 0 is the primary
1029 /// `self.session`). Sized by `NROS_EXECUTOR_MAX_NODES` since one
1030 /// extra session per Node is the worst case.
1031 pub(crate) extra_sessions:
1032 heapless::Vec<session::ConcreteSession, { crate::config::MAX_NODES }>,
1033 /// Phase 156 — primary session's rmw name + locator, captured
1034 /// at `open*` time so `NodeBuilder::resolve_session_slot`'s
1035 /// cache lookup can detect when a `.rmw(name).locator(loc)`
1036 /// matches the primary (slot 0) instead of falling through to
1037 /// `CffiRmw::open_with_rmw` and trying to open a SECOND
1038 /// session against the same backend. zenoh-pico's global state
1039 /// is a process singleton; opening twice fails. Empty when
1040 /// constructed via `from_session(_ptr)` without `open*`
1041 /// recording the metadata; in that case the cache check
1042 /// degrades to "always miss" (today's behaviour).
1043 pub(crate) primary_rmw_name: heapless::String<32>,
1044 pub(crate) primary_locator: heapless::String<128>,
1045 #[cfg(feature = "std")]
1046 pub(crate) halt_flag: std::sync::Arc<std::sync::atomic::AtomicBool>,
1047 /// Phase 104.C.6 — shared executor wake flag. Any source of work
1048 /// (foreign thread handing off a callback, signal handler, future
1049 /// per-session vtable wake hook) sets this; `spin_once` swaps it to
1050 /// `false` on entry and, if it was `true`, polls every session with
1051 /// a 0-ms timeout instead of blocking. Lets one notification wake
1052 /// the executor regardless of which session the user is currently
1053 /// blocked on (the multi-RMW bridge case).
1054 #[cfg(feature = "std")]
1055 pub(crate) wake_flag: std::sync::Arc<std::sync::atomic::AtomicBool>,
1056 /// Phase 124.B.2 — wake condvar paired with `wake_flag`. The
1057 /// runtime-supplied wake callback (`nros_rmw_runtime_wake_cb` in
1058 /// nros-rmw-cffi) writes `wake_flag = true` AND signals
1059 /// `wake_cv` atomically under `wake_mu`. `spin_once` blocks on
1060 /// the cv with a deadline instead of calling `drive_io` with the
1061 /// user's timeout — sub-poll-period wake latency.
1062 ///
1063 /// Poll-only backends (NULL `set_wake_callback` slot) leave the
1064 /// cb uninstalled; the cv wait still fires on its deadline,
1065 /// then drive_io(0) drains whatever the backend's internal
1066 /// poll has buffered.
1067 #[cfg(feature = "std")]
1068 #[allow(dead_code)] // Wired by spin_once after 124.B.4.
1069 pub(crate) wake_cv: std::sync::Arc<std::sync::Condvar>,
1070 #[cfg(feature = "std")]
1071 #[allow(dead_code)]
1072 pub(crate) wake_mu: std::sync::Arc<std::sync::Mutex<()>>,
1073 /// Phase 130.3 — Zephyr+std uses `nros_platform_wake_*` (k_sem)
1074 /// instead of `std::sync::Condvar` because Zephyr's libc
1075 /// `pthread_cond_timedwait` hangs past its deadline. `None`
1076 /// when the platform provider didn't link a wake primitive
1077 /// (e.g. test builds with `rmw-cffi` but no `platform-*`
1078 /// feature); spin_once falls back to driving the transport
1079 /// for the full timeout in that case.
1080 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
1081 pub(crate) node_wake: Option<std::sync::Arc<super::node_wake::NodeWake>>,
1082 /// Phase 130.4 — true when at least one session's backend
1083 /// installed the wake callback. Drives whether `spin_once`
1084 /// uses the wake-primitive wait (`NodeWake` / `Condvar`) or
1085 /// just `drive_io(timeout_ms)`. Poll-only backends
1086 /// (XRCE-DDS-Client, current Cyclone/dust-DDS shims) leave
1087 /// this `false`; the wait then becomes a no-op sleep that
1088 /// starves reliable retransmission (Phase 127.C.4 root
1089 /// cause: server's `send_reply` flushes 100 ms once, then a
1090 /// blind `wait_ms(100)` sleeps with zero session activity, so
1091 /// the agent's ACK arrives into a stalled session and reliable
1092 /// redelivery never fires).
1093 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
1094 pub(crate) has_async_wake: bool,
1095 /// Phase 124.B.2 — opaque context Arc handed to backends via
1096 /// `set_wake_callback`. Lazy-allocated on first install; stays
1097 /// alive for the Executor's lifetime so the raw pointer stored
1098 /// in backends remains valid.
1099 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
1100 pub(crate) wake_ctx: Option<std::sync::Arc<WakeCtx>>,
1101 // Phase 141.A.3 — alloc-mode (no_std RTOS) mirror of the wake
1102 // state above. Same semantics: `wake_flag_alloc` is set SeqCst
1103 // by the runtime cb + cleared by spin_once on entry;
1104 // `node_wake_alloc` is the kernel-native binary semaphore
1105 // (lifted to alloc cfg in e36ee8cf) the cb signals;
1106 // `wake_ctx_alloc` is the Arc handed to backends via
1107 // `set_wake_callback(Some(cb), Arc::as_ptr(ctx) as *mut _)`.
1108 // `has_async_wake_alloc` is `true` after the first session
1109 // accepts the wake-cb install (`supports_wake_callback`).
1110 // Drives the no_std spin_once wait branch to block on
1111 // `node_wake_alloc.wait_ms(deadline)` instead of relying on
1112 // `drive_io`'s transport-blocking recv for the full timeout.
1113 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
1114 pub(crate) wake_flag_alloc: portable_atomic_util::Arc<portable_atomic::AtomicBool>,
1115 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
1116 pub(crate) node_wake_alloc: Option<portable_atomic_util::Arc<super::node_wake::NodeWake>>,
1117 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
1118 pub(crate) wake_ctx_alloc: Option<portable_atomic_util::Arc<super::wake_alloc::WakeCtxAlloc>>,
1119 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
1120 pub(crate) has_async_wake_alloc: bool,
1121 /// Phase 124.B.7.c — lazily-allocated POSIX signalfd worker.
1122 /// Owned by the Executor; spawned on first `signal_fd()` call.
1123 /// Drop joins the worker thread and closes the fd.
1124 #[cfg(all(feature = "signal-fd-wake", target_os = "linux"))]
1125 pub(crate) signal_fd: Option<WakeSignalFd>,
1126 #[cfg(feature = "param-services")]
1127 pub(crate) params: Option<alloc::boxed::Box<crate::parameter_services::ParamState>>,
1128 #[cfg(feature = "lifecycle-services")]
1129 pub(crate) lifecycle:
1130 Option<alloc::boxed::Box<crate::lifecycle_services::LifecycleRuntimeState>>,
1131 /// Sub-millisecond wall-clock residual carried across `spin_once` calls
1132 /// so timers tick at true wall-clock rate even when `drive_io` returns
1133 /// in well under 1 ms (e.g. zenoh-pico condvar wakeups under load).
1134 #[cfg(feature = "std")]
1135 pub(crate) spin_residual_us: u64,
1136 /// Sub-millisecond residual for no_std wall-clock timer accounting.
1137 #[cfg(not(feature = "std"))]
1138 pub(crate) spin_residual_us: u64,
1139 /// Wall-clock instant at which the previous `spin_once` exited. The
1140 /// timer delta on the next call is measured from this point so any
1141 /// time the caller spent between `spin_once` invocations (e.g. an
1142 /// explicit `thread::sleep`) counts toward timer accumulation just
1143 /// like time spent inside `drive_io`.
1144 #[cfg(feature = "std")]
1145 pub(crate) last_spin_end: Option<std::time::Instant>,
1146 /// Monotonic clock endpoint for no_std timer accounting.
1147 #[cfg(not(feature = "std"))]
1148 pub(crate) last_spin_end_us: Option<u64>,
1149 /// Optional platform clock hook supplied by `ExecutorConfig`.
1150 #[cfg(not(feature = "std"))]
1151 pub(crate) clock_us_fn: Option<fn() -> u64>,
1152 /// RFC-0052 W3b.2 — wall-clock (epoch µs) source for age monitors.
1153 pub(crate) epoch_us_fn: Option<fn() -> u64>,
1154 /// RFC-0052 W3b.4 — baked contract-monitor table (empty = uncontracted
1155 /// image; every monitor path below folds away).
1156 pub(crate) monitor_table: &'static [super::monitor::MonitorSpec],
1157 pub(crate) monitor_states: [super::monitor::MonitorState; super::monitor::MAX_MONITORS],
1158 /// W3b.5 — baked subscriber age-contract table (empty = none).
1159 pub(crate) age_table: &'static [super::monitor::AgeMonitorSpec],
1160 pub(crate) age_states: [super::monitor::AgeState; super::monitor::MAX_MONITORS],
1161 /// W3b.5 — hook invoked on `DeadlineAction::Fault` (panic when unset).
1162 pub(crate) fault_fn: Option<fn(&super::monitor::Violation)>,
1163 pub(crate) monitor_violations:
1164 heapless::Vec<super::monitor::Violation, { super::monitor::MAX_VIOLATIONS }>,
1165 /// Monotonic base for the std monitor clock (µs derived per check).
1166 #[cfg(feature = "std")]
1167 pub(crate) monitor_clock_base: Option<std::time::Instant>,
1168}
1169
1170impl<'s> Executor<'s> {
1171 /// phase-271 — assemble an executor over already-carved, caller-owned
1172 /// storage (`slices`, from [`super::storage::carve`]). Fills every
1173 /// non-storage field and reserves SC slot 0 for the default Fifo SC (carve
1174 /// left it `None`). The single builder shared by every constructor path.
1175 fn assemble(session: SessionStore, slices: super::storage::ExecutorSlices<'s>) -> Self {
1176 let super::storage::ExecutorSlices {
1177 arena,
1178 entries,
1179 sched_contexts,
1180 sched_context_bindings,
1181 sporadic_states,
1182 #[cfg(feature = "alloc")]
1183 sporadic_atomic_states,
1184 } = slices;
1185 // Slot 0 = the auto-created default Fifo SC (see field doc). carve
1186 // initialised the whole table to `None`; populate the reserved slot.
1187 if let Some(slot0) = sched_contexts.first_mut() {
1188 *slot0 = Some(super::sched_context::SchedContext::default());
1189 }
1190 Self {
1191 session,
1192 arena,
1193 arena_used: 0,
1194 entries,
1195 sched_contexts,
1196 sched_context_bindings,
1197 sporadic_states,
1198 #[cfg(feature = "alloc")]
1199 sporadic_atomic_states,
1200 major_frame_us: 0,
1201 #[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
1202 os_priority_workers: std::collections::HashMap::new(),
1203 #[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
1204 os_priority_apply_policy: None,
1205 trigger: Trigger::Any,
1206 semantics: ExecutorSemantics::RclcppExecutor,
1207 node_name: heapless::String::new(),
1208 active_groups: None,
1209 nodes: heapless::Vec::new(),
1210 node_sched_table: heapless::Vec::new(),
1211 group_sched_table: heapless::Vec::new(),
1212 dispatch_slots: heapless::Vec::new(),
1213 component_slots: heapless::Vec::new(),
1214 extra_sessions: heapless::Vec::new(),
1215 primary_rmw_name: heapless::String::new(),
1216 primary_locator: heapless::String::new(),
1217 namespace: {
1218 let mut ns = heapless::String::new();
1219 let _ = ns.push_str("/");
1220 ns
1221 },
1222 #[cfg(feature = "std")]
1223 halt_flag: std::sync::Arc::new(std::sync::atomic::AtomicBool::new(false)),
1224 #[cfg(feature = "std")]
1225 wake_flag: std::sync::Arc::new(std::sync::atomic::AtomicBool::new(false)),
1226 #[cfg(feature = "std")]
1227 wake_cv: std::sync::Arc::new(std::sync::Condvar::new()),
1228 #[cfg(feature = "std")]
1229 wake_mu: std::sync::Arc::new(std::sync::Mutex::new(())),
1230 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
1231 node_wake: super::node_wake::NodeWake::new().map(std::sync::Arc::new),
1232 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
1233 wake_ctx: None,
1234 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
1235 has_async_wake: false,
1236 // Phase 141.A.3 — alloc-mode wake state init. Constructed
1237 // eagerly (NodeWake allocation) so the runtime cb can be
1238 // installed lazily on first session without a fallible
1239 // alloc inside spin_once. `None` when the platform
1240 // provider reports the primitive unavailable (matches
1241 // the std-RTOS path's `node_wake: Option<...>`).
1242 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
1243 wake_flag_alloc: portable_atomic_util::Arc::new(portable_atomic::AtomicBool::new(
1244 false,
1245 )),
1246 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
1247 node_wake_alloc: super::node_wake::NodeWake::new().map(portable_atomic_util::Arc::new),
1248 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
1249 wake_ctx_alloc: None,
1250 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
1251 has_async_wake_alloc: false,
1252 #[cfg(all(feature = "signal-fd-wake", target_os = "linux"))]
1253 signal_fd: None,
1254 #[cfg(feature = "param-services")]
1255 params: None,
1256 #[cfg(feature = "lifecycle-services")]
1257 lifecycle: None,
1258 #[cfg(feature = "std")]
1259 spin_residual_us: 0,
1260 #[cfg(not(feature = "std"))]
1261 spin_residual_us: 0,
1262 // Initialise the spin endpoint to construction time so the
1263 // very first `spin_once` credits time the caller spent
1264 // *before* it (e.g. setup, an explicit pre-spin sleep) just
1265 // like time spent between later calls.
1266 #[cfg(feature = "std")]
1267 last_spin_end: Some(std::time::Instant::now()),
1268 #[cfg(not(feature = "std"))]
1269 last_spin_end_us: None,
1270 #[cfg(not(feature = "std"))]
1271 clock_us_fn: None,
1272 // RFC-0052 W3b.5 — hosted builds get a wall clock by default so
1273 // native age monitors activate without extra wiring; embedded
1274 // builds install `config.epoch_us` from the board in
1275 // `from_session_in`/`open` (the `not(std)` blocks above).
1276 #[cfg(feature = "std")]
1277 epoch_us_fn: Some(super::types::std_epoch_us),
1278 #[cfg(not(feature = "std"))]
1279 epoch_us_fn: None,
1280 monitor_table: &[],
1281 monitor_states: [super::monitor::MonitorState::default(); super::monitor::MAX_MONITORS],
1282 age_table: &[],
1283 age_states: [super::monitor::AgeState::default(); super::monitor::MAX_MONITORS],
1284 fault_fn: None,
1285 monitor_violations: heapless::Vec::new(),
1286 #[cfg(feature = "std")]
1287 monitor_clock_base: None,
1288 }
1289 }
1290
1291 /// Create an owning executor over caller-supplied `backing`, sized by
1292 /// `sizing`. The core, non-generic, per-entry entry point (the `alloc`
1293 /// [`from_session`](Self::from_session) convenience leaks a default backing
1294 /// and calls this; the macro / C FFI pass an entry-sized backing).
1295 ///
1296 /// # Safety
1297 /// `backing` must be ≥ `sizing.u64_len()` words, stay alive for `'s`, and
1298 /// not be otherwise accessed while the executor lives (it aliases it).
1299 /// `sizing.cbs` must be ≤ 64 (the `u64` ready-set bitmask ceiling).
1300 pub unsafe fn from_session_in(
1301 session: session::ConcreteSession,
1302 backing: &'s mut [MaybeUninit<u64>],
1303 sizing: super::storage::ExecutorSizing,
1304 ) -> Self {
1305 let slices = unsafe { super::storage::carve(backing, sizing.cbs, sizing.sc, sizing.arena) };
1306 Self::assemble(SessionStore::Owned(session), slices)
1307 }
1308
1309 /// Create a borrowing executor over caller-supplied `backing`, sized by
1310 /// `sizing`. Counterpart to [`from_session_in`](Self::from_session_in) for
1311 /// the per-tier / C model (the session is borrowed, not owned).
1312 ///
1313 /// # Safety
1314 /// - `session_ptr` must point to a valid session that outlives the executor
1315 /// and is not moved/dropped while it exists.
1316 /// - `backing` obligations as in [`from_session_in`](Self::from_session_in).
1317 pub unsafe fn from_session_ptr_in(
1318 session_ptr: *mut session::ConcreteSession,
1319 backing: &'s mut [MaybeUninit<u64>],
1320 sizing: super::storage::ExecutorSizing,
1321 ) -> Self {
1322 let slices = unsafe { super::storage::carve(backing, sizing.cbs, sizing.sc, sizing.arena) };
1323 Self::assemble(SessionStore::Borrowed(session_ptr), slices)
1324 }
1325}
1326
1327impl Executor<'static> {
1328 /// Create an executor from an already-opened session, using the build-time
1329 /// default sizing (`MAX_CBS`/`MAX_SC`/`ARENA_SIZE`). Convenience for
1330 /// std/alloc callers that don't size per-entry: it leaks a default-sized
1331 /// backing (executor-lifetime, one-time) and calls
1332 /// [`from_session_in`](Self::from_session_in). Per-entry sizing goes through
1333 /// the macro / `open_in` instead.
1334 #[cfg(feature = "alloc")]
1335 pub fn from_session(session: session::ConcreteSession) -> Self {
1336 let sizing = super::storage::ExecutorSizing::DEFAULT;
1337 // SAFETY: the leaked backing is exactly `sizing.u64_len()` words,
1338 // `'static`, and uniquely owned by this executor.
1339 unsafe { Self::from_session_in(session, leak_default_backing(sizing), sizing) }
1340 }
1341
1342 /// Create an executor from a borrowed session pointer, default-sized. The
1343 /// `alloc` convenience wrapper over
1344 /// [`from_session_ptr_in`](Self::from_session_ptr_in) — leaks a default
1345 /// backing so existing callers keep the zero-storage-arg signature.
1346 ///
1347 /// # Safety
1348 /// - `session_ptr` must point to a valid, initialized session that lives at
1349 /// least as long as this executor.
1350 /// - The caller must not move or drop the session while the executor exists.
1351 #[cfg(feature = "alloc")]
1352 pub unsafe fn from_session_ptr(session_ptr: *mut session::ConcreteSession) -> Self {
1353 let sizing = super::storage::ExecutorSizing::DEFAULT;
1354 // SAFETY: leaked backing as in `from_session`; session_ptr contract
1355 // forwarded to `from_session_ptr_in`.
1356 unsafe { Self::from_session_ptr_in(session_ptr, leak_default_backing(sizing), sizing) }
1357 }
1358}
1359
1360impl<'s> Executor<'s> {
1361 /// Phase 228.B (RFC-0015) — construct a tier task's executor that **shares**
1362 /// a session opened once by the orchestration `main()`.
1363 ///
1364 /// In the per-tier execution model `main()` opens one RMW session, then
1365 /// spawns one RTOS task per priority tier; each task calls this to get an
1366 /// [`Executor`] over the *same* session (the `Borrowed` session store — this
1367 /// executor neither owns nor closes it), registers its tier's callback
1368 /// groups, and spins. Thin alias over [`Executor::from_session_ptr`].
1369 ///
1370 /// # Safety
1371 /// `session` must outlive every executor/task built from it (the
1372 /// orchestration `main()` holds it and never returns / WFIs), and must not
1373 /// be mutated except through these executors' spin calls.
1374 #[cfg(feature = "alloc")]
1375 pub unsafe fn open_with_session(session: *mut session::ConcreteSession) -> Executor<'static> {
1376 unsafe { Executor::<'static>::from_session_ptr(session) }
1377 }
1378
1379 /// phase-271 — per-tier borrowed-session constructor over caller-supplied,
1380 /// per-tier-sized `backing`. The sized counterpart to
1381 /// [`open_with_session`](Self::open_with_session): each RTOS tier task owns
1382 /// its own backing so tiers size independently.
1383 ///
1384 /// # Safety
1385 /// `session` obligations as in [`open_with_session`](Self::open_with_session);
1386 /// `backing`/`sizing` as in [`from_session_ptr_in`](Self::from_session_ptr_in).
1387 pub unsafe fn open_with_session_in(
1388 session: *mut session::ConcreteSession,
1389 backing: &'s mut [MaybeUninit<u64>],
1390 sizing: super::storage::ExecutorSizing,
1391 ) -> Self {
1392 unsafe { Self::from_session_ptr_in(session, backing, sizing) }
1393 }
1394
1395 /// Raw pointer to this executor's RMW session, for the per-tier model:
1396 /// the boot task opens the one session via [`Executor::open`] (the RMW
1397 /// session is a process-wide singleton — opening twice fails), then hands
1398 /// this pointer to each spawned tier task's
1399 /// [`Executor::open_with_session`]. The boot task's executor owns the
1400 /// session and outlives every borrower, so the pointer stays valid for the
1401 /// program's life. Works for both `Owned` and `Borrowed` stores.
1402 ///
1403 /// # Safety
1404 /// The returned pointer aliases `self.session`. Callers must keep `self`
1405 /// alive (not moved/dropped) for as long as any tier executor uses the
1406 /// pointer, and must only touch the session through executor spin calls
1407 /// (the RMW backend serializes concurrent access through its own locks).
1408 pub fn session_ptr(&mut self) -> *mut session::ConcreteSession {
1409 &mut *self.session as *mut session::ConcreteSession
1410 }
1411
1412 /// Opaque, `Send` form of [`session_ptr`](Self::session_ptr) — the per-tier
1413 /// model hands this to each spawned tier task (it can cross the RTOS task /
1414 /// thread boundary, which a bare `*mut` cannot). See [`SessionHandle`].
1415 ///
1416 /// # Safety
1417 /// Same contract as [`session_ptr`](Self::session_ptr): `self` (the session
1418 /// owner) must outlive every executor built from the handle.
1419 pub fn session_handle(&mut self) -> SessionHandle {
1420 SessionHandle(self.session_ptr())
1421 }
1422
1423 /// Open an [`Executor`] over the session a [`SessionHandle`] refers to (the
1424 /// `Borrowed` store — neither owns nor closes it). The tier-task counterpart
1425 /// to [`session_handle`](Self::session_handle).
1426 ///
1427 /// # Safety
1428 /// The handle's session must still be alive (its owning executor not moved
1429 /// or dropped); access only through executor spin calls.
1430 #[cfg(feature = "alloc")]
1431 pub unsafe fn open_with_session_handle(handle: SessionHandle) -> Executor<'static> {
1432 unsafe { Executor::<'static>::open_with_session(handle.0) }
1433 }
1434
1435 /// phase-271 — sized counterpart to
1436 /// [`open_with_session_handle`](Self::open_with_session_handle) (per-tier
1437 /// backing).
1438 ///
1439 /// # Safety
1440 /// As [`open_with_session_handle`](Self::open_with_session_handle) +
1441 /// [`from_session_ptr_in`](Self::from_session_ptr_in).
1442 pub unsafe fn open_with_session_handle_in(
1443 handle: SessionHandle,
1444 backing: &'s mut [MaybeUninit<u64>],
1445 sizing: super::storage::ExecutorSizing,
1446 ) -> Self {
1447 unsafe { Self::open_with_session_in(handle.0, backing, sizing) }
1448 }
1449
1450 /// Phase 228.C — set this tier executor's active callback-group filter. The
1451 /// generated per-tier task calls this before registering nodes; afterwards
1452 /// only callbacks whose `.callback_group()` is in `groups` register here.
1453 /// An empty slice (or never calling it) leaves the wildcard — register all
1454 /// callbacks (the single-tier degenerate case + today's behaviour).
1455 pub fn set_active_groups(&mut self, groups: &[&str]) {
1456 if groups.is_empty() {
1457 self.active_groups = None;
1458 return;
1459 }
1460 let mut v = heapless::Vec::new();
1461 for g in groups {
1462 let mut s = heapless::String::new();
1463 if s.push_str(g).is_ok() {
1464 let _ = v.push(s);
1465 }
1466 }
1467 self.active_groups = Some(v);
1468 }
1469
1470 /// Phase 228.C — whether a callback in `group` should register in this
1471 /// executor under the current filter. Wildcard (`None`) accepts everything.
1472 pub fn group_active(&self, group: &str) -> bool {
1473 group_filter_accepts(&self.active_groups, group)
1474 }
1475
1476 /// Set the node name and namespace used for liveliness tokens.
1477 ///
1478 /// Called by `open()` to propagate config values. When `register_subscription`
1479 /// or `register_service` creates entities, these values are attached to the
1480 /// Phase 156 — record the primary session's backend identity
1481 /// (rmw name + locator) so `NodeBuilder::resolve_session_slot`
1482 /// can detect when a `.rmw(name)` matches the primary instead
1483 /// of opening a SECOND backend session against the same
1484 /// singleton (zenoh-pico's `g_session` is process-wide;
1485 /// opening twice fails). `Executor::open*` calls this
1486 /// automatically; the C surface (`nros_executor_init`) calls
1487 /// it manually because it constructs via `from_session_ptr`
1488 /// which doesn't know the open metadata. Empty strings = "no
1489 /// primary identity tracked"; the cache check degrades to
1490 /// always-miss.
1491 pub fn set_primary_identity(&mut self, rmw_name: &str, locator: &str) {
1492 self.primary_rmw_name.clear();
1493 let _ = self.primary_rmw_name.push_str(rmw_name);
1494 self.primary_locator.clear();
1495 let _ = self.primary_locator.push_str(locator);
1496 }
1497
1498 /// `TopicInfo`/`ServiceInfo` so the zenoh backend can declare liveliness.
1499 pub fn set_node_identity(&mut self, node_name: &str, namespace: &str) {
1500 self.node_name.clear();
1501 let _ = self.node_name.push_str(node_name);
1502 if !namespace.is_empty() {
1503 self.namespace.clear();
1504 let _ = self.namespace.push_str(namespace);
1505 }
1506 }
1507
1508 // =========================================================================
1509 // Phase 272 (RFC-0047) — node-name → sched-context table
1510 // =========================================================================
1511
1512 /// Seed a config-resolved tier binding by `(name, namespace)` before the
1513 /// node is built. `NodeBuilder::build` consults this table when no
1514 /// explicit `.sched()` override is given — the table entry then wins over
1515 /// the `SchedContextId(0)` default (precedence: explicit > table > 0).
1516 ///
1517 /// Call BEFORE `node_builder(name).build()`. An existing entry for the
1518 /// same `(name, namespace)` key is overwritten (last-write wins). Overflow
1519 /// past `MAX_NODES` is silently ignored. An empty `namespace` is normalised
1520 /// to `"/"` to match what `NodeBuilder::build` computes for a root-NS node.
1521 pub fn bind_node_name_sched(
1522 &mut self,
1523 name: &str,
1524 namespace: &str,
1525 sc: super::sched_context::SchedContextId,
1526 ) {
1527 let norm_ns = if namespace.is_empty() { "/" } else { namespace };
1528 // Overwrite if there is already an entry for this (name, ns) pair.
1529 for entry in self.node_sched_table.iter_mut() {
1530 if entry.0.as_str() == name && entry.1.as_str() == norm_ns {
1531 entry.2 = sc;
1532 return;
1533 }
1534 }
1535 // New entry — build the heapless strings and push. Silently ignore
1536 // if the name/ns is too long or the table is at capacity.
1537 let mut name_s = heapless::String::<64>::new();
1538 let mut ns_s = heapless::String::<64>::new();
1539 if name_s.push_str(name).is_err() || ns_s.push_str(norm_ns).is_err() {
1540 return;
1541 }
1542 let _ = self.node_sched_table.push((name_s, ns_s, sc));
1543 }
1544
1545 /// Look up the seeded sched-context for `(name, namespace)`. Returns
1546 /// `None` when the table has no entry for this pair (unseed → default 0).
1547 /// `pub(super)` — visible only within the `executor` module (sibling
1548 /// `node_record` calls it from `NodeBuilder::build`).
1549 pub(super) fn lookup_node_sched(
1550 &self,
1551 name: &str,
1552 namespace: &str,
1553 ) -> Option<super::sched_context::SchedContextId> {
1554 for entry in self.node_sched_table.iter() {
1555 if entry.0.as_str() == name && entry.1.as_str() == namespace {
1556 return Some(entry.2);
1557 }
1558 }
1559 None
1560 }
1561
1562 // =========================================================================
1563 // Phase 273 (RFC-0047) — per-callback-group → sched-context table
1564 // =========================================================================
1565
1566 /// Seed a config-resolved tier binding by `(name, namespace, group)` before
1567 /// entities are registered. `apply_node_default_sched` consults this table
1568 /// first (group table > node default > `SchedContextId(0)`).
1569 ///
1570 /// Call BEFORE entity creation. An existing entry for the same
1571 /// `(name, namespace, group)` key is overwritten (last-write wins). Overflow
1572 /// past `MAX_CBS` is silently ignored. An empty `namespace` is normalised to
1573 /// `"/"` to match `NodeBuilder::build`. Mirror of `bind_node_name_sched`.
1574 pub fn bind_group_sched(
1575 &mut self,
1576 name: &str,
1577 namespace: &str,
1578 group: &str,
1579 sc: super::sched_context::SchedContextId,
1580 ) {
1581 let norm_ns = if namespace.is_empty() { "/" } else { namespace };
1582 // Overwrite if there is already an entry for this (name, ns, group).
1583 for entry in self.group_sched_table.iter_mut() {
1584 if entry.0.as_str() == name && entry.1.as_str() == norm_ns && entry.2.as_str() == group
1585 {
1586 entry.3 = sc;
1587 return;
1588 }
1589 }
1590 // New entry — build the heapless strings and push. Silently ignore
1591 // if name/ns/group is too long or the table is at capacity.
1592 let mut name_s = heapless::String::<64>::new();
1593 let mut ns_s = heapless::String::<64>::new();
1594 let mut grp_s = heapless::String::<32>::new();
1595 if name_s.push_str(name).is_err()
1596 || ns_s.push_str(norm_ns).is_err()
1597 || grp_s.push_str(group).is_err()
1598 {
1599 return;
1600 }
1601 let _ = self.group_sched_table.push((name_s, ns_s, grp_s, sc));
1602 }
1603
1604 /// Look up the seeded sched-context for `(name, namespace, group)`. Returns
1605 /// `None` when the table has no entry for this triple.
1606 fn lookup_group_sched(
1607 &self,
1608 name: &str,
1609 namespace: &str,
1610 group: &str,
1611 ) -> Option<super::sched_context::SchedContextId> {
1612 for entry in self.group_sched_table.iter() {
1613 if entry.0.as_str() == name
1614 && entry.1.as_str() == namespace
1615 && entry.2.as_str() == group
1616 {
1617 return Some(entry.3);
1618 }
1619 }
1620 None
1621 }
1622
1623 // =========================================================================
1624 // Phase 110.B — SchedContext API
1625 // =========================================================================
1626
1627 /// Identifier of the auto-created default `Fifo`-class scheduling
1628 /// context. Every callback registered without an explicit
1629 /// [`bind_handle_to_sched_context`] binds to this SC.
1630 pub fn default_sched_context_id(&self) -> super::sched_context::SchedContextId {
1631 super::sched_context::SchedContextId(0)
1632 }
1633
1634 /// Register a new scheduling context. Returns a [`SchedContextId`]
1635 /// callers pass to [`bind_handle_to_sched_context`] to attach
1636 /// callbacks. Phase 110.B.
1637 pub fn create_sched_context(
1638 &mut self,
1639 sc: super::sched_context::SchedContext,
1640 ) -> Result<super::sched_context::SchedContextId, NodeError> {
1641 // Slot 0 is reserved for the default Fifo SC; search 1..MAX_SC.
1642 for (i, slot) in self.sched_contexts.iter_mut().enumerate().skip(1) {
1643 if slot.is_none() {
1644 *slot = Some(sc);
1645 // Phase 110.E — Sporadic-class SCs get a sibling
1646 // `SporadicState` entry that the spin_once dispatch
1647 // path consults each cycle to refill the budget at
1648 // period boundaries and skip dispatch when budget
1649 // is exhausted.
1650 if matches!(sc.class, super::sched_context::SchedClass::Sporadic) {
1651 let budget = sc.budget_us.get().map(|nz| nz.get()).unwrap_or(u32::MAX);
1652 let period = sc.period_us.get().map(|nz| nz.get()).unwrap_or(u32::MAX);
1653 self.sporadic_states[i] =
1654 Some(super::sched_context::SporadicState::new(budget, period));
1655 }
1656 return Ok(super::sched_context::SchedContextId(i as u8));
1657 }
1658 }
1659 Err(NodeError::NoSchedContextSlot)
1660 }
1661
1662 /// RFC-0052 W3b.2 — wall-clock µs since the UNIX epoch, when this
1663 /// target has an epoch source (config `epoch_us`, defaulted from
1664 /// `SystemTime` on hosted configs). `None` = no wall clock; age
1665 /// monitors must not have been baked (the emitter refuses).
1666 pub fn epoch_now_us(&self) -> Option<u64> {
1667 self.epoch_us_fn.map(|f| f())
1668 }
1669
1670 /// RFC-0052 W3b.4 — install the baked contract-monitor table. Call
1671 /// BEFORE entity creation so `create_publisher` can attach each
1672 /// contracted endpoint's counter cell. Mirrors `set_qos_overrides`:
1673 /// `&'static`, codegen-baked, empty by default.
1674 pub fn set_monitor_table(&mut self, table: &'static [super::monitor::MonitorSpec]) {
1675 self.monitor_table = table;
1676 }
1677
1678 /// The installed monitor table (empty unless the entry set one).
1679 #[must_use]
1680 pub fn monitor_table(&self) -> &'static [super::monitor::MonitorSpec] {
1681 self.monitor_table
1682 }
1683
1684 /// W3b.5 — install the baked subscriber age-contract table. Call
1685 /// BEFORE entity creation so `create_subscription` can attach each
1686 /// contracted endpoint's age cell (needs an epoch source — see
1687 /// `ExecutorConfig::epoch_us`; without one the take path records
1688 /// nothing and age monitors stay silent).
1689 pub fn set_age_table(&mut self, table: &'static [super::monitor::AgeMonitorSpec]) {
1690 self.age_table = table;
1691 }
1692
1693 /// The installed age table (empty unless the entry set one).
1694 #[must_use]
1695 pub fn age_table(&self) -> &'static [super::monitor::AgeMonitorSpec] {
1696 self.age_table
1697 }
1698
1699 /// W3b.5 — override the wall-clock (epoch µs) source age monitors take
1700 /// message stamps against. Hosted builds default to `SystemTime`; a
1701 /// board with a synced RTC installs its own here (or via
1702 /// `ExecutorConfig::epoch_us`). Call BEFORE entity creation — the age
1703 /// hook captures this at `create_subscription` time.
1704 pub fn set_epoch_clock(&mut self, epoch_us: fn() -> u64) {
1705 self.epoch_us_fn = Some(epoch_us);
1706 }
1707
1708 /// W3b.5 — resolve a subscription's age hook at registration time:
1709 /// exact topic match against the baked age table, only for stamped
1710 /// types (`M::STAMP_OFFSET`) and only when an epoch source exists.
1711 fn age_lookup<M: RosMessage>(&self, topic: &str) -> Option<super::arena::AgeMon> {
1712 M::STAMP_OFFSET?;
1713 let epoch = self.epoch_us_fn?;
1714 self.age_table
1715 .iter()
1716 .find(|a| a.topic == topic)
1717 .map(|a| (a.cell, epoch))
1718 }
1719
1720 /// W3b.5 — install the `DeadlineAction::Fault` hook. Without one a
1721 /// fault-class deadline miss panics (watchdog-visible stop on
1722 /// embedded targets).
1723 pub fn set_fault_handler(&mut self, f: fn(&super::monitor::Violation)) {
1724 self.fault_fn = Some(f);
1725 }
1726
1727 /// RFC-0052 W3b.4 — drain pending contract violations (rate rule for
1728 /// now; age/latency land with W3b.5). The entry glue calls this after
1729 /// `spin_once` and feeds each entry to the `nros-diagnostics`
1730 /// reporter. Draining clears the ring.
1731 pub fn drain_violations(&mut self, mut f: impl FnMut(&super::monitor::Violation)) {
1732 for v in self.monitor_violations.iter() {
1733 f(v);
1734 }
1735 self.monitor_violations.clear();
1736 }
1737
1738 /// Monotonic µs for monitor windows: the no_std `clock_us` hook, or a
1739 /// process-local Instant base on std.
1740 fn monitor_now_us(&mut self) -> Option<u64> {
1741 #[cfg(not(feature = "std"))]
1742 {
1743 self.clock_us_fn.map(|clock| clock())
1744 }
1745 #[cfg(feature = "std")]
1746 {
1747 let base = *self
1748 .monitor_clock_base
1749 .get_or_insert_with(std::time::Instant::now);
1750 Some(base.elapsed().as_micros() as u64)
1751 }
1752 }
1753
1754 /// Run the rate/latency/age checks over the baked tables (single
1755 /// branch each when empty).
1756 fn run_contract_monitors(&mut self) {
1757 if !self.monitor_table.is_empty()
1758 && let Some(now_us) = self.monitor_now_us()
1759 {
1760 {
1761 for (i, spec) in self
1762 .monitor_table
1763 .iter()
1764 .take(super::monitor::MAX_MONITORS)
1765 .enumerate()
1766 {
1767 if let Some(v) =
1768 super::monitor::check_rate(spec, &mut self.monitor_states[i], now_us)
1769 {
1770 let _ = self.monitor_violations.push(v);
1771 }
1772 if let Some(v) =
1773 super::monitor::check_latency(spec, &mut self.monitor_states[i])
1774 {
1775 let _ = self.monitor_violations.push(v);
1776 }
1777 }
1778 }
1779 }
1780 if !self.age_table.is_empty() {
1781 for (i, spec) in self
1782 .age_table
1783 .iter()
1784 .take(super::monitor::MAX_MONITORS)
1785 .enumerate()
1786 {
1787 if let Some(v) = super::monitor::check_age(spec, &mut self.age_states[i]) {
1788 let _ = self.monitor_violations.push(v);
1789 }
1790 }
1791 }
1792 }
1793
1794 /// RFC-0052 / phase-296 W3a — replace the DEFAULT scheduling context
1795 /// (slot 0, the SC every unbound callback dispatches through).
1796 ///
1797 /// The run_tiers model runs one Executor per tier, so a tier-wide
1798 /// scheduling policy (`[tiers.<t>] class/budget_us/period_us` and the
1799 /// TT window) is exactly "this executor's default SC". Boards call
1800 /// this once, before entity creation; explicit per-handle/per-group
1801 /// bindings still take precedence (they never resolve to slot 0).
1802 ///
1803 /// Sporadic-class SCs get the same sibling `SporadicState` the
1804 /// `create_sched_context` path builds, so budget refill/exhaustion
1805 /// applies to the default queue too.
1806 pub fn set_default_sched_context(&mut self, sc: super::sched_context::SchedContext) {
1807 self.sched_contexts[0] = Some(sc);
1808 if matches!(sc.class, super::sched_context::SchedClass::Sporadic) {
1809 let budget = sc.budget_us.get().map(|nz| nz.get()).unwrap_or(u32::MAX);
1810 let period = sc.period_us.get().map(|nz| nz.get()).unwrap_or(u32::MAX);
1811 self.sporadic_states[0] =
1812 Some(super::sched_context::SporadicState::new(budget, period));
1813 } else {
1814 self.sporadic_states[0] = None;
1815 }
1816 }
1817
1818 /// Bind a registered callback to a scheduling context. The next
1819 /// `spin_once` cycle dispatches the callback through that SC's
1820 /// queue (FIFO bitmap or EDF heap). Phase 110.B.
1821 pub fn bind_handle_to_sched_context(
1822 &mut self,
1823 handle: HandleId,
1824 sc_id: super::sched_context::SchedContextId,
1825 ) -> Result<(), NodeError> {
1826 let i = handle.0;
1827 if i >= self.entries.len() {
1828 return Err(NodeError::InvalidSchedContextBinding);
1829 }
1830 if self.entries[i].is_none() {
1831 return Err(NodeError::InvalidSchedContextBinding);
1832 }
1833 let sc_idx = sc_id.0 as usize;
1834 if sc_idx >= self.sched_contexts.len() || self.sched_contexts[sc_idx].is_none() {
1835 return Err(NodeError::InvalidSchedContextBinding);
1836 }
1837 self.sched_context_bindings[i] = sc_id;
1838 Ok(())
1839 }
1840
1841 /// Phase 110.F — opt in to per-callback OS-priority dispatch.
1842 /// Once registered, every `spin_once` cycle routes ready entries
1843 /// whose bound SC has `os_pri > 0` onto a worker thread the OS
1844 /// scheduler has elevated to that numeric priority. Workers are
1845 /// spawned lazily on first use and self-halt when the Executor
1846 /// drops.
1847 ///
1848 /// `apply_policy` is the same `fn(SchedPolicy) -> Result<(),
1849 /// SchedError>` shape `open_threaded` takes — keeps the
1850 /// Executor non-generic over Platform.
1851 ///
1852 /// Calling this with `apply_policy = noop` is fine for testing
1853 /// (workers spawn but don't actually elevate priority); real
1854 /// hard-RT use needs `CAP_SYS_NICE` on Linux or the equivalent
1855 /// kernel config on RTOSes.
1856 #[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
1857 pub fn register_os_priority_dispatcher(
1858 &mut self,
1859 apply_policy: fn(
1860 nros_platform_api::SchedPolicy,
1861 ) -> Result<(), nros_platform_api::SchedError>,
1862 ) {
1863 self.os_priority_apply_policy = Some(apply_policy);
1864 }
1865
1866 /// Phase 110.G — enable time-triggered dispatch by setting the
1867 /// executor's major-frame length. Once set, every `spin_once`
1868 /// cycle gates dispatch through each entry's bound SC's
1869 /// `tt_window_offset_us` / `tt_window_duration_us` fields:
1870 /// dispatch only fires when the current monotonic time falls
1871 /// inside the window `[off, off + duration) mod major_frame`.
1872 ///
1873 /// `major_frame_us = 0` disables the TT gate (default state).
1874 /// Setting a non-zero major frame after callbacks are already
1875 /// registered is allowed — TT gates take effect on the next
1876 /// `spin_once` cycle.
1877 pub fn register_time_triggered_dispatcher(&mut self, major_frame_us: u32) {
1878 self.major_frame_us = major_frame_us;
1879 }
1880
1881 /// Phase 110.G — apply a declarative cyclic schedule.
1882 ///
1883 /// One-shot helper that wraps the underlying primitives:
1884 /// validates the schedule (`major_frame > 0`, no overlapping
1885 /// windows, every window fits inside the major frame), sets the
1886 /// executor's major-frame length, then materialises one
1887 /// `SchedContext` per window with `class = TimeTriggered` +
1888 /// the window's offset / duration. Returns the per-window
1889 /// [`SchedContextId`] array so callers can immediately
1890 /// `bind_handle_to_sched_context(handle, sc_id)` for their
1891 /// subscription / timer handles.
1892 ///
1893 /// `N` is the schedule's *declared* maximum window count;
1894 /// `schedule.window_count` gates how many SCs are actually
1895 /// created. Unused trailing slots return
1896 /// `SchedContextId::default()` (sentinel — callers must respect
1897 /// `window_count`).
1898 pub fn apply_time_triggered_schedule<const N: usize>(
1899 &mut self,
1900 schedule: &super::sched_context::TimeTriggeredSchedule<N>,
1901 ) -> Result<
1902 [super::sched_context::SchedContextId; N],
1903 super::sched_context::TimeTriggeredScheduleError,
1904 > {
1905 schedule.validate()?;
1906 self.major_frame_us = schedule.major_frame_us;
1907 // SC slot 0 is the auto-created default; reusing it as a
1908 // sentinel for unused trailing slots is safe because the
1909 // caller respects `schedule.window_count`.
1910 let mut ids: [super::sched_context::SchedContextId; N] =
1911 [super::sched_context::SchedContextId(0); N];
1912 for (i, window) in schedule.windows[..schedule.window_count].iter().enumerate() {
1913 // Deprecation note on `SchedClass::TimeTriggered`: TT
1914 // is implemented as a per-SC *window gate* on top of
1915 // the existing class-based dispatch (Fifo here keeps
1916 // the EDF / Sporadic budgets out of the picture for
1917 // pure cyclic schedules). The window-gate fields set
1918 // below are what `spin_once`'s 110.G runtime gate
1919 // actually reads.
1920 let sc = super::sched_context::SchedContext {
1921 tt_window_offset_us: super::sched_context::OptUs::from_us(window.offset_us),
1922 tt_window_duration_us: super::sched_context::OptUs::from_us(window.duration_us),
1923 ..super::sched_context::SchedContext::new_fifo()
1924 };
1925 ids[i] = self.create_sched_context(sc).map_err(|_| {
1926 super::sched_context::TimeTriggeredScheduleError::WindowCountOverflow
1927 })?;
1928 }
1929 Ok(ids)
1930 }
1931
1932 /// Phase 110.E.b — register an ISR-driven refill timer for an
1933 /// already-created Sporadic SC. The caller invokes their
1934 /// platform's `PlatformTimer::create_periodic` with the returned
1935 /// `Arc<AtomicSporadicState>` as `user_data` and the
1936 /// `atomic_sporadic_refill_thunk` as the callback, then hands
1937 /// the resulting platform handle to this method via
1938 /// `OpaqueTimerHandle::new(handle, destroy_fn)`.
1939 ///
1940 /// The Executor stores both the Arc and the handle so Drop can
1941 /// clean them up. Calling this on a non-Sporadic SC returns
1942 /// `Err(InvalidSchedContextBinding)`.
1943 #[cfg(feature = "alloc")]
1944 pub fn register_sporadic_timer(
1945 &mut self,
1946 sc_id: super::sched_context::SchedContextId,
1947 timer: OpaqueTimerHandle,
1948 ) -> Result<portable_atomic_util::Arc<super::sched_context::AtomicSporadicState>, NodeError>
1949 {
1950 let i = sc_id.0 as usize;
1951 if i >= self.sched_contexts.len() {
1952 return Err(NodeError::InvalidSchedContextBinding);
1953 }
1954 let sc = self.sched_contexts[i]
1955 .as_ref()
1956 .ok_or(NodeError::InvalidSchedContextBinding)?;
1957 if !matches!(sc.class, super::sched_context::SchedClass::Sporadic) {
1958 return Err(NodeError::InvalidSchedContextBinding);
1959 }
1960 let budget = sc.budget_us.get().map(|nz| nz.get()).unwrap_or(u32::MAX);
1961 let period = sc.period_us.get().map(|nz| nz.get()).unwrap_or(u32::MAX);
1962 let state = portable_atomic_util::Arc::new(super::sched_context::AtomicSporadicState::new(
1963 budget, period,
1964 ));
1965 self.sporadic_atomic_states[i] = Some((portable_atomic_util::Arc::clone(&state), timer));
1966 Ok(state)
1967 }
1968
1969 /// Inspect a registered scheduling context. Phase 110.B.
1970 pub fn sched_context(
1971 &self,
1972 sc_id: super::sched_context::SchedContextId,
1973 ) -> Option<&super::sched_context::SchedContext> {
1974 self.sched_contexts.get(sc_id.0 as usize)?.as_ref()
1975 }
1976
1977 /// Phase 104.C.2 — start a rclcpp-style Node builder for this
1978 /// Executor. The returned [`NodeBuilder`](super::node_record::NodeBuilder)
1979 /// is chainable:
1980 ///
1981 /// ```ignore
1982 /// let id = exec.node_builder("ingress")
1983 /// .rmw("zenoh")
1984 /// .locator("tcp/127.0.0.1:7447")
1985 /// .sched(my_sc_id)
1986 /// .build()?;
1987 /// ```
1988 ///
1989 /// In Phase 104.C.2 the Node table is storage-only — all
1990 /// registered Nodes share the Executor's primary session. Per-
1991 /// Node session binding (the bridge feature) lands in Phase
1992 /// 104.C.3 when the session cache is wired.
1993 pub fn node_builder<'a, 'cfg>(
1994 &'a mut self,
1995 name: &'cfg str,
1996 ) -> super::node_record::NodeBuilder<'a, 'cfg, 's> {
1997 super::node_record::NodeBuilder {
1998 executor: self,
1999 name,
2000 namespace: None,
2001 rmw_name: None,
2002 locator: None,
2003 domain_id: None,
2004 sched: None,
2005 session_idx: None,
2006 }
2007 }
2008
2009 /// Return the Node table — Phase 104.C.2 read accessor.
2010 pub fn nodes(&self) -> &[super::node_record::NodeRecord] {
2011 &self.nodes
2012 }
2013
2014 /// Borrow a Node's metadata by id, returning `None` if the id
2015 /// is out of range.
2016 pub fn node(&self, id: super::node_record::NodeId) -> Option<&super::node_record::NodeRecord> {
2017 self.nodes.get(id.index())
2018 }
2019
2020 /// Phase 189.M1 — an executor-borrowing node handle for the entity builders
2021 /// (`exec.node_mut(id).subscription(t)...` / `.create_subscription(...)`).
2022 /// A short-lived `&mut Executor` borrow — use one at a time; entity handles
2023 /// are owned and outlive it (see `NodeCtx`).
2024 pub fn node_mut(&mut self, id: super::node_record::NodeId) -> super::node::NodeCtx<'_, 's> {
2025 super::node::NodeCtx::new(self, id)
2026 }
2027
2028 /// Phase 104.C.3 — resolve a session-slot index to a mutable
2029 /// session reference. Slot 0 = the Executor's primary session;
2030 /// slots 1..=N = the `extra_sessions` vec opened by
2031 /// `node_builder.rmw(name)` calls that named a backend
2032 /// different from the primary.
2033 pub(crate) fn session_at_mut(&mut self, idx: u8) -> Option<&mut session::ConcreteSession> {
2034 if idx == 0 {
2035 Some(&mut *self.session)
2036 } else {
2037 self.extra_sessions.get_mut((idx - 1) as usize)
2038 }
2039 }
2040
2041 /// Phase 104.C.9.b — resolve the per-Node session for direct
2042 /// entity creation paths (C++ FFI publisher / subscription /
2043 /// service that bypass the `register_*_on` arena dispatch).
2044 /// Returns `None` when `node_id` is out of range or the Node's
2045 /// `session_idx` lands outside the executor's session table.
2046 pub fn node_session_mut(
2047 &mut self,
2048 node_id: super::node_record::NodeId,
2049 ) -> Option<&mut session::ConcreteSession> {
2050 let session_idx = self.nodes.get(node_id.index())?.session_idx;
2051 self.session_at_mut(session_idx)
2052 }
2053
2054 /// Phase 189.M1 — create a typed publisher bound to a node's session.
2055 /// Backs `node.publisher(t).typed::<M>().build()` on the
2056 /// executor-borrowing [`NodeCtx`](super::node::NodeCtx); the returned
2057 /// handle is owned and outlives the `NodeCtx`.
2058 pub fn create_publisher_on<M: crate::rmw_type_registry::MessageForRmw>(
2059 &mut self,
2060 node_id: super::node_record::NodeId,
2061 topic_name: &str,
2062 qos: QosSettings,
2063 ) -> Result<crate::executor::handles::EmbeddedPublisher<M>, NodeError> {
2064 // Phase 212.K.7.6.b — register `M`'s cyclonedds descriptor before
2065 // creating the underlying publisher handle. No-op for other RMWs.
2066 crate::rmw_type_registry::register_type::<M>()?;
2067 let handle = self.create_raw_publisher_handle_on(
2068 node_id,
2069 topic_name,
2070 <M as RosMessage>::TYPE_NAME,
2071 <M as RosMessage>::TYPE_HASH,
2072 qos,
2073 )?;
2074 // RFC-0052 W3b.4 — attach the contracted endpoint's counter cell.
2075 let monitor = self
2076 .monitor_table
2077 .iter()
2078 .find(|m| m.topic == topic_name)
2079 .map(|m| m.cell);
2080 Ok(crate::executor::handles::EmbeddedPublisher {
2081 handle,
2082 event_regs: crate::executor::handles::empty_event_regs(),
2083 monitor,
2084 _phantom: PhantomData,
2085 })
2086 }
2087
2088 /// Phase 189.M1 — create a generic (type-erased) publisher bound to a
2089 /// node's session. Backs `node.publisher(t).generic(ty, hash).build()`;
2090 /// the bridge re-publishes through this handle on the dest session.
2091 pub fn create_publisher_raw_on(
2092 &mut self,
2093 node_id: super::node_record::NodeId,
2094 topic_name: &str,
2095 type_name: &str,
2096 type_hash: &str,
2097 qos: QosSettings,
2098 ) -> Result<crate::executor::handles::EmbeddedRawPublisher, NodeError> {
2099 let handle =
2100 self.create_raw_publisher_handle_on(node_id, topic_name, type_name, type_hash, qos)?;
2101 Ok(crate::executor::handles::EmbeddedRawPublisher {
2102 handle,
2103 arena: crate::executor::handles::TxArena::new(),
2104 event_regs: crate::executor::handles::empty_event_regs(),
2105 })
2106 }
2107
2108 /// Shared prelude for the publisher-on-node paths: resolve the node's
2109 /// identity + session slot, build the [`TopicInfo`], validate QoS, and
2110 /// create the backend publisher handle. Mirrors
2111 /// `register_subscription_buffered_raw_on`'s session resolution so a
2112 /// bridge's source sub + dest pub agree on topic construction.
2113 fn create_raw_publisher_handle_on(
2114 &mut self,
2115 node_id: super::node_record::NodeId,
2116 topic_name: &str,
2117 type_name: &str,
2118 type_hash: &str,
2119 qos: QosSettings,
2120 ) -> Result<session::RmwPublisher, NodeError> {
2121 let (node_name, ns, session_idx) = {
2122 let r = self
2123 .nodes
2124 .get(node_id.index())
2125 .ok_or(NodeError::InvalidSchedContextBinding)?;
2126 (r.name.clone(), r.namespace.clone(), r.session_idx)
2127 };
2128 let mut topic = TopicInfo::new(topic_name, type_name, type_hash).with_namespace(&ns);
2129 if !node_name.is_empty() {
2130 topic = topic.with_node_name(&node_name);
2131 }
2132 let session = self
2133 .session_at_mut(session_idx)
2134 .ok_or(NodeError::BackendMismatch)?;
2135 qos.validate_against(Session::supported_qos_policies(session))
2136 .map_err(NodeError::Transport)?;
2137 session
2138 .create_publisher(&topic, qos)
2139 .map_err(|_| NodeError::Transport(TransportError::PublisherCreationFailed))
2140 }
2141
2142 /// Phase 124.B.1 — install the executor's wake callback onto the
2143 /// primary session. Best-effort: backends that don't override
2144 /// `Session::set_wake_callback` (poll-only XRCE, bare-metal)
2145 /// ignore the call and continue to be drained on the executor's
2146 /// deadline-bound cv-wait boundary.
2147 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
2148 fn install_wake_signal_on_primary(&mut self) {
2149 use nros_rmw::Session as _;
2150 let ctx = self.wake_ctx_ptr();
2151 // SAFETY: `ctx` points at executor-owned wake state that outlives
2152 // the session callback installation and is cleared on executor drop.
2153 unsafe {
2154 self.session
2155 .set_wake_callback(Some(nros_rmw_runtime_wake_cb), ctx);
2156 }
2157 if self.session.supports_wake_callback() {
2158 self.has_async_wake = true;
2159 }
2160 }
2161
2162 /// Phase 124.B.1 — install the wake callback onto an extra
2163 /// session opened by `node_builder.rmw(...)`. Called from
2164 /// `NodeBuilder::build()` right after `extra_sessions.push(...)`.
2165 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
2166 pub(crate) fn install_wake_signal_on_extra(&mut self, idx: usize) {
2167 use nros_rmw::Session as _;
2168 let ctx = self.wake_ctx_ptr();
2169 if let Some(s) = self.extra_sessions.get_mut(idx) {
2170 // SAFETY: same executor-owned wake state as the primary session;
2171 // the extra session is owned by this executor.
2172 unsafe {
2173 s.set_wake_callback(Some(nros_rmw_runtime_wake_cb), ctx);
2174 }
2175 if s.supports_wake_callback() {
2176 self.has_async_wake = true;
2177 }
2178 }
2179 }
2180
2181 /// Phase 124.B.2 — opaque context pointer the runtime wake
2182 /// callback receives. Encodes `(flag, mu, cv)` as a borrowed
2183 /// `&WakeCtx` reference; the callback decodes via
2184 /// `*const WakeCtx`.
2185 ///
2186 /// Lifetime: tied to the Executor instance. WakeCtx storage
2187 /// lives inside Executor (lazy-allocated on first install), so
2188 /// the pointer stays valid as long as the Executor is.
2189 /// Phase 124.B.7.c — POSIX signal-handler-safe wake fd.
2190 ///
2191 /// Returns a Linux `eventfd` that callers (typically POSIX
2192 /// signal handlers) can `write(fd, &1u64, 8)` to from any
2193 /// context, including signal handlers. A runtime-owned worker
2194 /// thread reads the fd and signals `wake_cv`, unblocking
2195 /// `spin_once`.
2196 ///
2197 /// The worker thread is spawned lazily on first call and
2198 /// joined on Executor drop. Linux-only and gated behind
2199 /// `feature = "signal-fd-wake"`; binaries that don't install
2200 /// signal handlers shouldn't enable it.
2201 ///
2202 /// Returns the raw fd. The Executor retains ownership; do not
2203 /// `close()` it from the caller.
2204 #[cfg(all(feature = "signal-fd-wake", feature = "rmw-cffi", target_os = "linux"))]
2205 pub fn signal_fd(&mut self) -> std::io::Result<core::ffi::c_int> {
2206 let ctx_ptr = self.wake_ctx_ptr() as *const WakeCtx;
2207 if self.signal_fd.is_none() {
2208 self.signal_fd = Some(WakeSignalFd::new(ctx_ptr)?);
2209 }
2210 Ok(self.signal_fd.as_ref().expect("just set").fd())
2211 }
2212
2213 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
2214 fn wake_ctx_ptr(&mut self) -> *mut core::ffi::c_void {
2215 if self.wake_ctx.is_none() {
2216 self.wake_ctx = Some(std::sync::Arc::new(WakeCtx {
2217 flag: std::sync::Arc::clone(&self.wake_flag),
2218 cv: std::sync::Arc::clone(&self.wake_cv),
2219 mu: std::sync::Arc::clone(&self.wake_mu),
2220 node_wake: self.node_wake.as_ref().map(std::sync::Arc::clone),
2221 }));
2222 }
2223 let arc = self.wake_ctx.as_ref().expect("just set");
2224 std::sync::Arc::as_ptr(arc) as *mut core::ffi::c_void
2225 }
2226
2227 // Phase 141.A.3 — alloc-mode (no_std RTOS) mirror of
2228 // `install_wake_signal_on_primary` /
2229 // `install_wake_signal_on_extra` / `wake_ctx_ptr`. Same
2230 // best-effort install contract: backends that don't override
2231 // `Session::set_wake_callback` ignore the call and continue
2232 // to be drained on the executor's deadline-bound wait.
2233 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
2234 fn wake_ctx_alloc_ptr(&mut self) -> Option<*mut core::ffi::c_void> {
2235 // Without a NodeWake there's no kernel primitive to signal;
2236 // skip the install + let spin_once fall back to drive_io
2237 // for the full timeout. Mirrors the std-RTOS path's
2238 // `if let Some(wake) = self.node_wake.as_ref()` predicate.
2239 let node_wake = self.node_wake_alloc.as_ref()?;
2240 if self.wake_ctx_alloc.is_none() {
2241 self.wake_ctx_alloc = Some(portable_atomic_util::Arc::new(
2242 super::wake_alloc::WakeCtxAlloc {
2243 flag: portable_atomic_util::Arc::clone(&self.wake_flag_alloc),
2244 node_wake: portable_atomic_util::Arc::clone(node_wake),
2245 },
2246 ));
2247 }
2248 let arc = self.wake_ctx_alloc.as_ref().expect("just set");
2249 Some(portable_atomic_util::Arc::as_ptr(arc) as *mut core::ffi::c_void)
2250 }
2251
2252 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
2253 fn install_wake_signal_on_primary_alloc(&mut self) {
2254 use nros_rmw::Session as _;
2255 let Some(ctx) = self.wake_ctx_alloc_ptr() else {
2256 return;
2257 };
2258 // SAFETY: `ctx` is the raw pointer of an Arc<WakeCtxAlloc>
2259 // owned by the Executor (`self.wake_ctx_alloc`); the Arc
2260 // lives as long as the Executor and is cleared on drop.
2261 unsafe {
2262 self.session
2263 .set_wake_callback(Some(super::wake_alloc::nros_rmw_runtime_wake_cb), ctx);
2264 }
2265 if self.session.supports_wake_callback() {
2266 self.has_async_wake_alloc = true;
2267 }
2268 }
2269
2270 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
2271 pub(crate) fn install_wake_signal_on_extra_alloc(&mut self, idx: usize) {
2272 use nros_rmw::Session as _;
2273 let Some(ctx) = self.wake_ctx_alloc_ptr() else {
2274 return;
2275 };
2276 if let Some(s) = self.extra_sessions.get_mut(idx) {
2277 // SAFETY: same wake state as the primary session; the
2278 // extra session is owned by this Executor.
2279 unsafe {
2280 s.set_wake_callback(Some(super::wake_alloc::nros_rmw_runtime_wake_cb), ctx);
2281 }
2282 if s.supports_wake_callback() {
2283 self.has_async_wake_alloc = true;
2284 }
2285 }
2286 }
2287
2288 /// Phase 104.C.4 — apply a Node's default SchedContext to a
2289 /// freshly-registered handle. Called from every `_inner`
2290 /// register variant after the entry slot is committed. No-op
2291 /// when `node_id` is None (legacy path), when the Node is
2292 /// out of range, or when the Node's `default_sched` is the
2293 /// auto-created Fifo slot (0) which matches the executor's
2294 /// default binding already.
2295 ///
2296 /// Phase 273 (RFC-0047) — extends with an optional `group` name.
2297 /// Precedence: **group table > node default > no binding** (SC 0).
2298 /// When `group` is `Some(g)`, consults `group_sched_table` first;
2299 /// if no entry exists for `(name, namespace, g)` falls back to the
2300 /// node's `default_sched`. When `group` is `None` the group table
2301 /// is not consulted (unchanged phase-272 path).
2302 ///
2303 /// Handles can still override per-call via
2304 /// `bind_handle_to_sched_context(handle, sc_id)` post-register.
2305 pub(crate) fn apply_node_default_sched(
2306 &mut self,
2307 slot: usize,
2308 node_id: Option<super::node_record::NodeId>,
2309 group: Option<&str>,
2310 ) {
2311 let Some(id) = node_id else { return };
2312 // Copy name, namespace, and default_sched out so the borrow on
2313 // `self.nodes` is released before the immutable `lookup_group_sched`
2314 // borrow and the mutable `sched_context_bindings` write below.
2315 let (name, namespace, node_sc) = {
2316 let Some(rec) = self.nodes.get(id.index()) else {
2317 return;
2318 };
2319 (rec.name.clone(), rec.namespace.clone(), rec.default_sched)
2320 };
2321 // Phase 273: group table > node default.
2322 let sc = match group {
2323 Some(g) => self
2324 .lookup_group_sched(name.as_str(), namespace.as_str(), g)
2325 .unwrap_or(node_sc),
2326 None => node_sc,
2327 };
2328 if sc.0 == 0 {
2329 return;
2330 }
2331 if slot >= self.entries.len() {
2332 return;
2333 }
2334 let sc_idx = sc.0 as usize;
2335 if sc_idx >= self.sched_contexts.len() || self.sched_contexts[sc_idx].is_none() {
2336 return;
2337 }
2338 self.sched_context_bindings[slot] = sc;
2339 }
2340
2341 /// Phase 104.C.3.2 — scoped Node-handle access. The closure
2342 /// receives a [`Node`] bound to the requested [`NodeId`]'s
2343 /// session + identity. Use the standard `Node::create_publisher`,
2344 /// `create_subscription`, etc. APIs inside.
2345 ///
2346 /// rclcpp-aligned bridge pattern:
2347 ///
2348 /// ```ignore
2349 /// let node_in = exec.node_builder("ingress").rmw("zenoh").build()?;
2350 /// let node_out = exec.node_builder("egress").rmw("xrce").build()?;
2351 ///
2352 /// let pub_out = exec.with_node(node_out, |n| {
2353 /// n.create_publisher::<Int32>("/fwd")
2354 /// })??;
2355 ///
2356 /// exec.with_node(node_in, |n| {
2357 /// n.create_subscription_buffered::<Int32, _, 1024>(
2358 /// "/src", qos(), move |m| { let _ = pub_out.publish(m); }
2359 /// )
2360 /// })??;
2361 /// ```
2362 ///
2363 /// The closure can return any type; double-`?` unwraps the
2364 /// outer `Result<R, NodeError>` from `with_node` and the inner
2365 /// result returned by the closure.
2366 /// Phase 104.C.3.3.d — flat-Result variant of
2367 /// [`with_node`](Self::with_node). When the closure already
2368 /// returns `Result<R, NodeError>`, this avoids the double-`?`:
2369 ///
2370 /// ```ignore
2371 /// // Without `with_node_try`:
2372 /// let pub_ = exec.with_node(id, |n| n.create_publisher(...))??;
2373 ///
2374 /// // With `with_node_try`:
2375 /// let pub_ = exec.with_node_try(id, |n| n.create_publisher(...))?;
2376 /// ```
2377 pub fn with_node_try<R>(
2378 &mut self,
2379 id: super::node_record::NodeId,
2380 f: impl FnOnce(&mut NodeHandle<'_>) -> Result<R, NodeError>,
2381 ) -> Result<R, NodeError> {
2382 self.with_node(id, f)?
2383 }
2384
2385 pub fn with_node<R>(
2386 &mut self,
2387 id: super::node_record::NodeId,
2388 f: impl FnOnce(&mut NodeHandle<'_>) -> R,
2389 ) -> Result<R, NodeError> {
2390 let (name, ns, session_idx) = {
2391 let r = self
2392 .nodes
2393 .get(id.index())
2394 .ok_or(NodeError::InvalidSchedContextBinding)?;
2395 (r.name.clone(), r.namespace.clone(), r.session_idx)
2396 };
2397 let monitors = self.monitor_table;
2398 let age_monitors = self.age_table;
2399 let epoch = self.epoch_us_fn;
2400 let session = self
2401 .session_at_mut(session_idx)
2402 .ok_or(NodeError::BackendMismatch)?;
2403 // SAFETY: short-lived scoped reference. `Node::new` takes
2404 // `&mut ConcreteSession`; lifetime is bound to this fn's
2405 // body via the closure's borrow of `node`.
2406 let mut node = NodeHandle::new(name, ns, session, 0);
2407 // RFC-0052 W3b.4/.5 — seed the baked monitor tables so contracted
2408 // publishers/subscribers attach their cells without entry glue.
2409 node.set_monitors(monitors);
2410 node.set_age_monitors(age_monitors, epoch);
2411 Ok(f(&mut node))
2412 }
2413
2414 /// Find a registered executor node by final name and namespace.
2415 pub fn node_id_by_name(
2416 &self,
2417 name: &str,
2418 namespace: &str,
2419 ) -> Option<super::node_record::NodeId> {
2420 self.nodes
2421 .iter()
2422 .enumerate()
2423 .find(|(_, node)| node.name.as_str() == name && node.namespace.as_str() == namespace)
2424 .map(|(index, _)| super::node_record::NodeId::from_raw(index as u8))
2425 }
2426
2427 /// Create a node on this executor.
2428 pub fn create_node(&mut self, name: &str) -> Result<NodeHandle<'_>, NodeError> {
2429 if name.len() > 64 {
2430 return Err(NodeError::NameTooLong);
2431 }
2432
2433 let mut node_name = heapless::String::<64>::new();
2434 node_name
2435 .push_str(name)
2436 .map_err(|_| NodeError::NameTooLong)?;
2437
2438 let mut node = NodeHandle::new(node_name, self.namespace.clone(), &mut self.session, 0);
2439 node.set_monitors(self.monitor_table);
2440 node.set_age_monitors(self.age_table, self.epoch_us_fn);
2441 Ok(node)
2442 }
2443
2444 /// Phase 128.F.2 — bridge-mode node factory. Registers a Node
2445 /// bound to the named RMW backend by opening (or reusing) an
2446 /// extra session via `node_builder().rmw(rmw).build()`, then
2447 /// returns a [`Node`] borrowing that session. Use when the
2448 /// binary intentionally links more than one backend and a Node
2449 /// must speak a specific one.
2450 ///
2451 /// The single-backend common case should keep using
2452 /// [`create_node`](Self::create_node) — this entry costs an
2453 /// extra session lookup and serves no purpose when only one
2454 /// backend is registered.
2455 #[cfg(feature = "rmw-cffi")]
2456 pub fn create_node_on(&mut self, name: &str, rmw: &str) -> Result<NodeHandle<'_>, NodeError> {
2457 self.create_node_on_with_domain(name, rmw, None, None)
2458 }
2459
2460 /// Like [`create_node_on`](Self::create_node_on) but pins the extra
2461 /// session's domain id. Required for a multi-domain config-driven bridge:
2462 /// an extra RMW session's participant domain follows the **node builder's**
2463 /// `domain_id` (`resolve_session_slot` → `domain_id.unwrap_or(0)`), NOT the
2464 /// `SessionSpec`'s — so without this an egress on a non-zero domain silently
2465 /// opens on domain 0 and never matches its receiver (phase-267 issue 0109).
2466 /// `None` domain preserves the legacy domain-0 default. `locator` pins the
2467 /// extra session's address — REQUIRED for an agent-based backend (xrce: the
2468 /// Micro-XRCE-DDS Agent addr) whose session can't be opened locator-less;
2469 /// `None` keeps the rmw-default (cyclonedds is domain-discovered, no locator).
2470 pub fn create_node_on_with_domain(
2471 &mut self,
2472 name: &str,
2473 rmw: &str,
2474 domain_id: Option<u32>,
2475 locator: Option<&str>,
2476 ) -> Result<NodeHandle<'_>, NodeError> {
2477 if name.len() > 64 {
2478 return Err(NodeError::NameTooLong);
2479 }
2480 // Reuse an existing Node of this name rather than growing the node table
2481 // (phase-267 non-flat): a config-driven bridge calls this once per bridge
2482 // ENDPOINT, and the same session node (`s0`/`s1`) recurs across every
2483 // `[[bridge]]`. Without dedup, N bridges push 2N records and overflow
2484 // `MAX_NODES`. Names are unique per session in a generated bridge config,
2485 // so matching by name is unambiguous.
2486 let session_idx = if let Some(rec) = self.nodes.iter().find(|n| n.name.as_str() == name) {
2487 rec.session_idx
2488 } else {
2489 // Register the Node (opens an extra session under `rmw` if
2490 // none exists yet for that backend).
2491 let mut builder = self.node_builder(name).rmw(rmw);
2492 if let Some(d) = domain_id {
2493 builder = builder.domain_id(d);
2494 }
2495 if let Some(loc) = locator {
2496 builder = builder.locator(loc);
2497 }
2498 let id = builder.build()?;
2499 self.node(id).ok_or(NodeError::NodeTableFull)?.session_idx
2500 };
2501
2502 let mut node_name = heapless::String::<64>::new();
2503 node_name
2504 .push_str(name)
2505 .map_err(|_| NodeError::NameTooLong)?;
2506 let namespace = self.namespace.clone();
2507 let monitors = self.monitor_table;
2508 let age_monitors = self.age_table;
2509 let epoch = self.epoch_us_fn;
2510 let session = self
2511 .session_at_mut(session_idx)
2512 .ok_or(NodeError::NodeTableFull)?;
2513 let mut node = NodeHandle::new(node_name, namespace, session, 0);
2514 node.set_monitors(monitors);
2515 node.set_age_monitors(age_monitors, epoch);
2516 Ok(node)
2517 }
2518
2519 /// Drive transport I/O (poll network, dispatch callbacks).
2520 #[allow(dead_code)]
2521 pub(crate) fn drive_io(&mut self, timeout_ms: i32) -> Result<(), NodeError> {
2522 self.session
2523 .drive_io(timeout_ms)
2524 .map_err(|_| NodeError::Transport(TransportError::PollFailed))
2525 }
2526
2527 /// Close the underlying session.
2528 pub fn close(&mut self) -> Result<(), NodeError> {
2529 self.session
2530 .close()
2531 .map_err(|_| NodeError::Transport(TransportError::ConnectionFailed))
2532 }
2533
2534 /// Phase 216 follow-up — register a per-Node dispatch trampoline.
2535 ///
2536 /// The board-side Entry pkg (or the macro-emitted
2537 /// `register_dispatch(executor)` wrapper, once wired) calls this
2538 /// once per deployed Node pkg, handing in the
2539 /// `__nros_node_<pkg>_on_callback` symbol + the Node's per-pkg
2540 /// `state` blob. [`Executor::dispatch_callback`] then linear-scans
2541 /// the registered slots when the dispatch task hands off a
2542 /// `SignaledCallback`.
2543 ///
2544 /// Returns `Err(())` when the registry is full (`MAX_NODES`
2545 /// entries — raise via `NROS_EXECUTOR_MAX_NODES` at build time).
2546 ///
2547 /// # Safety
2548 ///
2549 /// `state` must outlive the executor (the typical shape is a
2550 /// `*mut State` produced by
2551 /// `nros::__private_node_state_into_raw` from the
2552 /// macro-emitted `i()`; that pointer's lifetime IS the
2553 /// `Executor`'s by construction). `on_callback` must be safe to
2554 /// invoke with `(state, cb_id_ptr, cb_id_len, ctx)` matching the
2555 /// per-Node `__nros_node_<pkg>_on_callback` ABI emitted by the
2556 /// `nros::node!()` macro (Phase 216.A.5).
2557 #[allow(clippy::result_unit_err)]
2558 pub fn register_dispatch_slot(
2559 &mut self,
2560 state: *mut core::ffi::c_void,
2561 on_callback: unsafe extern "C" fn(
2562 *mut core::ffi::c_void,
2563 *const u8,
2564 usize,
2565 *mut core::ffi::c_void,
2566 ),
2567 ) -> Result<(), ()> {
2568 self.dispatch_slots
2569 .push(DispatchSlot { state, on_callback })
2570 .map_err(|_| ())
2571 }
2572
2573 /// Phase 216 follow-up — current registered dispatch-slot count.
2574 /// Diagnostic / test surface.
2575 pub fn dispatch_slot_count(&self) -> usize {
2576 self.dispatch_slots.len()
2577 }
2578
2579 /// Phase 258 (Track 2, 2a) — enroll a component into the executor-owned
2580 /// tick registry. Called by `nros`'s `install`/`register_node_borrowed`
2581 /// after it builds the `Arc<ComponentCell>`: `state` is the leaked
2582 /// `Arc<ComponentCell>` (the slot takes ownership), `tick`/`drop` are the
2583 /// `nros`-side trampolines (see [`ComponentSlot`]). The slot's `tick`
2584 /// runs at the tail of every [`spin_once`](Self::spin_once); its `drop`
2585 /// runs once on `Executor::drop`.
2586 ///
2587 /// Returns `Err(())` when the registry is full (`MAX_NODES` — raise via
2588 /// `NROS_EXECUTOR_MAX_NODES` at build time). On error the caller still
2589 /// owns `state` (the slot was not stored) and must drop it.
2590 ///
2591 /// # Safety
2592 /// `state` must be a `*mut` produced by leaking the component cell the
2593 /// `tick`/`drop` trampolines expect (an `Arc<ComponentCell>` via
2594 /// `Arc::into_raw` in the canonical `nros` caller), and must remain valid
2595 /// until the matching `drop` runs. `tick` must be safe to invoke with
2596 /// `(state, exec_ctx = *mut Executor)` each spin; `drop` must be safe to
2597 /// invoke exactly once with `state`.
2598 #[allow(clippy::result_unit_err)]
2599 pub unsafe fn enroll_component(
2600 &mut self,
2601 state: *mut core::ffi::c_void,
2602 tick: unsafe extern "C" fn(*mut core::ffi::c_void, *mut core::ffi::c_void),
2603 drop: unsafe extern "C" fn(*mut core::ffi::c_void),
2604 ) -> Result<(), ()> {
2605 self.component_slots
2606 .push(ComponentSlot { state, tick, drop })
2607 .map_err(|_| ())
2608 }
2609
2610 /// Phase 258 (Track 2, 2a) — current enrolled component-slot count.
2611 /// Diagnostic / test surface.
2612 pub fn component_slot_count(&self) -> usize {
2613 self.component_slots.len()
2614 }
2615
2616 /// issue #140 — the enrolled components' opaque `state` pointers, in enroll
2617 /// order. Each is the *leaked* `Arc<ComponentCell>` `enroll_component` was
2618 /// handed (see [`ComponentSlot::state`]); the `nros` layer re-borrows them
2619 /// to fold per-component dispatch counters into
2620 /// `observed_callback_counts` — install-seam components
2621 /// (`register_node_borrowed`) live ONLY here, not in
2622 /// `ExecutorNodeRuntime::components`, so the hosted-spin counts read zero
2623 /// without this surface.
2624 pub fn enrolled_component_states(&self) -> impl Iterator<Item = *mut core::ffi::c_void> + '_ {
2625 self.component_slots.iter().map(|slot| slot.state)
2626 }
2627
2628 /// Phase 216 final dispatch hook — stable entry point the
2629 /// framework's dispatch task (RTIC `__nros_run` /
2630 /// Embassy `__nros_run_task`) calls for each `SignaledCallback`
2631 /// envelope it dequeues from the board-side SPSC / Embassy
2632 /// channel.
2633 ///
2634 /// ## Signature shape
2635 ///
2636 /// `nros-node` sits below `nros` in the dep graph, so the typed
2637 /// `nros::CallbackId<'_>` / `nros::CallbackCtx<'_>` types
2638 /// referenced in the Phase 216 design notes cannot appear in the
2639 /// signature here. The macro emit translates the dequeued
2640 /// envelope to the layer-clean `(cb_id: &str, ctx: *mut c_void)`
2641 /// pair before calling this method; the per-Node `on_callback`
2642 /// trampoline ABI (Phase 216.A.5,
2643 /// `__nros_node_<pkg>_on_callback(state, cb_id_ptr, cb_id_len,
2644 /// ctx)`) uses the same untyped shape on the other side of the
2645 /// fence, so the round-trip stays type-consistent.
2646 ///
2647 /// ## Body — linear scan of the dispatch registry
2648 ///
2649 /// Each registered [`DispatchSlot`] holds an
2650 /// `__nros_node_<pkg>_on_callback` fn pointer + the owning Node's
2651 /// `state` blob. The macro-emitted trampoline body
2652 /// `match`es on `CallbackId` tags the Node declared and is a
2653 /// no-op for non-matching `cb_id`s — at most one Node per
2654 /// `cb_id` actually acts, the rest are cheap string-compare
2655 /// no-ops. This mirrors the strategy
2656 /// `ExecutorNodeRuntime::dispatch_callback` uses in
2657 /// `packages/core/nros/src/node_runtime.rs:470`.
2658 ///
2659 /// ## What's NOT auto-wired today
2660 ///
2661 /// The `nros::node!()` macro doesn't yet emit a
2662 /// `register_dispatch(executor)` wrapper that pushes the per-pkg
2663 /// `(state, on_callback)` into this registry. Until that wiring
2664 /// lands (Phase 216 follow-up — see commit msg), downstream
2665 /// consumers (board's `init_hardware`, or the codegen-emitted
2666 /// `run_plan`) must call
2667 /// [`Executor::register_dispatch_slot`] explicitly with the
2668 /// `__nros_node_<pkg>_on_callback` symbol + a `state` blob from
2669 /// the macro-emitted `i()`.
2670 //
2671 // `ctx` is an opaque FFI cookie forwarded verbatim to each slot's
2672 // `on_callback`; this fn never dereferences it (the registered callback
2673 // does, under the `register_dispatch_slot` safety contract), so it is sound
2674 // to call from safe code.
2675 #[allow(clippy::not_unsafe_ptr_arg_deref)]
2676 pub fn dispatch_callback(&mut self, cb_id: &str, ctx: *mut core::ffi::c_void) {
2677 let cb_id_ptr = cb_id.as_ptr();
2678 let cb_id_len = cb_id.len();
2679 // Snapshot pointer + length to avoid an outstanding borrow
2680 // across the unsafe fn calls below; each `DispatchSlot` is
2681 // `Copy`, so iterating by value sidesteps any aliasing
2682 // worry the borrow checker would flag if a slot's
2683 // `on_callback` re-entered the executor.
2684 for slot in self.dispatch_slots.iter().copied() {
2685 // SAFETY: caller of `register_dispatch_slot` guaranteed
2686 // `state` outlives the executor + `on_callback` matches
2687 // the per-Node `__nros_node_<pkg>_on_callback` ABI;
2688 // `cb_id_ptr`/`cb_id_len` describe the live `&str` the
2689 // caller passed in.
2690 unsafe {
2691 (slot.on_callback)(slot.state, cb_id_ptr, cb_id_len, ctx);
2692 }
2693 }
2694 }
2695
2696 /// Get a reference to the underlying session.
2697 pub fn session(&self) -> &session::ConcreteSession {
2698 &self.session
2699 }
2700
2701 /// Get a mutable reference to the underlying session.
2702 pub fn session_mut(&mut self) -> &mut session::ConcreteSession {
2703 &mut self.session
2704 }
2705
2706 /// Phase 124.F.3 — session-level connectivity probe. Wire-level
2707 /// round-trip "is the peer / agent / router still reachable?"
2708 /// — cheaper than the service-availability probe (no discovery
2709 /// state required).
2710 ///
2711 /// Returns `Ok(())` on reply within `timeout_ms`,
2712 /// `Err(NodeError::Transport(Timeout))` on no reply,
2713 /// `Err(NodeError::Transport(Unsupported))` when the active
2714 /// backend can't probe.
2715 ///
2716 /// Mirrors micro-ROS's `rmw_uros_ping_agent`. Useful for
2717 /// reconnect-on-link-loss patterns: bare-metal code can call
2718 /// `ping(100)` periodically and tear down / re-open the session
2719 /// on timeout.
2720 pub fn ping(&mut self, timeout_ms: i32) -> Result<(), NodeError> {
2721 use nros_rmw::Session;
2722 self.session
2723 .ping_session(timeout_ms)
2724 .map_err(NodeError::Transport)
2725 }
2726
2727 /// Get a mutable reference to an action client core in the arena by entry index.
2728 ///
2729 /// # Safety
2730 /// The caller must ensure that `entry_index` refers to an `ActionClientRawArenaEntry`.
2731 pub unsafe fn action_client_core_mut(
2732 &mut self,
2733 entry_index: usize,
2734 ) -> Option<&mut super::action_core::ActionClientCore> {
2735 let meta = self.entries.get(entry_index)?.as_ref()?;
2736 if !matches!(meta.kind, EntryKind::ActionClient) {
2737 return None;
2738 }
2739 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
2740 unsafe {
2741 let entry_ptr = arena_ptr.add(meta.offset)
2742 as *mut super::arena::ActionClientRawArenaEntry<
2743 { crate::config::DEFAULT_RX_BUF_SIZE },
2744 { crate::config::DEFAULT_RX_BUF_SIZE },
2745 { crate::config::DEFAULT_RX_BUF_SIZE },
2746 >;
2747 Some(&mut (*entry_ptr).core)
2748 }
2749 }
2750
2751 /// Get a mutable reference to a service-client arena entry (Phase 82).
2752 ///
2753 /// Returns `None` if `entry_index` doesn't refer to a service client
2754 /// entry. The default reply buffer size is assumed because the C API
2755 /// always uses the default — the entry was registered via
2756 /// `register_service_client_raw_sized::<DEFAULT_RX_BUF_SIZE>`.
2757 ///
2758 /// # Safety
2759 /// `entry_index` must refer to a `ServiceClientRawArenaEntry`.
2760 pub unsafe fn service_client_entry_mut(
2761 &mut self,
2762 entry_index: usize,
2763 ) -> Option<&mut super::arena::ServiceClientRawArenaEntry<{ crate::config::DEFAULT_RX_BUF_SIZE }>>
2764 {
2765 let meta = self.entries.get(entry_index)?.as_ref()?;
2766 if !matches!(meta.kind, EntryKind::ServiceClient) {
2767 return None;
2768 }
2769 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
2770 unsafe {
2771 let entry_ptr = arena_ptr.add(meta.offset)
2772 as *mut super::arena::ServiceClientRawArenaEntry<
2773 { crate::config::DEFAULT_RX_BUF_SIZE },
2774 >;
2775 Some(&mut *entry_ptr)
2776 }
2777 }
2778
2779 /// Set the executor-level trigger condition.
2780 ///
2781 /// Controls which handles must be ready before `spin_once` dispatches
2782 /// callbacks. Defaults to [`Trigger::AnyReady`](crate::Trigger).
2783 pub fn set_trigger(&mut self, trigger: Trigger) {
2784 self.trigger = trigger;
2785 }
2786
2787 /// Set the executor data communication semantics.
2788 ///
2789 /// Choose between `Direct` (process in place) and `LET`
2790 /// (snapshot-then-process) semantics. See [`ExecutorSemantics`].
2791 pub fn set_semantics(&mut self, semantics: ExecutorSemantics) {
2792 self.semantics = semantics;
2793 }
2794
2795 /// Set the invocation mode for a specific handle.
2796 ///
2797 /// Controls whether the callback fires on every spin
2798 /// ([`Always`](InvocationMode::Always)) or only when new data
2799 /// arrives ([`OnNewData`](InvocationMode::OnNewData), the default).
2800 pub fn set_invocation(&mut self, id: HandleId, mode: InvocationMode) {
2801 if let Some(Some(meta)) = self.entries.get_mut(id.0) {
2802 meta.invocation = mode;
2803 }
2804 }
2805
2806 // ========================================================================
2807 // Arena-based callback registration
2808 // ========================================================================
2809
2810 /// Bump-allocate space for `T` in the arena. Returns the byte offset.
2811 pub(crate) fn arena_alloc<T>(&mut self) -> Result<usize, NodeError> {
2812 let align = core::mem::align_of::<T>();
2813 let size = core::mem::size_of::<T>();
2814 let aligned_offset = (self.arena_used + align - 1) & !(align - 1);
2815 let new_used = aligned_offset + size;
2816 if new_used > self.arena.len() {
2817 return Err(NodeError::BufferTooSmall);
2818 }
2819 self.arena_used = new_used;
2820 Ok(aligned_offset)
2821 }
2822
2823 /// Bump-allocate space for `T` plus `trailing_bytes` extra bytes.
2824 ///
2825 /// Returns `(entry_offset, trailing_offset)`. The trailing region starts
2826 /// immediately after `T` (aligned to 8 bytes).
2827 pub(crate) fn arena_alloc_with_trailing<T>(
2828 &mut self,
2829 trailing_bytes: usize,
2830 ) -> Result<(usize, usize), NodeError> {
2831 let align = core::mem::align_of::<T>();
2832 let entry_size = core::mem::size_of::<T>();
2833 let entry_offset = self.arena_used.next_multiple_of(align);
2834 // Trailing region starts on an 8-byte (u64) boundary after the entry.
2835 let trailing_offset =
2836 (entry_offset + entry_size).next_multiple_of(core::mem::align_of::<u64>());
2837 let new_used = trailing_offset + trailing_bytes;
2838 if new_used > self.arena.len() {
2839 return Err(NodeError::BufferTooSmall);
2840 }
2841 self.arena_used = new_used;
2842 Ok((entry_offset, trailing_offset))
2843 }
2844
2845 /// Find the next free entry slot index.
2846 pub(crate) fn next_entry_slot(&self) -> Result<usize, NodeError> {
2847 self.entries
2848 .iter()
2849 .position(|e| e.is_none())
2850 // Issue 0095 — the callback-entry table (`NROS_EXECUTOR_MAX_CBS`,
2851 // default 4) is full. Distinct from `BufferTooSmall` so the register
2852 // seam can tell the user to raise the knob.
2853 .ok_or(NodeError::ExecutorFull)
2854 }
2855
2856 /// Typed buffered subscription core (the `node_mut(id).subscription(t)
2857 /// .typed::<M>()` builder lowers here). Routes the typed subscription
2858 /// through the [`NodeId`]'s session + identity (rclcpp `add_node` pattern).
2859 pub(crate) fn register_subscription_buffered_on<M, F, const RX_BUF: usize>(
2860 &mut self,
2861 node_id: super::node_record::NodeId,
2862 topic_name: &str,
2863 qos: QosSettings,
2864 callback: F,
2865 group: Option<&str>,
2866 ) -> Result<HandleId, NodeError>
2867 where
2868 M: crate::rmw_type_registry::MessageForRmw + 'static,
2869 F: FnMut(&M) + 'static,
2870 {
2871 type Entry<M, F> = SubBufferedEntry<M, F>;
2872
2873 // Phase 212.K.7.6.b — see `create_publisher_on`.
2874 crate::rmw_type_registry::register_type::<M>()?;
2875
2876 let slot = self.next_entry_slot()?;
2877 let (node_name, ns, session_idx) = {
2878 let r = self
2879 .nodes
2880 .get(node_id.index())
2881 .ok_or(NodeError::InvalidSchedContextBinding)?;
2882 (r.name.clone(), r.namespace.clone(), r.session_idx)
2883 };
2884 let mut topic = TopicInfo::new(
2885 topic_name,
2886 <M as RosMessage>::TYPE_NAME,
2887 <M as RosMessage>::TYPE_HASH,
2888 )
2889 .with_namespace(&ns)
2890 // Phase 231 (RFC-0038) — hand the backend the receive-buffer size so it
2891 // can size-class its receive storage (zenoh-pico: small vs large).
2892 .with_rx_buffer_hint(RX_BUF);
2893 if !node_name.is_empty() {
2894 topic = topic.with_node_name(&node_name);
2895 }
2896 // W3b.5 — contracted-endpoint age hook (None = free).
2897 let age_mon = self.age_lookup::<M>(topic_name);
2898 let handle = {
2899 let session = self
2900 .session_at_mut(session_idx)
2901 .ok_or(NodeError::BackendMismatch)?;
2902 session
2903 .create_subscriber(&topic, qos)
2904 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
2905 };
2906
2907 // Phase 231 Wave 0.2 (RFC-0038) — in-place dispatch when the backend
2908 // advertises it: deserialize straight from the borrowed receive slot,
2909 // no arena buffer (copy #1 removed). Else the buffered path below.
2910 {
2911 use nros_rmw::Subscriber as _;
2912 if handle.supports_process_in_place() {
2913 let entry_offset = self.arena_alloc::<SubInplaceEntry<M, F>>()?;
2914 unsafe {
2915 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
2916 let entry_ptr = arena_ptr.add(entry_offset) as *mut SubInplaceEntry<M, F>;
2917 core::ptr::write(
2918 entry_ptr,
2919 SubInplaceEntry {
2920 handle,
2921 callback,
2922 age_mon,
2923 _phantom: PhantomData,
2924 },
2925 );
2926 }
2927 self.entries[slot] = Some(CallbackMeta {
2928 offset: entry_offset,
2929 kind: EntryKind::Subscription,
2930 try_process: sub_inplace_try_process::<M, F>,
2931 has_data: sub_inplace_has_data::<M, F>,
2932 pre_sample: no_pre_sample,
2933 invocation: InvocationMode::OnNewData,
2934 drop_fn: drop_entry::<SubInplaceEntry<M, F>>,
2935 });
2936 self.apply_node_default_sched(slot, Some(node_id), group);
2937 return Ok(HandleId(slot));
2938 }
2939 }
2940
2941 let (_slot_count, trailing_bytes) = buffered_region_size(qos.depth, RX_BUF);
2942
2943 let (entry_offset, trailing_offset) =
2944 self.arena_alloc_with_trailing::<Entry<M, F>>(trailing_bytes)?;
2945
2946 let buf_ptr = unsafe { (self.arena.as_mut_ptr() as *mut u8).add(trailing_offset) };
2947
2948 let buffer = if qos.depth <= 1 {
2949 BufferStrategy::Triple(unsafe { TripleBuffer::init(buf_ptr, RX_BUF) })
2950 } else {
2951 BufferStrategy::Ring(unsafe { SpscRing::init(buf_ptr, RX_BUF, qos.depth as usize) })
2952 };
2953
2954 unsafe {
2955 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
2956 let entry_ptr = arena_ptr.add(entry_offset) as *mut Entry<M, F>;
2957 core::ptr::write(
2958 entry_ptr,
2959 Entry {
2960 handle,
2961 buffer,
2962 callback,
2963 age_mon,
2964 _phantom: PhantomData,
2965 },
2966 );
2967 }
2968
2969 self.entries[slot] = Some(CallbackMeta {
2970 offset: entry_offset,
2971 kind: EntryKind::Subscription,
2972 try_process: sub_buffered_try_process::<M, F>,
2973 has_data: sub_buffered_has_data::<M, F>,
2974 pre_sample: no_pre_sample,
2975 invocation: InvocationMode::OnNewData,
2976 drop_fn: drop_entry::<Entry<M, F>>,
2977 });
2978 // Phase 104.C.4 — apply Node's default SchedContext.
2979 self.apply_node_default_sched(slot, Some(node_id), group);
2980 Ok(HandleId(slot))
2981 }
2982
2983 /// Generic (type-erased) buffered subscription core (the
2984 /// `node_mut(id).subscription(t).generic(ty, hash)` builder lowers here).
2985 /// Routes the subscriber creation through the [`NodeId`]'s
2986 /// session + identity (rclcpp `add_node` pattern).
2987 ///
2988 /// Use this in bridge code where two Nodes bind to different RMW
2989 /// backends:
2990 ///
2991 /// ```ignore
2992 /// let node_in = exec.node_builder("ingress").rmw("zenoh").build()?;
2993 /// let pub_out = exec.with_node(node_out, |n| {
2994 /// n.create_publisher_raw("/fwd", TYPE, HASH)
2995 /// })??;
2996 /// exec.register_subscription_buffered_raw_on::<_, 1024>(
2997 /// node_in, "/src", TYPE, HASH, qos(),
2998 /// move |bytes: &[u8]| { let _ = pub_out.publish_raw(bytes); },
2999 /// )?;
3000 /// ```
3001 pub(crate) fn register_subscription_buffered_raw_on<F, const RX_BUF: usize>(
3002 &mut self,
3003 node_id: super::node_record::NodeId,
3004 topic_name: &str,
3005 type_name: &str,
3006 type_hash: &str,
3007 qos: QosSettings,
3008 callback: F,
3009 ) -> Result<HandleId, NodeError>
3010 where
3011 F: FnMut(&[u8]) + 'static,
3012 {
3013 // Pull the Node's identity + session slot out first so the
3014 // mutable session borrow doesn't conflict with the arena
3015 // alloc inside `add_arena_subscription_callback`.
3016 let (node_name, ns, session_idx) = {
3017 let r = self
3018 .nodes
3019 .get(node_id.index())
3020 .ok_or(NodeError::InvalidSchedContextBinding)?;
3021 (r.name.clone(), r.namespace.clone(), r.session_idx)
3022 };
3023 let mut topic = TopicInfo::new(topic_name, type_name, type_hash).with_namespace(&ns);
3024 if !node_name.is_empty() {
3025 topic = topic.with_node_name(&node_name);
3026 }
3027 let handle = {
3028 let session = self
3029 .session_at_mut(session_idx)
3030 .ok_or(NodeError::BackendMismatch)?;
3031 session
3032 .create_subscriber(&topic, qos)
3033 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
3034 };
3035 let handle_id = self.add_arena_subscription_callback::<F, RX_BUF>(handle, qos, callback)?;
3036 // Phase 104.C.4 — apply Node's default SchedContext.
3037 self.apply_node_default_sched(handle_id.0, Some(node_id), None);
3038 Ok(handle_id)
3039 }
3040
3041 /// Register a borrowed (zero-copy) buffered subscription (Phase 229.6,
3042 /// issue 0007 / RFC-0033 `borrowed` mode).
3043 ///
3044 /// `B` is the code-generated borrowed-message marker (e.g. `ImageBorrow`)
3045 /// implementing [`BorrowedMessage`](nros_core::BorrowedMessage); the
3046 /// callback receives `&B::View<'a>` — a lifetime-carrying message whose
3047 /// unbounded sequence/string fields borrow directly from the receive buffer
3048 /// (no `heapless::Vec` copy). The view is valid only for the callback's
3049 /// duration.
3050 ///
3051 /// **Triple-buffer only.** A borrowed view must reference exactly one
3052 /// well-defined buffer slot for the callback's duration; an SPSC ring
3053 /// (`qos.depth > 1`) keeps several samples in flight with no single such
3054 /// slot. `qos.depth > 1` is therefore rejected with
3055 /// [`TransportError::Unsupported`].
3056 pub(crate) fn register_subscription_buffered_borrowed_on<B, F, const RX_BUF: usize>(
3057 &mut self,
3058 node_id: super::node_record::NodeId,
3059 topic_name: &str,
3060 qos: QosSettings,
3061 callback: F,
3062 ) -> Result<HandleId, NodeError>
3063 where
3064 B: nros_core::BorrowedMessage + 'static,
3065 F: for<'a> FnMut(&B::View<'a>) + 'static,
3066 {
3067 type Entry<B, F> = SubBufferedBorrowedEntry<B, F>;
3068
3069 // Borrowed views require a single well-defined slot (triple buffer).
3070 if qos.depth > 1 {
3071 return Err(NodeError::Transport(TransportError::Unsupported));
3072 }
3073
3074 let slot = self.next_entry_slot()?;
3075 let (node_name, ns, session_idx) = {
3076 let r = self
3077 .nodes
3078 .get(node_id.index())
3079 .ok_or(NodeError::InvalidSchedContextBinding)?;
3080 (r.name.clone(), r.namespace.clone(), r.session_idx)
3081 };
3082 let mut topic = TopicInfo::new(
3083 topic_name,
3084 <B as BorrowedMessage>::TYPE_NAME,
3085 <B as BorrowedMessage>::TYPE_HASH,
3086 )
3087 .with_namespace(&ns);
3088 if !node_name.is_empty() {
3089 topic = topic.with_node_name(&node_name);
3090 }
3091 let handle = {
3092 let session = self
3093 .session_at_mut(session_idx)
3094 .ok_or(NodeError::BackendMismatch)?;
3095 session
3096 .create_subscriber(&topic, qos)
3097 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
3098 };
3099
3100 let (_slot_count, trailing_bytes) = buffered_region_size(qos.depth, RX_BUF);
3101 let (entry_offset, trailing_offset) =
3102 self.arena_alloc_with_trailing::<Entry<B, F>>(trailing_bytes)?;
3103 let buf_ptr = unsafe { (self.arena.as_mut_ptr() as *mut u8).add(trailing_offset) };
3104
3105 // depth <= 1 guaranteed above → always triple buffer.
3106 let buffer = BufferStrategy::Triple(unsafe { TripleBuffer::init(buf_ptr, RX_BUF) });
3107
3108 unsafe {
3109 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3110 let entry_ptr = arena_ptr.add(entry_offset) as *mut Entry<B, F>;
3111 core::ptr::write(
3112 entry_ptr,
3113 Entry {
3114 handle,
3115 buffer,
3116 callback,
3117 _phantom: PhantomData,
3118 },
3119 );
3120 }
3121
3122 self.entries[slot] = Some(CallbackMeta {
3123 offset: entry_offset,
3124 kind: EntryKind::Subscription,
3125 try_process: sub_buffered_borrowed_try_process::<B, F>,
3126 has_data: sub_buffered_borrowed_has_data::<B, F>,
3127 pre_sample: no_pre_sample,
3128 invocation: InvocationMode::OnNewData,
3129 drop_fn: drop_entry::<Entry<B, F>>,
3130 });
3131 self.apply_node_default_sched(slot, Some(node_id), None);
3132 Ok(HandleId(slot))
3133 }
3134
3135 /// Register a raw (type-erased) buffered subscription whose callback
3136 /// also receives a [`RawMessageInfo`](nros_core::RawMessageInfo)
3137 /// carrying the sample's wire **attachment** (Phase 189.M1).
3138 ///
3139 /// Backs the `node.subscription(t).generic(..).message_info().build(cb)`
3140 /// builder — the cross-RMW bridge reads the `bridge_origin` tag from
3141 /// `info.attachment()` for echo suppression. One sample per
3142 /// `spin_once`; the attachment is staged in a flat per-entry buffer
3143 /// (cap [`RAW_INFO_ATT_CAP`](super::arena::RAW_INFO_ATT_CAP)).
3144 pub fn register_subscription_buffered_raw_info_on<F, const RX_BUF: usize>(
3145 &mut self,
3146 node_id: super::node_record::NodeId,
3147 topic_name: &str,
3148 type_name: &str,
3149 type_hash: &str,
3150 qos: QosSettings,
3151 callback: F,
3152 ) -> Result<HandleId, NodeError>
3153 where
3154 F: FnMut(&[u8], &nros_core::RawMessageInfo) + 'static,
3155 {
3156 type Entry<F, const N: usize> = SubBufferedRawInfoEntry<F, N>;
3157
3158 let slot = self.next_entry_slot()?;
3159 let (node_name, ns, session_idx) = {
3160 let r = self
3161 .nodes
3162 .get(node_id.index())
3163 .ok_or(NodeError::InvalidSchedContextBinding)?;
3164 (r.name.clone(), r.namespace.clone(), r.session_idx)
3165 };
3166 let mut topic = TopicInfo::new(topic_name, type_name, type_hash).with_namespace(&ns);
3167 if !node_name.is_empty() {
3168 topic = topic.with_node_name(&node_name);
3169 }
3170 let handle = {
3171 let session = self
3172 .session_at_mut(session_idx)
3173 .ok_or(NodeError::BackendMismatch)?;
3174 session
3175 .create_subscriber(&topic, qos)
3176 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
3177 };
3178
3179 let offset = self.arena_alloc::<Entry<F, RX_BUF>>()?;
3180 unsafe {
3181 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3182 let entry_ptr = arena_ptr.add(offset) as *mut Entry<F, RX_BUF>;
3183 core::ptr::write(
3184 entry_ptr,
3185 Entry {
3186 handle,
3187 buffer: [0u8; RX_BUF],
3188 att: [0u8; super::arena::RAW_INFO_ATT_CAP],
3189 callback,
3190 },
3191 );
3192 }
3193
3194 self.entries[slot] = Some(CallbackMeta {
3195 offset,
3196 kind: EntryKind::Subscription,
3197 try_process: sub_buffered_raw_info_try_process::<F, RX_BUF>,
3198 has_data: sub_buffered_raw_info_has_data::<F, RX_BUF>,
3199 pre_sample: no_pre_sample,
3200 invocation: InvocationMode::OnNewData,
3201 drop_fn: drop_entry::<Entry<F, RX_BUF>>,
3202 });
3203 self.apply_node_default_sched(slot, Some(node_id), None);
3204 Ok(HandleId(slot))
3205 }
3206
3207 /// Phase 250 (Wave 2) — register a generic (type-erased) raw subscription
3208 /// that surfaces E2E [`IntegrityStatus`](nros_rmw::IntegrityStatus) (CRC +
3209 /// sequence gap/dup) alongside the raw CDR bytes
3210 /// (`FnMut(&[u8], &IntegrityStatus)`). The type-erased analog of
3211 /// [`register_subscription_with_safety_sized_inner`]: the validator lives in
3212 /// the `RmwSubscriber` (`try_recv_validated`), so the subscriber is created
3213 /// plainly and no `register_type::<M>()` is needed (the declarative `Node`
3214 /// path is generic). Used by the declarative runtime's `.safety()` opt-in.
3215 #[cfg(feature = "safety-e2e")]
3216 pub fn register_subscription_buffered_raw_safety_on<F, const RX_BUF: usize>(
3217 &mut self,
3218 node_id: super::node_record::NodeId,
3219 topic_name: &str,
3220 type_name: &str,
3221 type_hash: &str,
3222 qos: QosSettings,
3223 callback: F,
3224 ) -> Result<HandleId, NodeError>
3225 where
3226 F: FnMut(&[u8], &nros_rmw::IntegrityStatus) + 'static,
3227 {
3228 use super::arena::{
3229 SubBufferedRawSafetyEntry, sub_buffered_raw_safety_has_data,
3230 sub_buffered_raw_safety_try_process,
3231 };
3232 type Entry<F, const N: usize> = SubBufferedRawSafetyEntry<F, N>;
3233
3234 let slot = self.next_entry_slot()?;
3235 let (node_name, ns, session_idx) = {
3236 let r = self
3237 .nodes
3238 .get(node_id.index())
3239 .ok_or(NodeError::InvalidSchedContextBinding)?;
3240 (r.name.clone(), r.namespace.clone(), r.session_idx)
3241 };
3242 let mut topic = TopicInfo::new(topic_name, type_name, type_hash).with_namespace(&ns);
3243 if !node_name.is_empty() {
3244 topic = topic.with_node_name(&node_name);
3245 }
3246 let handle = {
3247 let session = self
3248 .session_at_mut(session_idx)
3249 .ok_or(NodeError::BackendMismatch)?;
3250 session
3251 .create_subscriber(&topic, qos)
3252 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
3253 };
3254
3255 let offset = self.arena_alloc::<Entry<F, RX_BUF>>()?;
3256 unsafe {
3257 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3258 let entry_ptr = arena_ptr.add(offset) as *mut Entry<F, RX_BUF>;
3259 core::ptr::write(
3260 entry_ptr,
3261 Entry {
3262 handle,
3263 buffer: [0u8; RX_BUF],
3264 callback,
3265 },
3266 );
3267 }
3268
3269 self.entries[slot] = Some(CallbackMeta {
3270 offset,
3271 kind: EntryKind::Subscription,
3272 try_process: sub_buffered_raw_safety_try_process::<F, RX_BUF>,
3273 has_data: sub_buffered_raw_safety_has_data::<F, RX_BUF>,
3274 pre_sample: no_pre_sample,
3275 invocation: InvocationMode::OnNewData,
3276 drop_fn: drop_entry::<Entry<F, RX_BUF>>,
3277 });
3278 self.apply_node_default_sched(slot, Some(node_id), None);
3279 Ok(HandleId(slot))
3280 }
3281
3282 /// Register a raw byte-shaped callback against a pre-built
3283 /// `RmwSubscriber` handle.
3284 ///
3285 /// Backend-agnostic primitive — the caller is responsible for
3286 /// obtaining the handle by whatever route the active backend
3287 /// supports:
3288 ///
3289 /// - **Generic ROS-typed flow**: call `Session::create_subscriber`
3290 /// on `self.session_mut()` with a [`TopicInfo`]. The
3291 /// `node_mut(id).subscription(t).generic(ty, hash)` builder is the
3292 /// convenience wrapper for this path.
3293 /// - **Backend-specific flow** (e.g. uORB needs `&'static orb_metadata`):
3294 /// reach into the concrete session via [`Self::session_mut`] and
3295 /// call its backend-specific create method, then hand the handle
3296 /// here. `nros-px4::uorb::create_subscription_with_callback` is
3297 /// the example.
3298 ///
3299 /// The arena-store + vtable wiring is identical to
3300 /// `register_subscription_buffered_raw`; the only thing that varies is
3301 /// where the handle came from. Callback fires on every message
3302 /// delivery during [`spin_once`](Self::spin_once); bytes are
3303 /// passed as `&[u8]`.
3304 pub fn add_arena_subscription_callback<F, const RX_BUF: usize>(
3305 &mut self,
3306 handle: session::RmwSubscriber,
3307 qos: QosSettings,
3308 callback: F,
3309 ) -> Result<HandleId, NodeError>
3310 where
3311 F: FnMut(&[u8]) + 'static,
3312 {
3313 type Entry<F> = SubBufferedRawEntry<F>;
3314
3315 let slot = self.next_entry_slot()?;
3316 let (_slot_count, trailing_bytes) = buffered_region_size(qos.depth, RX_BUF);
3317
3318 let (entry_offset, trailing_offset) =
3319 self.arena_alloc_with_trailing::<Entry<F>>(trailing_bytes)?;
3320
3321 let buf_ptr = unsafe { (self.arena.as_mut_ptr() as *mut u8).add(trailing_offset) };
3322
3323 let buffer = if qos.depth <= 1 {
3324 BufferStrategy::Triple(unsafe { TripleBuffer::init(buf_ptr, RX_BUF) })
3325 } else {
3326 BufferStrategy::Ring(unsafe { SpscRing::init(buf_ptr, RX_BUF, qos.depth as usize) })
3327 };
3328
3329 unsafe {
3330 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3331 let entry_ptr = arena_ptr.add(entry_offset) as *mut Entry<F>;
3332 core::ptr::write(
3333 entry_ptr,
3334 Entry {
3335 handle,
3336 buffer,
3337 callback,
3338 },
3339 );
3340 }
3341
3342 self.entries[slot] = Some(CallbackMeta {
3343 offset: entry_offset,
3344 kind: EntryKind::Subscription,
3345 try_process: sub_buffered_raw_try_process::<F>,
3346 has_data: sub_buffered_raw_has_data::<F>,
3347 pre_sample: no_pre_sample,
3348 invocation: InvocationMode::OnNewData,
3349 drop_fn: drop_entry::<Entry<F>>,
3350 });
3351 Ok(HandleId(slot))
3352 }
3353
3354 pub(crate) fn register_subscription_with_info_sized_inner<M, F, const RX_BUF: usize>(
3355 &mut self,
3356 node_id: Option<super::node_record::NodeId>,
3357 topic_name: &str,
3358 qos: QosSettings,
3359 callback: F,
3360 ) -> Result<HandleId, NodeError>
3361 where
3362 M: crate::rmw_type_registry::MessageForRmw + 'static,
3363 F: FnMut(&M, Option<&nros_core::MessageInfo>) + 'static,
3364 {
3365 type Entry<M, F, const N: usize> = SubInfoEntry<M, F, N>;
3366
3367 // Phase 212.K.7.6.b — see `create_publisher_on`.
3368 crate::rmw_type_registry::register_type::<M>()?;
3369
3370 let slot = self.next_entry_slot()?;
3371 let (node_name, ns, session_idx) = match node_id {
3372 Some(id) => {
3373 let r = self
3374 .nodes
3375 .get(id.index())
3376 .ok_or(NodeError::InvalidSchedContextBinding)?;
3377 (r.name.clone(), r.namespace.clone(), r.session_idx)
3378 }
3379 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
3380 };
3381 let mut topic = TopicInfo::new(
3382 topic_name,
3383 <M as RosMessage>::TYPE_NAME,
3384 <M as RosMessage>::TYPE_HASH,
3385 )
3386 .with_namespace(&ns);
3387 if !node_name.is_empty() {
3388 topic = topic.with_node_name(&node_name);
3389 }
3390 let handle = {
3391 let session = self
3392 .session_at_mut(session_idx)
3393 .ok_or(NodeError::BackendMismatch)?;
3394 session
3395 .create_subscriber(&topic, qos)
3396 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
3397 };
3398
3399 let offset = self.arena_alloc::<Entry<M, F, RX_BUF>>()?;
3400
3401 unsafe {
3402 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3403 let entry_ptr = arena_ptr.add(offset) as *mut Entry<M, F, RX_BUF>;
3404 core::ptr::write(
3405 entry_ptr,
3406 Entry {
3407 handle,
3408 buffer: [0u8; RX_BUF],
3409 sampled_len: 0,
3410 callback,
3411 _phantom: PhantomData,
3412 },
3413 );
3414 }
3415
3416 self.entries[slot] = Some(CallbackMeta {
3417 offset,
3418 kind: EntryKind::Subscription,
3419 try_process: sub_info_try_process::<M, F, RX_BUF>,
3420 has_data: sub_info_has_data::<M, F, RX_BUF>,
3421 pre_sample: sub_info_pre_sample::<M, F, RX_BUF>,
3422 invocation: InvocationMode::OnNewData,
3423 drop_fn: drop_entry::<Entry<M, F, RX_BUF>>,
3424 });
3425 self.apply_node_default_sched(slot, node_id, None);
3426 Ok(HandleId(slot))
3427 }
3428
3429 #[cfg(feature = "safety-e2e")]
3430 pub(crate) fn register_subscription_with_safety_sized_inner<M, F, const RX_BUF: usize>(
3431 &mut self,
3432 node_id: Option<super::node_record::NodeId>,
3433 topic_name: &str,
3434 qos: QosSettings,
3435 callback: F,
3436 ) -> Result<HandleId, NodeError>
3437 where
3438 M: crate::rmw_type_registry::MessageForRmw + 'static,
3439 F: FnMut(&M, &nros_rmw::IntegrityStatus) + 'static,
3440 {
3441 type Entry<M, F, const N: usize> = SubSafetyEntry<M, F, N>;
3442
3443 // Phase 212.K.7.6.b — see `create_publisher_on`.
3444 crate::rmw_type_registry::register_type::<M>()?;
3445
3446 let slot = self.next_entry_slot()?;
3447 let (node_name, ns, session_idx) = match node_id {
3448 Some(id) => {
3449 let r = self
3450 .nodes
3451 .get(id.index())
3452 .ok_or(NodeError::InvalidSchedContextBinding)?;
3453 (r.name.clone(), r.namespace.clone(), r.session_idx)
3454 }
3455 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
3456 };
3457 let mut topic = TopicInfo::new(
3458 topic_name,
3459 <M as RosMessage>::TYPE_NAME,
3460 <M as RosMessage>::TYPE_HASH,
3461 )
3462 .with_namespace(&ns);
3463 if !node_name.is_empty() {
3464 topic = topic.with_node_name(&node_name);
3465 }
3466 let handle = {
3467 let session = self
3468 .session_at_mut(session_idx)
3469 .ok_or(NodeError::BackendMismatch)?;
3470 session
3471 .create_subscriber(&topic, qos)
3472 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
3473 };
3474
3475 let offset = self.arena_alloc::<Entry<M, F, RX_BUF>>()?;
3476
3477 unsafe {
3478 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3479 let entry_ptr = arena_ptr.add(offset) as *mut Entry<M, F, RX_BUF>;
3480 core::ptr::write(
3481 entry_ptr,
3482 Entry {
3483 handle,
3484 buffer: [0u8; RX_BUF],
3485 sampled_len: 0,
3486 callback,
3487 _phantom: PhantomData,
3488 },
3489 );
3490 }
3491
3492 self.entries[slot] = Some(CallbackMeta {
3493 offset,
3494 kind: EntryKind::Subscription,
3495 try_process: sub_safety_try_process::<M, F, RX_BUF>,
3496 has_data: sub_safety_has_data::<M, F, RX_BUF>,
3497 pre_sample: sub_safety_pre_sample::<M, F, RX_BUF>,
3498 invocation: InvocationMode::OnNewData,
3499 drop_fn: drop_entry::<Entry<M, F, RX_BUF>>,
3500 });
3501 self.apply_node_default_sched(slot, node_id, None);
3502 Ok(HandleId(slot))
3503 }
3504
3505 /// Register a service callback with the default buffer size.
3506 ///
3507 /// The callback is stored in the arena and invoked during [`spin_once()`](Self::spin_once).
3508 pub fn register_service<Svc, F>(
3509 &mut self,
3510 service_name: &str,
3511 callback: F,
3512 ) -> Result<HandleId, NodeError>
3513 where
3514 Svc: RosService + 'static,
3515 Svc::Request: crate::rmw_type_registry::MessageForRmw,
3516 Svc::Reply: crate::rmw_type_registry::MessageForRmw,
3517 F: FnMut(&Svc::Request) -> Svc::Reply + 'static,
3518 {
3519 self.register_service_sized::<Svc, F, { crate::config::DEFAULT_RX_BUF_SIZE }, { crate::config::DEFAULT_RX_BUF_SIZE }>(service_name, callback)
3520 }
3521
3522 /// Register a service callback with custom request/reply buffer sizes.
3523 pub fn register_service_sized<Svc, F, const REQ_BUF: usize, const REPLY_BUF: usize>(
3524 &mut self,
3525 service_name: &str,
3526 callback: F,
3527 ) -> Result<HandleId, NodeError>
3528 where
3529 Svc: RosService + 'static,
3530 Svc::Request: crate::rmw_type_registry::MessageForRmw,
3531 Svc::Reply: crate::rmw_type_registry::MessageForRmw,
3532 F: FnMut(&Svc::Request) -> Svc::Reply + 'static,
3533 {
3534 type Entry<Svc, F, const RQ: usize, const RP: usize> = SrvEntry<Svc, F, RQ, RP>;
3535
3536 // Phase 212.K.7.7.b — register both halves of the service round-trip
3537 // under cyclonedds. No-op for other RMWs. Mirrors the K.7.6.b hook
3538 // on `Node::create_service_sized`.
3539 crate::rmw_type_registry::register_type::<Svc::Request>()?;
3540 crate::rmw_type_registry::register_type::<Svc::Reply>()?;
3541
3542 let slot = self.next_entry_slot()?;
3543 let node_name: heapless::String<64> = self.node_name.clone();
3544 let ns: heapless::String<64> = self.namespace.clone();
3545 let mut info = ServiceInfo::new(service_name, Svc::SERVICE_NAME, Svc::SERVICE_HASH)
3546 .with_namespace(&ns);
3547 if !node_name.is_empty() {
3548 info = info.with_node_name(&node_name);
3549 }
3550 let handle = self
3551 .session
3552 .create_service_server(&info, QosSettings::services_default())
3553 .map_err(|_| NodeError::Transport(TransportError::ServiceServerCreationFailed))?;
3554
3555 let offset = self.arena_alloc::<Entry<Svc, F, REQ_BUF, REPLY_BUF>>()?;
3556
3557 // SAFETY: same guarantees as register_subscription_sized.
3558 unsafe {
3559 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3560 let entry_ptr = arena_ptr.add(offset) as *mut Entry<Svc, F, REQ_BUF, REPLY_BUF>;
3561 core::ptr::write(
3562 entry_ptr,
3563 Entry {
3564 handle,
3565 req_buffer: [0u8; REQ_BUF],
3566 reply_buffer: [0u8; REPLY_BUF],
3567 callback,
3568 _phantom: PhantomData,
3569 },
3570 );
3571 }
3572
3573 self.entries[slot] = Some(CallbackMeta {
3574 offset,
3575 kind: EntryKind::Service,
3576 try_process: srv_try_process::<Svc, F, REQ_BUF, REPLY_BUF>,
3577 has_data: srv_has_data::<Svc, F, REQ_BUF, REPLY_BUF>,
3578 pre_sample: no_pre_sample,
3579 invocation: InvocationMode::OnNewData,
3580 drop_fn: drop_entry::<Entry<Svc, F, REQ_BUF, REPLY_BUF>>,
3581 });
3582 Ok(HandleId(slot))
3583 }
3584
3585 /// Phase 104.C.3.3.a — Node-aware variant of
3586 /// [`register_service_sized`](Self::register_service_sized).
3587 pub fn register_service_sized_on<Svc, F, const REQ_BUF: usize, const REPLY_BUF: usize>(
3588 &mut self,
3589 node_id: super::node_record::NodeId,
3590 service_name: &str,
3591 qos: QosSettings,
3592 callback: F,
3593 ) -> Result<HandleId, NodeError>
3594 where
3595 Svc: RosService + 'static,
3596 Svc::Request: crate::rmw_type_registry::MessageForRmw,
3597 Svc::Reply: crate::rmw_type_registry::MessageForRmw,
3598 F: FnMut(&Svc::Request) -> Svc::Reply + 'static,
3599 {
3600 type Entry<Svc, F, const RQ: usize, const RP: usize> = SrvEntry<Svc, F, RQ, RP>;
3601
3602 // Phase 212.K.7.7.b — see `register_service_sized`.
3603 crate::rmw_type_registry::register_type::<Svc::Request>()?;
3604 crate::rmw_type_registry::register_type::<Svc::Reply>()?;
3605
3606 let slot = self.next_entry_slot()?;
3607 let (node_name, ns, session_idx) = {
3608 let r = self
3609 .nodes
3610 .get(node_id.index())
3611 .ok_or(NodeError::InvalidSchedContextBinding)?;
3612 (r.name.clone(), r.namespace.clone(), r.session_idx)
3613 };
3614 let mut info = ServiceInfo::new(service_name, Svc::SERVICE_NAME, Svc::SERVICE_HASH)
3615 .with_namespace(&ns);
3616 if !node_name.is_empty() {
3617 info = info.with_node_name(&node_name);
3618 }
3619 let handle = {
3620 let session = self
3621 .session_at_mut(session_idx)
3622 .ok_or(NodeError::BackendMismatch)?;
3623 // Phase 193.5 — validate against the backend's supported policies
3624 // (no silent downgrade); request/reply effectively requires RELIABLE.
3625 qos.validate_against(session.supported_qos_policies())
3626 .map_err(NodeError::Transport)?;
3627 session
3628 .create_service_server(&info, qos)
3629 .map_err(|_| NodeError::Transport(TransportError::ServiceServerCreationFailed))?
3630 };
3631
3632 let offset = self.arena_alloc::<Entry<Svc, F, REQ_BUF, REPLY_BUF>>()?;
3633 unsafe {
3634 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3635 let entry_ptr = arena_ptr.add(offset) as *mut Entry<Svc, F, REQ_BUF, REPLY_BUF>;
3636 core::ptr::write(
3637 entry_ptr,
3638 Entry {
3639 handle,
3640 req_buffer: [0u8; REQ_BUF],
3641 reply_buffer: [0u8; REPLY_BUF],
3642 callback,
3643 _phantom: PhantomData,
3644 },
3645 );
3646 }
3647
3648 self.entries[slot] = Some(CallbackMeta {
3649 offset,
3650 kind: EntryKind::Service,
3651 try_process: srv_try_process::<Svc, F, REQ_BUF, REPLY_BUF>,
3652 has_data: srv_has_data::<Svc, F, REQ_BUF, REPLY_BUF>,
3653 pre_sample: no_pre_sample,
3654 invocation: InvocationMode::OnNewData,
3655 drop_fn: drop_entry::<Entry<Svc, F, REQ_BUF, REPLY_BUF>>,
3656 });
3657 self.apply_node_default_sched(slot, Some(node_id), None);
3658 Ok(HandleId(slot))
3659 }
3660
3661 /// Phase 104.C.3.3.a — Node-aware variant of
3662 /// [`register_service`](Self::register_service).
3663 pub fn register_service_on<Svc, F>(
3664 &mut self,
3665 node_id: super::node_record::NodeId,
3666 service_name: &str,
3667 callback: F,
3668 ) -> Result<HandleId, NodeError>
3669 where
3670 Svc: RosService + 'static,
3671 Svc::Request: crate::rmw_type_registry::MessageForRmw,
3672 Svc::Reply: crate::rmw_type_registry::MessageForRmw,
3673 F: FnMut(&Svc::Request) -> Svc::Reply + 'static,
3674 {
3675 self.register_service_sized_on::<
3676 Svc,
3677 F,
3678 { crate::config::DEFAULT_RX_BUF_SIZE },
3679 { crate::config::DEFAULT_RX_BUF_SIZE },
3680 >(node_id, service_name, QosSettings::services_default(), callback)
3681 }
3682
3683 // ========================================================================
3684 // Timer registration
3685 // ========================================================================
3686
3687 /// Register a repeating timer callback.
3688 ///
3689 /// The callback fires every `period` milliseconds during [`spin_once()`](Self::spin_once).
3690 /// The timer delta is approximated by the `timeout_ms` argument to `spin_once`.
3691 pub fn register_timer<F>(
3692 &mut self,
3693 period: TimerDuration,
3694 callback: F,
3695 ) -> Result<HandleId, NodeError>
3696 where
3697 F: FnMut() + 'static,
3698 {
3699 let slot = self.next_entry_slot()?;
3700 let offset = self.arena_alloc::<TimerEntry<F>>()?;
3701
3702 unsafe {
3703 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3704 let entry_ptr = arena_ptr.add(offset) as *mut TimerEntry<F>;
3705 core::ptr::write(
3706 entry_ptr,
3707 TimerEntry {
3708 period_ms: period.as_millis(),
3709 elapsed_ms: 0,
3710 oneshot: false,
3711 fired: false,
3712 cancelled: false,
3713 callback,
3714 },
3715 );
3716 }
3717
3718 self.entries[slot] = Some(CallbackMeta {
3719 offset,
3720 kind: EntryKind::Timer,
3721 try_process: timer_try_process::<F>,
3722 has_data: always_ready,
3723 pre_sample: no_pre_sample,
3724 invocation: InvocationMode::Always,
3725 drop_fn: drop_entry::<TimerEntry<F>>,
3726 });
3727 Ok(HandleId(slot))
3728 }
3729
3730 /// Register a one-shot timer callback.
3731 ///
3732 /// The callback fires once after `delay` milliseconds, then becomes inert.
3733 pub fn register_timer_oneshot<F>(
3734 &mut self,
3735 delay: TimerDuration,
3736 callback: F,
3737 ) -> Result<HandleId, NodeError>
3738 where
3739 F: FnMut() + 'static,
3740 {
3741 let slot = self.next_entry_slot()?;
3742 let offset = self.arena_alloc::<TimerEntry<F>>()?;
3743
3744 unsafe {
3745 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3746 let entry_ptr = arena_ptr.add(offset) as *mut TimerEntry<F>;
3747 core::ptr::write(
3748 entry_ptr,
3749 TimerEntry {
3750 period_ms: delay.as_millis(),
3751 elapsed_ms: 0,
3752 oneshot: true,
3753 fired: false,
3754 cancelled: false,
3755 callback,
3756 },
3757 );
3758 }
3759
3760 self.entries[slot] = Some(CallbackMeta {
3761 offset,
3762 kind: EntryKind::Timer,
3763 try_process: timer_try_process::<F>,
3764 has_data: always_ready,
3765 pre_sample: no_pre_sample,
3766 invocation: InvocationMode::Always,
3767 drop_fn: drop_entry::<TimerEntry<F>>,
3768 });
3769 Ok(HandleId(slot))
3770 }
3771
3772 /// Phase 273 (RFC-0047) — register a repeating timer callback bound to a
3773 /// specific node and optional callback group. The group name is threaded to
3774 /// `apply_node_default_sched` so the seeded `group_sched_table` assigns
3775 /// the timer's callback to the group's `SchedContext`. When `group` is
3776 /// `None` the node's `default_sched` applies (phase-272 behavior).
3777 ///
3778 /// This is the executor-level primitive called by the Rust `_in` API
3779 /// (`NodeCtx::create_timer_in`) and the C/C++ group-aware timer FFI.
3780 pub fn register_timer_on<F>(
3781 &mut self,
3782 node_id: Option<super::node_record::NodeId>,
3783 period: TimerDuration,
3784 callback: F,
3785 group: Option<&str>,
3786 ) -> Result<HandleId, NodeError>
3787 where
3788 F: FnMut() + 'static,
3789 {
3790 let slot = self.next_entry_slot()?;
3791 let offset = self.arena_alloc::<TimerEntry<F>>()?;
3792
3793 unsafe {
3794 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3795 let entry_ptr = arena_ptr.add(offset) as *mut TimerEntry<F>;
3796 core::ptr::write(
3797 entry_ptr,
3798 TimerEntry {
3799 period_ms: period.as_millis(),
3800 elapsed_ms: 0,
3801 oneshot: false,
3802 fired: false,
3803 cancelled: false,
3804 callback,
3805 },
3806 );
3807 }
3808
3809 self.entries[slot] = Some(CallbackMeta {
3810 offset,
3811 kind: EntryKind::Timer,
3812 try_process: timer_try_process::<F>,
3813 has_data: always_ready,
3814 pre_sample: no_pre_sample,
3815 invocation: InvocationMode::Always,
3816 drop_fn: drop_entry::<TimerEntry<F>>,
3817 });
3818 // Phase 273 — apply group sched binding (group > node default > SC 0).
3819 self.apply_node_default_sched(slot, node_id, group);
3820 Ok(HandleId(slot))
3821 }
3822
3823 // ========================================================================
3824 // Raw callback registration (for C API)
3825 // ========================================================================
3826
3827 /// The kept C-FFI subscription core (Phase 189.M2.b): registers a
3828 /// raw `RawSubscriptionCallback` fn-ptr + `context` against an
3829 /// optional node's session. The Rust ergonomic surface is the
3830 /// `node.subscription(t)` builder (closures); this is the single
3831 /// primitive the `nros-c` thin wrapper lowers to. `node_id == None`
3832 /// is the legacy single-node path.
3833 #[allow(clippy::too_many_arguments)]
3834 pub fn add_arena_subscription_c_callback<const RX_BUF: usize>(
3835 &mut self,
3836 node_id: Option<super::node_record::NodeId>,
3837 topic_name: &str,
3838 type_name: &str,
3839 type_hash: &str,
3840 qos: QosSettings,
3841 callback: RawSubscriptionCallback,
3842 context: *mut core::ffi::c_void,
3843 group: Option<&str>,
3844 ) -> Result<HandleId, NodeError> {
3845 let slot = self.next_entry_slot()?;
3846 let (node_name, ns, session_idx) = match node_id {
3847 Some(id) => {
3848 let r = self
3849 .nodes
3850 .get(id.index())
3851 .ok_or(NodeError::InvalidSchedContextBinding)?;
3852 (r.name.clone(), r.namespace.clone(), r.session_idx)
3853 }
3854 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
3855 };
3856 let mut topic = TopicInfo::new(topic_name, type_name, type_hash).with_namespace(&ns);
3857 if !node_name.is_empty() {
3858 topic = topic.with_node_name(&node_name);
3859 }
3860 let handle = {
3861 let session = self
3862 .session_at_mut(session_idx)
3863 .ok_or(NodeError::BackendMismatch)?;
3864 session
3865 .create_subscriber(&topic, qos)
3866 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
3867 };
3868
3869 let (_slot_count, trailing_bytes) = buffered_region_size(qos.depth, RX_BUF);
3870
3871 let (entry_offset, trailing_offset) =
3872 self.arena_alloc_with_trailing::<SubBufferedRawCEntry>(trailing_bytes)?;
3873
3874 let buf_ptr = unsafe { (self.arena.as_mut_ptr() as *mut u8).add(trailing_offset) };
3875
3876 let buffer = if qos.depth <= 1 {
3877 BufferStrategy::Triple(unsafe { TripleBuffer::init(buf_ptr, RX_BUF) })
3878 } else {
3879 BufferStrategy::Ring(unsafe { SpscRing::init(buf_ptr, RX_BUF, qos.depth as usize) })
3880 };
3881
3882 unsafe {
3883 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3884 let entry_ptr = arena_ptr.add(entry_offset) as *mut SubBufferedRawCEntry;
3885 core::ptr::write(
3886 entry_ptr,
3887 SubBufferedRawCEntry {
3888 handle,
3889 buffer,
3890 callback,
3891 context,
3892 },
3893 );
3894 }
3895
3896 self.entries[slot] = Some(CallbackMeta {
3897 offset: entry_offset,
3898 kind: EntryKind::Subscription,
3899 try_process: sub_buffered_raw_c_try_process,
3900 has_data: sub_buffered_raw_c_has_data,
3901 pre_sample: no_pre_sample,
3902 invocation: InvocationMode::OnNewData,
3903 drop_fn: drop_entry::<SubBufferedRawCEntry>,
3904 });
3905 self.apply_node_default_sched(slot, node_id, group);
3906 Ok(HandleId(slot))
3907 }
3908
3909 /// Phase 189.M3.4 — register a raw C-fn-ptr subscription whose callback
3910 /// also receives the sample's wire **attachment**
3911 /// ([`RawSubscriptionInfoCallback`]: `(data, len, attachment, att_len,
3912 /// context)`) — the C analog of the Rust
3913 /// `node.subscription(t).generic(..).message_info()` builder. Backs the C
3914 /// FFI `nros_executor_register_subscription_raw_with_info`. Flat per-entry
3915 /// payload + attachment buffers (cap [`RAW_INFO_ATT_CAP`](super::arena::RAW_INFO_ATT_CAP));
3916 /// one sample per `spin_once`.
3917 #[allow(clippy::too_many_arguments)]
3918 pub fn add_arena_subscription_c_info_callback<const RX_BUF: usize>(
3919 &mut self,
3920 node_id: Option<super::node_record::NodeId>,
3921 topic_name: &str,
3922 type_name: &str,
3923 type_hash: &str,
3924 qos: QosSettings,
3925 callback: RawSubscriptionInfoCallback,
3926 context: *mut core::ffi::c_void,
3927 ) -> Result<HandleId, NodeError> {
3928 type Entry<const N: usize> = SubBufferedRawInfoCEntry<N>;
3929
3930 let slot = self.next_entry_slot()?;
3931 let (node_name, ns, session_idx) = match node_id {
3932 Some(id) => {
3933 let r = self
3934 .nodes
3935 .get(id.index())
3936 .ok_or(NodeError::InvalidSchedContextBinding)?;
3937 (r.name.clone(), r.namespace.clone(), r.session_idx)
3938 }
3939 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
3940 };
3941 let mut topic = TopicInfo::new(topic_name, type_name, type_hash).with_namespace(&ns);
3942 if !node_name.is_empty() {
3943 topic = topic.with_node_name(&node_name);
3944 }
3945 let handle = {
3946 let session = self
3947 .session_at_mut(session_idx)
3948 .ok_or(NodeError::BackendMismatch)?;
3949 session
3950 .create_subscriber(&topic, qos)
3951 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
3952 };
3953
3954 let offset = self.arena_alloc::<Entry<RX_BUF>>()?;
3955 unsafe {
3956 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
3957 let entry_ptr = arena_ptr.add(offset) as *mut Entry<RX_BUF>;
3958 core::ptr::write(
3959 entry_ptr,
3960 Entry {
3961 handle,
3962 buffer: [0u8; RX_BUF],
3963 att: [0u8; super::arena::RAW_INFO_ATT_CAP],
3964 callback,
3965 context,
3966 },
3967 );
3968 }
3969
3970 self.entries[slot] = Some(CallbackMeta {
3971 offset,
3972 kind: EntryKind::Subscription,
3973 try_process: sub_buffered_raw_info_c_try_process::<RX_BUF>,
3974 has_data: sub_buffered_raw_info_c_has_data::<RX_BUF>,
3975 pre_sample: no_pre_sample,
3976 invocation: InvocationMode::OnNewData,
3977 drop_fn: drop_entry::<Entry<RX_BUF>>,
3978 });
3979 self.apply_node_default_sched(slot, node_id, None);
3980 Ok(HandleId(slot))
3981 }
3982
3983 /// Phase 269 W3 — register a raw C-fn-ptr subscription whose callback
3984 /// ALSO surfaces the sample's E2E integrity status (CRC + sequence gap/dup)
3985 /// alongside the CDR bytes — the C/C++ component-callback analog of Rust's
3986 /// `register_subscription_buffered_raw_safety_on` (`FnMut(&[u8], &IntegrityStatus)`).
3987 ///
3988 /// The executor validates the sample via `try_recv_validated` and unpacks the
3989 /// [`nros_rmw::IntegrityStatus`] into three plain scalars (gap, duplicate,
3990 /// crc_valid) before calling `callback`. This avoids introducing a
3991 /// cbindgen-visible struct at the executor layer; the C/C++ headers pack them
3992 /// back into their local integrity-status typedef.
3993 ///
3994 /// Requires the `safety-e2e` feature. Backed by [`SubBufferedRawSafetyCEntry`].
3995 #[cfg(feature = "safety-e2e")]
3996 #[allow(clippy::too_many_arguments)]
3997 pub fn add_arena_subscription_c_validated_callback<const RX_BUF: usize>(
3998 &mut self,
3999 node_id: Option<super::node_record::NodeId>,
4000 topic_name: &str,
4001 type_name: &str,
4002 type_hash: &str,
4003 qos: QosSettings,
4004 callback: super::types::RawSubscriptionSafetyCallback,
4005 context: *mut core::ffi::c_void,
4006 ) -> Result<HandleId, NodeError> {
4007 use super::arena::{
4008 SubBufferedRawSafetyCEntry, sub_buffered_raw_safety_c_has_data,
4009 sub_buffered_raw_safety_c_try_process,
4010 };
4011 type Entry<const N: usize> = SubBufferedRawSafetyCEntry<N>;
4012
4013 let slot = self.next_entry_slot()?;
4014 let (node_name, ns, session_idx) = match node_id {
4015 Some(id) => {
4016 let r = self
4017 .nodes
4018 .get(id.index())
4019 .ok_or(NodeError::InvalidSchedContextBinding)?;
4020 (r.name.clone(), r.namespace.clone(), r.session_idx)
4021 }
4022 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
4023 };
4024 let mut topic = TopicInfo::new(topic_name, type_name, type_hash).with_namespace(&ns);
4025 if !node_name.is_empty() {
4026 topic = topic.with_node_name(&node_name);
4027 }
4028 let handle = {
4029 let session = self
4030 .session_at_mut(session_idx)
4031 .ok_or(NodeError::BackendMismatch)?;
4032 session
4033 .create_subscriber(&topic, qos)
4034 .map_err(|_| NodeError::Transport(TransportError::SubscriberCreationFailed))?
4035 };
4036
4037 let offset = self.arena_alloc::<Entry<RX_BUF>>()?;
4038 unsafe {
4039 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
4040 let entry_ptr = arena_ptr.add(offset) as *mut Entry<RX_BUF>;
4041 core::ptr::write(
4042 entry_ptr,
4043 Entry {
4044 handle,
4045 buffer: [0u8; RX_BUF],
4046 callback,
4047 context,
4048 },
4049 );
4050 }
4051
4052 self.entries[slot] = Some(CallbackMeta {
4053 offset,
4054 kind: EntryKind::Subscription,
4055 try_process: sub_buffered_raw_safety_c_try_process::<RX_BUF>,
4056 has_data: sub_buffered_raw_safety_c_has_data::<RX_BUF>,
4057 pre_sample: no_pre_sample,
4058 invocation: InvocationMode::OnNewData,
4059 drop_fn: drop_entry::<Entry<RX_BUF>>,
4060 });
4061 self.apply_node_default_sched(slot, node_id, None);
4062 Ok(HandleId(slot))
4063 }
4064
4065 /// Register a raw (untyped) service callback.
4066 ///
4067 /// Register a raw (untyped) service callback with the default buffer size.
4068 ///
4069 /// The callback receives and produces CDR bytes without typed
4070 /// deserialization/serialization. Used by the C API wrapper.
4071 pub fn register_service_raw(
4072 &mut self,
4073 service_name: &str,
4074 service_type: &str,
4075 service_hash: &str,
4076 callback: RawServiceCallback,
4077 context: *mut core::ffi::c_void,
4078 ) -> Result<HandleId, NodeError> {
4079 self.register_service_raw_sized::<{ crate::config::DEFAULT_RX_BUF_SIZE }, { crate::config::DEFAULT_RX_BUF_SIZE }>(
4080 service_name,
4081 service_type,
4082 service_hash,
4083 QosSettings::services_default(),
4084 callback,
4085 context,
4086 )
4087 }
4088
4089 /// Register a raw (untyped) service callback with custom buffer sizes + QoS.
4090 ///
4091 /// `REQ_BUF` and `REPLY_BUF` set the stack-allocated CDR buffers
4092 /// for the request and reply respectively. Increase for services
4093 /// with large payloads (e.g., parameter services). `qos` applies to both
4094 /// the request + reply endpoints (Phase 193.2c).
4095 #[allow(clippy::too_many_arguments)]
4096 pub fn register_service_raw_sized<const REQ_BUF: usize, const REPLY_BUF: usize>(
4097 &mut self,
4098 service_name: &str,
4099 service_type: &str,
4100 service_hash: &str,
4101 qos: QosSettings,
4102 callback: RawServiceCallback,
4103 context: *mut core::ffi::c_void,
4104 ) -> Result<HandleId, NodeError> {
4105 self.register_service_raw_sized_inner::<REQ_BUF, REPLY_BUF>(
4106 None,
4107 service_name,
4108 service_type,
4109 service_hash,
4110 qos,
4111 callback,
4112 context,
4113 )
4114 }
4115
4116 /// Phase 104.C.3.3.a — Node-aware variant of
4117 /// [`register_service_raw_sized`]. C-FFI path.
4118 #[allow(clippy::too_many_arguments)]
4119 pub fn register_service_raw_sized_on<const REQ_BUF: usize, const REPLY_BUF: usize>(
4120 &mut self,
4121 node_id: super::node_record::NodeId,
4122 service_name: &str,
4123 service_type: &str,
4124 service_hash: &str,
4125 qos: QosSettings,
4126 callback: RawServiceCallback,
4127 context: *mut core::ffi::c_void,
4128 ) -> Result<HandleId, NodeError> {
4129 self.register_service_raw_sized_inner::<REQ_BUF, REPLY_BUF>(
4130 Some(node_id),
4131 service_name,
4132 service_type,
4133 service_hash,
4134 qos,
4135 callback,
4136 context,
4137 )
4138 }
4139
4140 #[allow(clippy::too_many_arguments)]
4141 fn register_service_raw_sized_inner<const REQ_BUF: usize, const REPLY_BUF: usize>(
4142 &mut self,
4143 node_id: Option<super::node_record::NodeId>,
4144 service_name: &str,
4145 service_type: &str,
4146 service_hash: &str,
4147 qos: QosSettings,
4148 callback: RawServiceCallback,
4149 context: *mut core::ffi::c_void,
4150 ) -> Result<HandleId, NodeError> {
4151 let slot = self.next_entry_slot()?;
4152 let (node_name, ns, session_idx) = match node_id {
4153 Some(id) => {
4154 let r = self
4155 .nodes
4156 .get(id.index())
4157 .ok_or(NodeError::InvalidSchedContextBinding)?;
4158 (r.name.clone(), r.namespace.clone(), r.session_idx)
4159 }
4160 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
4161 };
4162 let mut info =
4163 ServiceInfo::new(service_name, service_type, service_hash).with_namespace(&ns);
4164 if !node_name.is_empty() {
4165 info = info.with_node_name(&node_name);
4166 }
4167 let handle = {
4168 let session = self
4169 .session_at_mut(session_idx)
4170 .ok_or(NodeError::BackendMismatch)?;
4171 // Phase 193.5 — validate against the backend's supported policies
4172 // (no silent downgrade); request/reply effectively requires RELIABLE.
4173 qos.validate_against(session.supported_qos_policies())
4174 .map_err(NodeError::Transport)?;
4175 session
4176 .create_service_server(&info, qos)
4177 .map_err(|_| NodeError::Transport(TransportError::ServiceServerCreationFailed))?
4178 };
4179
4180 let offset = self.arena_alloc::<SrvRawEntry<REQ_BUF, REPLY_BUF>>()?;
4181
4182 unsafe {
4183 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
4184 let entry_ptr = arena_ptr.add(offset) as *mut SrvRawEntry<REQ_BUF, REPLY_BUF>;
4185 core::ptr::write(
4186 entry_ptr,
4187 SrvRawEntry {
4188 handle,
4189 req_buffer: [0u8; REQ_BUF],
4190 reply_buffer: [0u8; REPLY_BUF],
4191 callback,
4192 context,
4193 },
4194 );
4195 }
4196
4197 self.entries[slot] = Some(CallbackMeta {
4198 offset,
4199 kind: EntryKind::Service,
4200 try_process: srv_raw_try_process::<REQ_BUF, REPLY_BUF>,
4201 has_data: srv_raw_has_data::<REQ_BUF, REPLY_BUF>,
4202 pre_sample: no_pre_sample,
4203 invocation: InvocationMode::OnNewData,
4204 drop_fn: drop_entry::<SrvRawEntry<REQ_BUF, REPLY_BUF>>,
4205 });
4206 self.apply_node_default_sched(slot, node_id, None);
4207 Ok(HandleId(slot))
4208 }
4209
4210 // ========================================================================
4211 // Raw service client registration (Phase 82)
4212 // ========================================================================
4213
4214 /// Register a raw (untyped) service client with the default reply
4215 /// buffer size.
4216 ///
4217 /// The client is owned by the executor's arena. Each `spin_once`
4218 /// dispatch polls the in-flight reply slot via `try_recv_reply_raw`
4219 /// and fires the registered callback when the response arrives.
4220 /// Used by the C API thin wrapper — see Phase 82.
4221 pub fn register_service_client_raw(
4222 &mut self,
4223 service_name: &str,
4224 service_type: &str,
4225 service_hash: &str,
4226 callback: Option<RawResponseCallback>,
4227 context: *mut core::ffi::c_void,
4228 ) -> Result<HandleId, NodeError> {
4229 self.register_service_client_raw_sized::<{ crate::config::DEFAULT_RX_BUF_SIZE }>(
4230 service_name,
4231 service_type,
4232 service_hash,
4233 QosSettings::services_default(),
4234 callback,
4235 context,
4236 )
4237 }
4238
4239 /// Register a raw service client with a custom reply buffer size + QoS.
4240 ///
4241 /// `qos` applies to the client's request + reply endpoints (Phase 193.3b);
4242 /// defaults to [`QosSettings::services_default`] via the convenience
4243 /// wrapper.
4244 #[allow(clippy::too_many_arguments)]
4245 pub fn register_service_client_raw_sized<const REPLY_BUF: usize>(
4246 &mut self,
4247 service_name: &str,
4248 service_type: &str,
4249 service_hash: &str,
4250 qos: QosSettings,
4251 callback: Option<RawResponseCallback>,
4252 context: *mut core::ffi::c_void,
4253 ) -> Result<HandleId, NodeError> {
4254 self.register_service_client_raw_sized_inner::<REPLY_BUF>(
4255 None,
4256 service_name,
4257 service_type,
4258 service_hash,
4259 qos,
4260 callback,
4261 context,
4262 )
4263 }
4264
4265 /// Phase 104.C.3.3.a — Node-aware variant of
4266 /// [`register_service_client_raw_sized`]. Routes the client
4267 /// creation through the named Node's session.
4268 #[allow(clippy::too_many_arguments)]
4269 pub fn register_service_client_raw_sized_on<const REPLY_BUF: usize>(
4270 &mut self,
4271 node_id: super::node_record::NodeId,
4272 service_name: &str,
4273 service_type: &str,
4274 service_hash: &str,
4275 qos: QosSettings,
4276 callback: Option<RawResponseCallback>,
4277 context: *mut core::ffi::c_void,
4278 ) -> Result<HandleId, NodeError> {
4279 self.register_service_client_raw_sized_inner::<REPLY_BUF>(
4280 Some(node_id),
4281 service_name,
4282 service_type,
4283 service_hash,
4284 qos,
4285 callback,
4286 context,
4287 )
4288 }
4289
4290 #[allow(clippy::too_many_arguments)]
4291 fn register_service_client_raw_sized_inner<const REPLY_BUF: usize>(
4292 &mut self,
4293 node_id: Option<super::node_record::NodeId>,
4294 service_name: &str,
4295 service_type: &str,
4296 service_hash: &str,
4297 qos: QosSettings,
4298 callback: Option<RawResponseCallback>,
4299 context: *mut core::ffi::c_void,
4300 ) -> Result<HandleId, NodeError> {
4301 let slot = self.next_entry_slot()?;
4302 let (node_name, ns, session_idx) = match node_id {
4303 Some(id) => {
4304 let r = self
4305 .nodes
4306 .get(id.index())
4307 .ok_or(NodeError::InvalidSchedContextBinding)?;
4308 (r.name.clone(), r.namespace.clone(), r.session_idx)
4309 }
4310 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
4311 };
4312 let mut info =
4313 ServiceInfo::new(service_name, service_type, service_hash).with_namespace(&ns);
4314 if !node_name.is_empty() {
4315 info = info.with_node_name(&node_name);
4316 }
4317 let handle = {
4318 let session = self
4319 .session_at_mut(session_idx)
4320 .ok_or(NodeError::BackendMismatch)?;
4321 // Phase 193.5 — validate against the backend's supported policies
4322 // (no silent downgrade); request/reply effectively requires RELIABLE.
4323 qos.validate_against(session.supported_qos_policies())
4324 .map_err(NodeError::Transport)?;
4325 session
4326 .create_service_client(&info, qos)
4327 .map_err(|_| NodeError::Transport(TransportError::ServiceClientCreationFailed))?
4328 };
4329
4330 let offset = self.arena_alloc::<ServiceClientRawArenaEntry<REPLY_BUF>>()?;
4331 unsafe {
4332 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
4333 let entry_ptr = arena_ptr.add(offset) as *mut ServiceClientRawArenaEntry<REPLY_BUF>;
4334 core::ptr::write(
4335 entry_ptr,
4336 ServiceClientRawArenaEntry {
4337 handle,
4338 reply_buffer: [0u8; REPLY_BUF],
4339 pending: false,
4340 reply_ready: core::sync::atomic::AtomicBool::new(false),
4341 callback,
4342 context,
4343 },
4344 );
4345 }
4346
4347 self.entries[slot] = Some(CallbackMeta {
4348 offset,
4349 kind: EntryKind::ServiceClient,
4350 try_process: service_client_raw_try_process::<REPLY_BUF>,
4351 has_data: always_ready,
4352 pre_sample: no_pre_sample,
4353 invocation: InvocationMode::Always,
4354 drop_fn: drop_entry::<ServiceClientRawArenaEntry<REPLY_BUF>>,
4355 });
4356 self.apply_node_default_sched(slot, node_id, None);
4357 Ok(HandleId(slot))
4358 }
4359
4360 /// RFC-0041 / Phase 239.1 — register a **typed callback** service client.
4361 /// The reply is eager-drained at `spin_once` and dispatched to `callback` as
4362 /// a deserialized `Svc::Reply`. Returns the scheduling [`HandleId`] and a
4363 /// `*mut` to the arena entry's send header (used to build the typed
4364 /// [`ServiceClientCallback`](super::handles::ServiceClientCallback)).
4365 #[allow(clippy::too_many_arguments)]
4366 pub(crate) fn register_service_client_callback<Svc, F, const REPLY_BUF: usize>(
4367 &mut self,
4368 node_id: Option<super::node_record::NodeId>,
4369 service_name: &str,
4370 service_type: &str,
4371 service_hash: &str,
4372 qos: QosSettings,
4373 callback: F,
4374 ) -> Result<(HandleId, *mut ServiceClientSendHeader<REPLY_BUF>), NodeError>
4375 where
4376 Svc: nros_core::RosService + 'static,
4377 F: FnMut(&Svc::Reply) + 'static,
4378 {
4379 let slot = self.next_entry_slot()?;
4380 let (node_name, ns, session_idx) = match node_id {
4381 Some(id) => {
4382 let r = self
4383 .nodes
4384 .get(id.index())
4385 .ok_or(NodeError::InvalidSchedContextBinding)?;
4386 (r.name.clone(), r.namespace.clone(), r.session_idx)
4387 }
4388 None => (self.node_name.clone(), self.namespace.clone(), 0u8),
4389 };
4390 let mut info =
4391 ServiceInfo::new(service_name, service_type, service_hash).with_namespace(&ns);
4392 if !node_name.is_empty() {
4393 info = info.with_node_name(&node_name);
4394 }
4395 let handle = {
4396 let session = self
4397 .session_at_mut(session_idx)
4398 .ok_or(NodeError::BackendMismatch)?;
4399 qos.validate_against(session.supported_qos_policies())
4400 .map_err(NodeError::Transport)?;
4401 session
4402 .create_service_client(&info, qos)
4403 .map_err(|_| NodeError::Transport(TransportError::ServiceClientCreationFailed))?
4404 };
4405
4406 let offset = self.arena_alloc::<ServiceClientCallbackEntry<Svc, F, REPLY_BUF>>()?;
4407 let hdr_ptr = unsafe {
4408 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
4409 let entry_ptr =
4410 arena_ptr.add(offset) as *mut ServiceClientCallbackEntry<Svc, F, REPLY_BUF>;
4411 core::ptr::write(
4412 entry_ptr,
4413 ServiceClientCallbackEntry {
4414 hdr: ServiceClientSendHeader {
4415 handle,
4416 reply_buffer: [0u8; REPLY_BUF],
4417 pending: false,
4418 reply_ready: core::sync::atomic::AtomicBool::new(false),
4419 },
4420 callback,
4421 _phantom: core::marker::PhantomData,
4422 },
4423 );
4424 &mut (*entry_ptr).hdr as *mut ServiceClientSendHeader<REPLY_BUF>
4425 };
4426
4427 self.entries[slot] = Some(CallbackMeta {
4428 offset,
4429 kind: EntryKind::ServiceClient,
4430 try_process: service_client_callback_try_process::<Svc, F, REPLY_BUF>,
4431 has_data: always_ready,
4432 pre_sample: no_pre_sample,
4433 invocation: InvocationMode::Always,
4434 drop_fn: drop_entry::<ServiceClientCallbackEntry<Svc, F, REPLY_BUF>>,
4435 });
4436 self.apply_node_default_sched(slot, node_id, None);
4437 Ok((HandleId(slot), hdr_ptr))
4438 }
4439
4440 // ========================================================================
4441 // Guard condition registration
4442 // ========================================================================
4443
4444 /// Register a guard condition with a callback.
4445 ///
4446 /// Returns both the [`HandleId`] for trigger configuration and a
4447 /// [`GuardConditionHandle`] for triggering from other threads.
4448 pub fn register_guard_condition<F>(
4449 &mut self,
4450 callback: F,
4451 ) -> Result<(HandleId, GuardConditionHandle), NodeError>
4452 where
4453 F: FnMut() + 'static,
4454 {
4455 let slot = self.next_entry_slot()?;
4456 let offset = self.arena_alloc::<GuardConditionEntry<F>>()?;
4457
4458 unsafe {
4459 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
4460 let entry_ptr = arena_ptr.add(offset) as *mut GuardConditionEntry<F>;
4461 core::ptr::write(
4462 entry_ptr,
4463 GuardConditionEntry {
4464 flag: portable_atomic::AtomicBool::new(false),
4465 callback,
4466 },
4467 );
4468
4469 // Create a handle pointing to the flag in the arena
4470 let flag_ptr = &(*entry_ptr).flag as *const portable_atomic::AtomicBool;
4471 #[allow(unused_mut)]
4472 let mut guard_handle = GuardConditionHandle::new(flag_ptr);
4473 // Phase 124.B.5 — wire the wake callback so trigger()
4474 // also signals the executor's wake_cv.
4475 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
4476 {
4477 let ctx = self.wake_ctx_ptr();
4478 guard_handle.set_wake_cb(nros_rmw_runtime_wake_cb, ctx);
4479 }
4480
4481 self.entries[slot] = Some(CallbackMeta {
4482 offset,
4483 kind: EntryKind::GuardCondition,
4484 try_process: guard_try_process::<F>,
4485 has_data: guard_has_data::<F>,
4486 pre_sample: no_pre_sample,
4487 invocation: InvocationMode::OnNewData,
4488 drop_fn: drop_entry::<GuardConditionEntry<F>>,
4489 });
4490
4491 Ok((HandleId(slot), guard_handle))
4492 }
4493 }
4494
4495 // ========================================================================
4496 // Timer control methods
4497 // ========================================================================
4498
4499 /// Cancel a timer. A cancelled timer will not fire but still accumulates
4500 /// elapsed time. The timer can be restarted with [`reset_timer()`](Self::reset_timer).
4501 pub fn cancel_timer(&mut self, id: HandleId) -> Result<(), NodeError> {
4502 let meta = self
4503 .entries
4504 .get(id.0)
4505 .and_then(|e| e.as_ref())
4506 .ok_or(NodeError::BufferTooSmall)?;
4507 if !matches!(meta.kind, EntryKind::Timer) {
4508 return Err(NodeError::BufferTooSmall);
4509 }
4510 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
4511 // SAFETY: meta.offset points to a valid TimerEntry<F> which shares
4512 // layout with TimerHeader for its initial fields (both #[repr(C)]).
4513 let header = unsafe { &mut *(arena_ptr.add(meta.offset) as *mut TimerHeader) };
4514 header.cancelled = true;
4515 Ok(())
4516 }
4517
4518 /// Reset a timer. Clears the cancelled state and resets the elapsed time
4519 /// to zero, so the timer starts a fresh period.
4520 pub fn reset_timer(&mut self, id: HandleId) -> Result<(), NodeError> {
4521 let meta = self
4522 .entries
4523 .get(id.0)
4524 .and_then(|e| e.as_ref())
4525 .ok_or(NodeError::BufferTooSmall)?;
4526 if !matches!(meta.kind, EntryKind::Timer) {
4527 return Err(NodeError::BufferTooSmall);
4528 }
4529 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
4530 let header = unsafe { &mut *(arena_ptr.add(meta.offset) as *mut TimerHeader) };
4531 header.cancelled = false;
4532 header.elapsed_ms = 0;
4533 Ok(())
4534 }
4535
4536 /// Check if a timer is cancelled.
4537 pub fn timer_is_cancelled(&self, id: HandleId) -> bool {
4538 let meta = match self.entries.get(id.0).and_then(|e| e.as_ref()) {
4539 Some(m) if matches!(m.kind, EntryKind::Timer) => m,
4540 _ => return false,
4541 };
4542 let arena_ptr = self.arena.as_ptr() as *const u8;
4543 let header = unsafe { &*(arena_ptr.add(meta.offset) as *const TimerHeader) };
4544 header.cancelled
4545 }
4546
4547 /// Get the period of a timer in milliseconds, or `None` if the handle
4548 /// is not a valid timer.
4549 pub fn timer_period_ms(&self, id: HandleId) -> Option<u64> {
4550 let meta = self
4551 .entries
4552 .get(id.0)
4553 .and_then(|e| e.as_ref())
4554 .filter(|m| matches!(m.kind, EntryKind::Timer))?;
4555 let arena_ptr = self.arena.as_ptr() as *const u8;
4556 let header = unsafe { &*(arena_ptr.add(meta.offset) as *const TimerHeader) };
4557 Some(header.period_ms)
4558 }
4559
4560 // ========================================================================
4561 // spin_once (three-phase: readiness -> trigger -> dispatch)
4562 // ========================================================================
4563
4564 /// Drive I/O and dispatch registered callbacks once.
4565 ///
4566 /// Three-phase execution:
4567 /// 1. **Readiness scan** — query each handle's `has_data()`.
4568 /// 2. **Trigger evaluation** — check if the executor-level trigger passes.
4569 /// 3. **Dispatch** — invoke callbacks according to their `InvocationMode`.
4570 ///
4571 /// Returns a [`SpinOnceResult`] with counts of processed items and errors.
4572 ///
4573 /// # Arguments
4574 /// * `timeout` — upper bound on the I/O wait. Saturated at
4575 /// `i32::MAX` ms (~24 days) for the underlying transport call.
4576 ///
4577 /// Phase 84.D7: unified on `core::time::Duration`. The previous
4578 /// `timeout_ms: i32` signature had a latent footgun where
4579 /// `spin_once(-1)` silently froze timers while still polling I/O;
4580 /// `Duration` has no negative sentinel.
4581 pub fn spin_once(&mut self, timeout: core::time::Duration) -> SpinOnceResult {
4582 let timeout_ms = timeout.as_millis().min(i32::MAX as u128) as i32;
4583
4584 // Phase 110.0 — cap against the backend's next internal-event
4585 // deadline (lease keepalive, heartbeat, ACK-NACK timeout, ...).
4586 // Default backend impl returns `None`, so this is a no-op
4587 // unless the active backend opts in.
4588 #[allow(unused_variables)]
4589 let timeout_ms = match self.session.next_deadline_ms() {
4590 Some(next) => timeout_ms.min(next.min(i32::MAX as u32) as i32),
4591 None => timeout_ms,
4592 };
4593
4594 // Wall-clock-accurate timer accumulation. Measure real time
4595 // since the previous `spin_once` exited (or, on the first call,
4596 // since `drive_io` started). Two failure modes the requested
4597 // `timeout_ms` doesn't capture:
4598 // 1. `drive_io` returns early — e.g. zenoh-pico's condvar wakes
4599 // on data arrival, well under 1 ms.
4600 // 2. The caller spends time outside `spin_once` (explicit sleep,
4601 // ROS-2 cooperative scheduling, etc.) and that time should
4602 // still count toward timers.
4603 // Crediting the requested timeout to timers in either case ticks
4604 // them faster than wall-clock — observed as a 30 Hz control loop
4605 // overshooting to >200 Hz under sustained traffic. Carry the
4606 // sub-ms remainder across calls so precision is preserved.
4607 #[cfg(feature = "std")]
4608 let spin_start = std::time::Instant::now();
4609 #[cfg(not(feature = "std"))]
4610 let spin_start_us = self.clock_us_fn.map(|clock| clock());
4611
4612 // RFC-0052 W3b.4 — contract monitors tick once per spin (window
4613 // logic inside; single branch when the baked table is empty).
4614 self.run_contract_monitors();
4615
4616 // Phase 104.C.6 — shared executor wake. Swap-and-clear the
4617 // wake flag; if it was set before this `spin_once` entered,
4618 // skip the blocking wait on the primary session and poll
4619 // every session non-blockingly. Lets a wake signal from any
4620 // thread (or, post-104.C.6.b, any backend's vtable hook)
4621 // pre-empt whichever session the executor would otherwise
4622 // sleep on. Cost on the no-wake path is one atomic swap.
4623 #[cfg(feature = "std")]
4624 #[allow(unused_variables)]
4625 let was_woken = self
4626 .wake_flag
4627 .swap(false, std::sync::atomic::Ordering::SeqCst);
4628
4629 // Phase 124.B.4 — condvar-blocked wait.
4630 //
4631 // RT contract:
4632 // * cv.wait_timeout_while: bounded by `timeout_ms`.
4633 // Predicate is O(1) — one atomic swap + Instant::now.
4634 // No allocation. PI-mutex consideration: wake_mu held
4635 // only during predicate check (microseconds);
4636 // contended worst-case = notify_all execution time
4637 // (~10s of µs).
4638 // * Backend's `set_wake_callback`-installed cb is called
4639 // on async data arrival from its transport-notify path
4640 // (worker thread, ISR-safe variant via 124.B.7). The
4641 // runtime cb writes wake_flag + signals wake_cv,
4642 // unblocking this loop sub-poll-period.
4643 // * Poll-only backends (XRCE, bare-metal) leave the slot
4644 // NULL; the cv wait still fires on its deadline, then
4645 // drive_io(0) drains whatever the backend's internal
4646 // poll has buffered. Equivalent to their pre-124
4647 // behaviour minus the blocking wait inside drive_io.
4648 //
4649 // Lost-wakeup safe: SeqCst flag write happens-before
4650 // notify, and the waiter checks the flag under wake_mu in
4651 // the predicate. If wake fires between drain and cv.wait
4652 // entry, the predicate sees flag=true on first eval and
4653 // exits immediately.
4654 // Phase 130.4 — only sleep in the wake-primitive wait when a
4655 // backend actually installed `set_wake_callback`. Poll-only
4656 // backends (XRCE, current Cyclone / dust-DDS) leave the
4657 // vtable slot NULL → `has_async_wake == false` → drive_io
4658 // for the caller's full timeout instead of sleeping in a
4659 // never-signaled wait that starves reliable retransmission
4660 // (Phase 127.C.4 root cause: server's send_reply flushes
4661 // 100 ms once, then NodeWake.wait_ms(100) sleeps 100 ms
4662 // with zero session activity, so the agent's ACK arrives
4663 // into a stalled session and reliable redelivery never
4664 // fires). RTOS std builds with an event-driven backend
4665 // installed still use `NodeWake` (kernel-native binary
4666 // semaphore — honors its deadline, dodges Zephyr's libc
4667 // `pthread_cond_timedwait` hang); POSIX/macOS std keep
4668 // the existing `std::Condvar` path.
4669 // Phase 248 (C2) — platform-agnostic wake wait. The choice of
4670 // wait primitive is made at runtime from the platform vtable's
4671 // wake probe, NOT a compile-time per-RTOS `cfg`:
4672 //
4673 // * `node_wake.is_some()` → a kernel-native binary semaphore
4674 // (`nros_platform_wake_*`) is linked; block on it. It honors
4675 // its deadline (dodging e.g. Zephyr's libc
4676 // `pthread_cond_timedwait` hang) and `nros_rmw_runtime_wake_cb`
4677 // signals it on transport arrival.
4678 // * `node_wake.is_none()` → no platform wake primitive; fall
4679 // back to the std `Condvar` wake pair.
4680 //
4681 // Either way, only sleep in the wake-primitive/cv wait when a
4682 // backend actually installed `set_wake_callback`
4683 // (`has_async_wake`); poll-only backends (XRCE, current Cyclone)
4684 // leave the slot NULL and get a full-timeout `drive_io` so
4685 // reliable retransmission isn't starved (Phase 127.C.4).
4686 #[cfg(all(feature = "std", feature = "rmw-cffi"))]
4687 let primary_drive_timeout_ms = if let Some(wake) = self.node_wake.as_ref() {
4688 if !was_woken && self.has_async_wake {
4689 let _ = wake.wait_ms(timeout_ms as u32);
4690 // Clear any pending flag the cb set while we were
4691 // waiting; mirrors the std cv predicate's flag drain.
4692 let _ = self
4693 .wake_flag
4694 .swap(false, std::sync::atomic::Ordering::SeqCst);
4695 0
4696 } else {
4697 timeout_ms
4698 }
4699 } else {
4700 if !was_woken && self.has_async_wake {
4701 let dur = core::time::Duration::from_millis(timeout_ms as u64);
4702 // SAFETY-invariant: `wake_mu` guards `()` — it is purely the
4703 // companion mutex for `wake_cv`, protecting no shared state. A
4704 // poison (another thread panicked while holding it) cannot have
4705 // corrupted anything, so recover the guard rather than aborting
4706 // this hot spin loop.
4707 let g = self.wake_mu.lock().unwrap_or_else(|e| e.into_inner());
4708 let _ = self.wake_cv.wait_timeout_while(g, dur, |_| {
4709 !self
4710 .wake_flag
4711 .swap(false, std::sync::atomic::Ordering::SeqCst)
4712 });
4713 }
4714 // drive_io is non-blocking when the cv-wait above ran;
4715 // full-timeout otherwise so the transport's blocking recv
4716 // yields the thread instead of busy-spinning.
4717 if self.has_async_wake { 0 } else { timeout_ms }
4718 };
4719
4720 // std builds without rmw-cffi (mock-session tests, future
4721 // alternative backends) keep the original "drive_io is
4722 // non-blocking" assumption.
4723 #[cfg(all(feature = "std", not(feature = "rmw-cffi")))]
4724 let primary_drive_timeout_ms = 0;
4725
4726 // Phase 248 (C2) — no_std + alloc + rmw-cffi path. When a backend
4727 // installed the wake-cb (`has_async_wake_alloc`) and a platform
4728 // wake primitive is available (`node_wake_alloc.is_some()` — the
4729 // runtime vtable probe), block on `node_wake_alloc.wait_ms` so the
4730 // executor unblocks on transport arrival rather than relying on
4731 // drive_io's blocking recv for the full timeout. Then drive_io(0)
4732 // drains whatever the backend's poll path buffered. Platforms with
4733 // no wake primitive (bare-metal) fall through to the full timeout.
4734 #[cfg(all(feature = "alloc", not(feature = "std"), feature = "rmw-cffi"))]
4735 let primary_drive_timeout_ms = {
4736 let was_woken_alloc = self
4737 .wake_flag_alloc
4738 .swap(false, portable_atomic::Ordering::SeqCst);
4739 if !was_woken_alloc
4740 && self.has_async_wake_alloc
4741 && let Some(wake) = self.node_wake_alloc.as_ref()
4742 {
4743 let _ = wake.wait_ms(timeout_ms as u32);
4744 // Drain any flag the cb set while we were waiting.
4745 let _ = self
4746 .wake_flag_alloc
4747 .swap(false, portable_atomic::Ordering::SeqCst);
4748 0
4749 } else {
4750 timeout_ms
4751 }
4752 };
4753
4754 // no_std without (alloc + rmw-cffi) keeps the legacy
4755 // full-timeout drive_io call.
4756 #[cfg(all(
4757 not(feature = "std"),
4758 not(all(feature = "alloc", feature = "rmw-cffi"))
4759 ))]
4760 let primary_drive_timeout_ms = timeout_ms;
4761
4762 let _ = self.session.drive_io(primary_drive_timeout_ms);
4763 for extra in self.extra_sessions.iter_mut() {
4764 let _ = extra.drive_io(0);
4765 }
4766
4767 #[cfg(feature = "std")]
4768 let delta_ms = {
4769 let now = std::time::Instant::now();
4770 // `last_spin_end` is seeded at construction time, so this
4771 // path always has a Some(_) on every call.
4772 let prev = self.last_spin_end.unwrap_or(spin_start);
4773 let elapsed = now.saturating_duration_since(prev);
4774 self.last_spin_end = Some(now);
4775 let total_us = self
4776 .spin_residual_us
4777 .saturating_add(elapsed.as_micros() as u64);
4778 let ms = total_us / 1000;
4779 self.spin_residual_us = total_us % 1000;
4780 ms
4781 };
4782 #[cfg(not(feature = "std"))]
4783 let delta_ms = if let Some(clock) = self.clock_us_fn {
4784 let now = clock();
4785 let prev = self
4786 .last_spin_end_us
4787 .unwrap_or_else(|| spin_start_us.unwrap_or(now));
4788 self.last_spin_end_us = Some(now);
4789 let elapsed_us = now.saturating_sub(prev);
4790 let total_us = self.spin_residual_us.saturating_add(elapsed_us);
4791 let ms = total_us / 1000;
4792 self.spin_residual_us = total_us % 1000;
4793 ms
4794 } else {
4795 timeout_ms as u64
4796 };
4797 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
4798
4799 // Phase 1: Readiness scan (Phase 110.A.b — backed by FifoReadySet).
4800 //
4801 // `bits` carries data-readiness only (used by trigger eval +
4802 // by `InvocationMode::OnNewData`). `always_mask` carries the
4803 // `InvocationMode::Always` entries that fire regardless of
4804 // data presence. The dispatcher drains
4805 // `FifoReadySet(bits | always_mask)` after the trigger
4806 // passes; `pop_next` yields registration order (lowest bit
4807 // first) so behavior is bit-identical to the pre-refactor
4808 // `for (i, meta) in entries.iter().enumerate()` loop.
4809 let mut bits: u64 = 0;
4810 let mut count: usize = 0;
4811 let mut non_timer_mask: u64 = 0;
4812 let mut always_mask: u64 = 0;
4813
4814 for (i, meta) in self.entries.iter().enumerate() {
4815 if let Some(meta) = meta {
4816 let data_ptr = unsafe { arena_ptr.add(meta.offset) as *const u8 };
4817 if unsafe { (meta.has_data)(data_ptr) } {
4818 bits |= 1u64 << i;
4819 }
4820 if !matches!(meta.kind, EntryKind::Timer | EntryKind::GuardCondition) {
4821 non_timer_mask |= 1u64 << i;
4822 }
4823 if matches!(meta.invocation, InvocationMode::Always) {
4824 always_mask |= 1u64 << i;
4825 }
4826 count += 1;
4827 }
4828 }
4829
4830 let snapshot = ReadinessSnapshot { bits, count };
4831
4832 // Phase 2: Trigger evaluation
4833 let trigger_passes = match &self.trigger {
4834 Trigger::Any => bits & non_timer_mask != 0 || non_timer_mask == 0,
4835 Trigger::All => bits & non_timer_mask == non_timer_mask,
4836 Trigger::One(id) => snapshot.is_ready(*id),
4837 Trigger::AllOf(set) => snapshot.all_ready(*set),
4838 Trigger::AnyOf(set) => snapshot.any_ready(*set),
4839 Trigger::Always => true,
4840 Trigger::Predicate(f) => f(&snapshot),
4841 Trigger::RawPredicate { callback, context } => {
4842 // Convert ReadinessSnapshot bitmask to a bool array for the C callback
4843 let mut ready_array = [false; 64];
4844 for (i, slot) in ready_array
4845 .iter_mut()
4846 .enumerate()
4847 .take(snapshot.count.min(64))
4848 {
4849 *slot = snapshot.bits & (1u64 << i) != 0;
4850 }
4851 // SAFETY: The callback and context are provided by the C API caller.
4852 // The ready_array is valid for snapshot.count elements.
4853 unsafe { callback(ready_array.as_ptr(), snapshot.count, *context) }
4854 }
4855 };
4856
4857 if !trigger_passes {
4858 // Timers still need delta accumulation even when trigger doesn't pass
4859 for meta in self.entries.iter().flatten() {
4860 if matches!(meta.kind, EntryKind::Timer) {
4861 let data_ptr = unsafe { arena_ptr.add(meta.offset) };
4862 let _ = unsafe { (meta.try_process)(data_ptr, delta_ms) };
4863 }
4864 }
4865
4866 // Parameter services live outside the arena and must be processed
4867 // regardless of trigger state, otherwise ROS 2 param queries time out.
4868 #[cfg(feature = "param-services")]
4869 if let Some(params) = &mut self.params {
4870 {
4871 let crate::parameter_services::ParamState {
4872 server, services, ..
4873 } = &mut **params;
4874 let _ = services.process_services(server);
4875 }
4876 // Phase 172.H — persist any runtime override applied this tick.
4877 crate::parameter_services::flush_param_store(params);
4878 }
4879
4880 // Same treatment for lifecycle services — `ros2 lifecycle get`
4881 // must succeed even when no callbacks fired this tick.
4882 // SAFETY: see the matching invariant on the later call site.
4883 #[cfg(feature = "lifecycle-services")]
4884 if let Some(lc) = &mut self.lifecycle {
4885 let crate::lifecycle_services::LifecycleRuntimeState {
4886 state_machine,
4887 services,
4888 } = &mut **lc;
4889 let _ = unsafe { services.process_services(state_machine) };
4890 }
4891
4892 return SpinOnceResult::new();
4893 }
4894
4895 // Phase 2.5: LET pre-sample (only when LogicalExecutionTime)
4896 //
4897 // Sample all subscription data into entry buffers BEFORE dispatching
4898 // any callbacks. This ensures all callbacks in this cycle see a
4899 // consistent snapshot of data from the same point in time.
4900 // Services are NOT pre-sampled (request-reply is sequential).
4901 if matches!(self.semantics, ExecutorSemantics::LogicalExecutionTime) {
4902 for meta in self.entries.iter().flatten() {
4903 if matches!(meta.kind, EntryKind::Subscription) {
4904 let data_ptr = unsafe { arena_ptr.add(meta.offset) };
4905 unsafe { (meta.pre_sample)(data_ptr) };
4906 }
4907 }
4908 }
4909
4910 // Phase 3: Dispatch (Phase 110.C — bucketed by SC.priority).
4911 //
4912 // Two ready-set families, each split across `Priority::COUNT`
4913 // buckets (Critical / Normal / BestEffort). Per-entry SC
4914 // `class` selects FIFO bitmap vs EDF heap; SC `priority`
4915 // selects the bucket within. Drain order:
4916 // for each bucket in priority order (Critical first):
4917 // drain EDF heap (deadline-priority), then FIFO bitmap
4918 // (registration-order)
4919 // Default workloads — every entry on the auto-default Fifo SC
4920 // (Normal priority) — populate only `fifo[Normal]`, so
4921 // dispatch order is bit-identical to 110.B.b for those.
4922 const NB: usize = super::sched_context::Priority::COUNT;
4923 let mut result = SpinOnceResult::new();
4924 let mut fifo: super::ready_set::BucketedFifoSet<NB, { MAX_CALLBACK_SLOTS }> =
4925 super::ready_set::BucketedFifoSet::new();
4926 let mut edf: super::ready_set::BucketedEdfSet<NB, { MAX_CALLBACK_SLOTS }> =
4927 super::ready_set::BucketedEdfSet::new();
4928 let active_mask = bits | always_mask;
4929
4930 // Phase 110.E — refill any Sporadic SC budgets at period
4931 // boundaries before deciding what to dispatch this cycle.
4932 // Refill is polled (not ISR-driven) — coarse but correct
4933 // upper-bound bandwidth limiter.
4934 #[cfg(feature = "std")]
4935 {
4936 // Monotonic ms relative to a process-static epoch so the
4937 // refill clock survives wall-clock jumps.
4938 use std::sync::OnceLock;
4939 static EPOCH: OnceLock<std::time::Instant> = OnceLock::new();
4940 let now_ms = std::time::Instant::now()
4941 .saturating_duration_since(*EPOCH.get_or_init(std::time::Instant::now))
4942 .as_millis() as u64;
4943 // Use the cycle's `delta_ms` as the per-SC consumption
4944 // estimate — worst-case attribution. Per-callback
4945 // measurement lands with a higher-precision clock hook.
4946 let delta_us = (delta_ms as u32).saturating_mul(1000);
4947 for slot in self.sporadic_states.iter_mut().flatten() {
4948 let _ = slot.tick(now_ms, delta_us);
4949 }
4950 }
4951
4952 for i in 0..self.entries.len() {
4953 if active_mask & (1u64 << i) == 0 {
4954 continue;
4955 }
4956 let sc_idx = self.sched_context_bindings[i].0 as usize;
4957 let sc_class_priority_deadline = self
4958 .sched_contexts
4959 .get(sc_idx)
4960 .and_then(|s| s.as_ref())
4961 .map(|sc| {
4962 (
4963 sc.class,
4964 sc.priority.index(),
4965 sc.deadline_us.get().map(|nz| nz.get()).unwrap_or(u32::MAX),
4966 )
4967 });
4968 let (sc_class, bucket, deadline_us) = sc_class_priority_deadline.unwrap_or((
4969 super::sched_context::SchedClass::Fifo,
4970 super::sched_context::Priority::Normal.index(),
4971 u32::MAX,
4972 ));
4973 // Phase 110.E — Sporadic SC dispatch is suppressed when
4974 // its budget is exhausted. Atomic path (110.E.b PlatformTimer
4975 // refill) takes precedence when registered; polled path
4976 // (cycle-level delta_us attribution) handles the unregistered
4977 // case. Either way, exhausted budget skips dispatch.
4978 if matches!(sc_class, super::sched_context::SchedClass::Sporadic) {
4979 #[cfg(feature = "alloc")]
4980 let atomic_has_budget = self
4981 .sporadic_atomic_states
4982 .get(sc_idx)
4983 .and_then(|s| s.as_ref())
4984 .map(|(state, _)| state.has_budget());
4985 #[cfg(not(feature = "alloc"))]
4986 let atomic_has_budget: Option<bool> = None;
4987 let has_budget = match atomic_has_budget {
4988 Some(b) => b,
4989 None => self
4990 .sporadic_states
4991 .get(sc_idx)
4992 .and_then(|s| s.as_ref())
4993 .map(|s| s.budget_remaining_us > 0)
4994 .unwrap_or(true),
4995 };
4996 if !has_budget {
4997 continue;
4998 }
4999 // Phase 110.E.b follow-up — per-callback runtime
5000 // accounting (replaces this cycle-level attribution)
5001 // is applied at dispatch time below via
5002 // `consume_dispatch_runtime_us`. We only update the
5003 // polled-path `SporadicState` (no_std fallback) here
5004 // because the atomic path now records actual
5005 // wall-clock per-callback runtime. The
5006 // `delta_us` over-attribution that previously hit the
5007 // atomic state was a worst-case bandwidth limiter;
5008 // per-callback measurement is strictly tighter.
5009 #[cfg(not(feature = "alloc"))]
5010 {
5011 let _ = sc_idx; // polled-state path lives in
5012 // `sporadic_states`; this branch is a no-op when
5013 // the atomic path is enabled.
5014 }
5015 }
5016 // Phase 110.F — per-callback OS priority routing. Entries
5017 // bound to an SC with `os_pri > 0` dispatch onto a worker
5018 // thread the OS has elevated to that priority; the
5019 // cooperative path is skipped for those entries. Workers
5020 // are spawned lazily.
5021 #[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
5022 {
5023 let os_pri = self
5024 .sched_contexts
5025 .get(sc_idx)
5026 .and_then(|s| s.as_ref())
5027 .map(|sc| sc.os_pri)
5028 .unwrap_or(0);
5029 if os_pri > 0
5030 && let Some(apply_policy) = self.os_priority_apply_policy
5031 {
5032 let worker = self
5033 .os_priority_workers
5034 .entry(os_pri)
5035 .or_insert_with(|| OsPriorityWorker::spawn(os_pri, apply_policy));
5036 if let Some(meta) = self.entries[i].as_ref() {
5037 let _ = worker.try_dispatch(WorkItem {
5038 arena_base: arena_ptr as usize,
5039 arena_offset: meta.offset,
5040 try_process: meta.try_process,
5041 delta_ms,
5042 });
5043 }
5044 continue;
5045 }
5046 }
5047 // Phase 110.G — TT window gate, orthogonal to class.
5048 // Skips dispatch when the SC has a TT window AND the
5049 // current monotonic time is outside it. Both gates apply
5050 // independently — a Sporadic SC with a TT window must
5051 // pass both.
5052 if self.major_frame_us > 0 {
5053 let sc_opt = self.sched_contexts.get(sc_idx).and_then(|s| s.as_ref());
5054 if let Some(sc) = sc_opt {
5055 let off = sc.tt_window_offset_us.get().map(|nz| nz.get()).unwrap_or(0);
5056 let dur = sc
5057 .tt_window_duration_us
5058 .get()
5059 .map(|nz| nz.get())
5060 .unwrap_or(0);
5061 if dur > 0 {
5062 // Compute current phase within the major
5063 // frame using the accumulated `delta_ms` clock
5064 // (std-only precise; no_std uses `delta_ms`
5065 // approximation from spin cadence).
5066 #[cfg(feature = "std")]
5067 let now_us = {
5068 use std::sync::OnceLock;
5069 static EPOCH: OnceLock<std::time::Instant> = OnceLock::new();
5070 std::time::Instant::now()
5071 .saturating_duration_since(
5072 *EPOCH.get_or_init(std::time::Instant::now),
5073 )
5074 .as_micros() as u64
5075 };
5076 #[cfg(not(feature = "std"))]
5077 let now_us = delta_ms.saturating_mul(1000);
5078 let phase = (now_us % self.major_frame_us as u64) as u32;
5079 let in_window = if off + dur <= self.major_frame_us {
5080 phase >= off && phase < off + dur
5081 } else {
5082 // Window wraps the major frame boundary.
5083 let end = (off as u64 + dur as u64) % self.major_frame_us as u64;
5084 phase >= off || (phase as u64) < end
5085 };
5086 if !in_window {
5087 continue;
5088 }
5089 }
5090 }
5091 }
5092 let is_edf = matches!(sc_class, super::sched_context::SchedClass::Edf);
5093 let job = super::types::ActiveJob {
5094 sort_key: if is_edf { deadline_us } else { i as u32 },
5095 desc_idx: i as super::types::DescIdx,
5096 };
5097 if is_edf {
5098 let _ = edf.insert_into(bucket, job);
5099 } else {
5100 let _ = fifo.insert_into(bucket, job);
5101 }
5102 }
5103
5104 // SAFETY: each `desc_idx` we pop was set above only when the
5105 // corresponding `entries[i]` slot was `Some`; no Executor
5106 // mutation happens between that scan and this dispatch.
5107 let dispatch_one = |meta: &CallbackMeta,
5108 arena_ptr: *mut u8,
5109 delta_ms: u64,
5110 result: &mut SpinOnceResult| {
5111 // Phase 141.B.2 — capture T1 at subscription dispatch
5112 // entry. Probe pairs it with the most recent T0 from
5113 // `nros_rmw_runtime_wake_cb` (std + alloc variants)
5114 // and pushes `T1 - T0` onto the ring buffer 141.C
5115 // drains. No-op when the probe feature is off or
5116 // no cycle reader is installed. Other entry kinds
5117 // (Service / Timer / GuardCondition) skip the probe
5118 // because the 141 acceptance is specifically
5119 // wake-to-subscription-dispatch latency.
5120 #[cfg(feature = "wake-latency-probe")]
5121 if matches!(meta.kind, EntryKind::Subscription) {
5122 super::wake_probe::on_dispatch();
5123 }
5124 let data_ptr = unsafe { arena_ptr.add(meta.offset) };
5125 match unsafe { (meta.try_process)(data_ptr, delta_ms) } {
5126 Ok(true) => match meta.kind {
5127 EntryKind::Subscription => result.subscriptions_processed += 1,
5128 EntryKind::Service
5129 | EntryKind::ServiceClient
5130 | EntryKind::ActionServer
5131 | EntryKind::ActionClient => result.services_handled += 1,
5132 EntryKind::Timer => result.timers_fired += 1,
5133 EntryKind::GuardCondition => {}
5134 },
5135 Ok(false) => {}
5136 Err(_) => match meta.kind {
5137 EntryKind::Subscription => result.subscription_errors += 1,
5138 EntryKind::Service
5139 | EntryKind::ServiceClient
5140 | EntryKind::ActionServer
5141 | EntryKind::ActionClient => result.service_errors += 1,
5142 EntryKind::Timer | EntryKind::GuardCondition => {}
5143 },
5144 }
5145 };
5146
5147 // Phase 110.E.b follow-up — per-callback runtime accounting
5148 // for Sporadic SCs. Wall-clock-measure each dispatch and
5149 // consume the elapsed microseconds from the bound SC's
5150 // atomic budget. This replaces the cycle-level over-
5151 // attribution that previously charged the FULL `delta_us`
5152 // against every Sporadic SC regardless of which entries
5153 // actually fired — accurate per-callback measurement is the
5154 // shape the design doc's per-callback runtime acceptance
5155 // calls out. The closure is `feature = "std"`-gated because
5156 // it needs a `core::time::Instant`-equivalent monotonic
5157 // clock; the no_std fallback continues to use the polled
5158 // `SporadicState` path (cycle delta_us) until a board-side
5159 // monotonic-microsecond accessor lands.
5160 #[cfg(feature = "std")]
5161 let consume_dispatch_runtime_us =
5162 |desc_idx: usize,
5163 elapsed_us: u32,
5164 sched_context_bindings: &[super::sched_context::SchedContextId],
5165 sched_contexts: &[Option<super::sched_context::SchedContext>],
5166 #[cfg(feature = "alloc")] sporadic_atomic_states: &[Option<(
5167 portable_atomic_util::Arc<super::sched_context::AtomicSporadicState>,
5168 OpaqueTimerHandle,
5169 )>]| {
5170 let sc_idx = sched_context_bindings[desc_idx].0 as usize;
5171 let sc_class = sched_contexts
5172 .get(sc_idx)
5173 .and_then(|s| s.as_ref())
5174 .map(|sc| sc.class)
5175 .unwrap_or(super::sched_context::SchedClass::Fifo);
5176 if !matches!(sc_class, super::sched_context::SchedClass::Sporadic) {
5177 return;
5178 }
5179 #[cfg(feature = "alloc")]
5180 if let Some((state, _)) =
5181 sporadic_atomic_states.get(sc_idx).and_then(|s| s.as_ref())
5182 {
5183 state.consume(elapsed_us);
5184 // Phase 110.E.b — overrun detection. Cooperative
5185 // single-thread can't preempt a runaway callback,
5186 // so post-dispatch wall-clock comparison delivers
5187 // the same observable signal as the design's
5188 // oneshot-IRQ-and-cancel pattern, without needing
5189 // a separate timer per SC. `budget_capacity_us` is
5190 // the per-period budget the SC was sized against;
5191 // any callback exceeding that has run past its
5192 // bandwidth allotment.
5193 if elapsed_us > state.budget_capacity_us {
5194 state.record_overrun(elapsed_us - state.budget_capacity_us);
5195 }
5196 }
5197 #[cfg(not(feature = "alloc"))]
5198 {
5199 let _ = (sc_idx, elapsed_us);
5200 }
5201 };
5202
5203 // W3b.5 — post-dispatch contract checks. `lat_active` gates the
5204 // per-dispatch publish-count snapshot (attribution of dispatch
5205 // elapsed time to monitored publishers whose counter advanced);
5206 // `dl_active` gates elapsed measurement for deadline actions on
5207 // no_std (std measures anyway for the sporadic path).
5208 let mon_table = self.monitor_table;
5209 let lat_active = mon_table.iter().any(|m| m.max_latency_ms > 0);
5210 #[cfg(not(feature = "std"))]
5211 let dl_active = self.sched_contexts.iter().flatten().any(|sc| {
5212 sc.deadline_us.is_some()
5213 && !matches!(
5214 sc.deadline_action,
5215 super::sched_context::DeadlineAction::Ignore
5216 )
5217 });
5218 #[cfg(not(feature = "std"))]
5219 let mon_clock = self.clock_us_fn;
5220 // Deferred deadline-miss violations (the loop body holds an
5221 // immutable borrow of `self.entries`, so the ring is fed after).
5222 let mut deadline_misses: heapless::Vec<
5223 super::monitor::Violation,
5224 { super::monitor::MAX_VIOLATIONS },
5225 > = heapless::Vec::new();
5226 // SCs whose remaining callbacks this cycle are skipped
5227 // (`DeadlineAction::Skip`) — bitmask over SC slots.
5228 let mut skipped_scs: u64 = 0;
5229
5230 // For each priority bucket (Critical → Normal → BestEffort),
5231 // drain EDF first then FIFO so an EDF callback in this bucket
5232 // beats a FIFO peer at the same priority, but no lower-priority
5233 // entry runs while a higher-priority bucket has work pending.
5234 // Strict static priority across buckets; non-preemptive within
5235 // an in-flight callback (see Phase 110.D).
5236 for bucket in 0..NB {
5237 while let Some(job) = edf.pop_from(bucket) {
5238 let i = job.desc_idx as usize;
5239 let sc_idx = self.sched_context_bindings[i].0 as usize;
5240 if sc_idx < 64 && skipped_scs & (1u64 << sc_idx) != 0 {
5241 continue; // W3b.5 DeadlineAction::Skip containment
5242 }
5243 if let Some(meta) = self.entries[i].as_ref() {
5244 let counts_before = snapshot_pub_counts(mon_table, lat_active);
5245 #[cfg(feature = "std")]
5246 let start = std::time::Instant::now();
5247 #[cfg(not(feature = "std"))]
5248 let start_us = if lat_active || dl_active {
5249 mon_clock.map(|c| c())
5250 } else {
5251 None
5252 };
5253 dispatch_one(meta, arena_ptr, delta_ms, &mut result);
5254 #[cfg(feature = "std")]
5255 let elapsed_us: Option<u32> =
5256 Some(start.elapsed().as_micros().min(u32::MAX as u128) as u32);
5257 #[cfg(not(feature = "std"))]
5258 let elapsed_us: Option<u32> = start_us
5259 .and_then(|t0| mon_clock.map(|c| c().saturating_sub(t0)))
5260 .map(|d| d.min(u32::MAX as u64) as u32);
5261 #[cfg(feature = "std")]
5262 if let Some(elapsed_us) = elapsed_us {
5263 consume_dispatch_runtime_us(
5264 i,
5265 elapsed_us,
5266 &self.sched_context_bindings[..],
5267 &self.sched_contexts[..],
5268 #[cfg(feature = "alloc")]
5269 &self.sporadic_atomic_states[..],
5270 );
5271 }
5272 if let Some(elapsed_us) = elapsed_us {
5273 attribute_latency(mon_table, lat_active, &counts_before, elapsed_us);
5274 check_deadline_miss(
5275 self.sched_contexts.get(sc_idx).and_then(|s| s.as_ref()),
5276 sc_idx,
5277 elapsed_us,
5278 &mut deadline_misses,
5279 &mut skipped_scs,
5280 self.fault_fn,
5281 );
5282 }
5283 }
5284 }
5285 while let Some(job) = fifo.pop_from(bucket) {
5286 let i = job.desc_idx as usize;
5287 let sc_idx = self.sched_context_bindings[i].0 as usize;
5288 if sc_idx < 64 && skipped_scs & (1u64 << sc_idx) != 0 {
5289 continue; // W3b.5 DeadlineAction::Skip containment
5290 }
5291 if let Some(meta) = self.entries[i].as_ref() {
5292 let counts_before = snapshot_pub_counts(mon_table, lat_active);
5293 #[cfg(feature = "std")]
5294 let start = std::time::Instant::now();
5295 #[cfg(not(feature = "std"))]
5296 let start_us = if lat_active || dl_active {
5297 mon_clock.map(|c| c())
5298 } else {
5299 None
5300 };
5301 dispatch_one(meta, arena_ptr, delta_ms, &mut result);
5302 #[cfg(feature = "std")]
5303 let elapsed_us: Option<u32> =
5304 Some(start.elapsed().as_micros().min(u32::MAX as u128) as u32);
5305 #[cfg(not(feature = "std"))]
5306 let elapsed_us: Option<u32> = start_us
5307 .and_then(|t0| mon_clock.map(|c| c().saturating_sub(t0)))
5308 .map(|d| d.min(u32::MAX as u64) as u32);
5309 #[cfg(feature = "std")]
5310 if let Some(elapsed_us) = elapsed_us {
5311 consume_dispatch_runtime_us(
5312 i,
5313 elapsed_us,
5314 &self.sched_context_bindings[..],
5315 &self.sched_contexts[..],
5316 #[cfg(feature = "alloc")]
5317 &self.sporadic_atomic_states[..],
5318 );
5319 }
5320 if let Some(elapsed_us) = elapsed_us {
5321 attribute_latency(mon_table, lat_active, &counts_before, elapsed_us);
5322 check_deadline_miss(
5323 self.sched_contexts.get(sc_idx).and_then(|s| s.as_ref()),
5324 sc_idx,
5325 elapsed_us,
5326 &mut deadline_misses,
5327 &mut skipped_scs,
5328 self.fault_fn,
5329 );
5330 }
5331 }
5332 }
5333 }
5334
5335 // W3b.5 — feed deferred deadline misses into the violation ring.
5336 for v in deadline_misses {
5337 let _ = self.monitor_violations.push(v);
5338 }
5339
5340 // Process parameter services (outside the arena)
5341 #[cfg(feature = "param-services")]
5342 if let Some(params) = &mut self.params {
5343 {
5344 let crate::parameter_services::ParamState {
5345 server, services, ..
5346 } = &mut **params;
5347 if let Ok(n) = services.process_services(server) {
5348 result.services_handled += n;
5349 }
5350 }
5351 // Phase 172.H — persist any runtime override applied this tick.
5352 crate::parameter_services::flush_param_store(params);
5353 }
5354
5355 // Process lifecycle services (outside the arena).
5356 //
5357 // SAFETY: `change_state` dispatches a user-supplied C callback through a
5358 // raw function pointer stored in `LifecyclePollingNodeCtx`. The caller
5359 // of `register_lifecycle_services` guarantees the callback/context pair
5360 // stays live for as long as the executor (see that method's docs).
5361 #[cfg(feature = "lifecycle-services")]
5362 if let Some(lc) = &mut self.lifecycle {
5363 let crate::lifecycle_services::LifecycleRuntimeState {
5364 state_machine,
5365 services,
5366 } = &mut **lc;
5367 if let Ok(n) = unsafe { services.process_services(state_machine) } {
5368 result.services_handled += n;
5369 }
5370 }
5371
5372 // Phase 258 (Track 2, 2a) — executor-owned component tick pass.
5373 // Mirrors `ExecutorNodeRuntime::run_ticks`: after the transport +
5374 // callbacks have been pumped, drive each enrolled component's `tick`
5375 // (service-client/action poll, etc.). `exec_ctx` hands the component
5376 // the whole executor as a raw `*mut Executor` so its tick can
5377 // reborrow it (the same disjoint-field raw-ptr pattern run_ticks
5378 // uses). Index-iterate over `Copy` slots so no borrow of
5379 // `self.component_slots` is held while `tick` runs (which aliases
5380 // `self` through `exec_ctx`).
5381 let exec_ctx = self as *mut Executor as *mut core::ffi::c_void;
5382 let slot_count = self.component_slots.len();
5383 for i in 0..slot_count {
5384 let slot = self.component_slots[i];
5385 // SAFETY: `slot.state` is the leaked cell the matching `tick`
5386 // expects (enrolled via `enroll_component`); `exec_ctx` is a live
5387 // `*mut Executor` for `self`. The slot was copied out, so no
5388 // borrow of `component_slots` is outstanding during the call.
5389 unsafe {
5390 (slot.tick)(slot.state, exec_ctx);
5391 }
5392 }
5393
5394 result
5395 }
5396
5397 /// Drive I/O and dispatch callbacks in an infinite loop.
5398 ///
5399 /// Each iteration calls [`spin_once(timeout_ms)`](Self::spin_once),
5400 /// which pumps the transport and dispatches all registered callbacks.
5401 ///
5402 /// This is the primary run loop for embedded applications:
5403 ///
5404 /// ```ignore
5405 /// let mut executor = Executor::open(&config)?;
5406 /// executor.register_subscription::<Int32, _>("/topic", |msg| { /* ... */ })?;
5407 /// executor.spin(10); // never returns
5408 /// ```
5409 pub fn spin(&mut self, timeout: core::time::Duration) -> ! {
5410 loop {
5411 self.spin_once(timeout);
5412 }
5413 }
5414
5415 /// Phase 104.C.3.3.c — rclcpp-`spin()`-shape no-arg variant.
5416 /// Defaults the per-iteration timeout to 50 ms, which keeps
5417 /// idle binaries from busy-spinning while staying responsive
5418 /// enough for default-QoS messaging.
5419 pub fn spin_default(&mut self) -> ! {
5420 self.spin(core::time::Duration::from_millis(50))
5421 }
5422
5423 /// Drive I/O and dispatch callbacks asynchronously.
5424 ///
5425 /// Runs forever, yielding between poll cycles so that other async tasks
5426 /// (e.g., [`Promise`](super::handles::Promise)) can make progress.
5427 ///
5428 /// Uses only `core::future` — no external async runtime dependency.
5429 ///
5430 /// # Usage patterns
5431 ///
5432 /// ```ignore
5433 /// // Pattern 1: select with a promise (embassy-futures)
5434 /// use embassy_futures::select::{select, Either};
5435 /// let promise = client.call(&req)?;
5436 /// let Either::Second(reply) = select(executor.spin_async(), promise).await
5437 /// else { unreachable!() };
5438 ///
5439 /// // Pattern 2: manual polling (no async runtime)
5440 /// let mut promise = client.call(&req)?;
5441 /// loop {
5442 /// executor.spin_once(core::time::Duration::from_millis(10));
5443 /// if let Ok(Some(r)) = promise.try_recv() { break r; }
5444 /// }
5445 /// ```
5446 pub async fn spin_async(&mut self) -> ! {
5447 loop {
5448 self.spin_once(core::time::Duration::from_millis(1));
5449 core::future::poll_fn::<(), _>(|cx| {
5450 cx.waker().wake_by_ref();
5451 core::task::Poll::Pending
5452 })
5453 .await;
5454 }
5455 }
5456
5457 // ========================================================================
5458 // spin_one_period (no_std)
5459 // ========================================================================
5460
5461 /// Process one iteration and return remaining sleep time.
5462 ///
5463 /// This is `no_std` compatible — the caller is responsible for the actual
5464 /// delay using platform-specific sleep.
5465 ///
5466 /// # Arguments
5467 /// * `period_ms` - Target period in milliseconds
5468 /// * `elapsed_ms` - Time elapsed since last call (used for timer ticking)
5469 ///
5470 /// # Example
5471 ///
5472 /// ```ignore
5473 /// loop {
5474 /// let r = executor.spin_one_period(10, elapsed_ms);
5475 /// platform_sleep_ms(r.remaining_ms);
5476 /// }
5477 /// ```
5478 pub fn spin_one_period(&mut self, period_ms: u64, elapsed_ms: u64) -> SpinPeriodPollingResult {
5479 let result = self.spin_once(core::time::Duration::from_millis(elapsed_ms));
5480 SpinPeriodPollingResult {
5481 work: result,
5482 remaining_ms: period_ms.saturating_sub(elapsed_ms),
5483 }
5484 }
5485}
5486
5487// ============================================================================
5488// Parameter services (cfg param-services)
5489// ============================================================================
5490
5491#[cfg(feature = "param-services")]
5492impl<'s> Executor<'s> {
5493 /// Register the 6 ROS 2 parameter services for this node.
5494 ///
5495 /// Creates service servers for `get_parameters`, `set_parameters`,
5496 /// `set_parameters_atomically`, `list_parameters`, `describe_parameters`,
5497 /// and `get_parameter_types`.
5498 ///
5499 /// The service names follow the ROS 2 convention: `/{namespace}/{node_name}/{suffix}`.
5500 /// For the default namespace `/`, this becomes `/{node_name}/{suffix}` (e.g.
5501 /// `/sentinel/list_parameters`).
5502 ///
5503 /// Parameter services are stored outside the arena and don't consume
5504 /// callback slots.
5505 ///
5506 /// # Example
5507 ///
5508 /// ```ignore
5509 /// let config = ExecutorConfig::from_env().node_name("talker");
5510 /// let mut executor = Executor::open(&config)?;
5511 /// executor.register_parameter_services()?;
5512 /// executor.declare_parameter("start_value", ParameterValue::Integer(0));
5513 /// ```
5514 pub fn register_parameter_services(&mut self) -> Result<(), NodeError> {
5515 use crate::parameter_services::{
5516 DescribeParameters, GetParameterTypes, GetParameters, ListParameters,
5517 PARAM_SERVICE_BUFFER_SIZE, ParameterServiceServers, SetParameters,
5518 SetParametersAtomically,
5519 };
5520 use nros_core::RosService;
5521
5522 type PSrv<Svc> = super::handles::EmbeddedServiceServer<
5523 Svc,
5524 PARAM_SERVICE_BUFFER_SIZE,
5525 PARAM_SERVICE_BUFFER_SIZE,
5526 >;
5527
5528 // Build the node FQN from namespace + node_name, following ROS 2 convention.
5529 // Default namespace "/" → "/{node_name}"; otherwise "/{namespace}/{node_name}".
5530 let mut node_fqn = heapless::String::<256>::new();
5531 let ns: &str = &self.namespace;
5532 let nn: &str = &self.node_name;
5533 if ns.is_empty() || ns == "/" {
5534 node_fqn.push_str("/").map_err(|_| NodeError::NameTooLong)?;
5535 node_fqn.push_str(nn).map_err(|_| NodeError::NameTooLong)?;
5536 } else {
5537 node_fqn.push_str("/").map_err(|_| NodeError::NameTooLong)?;
5538 node_fqn
5539 .push_str(ns.trim_matches('/'))
5540 .map_err(|_| NodeError::NameTooLong)?;
5541 node_fqn.push_str("/").map_err(|_| NodeError::NameTooLong)?;
5542 node_fqn.push_str(nn).map_err(|_| NodeError::NameTooLong)?;
5543 }
5544
5545 /// Build a service name like `{node_fqn}/{suffix}` and create the server handle.
5546 fn create_param_srv<Svc: RosService>(
5547 session: &mut session::ConcreteSession,
5548 node_fqn: &str,
5549 namespace: &str,
5550 node_name: &str,
5551 suffix: &str,
5552 ) -> Result<session::RmwServiceServer, NodeError> {
5553 let mut name = heapless::String::<256>::new();
5554 name.push_str(node_fqn)
5555 .map_err(|_| NodeError::NameTooLong)?;
5556 name.push_str("/").map_err(|_| NodeError::NameTooLong)?;
5557 name.push_str(suffix).map_err(|_| NodeError::NameTooLong)?;
5558 let mut info = ServiceInfo::new(&name, Svc::SERVICE_NAME, Svc::SERVICE_HASH)
5559 .with_namespace(namespace);
5560 if !node_name.is_empty() {
5561 info = info.with_node_name(node_name);
5562 }
5563 session
5564 .create_service_server(&info, QosSettings::services_default())
5565 .map_err(|_| NodeError::Transport(TransportError::ServiceServerCreationFailed))
5566 }
5567
5568 let get_handle = create_param_srv::<GetParameters>(
5569 &mut self.session,
5570 &node_fqn,
5571 ns,
5572 nn,
5573 "get_parameters",
5574 )?;
5575 let set_handle = create_param_srv::<SetParameters>(
5576 &mut self.session,
5577 &node_fqn,
5578 ns,
5579 nn,
5580 "set_parameters",
5581 )?;
5582 let set_atomic_handle = create_param_srv::<SetParametersAtomically>(
5583 &mut self.session,
5584 &node_fqn,
5585 ns,
5586 nn,
5587 "set_parameters_atomically",
5588 )?;
5589 let list_handle = create_param_srv::<ListParameters>(
5590 &mut self.session,
5591 &node_fqn,
5592 ns,
5593 nn,
5594 "list_parameters",
5595 )?;
5596 let desc_handle = create_param_srv::<DescribeParameters>(
5597 &mut self.session,
5598 &node_fqn,
5599 ns,
5600 nn,
5601 "describe_parameters",
5602 )?;
5603 let types_handle = create_param_srv::<GetParameterTypes>(
5604 &mut self.session,
5605 &node_fqn,
5606 ns,
5607 nn,
5608 "get_parameter_types",
5609 )?;
5610
5611 let servers = ParameterServiceServers::new(
5612 PSrv::<GetParameters> {
5613 handle: get_handle,
5614 req_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5615 reply_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5616 _phantom: core::marker::PhantomData,
5617 },
5618 PSrv::<SetParameters> {
5619 handle: set_handle,
5620 req_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5621 reply_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5622 _phantom: core::marker::PhantomData,
5623 },
5624 PSrv::<SetParametersAtomically> {
5625 handle: set_atomic_handle,
5626 req_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5627 reply_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5628 _phantom: core::marker::PhantomData,
5629 },
5630 PSrv::<ListParameters> {
5631 handle: list_handle,
5632 req_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5633 reply_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5634 _phantom: core::marker::PhantomData,
5635 },
5636 PSrv::<DescribeParameters> {
5637 handle: desc_handle,
5638 req_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5639 reply_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5640 _phantom: core::marker::PhantomData,
5641 },
5642 PSrv::<GetParameterTypes> {
5643 handle: types_handle,
5644 req_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5645 reply_buffer: [0u8; PARAM_SERVICE_BUFFER_SIZE],
5646 _phantom: core::marker::PhantomData,
5647 },
5648 );
5649
5650 self.params = Some(alloc::boxed::Box::new(
5651 crate::parameter_services::ParamState {
5652 server: nros_params::ParameterServer::new(),
5653 services: alloc::boxed::Box::new(servers),
5654 store: alloc::boxed::Box::new(nros_params::NullParamStore),
5655 },
5656 ));
5657
5658 Ok(())
5659 }
5660
5661 /// Phase 172.H — attach a parameter-override persistence backend.
5662 ///
5663 /// Call this **after** [`register_parameter_services`](Self::register_parameter_services)
5664 /// and after declaring the plan's default parameters, so persisted
5665 /// overrides win over compile-time defaults. It immediately overlays any
5666 /// values the backend already holds onto the declared parameters, then
5667 /// keeps the store to flush future runtime `set_parameters` changes (the
5668 /// executor flushes from its spin loop whenever a value changed).
5669 ///
5670 /// Returns [`NodeError::NotInitialized`] if parameter services have not
5671 /// been registered yet.
5672 pub fn enable_parameter_persistence(
5673 &mut self,
5674 store: alloc::boxed::Box<dyn nros_params::ParamStore>,
5675 ) -> Result<(), NodeError> {
5676 let state = self.params.as_mut().ok_or(NodeError::NotInitialized)?;
5677 // Overlay persisted overrides onto the declared defaults.
5678 store.load(&mut |name, value| {
5679 let _ = state.server.set(name, value);
5680 });
5681 // Loading persisted state is restoration, not a new runtime change —
5682 // don't let it trigger an immediate re-flush.
5683 state.server.take_dirty();
5684 state.store = store;
5685 Ok(())
5686 }
5687
5688 /// Phase 172.H — like [`enable_parameter_persistence`](Self::enable_parameter_persistence)
5689 /// but boxes the backend for you, so callers (and generated code) need no
5690 /// `Box` import.
5691 pub fn enable_parameter_persistence_with<S>(&mut self, store: S) -> Result<(), NodeError>
5692 where
5693 S: nros_params::ParamStore + 'static,
5694 {
5695 self.enable_parameter_persistence(alloc::boxed::Box::new(store))
5696 }
5697}
5698
5699// ============================================================================
5700// Lifecycle services (cfg lifecycle-services)
5701// ============================================================================
5702
5703#[cfg(feature = "lifecycle-services")]
5704impl<'s> Executor<'s> {
5705 /// Register the five REP-2002 lifecycle services on this executor.
5706 ///
5707 /// After this call, `ros2 lifecycle set|get|list|nodes` can drive the
5708 /// stored [`LifecyclePollingNodeCtx`](crate::lifecycle::LifecyclePollingNodeCtx)
5709 /// through the node's lifecycle. The state machine is created fresh
5710 /// (starting in `Unconfigured`); callers register their transition
5711 /// callbacks via [`Executor::lifecycle_state_machine_mut`].
5712 ///
5713 /// # Safety
5714 /// Registered callbacks on the state machine are C FFI function pointers.
5715 /// The caller must keep the callback code and any context it captures
5716 /// valid for as long as the executor processes services.
5717 pub fn register_lifecycle_services(&mut self) -> Result<(), NodeError> {
5718 use crate::{
5719 lifecycle::LifecyclePollingNodeCtx,
5720 lifecycle_services::{
5721 ChangeState, GetAvailableStates, GetAvailableTransitions, GetState,
5722 LIFECYCLE_SERVICE_BUFFER_SIZE, LifecycleRuntimeState, LifecycleServiceServers,
5723 },
5724 };
5725 use nros_core::RosService;
5726
5727 type LcSrv<Svc> = super::handles::EmbeddedServiceServer<
5728 Svc,
5729 LIFECYCLE_SERVICE_BUFFER_SIZE,
5730 LIFECYCLE_SERVICE_BUFFER_SIZE,
5731 >;
5732
5733 // Build the node FQN from namespace + node_name (same convention as
5734 // register_parameter_services).
5735 let mut node_fqn = heapless::String::<256>::new();
5736 let ns: &str = &self.namespace;
5737 let nn: &str = &self.node_name;
5738 if ns.is_empty() || ns == "/" {
5739 node_fqn.push_str("/").map_err(|_| NodeError::NameTooLong)?;
5740 node_fqn.push_str(nn).map_err(|_| NodeError::NameTooLong)?;
5741 } else {
5742 node_fqn.push_str("/").map_err(|_| NodeError::NameTooLong)?;
5743 node_fqn
5744 .push_str(ns.trim_matches('/'))
5745 .map_err(|_| NodeError::NameTooLong)?;
5746 node_fqn.push_str("/").map_err(|_| NodeError::NameTooLong)?;
5747 node_fqn.push_str(nn).map_err(|_| NodeError::NameTooLong)?;
5748 }
5749
5750 fn create_lc_srv<Svc: RosService>(
5751 session: &mut session::ConcreteSession,
5752 node_fqn: &str,
5753 namespace: &str,
5754 node_name: &str,
5755 suffix: &str,
5756 ) -> Result<session::RmwServiceServer, NodeError> {
5757 let mut name = heapless::String::<256>::new();
5758 name.push_str(node_fqn)
5759 .map_err(|_| NodeError::NameTooLong)?;
5760 name.push_str("/").map_err(|_| NodeError::NameTooLong)?;
5761 name.push_str(suffix).map_err(|_| NodeError::NameTooLong)?;
5762 let mut info = ServiceInfo::new(&name, Svc::SERVICE_NAME, Svc::SERVICE_HASH)
5763 .with_namespace(namespace);
5764 if !node_name.is_empty() {
5765 info = info.with_node_name(node_name);
5766 }
5767 session
5768 .create_service_server(&info, QosSettings::services_default())
5769 .map_err(|_| NodeError::Transport(TransportError::ServiceServerCreationFailed))
5770 }
5771
5772 let cs_handle =
5773 create_lc_srv::<ChangeState>(&mut self.session, &node_fqn, ns, nn, "change_state")?;
5774 let gs_handle =
5775 create_lc_srv::<GetState>(&mut self.session, &node_fqn, ns, nn, "get_state")?;
5776 let gas_handle = create_lc_srv::<GetAvailableStates>(
5777 &mut self.session,
5778 &node_fqn,
5779 ns,
5780 nn,
5781 "get_available_states",
5782 )?;
5783 let gat_handle = create_lc_srv::<GetAvailableTransitions>(
5784 &mut self.session,
5785 &node_fqn,
5786 ns,
5787 nn,
5788 "get_available_transitions",
5789 )?;
5790 let gtg_handle = create_lc_srv::<GetAvailableTransitions>(
5791 &mut self.session,
5792 &node_fqn,
5793 ns,
5794 nn,
5795 "get_transition_graph",
5796 )?;
5797
5798 let servers = LifecycleServiceServers::new(
5799 LcSrv::<ChangeState> {
5800 handle: cs_handle,
5801 req_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5802 reply_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5803 _phantom: core::marker::PhantomData,
5804 },
5805 LcSrv::<GetState> {
5806 handle: gs_handle,
5807 req_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5808 reply_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5809 _phantom: core::marker::PhantomData,
5810 },
5811 LcSrv::<GetAvailableStates> {
5812 handle: gas_handle,
5813 req_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5814 reply_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5815 _phantom: core::marker::PhantomData,
5816 },
5817 LcSrv::<GetAvailableTransitions> {
5818 handle: gat_handle,
5819 req_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5820 reply_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5821 _phantom: core::marker::PhantomData,
5822 },
5823 LcSrv::<GetAvailableTransitions> {
5824 handle: gtg_handle,
5825 req_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5826 reply_buffer: [0u8; LIFECYCLE_SERVICE_BUFFER_SIZE],
5827 _phantom: core::marker::PhantomData,
5828 },
5829 );
5830
5831 self.lifecycle = Some(alloc::boxed::Box::new(LifecycleRuntimeState {
5832 state_machine: LifecyclePollingNodeCtx::new(),
5833 services: alloc::boxed::Box::new(servers),
5834 }));
5835
5836 Ok(())
5837 }
5838
5839 /// Mutable access to the lifecycle state machine, if registered.
5840 ///
5841 /// Used to register transition callbacks before spinning and to read the
5842 /// current state from application code.
5843 pub fn lifecycle_state_machine_mut(
5844 &mut self,
5845 ) -> Option<&mut crate::lifecycle::LifecyclePollingNodeCtx> {
5846 self.lifecycle.as_mut().map(|lc| &mut lc.state_machine)
5847 }
5848
5849 /// Immutable access to the lifecycle state machine, if registered.
5850 pub fn lifecycle_state_machine(&self) -> Option<&crate::lifecycle::LifecyclePollingNodeCtx> {
5851 self.lifecycle.as_ref().map(|lc| &lc.state_machine)
5852 }
5853}
5854
5855// ============================================================================
5856// Parameter declaration API (cfg param-services)
5857// ============================================================================
5858
5859#[cfg(feature = "param-services")]
5860impl<'s> Executor<'s> {
5861 /// Declare a parameter with a value. Returns `true` if successful.
5862 pub fn declare_parameter(&mut self, name: &str, value: nros_params::ParameterValue) -> bool {
5863 if let Some(params) = &mut self.params {
5864 params.server.declare(name, value)
5865 } else {
5866 false
5867 }
5868 }
5869
5870 /// Declare a parameter with a value and descriptor. Returns `true` if successful.
5871 pub fn declare_parameter_with_descriptor(
5872 &mut self,
5873 name: &str,
5874 value: nros_params::ParameterValue,
5875 descriptor: nros_params::ParameterDescriptor,
5876 ) -> bool {
5877 if let Some(params) = &mut self.params {
5878 params
5879 .server
5880 .declare_with_descriptor(name, value, Some(descriptor))
5881 } else {
5882 false
5883 }
5884 }
5885
5886 /// Get a parameter value by name.
5887 pub fn get_parameter(&self, name: &str) -> Option<&nros_params::ParameterValue> {
5888 self.params.as_ref()?.server.get(name)
5889 }
5890
5891 /// Get an integer parameter value by name (convenience).
5892 pub fn get_parameter_integer(&self, name: &str) -> Option<i64> {
5893 self.params.as_ref()?.server.get_integer(name)
5894 }
5895
5896 /// Get a reference to the parameter server (if registered).
5897 pub fn params(&self) -> Option<&nros_params::ParameterServer> {
5898 self.params.as_ref().map(|p| &p.server)
5899 }
5900
5901 /// Get a mutable reference to the parameter server (if registered).
5902 pub fn params_mut(&mut self) -> Option<&mut nros_params::ParameterServer> {
5903 self.params.as_mut().map(|p| &mut p.server)
5904 }
5905
5906 /// Create a typed parameter builder (rclrs-compatible API).
5907 ///
5908 /// Returns a [`ParameterBuilder`] for fluent parameter declaration with
5909 /// `.default()`, `.description()`, `.range()`, and terminal methods
5910 /// `.mandatory()`, `.optional()`, or `.read_only()`.
5911 ///
5912 /// Returns [`NodeError::NotInitialized`] if parameter services have
5913 /// not been registered yet — call [`register_parameter_services`]
5914 /// first.
5915 ///
5916 /// # Example
5917 ///
5918 /// ```ignore
5919 /// let max_speed = executor.parameter::<f64>("max_speed")?
5920 /// .default(25.0)
5921 /// .description("Maximum velocity (m/s)")
5922 /// .read_only()?;
5923 /// ```
5924 ///
5925 /// [`ParameterBuilder`]: nros_params::ParameterBuilder
5926 /// [`register_parameter_services`]: Self::register_parameter_services
5927 pub fn parameter<'a, T: nros_params::ParameterVariant>(
5928 &'a mut self,
5929 name: &'a str,
5930 ) -> Result<nros_params::ParameterBuilder<'a, T>, NodeError> {
5931 let server = self
5932 .params
5933 .as_mut()
5934 .map(|p| &mut p.server)
5935 .ok_or(NodeError::NotInitialized)?;
5936 Ok(nros_params::ParameterBuilder::new(server, name))
5937 }
5938}
5939
5940// ============================================================================
5941// std-gated spin and halt methods
5942// ============================================================================
5943
5944#[cfg(feature = "std")]
5945impl<'s> Executor<'s> {
5946 /// Blocking spin loop with configurable exit conditions.
5947 ///
5948 /// Runs until one of:
5949 /// - [`halt()`](Self::halt) is called (from another thread or signal handler)
5950 /// - Timeout expires (if set in options)
5951 /// - Max callbacks reached (if set in options)
5952 /// - `only_next` is true (single iteration)
5953 ///
5954 /// # Example
5955 ///
5956 /// ```ignore
5957 /// // Spin forever until halted
5958 /// executor.spin_blocking(SpinOptions::default())?;
5959 ///
5960 /// // Spin with 5-second timeout
5961 /// executor.spin_blocking(SpinOptions::new().timeout_ms(5000))?;
5962 ///
5963 /// // Single iteration
5964 /// executor.spin_blocking(SpinOptions::spin_once())?;
5965 /// ```
5966 pub fn spin_blocking(&mut self, opts: SpinOptions) -> Result<(), NodeError> {
5967 use std::time::{Duration, Instant};
5968
5969 const POLL_INTERVAL: core::time::Duration = core::time::Duration::from_millis(10);
5970
5971 let start = Instant::now();
5972 let timeout = opts.timeout_ms.map(Duration::from_millis);
5973 let mut total_callbacks = 0usize;
5974
5975 self.halt_flag
5976 .store(false, std::sync::atomic::Ordering::SeqCst);
5977
5978 loop {
5979 if self.halt_flag.load(std::sync::atomic::Ordering::SeqCst) {
5980 break;
5981 }
5982
5983 if timeout.is_some_and(|t| start.elapsed() >= t) {
5984 break;
5985 }
5986
5987 let result = self.spin_once(POLL_INTERVAL);
5988 total_callbacks += result.total();
5989
5990 if opts.max_callbacks.is_some_and(|max| total_callbacks >= max) {
5991 break;
5992 }
5993
5994 if opts.only_next {
5995 break;
5996 }
5997 }
5998
5999 Ok(())
6000 }
6001
6002 /// Execute one period with wall-clock overrun detection.
6003 ///
6004 /// Calls [`spin_once()`](Self::spin_once), measures wall-clock time, sleeps
6005 /// for the remainder if under budget.
6006 ///
6007 /// # Example
6008 ///
6009 /// ```ignore
6010 /// let period = std::time::Duration::from_millis(10);
6011 /// let result = executor.spin_one_period_timed(period);
6012 /// if result.overrun {
6013 /// log::warn!("Period overrun: {:?}", result.elapsed);
6014 /// }
6015 /// ```
6016 pub fn spin_one_period_timed(
6017 &mut self,
6018 period: std::time::Duration,
6019 ) -> super::types::SpinPeriodResult {
6020 let start = std::time::Instant::now();
6021 let result = self.spin_once(period);
6022 let elapsed = start.elapsed();
6023 let overrun = elapsed > period;
6024 if !overrun {
6025 std::thread::sleep(period - elapsed);
6026 }
6027 super::types::SpinPeriodResult {
6028 work: result,
6029 overrun,
6030 elapsed,
6031 }
6032 }
6033
6034 /// Spin at a fixed rate with drift compensation. Blocks until halted.
6035 ///
6036 /// Uses wall-clock time to maintain the target rate. The next invocation
6037 /// time is accumulated (not reset to `now + period`) to prevent cumulative
6038 /// drift.
6039 ///
6040 /// # Example
6041 ///
6042 /// ```ignore
6043 /// // 100Hz control loop — blocks until halt() is called
6044 /// executor.spin_period(std::time::Duration::from_millis(10))?;
6045 /// ```
6046 pub fn spin_period(&mut self, period: std::time::Duration) -> Result<(), NodeError> {
6047 self.halt_flag
6048 .store(false, std::sync::atomic::Ordering::SeqCst);
6049 let mut next_invocation = std::time::Instant::now() + period;
6050
6051 loop {
6052 if self.halt_flag.load(std::sync::atomic::Ordering::SeqCst) {
6053 break;
6054 }
6055
6056 self.spin_once(period);
6057
6058 let now = std::time::Instant::now();
6059 if now < next_invocation {
6060 std::thread::sleep(next_invocation - now);
6061 }
6062 // Accumulate to prevent drift (not = now + period)
6063 next_invocation += period;
6064 }
6065 Ok(())
6066 }
6067
6068 /// Request the executor to stop spinning.
6069 ///
6070 /// Sets a flag that causes [`spin_blocking()`](Self::spin_blocking) or
6071 /// [`spin_period()`](Self::spin_period) to exit on the next iteration.
6072 /// Safe to call from another thread or signal handler.
6073 ///
6074 /// Also raises the Phase 104.C.6 wake flag so a `spin_once` already
6075 /// blocked inside a backend's `drive_io` falls through to the halt
6076 /// check on its next loop iteration instead of waiting out its full
6077 /// `timeout_ms` first.
6078 pub fn halt(&self) {
6079 self.halt_flag
6080 .store(true, std::sync::atomic::Ordering::SeqCst);
6081 self.wake_flag
6082 .store(true, std::sync::atomic::Ordering::SeqCst);
6083 }
6084
6085 /// Phase 110.D.b — move this Executor onto a fresh OS thread,
6086 /// apply a per-thread scheduling policy via the caller-supplied
6087 /// `apply_policy` function, and run the spin loop until
6088 /// [`ThreadHandle::halt`] fires.
6089 ///
6090 /// The function-pointer indirection on `apply_policy` lets the
6091 /// caller pass any platform's `PlatformScheduler::set_current_thread_policy`
6092 /// without forcing `Executor` to be generic over the platform —
6093 /// keeps the existing `Executor` type stable.
6094 ///
6095 /// Multi-executor preemption (the actual hard-RT win) comes from
6096 /// the OS scheduler — call `open_threaded` once per criticality
6097 /// tier, each with its own policy / priority. The kernel handles
6098 /// preemption across executors; within a single executor,
6099 /// dispatch remains non-preemptive (110.A–C bucketed sets).
6100 ///
6101 /// # Safety
6102 ///
6103 /// Moves `self` across thread boundaries. `Executor` contains a
6104 /// raw `*mut session::ConcreteSession` when constructed via
6105 /// `from_session_ptr`; the caller must ensure that pointer's
6106 /// referent stays valid across the lifetime of the spawned thread
6107 /// and that no other thread mutates the session concurrently.
6108 /// `from_session` (Owned) is safer — `ConcreteSession` ownership
6109 /// transfers cleanly into the thread.
6110 #[cfg(feature = "std")]
6111 pub unsafe fn open_threaded(
6112 self,
6113 policy: nros_platform_api::SchedPolicy,
6114 apply_policy: fn(
6115 nros_platform_api::SchedPolicy,
6116 ) -> Result<(), nros_platform_api::SchedError>,
6117 spin_period: core::time::Duration,
6118 ) -> ThreadHandle
6119 where
6120 // phase-271 — the spawned thread owns `self` for an unbounded lifetime,
6121 // so its borrowed storage must be `'static` (a leaked/`static` backing —
6122 // the `alloc` convenience constructors or a program-lifetime region).
6123 's: 'static,
6124 {
6125 let halt = std::sync::Arc::clone(&self.halt_flag);
6126 // SAFETY: Send is asserted via `unsafe impl Send for Executor`
6127 // below; the caller's safety contract on `from_session_ptr`
6128 // covers the pointer-validity invariant.
6129 let mut executor = self;
6130 let join = std::thread::spawn(move || {
6131 // Apply the requested OS scheduling policy to this fresh
6132 // thread. Failure is reported but not propagated — a
6133 // runtime that fails to lift to SCHED_FIFO still spins
6134 // correctly at SCHED_OTHER (just without RT guarantees).
6135 let _ = apply_policy(policy);
6136 while !executor.is_halted() {
6137 executor.spin_once(spin_period);
6138 }
6139 });
6140 ThreadHandle {
6141 join: Some(join),
6142 halt,
6143 }
6144 }
6145
6146 /// Check if halt has been requested.
6147 pub fn is_halted(&self) -> bool {
6148 self.halt_flag.load(std::sync::atomic::Ordering::SeqCst)
6149 }
6150
6151 /// Get a clone of the halt flag for use in signal handlers or other threads.
6152 ///
6153 /// # Example
6154 ///
6155 /// ```ignore
6156 /// let halt = executor.halt_flag();
6157 /// std::thread::spawn(move || {
6158 /// std::thread::sleep(Duration::from_secs(5));
6159 /// halt.store(true, Ordering::SeqCst);
6160 /// });
6161 /// executor.spin_blocking(SpinOptions::default())?;
6162 /// ```
6163 pub fn halt_flag(&self) -> std::sync::Arc<std::sync::atomic::AtomicBool> {
6164 self.halt_flag.clone()
6165 }
6166
6167 /// Phase 104.C.6 — wake the executor from another thread / ISR /
6168 /// signal handler.
6169 ///
6170 /// Sets the shared `wake_flag`. The next `spin_once` swap-clears the
6171 /// flag, skips the blocking wait on the primary session, and polls
6172 /// every session non-blockingly so whatever queued the wake is
6173 /// observed in a single iteration. Idempotent — multiple `wake()`
6174 /// calls collapse into one observed wake per `spin_once`.
6175 pub fn wake(&self) {
6176 self.wake_flag
6177 .store(true, std::sync::atomic::Ordering::SeqCst);
6178 }
6179
6180 /// Phase 104.C.6 — clone of the shared wake flag for cross-thread
6181 /// use (signal handlers, foreign threads, future per-backend vtable
6182 /// wake hooks).
6183 ///
6184 /// # Example
6185 ///
6186 /// ```ignore
6187 /// let wake = executor.wake_handle();
6188 /// std::thread::spawn(move || {
6189 /// // ... compute something ...
6190 /// // hand off to executor by setting the flag.
6191 /// wake.store(true, Ordering::SeqCst);
6192 /// });
6193 /// loop { executor.spin_once(Duration::from_millis(100)); }
6194 /// ```
6195 pub fn wake_handle(&self) -> std::sync::Arc<std::sync::atomic::AtomicBool> {
6196 self.wake_flag.clone()
6197 }
6198}
6199
6200/// Phase 110.E.b — opaque per-platform timer handle. Stores the
6201/// raw platform handle (POSIX `timer_t` boxed via `PosixTimerHandle`,
6202/// FreeRTOS `TimerHandle_t`, etc.) plus a destroy thunk so the
6203/// Executor can clean up without being generic over the platform.
6204///
6205/// Caller of `register_sporadic_timer` builds this via
6206/// `OpaqueTimerHandle::new(handle, destroy_fn)` after their
6207/// `PlatformTimer::create_periodic` call returns.
6208#[cfg(feature = "alloc")]
6209pub struct OpaqueTimerHandle {
6210 handle: *mut core::ffi::c_void,
6211 destroy_fn: extern "C" fn(*mut core::ffi::c_void),
6212}
6213
6214#[cfg(feature = "alloc")]
6215unsafe impl Send for OpaqueTimerHandle {}
6216#[cfg(feature = "alloc")]
6217unsafe impl Sync for OpaqueTimerHandle {}
6218
6219#[cfg(feature = "alloc")]
6220impl OpaqueTimerHandle {
6221 /// # Safety
6222 /// `handle` must be a live platform-specific timer handle that
6223 /// `destroy_fn` knows how to drop. Caller surrenders ownership
6224 /// of the underlying handle to the Executor.
6225 pub unsafe fn new(
6226 handle: *mut core::ffi::c_void,
6227 destroy_fn: extern "C" fn(*mut core::ffi::c_void),
6228 ) -> Self {
6229 Self { handle, destroy_fn }
6230 }
6231}
6232
6233#[cfg(feature = "alloc")]
6234impl Drop for OpaqueTimerHandle {
6235 fn drop(&mut self) {
6236 if !self.handle.is_null() {
6237 (self.destroy_fn)(self.handle);
6238 self.handle = core::ptr::null_mut();
6239 }
6240 }
6241}
6242
6243/// Handle returned from [`Executor::open_threaded`]. Holds the
6244/// spawned thread's join handle and a clone of the executor's halt
6245/// flag. Drop runs `halt() + join()` so the thread can't outlive the
6246/// handle.
6247#[cfg(feature = "std")]
6248pub struct ThreadHandle {
6249 join: Option<std::thread::JoinHandle<()>>,
6250 halt: std::sync::Arc<std::sync::atomic::AtomicBool>,
6251}
6252
6253#[cfg(feature = "std")]
6254impl ThreadHandle {
6255 /// Signal the spawned executor thread to stop. The thread exits
6256 /// on its next `spin_once` iteration.
6257 pub fn halt(&self) {
6258 self.halt.store(true, std::sync::atomic::Ordering::SeqCst);
6259 }
6260
6261 /// Wait for the spawned thread to exit. Returns the join result.
6262 /// After `join`, calling it again is a no-op (returns `Ok(())`).
6263 pub fn join(mut self) -> std::thread::Result<()> {
6264 self.halt();
6265 match self.join.take() {
6266 Some(j) => j.join(),
6267 None => Ok(()),
6268 }
6269 }
6270}
6271
6272#[cfg(feature = "std")]
6273impl Drop for ThreadHandle {
6274 fn drop(&mut self) {
6275 self.halt.store(true, std::sync::atomic::Ordering::SeqCst);
6276 if let Some(j) = self.join.take() {
6277 let _ = j.join();
6278 }
6279 }
6280}
6281
6282// SAFETY: Phase 110.D.b — `Executor` contains a raw `*mut
6283// session::ConcreteSession` only on the `from_session_ptr` (Borrowed)
6284// path; the `from_session` (Owned) path is plain Send-able. The
6285// `unsafe fn open_threaded` entry point documents the safety
6286// contract for Borrowed sessions; for Owned sessions the Send claim
6287// is unconditional.
6288#[cfg(feature = "std")]
6289unsafe impl<'s> Send for Executor<'s> {}
6290
6291// =============================================================================
6292// Phase 110.F — `OsPriorityWorker` + `WorkItem`
6293// =============================================================================
6294
6295/// One worker thread per distinct `SchedContext.os_pri` value used
6296/// across registered SCs. Self-elevates via the executor's stored
6297/// `apply_policy` fn pointer at startup; drains a bounded mpsc
6298/// mailbox of `WorkItem`s. Phase 110.F.
6299#[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
6300pub(crate) struct OsPriorityWorker {
6301 sender: std::sync::mpsc::Sender<WorkItem>,
6302 halt: std::sync::Arc<std::sync::atomic::AtomicBool>,
6303 join: Option<std::thread::JoinHandle<()>>,
6304}
6305
6306#[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
6307struct WorkItem {
6308 arena_base: usize,
6309 arena_offset: usize,
6310 try_process: unsafe fn(*mut u8, u64) -> Result<bool, nros_rmw::TransportError>,
6311 delta_ms: u64,
6312}
6313
6314// SAFETY: Phase 110.F per-DescIdx exclusive-access invariant — the
6315// activator scan in `spin_once` only sends a `WorkItem` for a given
6316// `arena_offset` to one worker per cycle, and won't re-send the same
6317// offset until the worker drains the previous one (`os_pri` dispatch
6318// is the worker's exclusive path; cooperative dispatch is skipped
6319// for SCs with non-zero `os_pri`). The fn pointer is Send-clean.
6320#[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
6321unsafe impl Send for WorkItem {}
6322
6323#[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
6324impl OsPriorityWorker {
6325 fn spawn(
6326 os_pri: u8,
6327 apply_policy: fn(
6328 nros_platform_api::SchedPolicy,
6329 ) -> Result<(), nros_platform_api::SchedError>,
6330 ) -> Self {
6331 use std::sync::atomic::Ordering;
6332 let (tx, rx) = std::sync::mpsc::channel::<WorkItem>();
6333 let halt = std::sync::Arc::new(std::sync::atomic::AtomicBool::new(false));
6334 let halt_w = std::sync::Arc::clone(&halt);
6335 let join = std::thread::Builder::new()
6336 .name(alloc::format!("nros-os-pri-{os_pri}"))
6337 .spawn(move || {
6338 // Self-elevate. Failure is logged but doesn't stop
6339 // the worker — running at SCHED_OTHER is still
6340 // correct, just without the priority guarantee.
6341 let _ = apply_policy(nros_platform_api::SchedPolicy::Fifo { os_pri });
6342 while !halt_w.load(Ordering::Acquire) {
6343 match rx.recv_timeout(core::time::Duration::from_millis(10)) {
6344 Ok(item) => {
6345 // SAFETY: arena_base + arena_offset point
6346 // into the executor's arena, which
6347 // outlives the worker per Drop ordering
6348 // (Executor::Drop halts + joins workers
6349 // before the arena is freed).
6350 let data = (item.arena_base as *mut u8).wrapping_add(item.arena_offset);
6351 let _ = unsafe { (item.try_process)(data, item.delta_ms) };
6352 }
6353 Err(std::sync::mpsc::RecvTimeoutError::Timeout) => continue,
6354 Err(std::sync::mpsc::RecvTimeoutError::Disconnected) => break,
6355 }
6356 }
6357 })
6358 // SAFETY-invariant: spawn failure means the OS refused a new
6359 // thread (resource exhaustion). This runs once per priority
6360 // level at lazy worker setup — not on any hot/spin path — and
6361 // a runtime that cannot create its priority worker has no
6362 // correct way to continue, so fail fast at the setup point.
6363 .expect("os-priority worker spawn");
6364 Self {
6365 sender: tx,
6366 halt,
6367 join: Some(join),
6368 }
6369 }
6370
6371 fn try_dispatch(&self, item: WorkItem) -> bool {
6372 self.sender.send(item).is_ok()
6373 }
6374}
6375
6376#[cfg(all(feature = "std", feature = "scheduler-os-priority"))]
6377impl Drop for OsPriorityWorker {
6378 fn drop(&mut self) {
6379 self.halt.store(true, std::sync::atomic::Ordering::Release);
6380 if let Some(j) = self.join.take() {
6381 let _ = j.join();
6382 }
6383 }
6384}
6385
6386impl<'s> Drop for Executor<'s> {
6387 fn drop(&mut self) {
6388 // Phase 258 (Track 2, 2a) — release executor-owned component cells
6389 // first (before the arena entries), so a component's `drop`
6390 // trampoline can still touch its own (cell-owned) state. Each slot
6391 // owns a leaked `Arc<ComponentCell>`; its `drop` reconstitutes +
6392 // drops that Arc exactly once.
6393 for slot in self.component_slots.iter() {
6394 // SAFETY: `slot.state` is the leaked cell enrolled via
6395 // `enroll_component`; `slot.drop` is its matching trampoline, run
6396 // exactly once here (slots are not removed before Drop).
6397 unsafe {
6398 (slot.drop)(slot.state);
6399 }
6400 }
6401 let arena_ptr = self.arena.as_mut_ptr() as *mut u8;
6402 for meta in self.entries.iter().flatten() {
6403 // SAFETY: each entry was written by `ptr::write` in `add_*` and
6404 // has not been dropped yet. `drop_fn` matches the concrete type.
6405 unsafe {
6406 let data_ptr = arena_ptr.add(meta.offset);
6407 (meta.drop_fn)(data_ptr);
6408 }
6409 }
6410 }
6411}
6412
6413#[cfg(all(test, not(feature = "rmw-cffi")))]
6414mod dispatch_registry_tests {
6415 //! Phase 216 follow-up — `Executor::register_dispatch_slot` +
6416 //! `Executor::dispatch_callback` round-trip.
6417 //!
6418 //! Uses `MockSession` (same pattern as
6419 //! `lifecycle_services::tests::mock_integration`) so the test
6420 //! doesn't need a live RMW backend. Gated `not(feature =
6421 //! "rmw-cffi")` because under `rmw-cffi` the `ConcreteSession`
6422 //! type alias resolves to the cffi session, which `MockSession`
6423 //! can't impersonate.
6424
6425 extern crate alloc;
6426
6427 use super::Executor;
6428 use crate::mock::MockSession;
6429 use std::sync::Mutex;
6430
6431 static CAPTURED: Mutex<alloc::vec::Vec<(usize, alloc::vec::Vec<u8>, usize)>> =
6432 Mutex::new(alloc::vec::Vec::new());
6433
6434 /// Test trampoline matching the per-Node
6435 /// `__nros_node_<pkg>_on_callback` ABI shape (Phase 216.A.5).
6436 unsafe extern "C" fn recording_on_callback(
6437 state: *mut core::ffi::c_void,
6438 cb_id_ptr: *const u8,
6439 cb_id_len: usize,
6440 ctx: *mut core::ffi::c_void,
6441 ) {
6442 // SAFETY: caller (test body below) holds storage live;
6443 // `cb_id_ptr..len` points into a `&str` literal.
6444 let cb_id_bytes = unsafe { core::slice::from_raw_parts(cb_id_ptr, cb_id_len).to_vec() };
6445 let mut guard = CAPTURED.lock().expect("CAPTURED poisoned");
6446 guard.push((state as usize, cb_id_bytes, ctx as usize));
6447 }
6448
6449 #[test]
6450 fn register_dispatch_slot_round_trip() {
6451 let session = MockSession::new();
6452 let mut executor: Executor = Executor::from_session(session);
6453
6454 // Pre-condition: empty registry.
6455 assert_eq!(executor.dispatch_slot_count(), 0);
6456
6457 // Two distinct "states" so we prove every slot gets called
6458 // with its OWN state.
6459 let mut state_blob_a: u32 = 0xABCD_0001;
6460 let mut state_blob_b: u32 = 0xABCD_0002;
6461 let state_a_ptr = &mut state_blob_a as *mut u32 as *mut core::ffi::c_void;
6462 let state_b_ptr = &mut state_blob_b as *mut u32 as *mut core::ffi::c_void;
6463
6464 executor
6465 .register_dispatch_slot(state_a_ptr, recording_on_callback)
6466 .expect("register slot A");
6467 executor
6468 .register_dispatch_slot(state_b_ptr, recording_on_callback)
6469 .expect("register slot B");
6470 assert_eq!(executor.dispatch_slot_count(), 2);
6471
6472 let mut ctx_blob: u32 = 0xFEED_BEEF;
6473 let ctx_ptr = &mut ctx_blob as *mut u32 as *mut core::ffi::c_void;
6474 let cb_id = "/talker/timer/publish";
6475
6476 CAPTURED.lock().expect("CAPTURED poisoned").clear();
6477 executor.dispatch_callback(cb_id, ctx_ptr);
6478
6479 let captured = CAPTURED.lock().expect("CAPTURED poisoned").clone();
6480 assert_eq!(
6481 captured.len(),
6482 2,
6483 "every registered slot must be invoked — linear scan, \
6484 no self-filter at the registry layer"
6485 );
6486 // heapless::Vec iterates in insertion order.
6487 assert_eq!(captured[0].0, state_a_ptr as usize, "slot A's state");
6488 assert_eq!(captured[1].0, state_b_ptr as usize, "slot B's state");
6489 for (idx, capture) in captured.iter().enumerate() {
6490 assert_eq!(
6491 capture.1.as_slice(),
6492 cb_id.as_bytes(),
6493 "slot {idx} cb_id bytes round-trip"
6494 );
6495 assert_eq!(
6496 capture.2, ctx_ptr as usize,
6497 "slot {idx} ctx pointer round-trip"
6498 );
6499 }
6500 }
6501
6502 #[test]
6503 fn register_dispatch_slot_capacity_full() {
6504 let session = MockSession::new();
6505 let mut executor: Executor = Executor::from_session(session);
6506
6507 let mut state_blob: u32 = 0;
6508 let state_ptr = &mut state_blob as *mut u32 as *mut core::ffi::c_void;
6509
6510 // `MAX_NODES` slots fit; the next one must error.
6511 for _ in 0..crate::config::MAX_NODES {
6512 executor
6513 .register_dispatch_slot(state_ptr, recording_on_callback)
6514 .expect("under-capacity push must succeed");
6515 }
6516 assert_eq!(executor.dispatch_slot_count(), crate::config::MAX_NODES);
6517 let overflow = executor.register_dispatch_slot(state_ptr, recording_on_callback);
6518 assert!(
6519 overflow.is_err(),
6520 "over-capacity push must return Err(()) — raise \
6521 NROS_EXECUTOR_MAX_NODES at build time to grow the registry"
6522 );
6523 }
6524}
6525
6526/// Phase 274.W1 — borrowed-executor session sharing + active-groups gating.
6527///
6528/// Validates three primitives introduced for RFC-0015 Model 1:
6529/// - `session_handle` / `open_with_session_handle` (Borrowed session store —
6530/// the borrowed executor does not own or close the session on drop).
6531/// - `set_active_groups` + `group_active` (callback-group filter gating).
6532///
6533/// Uses `MockSession` (same pattern as `dispatch_registry_tests`); gated
6534/// `not(feature = "rmw-cffi")` for the same reason.
6535#[cfg(all(test, not(feature = "rmw-cffi")))]
6536mod p274_w1_tier_executor_tests {
6537 use super::Executor;
6538 use crate::mock::MockSession;
6539
6540 #[test]
6541 fn session_handle_borrowed_executor_shares_session_ptr() {
6542 // Open the primary executor (session owner).
6543 let session = MockSession::new();
6544 let mut primary = Executor::from_session(session);
6545
6546 // Record the primary's session pointer for later comparison.
6547 let primary_session_ptr = primary.session_ptr();
6548
6549 // Get the opaque session handle (into_raw for C FFI; here we keep it raw).
6550 let handle = primary.session_handle();
6551
6552 // Open a second executor over the SAME session (Borrowed — does not own it).
6553 // SAFETY: `primary` (the session owner) outlives `borrowed` in this scope.
6554 let mut borrowed = unsafe { Executor::open_with_session_handle(handle) };
6555
6556 // Both executors must expose the same session pointer.
6557 assert_eq!(
6558 primary.session_ptr(),
6559 borrowed.session_ptr(),
6560 "borrowed executor must share the primary's session pointer"
6561 );
6562 assert_eq!(
6563 borrowed.session_ptr(),
6564 primary_session_ptr,
6565 "session pointer must be stable across session_handle / open_with_session_handle"
6566 );
6567
6568 // Drop the borrowed executor — the primary's session must remain valid.
6569 drop(borrowed);
6570
6571 // Primary still exposes the same session pointer (session was NOT closed by drop).
6572 assert_eq!(
6573 primary.session_ptr(),
6574 primary_session_ptr,
6575 "primary session pointer must be unchanged after dropping the borrowed executor"
6576 );
6577 }
6578
6579 #[test]
6580 fn set_active_groups_gates_group_active() {
6581 let session = MockSession::new();
6582 let mut primary = Executor::from_session(session);
6583 let handle = primary.session_handle();
6584
6585 // SAFETY: primary outlives borrowed.
6586 let mut borrowed = unsafe { Executor::open_with_session_handle(handle) };
6587
6588 // Before gating: wildcard — every group is accepted.
6589 assert!(
6590 borrowed.group_active("ctrl"),
6591 "default (wildcard) must accept every group"
6592 );
6593 assert!(
6594 borrowed.group_active("telem"),
6595 "default (wildcard) must accept every group"
6596 );
6597
6598 // Gate borrowed to only the "ctrl" group (one-tier filter).
6599 borrowed.set_active_groups(&["ctrl"]);
6600
6601 assert!(
6602 borrowed.group_active("ctrl"),
6603 "\"ctrl\" must be active after set_active_groups([\"ctrl\"])"
6604 );
6605 assert!(
6606 !borrowed.group_active("telem"),
6607 "\"telem\" must NOT be active when only \"ctrl\" is gated"
6608 );
6609 assert!(
6610 !borrowed.group_active("planning"),
6611 "\"planning\" must NOT be active when only \"ctrl\" is gated"
6612 );
6613
6614 // Primary is unaffected (it still uses the wildcard).
6615 assert!(
6616 primary.group_active("telem"),
6617 "primary executor must remain unaffected (still wildcard)"
6618 );
6619
6620 // Clear the filter on borrowed — back to wildcard.
6621 borrowed.set_active_groups(&[]);
6622 assert!(
6623 borrowed.group_active("telem"),
6624 "after clearing, borrowed must accept all groups again"
6625 );
6626 }
6627
6628 #[test]
6629 fn session_handle_into_raw_from_raw_round_trip() {
6630 let session = MockSession::new();
6631 let mut primary = Executor::from_session(session);
6632 let session_ptr = primary.session_ptr();
6633
6634 let handle = primary.session_handle();
6635 let raw = handle.into_raw();
6636
6637 // into_raw must return a non-null pointer matching the session address.
6638 assert!(!raw.is_null());
6639 assert_eq!(raw as *mut _, session_ptr);
6640
6641 // from_raw must reconstruct a handle that opens the same borrowed executor.
6642 // SAFETY: primary still owns the session, raw is its address.
6643 let handle2 = unsafe { crate::executor::SessionHandle::from_raw(raw) };
6644 let mut borrowed = unsafe { Executor::open_with_session_handle(handle2) };
6645 assert_eq!(
6646 borrowed.session_ptr(),
6647 session_ptr,
6648 "from_raw reconstructed handle must open executor on the same session"
6649 );
6650 }
6651}
6652
6653/// W3b.5 — snapshot the monitored publishers' counters before a dispatch
6654/// (only when a latency contract exists; otherwise a zeroed array that
6655/// `attribute_latency` never reads).
6656fn snapshot_pub_counts(
6657 table: &'static [super::monitor::MonitorSpec],
6658 active: bool,
6659) -> [u32; super::monitor::MAX_MONITORS] {
6660 let mut counts = [0u32; super::monitor::MAX_MONITORS];
6661 if active {
6662 for (k, spec) in table.iter().take(super::monitor::MAX_MONITORS).enumerate() {
6663 counts[k] = spec.cell.count.load(core::sync::atomic::Ordering::Relaxed);
6664 }
6665 }
6666 counts
6667}
6668
6669/// W3b.5 — attribute one dispatch's elapsed time to every monitored
6670/// publisher whose counter advanced during it (an upper bound on the
6671/// node-path take → publish latency: the callback deserialized, ran, and
6672/// published within `elapsed_us`).
6673fn attribute_latency(
6674 table: &'static [super::monitor::MonitorSpec],
6675 active: bool,
6676 counts_before: &[u32; super::monitor::MAX_MONITORS],
6677 elapsed_us: u32,
6678) {
6679 if !active {
6680 return;
6681 }
6682 for (k, spec) in table.iter().take(super::monitor::MAX_MONITORS).enumerate() {
6683 if spec.max_latency_ms == 0 {
6684 continue;
6685 }
6686 let now = spec.cell.count.load(core::sync::atomic::Ordering::Relaxed);
6687 if now != counts_before[k] {
6688 spec.cell
6689 .max_latency_us
6690 .fetch_max(elapsed_us, core::sync::atomic::Ordering::Relaxed);
6691 }
6692 }
6693}
6694
6695/// W3b.5 — enforce the bound SC's deadline after a dispatch. A miss maps
6696/// through [`DeadlineAction`](super::sched_context::DeadlineAction):
6697/// `Warn`/`Skip`/`Fault` all report; `Skip` additionally masks the SC's
6698/// remaining callbacks for this cycle; `Fault` invokes the fault hook
6699/// (panic when none is registered).
6700fn check_deadline_miss(
6701 sc: Option<&super::sched_context::SchedContext>,
6702 sc_idx: usize,
6703 elapsed_us: u32,
6704 misses: &mut heapless::Vec<super::monitor::Violation, { super::monitor::MAX_VIOLATIONS }>,
6705 skipped_scs: &mut u64,
6706 fault_fn: Option<fn(&super::monitor::Violation)>,
6707) {
6708 use super::sched_context::DeadlineAction;
6709 let Some(sc) = sc else { return };
6710 let Some(deadline_us) = sc.deadline_us.get().map(|nz| nz.get()) else {
6711 return;
6712 };
6713 if matches!(sc.deadline_action, DeadlineAction::Ignore) || elapsed_us <= deadline_us {
6714 return;
6715 }
6716 let v = super::monitor::Violation {
6717 rule: "deadline-miss-runtime",
6718 // Entries carry no name at this altitude; the SC slot stands in.
6719 fqn: "sched-context",
6720 measured: elapsed_us,
6721 declared: deadline_us,
6722 };
6723 match sc.deadline_action {
6724 DeadlineAction::Ignore => {}
6725 DeadlineAction::Warn => {
6726 let _ = misses.push(v);
6727 }
6728 DeadlineAction::Skip => {
6729 if sc_idx < 64 {
6730 *skipped_scs |= 1u64 << sc_idx;
6731 }
6732 let _ = misses.push(v);
6733 }
6734 DeadlineAction::Fault => {
6735 if let Some(f) = fault_fn {
6736 f(&v);
6737 let _ = misses.push(v);
6738 } else {
6739 panic!(
6740 "nros: deadline fault — dispatch ran {elapsed_us} us past a {deadline_us} us deadline"
6741 );
6742 }
6743 }
6744 }
6745}