ethercat-linux/src/rt.rs

//! Linux real-time helpers: memory locking, SCHED_FIFO promotion, CPU
//! affinity, stack prefaulting and absolute-time periodic wakeups.
//!
//! Everything is thin `libc` wrappers; `sched_setscheduler` goes through a raw
//! syscall because statically-linked musl builds have been observed to return
//! ENOSYS through the libc wrapper.

use std::io::Error as IoError;

#[derive(Debug)]
pub struct RtError {
    pub op: &'static str,
    pub source: IoError,
}

impl std::fmt::Display for RtError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "{} failed: {}", self.op, self.source)
    }
}

impl std::error::Error for RtError {}

fn sys_err(op: &'static str) -> RtError {
    RtError {
        op,
        source: IoError::last_os_error(),
    }
}

/// CLOCK_MONOTONIC in nanoseconds.
pub fn now_ns() -> u64 {
    // SAFETY: ts is fully written by clock_gettime on success.
    unsafe {
        let mut ts: libc::timespec = core::mem::zeroed();
        libc::clock_gettime(libc::CLOCK_MONOTONIC, &mut ts);
        (ts.tv_sec as u64) * 1_000_000_000 + ts.tv_nsec as u64
    }
}

/// Absolute-time sleep on CLOCK_MONOTONIC, retrying on EINTR.
pub fn sleep_until_ns(deadline_ns: u64) {
    let mut ts: libc::timespec = unsafe { core::mem::zeroed() };
    ts.tv_sec = (deadline_ns / 1_000_000_000) as _;
    ts.tv_nsec = (deadline_ns % 1_000_000_000) as _;
    loop {
        // SAFETY: ts is a valid timespec; remainder pointer is null.
        let rc = unsafe {
            libc::clock_nanosleep(
                libc::CLOCK_MONOTONIC,
                libc::TIMER_ABSTIME,
                &ts,
                core::ptr::null_mut(),
            )
        };
        if rc != libc::EINTR {
            break;
        }
    }
}

/// Sleep until `deadline_ns`, but hand off to a spin loop for the final
/// `spin_ns` to minimise wakeup latency. Cheap when already past deadline.
pub fn hybrid_sleep_until(deadline_ns: u64, spin_ns: u64) {
    loop {
        let now = now_ns();
        if now >= deadline_ns {
            return;
        }
        let remain = deadline_ns - now;
        if remain > spin_ns {
            sleep_until_ns(deadline_ns - spin_ns);
        } else {
            std::hint::spin_loop();
        }
    }
}

/// Lock all current and future process memory (no page faults in the RT loop).
pub fn lock_all_memory() -> Result<(), RtError> {
    // SAFETY: mlockall has no memory-safety preconditions.
    let rc = unsafe { libc::mlockall(libc::MCL_CURRENT | libc::MCL_FUTURE) };
    if rc != 0 {
        return Err(sys_err("mlockall"));
    }
    Ok(())
}

/// Promote the current thread: pin to `core_id`, SCHED_FIFO `fifo_priority`,
/// and (optionally) lock memory.
pub fn promote_current_thread(
    core_id: usize,
    fifo_priority: u8,
    lock_memory: bool,
) -> Result<(), RtError> {
    // SAFETY: cpu_set is zero-initialised then filled via the macros.
    unsafe {
        let mut set: libc::cpu_set_t = core::mem::zeroed();
        libc::CPU_ZERO(&mut set);
        libc::CPU_SET(core_id, &mut set);
        let rc = libc::sched_setaffinity(0, core::mem::size_of::<libc::cpu_set_t>(), &set);
        if rc != 0 {
            return Err(sys_err("sched_setaffinity"));
        }
    }

    // Raw syscall avoids musl/glibc ABI differences (ENOSYS on static musl).
    // SAFETY: param is a valid sched_param for the current thread.
    unsafe {
        let mut param: libc::sched_param = core::mem::zeroed();
        param.sched_priority = fifo_priority as libc::c_int;
        let rc = libc::syscall(libc::SYS_sched_setscheduler, 0 as libc::c_int, libc::SCHED_FIFO, &param);
        if rc != 0 {
            return Err(sys_err("sched_setscheduler"));
        }
    }

    if lock_memory {
        lock_all_memory()?;
    }
    Ok(())
}

/// Fault in `bytes` of stack now so the RT loop never takes a minor fault.
pub fn prefault_stack(bytes: usize) {
    const CHUNK: usize = 4096;
    let mut remaining = bytes;
    while remaining > 0 {
        let mut buf = [0u8; CHUNK];
        for (i, b) in buf.iter_mut().enumerate() {
            *b = (i & 0xff) as u8;
        }
        core::hint::black_box(&buf);
        remaining = remaining.saturating_sub(CHUNK);
    }
}

/// Absolute-time periodic ticker. Deadlines are anchored to an absolute
/// grid, so a single late cycle does not shift subsequent ones (no drift).
pub struct Ticker {
    next_wakeup_ns: u64,
    period_ns: u64,
}

impl Ticker {
    /// First deadline is `now + period`.
    pub fn new(period_ns: u64) -> Self {
        Self {
            next_wakeup_ns: now_ns().saturating_add(period_ns),
            period_ns,
        }
    }

    /// Sleep until the next grid point. Returns wake jitter in ns
    /// (actual wake time minus planned deadline; positive = late).
    pub fn wait_next_period(&mut self) -> i64 {
        let deadline = self.next_wakeup_ns;
        sleep_until_ns(deadline);
        let actual = now_ns();
        // Skip grid points already missed so we re-sync instead of
        // hammering through a backlog after a long overrun.
        self.next_wakeup_ns = self.next_wakeup_ns.saturating_add(self.period_ns);
        if self.next_wakeup_ns <= actual {
            let missed = (actual - self.next_wakeup_ns) / self.period_ns + 1;
            self.next_wakeup_ns = self.next_wakeup_ns.saturating_add(missed * self.period_ns);
        }
        actual as i64 - deadline as i64
    }
}