tonycao/ethercat-linux
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
3720737ab3
ethercat-linux/src/main.rs

2972 lines
139 KiB
Rust
Raw Blame History

//! 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<PduStorage<MAX_FRAMES, MAX_PDU_DATA>> =
std::sync::OnceLock::new();
extern "C" fn on_signal(_sig: libc::c_int) {
STOP.store(true, Ordering::Relaxed);
}
#[derive(Clone, Copy, PartialEq)]
enum Mode {
Ticker,
Probe,
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<String>,
}
fn parse_args() -> Result<Config, String> {
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<String, String> {
args.next().ok_or_else(|| format!("missing value for {}", name))
};
match a.as_str() {
"--mode" => {
cfg.mode = match val("--mode")?.as_str() {
"ticker" => Mode::Ticker,
"probe" => Mode::Probe,
"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<dyn std::error::Error>> {
let cycle_ns = cfg.cycle_us * 1000;
println!("el6695-rt: J1900 <-> TwinCAT 1 kHz verification (EL6695 bridge, secondary side)");
println!(
" iface={} mode={} duration={}s cycle={}us core={} prio={}",
cfg.iface,
match cfg.mode {
Mode::Ticker => "ticker",
Mode::Probe => "probe",
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::<libc::sockaddr_ll>() 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::<libc::timeval>() 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<String> = 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::<MAX_SUBDEVICES, PDI_LEN>(ethercat_now)
.await?;
let mut el6695_index = None;
for (idx, subdevice) in group.iter(&maindevice).enumerate() {
if subdevice.name().contains("EL6695") {
el6695_index = Some(idx);
println!("[{}] Found EL6695", idx);
}
}
let el6695_index = el6695_index.ok_or("EL6695 not found on bus")?;
let sd = group.subdevice(&maindevice, el6695_index)?;
let station = sd.configured_address();
println!("EL6695 station address: 0x{:04X}", station);
// ---- Probe SM registers ----
async fn read_sm(md: &MainDevice<'_>, st: u16, n: u8) -> (u16, u16, u8, u8) {
let start = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_START).receive::<u16>(md).await.unwrap_or(0xFFFF);
let len = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_LEN).receive::<u16>(md).await.unwrap_or(0xFFFF);
let ctrl = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_CTRL).receive::<u8>(md).await.unwrap_or(0xFF);
let ena = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_ENABLE).receive::<u8>(md).await.unwrap_or(0xFF);
(start, len, ctrl, ena)
}
for n in [0u8, 1, 2, 3] {
let (s, l, c, e) = read_sm(&maindevice, station, n).await;
println!(" SM{}: start=0x{:04X} len={} ctrl=0x{:02X} enable=0x{:02X}", n, s, l, c, e);
}
// Probe FMMU registers
for n in 0u16..4 {
let reg = 0x0600 + n * 16;
match ethercrab::Command::fprd(station, reg)
.receive_slice(&maindevice, 16)
.await
{
Ok(pdu) => {
let raw: &[u8] = &pdu;
let logical_start = u32::from_le_bytes(raw[0..4].try_into().unwrap());
let length = u16::from_le_bytes(raw[4..6].try_into().unwrap());
let phy_start = u16::from_le_bytes(raw[8..10].try_into().unwrap());
println!(" FMMU{}: logical_start=0x{:08X} len={} physical=0x{:04X} enable=0x{:02X}", n, logical_start, length, phy_start, raw[12]);
}
Err(e) => println!(" FMMU{} read error: {:?}", n, e),
}
}
// ---- Configure SM2/SM3 for process data (SM0/SM1 are EEPROM-fixed mailbox) ----
// 0x1C12/0x1C13 exist (empty), 0x1C10/0x1C11 do not.
// SM2/SM3 registers are writable because they are not defined in EEPROM.
println!("\nTesting SM0/SM1 data access...");
// Read SM1 (buffered input, 0x1600)
match ethercrab::Command::fprd(station, 0x1600)
.receive_slice(&maindevice, 32)
.await
{
Ok(pdu) => {
let raw: &[u8] = &pdu;
println!(" SM1 FPRD(0x1600,32) OK: {:02X?}", &raw[..32.min(raw.len())]);
}
Err(e) => println!(" SM1 FPRD error: {:?}", e),
}
// Write SM0 (mailbox, 0x1000)
match ethercrab::Command::fpwr(station, 0x1000)
.send(&maindevice, &[0xBBu8; 16])
.await
{
Ok(_) => println!(" SM0 FPWR(0x1000,16) OK"),
Err(e) => println!(" SM0 FPWR error: {:?}", e),
}
// Read back SM0
match ethercrab::Command::fprd(station, 0x1000)
.receive_slice(&maindevice, 16)
.await
{
Ok(pdu) => {
let raw: &[u8] = &pdu;
println!(" SM0 FPRD(0x1000,16) back: {:02X?}", &raw[..16.min(raw.len())]);
}
Err(e) => println!(" SM0 FPRD back error: {:?}", e),
}
// Remove SM2/SM3 config (was causing SAFEOP 0x001D)
// Safety: clear SM2/SM3 in case previous runs left stale values
let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE)
.send(&maindevice, 0x00u8).await;
let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE)
.send(&maindevice, 0x00u8).await;
// ---- 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::<libc::sockaddr_ll>() 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::<libc::timeval>() as u32) };
}
let srcmac: Vec<u8> = 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::<String>());
let dump = |tag: &str, buf: &[u8], n: usize| {
let f = &buf[..n];
let hex: Vec<String> = 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::<libc::sockaddr_ll>() 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::<u32>(0xF008, 0).await;
let f008_1 = sd.sdo_read::<u8>(0xF008, 1).await;
println!(" 0xF008:00 = {:?} sub1={:?}", f008_0, f008_1);
let f800_0 = sd.sdo_read::<u8>(0xF800, 0).await;
let f800_1 = sd.sdo_read::<u16>(0xF800, 1).await;
let f800_2 = sd.sdo_read::<u16>(0xF800, 2).await;
let f800_3 = sd.sdo_read::<u16>(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::<u16>(0xF800, sub, v).await;
let rb = sd.sdo_read::<u16>(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::<u32>(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::<u8>(0x6000, 0).await.unwrap_or(0xFF));
println!(" 0x7000[0] = {}", sd.sdo_read::<u8>(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::<u8>(0x0E00, 1, 1).await {
Ok(()) => println!(" write OK"),
Err(e) => println!(" write FAILED: {:?}", e),
}
// 璇诲洖 (閫氬父绔嬪嵆璇诲洖浠嶆槸鏃у€? 鏂數/INIT 鍚庣敓鏁?
let rb = sd.sdo_read::<u8>(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::<u8>(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::<u8>(0x1C13, 0).await.unwrap_or(0);
let mut entries13 = Vec::new();
for sub in 1..=cnt13.min(6) {
let e = sd.sdo_read::<u16>(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::<u8>(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::<u32>(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::<u16>(0x1C13, 1).await.unwrap_or(0);
if first13 == 0x1A01 {
let cnt = sd.sdo_read::<u8>(0x1A01, 0).await.unwrap_or(0);
let mut bits = 0u32;
for sub in 1..=cnt {
let e = sd.sdo_read::<u32>(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::<u8>(0x10F5, 1, 0).await;
let cur = sd.sdo_read::<u8>(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::<u64>(&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::<u64>(&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::<u8>(0x1608, 0).await.unwrap_or(0xFF);
let e1a = sd.sdo_read::<u8>(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::<u16>(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::<u16>(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::<u8>(0x7000, 0).await.unwrap_or(0xFF);
let _o6 = sd.sdo_read::<u8>(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::<u16>(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::<u16>(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::<u8>(0x7000, 0).await.unwrap_or(0xFF);
let _o6 = sd.sdo_read::<u8>(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::<u16>(&maindevice).await;
let sm3_len_rb = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_LEN).receive::<u16>(&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::<u64>(&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::<u16>(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::<u8>(&maindevice).await;
let a81 = ethercrab::Command::fprd(station, 0x0981u16).receive::<u8>(&maindevice).await;
let t_a = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::<u64>(&maindevice).await.unwrap_or(0);
std::thread::sleep(Duration::from_millis(200));
let t_b = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::<u64>(&maindevice).await.unwrap_or(0);
let s9ae = ethercrab::Command::fprd(station, 0x09AEu16).receive::<u8>(&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::<u8>(0x1C13, 0).await.unwrap_or(0xFF);
let first = sd.sdo_read::<u16>(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::<u8>(first, 0).await.unwrap_or(0);
let mut bits = 0u32;
for sub in 1..=cnt {
let e = sd.sdo_read::<u32>(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::<u16>(&maindevice).await.unwrap_or(0xFFFF);
let l = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_LEN).receive::<u16>(&maindevice).await.unwrap_or(0);
let c = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_CTRL).receive::<u8>(&maindevice).await.unwrap_or(0xFF);
let e = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_ENABLE).receive::<u8>(&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::<u16>(&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::<u16>(&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::<u16>(&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<String> = 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::<u16>(&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::<u16>(&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::<u16>(&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::<u8>(0xF130, 0).await;
let f130_1 = sd.sdo_read::<u8>(0xF130, 1).await;
println!("\n [0xF130] sub0={:?} sub1={:?}", f130_0, f130_1);
// ---- 0xF800 device config ----
let f800_sub0 = sd.sdo_read::<u8>(0xF800, 0).await;
let f800_1 = sd.sdo_read::<u16>(0xF800, 1).await;
let f800_2 = sd.sdo_read::<u16>(0xF800, 2).await;
let f800_3 = sd.sdo_read::<u16>(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::<u8>(0xF630, 0).await;
println!(" 0xF630 sub0={:?}", f630_0);
for sub in 1..=4u8 {
let v = sd.sdo_read::<u16>(0xF630, sub).await;
match v { Ok(x) => println!(" 0xF630:{}=0x{:04X}", sub, x), Err(_) => {} }
}
let f640_0 = sd.sdo_read::<u8>(0xF640, 0).await;
println!(" 0xF640 sub0={:?}", f640_0);
let c16 = sd.sdo_read::<u16>(0xF640, 1).await;
let c_lo = sd.sdo_read::<u8>(0xF640, 2).await;
let c_hi = sd.sdo_read::<u8>(0xF640, 3).await;
println!(" 0xF640:1 u16Count={:?} :2 LO={:?} :3 HI={:?}", c16, c_lo, c_hi);
let s10f4_0 = sd.sdo_read::<u8>(0x10F4, 0).await;
let s10f4_2 = sd.sdo_read::<u16>(0x10F4, 2).await;
let s10f4_10 = sd.sdo_read::<u16>(0x10F4, 0x10).await;
println!(" 0x10F4 sub0={:?} SyncMode(2)={:?} ExtDevNotConn(0x10)={:?}", s10f4_0, s10f4_2, s10f4_10);
let s10f5_0 = sd.sdo_read::<u8>(0x10F5, 0).await;
let s10f5_1 = sd.sdo_read::<u8>(0x10F5, 1).await;
println!(" 0x10F5 sub0={:?} SyncMaster(1)={:?}", s10f5_0, s10f5_1);
let fr = sd.sdo_write::<u32>(0x1011, 1, 0x64616F6C).await;
let frb = sd.sdo_read::<u32>(0x1011, 1).await;
println!(" FACTORY-RESET 0x1011:01 write={:?} readback=0x{:08X?}", fr.as_ref().map(|_| "OK"), frb);
let w = sd.sdo_write::<u8>(0x10F5, 1, 0).await;
let rb = sd.sdo_read::<u8>(0x10F5, 1).await;
println!(" WRITE 0x10F5:01=1 -> {:?}, readback={:?}", w.as_ref().map(|_| "OK"), rb);
let p32 = sd.sdo_read::<u8>(0x1C32, 1).await;
let p33 = sd.sdo_read::<u8>(0x1C33, 1).await;
println!(" 0x1C32:01(SMout syncmode)={:?} 0x1C33:01(SMin syncmode)={:?}", p32, p33);
let fa20_0 = sd.sdo_read::<u8>(0xFA20, 0).await;
println!(" 0xFA20 sub0={:?}", fa20_0);
for sub in 1..=8u8 {
let v = sd.sdo_read::<u16>(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::<u8>(idx, 0).await.unwrap_or(0);
println!(" 0x{:04X} sub0={}", idx, n);
for sub in 1..=subs {
if let Ok(x) = sd.sdo_read::<u32>(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<u32> = 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::<u16>(0xF640, 1).await;
let c_lo = sd.sdo_read::<u8>(0xF640, 2).await;
let c_hi = sd.sdo_read::<u8>(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::<u16>(0xF640, 1).await;
let c_lo = sd.sdo_read::<u8>(0xF640, 2).await;
let c_hi = sd.sdo_read::<u8>(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::<u32>(0xF640, sub).await;
match v { Ok(x) => println!(" [after TestB] 0xF640:{}={}", sub, x), Err(_) => {} }
}
let alr = ethercrab::Command::fprd(station, 0x0130u16).receive::<u16>(&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::<u64>(&maindevice)
.await
.unwrap_or(0);
std::thread::sleep(Duration::from_millis(100));
let t2 = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME)
.receive::<u64>(&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::<u8>(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::<u16>(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::<u32>(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::<u8>(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::<u16>(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::<u16>(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::<u64>(&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::<u8>(&maindevice)
.await;
let cyc0 = ethercrab::Command::fprd(station, regs::DC_SYNC0_CYCLE)
.receive::<u32>(&maindevice)
.await;
let t3 = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME)
.receive::<u64>(&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::<u8>(&maindevice)
.await
.unwrap_or(0xFF);
if rb & regs::EL2202_CH1_DATA == regs::EL2202_CH1_DATA {
println!(
"EL2202 scope output: verified, toggle 1 byte @0x{:04X} every cycle",
EL2202_OUT_ADDR
);
let _ = ethercrab::Command::fpwr(st2, EL2202_OUT_ADDR)
.send(&maindevice, 0x00u8)
.await;
el2202 = Some((st2, EL2202_OUT_ADDR));
} else {
println!("EL2202 output readback failed (0x{:02X}); scope output disabled", rb);
}
}
}
if !found {
println!("--el2202 given but no EL2202 on bus; scope output disabled");
}
if found && el2202.is_none() {
println!("EL2202 present but output setup failed; scope output disabled");
}
}
// ---- Optional EL2262 DC-locked reference output ----
const EL2262_OUT_ADDR: u16 = 0x1000;
let mut el2262: Option<u16> = None;
if cfg.el2262 {
let mut found = false;
for (idx, subdevice) in group.iter(&maindevice).enumerate() {
if subdevice.name().contains("EL2262") {
found = true;
let sd3 = group.subdevice(&maindevice, idx)?;
let st3 = sd3.configured_address();
println!("[{}] Found EL2262, station 0x{:04X}", idx, st3);
let t = ethercrab::Command::fprd(st3, regs::DC_SYSTEM_TIME)
.receive::<u64>(&maindevice)
.await
.unwrap_or(0);
let start = t.wrapping_add(100_000_000);
let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC_START_TIME)
.send(&maindevice, start)
.await;
let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC0_CYCLE)
.send(&maindevice, cycle_ns as u32)
.await;
let _ = ethercrab::Command::fpwr(st3, regs::DC_SYNC1_CYCLE)
.send(&maindevice, cycle_ns as u32)
.await;
let _ = regs::rmw_u8(
&maindevice,
st3,
regs::DC_SYNC_ACTIVATION,
regs::DC_SYNC0_ACTIVATE | regs::DC_SYNC1_ACTIVATE,
0,
)
.await;
println!("EL2262 DC sync: sync0/1 cycle={}ns start=+100ms (t={})", cycle_ns, t);
let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_START)
.send(&maindevice, EL2262_OUT_ADDR)
.await;
let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_LEN)
.send(&maindevice, 1u16)
.await;
let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_CTRL)
.send(&maindevice, regs::SM_CTRL_OUTPUTS_ESI)
.await;
let _ = regs::rmw_u8(
&maindevice,
st3,
regs::sm_base(0) + regs::SM_OFF_ENABLE,
regs::SM_ENABLE | regs::SM_ENABLE_FAMILY,
0,
)
.await;
let (al, alc) = bridge::drive_state(&maindevice, st3, regs::AL_SAFEOP).await;
println!("EL2262 SAFEOP: {}", bridge::verdict(al, alc));
let (al, alc) = bridge::drive_state(&maindevice, st3, regs::AL_OP).await;
println!("EL2262 OP: {}", bridge::verdict(al, alc));
if al & regs::AL_STATE_MASK != regs::AL_OP || al & regs::AL_ERROR != 0 {
return Err(format!("EL2262 OP rejected: {}", bridge::verdict(al, alc)).into());
}
el2262 = Some(st3);
}
}
if !found {
println!("--el2262 given but no EL2262 on bus; reference output disabled");
}
}
// ---- Optional EL1252 DC latch timestamp readback ----
let mut el1252: Option<u16> = None;
if cfg.el1252 {
let mut found = false;
for (idx, subdevice) in group.iter(&maindevice).enumerate() {
if subdevice.name().contains("EL1252") {
found = true;
let sd4 = group.subdevice(&maindevice, idx)?;
let st4 = sd4.configured_address();
println!("[{}] Found EL1252, station 0x{:04X} (latch readback)", idx, st4);
let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_START)
.send(&maindevice, 0x1000u16)
.await;
let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_LEN)
.send(&maindevice, 1u16)
.await;
let _ = ethercrab::Command::fpwr(st4, regs::sm_base(0) + regs::SM_OFF_CTRL)
.send(&maindevice, regs::SM_CTRL_EL1252_INPUTS)
.await;
let _ = regs::rmw_u8(
&maindevice,
st4,
regs::sm_base(0) + regs::SM_OFF_ENABLE,
regs::SM_ENABLE,
0,
)
.await;
let (al, alc) = bridge::drive_state(&maindevice, st4, regs::AL_SAFEOP).await;
println!("EL1252 SAFEOP: {}", bridge::verdict(al, alc));
let (al, alc) = bridge::drive_state(&maindevice, st4, regs::AL_OP).await;
println!("EL1252 OP: {}", bridge::verdict(al, alc));
if al & regs::AL_STATE_MASK != regs::AL_OP || al & regs::AL_ERROR != 0 {
return Err(format!("EL1252 OP rejected: {}", bridge::verdict(al, alc)).into());
}
el1252 = Some(st4);
}
}
if !found {
println!("--el1252 given but no EL1252 on bus; latch readback disabled");
}
}
// ---- Pre-allocate everything, then promote to RT ----
let mut tx_buf = vec![0u8; SM_TX_LEN];
let mut rx_buf = vec![0u8; SM_RX_LEN];
let mut jitter_st = OnlineStats::new("period_jitter", 100, -5_000_000, 5_000_000);
let mut exec_st = OnlineStats::new("exec_time", 1_000, 0, 20_000_000);
let mut rx_rtt_st = OnlineStats::new("fprd_rtt", 1_000, 0, 20_000_000);
let mut tx_rtt_st = OnlineStats::new("fpwr_rtt", 1_000, 0, 20_000_000);
println!("Promoting to RT: core={}, fifo={}, mlock=true", cfg.core, cfg.prio);
if let Err(e) = rt::promote_current_thread(cfg.core, cfg.prio, true) {
eprintln!("RT promotion failed (needs root, continuing anyway): {}", e);
}
rt::prefault_stack(256 * 1024);
// ---- Real-time data loop ----
println!(
"\n>>> running: {} loop, echo payload, stats (Ctrl-C for graceful stop)",
match cfg.mode {
Mode::Ticker => "ticker",
Mode::Probe => "probe (read-only, no echo)",
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<Instant> = 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<Box<dyn std::future::Future<Output = Result<(), ethercrab::error::Error>> + '_>>,
> = None;
let mut tx_late: u64 = 0;
let mut aborted: Option<String> = 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<Instant> = 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<el6695_rt::rfm::Rfm> = 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::<u8>(&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::<u64>(&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::<u16>(idx, 0x01).await.unwrap_or(0xFFFF);
let missed = sd.sdo_read::<u16>(idx, 0x0B).await.unwrap_or(0xFFFF);
let exceeded = sd.sdo_read::<u16>(idx, 0x0C).await.unwrap_or(0xFFFF);
let serr = sd.sdo_read::<u8>(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<dyn std::error::Error>> {
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: &ethercrab::MainDevice<'_>,
station: u16,
mut rfm: el6695_rt::rfm::Rfm,
duration_sec: u64,
) -> Result<(), Box<dyn std::error::Error>> {
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: &ethercrab::MainDevice<'_>,
station: u16,
mut rfm: el6695_rt::rfm::Rfm,
duration_sec: u64,
dc_discipline: bool,
sd: &ethercrab::SubDeviceRef<'_, &ethercrab::SubDevice>,
) -> Result<(), Box<dyn std::error::Error>> {
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<dyn std::future::Future<Output = Result<Vec<u8>, ethercrab::error::Error>> + 'a>,
>;
type FpwrFut<'a> = std::pin::Pin<
Box<dyn std::future::Future<Output = Result<(), ethercrab::error::Error>> + 'a>,
>;
let mut pending_fprd: Option<FprdFut<'_>> = None;
let mut pending_fpwr: Option<FpwrFut<'_>> = 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<Instant> = None;
let mut last_rep_detect: Option<Instant> = None;
let mut last_iter: Option<Instant> = 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<std::time::Instant> = 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<std::time::Instant> = 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::<u64>(&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::<u16>(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::<u8>(maindevice).await.unwrap_or(0xFF);
let sm2e = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_ENABLE)
.receive::<u8>(maindevice).await.unwrap_or(0xFF);
let sm3c = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_CTRL)
.receive::<u8>(maindevice).await.unwrap_or(0xFF);
let sm3e = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_ENABLE)
.receive::<u8>(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::<u16>(maindevice).await.unwrap_or(0xFFFF);
let sm3start = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_START)
.receive::<u16>(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(())
}
Reference in New Issue View Git Blame Copy Permalink