//! 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 std::os::unix::io::AsRawFd; use el6695_rt::{bridge, regs, rt, stats}; use ethercrab::{ std::{ethercat_now, tx_rx_task, tx_rx_task_io_uring}, MainDevice, MainDeviceConfig, PduStorage, Timeouts, }; use stats::OnlineStats; const MAX_SUBDEVICES: usize = 16; const MAX_PDU_DATA: usize = 2048; 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 = 1400; const SM_RX_ADDR: u16 = 0x8E00; // SM3: inputs (J1900 reads data from TwinCAT) // Full PDO: 2B diagnostic word + 8B payload (single-entry twin_layout blob // expands 0x6000:01 to 8 bytes). const SM_RX_LEN: usize = 1400; // SM3 buffer (no diag word, pure 8B payload) const SM_RX_PLD_LEN: usize = 1400; // actual payload after stripping diagnostic word const RX_PREFIX: usize = 0; // no diag word prefix (0x1C13=[0x1A08] only) const PAYLOAD_LEN: usize = 1400; /// 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 RX_PREFIX_DYN: std::sync::atomic::AtomicUsize = std::sync::atomic::AtomicUsize::new(0); #[inline] fn rx_prefix() -> usize { RX_PREFIX_DYN.load(Ordering::Relaxed) } static PDU_STORAGE: std::sync::OnceLock> = 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, Verify, // CoE audit: read 0xF008, layout hash, SM, identity Sniff, // raw frame diagnostics: passive listen + BRD probe /// RFM ping: periodic reflective-memory write, wait for remote echo, /// measure round-trip latency (same-clock RTT). RfmPing, /// RFM echo: poll REQ mailbox, copy to REP mailbox (remote ping peer). RfmEcho, /// RFM bridge: EL6695 RX -> RFM -> remote echo -> RFM -> EL6695 TX. /// End-to-end CX<->x86 latency via reflective memory hop. RfmBridge, /// One-shot hardware ring-continuity check (prints ring=OK/BROKEN, exits). RfmRing, /// Event-driven bridge: no tick. Poll FPRD continuously (in flight), /// spin-poll RFM REP mailbox between PDU polls, forward on data change /// in both directions immediately. For <=1ms one-way J1900<->CX. RfmAsync, /// LRW single-frame read+write: one frame per cycle writes SM2 and reads /// SM3 (FMMU logical image). Halves the per-cycle wire RTTs. RfmLrw, /// reverse-path deep diagnosis: dump all SMs/FMMUs, test SM2 write+forward RevProbe, } #[derive(Clone, Copy, Debug, PartialEq)] enum Role { /// Full PDO config owner: writes 0x1608/0x1A08/0x1C12/0x1C13. /// Used when J1900 is the first/only master to configure the bridge. ConfigOwner, /// Read-only consumer: never writes PDO mapping/assignment. /// Adopts whatever layout the primary-side owner (TwinCAT) left. Consumer, } struct Config { iface: String, mode: Mode, role: Role, 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, /// Spin-lock mode: instead of a free-running ticker, poll FPRD until the /// payload changes (new TwinCAT frame), then process immediately. /// Phase-locks J1900 processing to the primary master's frame arrival. spin: bool, /// verify 妯″紡闄勫姞: 鍐?0x0E00:01=1 鎭㈠ CoE 鍑哄巶榛樿 (闇€闅忓悗鏂數鎴?INIT 閲嶈浇) coe_restore: bool, f800_set: bool, /// revprobe 附加: 修复 SII section2 CRC (写 word15=calc) sii_fix: bool, /// 完全采纳从站 EEPROM 默认配置, 不写任何 SM/FMMU/PDO adopt_pure: bool, sync_ts: bool, dc_discipline: bool, /// 强制 SM3 长度 (0=默认 PAYLOAD_LEN+sm3_prefix) sm3_len_force: u16, /// Enable DC SYNC0 on the EL6695 secondary (cycle = --cycle-us) and print /// per-second DC phase/drift telemetry in the report line. dc: bool, /// Use the io_uring-based blocking TX/RX loop instead of AF_PACKET poll. iouring: bool, /// RFM device path override (e.g. /dev/rfm2g1) for multi-card hosts. rfm_dev: Option, } fn parse_args() -> Result { let mut cfg = Config { iface: "eth1".into(), mode: Mode::Ticker, role: Role::ConfigOwner, duration_sec: 60, cycle_us: 1000, core: 2, prio: 80, quiet: false, el2202: false, el2202_dual: false, el1252: false, el2262: false, el1252_every: 1, spin: false, coe_restore: false, f800_set: false, sii_fix: false, adopt_pure: false, sync_ts: false, dc_discipline: false, sm3_len_force: 0, dc: false, iouring: false, rfm_dev: None, }; 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 { 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, "verify" => Mode::Verify, "sniff" => Mode::Sniff, "rfm-ping" => Mode::RfmPing, "rfm-echo" => Mode::RfmEcho, "rfm-bridge" => Mode::RfmBridge, "rfm-ring" => Mode::RfmRing, "rfm-async" => Mode::RfmAsync, "rfm-lrw" => Mode::RfmLrw, "revprobe" => Mode::RevProbe, 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, "--spin" => cfg.spin = true, "--coe-restore" => cfg.coe_restore = true, "--f800-set" => cfg.f800_set = true, "--sii-fix" => cfg.sii_fix = true, "--adopt-pure" => cfg.adopt_pure = true, "--sync-ts" => cfg.sync_ts = true, "--dc-discipline" => cfg.dc_discipline = true, "--passive" => {}, "--sm3-len" => { cfg.sm3_len_force = val("--sm3-len")?.parse().map_err(|_| "bad --sm3-len")?; }, "--dc" => cfg.dc = true, "--iouring" => cfg.iouring = true, "--rfm-dev" => { let n: u32 = val("--rfm-dev")?.parse().map_err(|_| "bad --rfm-dev (use card index, e.g. 0 or 1)")?; cfg.rfm_dev = Some(format!("/dev/rfm2g{}", n)); } "--el2202" => cfg.el2202 = true, "--el2202-dual" => { cfg.el2202 = true; cfg.el2202_dual = true; } "--el1252" => cfg.el1252 = true, "--el2262" => cfg.el2262 = true, "--consumer" => cfg.role = Role::Consumer, "--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] [--consumer]" ); 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> { 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", Mode::Verify => "verify", Mode::Sniff => "sniff", Mode::RfmPing => "rfm-ping", Mode::RfmEcho => "rfm-echo", Mode::RfmBridge => "rfm-bridge", Mode::RfmRing => "rfm-ring", Mode::RfmAsync => "rfm-async", Mode::RfmLrw => "rfm-lrw", Mode::RevProbe => "revprobe", }, cfg.duration_sec, cfg.cycle_us, cfg.core, cfg.prio ); let pdu_storage = PDU_STORAGE.get_or_init(PduStorage::new); // ---- RFM-only modes: no EtherCAT bring-up needed ---- if matches!(cfg.mode, Mode::RfmPing | Mode::RfmEcho | Mode::RfmRing) { if cfg.mode == Mode::RfmRing { let rfm = el6695_rt::rfm::Rfm::open_path(cfg.rfm_dev.as_deref())?; let fd = rfm.device_fd.as_raw_fd(); let ok = el6695_rt::rfm::ring_cont(fd)?; println!("ring={}", if ok { "OK" } else { "BROKEN" }); std::process::exit(if ok { 0 } else { 1 }); } return rfm_mode_main(cfg).await; } 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 use_iouring = cfg.iouring; let _txrx = std::thread::spawn(move || { if let Err(e) = rt::promote_current_thread(1, 79, false) { eprintln!("tx_rx RT promotion failed (continuing): {}", e); } if use_iouring { match tx_rx_task_io_uring(&iface, tx, rx) { Ok(_) => {} Err(e) => eprintln!("io_uring tx_rx task ended with error: {:?}", e), } } else { 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); } } }); if std::env::args().any(|a| a == "--passive") { use std::ffi::CString; let ciface = CString::new(cfg.iface.as_bytes()).unwrap(); let fd = unsafe { libc::socket(libc::AF_PACKET, libc::SOCK_RAW, (libc::ETH_P_ALL as u16).to_be() as i32) }; assert!(fd >= 0, "raw socket failed"); let ifidx = unsafe { libc::if_nametoindex(ciface.as_ptr()) }; assert!(ifidx > 0); let mut sll: libc::sockaddr_ll = unsafe { std::mem::zeroed() }; sll.sll_family = libc::AF_PACKET as u16; sll.sll_protocol = (libc::ETH_P_ALL as u16).to_be(); sll.sll_ifindex = ifidx as i32; let r = unsafe { libc::bind(fd, &sll as *const libc::sockaddr_ll as *const libc::sockaddr, std::mem::size_of::() as u32) }; assert!(r == 0, "bind failed"); let tv = libc::timeval { tv_sec: 0, tv_usec: 50_000 }; unsafe { libc::setsockopt(fd, libc::SOL_SOCKET, libc::SO_RCVTIMEO, &tv as *const _ as *const libc::c_void, std::mem::size_of::() as u32) }; let t0 = std::time::Instant::now(); let (mut n_fpwr, mut n_fprd, mut n_apwr, mut n_aprd, mut n_other) = (0u64, 0u64, 0u64, 0u64, 0u64); let (mut n_sm2, mut n_other_addr) = (0u64, 0u64); let (mut n_sm2_wkc1, mut n_sm2_wkc0) = (0u64, 0u64); let mut n_lrw = 0u64; let mut lrw_wkc1 = 0u64; let mut samples = 0; println!("PASSIVE: listening 12s for EtherCAT frames (FPWR cmd=0x05 to 0x1C00)?"); while t0.elapsed() < std::time::Duration::from_secs(12) { let mut buf = [0u8; 2048]; let n = unsafe { libc::recv(fd, buf.as_mut_ptr() as *mut libc::c_void, buf.len(), 0) }; if n <= 0 { continue; } let n = n as usize; if n < 27 { continue; } let et = u16::from_be_bytes([buf[12], buf[13]]); if et != 0x88A4 { continue; } let cmd = buf[16]; match cmd { 0x05 => { n_fpwr += 1; let ado = u16::from_le_bytes([buf[20], buf[21]]); if ado == 0x1C00 { n_sm2 += 1; if n >= 828 { let wkc = u16::from_le_bytes([buf[826], buf[827]]); if wkc == 1 { n_sm2_wkc1 += 1; } else { n_sm2_wkc0 += 1; } } if samples < 6 && n >= 42 { let p = &buf[26..42]; let hex: Vec = p.iter().map(|b| format!("{:02X}", b)).collect(); let wkc = if n >= 828 { u16::from_le_bytes([buf[826], buf[827]]) } else { 0 }; println!(" SM2 pay={}.. wkc={}", &hex.join(" ")[..23], wkc); samples += 1; } } else { n_other_addr += 1; if n_other_addr % 5000 == 1 { let adp = u16::from_le_bytes([buf[18], buf[19]]); } } } 0x04 => { n_fprd += 1; if n_fprd % 4000 == 1 { let adp = u16::from_le_bytes([buf[18], buf[19]]); let ado = u16::from_le_bytes([buf[20], buf[21]]); } } 0x0C => n_lrw += 1, 0x02 => n_apwr += 1, 0x01 => n_aprd += 1, _ => n_other += 1, } } println!("PASSIVE done: FPWR={} (toSM2={} wkc1={} wkc0={}) FPRD={} LRW={}", n_fpwr, n_sm2, n_sm2_wkc1, n_sm2_wkc0, n_fprd, n_lrw); return Ok(()); } println!("Scanning segment (INIT -> PREOP)..."); let group = maindevice .init_single_group::(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::(md).await.unwrap_or(0xFFFF); let len = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_LEN).receive::(md).await.unwrap_or(0xFFFF); let ctrl = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_CTRL).receive::(md).await.unwrap_or(0xFF); let ena = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_ENABLE).receive::(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; // ---- Sniff mode: raw frame diagnostics ---- if cfg.mode == Mode::Sniff { use std::ffi::CString; use std::time::{Duration, Instant}; let iface_s = cfg.iface.clone(); let ciface = CString::new(iface_s.as_bytes()).unwrap(); let fd = unsafe { libc::socket(libc::AF_PACKET, libc::SOCK_RAW, (libc::ETH_P_ALL as u16).to_be() as i32) }; assert!(fd >= 0, "raw socket failed"); let ifidx = unsafe { libc::if_nametoindex(ciface.as_ptr()) }; assert!(ifidx > 0, "if_nametoindex failed"); { let mut sll: libc::sockaddr_ll = unsafe { std::mem::zeroed() }; sll.sll_family = libc::AF_PACKET as u16; sll.sll_protocol = (libc::ETH_P_ALL as u16).to_be(); sll.sll_ifindex = ifidx as i32; let r = unsafe { libc::bind(fd, &sll as *const libc::sockaddr_ll as *const libc::sockaddr, std::mem::size_of::() as u32) }; assert!(r == 0, "bind failed"); } { let tv = libc::timeval { tv_sec: 0, tv_usec: 100_000 }; unsafe { libc::setsockopt(fd, libc::SOL_SOCKET, libc::SO_RCVTIMEO, &tv as *const _ as *const libc::c_void, std::mem::size_of::() as u32) }; } let srcmac: Vec = std::fs::read_to_string(format!("/sys/class/net/{}/address", iface_s)).unwrap().trim().split(':').map(|h| u8::from_str_radix(h, 16).unwrap()).collect(); println!("sniff: ifindex={} mac={} (passive 3s, then 3x BRD 0x0110 probe)", ifidx, srcmac.iter().map(|b| format!("{:02X}", b)).collect::()); let dump = |tag: &str, buf: &[u8], n: usize| { let f = &buf[..n]; let hex: Vec = f.iter().take(40).map(|b| format!("{:02X}", b)).collect(); let et = u16::from_be_bytes([f[12], f[13]]); let mut extra = String::new(); if et == 0x88A4 && n >= 27 { let hdr = u16::from_be_bytes([f[14], f[15]]); let len = hdr & 0x0FFF; let typ = hdr >> 12; let cmd = f[16]; let adp = u16::from_be_bytes([f[18], f[19]]); let ado = u16::from_be_bytes([f[20], f[21]]); extra = format!(" || ecat len={} type={} cmd=0x{:02X} adp={:04X} ado={:04X} wkc={}", len, typ, cmd, adp, ado, f[13 + len as usize]); } println!("{} [{}B] {}{}", tag, n, hex.join(" "), extra); }; let t0 = Instant::now(); let (mut shown, mut total) = (0, 0); while t0.elapsed() < Duration::from_secs(3) { let mut buf = [0u8; 2048]; let n = unsafe { libc::recv(fd, buf.as_mut_ptr() as *mut libc::c_void, buf.len(), 0) }; if n > 0 { total += 1; if shown < 6 { dump("PASSIVE", &buf, n as usize); shown += 1; } } } println!("passive: {} frames in 3s", total); let mut frame = [0u8; 64]; frame[0..6].copy_from_slice(&[0xFF; 6]); frame[6..12].copy_from_slice(&srcmac); frame[12..14].copy_from_slice(&0x88A4u16.to_be_bytes()); frame[14..16].copy_from_slice(&0x100Au16.to_be_bytes()); frame[16] = 0x07; frame[17] = 0x01; frame[18..20].copy_from_slice(&0x0000u16.to_be_bytes()); frame[20..22].copy_from_slice(&0x0110u16.to_be_bytes()); frame[22] = 0x00; frame[23..25].copy_from_slice(&0x0000u16.to_be_bytes()); for k in 0..3u32 { let mut sa: libc::sockaddr_ll = unsafe { std::mem::zeroed() }; sa.sll_family = libc::AF_PACKET as u16; sa.sll_ifindex = ifidx as i32; sa.sll_halen = 6; sa.sll_addr[..6].copy_from_slice(&[0xFF; 6]); let n = unsafe { libc::sendto(fd, frame.as_ptr() as *const libc::c_void, 64, 0, &sa as *const libc::sockaddr_ll as *const libc::sockaddr, std::mem::size_of::() as u32) }; println!("BRD#{} sendto={} (64B)", k, n); let t1 = Instant::now(); while t1.elapsed() < Duration::from_millis(500) { let mut buf = [0u8; 2048]; let n = unsafe { libc::recv(fd, buf.as_mut_ptr() as *mut libc::c_void, buf.len(), 0) }; if n > 0 { dump(&format!("RESP#{}", k), &buf, n as usize); } } } unsafe { libc::close(fd); }; return Ok(()); } // ---- Verify mode: CoE audit (then exit, no SAFEOP) ---- if cfg.mode == Mode::Verify { println!("\n=== CoE Audit ==="); // 0xF008: NoCoeStorage / vendor reset let f008_0 = sd.sdo_read::(0xF008, 0).await; let f008_1 = sd.sdo_read::(0xF008, 1).await; println!(" 0xF008:00 = {:?} sub1={:?}", f008_0, f008_1); let f800_0 = sd.sdo_read::(0xF800, 0).await; let f800_1 = sd.sdo_read::(0xF800, 1).await; let f800_2 = sd.sdo_read::(0xF800, 2).await; let f800_3 = sd.sdo_read::(0xF800, 3).await; println!(" 0xF800 DeviceConfig: sub0={:?} sub1={:?} sub2={:?} sub3={:?}", f800_0, f800_1, f800_2, f800_3); if cfg.f800_set { for (sub, vals) in [(1u8, vec![1u16, 0xFFFF, 0xFF, 3]), (2, vec![1u16, 0xFFFF]), (3, vec![1u16])] { for v in vals { let w = sd.sdo_write::(0xF800, sub, v).await; let rb = sd.sdo_read::(0xF800, sub).await; println!(" --f800-set sub{}={:#06X} -> ok={} readback={:?}", sub, v, w.is_ok(), rb); } } } let no_coe_storage = f008_0.unwrap_or(0) == 0x12345678; println!(" NoCoeStorage: {}", if no_coe_storage { "锟?ENABLED (0x12345678) 锟?config will NOT persist to EEPROM" } else { "锟?disabled" }); // 0x1018: Identity for sub in 1..=4u8 { let v = sd.sdo_read::(0x1018, sub).await; let label = match sub { 1 => "Vendor", 2 => "Product", 3 => "Revision", _ => "Serial" }; println!(" 0x1018:{:02} ({}) = 0x{:08X}", sub, label, v.unwrap_or(0xFFFFFFFF)); } // Full PDO layout let snap = bridge::snapshot_layout(&sd, &maindevice, station).await; let h = bridge::compute_layout_hash(&snap); println!(" 0x1C12 assign: {:?}", snap.tx_assign); println!(" 0x1C13 assign: {:?}", snap.rx_assign); println!(" 0x1608 entries: {} (total {} bits)", snap.tx_entries.len(), snap.tx_total_bits); if !snap.tx_entries.is_empty() { for (i, (idx, sub, bl)) in snap.tx_entries.iter().enumerate().take(3) { println!(" [{:02}] 0x{:04X}:{:02} @ {} bits", i+1, idx, sub, bl); } if snap.tx_entries.len() > 3 { println!(" ... ({} more)", snap.tx_entries.len() - 3); } } println!(" 0x1A08 entries: {} (total {} bits)", snap.rx_entries.len(), snap.rx_total_bits); if !snap.rx_entries.is_empty() { for (i, (idx, sub, bl)) in snap.rx_entries.iter().enumerate().take(3) { println!(" [{:02}] 0x{:04X}:{:02} @ {} bits", i+1, idx, sub, bl); } if snap.rx_entries.len() > 3 { println!(" ... ({} more)", snap.rx_entries.len() - 3); } } println!(" SM2 len={} SM3 len={}", snap.sm2_len, snap.sm3_len); println!(" Layout CRC32 = 0x{:08X}", h); println!(" Expected hash = 0x{:08X}", bridge::EXPECTED_LAYOUT_HASH.load(std::sync::atomic::Ordering::Relaxed)); println!(" 0x6000[0] = {}", sd.sdo_read::(0x6000, 0).await.unwrap_or(0xFF)); println!(" 0x7000[0] = {}", sd.sdo_read::(0x7000, 0).await.unwrap_or(0xFF)); // AL state / error let (al, _) = bridge::drive_state(&maindevice, station, 0x0002).await; println!(" AL=0x{:04X} err={}", al, al & regs::AL_ERROR != 0); println!("=== CoE Audit complete ==="); // ---- 0x0E00 restore-default: 閲嶇疆 CoE/PDO/SM 鍒?EEPROM 鍑哄巶鍊?---- if cfg.coe_restore { println!("\n--coe-restore: writing 0x0E00:01 = 1 (restore default)..."); match sd.sdo_write::(0x0E00, 1, 1).await { Ok(()) => println!(" write OK"), Err(e) => println!(" write FAILED: {:?}", e), } // 璇诲洖 (閫氬父绔嬪嵆璇诲洖浠嶆槸鏃у€? 鏂數/INIT 鍚庣敓鏁? let rb = sd.sdo_read::(0x0E00, 1).await; println!(" readback 0x0E00:01 = {:?}", rb); // INIT 寰幆瑙﹀彂閲嶈浇 println!(" INIT cycle to trigger reload..."); let (al_i, _) = bridge::drive_state(&maindevice, station, 0x0001).await; println!(" INIT: {}", bridge::verdict(al_i, 0)); std::thread::sleep(Duration::from_millis(500)); let (al_p, _) = bridge::drive_state(&maindevice, station, 0x0002).await; println!(" PREOP: {}", bridge::verdict(al_p, 0)); // 閲嶈浇鍚庡啀鐪嬪竷灞€ let snap2 = bridge::snapshot_layout(&sd, &maindevice, station).await; println!(" after restore: 0x1C12={:?} 0x1C13={:?} 0x1608 entries={} 0x1A08 entries={}", snap2.tx_assign, snap2.rx_assign, snap2.tx_entries.len(), snap2.rx_entries.len()); let rb2 = sd.sdo_read::(0x0E00, 1).await; println!(" 0x0E00:01 after INIT = {:?}", rb2); println!("=== coe-restore done (濡備粛寮傚父璇风瀛愭柇鐢典竴娆? ==="); } return Ok(()); } // ---- ensure_link: adopt-if-valid (Consumer) or full reconfigure (ConfigOwner) ---- println!("\nensure_link: role={:?} ...", cfg.role); // Read current PDO layout from the bridge let layout_ok = bridge::layout_matches(&sd, &maindevice, station).await.unwrap_or(false); let (al, alc) = bridge::drive_state(&maindevice, station, 0x0002).await; let has_err = al & regs::AL_ERROR != 0; let al_state = al & 0x0F; println!(" AL=0x{:04X} err={} layout_ok={}", al, has_err, layout_ok); // 新固件(Rev6)检测: SM3 默认分配含 0x1A01 SYNC 前缀 -> 桥引擎需要它才转发 let sm3_prefix: usize = { let cnt13 = sd.sdo_read::(0x1C13, 0).await.unwrap_or(0); let mut entries13 = Vec::new(); for sub in 1..=cnt13.min(6) { let e = sd.sdo_read::(0x1C13, sub).await.unwrap_or(0); entries13.push(e); } println!(" default 0x1C13: count={} entries={:04X?}", cnt13, entries13); for pdo in [0x1A01u16, 0x1A02, 0x1A03] { let cnt = sd.sdo_read::(pdo, 0).await.unwrap_or(0); let mut bits = 0u32; let mut raw_entries = Vec::new(); for sub in 1..=cnt { let e = sd.sdo_read::(pdo, sub).await.unwrap_or(0); bits += e & 0xFF; raw_entries.push(format!("0x{:08X}", e)); } println!(" 0x{:04X}: {} entries, {} bits = {} bytes [{}]", pdo, cnt, bits, (bits + 7) / 8, raw_entries.join(" ")); } let first13 = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); if first13 == 0x1A01 { let cnt = sd.sdo_read::(0x1A01, 0).await.unwrap_or(0); let mut bits = 0u32; for sub in 1..=cnt { let e = sd.sdo_read::(0x1A01, sub).await.unwrap_or(0); bits += e & 0xFF; } let bytes = ((bits + 7) / 8) as usize; println!(" new-firmware SYNC prefix: 0x1A01 = {} bits = {} bytes", bits, bytes); bytes } else { 0 } }; let sm3_prefix: usize = if cfg.sync_ts { 22 } else { 0 }; // 0x1A02 timestamp prefix = 22 bytes if cfg.sync_ts { let blob13 = bridge::assign_blob(&[0x1A02, 0x1A08]); match bridge::sdo_write_ca_checked(&sd, 0x1C13, 0, &blob13).await { Ok(_) => println!(" sync-ts: 0x1C13=[0x1A02,0x1A08] written"), Err(e) => println!(" sync-ts: 0x1C13 write ERR {:?}", e), } // Manual: 10F5:01 0=SyncMaster on primary, 1=on secondary. Follower(D2000)=SyncSlave // with CX(primary)=SyncMaster -> correct value is 0. Explicitly restore (may be persisted as 1). let w = sd.sdo_write::(0x10F5, 1, 0).await; let cur = sd.sdo_read::(0x10F5, 1).await.unwrap_or(0xFF); println!(" sync-ts: 0x10F5:01 write0={:?} readback={}", w.is_ok(), cur); // Manual §6.7 direct DC coupling: each master must write 0x0920 system-time-offset // from its side; EL6695 then frequency-locks both ESCs. Align ESC time to local mono clock. let dc_t = ethercrab::Command::fprd(station, 0x0910u16).receive::(&maindevice).await.unwrap_or(0); let loc = rt::now_ns(); let off = loc.wrapping_sub(dc_t); let wo = ethercrab::Command::fpwr(station, 0x0920u16).send(&maindevice, off).await; let dc_t2 = ethercrab::Command::fprd(station, 0x0910u16).receive::(&maindevice).await.unwrap_or(0); let loc2 = rt::now_ns(); println!(" sync-ts: 0x0920 offset write={:?}; now delta(local-esc)={}ns", wo.is_ok(), loc2.wrapping_sub(dc_t2) as i64); } match cfg.role { Role::Consumer => { // ---- Consumer: idempotent writes, no INIT鈫扨REOP cycle ---- // SM/PDO config must ALWAYS be written after scan (scanning wipes // SM registers and PDO assign/map). The writes are idempotent: // same layout as the expected expanded 1024-byte config. // Clear any PREOP+ERR (L1 recovery) if has_err { println!(" Consumer: clearing PREOP+ERR ..."); // PREOP+ERR 恢复必须带 ERROR_ACK (0x0010) let _ = ethercrab::Command::fpwr(station, regs::AL_CONTROL) .send(&maindevice, 0x0012u16).await; std::thread::sleep(Duration::from_millis(200)); let _ = ethercrab::Command::fpwr(station, regs::AL_CONTROL) .send(&maindevice, 0x0002u16).await; std::thread::sleep(Duration::from_millis(300)); } // Skip INIT鈫扨REOP 锟?preserves TwinCAT's persisted CoE objects. println!(" Consumer: idempotent SM/FMMU/PDO config"); // SM2/SM3 registers 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, PAYLOAD_LEN as u16).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, if cfg.sm3_len_force > 0 { cfg.sm3_len_force } else { (PAYLOAD_LEN + sm3_prefix) as u16 }).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, (PAYLOAD_LEN + sm3_prefix) as u16, PAYLOAD_LEN as u16).await; // PDO maps and assigns (whitelisted, idempotent) println!(" Consumer: write PDO mappings..."); let blob = bridge::twin_layout_blob_sized(0x7000, PAYLOAD_LEN); let r1 = bridge::sdo_write_ca_checked(&sd, 0x1608, 0, &blob).await; println!(" 0x1608 CA write: {:?}", r1.as_ref().map(|_| "OK").map_err(|e| format!("{:?}", e))); let blob = bridge::twin_layout_blob_sized(0x6000, PAYLOAD_LEN); let r2 = bridge::sdo_write_ca_checked(&sd, 0x1A08, 0, &blob).await; println!(" 0x1A08 CA write: {:?}", r2.as_ref().map(|_| "OK").map_err(|e| format!("{:?}", e))); let e16 = sd.sdo_read::(0x1608, 0).await.unwrap_or(0xFF); let e1a = sd.sdo_read::(0x1A08, 0).await.unwrap_or(0xFF); println!(" readback: 0x1608 entries={} 0x1A08 entries={}", e16, e1a); println!(" Consumer: write PDO assignments..."); let blob = bridge::assign_blob(&[0x1608]); let cur12 = sd.sdo_read::(0x1C12, 1).await.unwrap_or(0); if cfg.adopt_pure || cur12 == 0x1608 { println!(" 0x1C12 adopted (has 0x1608)"); } else { let r_1c12 = bridge::sdo_write_ca_checked(&sd, 0x1C12, 0, &blob).await; println!(" 0x1C12 write ok={}", r_1c12.is_ok()); } let blob = bridge::assign_blob(&[0x1A08]); let cur13 = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); if false { println!(" 0x1C13 adopted (first=0x{:04X})", cur13); } else { let blob13 = if sm3_prefix > 0 { bridge::assign_blob(&[0x1A01, 0x1A08]) } else { blob.clone() }; let r_1c13 = bridge::sdo_write_ca_checked(&sd, 0x1C13, 0, &blob13).await; println!(" 0x1C13 write ok={}", r_1c13.is_ok()); } let _o7 = sd.sdo_read::(0x7000, 0).await.unwrap_or(0xFF); let _o6 = sd.sdo_read::(0x6000, 0).await.unwrap_or(0xFF); let snap = bridge::snapshot_layout(&sd, &maindevice, station).await; let h = bridge::compute_layout_hash(&snap); println!(" Consumer: dyn 0x6000[0]={} 0x7000[0]={} hash=0x{:08X}", _o6, _o7, h); } Role::ConfigOwner => { // ---- ConfigOwner: full INIT鈫扨REOP + PDO rebuild ---- // Clear any PREOP+ERR by re-requesting PREOP if has_err { println!(" ConfigOwner: clearing PREOP+ERR ..."); // PREOP+ERR 恢复必须带 ERROR_ACK (0x0010) let _ = ethercrab::Command::fpwr(station, regs::AL_CONTROL) .send(&maindevice, 0x0012u16).await; std::thread::sleep(Duration::from_millis(200)); let _ = ethercrab::Command::fpwr(station, regs::AL_CONTROL) .send(&maindevice, 0x0002u16).await; std::thread::sleep(Duration::from_millis(300)); } // INIT鈫扨REOP cycle to reset CoE state println!(" INIT鈫扨REOP cycle..."); let (al_i, _) = bridge::drive_state(&maindevice, station, 0x0001).await; println!(" INIT: {}", bridge::verdict(al_i, 0)); if al_i & 0x0F != 0x01 { return Err(format!("INIT failed: {}", bridge::verdict(al_i, 0)).into()); } let (al_p, alc_p) = bridge::drive_state(&maindevice, station, 0x0002).await; println!(" PREOP: {}", bridge::verdict(al_p, alc_p)); if al_p & 0x0F != 0x02 { return Err(format!("PREOP failed: {}", bridge::verdict(al_p, alc_p)).into()); } // Write PDO mappings (0x1608 TxPDO, 0x1A08 RxPDO) // Whitelist-checked: only {0x1608,0x1A08,0x1C12,0x1C13,0xF008,0xF800} allowed. println!(" Write PDO mappings..."); let blob = bridge::twin_layout_blob_sized(0x7000, PAYLOAD_LEN); let _ = bridge::sdo_write_ca_checked(&sd, 0x1608, 0, &blob).await; let blob = bridge::twin_layout_blob_sized(0x6000, PAYLOAD_LEN); let _ = bridge::sdo_write_ca_checked(&sd, 0x1A08, 0, &blob).await; // Write PDO assignments (triggers 0x6000/0x7000 object creation) println!(" Write PDO assignments..."); let blob = bridge::assign_blob(&[0x1608]); let cur12 = sd.sdo_read::(0x1C12, 1).await.unwrap_or(0); if cfg.adopt_pure || cur12 == 0x1608 { println!(" 0x1C12 adopted (has 0x1608)"); } else { let r_1c12 = bridge::sdo_write_ca_checked(&sd, 0x1C12, 0, &blob).await; println!(" 0x1C12 write ok={}", r_1c12.is_ok()); } let blob = bridge::assign_blob(&[0x1A08]); let cur13 = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); if false { println!(" 0x1C13 adopted (first=0x{:04X})", cur13); } else { let blob13 = if sm3_prefix > 0 { bridge::assign_blob(&[0x1A01, 0x1A08]) } else { blob.clone() }; let r_1c13 = bridge::sdo_write_ca_checked(&sd, 0x1C13, 0, &blob13).await; println!(" 0x1C13 write ok={}", r_1c13.is_ok()); } // Verify the dynamic objects were instantiated let _o7 = sd.sdo_read::(0x7000, 0).await.unwrap_or(0xFF); let _o6 = sd.sdo_read::(0x6000, 0).await.unwrap_or(0xFF); // Compute layout hash after successful config let snap = bridge::snapshot_layout(&sd, &maindevice, station).await; let h = bridge::compute_layout_hash(&snap); println!(" dyn: 0x6000[0]={} 0x7000[0]={} layout_hash=0x{:08X}", _o6, _o7, h); // Configure SM2/SM3 + 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, PAYLOAD_LEN as u16).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, if cfg.sm3_len_force > 0 { cfg.sm3_len_force } else { PAYLOAD_LEN as u16 }).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, (PAYLOAD_LEN + sm3_prefix) as u16, PAYLOAD_LEN as u16).await; let sm2_len_rb = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_LEN).receive::(&maindevice).await; let sm3_len_rb = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_LEN).receive::(&maindevice).await; println!(" SM LEN readback: SM2={:?} SM3={:?}", sm2_len_rb, sm3_len_rb); } } // 新固件 SYNC PDO (0x1A01) 需要 DC SYNC0 已配置才接受 SAFEOP // Rev6 firmware empirically REQUIRES these writes to accept a SYNC PDO at SAFEOP // (manual §6.7 says these registers don't exist - true for old FW; Rev6 accepts them). if sm3_prefix > 0 { let t_now = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::(&maindevice).await.unwrap_or(0); let start_t = t_now.wrapping_add(200_000_000); let w1 = ethercrab::Command::fpwr(station, regs::DC_SYNC_START_TIME).send(&maindevice, start_t).await; let w2 = ethercrab::Command::fpwr(station, regs::DC_SYNC0_CYCLE).send(&maindevice, 1_000_000u32).await; let w3 = ethercrab::Command::fpwr(station, regs::DC_SYNC_ACTIVATION).send(&maindevice, regs::DC_SYNC0_ACTIVATE).await; println!(" pre-SAFEOP DC(Rev6): start={:?} cycle={:?} act={:?}", w1.is_ok(), w2.is_ok(), w3.is_ok()); for idx in [0x1C32u16, 0x1C33] { match sd.sdo_write::(idx, 0x01, 2).await { Ok(_) => println!(" pre-SAFEOP: CoE 0x{:04X}:01 sync_mode=2 OK", idx), Err(e) => println!(" pre-SAFEOP: CoE 0x{:04X}:01 sync_mode=2 ERR {:?}", idx, e), } } } // 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! <<<"); { let a80 = ethercrab::Command::fprd(station, 0x0980u16).receive::(&maindevice).await; let a81 = ethercrab::Command::fprd(station, 0x0981u16).receive::(&maindevice).await; let t_a = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::(&maindevice).await.unwrap_or(0); std::thread::sleep(Duration::from_millis(200)); let t_b = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::(&maindevice).await.unwrap_or(0); let s9ae = ethercrab::Command::fprd(station, 0x09AEu16).receive::(&maindevice).await; println!(" SYNC0@OP: act={:?} status={:?} 9AE={:?} dc_time adv={}ns/200ms", a80.as_ref().map(|v| format!("0x{:02X}", v)), a81.as_ref().map(|v| format!("0x{:02X}", v)), s9ae.as_ref().map(|v| format!("0x{:02X}", v)), t_b.wrapping_sub(t_a)); } // 运行时分配读回 + 动态前缀检测 (新固件 SM3 可能带 0x1A01 SYNC 前缀) let rx_prefix_rt: usize = { let n = sd.sdo_read::(0x1C13, 0).await.unwrap_or(0xFF); let first = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); println!(" runtime 0x1C13: count={} first=0x{:04X}", n, first); if first == 0x1A01 || first == 0x1A02 { let cnt = sd.sdo_read::(first, 0).await.unwrap_or(0); let mut bits = 0u32; for sub in 1..=cnt { let e = sd.sdo_read::(first, sub).await.unwrap_or(0); bits += e & 0xFF; } let bytes = ((bits + 7) / 8) as usize; println!(" SYNC prefix detected: 0x{:04X} = {} bits = {} bytes", first, bits, bytes); bytes } else { 0 } }; RX_PREFIX_DYN.store(rx_prefix_rt, Ordering::Relaxed); // ---- revprobe: reverse-path deep diagnosis ---- if cfg.mode == Mode::RevProbe { println!("\nrevprobe: dump SM0-7 + FMMU0-3"); let mut sms = vec![(0u16, 0u16, 0u8, 0u8); 8]; for n in 0..8u16 { let s = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_START).receive::(&maindevice).await.unwrap_or(0xFFFF); let l = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_LEN).receive::(&maindevice).await.unwrap_or(0); let c = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_CTRL).receive::(&maindevice).await.unwrap_or(0xFF); let e = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_ENABLE).receive::(&maindevice).await.unwrap_or(0xFF); sms[n as usize] = (s, l, c, e); println!(" SM{}: start=0x{:04X} len={} ctrl=0x{:02X} ena=0x{:02X}", n, s, l, c, e); } for n in 0..4u16 { let base = 0x0600u16 + n * 16; let raw = ethercrab::Command::fprd(station, base).receive_slice(&maindevice, 16).await .map(|p| { let mut v = [0u8; 16]; v.copy_from_slice(&p); v }).unwrap_or([0u8; 16]); println!(" FMMU{}: {:02X?}", n, raw); } // ---- SII EEPROM dump + CRC check ---- println!("\n [SII] EEPROM config reg + first 128 bytes"); let eep_cfg = ethercrab::Command::fprd(station, 0x0500u16).receive::(&maindevice).await.unwrap_or(0xFFFF); println!(" eeprom_cfg=0x{:04X} (bit0=detected {})", eep_cfg, eep_cfg & 1); fn sii_crc8(data: &[u8]) -> u8 { let mut crc: u8 = 0xFF; for &b in data { crc ^= b; for _ in 0..8 { crc = if crc & 0x80 != 0 { (crc << 1) ^ 0x07 } else { crc << 1 }; } } crc } let mut eeprom = [0u8; 128]; let mut sii_err = 0u32; for wa in 0..32u16 { // wait idle let mut ready = false; for _ in 0..200 { let ctl = ethercrab::Command::fprd(station, 0x0502u16).receive::(&maindevice).await.unwrap_or(0x8000); if ctl & 0x8000 == 0 { ready = true; break; } std::thread::sleep(Duration::from_micros(100)); } if !ready { sii_err += 1; continue; } let _ = ethercrab::Command::fpwr(station, 0x0504u16).send(&maindevice, (wa * 2) as u32).await; let _ = ethercrab::Command::fpwr(station, 0x0502u16).send(&maindevice, 0x0100u16).await; let mut done = false; for _ in 0..200 { let ctl = ethercrab::Command::fprd(station, 0x0502u16).receive::(&maindevice).await.unwrap_or(0x8000); if ctl & 0x8000 == 0 { if ctl & 0x4000 == 0 { done = true; } break; } std::thread::sleep(Duration::from_micros(100)); } if !done { sii_err += 1; continue; } if let Ok(d) = ethercrab::Command::fprd(station, 0x0508u16).receive_slice(&maindevice, 4).await { let off = (wa as usize) * 4; if off + 4 <= 128 { eeprom[off..off + 4].copy_from_slice(&d); } } else { sii_err += 1; } } for row in 0..8 { let off = row * 16; let hexs: Vec = eeprom[off..off + 16].iter().map(|b| format!("{:02X}", b)).collect(); println!(" {:04X}: {}", off, hexs.join(" ")); } let crc1 = sii_crc8(&eeprom[0..14]); let crc2 = sii_crc8(&eeprom[16..30]); println!(" section1 CRC: calc=0x{:02X} stored=0x{:02X} {}", crc1, eeprom[14], if crc1 == eeprom[14] { "OK" } else { "BAD!" }); println!(" section2 CRC: calc=0x{:02X} stored=0x{:02X} {}", crc2, eeprom[30], if crc2 == eeprom[30] { "OK" } else { "BAD!" }); println!(" sii_errors={}", sii_err); if cfg.sii_fix && crc2 != eeprom[30] { println!("\n [SII-FIX] writing corrected CRC 0x{:02X} to EEPROM word 15", crc2); // SII write: data reg 0x0508 <- FA 00, addr 0x0504 <- 30, ctrl 0x0502 <- 0x0200 (write) let wdata: [u8; 4] = [crc2, 0x00, 0x00, 0x00]; // ETG 顺序: 地址 -> 数据 -> 命令; 写前 dump 控制/状态块 let mut regs8 = [0u8; 16]; if let Ok(d) = ethercrab::Command::fprd(station, 0x0500u16).receive_slice(&maindevice, 16).await { if d.len() >= 16 { regs8.copy_from_slice(&d[..16]); } } println!(" sii_regs pre-write: {:02X?}", regs8); // 变体A: 字节地址30 let _ = ethercrab::Command::fpwr(station, 0x0504u16).send(&maindevice, 30u32).await; let _ = ethercrab::Command::fpwr(station, 0x0508u16).send(&maindevice, &wdata).await; let _ = ethercrab::Command::fpwr(station, 0x0502u16).send(&maindevice, 0x0200u16).await; std::thread::sleep(Duration::from_millis(20)); // 变体B: 字地址15 let _ = ethercrab::Command::fpwr(station, 0x0504u16).send(&maindevice, 15u32).await; let _ = ethercrab::Command::fpwr(station, 0x0508u16).send(&maindevice, &wdata).await; let _ = ethercrab::Command::fpwr(station, 0x0502u16).send(&maindevice, 0x0200u16).await; std::thread::sleep(Duration::from_millis(20)); // reload: 0x0500 bit4 let _ = ethercrab::Command::fpwr(station, 0x0500u16).send(&maindevice, 0x0010u16).await; std::thread::sleep(Duration::from_millis(300)); let cfg_after = ethercrab::Command::fprd(station, 0x0500u16).receive::(&maindevice).await.unwrap_or(0xFFFF); println!(" eeprom_cfg after reload: 0x{:04X}", cfg_after); let mut wok = false; for _ in 0..400 { let ctl = ethercrab::Command::fprd(station, 0x0502u16).receive::(&maindevice).await.unwrap_or(0x8000); if ctl & 0x8000 == 0 { wok = ctl & 0x4000 == 0; break; } std::thread::sleep(Duration::from_micros(100)); } println!(" write done ok={}", wok); // 回读验证 std::thread::sleep(Duration::from_millis(50)); let _ = ethercrab::Command::fpwr(station, 0x0504u16).send(&maindevice, 28u32).await; let _ = ethercrab::Command::fpwr(station, 0x0502u16).send(&maindevice, 0x0100u16).await; for _ in 0..400 { let ctl = ethercrab::Command::fprd(station, 0x0502u16).receive::(&maindevice).await.unwrap_or(0x8000); if ctl & 0x8000 == 0 { break; } std::thread::sleep(Duration::from_micros(100)); } if let Ok(d) = ethercrab::Command::fprd(station, 0x0508u16).receive_slice(&maindevice, 4).await { println!(" readback words14-15: {:02X?}", &d[..]); } println!(" -> 请端子断电重启, 再跑 revprobe 验证 CRC/桥"); } // ---- 0xF130 sync mode ---- let f130_0 = sd.sdo_read::(0xF130, 0).await; let f130_1 = sd.sdo_read::(0xF130, 1).await; println!("\n [0xF130] sub0={:?} sub1={:?}", f130_0, f130_1); // ---- 0xF800 device config ---- let f800_sub0 = sd.sdo_read::(0xF800, 0).await; let f800_1 = sd.sdo_read::(0xF800, 1).await; let f800_2 = sd.sdo_read::(0xF800, 2).await; let f800_3 = sd.sdo_read::(0xF800, 3).await; println!("\n [0xF800 DeviceConfig] sub0={:?} Config1={:?} Config2={:?} Config3={:?}", f800_sub0, f800_1, f800_2, f800_3); // ---- 桥诊断对象 ---- println!(" [BridgeDiag]"); let f630_0 = sd.sdo_read::(0xF630, 0).await; println!(" 0xF630 sub0={:?}", f630_0); for sub in 1..=4u8 { let v = sd.sdo_read::(0xF630, sub).await; match v { Ok(x) => println!(" 0xF630:{}=0x{:04X}", sub, x), Err(_) => {} } } let f640_0 = sd.sdo_read::(0xF640, 0).await; println!(" 0xF640 sub0={:?}", f640_0); let c16 = sd.sdo_read::(0xF640, 1).await; let c_lo = sd.sdo_read::(0xF640, 2).await; let c_hi = sd.sdo_read::(0xF640, 3).await; println!(" 0xF640:1 u16Count={:?} :2 LO={:?} :3 HI={:?}", c16, c_lo, c_hi); let s10f4_0 = sd.sdo_read::(0x10F4, 0).await; let s10f4_2 = sd.sdo_read::(0x10F4, 2).await; let s10f4_10 = sd.sdo_read::(0x10F4, 0x10).await; println!(" 0x10F4 sub0={:?} SyncMode(2)={:?} ExtDevNotConn(0x10)={:?}", s10f4_0, s10f4_2, s10f4_10); let s10f5_0 = sd.sdo_read::(0x10F5, 0).await; let s10f5_1 = sd.sdo_read::(0x10F5, 1).await; println!(" 0x10F5 sub0={:?} SyncMaster(1)={:?}", s10f5_0, s10f5_1); let fr = sd.sdo_write::(0x1011, 1, 0x64616F6C).await; let frb = sd.sdo_read::(0x1011, 1).await; println!(" FACTORY-RESET 0x1011:01 write={:?} readback=0x{:08X?}", fr.as_ref().map(|_| "OK"), frb); let w = sd.sdo_write::(0x10F5, 1, 0).await; let rb = sd.sdo_read::(0x10F5, 1).await; println!(" WRITE 0x10F5:01=1 -> {:?}, readback={:?}", w.as_ref().map(|_| "OK"), rb); let p32 = sd.sdo_read::(0x1C32, 1).await; let p33 = sd.sdo_read::(0x1C33, 1).await; println!(" 0x1C32:01(SMout syncmode)={:?} 0x1C33:01(SMin syncmode)={:?}", p32, p33); let fa20_0 = sd.sdo_read::(0xFA20, 0).await; println!(" 0xFA20 sub0={:?}", fa20_0); for sub in 1..=8u8 { let v = sd.sdo_read::(0xFA20, sub).await; match v { Ok(x) => println!(" 0xFA20:{}={}", sub, x), Err(_) => {} } } let fwd = ethercrab::Command::fprd(station, SM_RX_ADDR).receive_slice(&maindevice, 8).await .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }).unwrap_or([0xFFu8; 8]); println!(" [CTRL+] SM3(forward) buffer: {:02X?}", fwd); // ---- 桥诊断对象 (EL6695 特有) ---- println!(" [BridgeDiag BEFORE]"); for (idx, subs) in [(0xF630u16, 6u8), (0xF640, 4), (0xFA20, 6)] { let n = sd.sdo_read::(idx, 0).await.unwrap_or(0); println!(" 0x{:04X} sub0={}", idx, n); for sub in 1..=subs { if let Ok(x) = sd.sdo_read::(idx, sub).await { println!(" 0x{:04X}:{} = 0x{:08X}", idx, sub, x); } } } println!("\n [TestA] disable SM2 -> write magic -> readback -> re-enable"); let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE).send(&maindevice, 0x00u8).await; std::thread::sleep(Duration::from_millis(10)); let magic: [u8; 8] = [0xDE, 0xAD, 0xBE, 0xEF, 0xCA, 0xFE, 0xBA, 0xBE]; let w = ethercrab::Command::fpwr(station, SM_TX_ADDR).send(&maindevice, &magic).await; std::thread::sleep(Duration::from_millis(5)); let rb = ethercrab::Command::fprd(station, SM_TX_ADDR).receive_slice(&maindevice, 8).await .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }).unwrap_or([0xFFu8; 8]); println!(" write_ok={} readback={:02X?}", w.is_ok(), rb); let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE).send(&maindevice, 0x01u8).await; // TestA2: SM2 ENABLED -> write magic -> poll readback @2ms (consumption check) println!("\n [TestA2] SM2 enabled -> magic -> poll consumption"); let mut last_rb = [0u8; 8]; let mut consumed_at: Option = None; for i in 0..10u32 { if i == 0 { let _ = ethercrab::Command::fpwr(station, SM_TX_ADDR).send(&maindevice, &magic).await; } std::thread::sleep(Duration::from_millis(2)); let rb = ethercrab::Command::fprd(station, SM_TX_ADDR).receive_slice(&maindevice, 8).await .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }).unwrap_or([0xFFu8; 8]); println!(" t+{}ms: {:02X?}", i * 2, rb); last_rb = rb; if rb != magic && consumed_at.is_none() { consumed_at = Some(i * 2); } } match consumed_at { Some(ms) => println!(" buffer CHANGED at t+{}ms -> firmware CONSUMES SM2 (reads our data, forwarding broken internally)", ms), None => println!(" magic PERSISTED -> firmware NOT consuming SM2 (bridge never reads our writes)"), } let _ = last_rb; println!("\n [TestB] write changing pattern to SM2, scan other SM buffers (10s)"); let mut found: Option<(u16, [u8; 8])> = None; for t in 0..200u32 { let tag = 0xA0u8 + (t % 16) as u8; let pat: [u8; 8] = [tag, tag, 0x55, 0xAA, tag, 0x5A, 0xA5, tag]; let _ = ethercrab::Command::fpwr(station, SM_TX_ADDR).send(&maindevice, &pat).await; for n in 0..8u16 { if n == 2 { continue; } let (s, l, _, _) = sms[n as usize]; if l == 0 || s == 0xFFFF { continue; } let buf = ethercrab::Command::fprd(station, s).receive_slice(&maindevice, 8).await .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }).unwrap_or([0u8; 8]); if buf[0] == tag && buf[2] == 0x55 && buf[3] == 0xAA { found = Some((n, buf)); break; } } if found.is_some() { break; } std::thread::sleep(Duration::from_millis(50)); } println!(" [BridgeDiag AFTER TestB]"); { let c16 = sd.sdo_read::(0xF640, 1).await; let c_lo = sd.sdo_read::(0xF640, 2).await; let c_hi = sd.sdo_read::(0xF640, 3).await; println!(" 0xF640:1 u16Count={:?} :2 LO={:?} :3 HI={:?}", c16, c_lo, c_hi); } // ---- TestC: 逻辑寻址 (FMMU PDO 路径) 写入 ---- println!("\n [TestC] LRW logical write to SM2 via FMMU0"); let magic2: [u8; 8] = [0xC0, 0xDE, 0xC0, 0xDE, 0xC0, 0xDE, 0xC0, 0xDE]; for _ in 0..5 { let w = ethercrab::Command::lwr(0x00000000u32).send(&maindevice, &magic2).await; println!(" lwr(0x0): {:?}", w.is_ok()); std::thread::sleep(Duration::from_millis(5)); } // 等 bridge 转发后重读计数 std::thread::sleep(Duration::from_millis(50)); { let c16 = sd.sdo_read::(0xF640, 1).await; let c_lo = sd.sdo_read::(0xF640, 2).await; let c_hi = sd.sdo_read::(0xF640, 3).await; println!(" [after TestC] 0xF640:1 u16Count={:?} :2 LO={:?} :3 HI={:?}", c16, c_lo, c_hi); } match found { Some((n, buf)) => println!(" FOUND! pattern in SM{} buffer: {:02X?} -> bridge forwards OK, problem is CX read side!", n, buf), None => println!(" pattern NOT found in any SM buffer -> bridge firmware not forwarding (device level)"), } // TestB 后重读桥写计数 for sub in 1..=4u8 { let v = sd.sdo_read::(0xF640, sub).await; match v { Ok(x) => println!(" [after TestB] 0xF640:{}={}", sub, x), Err(_) => {} } } let alr = ethercrab::Command::fprd(station, 0x0130u16).receive::(&maindevice).await.unwrap_or(0xFFFF); println!(" final AL=0x{:04X}", alr); return Ok(()); } // ---- Optional DC SYNC0 on the EL6695 secondary ---- // ethercrab init already distributed the system time (offset/propagation // delay/static drift). Here we only start the SYNC0 unit on --cycle-us // grid and verify the clock actually advances. let mut dc_ref: Option<(u64, u64)> = None; // (dc_time, mono_time) at setup if cfg.dc { let t1 = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME) .receive::(&maindevice) .await .unwrap_or(0); std::thread::sleep(Duration::from_millis(100)); let t2 = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME) .receive::(&maindevice) .await .unwrap_or(0); let adv = t2.wrapping_sub(t1); println!("DC system time: {} -> {} (adv {} ns in 100ms)", t1, t2, adv); if adv < 50_000_000 || adv > 500_000_000 { println!("DC: clock not advancing sanely; SYNC0 setup skipped"); } else { // Diagnostics: which DC register groups actually exist? for (reg, name) in [ (0x0900u16, "rx_time_port0(u32)"), (0x0920u16, "sys_time_offset(u64)"), (0x0928u16, "prop_delay(u32)"), (0x0930u16, "drift... (u16)"), (0x0980u16, "sync_activation(u8)"), (0x0981u16, "sync_active_byte(u8)"), (0x09A0u16, "sync0_cycle(u32)"), (0x09AEu16, "sync0_status(u8)"), ] { match ethercrab::Command::fprd(station, reg) .receive_slice(&maindevice, 8) .await { Ok(pdu) => { let raw: &[u8] = &pdu; println!(" DC probe 0x{:04X} {}: {:02X?}", reg, name, &raw[..8.min(raw.len())]); } Err(e) => println!(" DC probe 0x{:04X} {}: ERR {:?}", reg, name, e), } } // CoE view: 0x1C32 (SM2 outputs) / 0x1C33 (SM3 inputs) sync params for idx in [0x1C32u16, 0x1C33] { let n = sd.sdo_read::(idx, 0).await.unwrap_or(0xFF); println!(" CoE 0x{:04X}: {} subentries", idx, n); for (sub, name) in [(0x01u8, "sync_mode"), (0x04, "modes_supported"), (0x0B, "sm_event_missed"), (0x0C, "cycle_exceeded")] { match sd.sdo_read::(idx, sub).await { Ok(x) => println!(" CoE 0x{:04X}:{:02X} {} = 0x{:04X}", idx, sub, name, x), Err(e) => println!(" CoE 0x{:04X}:{:02X} {} ERR {:?}", idx, sub, name, e), } } match sd.sdo_read::(idx, 0x02).await { Ok(x) => println!(" CoE 0x{:04X}:02 cycle_time_ns = {}", idx, x), Err(e) => println!(" CoE 0x{:04X}:02 cycle_time_ns ERR {:?}", idx, e), } match sd.sdo_read::(idx, 0x20).await { Ok(x) => println!(" CoE 0x{:04X}:20 sync_error = {}", idx, x), Err(e) => println!(" CoE 0x{:04X}:20 sync_error ERR {:?}", idx, e), } } let start = t2.wrapping_add(100_000_000); let w1 = ethercrab::Command::fpwr(station, regs::DC_SYNC_START_TIME) .send(&maindevice, start) .await; println!(" DC write start_time(0x0990): {:?}", w1.as_ref().map(|_| "OK").map_err(|e| format!("{:?}", e))); let w2 = ethercrab::Command::fpwr(station, regs::DC_SYNC0_CYCLE) .send(&maindevice, cycle_ns as u32) .await; println!(" DC write sync0_cycle(0x09A0): {:?}", w2.as_ref().map(|_| "OK").map_err(|e| format!("{:?}", e))); let w3 = regs::rmw_u8( &maindevice, station, regs::DC_SYNC_ACTIVATION, regs::DC_SYNC0_ACTIVATE, 0, ) .await; println!(" DC rmw activation(0x0980): {:?}", w3.as_ref().map(|v| format!("0x{:02X}", v)).map_err(|e| format!("{:?}", e))); // rmw needs a read (fails here); try a blind full-byte write instead let w4 = ethercrab::Command::fpwr(station, regs::DC_SYNC_ACTIVATION) .send(&maindevice, regs::DC_SYNC0_ACTIVATE) .await; println!(" DC blind write activation(0x0980)=0x01: {:?}", w4.as_ref().map(|_| "OK").map_err(|e| format!("{:?}", e))); std::thread::sleep(Duration::from_millis(150)); // Try selecting DC-Synchron SYNC0 via firmware object (mode 2). // May be rejected in OP; worth observing the abort code either way. let mut coe_dc_ok = true; for idx in [0x1C32u16, 0x1C33] { match sd.sdo_write::(idx, 0x01, 2).await { Ok(_) => println!(" CoE write 0x{:04X}:01 sync_mode=2 OK", idx), Err(e) => { coe_dc_ok = false; println!(" CoE write 0x{:04X}:01 sync_mode=2 ERR {:?}", idx, e); } } match sd.sdo_read::(idx, 0x01).await { Ok(x) => println!(" CoE-after 0x{:04X}:01 sync_mode = {}", idx, x), Err(e) => { coe_dc_ok = false; println!(" CoE-after 0x{:04X}:01 sync_mode ERR {:?}", idx, e); } } } if coe_dc_ok { let t_dc = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME) .receive::(&maindevice) .await .unwrap_or(0); dc_ref = Some((t_dc, rt::now_ns())); println!("DC-Synchron: firmware sync_mode=2 accepted, DC telemetry on"); } let act = ethercrab::Command::fprd(station, regs::DC_SYNC_ACTIVATION) .receive::(&maindevice) .await; let cyc0 = ethercrab::Command::fprd(station, regs::DC_SYNC0_CYCLE) .receive::(&maindevice) .await; let t3 = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME) .receive::(&maindevice) .await .unwrap_or(0); let (al, alc) = bridge::drive_state(&maindevice, station, 0x0008).await; println!(" AL after DC writes: {}", bridge::verdict(al, alc)); println!( "DC SYNC0: act={:?} cycle={:?} start=+100ms (t3={} start_delta={}ns)", act.as_ref().map(|v| format!("0x{:02X}", v)).map_err(|e| format!("{:?}", e)), cyc0.as_ref().map_err(|e| format!("{:?}", e)), t3, t3.wrapping_sub(start) as i64 ); if matches!(act, Ok(a) if a & regs::DC_SYNC0_ACTIVATE != 0) && matches!(cyc0, Ok(c) if c == cycle_ns as u32) { dc_ref = Some((t3, rt::now_ns())); println!("DC SYNC0: active on EL6695 secondary"); } else { println!("DC SYNC0: readback mismatch; running without DC telemetry"); } } } // ---- 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::(&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 = 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::(&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 = 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)", Mode::Verify => "verify (CoE audit, no data loop)", Mode::Sniff => "sniff (raw frame diagnostics)", Mode::RfmBridge => "rfm-bridge (EL6695 <-> RFM <-> remote)", Mode::RfmAsync => "rfm-async (event-driven)", Mode::RfmLrw => "rfm-lrw (single-frame read+write)", Mode::RfmPing | Mode::RfmEcho | Mode::RfmRing | Mode::RevProbe | Mode::RfmLrw => unreachable!(), } ); 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(); // J1900 data verification counters let mut prev_tc_seq: u64 = 0; let mut rx_stale: u64 = 0; // tc_seq unchanged (stale read) let mut rx_nonmono: u64 = 0; // tc_seq decreased (non-monotonic) let mut comp_err: u64 = 0; // internal computation errors let mut payload_checked: u64 = 0; // J1900 payload verification cycles let mut payload_mismatch: u64 = 0; // J1900 payload mismatch bytes accumulated let mut prev_rx8 = [0u8; 8]; // spin-lock: previous payload (change detect) let mut last_fresh: Option = None; // spin-lock: last fresh-data instant // Async pipelined FPWR: two alternating leaked tx slots so the in-flight // write future can hold a 'static buffer while the response is reaped // at the start of the next cycle. let tx_slot_a = Box::leak(Box::new([0u8; PAYLOAD_LEN])); let tx_slot_b = Box::leak(Box::new([0u8; PAYLOAD_LEN])); let mut tx_slot_toggle = false; let mut pending_tx: Option< std::pin::Pin> + '_>>, > = None; let mut tx_late: u64 = 0; let mut aborted: Option = None; // Stall segment tracing: per-stage timestamps, dumped when a cycle // exceeds STALL_TRACE_NS so we can localize which communication // segment (reap/rx/rfm/tx/el/wait) the maximum delay lives in. const STALL_TRACE_NS: i64 = 2_000_000; let mut prev_cycle_t0: Option = None; let mut prev_tc_seq_all: u64 = 0; // RFM bridge context (only opened in rfm-bridge / rfm-async modes). let mut rfm_ctx: Option = if matches!(cfg.mode, Mode::RfmBridge | Mode::RfmAsync | Mode::RfmLrw) { let r = el6695_rt::rfm::Rfm::open_path(cfg.rfm_dev.as_deref())?; println!( "rfm-bridge: BAR2 reflective mem = {} MB, node_id={}", r.mem.len() / (1024 * 1024), r.node_id ); Some(r) } else { None }; let mut bridge_req_seq: u64 = 0; let mut bridge_ok: u64 = 0; let mut bridge_timeouts: u64 = 0; let mut bridge_rtt_st = OnlineStats::new("rfm_hop_rtt", 100, 0, 20_000_000); // ---- rfm-async: event-driven forwarder, no tick ---- if cfg.mode == Mode::RfmAsync { let rfm = rfm_ctx.take().expect("rfm not opened"); return rfm_async_loop(&maindevice, station, rfm, cfg.duration_sec, cfg.dc_discipline, &sd).await; } // ---- rfm-lrw: single-frame read+write loop ---- if cfg.mode == Mode::RfmLrw { let rfm = rfm_ctx.take().expect("rfm not opened"); return rfm_lrw_loop(&maindevice, station, rfm, cfg.duration_sec).await; } 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 mut rx_ok = true; let spin_mode = cfg.spin && cfg.mode == Mode::Ticker; let mut jitter_ns: i64 = 0; if spin_mode { // ---- Spin-lock: poll FPRD until the payload changes ---- // Phase-locks processing to TwinCAT's frame arrival instead of a // free-running ticker. Timeout at 1.5 periods keeps the loop // alive (counted as stale) if the primary master stops. let spin_t0 = Instant::now(); let timeout_ns = cycle_ns + cycle_ns / 2; let mut fresh = false; loop { 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); if n > rx_prefix() { let copy_len = (n - rx_prefix()).min(SM_RX_PLD_LEN); rx_buf[..copy_len].copy_from_slice(&raw[rx_prefix()..rx_prefix() + copy_len]); } if rx_buf[..8] != prev_rx8 { prev_rx8.copy_from_slice(&rx_buf[..8]); fresh = true; break; } } Err(_) => { rx_err += 1; } } if spin_t0.elapsed().as_nanos() as u64 > timeout_ns { break; } } if !fresh { rx_ok = false; rx_stale += 1; } // jitter = deviation of fresh-data arrival interval from nominal let now = Instant::now(); if let Some(prev) = last_fresh { let dt = now.duration_since(prev).as_nanos() as i64 - cycle_ns as i64; jitter_st.push(dt); } last_fresh = Some(now); } else { jitter_ns = ticker.wait_next_period(); jitter_st.push(jitter_ns); } let t0 = Instant::now(); cyc += 1; // Reap async FPWR kicked last cycle: the spin-detect above gave the // response ~one full cycle to arrive, so this should never block. if let Some(mut f) = pending_tx.take() { match futures_lite::future::poll_once(f.as_mut()).await { Some(Ok(_)) => {} Some(Err(_)) => { tx_err += 1; } None => { tx_late += 1; let _ = f.await; } } } let t_reap = Instant::now(); if !spin_mode { // 1. Read SM3 (TwinCAT data via bridge) let t_rx0 = Instant::now(); match ethercrab::Command::fprd(station, SM_RX_ADDR) .receive_slice(&maindevice, SM_RX_LEN as u16) .await { Ok(pdu) => { let raw: &[u8] = &pdu; // Strip 2-byte diagnostic word prefix (0x1A01) from plain mode let n = raw.len().min(SM_RX_LEN); if n > rx_prefix() { let copy_len = (n - rx_prefix()).min(SM_RX_PLD_LEN); rx_buf[..copy_len].copy_from_slice(&raw[rx_prefix()..rx_prefix() + copy_len]); } } Err(_) => { rx_ok = false; rx_err += 1; } } rx_rtt_st.push(t_rx0.elapsed().as_nanos() as i64); } let t_rx = Instant::now(); if cyc <= 3 && rx_ok { let live: u32 = rx_buf[..SM_RX_PLD_LEN.min(256)].iter().map(|&b| if b != 0 { 1 } else { 0 }).sum(); let show = SM_RX_LEN.min(64); println!(" rx_buf[0..{}]: {} non-zero (of {}) at cycle {}", show, live, SM_RX_PLD_LEN.min(256), cyc); print!(" rx_buf[..{}] hex: ", show); for b in rx_buf[..show].iter() { print!("{:02X}", b); } println!(); } // 2. In ticker mode: prove J1900 processing by transforming TwinCAT's // tc_seq instead of blind echo. The function f(x)=x*7+3 is a // non-trivial mathematical transform that no hardware bridge could // produce 锟?it proves J1900 read, computed AND wrote the data. // The original tc_seq is preserved at offset 272 for TwinCAT to // verify: aIn1[0..7] == f(aOut1.tc_seq). // In probe mode: skip TX (read-only observation). // In rfm-bridge mode: forward the RX payload to the RFM mailbox and // use the remote (J1900) processed REP as the TX payload, so the // PLC-measured RTT covers the full CX->EL6695->D2000->RFM->x86 path. if cfg.mode == Mode::RfmBridge { if rx_ok { let rfm_ref = rfm_ctx.as_mut().expect("rfm not opened"); let mem_ptr = rfm_ref.mem.as_ptr() as *mut u8; let n = SM_RX_PLD_LEN.min(el6695_rt::rfm::MB_MAX_PAYLOAD); let ts = el6695_rt::rfm::now_ns(); bridge_req_seq = bridge_req_seq.wrapping_add(1); unsafe { let mb = el6695_rt::rfm::Mailbox::at(mem_ptr, el6695_rt::rfm::MB_REQ_OFF); el6695_rt::rfm::dev_write(mb.payload.as_mut_ptr(), &rx_buf[..n]); mb.len = n as u32; mb.ts_ns = ts; el6695_rt::rfm::barrier(); mb.seq = bridge_req_seq; el6695_rt::rfm::barrier(); mb.magic = el6695_rt::rfm::MB_MAGIC; } // Spin-poll REP for the remote-processed payload. Budget is // half a cycle (matches the Sep-21 baseline binary behavior; // 1.5ms broke the 1ms cadence when the peer is offline). let deadline = Instant::now() + Duration::from_nanos(cycle_ns / 2); let mut got = false; loop { unsafe { let rep = el6695_rt::rfm::Mailbox::at(mem_ptr, el6695_rt::rfm::MB_REP_OFF); el6695_rt::rfm::barrier(); if rep.magic == el6695_rt::rfm::MB_MAGIC && rep.seq == bridge_req_seq && rep.len as usize >= SM_RX_PLD_LEN.min(el6695_rt::rfm::MB_MAX_PAYLOAD) { let m = SM_RX_PLD_LEN.min(el6695_rt::rfm::MB_MAX_PAYLOAD); el6695_rt::rfm::dev_read(&mut tx_buf[..m], rep.payload.as_ptr()); // Apply f(x)=x*7+3 to the echoed frame's tc_seq // here on the D2000 side (single transform point, // robust): the remote just reflects the frame, the // PLC's echo check needs [0..8] = f(rx tc_seq). if m >= 280 { let rx_seq = u64::from_le_bytes(rx_buf[..8].try_into().unwrap_or([0; 8])); let processed = rx_seq.wrapping_mul(7).wrapping_add(3); tx_buf[..8].copy_from_slice(&processed.to_le_bytes()); } bridge_rtt_st.push(el6695_rt::rfm::now_ns().saturating_sub(rep.ts_ns) as i64); got = true; break; } } if Instant::now() > deadline { break; } core::hint::spin_loop(); } if got { bridge_ok += 1; } else { bridge_timeouts += 1; // Fall back to local echo so downstream sees valid data let echo_len = SM_RX_PLD_LEN.min(tx_buf.len()); tx_buf[..echo_len].copy_from_slice(&rx_buf[..echo_len]); } payload_checked += 1; } } if cfg.mode == Mode::Ticker { if rx_ok { if PAYLOAD_LEN == 8 { // 8B proof mode: per-byte transform tx[i] = rx[i] XOR 0x5A. // Each byte is read, transformed and written back // independently, so TwinCAT can verify all 8 bytes. for i in 0..PAYLOAD_LEN { tx_buf[i] = rx_buf[i] ^ 0x5A; } payload_checked += 1; } else { // Echo received data within available bounds let echo_len = SM_RX_PLD_LEN.min(tx_buf.len()); tx_buf[..echo_len].copy_from_slice(&rx_buf[..echo_len]); // Read TwinCAT's sequence number let tc_seq = u64::from_le_bytes( rx_buf[..8].try_into().unwrap_or([0; 8]), ); // J1900 payload verification: pattern at [16..255] = (tc_seq + i) % 256 // (only present in 1KB layout) if SM_RX_PLD_LEN >= 256 { let mut mism: u32 = 0; for i in 16..256 { let expected = ((tc_seq + i as u64) % 256) as u8; if rx_buf[i] != expected { mism += 1; } } payload_checked += 1; payload_mismatch += mism as u64; if PAYLOAD_LEN >= 280 { // Write J1900 payload mismatch count for TwinCAT (offset 268) tx_buf[268..272].copy_from_slice(&mism.to_le_bytes()); } } // Transform: f(x) = x * 7 + 3 (irreversible by simple subtraction) let processed = tc_seq.wrapping_mul(7).wrapping_add(3); tx_buf[..8].copy_from_slice(&processed.to_le_bytes()); if PAYLOAD_LEN >= 280 { // Preserve original for TwinCAT verification (offset 272) tx_buf[272..280].copy_from_slice(&tc_seq.to_le_bytes()); } } } } let t_rfm = Instant::now(); // TX (FPWR) runs for ticker AND rfm-bridge (J1900->CX path needs the // REP payload written to SM2). Probe mode stays read-only. if cfg.mode != Mode::Probe { if PAYLOAD_LEN >= 16 { tx_buf[8..16].copy_from_slice(&cyc.to_le_bytes()); } if PAYLOAD_LEN >= 280 { tx_buf[264..268].copy_from_slice(&(jitter_ns as i32).to_le_bytes()); } if spin_mode { // Async pipelined FPWR: kick the frame onto the wire now // (poll_once drives it to sendable), reap response next // cycle. Frees ~240us of blocking write time per cycle. let p = if tx_slot_toggle { tx_slot_b.as_mut_ptr() } else { tx_slot_a.as_mut_ptr() }; tx_slot_toggle = !tx_slot_toggle; let slot: &'static mut [u8] = unsafe { std::slice::from_raw_parts_mut(p, PAYLOAD_LEN) }; slot.copy_from_slice(&tx_buf[..PAYLOAD_LEN]); let mut fut = Box::pin( ethercrab::Command::fpwr(station, SM_TX_ADDR).send(&maindevice, &slot[..]), ); // Kick: drive the write future for up to ~20us so the frame // is marked sendable and the TX thread puts it on the wire // NOW. poll_once alone does not reach sendable state. let t_kick = Instant::now(); let _ = futures_lite::future::or( async { fut.as_mut().await.is_ok() }, async { while t_kick.elapsed().as_micros() < 20 { futures_lite::future::yield_now().await; } false }, ) .await; pending_tx = Some(fut); } else { 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; } } } let t_tx = Instant::now(); // 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, } } } let t_el = Instant::now(); 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; } // J1900 data verification if rx_ok && cfg.mode == Mode::Ticker { if tc_seq == prev_tc_seq { rx_stale += 1; } if prev_tc_seq != 0 && tc_seq != 0 { let delta = (tc_seq as i64).wrapping_sub(prev_tc_seq as i64); if delta < 0 { rx_nonmono += 1; } } prev_tc_seq = tc_seq; // Self-consistency: verify proc == tc_seq * 7 + 3 // (always true for 1KB ticker mode; 8B mode uses XOR echo instead) if PAYLOAD_LEN != 8 { let computed = tc_seq.wrapping_mul(7).wrapping_add(3); if cfg.mode == Mode::Ticker && tx_buf[..8] != computed.to_le_bytes() { comp_err += 1; // internal computation error (should never fire) } } } let t_end = Instant::now(); let exec_ns = t0.elapsed().as_nanos() as i64; exec_st.push(exec_ns); // Stall localization: period (t0-to-t0, includes ticker wait) and // per-segment durations. tc_delta tells whether the CX PLC kept // producing during the stall (delta ~= stall_ms -> secondary side; // delta ~= 1 -> CX side stalled too). let period_ns = prev_cycle_t0.map(|p| t0.duration_since(p).as_nanos() as i64); prev_cycle_t0 = Some(t0); let tc_delta = tc_seq.wrapping_sub(prev_tc_seq_all); prev_tc_seq_all = tc_seq; let us = |a: Instant, b: Instant| b.duration_since(a).as_nanos() as i64 / 1000; let seg_reap = us(t0, t_reap); let seg_rx = us(t_reap, t_rx); let seg_rfm = us(t_rx, t_rfm); let seg_tx = us(t_rfm, t_tx); let seg_el = us(t_tx, t_el); let seg_tail = us(t_el, t_end); if exec_ns > STALL_TRACE_NS || period_ns.map_or(false, |p| p > cycle_ns as i64 + STALL_TRACE_NS) { println!( "STALL cyc={} tms={} jitter={}us reap={}us rx={}us rfm={}us tx={}us el={}us tail={}us exec={}us period={}us tc_delta={} rx_ok={}", cyc, start.elapsed().as_millis(), jitter_ns / 1000, seg_reap, seg_rx, seg_rfm, seg_tx, seg_el, seg_tail, exec_ns / 1000, period_ns.unwrap_or(0) / 1000, tc_delta, rx_ok, ); } 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::(&maindevice) .await { el1252_in = b; } } let proc_val = tc_seq.wrapping_mul(7).wrapping_add(3); let stale_pct = if cyc > 0 { (rx_stale * 100 / cyc) } else { 0 }; let mut dc_info = String::new(); if let Some((dc0, mono0)) = dc_ref { if let Ok(dc_now) = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME) .receive::(&maindevice) .await { let mono_now = rt::now_ns(); let drift = (dc_now.wrapping_sub(dc0)) as i64 - (mono_now - mono0) as i64; dc_info = format!( " dcph={} dcdr={}ns", dc_now % cycle_ns, drift ); } } println!( "S tms={} cyc={} raw={} proc={} txe={} rxe={} late={} stale={}% mono={} cerr={} mism={} e0={} e1={}{}", start.elapsed().as_millis(), cyc, tc_seq, proc_val, tx_err, rx_err, tx_late, stale_pct, rx_nonmono, comp_err, payload_mismatch, pos_edges, pos1_edges, dc_info, ); if cfg.mode == Mode::RfmBridge { println!( " bridge: ok={} timeouts={} rfm_hop_avg={}us", bridge_ok, bridge_timeouts, if bridge_ok > 0 { bridge_rtt_st.mean() / 1000.0 } else { 0.0 }, ); } last_report = Instant::now(); } if aborted.is_some() { break; } } // Reap any in-flight async FPWR before reporting if let Some(f) = pending_tx.take() { let _ = f.await; } // ---- DC post-run: re-read firmware sync counters accumulated during the run ---- if cfg.dc { println!("\n=== DC post-run sync counters (0x1C32 SM2 / 0x1C33 SM3) ==="); for idx in [0x1C32u16, 0x1C33] { let mode = sd.sdo_read::(idx, 0x01).await.unwrap_or(0xFFFF); let missed = sd.sdo_read::(idx, 0x0B).await.unwrap_or(0xFFFF); let exceeded = sd.sdo_read::(idx, 0x0C).await.unwrap_or(0xFFFF); let serr = sd.sdo_read::(idx, 0x20).await.unwrap_or(0xFF); println!( " 0x{:04X}: sync_mode={} sm_event_missed={} cycle_exceeded={} sync_error={}", idx, mode, missed, exceeded, serr ); } } // ---- 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 cfg.mode == Mode::RfmBridge { bridge_rtt_st.report(1000, "us"); println!( "rfm-bridge: ok={} timeouts={} (rep_miss_rate={:.2}%)", bridge_ok, bridge_timeouts, if bridge_ok + bridge_timeouts > 0 { bridge_timeouts as f64 * 100.0 / (bridge_ok + bridge_timeouts) as f64 } else { 0.0 } ); } 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!("=== J1900 data verification ==="); println!(" tc_seq: stale={} ({}%) nonmono={} comp_err={}", rx_stale, if cyc > 0 { rx_stale * 100 / cyc } else { 0 }, rx_nonmono, comp_err); println!(" payload: checked={} mismatch={}", payload_checked, payload_mismatch); 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(()) } // ===================================================================== // RFM (PCI-5565 reflective memory) modes // ===================================================================== /// RFM ping/echo modes: pure reflective-memory latency, no EtherCAT. async fn rfm_mode_main(cfg: &Config) -> Result<(), Box> { let rfm = el6695_rt::rfm::Rfm::open_path(cfg.rfm_dev.as_deref())?; println!( "rfm: {} BAR reflective mem = {} MB, node_id={}", cfg.rfm_dev.as_deref().unwrap_or("/dev/rfm2g0(default)"), rfm.mem.len() / (1024 * 1024), rfm.node_id ); let mem_ptr = rfm.mem.as_ptr() as *mut u8; // RT-rotate this thread onto its own core (both ping and echo spin; // sharing a core with the peer would add ~0.5ms scheduling latency). if let Err(e) = rt::promote_current_thread(cfg.core, cfg.prio, true) { eprintln!("RT promotion failed (continuing): {}", e); } // Clear both mailboxes so a stale REQ/REP from a previous run cannot // poison the first cycle's RTT sample. unsafe { let zero = [0u8; 32]; el6695_rt::rfm::dev_write(mem_ptr.add(el6695_rt::rfm::MB_REQ_OFF), &zero); el6695_rt::rfm::dev_write(mem_ptr.add(el6695_rt::rfm::MB_REP_OFF), &zero); el6695_rt::rfm::barrier(); } let mut rtt = OnlineStats::new("rfm_rtt", 100, 0, 5_000_000); let mut period = OnlineStats::new("period", 100, 0, 10_000_000); let cycle_ns = cfg.cycle_us * 1000; let mut seq: u64 = 0; let start = Instant::now(); let mut last_rep_seq: u64 = 0; let mut timeouts: u64 = 0; let mut dseq_sum: u64 = 0; let mut last_report = Instant::now(); let mut last_cyc: u64 = 0; match cfg.mode { Mode::RfmPing => { let mut next = Instant::now(); while start.elapsed().as_secs() < cfg.duration_sec || cfg.duration_sec == 0 { if STOP.load(Ordering::Relaxed) { break; } // ---- write REQ ---- let ts = el6695_rt::rfm::now_ns(); unsafe { let mb = el6695_rt::rfm::Mailbox::at(mem_ptr, el6695_rt::rfm::MB_REQ_OFF); let req_seq = seq.wrapping_add(1); let pattern = [(seq & 0xFF) as u8; 64]; el6695_rt::rfm::dev_write(mb.payload.as_mut_ptr(), &pattern); mb.len = 64; mb.ts_ns = ts; el6695_rt::rfm::barrier(); mb.seq = req_seq; el6695_rt::rfm::barrier(); mb.magic = el6695_rt::rfm::MB_MAGIC; } // ---- spin-wait REP ---- let deadline = Instant::now() + Duration::from_nanos(cycle_ns / 2); let mut got = false; loop { unsafe { let rep = el6695_rt::rfm::Mailbox::at(mem_ptr, el6695_rt::rfm::MB_REP_OFF); el6695_rt::rfm::barrier(); if rep.magic == el6695_rt::rfm::MB_MAGIC && rep.seq != last_rep_seq { dseq_sum += seq.wrapping_add(1).saturating_sub(rep.seq); last_rep_seq = rep.seq; let rtt_ns = el6695_rt::rfm::now_ns().saturating_sub(rep.ts_ns); rtt.push(rtt_ns as i64); got = true; break; } } if Instant::now() > deadline { break; } core::hint::spin_loop(); } if !got { timeouts += 1; } seq = seq.wrapping_add(1); period.push(next.elapsed().as_nanos() as i64); next += Duration::from_nanos(cycle_ns); let now = Instant::now(); if next > now { spin_sleep(next - now); } else { next = now; } if last_report.elapsed().as_secs() >= 1 { println!( "S tms={} cyc={} rtt_n={} timeouts={}", start.elapsed().as_millis(), seq, rtt_n(&rtt), timeouts ); last_cyc = seq; last_report = Instant::now(); } } } Mode::RfmEcho => { println!("rfm: echo mode (reflect REQ -> REP), ctrl-C to stop"); let mut last_req_seq: u64 = 0; while !STOP.load(Ordering::Relaxed) { unsafe { let req = el6695_rt::rfm::Mailbox::at(mem_ptr, el6695_rt::rfm::MB_REQ_OFF); el6695_rt::rfm::barrier(); if req.magic == el6695_rt::rfm::MB_MAGIC && req.seq != last_req_seq { let n = (req.len as usize).min(el6695_rt::rfm::MB_MAX_PAYLOAD); let cur_seq = req.seq; let cur_ts = req.ts_ns; let cur_len = req.len; let mut buf = [0u8; el6695_rt::rfm::MB_MAX_PAYLOAD]; el6695_rt::rfm::dev_read(&mut buf[..n], req.payload.as_ptr()); let rep = el6695_rt::rfm::Mailbox::at(mem_ptr, el6695_rt::rfm::MB_REP_OFF); el6695_rt::rfm::dev_write(rep.payload.as_mut_ptr(), &buf[..n]); // Echo is a dumb reflector: f(x) is applied by the // D2000 bridge (single transform point, avoids double // transform). rep.len = cur_len; rep.ts_ns = cur_ts; el6695_rt::rfm::barrier(); rep.seq = cur_seq; el6695_rt::rfm::barrier(); rep.magic = el6695_rt::rfm::MB_MAGIC; last_req_seq = cur_seq; seq = seq.wrapping_add(1); } } core::hint::spin_loop(); if last_report.elapsed().as_secs() >= 1 { println!("S tms={} echoed={}", start.elapsed().as_millis(), seq); last_report = Instant::now(); } } } _ => unreachable!(), } println!("\n=== rfm done: {} ops in {:.1}s, timeouts={} ===", seq, start.elapsed().as_secs_f64(), timeouts); print_stats(&rtt); print_stats(&period); let _ = last_cyc; Ok(()) } fn rtt_n(s: &OnlineStats) -> u64 { s.len() } #[inline] fn spin_sleep(d: Duration) { let target = Instant::now() + d; while Instant::now() < target { if d > Duration::from_micros(200) { std::thread::yield_now(); } else { core::hint::spin_loop(); } } } fn print_stats(s: &OnlineStats) { s.report(1000, "us"); } // ---- rfm-async: event-driven bidirectional forwarder ---- // // No tick. The FPRD (SM3 read) stays in flight continuously; between PDU // polls the RFM REP mailbox is spin-polled over local MMIO (~us detection). // Fresh CX data is written to the RFM REQ mailbox immediately after each // FPRD completes; fresh REP data is FPWR'd to SM2 immediately on detection // (async pipelined, latest-wins if the wire is busy). /// rfm-lrw: one LRW frame per cycle does SM2 write + SM3 read. /// The FMMU maps logical [0..PAYLOAD]=SM2(write) and [PAYLOAD..2P]=SM3(read), /// so a single frame RTT covers both directions. No concurrent PDU futures. async fn rfm_lrw_loop( maindevice: ðercrab::MainDevice<'_>, station: u16, mut rfm: el6695_rt::rfm::Rfm, duration_sec: u64, ) -> Result<(), Box> { use el6695_rt::rfm::*; println!("rfm-lrw: single-frame read+write per cycle (no tick, Ctrl-C to stop)"); { let f0 = ethercrab::Command::fprd(station, 0x0600u16).receive_slice(maindevice, 16).await; let f1 = ethercrab::Command::fprd(station, 0x0610u16).receive_slice(maindevice, 16).await; match (&f0, &f1) { (Ok(p0), Ok(p1)) => { let r0: &[u8] = p0; let r1: &[u8] = p1; let len0 = u16::from_le_bytes([r0[4], r0[5]]); let len1 = u16::from_le_bytes([r1[4], r1[5]]); let en0 = r0[11]; let en1 = r1[11]; println!(" FMMU0: len={} enable={} | FMMU1: len={} enable={}", len0, en0, len1, en1); } _ => println!(" FMMU readback failed"), } let f0f = ethercrab::Command::fprd(station, 0x0600u16).receive_slice(maindevice, 16).await; let f1f = ethercrab::Command::fprd(station, 0x0610u16).receive_slice(maindevice, 16).await; if let Ok(p) = &f0f { let r: &[u8] = p; println!(" FMMU0 raw: {:02X?}", &r[..16]); } if let Ok(p) = &f1f { let r: &[u8] = p; println!(" FMMU1 raw: {:02X?}", &r[..16]); } } let mem_ptr = rfm.mem.as_mut_ptr(); unsafe { let zero = [0u8; 32]; dev_write(mem_ptr.add(MB_REQ_OFF), &zero); dev_write(mem_ptr.add(MB_REP_OFF), &zero); barrier(); } let start = Instant::now(); let mut prev_rx8 = [0u8; 8]; let mut last_rep_seq: u64 = 0; let mut req_seq: u64 = 0; let mut tx_latest = vec![0u8; PAYLOAD_LEN]; let mut frame = vec![0u8; 1344usize]; let (mut n_iter, mut n_fresh, mut n_req, mut n_rep, mut n_err) = (0u64, 0u64, 0u64, 0u64, 0u64); let (mut lrw_sum, mut hop_sum, mut cx2j_sum) = (0u64, 0u64, 0u64); let mut lrw_st = OnlineStats::new("lrw_rtt", 100, 0, 20_000_000); let mut hop_st = OnlineStats::new("rfm_hop_plus_echo", 100, 0, 20_000_000); let mut cx2j_st = OnlineStats::new("fresh_to_req", 100, 0, 20_000_000); let mut last_report = Instant::now(); while !STOP.load(Ordering::Relaxed) { if duration_sec > 0 && start.elapsed().as_secs() >= duration_sec { break; } n_iter += 1; let pace_t0 = Instant::now(); // 1. poll REP mailbox -> update tx_latest (apply the single-point transform) let (magic, rseq, rts) = unsafe { let rep = Mailbox::at(mem_ptr, MB_REP_OFF); barrier(); (rep.magic, rep.seq, rep.ts_ns) }; if magic == MB_MAGIC && rseq != last_rep_seq { last_rep_seq = rseq; let n = unsafe { let rep = Mailbox::at(mem_ptr, MB_REP_OFF); (rep.len as usize).min(PAYLOAD_LEN).min(MB_MAX_PAYLOAD) }; unsafe { let rep = Mailbox::at(mem_ptr, MB_REP_OFF); dev_read(&mut tx_latest[..n], rep.payload.as_ptr()); } if n >= 8 { let rx_seq = u64::from_le_bytes(tx_latest[..8].try_into().unwrap()); let processed = rx_seq.wrapping_mul(7).wrapping_add(3); tx_latest[..8].copy_from_slice(&processed.to_le_bytes()); } n_rep += 1; hop_sum += now_ns().saturating_sub(rts); hop_st.push(now_ns().saturating_sub(rts) as i64); } // 2. single LRW: [0..P] write SM2 (latest REP), [P..2P] read SM3 (fresh CX data) frame[..672].copy_from_slice(&tx_latest[..672]); let t0 = Instant::now(); match ethercrab::Command::lrw(0) .ignore_wkc() .send_receive_slice(maindevice, &frame[..]) .await { Ok(pdu) => { let raw: &[u8] = &pdu; let off = 672usize; lrw_st.push((t0.elapsed().as_micros() * 1000) as i64); lrw_sum += t0.elapsed().as_micros() as u64; if raw.len() >= off + 672 { let cur: [u8; 8] = raw[off..off + 8].try_into().unwrap(); if cur != prev_rx8 { prev_rx8 = cur; n_fresh += 1; let t1 = Instant::now(); let n = 672usize.min(MB_MAX_PAYLOAD); req_seq = req_seq.wrapping_add(1); unsafe { let mb = Mailbox::at(mem_ptr, MB_REQ_OFF); dev_write(mb.payload.as_mut_ptr(), &raw[off..off + n]); mb.len = n as u32; mb.ts_ns = now_ns(); barrier(); mb.seq = req_seq; barrier(); mb.magic = MB_MAGIC; } n_req += 1; cx2j_sum += t1.elapsed().as_nanos() as u64; cx2j_st.push(t1.elapsed().as_nanos() as i64); } } } Err(e) => { n_err += 1; if n_err <= 8 { println!(" LRW ERR n={}: {:?}", n_err, e); } } } if n_iter % 2000 == 0 { println!(" TXDBG tx[0..16]={:02X?}", &tx_latest[..16]); } if last_report.elapsed() >= Duration::from_secs(1) { last_report = Instant::now(); println!( "S tms={} iter={} fresh={} req={} rep={} err={} lrw_avg={}us hop_avg={}us cx2j_avg={}us", start.elapsed().as_millis(), n_iter, n_fresh, n_req, n_rep, n_err, if n_iter > 0 { lrw_sum / n_iter } else { 0 }, if n_rep > 0 { hop_sum / n_rep / 1000 } else { 0 }, if n_req > 0 { cx2j_sum / n_req / 1000 } else { 0 }, ); } } println!("\n=== rfm-lrw done: {:.1}s ===", start.elapsed().as_secs_f64()); println!("iter={} fresh={} req={} rep={} err={}", n_iter, n_fresh, n_req, n_rep, n_err); print_stats(&lrw_st); print_stats(&hop_st); print_stats(&cx2j_st); Ok(()) } async fn rfm_async_loop( maindevice: ðercrab::MainDevice<'_>, station: u16, mut rfm: el6695_rt::rfm::Rfm, duration_sec: u64, dc_discipline: bool, sd: ðercrab::SubDeviceRef<'_, ðercrab::SubDevice>, ) -> Result<(), Box> { use el6695_rt::rfm::*; println!("rfm-async: event-driven forwarding, no tick (Ctrl-C to stop)"); let mem_ptr = rfm.mem.as_mut_ptr(); // Clear both mailboxes so a stale REP from a previous run cannot be // mistaken for a fresh echo (its old ts would poison hop stats). unsafe { let zero = [0u8; 32]; dev_write(mem_ptr.add(MB_REQ_OFF), &zero); dev_write(mem_ptr.add(MB_REP_OFF), &zero); barrier(); } let start = Instant::now(); let mut rx_buf = vec![0u8; SM_RX_LEN]; let mut prev_rx8 = [0u8; 8]; let mut last_rep_seq: u64 = 0; let mut req_seq: u64 = 0; let mut n_fprd: u64 = 0; let mut n_fresh: u64 = 0; let mut n_req: u64 = 0; let mut n_rep: u64 = 0; let mut n_fpwr: u64 = 0; let mut n_fpwr_err: u64 = 0; let mut n_fprd_err: u64 = 0; let mut n_ovf: u64 = 0; // REP arrived while a FPWR was still on the wire let mut hop_st = OnlineStats::new("rfm_hop_plus_echo", 100, 0, 20_000_000); // REQ ts -> REP detect let mut j2cx_st = OnlineStats::new("j2cx_detect_to_kick", 100, 0, 20_000_000); // REP detect -> FPWR kicked let mut cx2j_st = OnlineStats::new("cx2j_fprd_to_req", 100, 0, 20_000_000); // FPRD done -> REQ written let mut fprd_st = OnlineStats::new("fprd_period", 100, 0, 20_000_000); type FprdFut<'a> = std::pin::Pin< Box, ethercrab::error::Error>> + 'a>, >; type FpwrFut<'a> = std::pin::Pin< Box> + 'a>, >; let mut pending_fprd: Option> = None; let mut pending_fpwr: Option> = None; let tx_slot_a = Box::leak(Box::new([0u8; PAYLOAD_LEN])); let tx_slot_b = Box::leak(Box::new([0u8; PAYLOAD_LEN])); let mut tx_slot_toggle = false; let mut tx_latest = vec![0u8; PAYLOAD_LEN]; let mut tx_latest_seq: u64 = 0; let mut tx_inflight_seq: u64 = 0; let mut last_fprd_done: Option = None; let mut last_rep_detect: Option = None; let mut last_iter: Option = None; let mut last_report = Instant::now(); let mut seg_kick_us: u64 = 0; let mut seg_rep_us: u64 = 0; let mut seg_fprd_us: u64 = 0; let mut seg_fpwr_us: u64 = 0; let mut seg_rpt_us: u64 = 0; // IgH-style continuous DC discipline (manual 6.7 direct coupling) let mut dc_prev: Option<(u64, u64)> = None; // (esc_time_ns, local_ns) let mut dc_first: Option<(u64, u64)> = None; let mut dc_off_applied: u64 = 0; let mut dc_last_tick: Option = None; let mut dc_err_ns: i64 = 0; let mut dc_drift_ppm: f64 = 0.0; let mut phase_st = OnlineStats::new("fresh_phase_us", 200, -1_500_000, 1_500_000); let mut phase_est: i64 = 0; // estimated TwinCAT frame boundary phase (ns within 1ms) let mut wphase_st = OnlineStats::new("write_phase_us", 200, -1_500_000, 1_500_000); let mut j2cx_pending: Option = None; let mut last_f640_print: u64 = 0; let mut rep_new = false; while !STOP.load(Ordering::Relaxed) { if duration_sec > 0 && start.elapsed().as_secs() >= duration_sec { break; } // DC discipline tick (every ~10ms): align ESC time to local clock if dc_discipline { let tick = dc_last_tick.map(|t| t.elapsed().as_millis() as u64).unwrap_or(999); if tick >= 10 { dc_last_tick = Some(std::time::Instant::now()); if let Ok(esc) = ethercrab::Command::fprd(station, 0x0910u16) .receive::(&maindevice).await { let loc = rt::now_ns(); if let Some((e0, l0)) = dc_prev { let dt_loc = loc.wrapping_sub(l0) as f64; let dt_esc = esc.wrapping_sub(e0) as f64; if dt_loc > 1_000_000.0 { dc_drift_ppm = ((dt_loc - dt_esc) / dt_loc) * 1e6; } } let err = loc.wrapping_sub(esc) as i64; dc_err_ns = err; // measurement only: do NOT write offsets (naive actuation wrapped clock) if dc_prev.is_none() { let _ = ethercrab::Command::fpwr(station, 0x0920u16) .send(&maindevice, 0u64).await; // reset offset once to raw } if dc_first.is_none() { dc_first = Some((esc, loc)); } dc_prev = Some((esc, loc)); } } } let iter_t0 = Instant::now(); if let Some(p) = last_iter { let gap = iter_t0.duration_since(p).as_micros() as u64; if gap > 2000 { // 璇?/proc/self 鐨勬姠鍗犺鏁? 鍒ゆ柇鏄璋冨害璧拌繕鏄鍐呴樆濉? let nvc = std::fs::read_to_string("/proc/self/status") .ok() .and_then(|s| { s.lines() .find(|l| l.starts_with("nonvoluntary")) .and_then(|l| l.split_whitespace().last().map(String::from)) }) .unwrap_or_default(); println!( "STALL-ITER tms={} gap={}us sections[kick={}us rep={}us fprd={}us fpwr={}us rpt={}us] nvc={}", start.elapsed().as_millis(), gap, seg_kick_us, seg_rep_us, seg_fprd_us, seg_fpwr_us, seg_rpt_us, nvc ); } } last_iter = Some(iter_t0); let mut cursor = iter_t0; // 1. keep one FPRD in flight (rate-limited: EL6695 secondary firmware // stalls ~50ms/1s when polled at >2kHz; cap at ~1.4kHz) if pending_fprd.is_none() && last_fprd_done.map_or(true, |p| p.elapsed().as_micros() >= 350) { let fut = async move { ethercrab::Command::fprd(station, SM_RX_ADDR) .receive_slice(maindevice, SM_RX_LEN as u16) .await .map(|p| { let mut v = Vec::with_capacity(SM_RX_LEN); v.extend_from_slice(&p); v }) }; let mut f: FprdFut<'_> = Box::pin(fut); // drive ~10us so the frame goes onto the wire now let t_kick = Instant::now(); let _ = futures_lite::future::or( async { f.as_mut().await.is_ok() }, async { while t_kick.elapsed().as_micros() < 10 { futures_lite::future::yield_now().await; } false }, ) .await; // if it (improbably) completed during the kick, reap immediately match futures_lite::future::poll_once(f.as_mut()).await { Some(res) => { n_fprd += 1; match res { Ok(raw) => { let nn = raw.len().min(SM_RX_LEN); let cl = (nn - rx_prefix()).min(SM_RX_PLD_LEN); rx_buf[..cl].copy_from_slice(&raw[rx_prefix()..rx_prefix() + cl]); } Err(_) => n_fprd_err += 1, } } None => pending_fprd = Some(f), } } { let n = Instant::now(); seg_kick_us = n.duration_since(cursor).as_micros() as u64; cursor = n; } // 2. spin-poll RFM REP mailbox (cheap: header only unless changed) let (magic, rseq, rts) = unsafe { let rep = Mailbox::at(mem_ptr, MB_REP_OFF); barrier(); (rep.magic, rep.seq, rep.ts_ns) }; if magic == MB_MAGIC && rseq != last_rep_seq { let t_detect = Instant::now(); if let Some(p) = last_rep_detect { let gap = t_detect.duration_since(p).as_micros() as u64; if gap > 5000 { println!( "STALL-REP tms={} gap={}us (req flowing: {} pending_fpwr={})", start.elapsed().as_millis(), gap, n_req, pending_fpwr.is_some() ); } } last_rep_detect = Some(t_detect); last_rep_seq = rseq; let n = unsafe { let rep = Mailbox::at(mem_ptr, MB_REP_OFF); (rep.len as usize).min(PAYLOAD_LEN).min(MB_MAX_PAYLOAD) }; unsafe { let rep = Mailbox::at(mem_ptr, MB_REP_OFF); dev_read(&mut tx_latest[..n], rep.payload.as_ptr()); } // single transform point: f(x) = x*7+3 on the echoed tc_seq if n >= 8 { let rx_seq = u64::from_le_bytes(tx_latest[..8].try_into().unwrap()); let processed = rx_seq.wrapping_mul(7).wrapping_add(3); tx_latest[..8].copy_from_slice(&processed.to_le_bytes()); } tx_latest_seq = rseq; n_rep += 1; hop_st.push(now_ns().saturating_sub(rts) as i64); rep_new = true; // defer the write to the wire-free point after fprd completes } { let n = Instant::now(); seg_rep_us = n.duration_since(cursor).as_micros() as u64; cursor = n; } // 4. poll in-flight FPRD if let Some(f) = pending_fprd.as_mut() { if let Some(res) = futures_lite::future::poll_once(f.as_mut()).await { pending_fprd = None; n_fprd += 1; let now = Instant::now(); if let Some(p) = last_fprd_done { let gap_us = now.duration_since(p).as_micros() as u64; fprd_st.push((gap_us * 1000) as i64); if gap_us > 5000 { println!( "STALL-FPRD tms={} gap={}us fresh={} (bus-side freeze?)", start.elapsed().as_millis(), gap_us, n_fresh ); } } last_fprd_done = Some(now); match res { Ok(raw) => { let nn = raw.len().min(SM_RX_LEN); let cl = (nn - rx_prefix()).min(SM_RX_PLD_LEN); rx_buf[..cl].copy_from_slice(&raw[rx_prefix()..rx_prefix() + cl]); if rx_buf[..8] != prev_rx8 { prev_rx8.copy_from_slice(&rx_buf[..8]); n_fresh += 1; { let p = (rt::now_ns() % 1_000_000) as i64; let mut adj = p; if phase_est != 0 { let d = p - phase_est; if d > 500_000 { adj = p - 1_000_000; } else if d < -500_000 { adj = p + 1_000_000; } phase_est = phase_est.min(adj); } else { phase_est = p; } phase_est += 2; // 2ns creep per fresh event (~2us/s drift compensation) phase_st.push((p / 1000) as i64); } // 5. forward fresh CX data to RFM REQ immediately let t0 = Instant::now(); let n = SM_RX_PLD_LEN.min(MB_MAX_PAYLOAD); let ts = now_ns(); req_seq = req_seq.wrapping_add(1); unsafe { let mb = Mailbox::at(mem_ptr, MB_REQ_OFF); dev_write(mb.payload.as_mut_ptr(), &rx_buf[..n]); mb.len = n as u32; mb.ts_ns = ts; barrier(); mb.seq = req_seq; barrier(); mb.magic = MB_MAGIC; } n_req += 1; cx2j_st.push(t0.elapsed().as_nanos() as i64); } } Err(_) => n_fprd_err += 1, } } } { let n = Instant::now(); seg_fprd_us = n.duration_since(cursor).as_micros() as u64; cursor = n; } // F640 remote-write-cycle counter (independent of reap path) if n_fpwr > 0 && n_fpwr % 512 == 0 && last_f640_print != n_fpwr { last_f640_print = n_fpwr; let c = sd.sdo_read::(0xF640, 1).await; println!(" F640CNT n={} count={:?}", n_fpwr, c); } // 5.5 serial write when wire is free (fprd completed this iteration) if rep_new && pending_fprd.is_none() { rep_new = false; let p = if tx_slot_toggle { tx_slot_b.as_mut_ptr() } else { tx_slot_a.as_mut_ptr() }; tx_slot_toggle = !tx_slot_toggle; let slot: &'static mut [u8] = unsafe { std::slice::from_raw_parts_mut(p, PAYLOAD_LEN) }; slot.copy_from_slice(&tx_latest[..PAYLOAD_LEN]); tx_inflight_seq = tx_latest_seq; let t_w = Instant::now(); let tx_ok = ethercrab::Command::fpwr(station, SM_TX_ADDR) .send(maindevice, &slot[..PAYLOAD_LEN]) .await .is_ok(); n_fpwr += 1; if tx_ok { j2cx_st.push(t_w.elapsed().as_nanos() as i64); } else { n_fpwr_err += 1; } } // 6. reap FPWR completion if let Some(f) = pending_fpwr.as_mut() { if let Some(res) = futures_lite::future::poll_once(f.as_mut()).await { pending_fpwr = None; if res.is_err() { n_fpwr_err += 1; } if let Some(td) = j2cx_pending.take() { j2cx_st.push(td.elapsed().as_nanos() as i64); } } } { let n = Instant::now(); seg_fpwr_us = n.duration_since(cursor).as_micros() as u64; cursor = n; } core::hint::spin_loop(); if last_report.elapsed() >= Duration::from_secs(1) { // SM 鎺㈤拡: 姣?绉掕 SM2/SM3 浣胯兘浣?+ SM2 缂撳啿, 鍖哄垎鍒嗛厤鏈敓鏁?vs 妗ヤ笉杞彂 if start.elapsed().as_secs() % 5 == 0 { let sm2c = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_CTRL) .receive::(maindevice).await.unwrap_or(0xFF); let sm2e = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_ENABLE) .receive::(maindevice).await.unwrap_or(0xFF); let sm3c = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_CTRL) .receive::(maindevice).await.unwrap_or(0xFF); let sm3e = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_ENABLE) .receive::(maindevice).await.unwrap_or(0xFF); let sm2buf = ethercrab::Command::fprd(station, SM_TX_ADDR) .receive_slice(maindevice, 8).await .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }) .unwrap_or([0u8; 8]); let sm2start = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_START) .receive::(maindevice).await.unwrap_or(0xFFFF); let sm3start = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_START) .receive::(maindevice).await.unwrap_or(0xFFFF); // 鍚屾鍐欒鍥炴祴璇? 榄旀暟鍐欏叆 SM2, 绔嬪嵆璇诲洖 let magic: [u8; 8] = [0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0x11, 0x22, 0x33]; let wres = ethercrab::Command::fpwr(station, SM_TX_ADDR) .send(maindevice, &magic).await; let rb = ethercrab::Command::fprd(station, SM_TX_ADDR) .receive_slice(maindevice, 8).await .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }) .unwrap_or([0xFFu8; 8]); println!( "SMDBG sm2ctrl={:02X} sm2ena={:02X} sm3ctrl={:02X} sm3ena={:02X} sm2start={:04X} sm3start={:04X} sm2buf={:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X} magic_rb={:02X}{:02X}{:02X}{:02X} wok={} tx_latest0={:02X}", sm2c, sm2e, sm3c, sm3e, sm2start, sm3start, sm2buf[0], sm2buf[1], sm2buf[2], sm2buf[3], sm2buf[4], sm2buf[5], sm2buf[6], sm2buf[7], rb[0], rb[1], rb[2], rb[3], wres.is_ok(), tx_latest[0] ); } println!( "S tms={} fprd={} fresh={} req={} rep={} fpwr={} ferr={} ovf={} hop_avg={}us j2cx_avg={}us cx2j_avg={}us", start.elapsed().as_millis(), n_fprd, n_fresh, n_req, n_rep, n_fpwr, n_fpwr_err, n_ovf, if n_rep > 0 { hop_st.mean() / 1000.0 } else { 0.0 }, if n_rep > 0 { j2cx_st.mean() / 1000.0 } else { 0.0 }, if n_fresh > 0 { cx2j_st.mean() / 1000.0 } else { 0.0 }, ); last_report = Instant::now(); } { let n = Instant::now(); seg_rpt_us = n.duration_since(cursor).as_micros() as u64; cursor = n; } let _ = cursor; } // drain if let Some(f) = pending_fprd.take() { let _ = f.await; } if let Some(f) = pending_fpwr.take() { let _ = f.await; } println!("\n=== rfm-async done: {:.1}s ===", start.elapsed().as_secs_f64()); print_stats(&phase_st); print_stats(&wphase_st); if let (Some((e0, l0)), Some((e1, l1))) = (dc_first, dc_prev) { let ratio_ppm = ((e1.wrapping_sub(e0) as f64) / (l1.wrapping_sub(l0) as f64 - 1.0) - 1.0) * 1e6; println!(" ESC-vs-local rate ratio: {:+.2} ppm (esc_free_running if nonzero)", ratio_ppm); } println!( "fprd={} (err={}) fresh={} req={} rep={} fpwr={} (err={}) overflow={}", n_fprd, n_fprd_err, n_fresh, n_req, n_rep, n_fpwr, n_fpwr_err, n_ovf ); print_stats(&fprd_st); print_stats(&hop_st); print_stats(&j2cx_st); print_stats(&cx2j_st); Ok(()) }