ethercat-linux/src/main.rs

//! J1900 <-> TwinCAT 1 kHz communication verification over an EL6695 bridge.
//!
//! SECONDARY-SIDE build: J1900 talks to the EL6695 secondary port. The
//! secondary side SM layout (probed from EEPROM) uses SM0/TX (0x1000, 1024 B,
//! ctrl=0x26) for J1900->TwinCAT data and SM1/RX (0x1600, 1024 B, ctrl=0x22)
//! for TwinCAT->J1900 data. Unlike the primary side there is NO external
//! timestamp object (0x1A02) — the data is raw 1024 B payload with no prefix.
//!
//! Two modes:
//! - `ticker` (default): echo TwinCAT's payload back on SM0 with the local
//!   cycle number stamped at bytes 8..16.
//! - `probe`: read-only; dump the first bytes of SM1 every N cycles and do
//!   NOT write to SM0 (avoids disturbing TwinCAT's view).
//!
//! Graceful SIGINT/SIGTERM shutdown with full statistics.

use std::sync::atomic::{AtomicBool, Ordering};
use std::time::{Duration, Instant};

use el6695_rt::{bridge, regs, rt, stats};
use ethercrab::{
    std::{ethercat_now, tx_rx_task},
    MainDevice, MainDeviceConfig, PduStorage, Timeouts,
};
use stats::OnlineStats;

const MAX_SUBDEVICES: usize = 16;
const MAX_PDU_DATA: usize = 1100;
const MAX_FRAMES: usize = 16;
const PDI_LEN: usize = 3072;

// Secondary side SM layout (ESI-defined process data SMs)
const SM_TX_ADDR: u16 = 0x1C00;  // SM2: outputs (J1900 writes data to TwinCAT)
const SM_TX_LEN: usize = 1024;
const SM_RX_ADDR: u16 = 0x8E00;  // SM3: inputs (J1900 reads data from TwinCAT)
const SM_RX_LEN: usize = 1024;

const PAYLOAD_LEN: usize = 1024;

/// Abort after this many consecutive cycles with a TX or RX error.
const MAX_ERR_STREAK: u64 = 1000;

static STOP: AtomicBool = AtomicBool::new(false);
static PDU_STORAGE: std::sync::OnceLock<PduStorage<MAX_FRAMES, MAX_PDU_DATA>> =
    std::sync::OnceLock::new();

extern "C" fn on_signal(_sig: libc::c_int) {
    STOP.store(true, Ordering::Relaxed);
}

#[derive(Clone, Copy, PartialEq)]
enum Mode {
    Ticker,
    Probe,
}

struct Config {
    iface: String,
    mode: Mode,
    duration_sec: u64,
    cycle_us: u64,
    core: usize,
    prio: u8,
    quiet: bool,
    /// Toggle an EL2202 digital output every cycle (oscilloscope jitter
    /// waveform). Off by default: the outputs may drive real actuators.
    el2202: bool,
    /// Toggle BOTH EL2202 channels (0x03/0x00): output 1 -> EL1252 readback,
    /// output 2 -> oscilloscope, same edges in two views.
    el2202_dual: bool,
    /// Read EL1252 DC latch timestamps every cycle (edge interval/phase
    /// statistics; wire the signal under test to EL1252 channel 1).
    el1252: bool,
    /// Drive an EL2262 XFC output with DC-sync-locked edges (reference
    /// square wave on the DC 1 ms grid).
    el2262: bool,
    /// Read the EL1252 every N cycles (frame-budget throttle when combined
    /// with other per-cycle frames; edges are latched so nothing is lost
    /// beyond time resolution).
    el1252_every: u32,
}

fn parse_args() -> Result<Config, String> {
    let mut cfg = Config {
        iface: "eth1".into(),
        mode: Mode::Ticker,
        duration_sec: 60,
        cycle_us: 1000,
        core: 2,
        prio: 80,
        quiet: false,
        el2202: false,
        el2202_dual: false,
        el1252: false,
        el2262: false,
        el1252_every: 1,
    };
    let mut args = std::env::args().skip(1).peekable();
    if let Some(a) = args.peek() {
        if !a.starts_with("--") {
            cfg.iface = args.next().unwrap();
        }
    }
    while let Some(a) = args.next() {
        let mut val = |name: &str| -> Result<String, String> {
            args.next().ok_or_else(|| format!("missing value for {}", name))
        };
        match a.as_str() {
            "--mode" => {
                cfg.mode = match val("--mode")?.as_str() {
                    "ticker" => Mode::Ticker,
                    "probe" => Mode::Probe,
                    other => return Err(format!("unknown mode '{}'", other)),
                }
            }
            "--duration" => {
                cfg.duration_sec = val("--duration")?
                    .parse()
                    .map_err(|_| "bad --duration")?
            }
            "--cycle-us" => cfg.cycle_us = val("--cycle-us")?.parse().map_err(|_| "bad --cycle-us")?,
            "--core" => cfg.core = val("--core")?.parse().map_err(|_| "bad --core")?,
            "--prio" => cfg.prio = val("--prio")?.parse().map_err(|_| "bad --prio")?,
            "--quiet" => cfg.quiet = true,
            "--el2202" => cfg.el2202 = true,
            "--el2202-dual" => {
                cfg.el2202 = true;
                cfg.el2202_dual = true;
            }
            "--el1252" => cfg.el1252 = true,
            "--el2262" => cfg.el2262 = true,
            "--el1252-every" => {
                cfg.el1252_every = val("--el1252-every")?.parse().map_err(|_| "bad --el1252-every")?
            }
            other => return Err(format!("unknown argument '{}'", other)),
        }
    }
    if cfg.cycle_us == 0 {
        return Err("--cycle-us must be > 0".into());
    }
    Ok(cfg)
}

fn main() {
    let cfg = match parse_args() {
        Ok(c) => c,
        Err(e) => {
            eprintln!("error: {}", e);
            eprintln!(
                "usage: el6695_rt [iface] [--mode ticker|probe] [--duration SEC] \
                 [--cycle-us N] [--core N] [--prio N] [--quiet] [--el2202]"
            );
            std::process::exit(2);
        }
    };

    unsafe {
        let mut sa: libc::sigaction = core::mem::zeroed();
        sa.sa_sigaction = on_signal as usize;
        sa.sa_flags = 0;
        libc::sigemptyset(&mut sa.sa_mask);
        libc::sigaction(libc::SIGINT, &sa, core::ptr::null_mut());
        libc::sigaction(libc::SIGTERM, &sa, core::ptr::null_mut());
    }

    let code = match futures_lite::future::block_on(run(&cfg)) {
        Ok(()) => 0,
        Err(e) => {
            eprintln!("Failed: {}", e);
            1
        }
    };
    std::process::exit(code);
}

async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
    let cycle_ns = cfg.cycle_us * 1000;

    println!("el6695-rt: J1900 <-> TwinCAT 1 kHz verification (EL6695 bridge, secondary side)");
    println!(
        "  iface={} mode={} duration={}s cycle={}us core={} prio={}",
        cfg.iface,
        match cfg.mode {
            Mode::Ticker => "ticker",
            Mode::Probe => "probe",
        },
        cfg.duration_sec,
        cfg.cycle_us,
        cfg.core,
        cfg.prio
    );

    let pdu_storage = PDU_STORAGE.get_or_init(PduStorage::new);
    let (tx, rx, pdu_loop) = pdu_storage.try_split().map_err(|_| "PDU storage already in use")?;

    let maindevice = MainDevice::new(
        pdu_loop,
        Timeouts {
            state_transition: Duration::from_millis(10000),
            mailbox_response: Duration::from_millis(2000),
            eeprom: Duration::from_millis(500),
            ..Default::default()
        },
        MainDeviceConfig::default(),
    );

    let iface = cfg.iface.clone();
    let _txrx = std::thread::spawn(move || {
        let task = tx_rx_task(&iface, tx, rx).expect("create tx_rx task");
        if let Err(e) = futures_lite::future::block_on(task) {
            eprintln!("tx_rx task ended with error: {:?}", e);
        }
    });

    println!("Scanning segment (INIT -> PREOP)...");
    let group = maindevice
        .init_single_group::<MAX_SUBDEVICES, PDI_LEN>(ethercat_now)
        .await?;

    let mut el6695_index = None;
    for (idx, subdevice) in group.iter(&maindevice).enumerate() {
        if subdevice.name().contains("EL6695") {
            el6695_index = Some(idx);
            println!("[{}] Found EL6695", idx);
        }
    }
    let el6695_index = el6695_index.ok_or("EL6695 not found on bus")?;
    let sd = group.subdevice(&maindevice, el6695_index)?;
    let station = sd.configured_address();
    println!("EL6695 station address: 0x{:04X}", station);

    // ---- Probe SM registers ----
    async fn read_sm(md: &MainDevice<'_>, st: u16, n: u8) -> (u16, u16, u8, u8) {
        let start = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_START).receive::<u16>(md).await.unwrap_or(0xFFFF);
        let len = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_LEN).receive::<u16>(md).await.unwrap_or(0xFFFF);
        let ctrl = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_CTRL).receive::<u8>(md).await.unwrap_or(0xFF);
        let ena = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_ENABLE).receive::<u8>(md).await.unwrap_or(0xFF);
        (start, len, ctrl, ena)
    }
    for n in [0u8, 1, 2, 3] {
        let (s, l, c, e) = read_sm(&maindevice, station, n).await;
        println!("  SM{}: start=0x{:04X} len={} ctrl=0x{:02X} enable=0x{:02X}", n, s, l, c, e);
    }
    // Probe FMMU registers
    for n in 0u16..4 {
        let reg = 0x0600 + n * 16;
        match ethercrab::Command::fprd(station, reg)
            .receive_slice(&maindevice, 16)
            .await
        {
            Ok(pdu) => {
                let raw: &[u8] = &pdu;
                let logical_start = u32::from_le_bytes(raw[0..4].try_into().unwrap());
                let length = u16::from_le_bytes(raw[4..6].try_into().unwrap());
                let phy_start = u16::from_le_bytes(raw[8..10].try_into().unwrap());
                println!("  FMMU{}: logical_start=0x{:08X} len={} physical=0x{:04X} enable=0x{:02X}", n, logical_start, length, phy_start, raw[12]);
            }
            Err(e) => println!("  FMMU{} read error: {:?}", n, e),
        }
    }

    // ---- Configure SM2/SM3 for process data (SM0/SM1 are EEPROM-fixed mailbox) ----
    // 0x1C12/0x1C13 exist (empty), 0x1C10/0x1C11 do not.
    // SM2/SM3 registers are writable because they are not defined in EEPROM.
    println!("\nTesting SM0/SM1 data access...");
    // Read SM1 (buffered input, 0x1600)
    match ethercrab::Command::fprd(station, 0x1600)
        .receive_slice(&maindevice, 32)
        .await
    {
        Ok(pdu) => {
            let raw: &[u8] = &pdu;
            println!("  SM1 FPRD(0x1600,32) OK: {:02X?}", &raw[..32.min(raw.len())]);
        }
        Err(e) => println!("  SM1 FPRD error: {:?}", e),
    }
    // Write SM0 (mailbox, 0x1000)
    match ethercrab::Command::fpwr(station, 0x1000)
        .send(&maindevice, &[0xBBu8; 16])
        .await
    {
        Ok(_) => println!("  SM0 FPWR(0x1000,16) OK"),
        Err(e) => println!("  SM0 FPWR error: {:?}", e),
    }
    // Read back SM0
    match ethercrab::Command::fprd(station, 0x1000)
        .receive_slice(&maindevice, 16)
        .await
    {
        Ok(pdu) => {
            let raw: &[u8] = &pdu;
            println!("  SM0 FPRD(0x1000,16) back: {:02X?}", &raw[..16.min(raw.len())]);
        }
        Err(e) => println!("  SM0 FPRD back error: {:?}", e),
    }
    
    // Remove SM2/SM3 config (was causing SAFEOP 0x001D)
    // Safety: clear SM2/SM3 in case previous runs left stale values
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE)
        .send(&maindevice, 0x00u8).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE)
        .send(&maindevice, 0x00u8).await;
    
    // ---- Reconfigure + SAFEOP after TwinCAT activation ----
    // TwinCAT was just re-activated (writes PDO config to terminal flash).
    // Now configure SM2/SM3 + PDO assignment and try SAFEOP.
    println!("\nReconfiguring + SAFEOP after TwinCAT activation...");
    
    // Check if PDO assignment was pre-configured by TwinCAT
    for idx in [0x1C12u16, 0x1C13u16] {
        match sd.sdo_read::<u8>(idx, 0).await {
            Ok(n) => {
                println!("  0x{:04X} sub0={} (TwinCAT config)", idx, n);
                for i in 1..=n {
                    if let Ok(v) = sd.sdo_read::<u16>(idx, i as u8).await {
                        println!("    [{:02}] = 0x{:04X}", i, v);
                    }
                }
            }
            Err(e) => println!("  0x{:04X} error: {:?}", idx, e),
        }
    }
    
    // Write PDO definitions and assignment (override TwinCAT if needed)
    let blob = bridge::twin_layout_blob(0x7000);
    let _ = sd.sdo_write_complete(0x1608, 0, &blob).await;
    let blob = bridge::twin_layout_blob(0x6000);
    let _ = sd.sdo_write_complete(0x1A08, 0, &blob).await;
    let blob = bridge::assign_blob(&[0x1608]);
    let _ = sd.sdo_write_complete(0x1C12, 0, &blob).await;
    let blob = bridge::assign_blob(&[0x1A08]);
    let _ = sd.sdo_write_complete(0x1C13, 0, &blob).await;
    
    // Configure SM2/SM3 with ESI values + FMMU
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE)
        .send(&maindevice, 0x00u8).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_START)
        .send(&maindevice, 0x1C00u16).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_LEN)
        .send(&maindevice, 1024u16).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_CTRL)
        .send(&maindevice, 0x64u8).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE)
        .send(&maindevice, 0x01u8).await;
    
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE)
        .send(&maindevice, 0x00u8).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_START)
        .send(&maindevice, 0x8E00u16).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_LEN)
        .send(&maindevice, 1024u16).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_CTRL)
        .send(&maindevice, 0x20u8).await;
    let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE)
        .send(&maindevice, 0x01u8).await;
    
    bridge::write_fmmus_secondary(&maindevice, station).await;
    
    // SAFEOP
    let (al, alc) = bridge::drive_state(&maindevice, station, 0x0004).await;
    println!("  SAFEOP: {}", bridge::verdict(al, alc));
    if al & 0x0F != 0x04 || al & 0x10 != 0 {
        return Err(format!("Cannot reach SAFEOP: {}", bridge::verdict(al, alc)).into());
    }
    println!("  >>> SAFEOP achieved! <<<");
    let (al2, alc2) = bridge::drive_state(&maindevice, station, 0x0008).await;
    println!("  OP: {}", bridge::verdict(al2, alc2));
    if al2 & 0x0F != 0x08 || al2 & 0x10 != 0 {
        return Err(format!("OP rejected: {}", bridge::verdict(al2, alc2)).into());
    }
    println!("  >>> OP achieved! <<<");

    // ---- Optional EL2202 oscilloscope waveform (toggle one DO per cycle) ----
    const EL2202_OUT_ADDR: u16 = 0x0F00;
    let mut el2202: Option<(u16, u16)> = None;
    if cfg.el2202 {
        let mut found = false;
        for (idx, subdevice) in group.iter(&maindevice).enumerate() {
            if subdevice.name().contains("EL2202") {
                found = true;
                let sd2 = group.subdevice(&maindevice, idx)?;
                let st2 = sd2.configured_address();
                println!("[{}] Found EL2202, station 0x{:04X}", idx, st2);
                let _ = ethercrab::Command::fpwr(st2, regs::sm_base(0) + regs::SM_OFF_START)
                    .send(&maindevice, EL2202_OUT_ADDR)
                    .await;
                let _ = ethercrab::Command::fpwr(st2, regs::sm_base(0) + regs::SM_OFF_LEN)
                    .send(&maindevice, 1u16)
                    .await;
                let _ = ethercrab::Command::fpwr(st2, regs::sm_base(0) + regs::SM_OFF_CTRL)
                    .send(&maindevice, regs::SM_CTRL_EL2202_OUTPUTS)
                    .await;
                let _ = regs::rmw_u8(
                    &maindevice,
                    st2,
                    regs::sm_base(0) + regs::SM_OFF_ENABLE,
                    regs::SM_ENABLE | regs::SM_ENABLE_FAMILY,
                    0,
                )
                .await;
                let (al, alc) = bridge::drive_state(&maindevice, st2, regs::AL_SAFEOP).await;
                println!("EL2202 SAFEOP: {}", bridge::verdict(al, alc));
                let (al, alc) = bridge::drive_state(&maindevice, st2, regs::AL_OP).await;
                println!("EL2202 OP:     {}", bridge::verdict(al, alc));
                if al & regs::AL_STATE_MASK != regs::AL_OP || al & regs::AL_ERROR != 0 {
                    return Err(format!("EL2202 OP rejected: {}", bridge::verdict(al, alc)).into());
                }
                let _ = ethercrab::Command::fpwr(st2, EL2202_OUT_ADDR)
                    .send(&maindevice, regs::EL2202_CH1_DATA)
                    .await;
                let rb = ethercrab::Command::fprd(st2, EL2202_OUT_ADDR)
                    .receive::<u8>(&maindevice)
                    .await
                    .unwrap_or(0xFF);
                if rb & regs::EL2202_CH1_DATA == regs::EL2202_CH1_DATA {
                    println!(
                        "EL2202 scope output: verified, toggle 1 byte @0x{:04X} every cycle",
                        EL2202_OUT_ADDR
                    );
                    let _ = ethercrab::Command::fpwr(st2, EL2202_OUT_ADDR)
                        .send(&maindevice, 0x00u8)
                        .await;
                    el2202 = Some((st2, EL2202_OUT_ADDR));
                } else {
                    println!("EL2202 output readback failed (0x{:02X}); scope output disabled", rb);
                }
            }
        }
        if !found {
            println!("--el2202 given but no EL2202 on bus; scope output disabled");
        }
        if found && el2202.is_none() {
            println!("EL2202 present but output setup failed; scope output disabled");
        }
    }

    // ---- Optional EL2262 DC-locked reference output ----
    const EL2262_OUT_ADDR: u16 = 0x1000;
    let mut el2262: Option<u16> = None;
    if cfg.el2262 {
        let mut found = false;
        for (idx, subdevice) in group.iter(&maindevice).enumerate() {
            if subdevice.name().contains("EL2262") {
                found = true;
                let sd3 = group.subdevice(&maindevice, idx)?;
                let st3 = sd3.configured_address();
                println!("[{}] Found EL2262, station 0x{:04X}", idx, st3);
                let t = ethercrab::Command::fprd(st3, regs::DC_SYSTEM_TIME)
                    .receive::<u64>(&maindevice)
                    .await
                    .unwrap_or(0);
                let start = t.wrapping_add(100_000_000);
                let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC_START_TIME)
                    .send(&maindevice, start)
                    .await;
                let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC0_CYCLE)
                    .send(&maindevice, cycle_ns as u32)
                    .await;
                let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC1_CYCLE)
                    .send(&maindevice, cycle_ns as u32)
                    .await;
                let _ = regs::rmw_u8(
                    &maindevice,
                    st3,
                    regs::DC_SYNC_ACTIVATION,
                    regs::DC_SYNC0_ACTIVATE | regs::DC_SYNC1_ACTIVATE,
                    0,
                )
                .await;
                println!("EL2262 DC sync: sync0/1 cycle={}ns start=+100ms (t={})", cycle_ns, t);
                let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_START)
                    .send(&maindevice, EL2262_OUT_ADDR)
                    .await;
                let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_LEN)
                    .send(&maindevice, 1u16)
                    .await;
                let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_CTRL)
                    .send(&maindevice, regs::SM_CTRL_OUTPUTS_ESI)
                    .await;
                let _ = regs::rmw_u8(
                    &maindevice,
                    st3,
                    regs::sm_base(0) + regs::SM_OFF_ENABLE,
                    regs::SM_ENABLE | regs::SM_ENABLE_FAMILY,
                    0,
                )
                .await;
                let (al, alc) = bridge::drive_state(&maindevice, st3, regs::AL_SAFEOP).await;
                println!("EL2262 SAFEOP: {}", bridge::verdict(al, alc));
                let (al, alc) = bridge::drive_state(&maindevice, st3, regs::AL_OP).await;
                println!("EL2262 OP:     {}", bridge::verdict(al, alc));
                if al & regs::AL_STATE_MASK != regs::AL_OP || al & regs::AL_ERROR != 0 {
                    return Err(format!("EL2262 OP rejected: {}", bridge::verdict(al, alc)).into());
                }
                el2262 = Some(st3);
            }
        }
        if !found {
            println!("--el2262 given but no EL2262 on bus; reference output disabled");
        }
    }

    // ---- Optional EL1252 DC latch timestamp readback ----
    let mut el1252: Option<u16> = None;
    if cfg.el1252 {
        let mut found = false;
        for (idx, subdevice) in group.iter(&maindevice).enumerate() {
            if subdevice.name().contains("EL1252") {
                found = true;
                let sd4 = group.subdevice(&maindevice, idx)?;
                let st4 = sd4.configured_address();
                println!("[{}] Found EL1252, station 0x{:04X} (latch readback)", idx, st4);
                let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_START)
                    .send(&maindevice, 0x1000u16)
                    .await;
                let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_LEN)
                    .send(&maindevice, 1u16)
                    .await;
                let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_CTRL)
                    .send(&maindevice, regs::SM_CTRL_EL1252_INPUTS)
                    .await;
                let _ = regs::rmw_u8(
                    &maindevice,
                    st4,
                    regs::sm_base(0) + regs::SM_OFF_ENABLE,
                    regs::SM_ENABLE,
                    0,
                )
                .await;
                let (al, alc) = bridge::drive_state(&maindevice, st4, regs::AL_SAFEOP).await;
                println!("EL1252 SAFEOP: {}", bridge::verdict(al, alc));
                let (al, alc) = bridge::drive_state(&maindevice, st4, regs::AL_OP).await;
                println!("EL1252 OP:     {}", bridge::verdict(al, alc));
                if al & regs::AL_STATE_MASK != regs::AL_OP || al & regs::AL_ERROR != 0 {
                    return Err(format!("EL1252 OP rejected: {}", bridge::verdict(al, alc)).into());
                }
                el1252 = Some(st4);
            }
        }
        if !found {
            println!("--el1252 given but no EL1252 on bus; latch readback disabled");
        }
    }

    // ---- Pre-allocate everything, then promote to RT ----
    let mut tx_buf = vec![0u8; SM_TX_LEN];
    let mut rx_buf = vec![0u8; SM_RX_LEN];
    let mut jitter_st = OnlineStats::new("period_jitter", 100, -5_000_000, 5_000_000);
    let mut exec_st = OnlineStats::new("exec_time", 1_000, 0, 20_000_000);
    let mut rx_rtt_st = OnlineStats::new("fprd_rtt", 1_000, 0, 20_000_000);
    let mut tx_rtt_st = OnlineStats::new("fpwr_rtt", 1_000, 0, 20_000_000);

    println!("Promoting to RT: core={}, fifo={}, mlock=true", cfg.core, cfg.prio);
    if let Err(e) = rt::promote_current_thread(cfg.core, cfg.prio, true) {
        eprintln!("RT promotion failed (needs root, continuing anyway): {}", e);
    }
    rt::prefault_stack(256 * 1024);

    // ---- Real-time data loop ----
    println!(
        "\n>>> running: {} loop, echo payload, stats (Ctrl-C for graceful stop)",
        match cfg.mode {
            Mode::Ticker => "ticker",
            Mode::Probe => "probe (read-only, no echo)",
        }
    );
    let start = Instant::now();
    let mut ticker = rt::Ticker::new(cycle_ns);

    let mut cyc: u64 = 0;
    let mut tx_err: u64 = 0;
    let mut rx_err: u64 = 0;
    let mut err_streak: u64 = 0;
    let mut no_tc_seq: u64 = 0;
    let mut el2202_err: u64 = 0;
    let mut el2262_err: u64 = 0;
    let mut el1252_err: u64 = 0;
    let mut latch_pos0: u64 = 0;
    let mut latch_neg0: u64 = 0;
    let mut latch_pos1: u64 = 0;
    let mut latch_neg1: u64 = 0;
    let mut pos_edges: u64 = 0;
    let mut neg_edges: u64 = 0;
    let mut pos1_edges: u64 = 0;
    let mut el1252_st: [u8; 2] = [0, 0];
    let mut el1252_in: u8 = 0;
    let mut latch_interval_st = OnlineStats::new("el1252_ch1_edge_interval", 1_000, 0, 20_000_000);
    let mut latch_phase_st = OnlineStats::new("el1252_ch1_edge_phase", 100, 0, 1_000_000);
    let mut latch1_interval_st = OnlineStats::new("el1252_ch2_edge_interval", 1_000, 0, 20_000_000);
    let mut latch1_phase_st = OnlineStats::new("el1252_ch2_edge_phase", 100, 0, 1_000_000);
    let mut last_report = Instant::now();
    let mut aborted: Option<String> = None;

    loop {
        if STOP.load(Ordering::Relaxed) {
            println!("\n(stop requested, finishing cycle {})", cyc);
            break;
        }
        if cfg.duration_sec > 0 && start.elapsed().as_secs() >= cfg.duration_sec {
            break;
        }

        let jitter_ns = ticker.wait_next_period();
        let t0 = Instant::now();
        cyc += 1;
        jitter_st.push(jitter_ns);

        // 1. Read SM1 (TwinCAT data)
        let t_rx0 = Instant::now();
        let mut rx_ok = true;
        match ethercrab::Command::fprd(station, SM_RX_ADDR)
            .receive_slice(&maindevice, SM_RX_LEN as u16)
            .await
        {
            Ok(pdu) => {
                let raw: &[u8] = &pdu;
                let n = raw.len().min(SM_RX_LEN);
                rx_buf[..n].copy_from_slice(&raw[..n]);
            }
            Err(_) => {
                rx_ok = false;
                rx_err += 1;
            }
        }
        rx_rtt_st.push(t_rx0.elapsed().as_nanos() as i64);

        // 2. In ticker mode: echo payload back and stamp cycle number.
        //    In probe mode: skip TX (read-only observation).
        if cfg.mode == Mode::Ticker {
            if rx_ok {
                let n = PAYLOAD_LEN.min(rx_buf.len());
                tx_buf[..n].copy_from_slice(&rx_buf[..n]);
            }
            tx_buf[8..16].copy_from_slice(&cyc.to_le_bytes());
            tx_buf[264..268].copy_from_slice(&(jitter_ns as i32).to_le_bytes());

            let t_tx0 = Instant::now();
            let tx_ok = ethercrab::Command::fpwr(station, SM_TX_ADDR)
                .send(&maindevice, &tx_buf[..PAYLOAD_LEN])
                .await
                .is_ok();
            tx_rtt_st.push(t_tx0.elapsed().as_nanos() as i64);
            if !tx_ok {
                tx_err += 1;
            }
        }

        // Optional EL2202 scope waveform
        if let Some((st2, addr)) = el2202 {
            let v: u8 = if cfg.el2202_dual {
                (cyc & 1) as u8 * (regs::EL2202_CH1_DATA | regs::EL2202_CH2_DATA)
            } else {
                (cyc & 1) as u8 * regs::EL2202_CH1_DATA
            };
            if ethercrab::Command::fpwr(st2, addr)
                .send(&maindevice, v)
                .await
                .is_err()
            {
                el2202_err += 1;
            }
        }

        // Optional EL2262 DC-locked reference output
        if let Some(st3) = el2262 {
            let v: u8 = (cyc & 1) as u8;
            if ethercrab::Command::fpwr(st3, EL2262_OUT_ADDR)
                .send(&maindevice, v)
                .await
                .is_err()
            {
                el2262_err += 1;
            }
        }

        // Optional EL1252 latch readback
        if let Some(st4) = el1252 {
            if cyc % (cfg.el1252_every.max(1) as u64) == 0 {
                match ethercrab::Command::fprd(st4, 0x09AEu16)
                .receive_slice(&maindevice, 34)
                .await
            {
                Ok(pdu) => {
                    let e: &[u8] = &pdu;
                    if e.len() >= 34 {
                        el1252_st = [e[0], e[1]];
                        let pos0 = u64::from_le_bytes(e[2..10].try_into().unwrap());
                        let neg0 = u64::from_le_bytes(e[10..18].try_into().unwrap());
                        let pos1 = u64::from_le_bytes(e[18..26].try_into().unwrap());
                        let neg1 = u64::from_le_bytes(e[26..34].try_into().unwrap());
                        if pos0 != 0 && pos0 != latch_pos0 {
                            if latch_pos0 != 0 {
                                latch_interval_st.push((pos0 - latch_pos0) as i64);
                            }
                            latch_phase_st.push((pos0 % cycle_ns) as i64);
                            latch_pos0 = pos0;
                            pos_edges += 1;
                        }
                        if neg0 != 0 && neg0 != latch_neg0 {
                            latch_neg0 = neg0;
                            neg_edges += 1;
                        }
                        if pos1 != 0 && pos1 != latch_pos1 {
                            if latch_pos1 != 0 {
                                latch1_interval_st.push((pos1 - latch_pos1) as i64);
                            }
                            latch1_phase_st.push((pos1 % cycle_ns) as i64);
                            latch_pos1 = pos1;
                            pos1_edges += 1;
                        }
                        if neg1 != 0 && neg1 != latch_neg1 {
                            latch_neg1 = neg1;
                        }
                    }
                }
                Err(_) => el1252_err += 1,
            }
            }
        }

        if rx_ok && (cfg.mode == Mode::Probe || !tx_err > 0) {
            err_streak = 0;
        } else {
            err_streak += 1;
            if err_streak > MAX_ERR_STREAK {
                aborted = Some(format!(
                    "aborting: {} consecutive cycles with TX/RX errors (link dead?)",
                    err_streak
                ));
            }
        }

        let tc_seq = u64::from_le_bytes(
            rx_buf[..8].try_into().unwrap_or([0; 8]),
        );
        if tc_seq == 0 {
            no_tc_seq += 1;
        }

        exec_st.push(t0.elapsed().as_nanos() as i64);

        if !cfg.quiet && last_report.elapsed() >= Duration::from_millis(1000) {
            if let Some(st4) = el1252 {
                if let Ok(b) = ethercrab::Command::fprd(st4, 0x1000u16)
                    .receive::<u8>(&maindevice)
                    .await
                {
                    el1252_in = b;
                }
            }
            println!(
                "S tms={} cyc={} tc_seq={} txe={} rxe={} notc={} e0={} e1={} in=0x{:02X} lst={:02X}{:02X}",
                start.elapsed().as_millis(),
                cyc,
                tc_seq,
                tx_err,
                rx_err,
                no_tc_seq,
                pos_edges,
                pos1_edges,
                el1252_in,
                el1252_st[0],
                el1252_st[1]
            );
            last_report = Instant::now();
        }

        if aborted.is_some() {
            break;
        }
    }

    // ---- Final report ----
    println!("\n=== timing stats ===");
    jitter_st.report(1000, "us");
    exec_st.report(1000, "us");
    rx_rtt_st.report(1000, "us");
    tx_rtt_st.report(1000, "us");
    if el1252.is_some() {
        println!("=== EL1252 latch: rising-edge intervals and phase in cycle ===");
        latch_interval_st.report(1000, "us");
        latch_phase_st.report(1000, "us");
        latch1_interval_st.report(1000, "us");
        latch1_phase_st.report(1000, "us");
        println!(
            "el1252: ch1 pos_edges={}, ch1 neg_edges={}, ch2 pos_edges={}, read_err={}",
            pos_edges, neg_edges, pos1_edges, el1252_err
        );
    }
    println!(
        "\n=== done: {} cycles in {:.1}s, tx_err={}, rx_err={}, no_tc_seq={}, el2202_err={}, el2262_err={} ===",
        cyc,
        start.elapsed().as_secs_f64(),
        tx_err,
        rx_err,
        no_tc_seq,
        el2202_err,
        el2262_err
    );

    if let Some(msg) = aborted {
        return Err(msg.into());
    }
    Ok(())
}