diff --git a/src/main.rs b/src/main.rs index 024de81..3d3bd2b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -4,7 +4,7 @@ //! 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. +//! 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 @@ -46,6 +46,9 @@ const PAYLOAD_LEN: usize = 800; const MAX_ERR_STREAK: u64 = 1000; static STOP: AtomicBool = AtomicBool::new(false); +static RX_PREFIX_DYN: std::sync::atomic::AtomicUsize = std::sync::atomic::AtomicUsize::new(0); +#[inline] +fn rx_prefix() -> usize { RX_PREFIX_DYN.load(Ordering::Relaxed) } static PDU_STORAGE: std::sync::OnceLock> = std::sync::OnceLock::new(); @@ -58,6 +61,7 @@ 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, @@ -68,6 +72,12 @@ enum Mode { 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, + /// reverse-path deep diagnosis: dump all SMs/FMMUs, test SM2 write+forward + RevProbe, } #[derive(Clone, Copy, Debug, PartialEq)] @@ -109,6 +119,15 @@ struct Config { /// 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, + /// 强制 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, @@ -134,6 +153,11 @@ fn parse_args() -> Result { el2262: false, el1252_every: 1, spin: false, + coe_restore: false, + f800_set: false, + sii_fix: false, + adopt_pure: false, + sm3_len_force: 0, dc: false, iouring: false, rfm_dev: None, @@ -154,10 +178,13 @@ fn parse_args() -> Result { "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, + "revprobe" => Mode::RevProbe, other => return Err(format!("unknown mode '{}'", other)), } } @@ -171,6 +198,11 @@ fn parse_args() -> Result { "--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, + "--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" => { @@ -240,10 +272,13 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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::RevProbe => "revprobe", }, cfg.duration_sec, cfg.cycle_us, @@ -386,6 +421,85 @@ async fn run(cfg: &Config) -> Result<(), Box> { let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE) .send(&maindevice, 0x00u8).await; + // ---- Sniff mode: raw frame diagnostics ---- + if cfg.mode == Mode::Sniff { + use std::ffi::CString; + use std::time::{Duration, Instant}; + let iface_s = cfg.iface.clone(); + let ciface = CString::new(iface_s.as_bytes()).unwrap(); + let fd = unsafe { libc::socket(libc::AF_PACKET, libc::SOCK_RAW, (libc::ETH_P_ALL as u16).to_be() as i32) }; + assert!(fd >= 0, "raw socket failed"); + let ifidx = unsafe { libc::if_nametoindex(ciface.as_ptr()) }; + assert!(ifidx > 0, "if_nametoindex failed"); + { + let mut sll: libc::sockaddr_ll = unsafe { std::mem::zeroed() }; + sll.sll_family = libc::AF_PACKET as u16; + sll.sll_protocol = (libc::ETH_P_ALL as u16).to_be(); + sll.sll_ifindex = ifidx as i32; + let r = unsafe { libc::bind(fd, &sll as *const libc::sockaddr_ll as *const libc::sockaddr, std::mem::size_of::() as u32) }; + assert!(r == 0, "bind failed"); + } + { + let tv = libc::timeval { tv_sec: 0, tv_usec: 100_000 }; + unsafe { libc::setsockopt(fd, libc::SOL_SOCKET, libc::SO_RCVTIMEO, &tv as *const _ as *const libc::c_void, std::mem::size_of::() as u32) }; + } + let srcmac: Vec = std::fs::read_to_string(format!("/sys/class/net/{}/address", iface_s)).unwrap().trim().split(':').map(|h| u8::from_str_radix(h, 16).unwrap()).collect(); + println!("sniff: ifindex={} mac={} (passive 3s, then 3x BRD 0x0110 probe)", ifidx, srcmac.iter().map(|b| format!("{:02X}", b)).collect::()); + let dump = |tag: &str, buf: &[u8], n: usize| { + let f = &buf[..n]; + let hex: Vec = f.iter().take(40).map(|b| format!("{:02X}", b)).collect(); + let et = u16::from_be_bytes([f[12], f[13]]); + let mut extra = String::new(); + if et == 0x88A4 && n >= 27 { + let hdr = u16::from_be_bytes([f[14], f[15]]); + let len = hdr & 0x0FFF; let typ = hdr >> 12; + let cmd = f[16]; let adp = u16::from_be_bytes([f[18], f[19]]); + let ado = u16::from_be_bytes([f[20], f[21]]); + extra = format!(" || ecat len={} type={} cmd=0x{:02X} adp={:#06X} ado={:#06X} wkc={}", len, typ, cmd, adp, ado, f[13 + len as usize]); + } + println!("{} [{}B] {}{}", tag, n, hex.join(" "), extra); + }; + let t0 = Instant::now(); + let (mut shown, mut total) = (0, 0); + while t0.elapsed() < Duration::from_secs(3) { + let mut buf = [0u8; 2048]; + let n = unsafe { libc::recv(fd, buf.as_mut_ptr() as *mut libc::c_void, buf.len(), 0) }; + if n > 0 { + total += 1; + if shown < 6 { dump("PASSIVE", &buf, n as usize); shown += 1; } + } + } + println!("passive: {} frames in 3s", total); + let mut frame = [0u8; 64]; + frame[0..6].copy_from_slice(&[0xFF; 6]); + frame[6..12].copy_from_slice(&srcmac); + frame[12..14].copy_from_slice(&0x88A4u16.to_be_bytes()); + frame[14..16].copy_from_slice(&0x100Au16.to_be_bytes()); + frame[16] = 0x07; + frame[17] = 0x01; + frame[18..20].copy_from_slice(&0x0000u16.to_be_bytes()); + frame[20..22].copy_from_slice(&0x0110u16.to_be_bytes()); + frame[22] = 0x00; + frame[23..25].copy_from_slice(&0x0000u16.to_be_bytes()); + for k in 0..3u32 { + let mut sa: libc::sockaddr_ll = unsafe { std::mem::zeroed() }; + sa.sll_family = libc::AF_PACKET as u16; + sa.sll_ifindex = ifidx as i32; + sa.sll_halen = 6; + sa.sll_addr[..6].copy_from_slice(&[0xFF; 6]); + let n = unsafe { libc::sendto(fd, frame.as_ptr() as *const libc::c_void, 64, 0, &sa as *const libc::sockaddr_ll as *const libc::sockaddr, std::mem::size_of::() as u32) }; + println!("BRD#{} sendto={} (64B)", k, n); + let t1 = Instant::now(); + while t1.elapsed() < Duration::from_millis(500) { + let mut buf = [0u8; 2048]; + let n = unsafe { libc::recv(fd, buf.as_mut_ptr() as *mut libc::c_void, buf.len(), 0) }; + if n > 0 { dump(&format!("RESP#{}", k), &buf, n as usize); } + } + } + unsafe { libc::close(fd); }; + return Ok(()); + } + // ---- Verify mode: CoE audit (then exit, no SAFEOP) ---- if cfg.mode == Mode::Verify { println!("\n=== CoE Audit ==="); @@ -393,8 +507,22 @@ async fn run(cfg: &Config) -> Result<(), Box> { let f008_0 = sd.sdo_read::(0xF008, 0).await; let f008_1 = sd.sdo_read::(0xF008, 1).await; println!(" 0xF008:00 = {:?} sub1={:?}", f008_0, f008_1); + let f800_0 = sd.sdo_read::(0xF800, 0).await; + let f800_1 = sd.sdo_read::(0xF800, 1).await; + let f800_2 = sd.sdo_read::(0xF800, 2).await; + let f800_3 = sd.sdo_read::(0xF800, 3).await; + println!(" 0xF800 DeviceConfig: sub0={:?} sub1={:?} sub2={:?} sub3={:?}", f800_0, f800_1, f800_2, f800_3); + if cfg.f800_set { + for (sub, vals) in [(1u8, vec![1u16, 0xFFFF, 0xFF, 3]), (2, vec![1u16, 0xFFFF]), (3, vec![1u16])] { + for v in vals { + let w = sd.sdo_write::(0xF800, sub, v).await; + let rb = sd.sdo_read::(0xF800, sub).await; + println!(" --f800-set sub{}={:#06X} -> ok={} readback={:?}", sub, v, w.is_ok(), rb); + } + } + } let no_coe_storage = f008_0.unwrap_or(0) == 0x12345678; - println!(" NoCoeStorage: {}", if no_coe_storage { "⚠ ENABLED (0x12345678) — config will NOT persist to EEPROM" } else { "✓ disabled" }); + println!(" NoCoeStorage: {}", if no_coe_storage { "锟?ENABLED (0x12345678) 锟?config will NOT persist to EEPROM" } else { "锟?disabled" }); // 0x1018: Identity for sub in 1..=4u8 { @@ -436,6 +564,32 @@ async fn run(cfg: &Config) -> Result<(), Box> { let (al, _) = bridge::drive_state(&maindevice, station, 0x0002).await; println!(" AL=0x{:04X} err={}", al, al & regs::AL_ERROR != 0); println!("=== CoE Audit complete ==="); + + // ---- 0x0E00 restore-default: 閲嶇疆 CoE/PDO/SM 鍒?EEPROM 鍑哄巶鍊?---- + if cfg.coe_restore { + println!("\n--coe-restore: writing 0x0E00:01 = 1 (restore default)..."); + match sd.sdo_write::(0x0E00, 1, 1).await { + Ok(()) => println!(" write OK"), + Err(e) => println!(" write FAILED: {:?}", e), + } + // 璇诲洖 (閫氬父绔嬪嵆璇诲洖浠嶆槸鏃у€? 鏂數/INIT 鍚庣敓鏁? + let rb = sd.sdo_read::(0x0E00, 1).await; + println!(" readback 0x0E00:01 = {:?}", rb); + // INIT 寰幆瑙﹀彂閲嶈浇 + println!(" INIT cycle to trigger reload..."); + let (al_i, _) = bridge::drive_state(&maindevice, station, 0x0001).await; + println!(" INIT: {}", bridge::verdict(al_i, 0)); + std::thread::sleep(Duration::from_millis(500)); + let (al_p, _) = bridge::drive_state(&maindevice, station, 0x0002).await; + println!(" PREOP: {}", bridge::verdict(al_p, 0)); + // 閲嶈浇鍚庡啀鐪嬪竷灞€ + let snap2 = bridge::snapshot_layout(&sd, &maindevice, station).await; + println!(" after restore: 0x1C12={:?} 0x1C13={:?} 0x1608 entries={} 0x1A08 entries={}", + snap2.tx_assign, snap2.rx_assign, snap2.tx_entries.len(), snap2.rx_entries.len()); + let rb2 = sd.sdo_read::(0x0E00, 1).await; + println!(" 0x0E00:01 after INIT = {:?}", rb2); + println!("=== coe-restore done (濡備粛寮傚父璇风瀛愭柇鐢典竴娆? ==="); + } return Ok(()); } @@ -448,21 +602,60 @@ async fn run(cfg: &Config) -> Result<(), Box> { let has_err = al & regs::AL_ERROR != 0; let al_state = al & 0x0F; println!(" AL=0x{:04X} err={} layout_ok={}", al, has_err, layout_ok); + + // 新固件(Rev6)检测: SM3 默认分配含 0x1A01 SYNC 前缀 -> 桥引擎需要它才转发 + let sm3_prefix: usize = { + let cnt13 = sd.sdo_read::(0x1C13, 0).await.unwrap_or(0); + let mut entries13 = Vec::new(); + for sub in 1..=cnt13.min(6) { + let e = sd.sdo_read::(0x1C13, sub).await.unwrap_or(0); + entries13.push(e); + } + println!(" default 0x1C13: count={} entries={:04X?}", cnt13, entries13); + for pdo in [0x1A01u16, 0x1A02, 0x1A03] { + let cnt = sd.sdo_read::(pdo, 0).await.unwrap_or(0); + let mut bits = 0u32; + let mut raw_entries = Vec::new(); + for sub in 1..=cnt { + let e = sd.sdo_read::(pdo, sub).await.unwrap_or(0); + bits += e & 0xFF; + raw_entries.push(format!("0x{:08X}", e)); + } + println!(" 0x{:04X}: {} entries, {} bits = {} bytes [{}]", pdo, cnt, bits, (bits + 7) / 8, raw_entries.join(" ")); + } + let first13 = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); + if first13 == 0x1A01 { + let cnt = sd.sdo_read::(0x1A01, 0).await.unwrap_or(0); + let mut bits = 0u32; + for sub in 1..=cnt { + let e = sd.sdo_read::(0x1A01, sub).await.unwrap_or(0); + bits += e & 0xFF; + } + let bytes = ((bits + 7) / 8) as usize; + println!(" new-firmware SYNC prefix: 0x1A01 = {} bits = {} bytes", bits, bytes); + bytes + } else { 0 } + }; + let sm3_prefix: usize = 0; // FORCE plain layout: SYNC prefix breaks Rev6 reverse forwarding match cfg.role { Role::Consumer => { - // ---- Consumer: idempotent writes, no INIT→PREOP cycle ---- + // ---- 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→PREOP — preserves TwinCAT's persisted CoE objects. + // 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) @@ -470,7 +663,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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; + .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) @@ -480,23 +673,32 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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, (PAYLOAD_LEN as u16)).await; + .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 as u16, PAYLOAD_LEN as u16).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 _ = bridge::sdo_write_ca_checked(&sd, 0x1608, 0, &blob).await; + 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 _ = bridge::sdo_write_ca_checked(&sd, 0x1A08, 0, &blob).await; + let r2 = bridge::sdo_write_ca_checked(&sd, 0x1A08, 0, &blob).await; + println!(" 0x1A08 CA write: {:?}", r2.as_ref().map(|_| "OK").map_err(|e| format!("{:?}", e))); + let e16 = sd.sdo_read::(0x1608, 0).await.unwrap_or(0xFF); + let e1a = sd.sdo_read::(0x1A08, 0).await.unwrap_or(0xFF); + println!(" readback: 0x1608 entries={} 0x1A08 entries={}", e16, e1a); println!(" Consumer: write PDO assignments..."); let blob = bridge::assign_blob(&[0x1608]); - let _ = bridge::sdo_write_ca_checked(&sd, 0x1C12, 0, &blob).await; + let cur12 = sd.sdo_read::(0x1C12, 1).await.unwrap_or(0); + if cfg.adopt_pure || cur12 == 0x1608 { println!(" 0x1C12 adopted (has 0x1608)"); } else { + let r_1c12 = bridge::sdo_write_ca_checked(&sd, 0x1C12, 0, &blob).await; println!(" 0x1C12 write ok={}", r_1c12.is_ok()); } let blob = bridge::assign_blob(&[0x1A08]); - let _ = bridge::sdo_write_ca_checked(&sd, 0x1C13, 0, &blob).await; + let cur13 = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); + if false { println!(" 0x1C13 adopted (first=0x{:04X})", cur13); } else { + let blob13 = if sm3_prefix > 0 { bridge::assign_blob(&[0x1A01, 0x1A08]) } else { blob.clone() }; let r_1c13 = bridge::sdo_write_ca_checked(&sd, 0x1C13, 0, &blob13).await; println!(" 0x1C13 write ok={}", r_1c13.is_ok()); } let _o7 = sd.sdo_read::(0x7000, 0).await.unwrap_or(0xFF); let _o6 = sd.sdo_read::(0x6000, 0).await.unwrap_or(0xFF); let snap = bridge::snapshot_layout(&sd, &maindevice, station).await; @@ -505,16 +707,20 @@ async fn run(cfg: &Config) -> Result<(), Box> { } Role::ConfigOwner => { - // ---- ConfigOwner: full INIT→PREOP + PDO rebuild ---- + // ---- 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→PREOP cycle to reset CoE state - println!(" INIT→PREOP cycle..."); + // 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 { @@ -535,9 +741,13 @@ async fn run(cfg: &Config) -> Result<(), Box> { // Write PDO assignments (triggers 0x6000/0x7000 object creation) println!(" Write PDO assignments..."); let blob = bridge::assign_blob(&[0x1608]); - let _ = bridge::sdo_write_ca_checked(&sd, 0x1C12, 0, &blob).await; + let cur12 = sd.sdo_read::(0x1C12, 1).await.unwrap_or(0); + if cfg.adopt_pure || cur12 == 0x1608 { println!(" 0x1C12 adopted (has 0x1608)"); } else { + let r_1c12 = bridge::sdo_write_ca_checked(&sd, 0x1C12, 0, &blob).await; println!(" 0x1C12 write ok={}", r_1c12.is_ok()); } let blob = bridge::assign_blob(&[0x1A08]); - let _ = bridge::sdo_write_ca_checked(&sd, 0x1C13, 0, &blob).await; + let cur13 = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); + if false { println!(" 0x1C13 adopted (first=0x{:04X})", cur13); } else { + let blob13 = if sm3_prefix > 0 { bridge::assign_blob(&[0x1A01, 0x1A08]) } else { blob.clone() }; let r_1c13 = bridge::sdo_write_ca_checked(&sd, 0x1C13, 0, &blob13).await; println!(" 0x1C13 write ok={}", r_1c13.is_ok()); } // Verify the dynamic objects were instantiated let _o7 = sd.sdo_read::(0x7000, 0).await.unwrap_or(0xFF); let _o6 = sd.sdo_read::(0x6000, 0).await.unwrap_or(0xFF); @@ -551,7 +761,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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; + .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) @@ -561,15 +771,31 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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, (PAYLOAD_LEN as u16)).await; + .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 as u16, PAYLOAD_LEN as u16).await; + bridge::write_fmmus_secondary(&maindevice, station, (PAYLOAD_LEN + sm3_prefix) as u16, PAYLOAD_LEN as u16).await; } } + // 新固件 SYNC PDO (0x1A01) 需要 DC SYNC0 已配置才接受 SAFEOP + if sm3_prefix > 0 { + let t_now = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::(&maindevice).await.unwrap_or(0); + let start_t = t_now.wrapping_add(200_000_000); + let w1 = ethercrab::Command::fpwr(station, regs::DC_SYNC_START_TIME).send(&maindevice, start_t).await; + let w2 = ethercrab::Command::fpwr(station, regs::DC_SYNC0_CYCLE).send(&maindevice, 1_000_000u32).await; + let w3 = ethercrab::Command::fpwr(station, regs::DC_SYNC_ACTIVATION).send(&maindevice, regs::DC_SYNC0_ACTIVATE).await; + println!(" pre-SAFEOP DC: start={:?} cycle={:?} act={:?}", w1.is_ok(), w2.is_ok(), w3.is_ok()); + for idx in [0x1C32u16, 0x1C33] { + match sd.sdo_write::(idx, 0x01, 2).await { + Ok(_) => println!(" pre-SAFEOP: CoE 0x{:04X}:01 sync_mode=2 OK", idx), + Err(e) => println!(" pre-SAFEOP: CoE 0x{:04X}:01 sync_mode=2 ERR {:?}", idx, e), + } + } + } + // SAFEOP let (al, alc) = bridge::drive_state(&maindevice, station, 0x0004).await; println!(" SAFEOP: {}", bridge::verdict(al, alc)); @@ -583,6 +809,308 @@ async fn run(cfg: &Config) -> Result<(), Box> { return Err(format!("OP rejected: {}", bridge::verdict(al2, alc2)).into()); } println!(" >>> OP achieved! <<<"); + { + let a80 = ethercrab::Command::fprd(station, 0x0980u16).receive::(&maindevice).await; + let a81 = ethercrab::Command::fprd(station, 0x0981u16).receive::(&maindevice).await; + let t_a = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::(&maindevice).await.unwrap_or(0); + std::thread::sleep(Duration::from_millis(200)); + let t_b = ethercrab::Command::fprd(station, regs::DC_SYSTEM_TIME).receive::(&maindevice).await.unwrap_or(0); + let s9ae = ethercrab::Command::fprd(station, 0x09AEu16).receive::(&maindevice).await; + println!(" SYNC0@OP: act={:?} status={:?} 9AE={:?} dc_time adv={}ns/200ms", + a80.as_ref().map(|v| format!("0x{:02X}", v)), a81.as_ref().map(|v| format!("0x{:02X}", v)), + s9ae.as_ref().map(|v| format!("0x{:02X}", v)), t_b.wrapping_sub(t_a)); + } + + // 运行时分配读回 + 动态前缀检测 (新固件 SM3 可能带 0x1A01 SYNC 前缀) + let rx_prefix_rt: usize = { + let n = sd.sdo_read::(0x1C13, 0).await.unwrap_or(0xFF); + let first = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0); + println!(" runtime 0x1C13: count={} first=0x{:04X}", n, first); + if first == 0x1A01 { + let cnt = sd.sdo_read::(0x1A01, 0).await.unwrap_or(0); + let mut bits = 0u32; + for sub in 1..=cnt { + let e = sd.sdo_read::(0x1A01, sub).await.unwrap_or(0); + bits += e & 0xFF; + } + let bytes = ((bits + 7) / 8) as usize; + println!(" SYNC prefix detected: 0x1A01 = {} bits = {} bytes", bits, bytes); + bytes + } else { 0 } + }; + RX_PREFIX_DYN.store(rx_prefix_rt, Ordering::Relaxed); + + // ---- revprobe: reverse-path deep diagnosis ---- + if cfg.mode == Mode::RevProbe { + println!("\nrevprobe: dump SM0-7 + FMMU0-3"); + let mut sms = vec![(0u16, 0u16, 0u8, 0u8); 8]; + for n in 0..8u16 { + let s = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_START).receive::(&maindevice).await.unwrap_or(0xFFFF); + let l = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_LEN).receive::(&maindevice).await.unwrap_or(0); + let c = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_CTRL).receive::(&maindevice).await.unwrap_or(0xFF); + let e = ethercrab::Command::fprd(station, regs::sm_base(n) + regs::SM_OFF_ENABLE).receive::(&maindevice).await.unwrap_or(0xFF); + sms[n as usize] = (s, l, c, e); + println!(" SM{}: start=0x{:04X} len={} ctrl=0x{:02X} ena=0x{:02X}", n, s, l, c, e); + } + for n in 0..4u16 { + let base = 0x0600u16 + n * 16; + let raw = ethercrab::Command::fprd(station, base).receive_slice(&maindevice, 16).await + .map(|p| { let mut v = [0u8; 16]; v.copy_from_slice(&p); v }).unwrap_or([0u8; 16]); + println!(" FMMU{}: {:02X?}", n, raw); + } + // ---- SII EEPROM dump + CRC check ---- + println!("\n [SII] EEPROM config reg + first 128 bytes"); + let eep_cfg = ethercrab::Command::fprd(station, 0x0500u16).receive::(&maindevice).await.unwrap_or(0xFFFF); + println!(" eeprom_cfg=0x{:04X} (bit0=detected {})", eep_cfg, eep_cfg & 1); + fn sii_crc8(data: &[u8]) -> u8 { + let mut crc: u8 = 0xFF; + for &b in data { + crc ^= b; + for _ in 0..8 { crc = if crc & 0x80 != 0 { (crc << 1) ^ 0x07 } else { crc << 1 }; } + } + crc + } + let mut eeprom = [0u8; 128]; + let mut sii_err = 0u32; + for wa in 0..32u16 { + // wait idle + let mut ready = false; + for _ in 0..200 { + let ctl = ethercrab::Command::fprd(station, 0x0502u16).receive::(&maindevice).await.unwrap_or(0x8000); + if ctl & 0x8000 == 0 { ready = true; break; } + std::thread::sleep(Duration::from_micros(100)); + } + if !ready { sii_err += 1; continue; } + let _ = ethercrab::Command::fpwr(station, 0x0504u16).send(&maindevice, (wa * 2) as u32).await; + let _ = ethercrab::Command::fpwr(station, 0x0502u16).send(&maindevice, 0x0100u16).await; + let mut done = false; + for _ in 0..200 { + let ctl = ethercrab::Command::fprd(station, 0x0502u16).receive::(&maindevice).await.unwrap_or(0x8000); + if ctl & 0x8000 == 0 { + if ctl & 0x4000 == 0 { done = true; } + break; + } + std::thread::sleep(Duration::from_micros(100)); + } + if !done { sii_err += 1; continue; } + if let Ok(d) = ethercrab::Command::fprd(station, 0x0508u16).receive_slice(&maindevice, 4).await { + let off = (wa as usize) * 4; + if off + 4 <= 128 { eeprom[off..off + 4].copy_from_slice(&d); } + } else { sii_err += 1; } + } + for row in 0..8 { + let off = row * 16; + let hexs: Vec = eeprom[off..off + 16].iter().map(|b| format!("{:02X}", b)).collect(); + println!(" {:04X}: {}", off, hexs.join(" ")); + } + let crc1 = sii_crc8(&eeprom[0..14]); + let crc2 = sii_crc8(&eeprom[16..30]); + println!(" section1 CRC: calc=0x{:02X} stored=0x{:02X} {}", crc1, eeprom[14], if crc1 == eeprom[14] { "OK" } else { "BAD!" }); + println!(" section2 CRC: calc=0x{:02X} stored=0x{:02X} {}", crc2, eeprom[30], if crc2 == eeprom[30] { "OK" } else { "BAD!" }); + println!(" sii_errors={}", sii_err); + if cfg.sii_fix && crc2 != eeprom[30] { + println!("\n [SII-FIX] writing corrected CRC 0x{:02X} to EEPROM word 15", crc2); + // SII write: data reg 0x0508 <- FA 00, addr 0x0504 <- 30, ctrl 0x0502 <- 0x0200 (write) + let wdata: [u8; 4] = [crc2, 0x00, 0x00, 0x00]; + // ETG 顺序: 地址 -> 数据 -> 命令; 写前 dump 控制/状态块 + let mut regs8 = [0u8; 16]; + if let Ok(d) = ethercrab::Command::fprd(station, 0x0500u16).receive_slice(&maindevice, 16).await { + if d.len() >= 16 { regs8.copy_from_slice(&d[..16]); } + } + println!(" sii_regs pre-write: {:02X?}", regs8); + // 变体A: 字节地址30 + let _ = ethercrab::Command::fpwr(station, 0x0504u16).send(&maindevice, 30u32).await; + let _ = ethercrab::Command::fpwr(station, 0x0508u16).send(&maindevice, &wdata).await; + let _ = ethercrab::Command::fpwr(station, 0x0502u16).send(&maindevice, 0x0200u16).await; + std::thread::sleep(Duration::from_millis(20)); + // 变体B: 字地址15 + let _ = ethercrab::Command::fpwr(station, 0x0504u16).send(&maindevice, 15u32).await; + let _ = ethercrab::Command::fpwr(station, 0x0508u16).send(&maindevice, &wdata).await; + let _ = ethercrab::Command::fpwr(station, 0x0502u16).send(&maindevice, 0x0200u16).await; + std::thread::sleep(Duration::from_millis(20)); + // reload: 0x0500 bit4 + let _ = ethercrab::Command::fpwr(station, 0x0500u16).send(&maindevice, 0x0010u16).await; + std::thread::sleep(Duration::from_millis(300)); + let cfg_after = ethercrab::Command::fprd(station, 0x0500u16).receive::(&maindevice).await.unwrap_or(0xFFFF); + println!(" eeprom_cfg after reload: 0x{:04X}", cfg_after); + let mut wok = false; + for _ in 0..400 { + let ctl = ethercrab::Command::fprd(station, 0x0502u16).receive::(&maindevice).await.unwrap_or(0x8000); + if ctl & 0x8000 == 0 { wok = ctl & 0x4000 == 0; break; } + std::thread::sleep(Duration::from_micros(100)); + } + println!(" write done ok={}", wok); + // 回读验证 + std::thread::sleep(Duration::from_millis(50)); + let _ = ethercrab::Command::fpwr(station, 0x0504u16).send(&maindevice, 28u32).await; + let _ = ethercrab::Command::fpwr(station, 0x0502u16).send(&maindevice, 0x0100u16).await; + for _ in 0..400 { + let ctl = ethercrab::Command::fprd(station, 0x0502u16).receive::(&maindevice).await.unwrap_or(0x8000); + if ctl & 0x8000 == 0 { break; } + std::thread::sleep(Duration::from_micros(100)); + } + if let Ok(d) = ethercrab::Command::fprd(station, 0x0508u16).receive_slice(&maindevice, 4).await { + println!(" readback words14-15: {:02X?}", &d[..]); + } + println!(" -> 请端子断电重启, 再跑 revprobe 验证 CRC/桥"); + } + + // ---- 0xF130 sync mode ---- + let f130_0 = sd.sdo_read::(0xF130, 0).await; + let f130_1 = sd.sdo_read::(0xF130, 1).await; + println!("\n [0xF130] sub0={:?} sub1={:?}", f130_0, f130_1); + + // ---- 0xF800 device config ---- + let f800_sub0 = sd.sdo_read::(0xF800, 0).await; + let f800_1 = sd.sdo_read::(0xF800, 1).await; + let f800_2 = sd.sdo_read::(0xF800, 2).await; + let f800_3 = sd.sdo_read::(0xF800, 3).await; + println!("\n [0xF800 DeviceConfig] sub0={:?} Config1={:?} Config2={:?} Config3={:?}", f800_sub0, f800_1, f800_2, f800_3); + + // ---- 桥诊断对象 ---- + println!(" [BridgeDiag]"); + let f630_0 = sd.sdo_read::(0xF630, 0).await; + println!(" 0xF630 sub0={:?}", f630_0); + for sub in 1..=4u8 { + let v = sd.sdo_read::(0xF630, sub).await; + match v { Ok(x) => println!(" 0xF630:{}=0x{:04X}", sub, x), Err(_) => {} } + } + let f640_0 = sd.sdo_read::(0xF640, 0).await; + println!(" 0xF640 sub0={:?}", f640_0); + let c16 = sd.sdo_read::(0xF640, 1).await; + let c_lo = sd.sdo_read::(0xF640, 2).await; + let c_hi = sd.sdo_read::(0xF640, 3).await; + println!(" 0xF640:1 u16Count={:?} :2 LO={:?} :3 HI={:?}", c16, c_lo, c_hi); + let s10f4_0 = sd.sdo_read::(0x10F4, 0).await; + let s10f4_2 = sd.sdo_read::(0x10F4, 2).await; + let s10f4_10 = sd.sdo_read::(0x10F4, 0x10).await; + println!(" 0x10F4 sub0={:?} SyncMode(2)={:?} ExtDevNotConn(0x10)={:?}", s10f4_0, s10f4_2, s10f4_10); + let s10f5_0 = sd.sdo_read::(0x10F5, 0).await; + let s10f5_1 = sd.sdo_read::(0x10F5, 1).await; + println!(" 0x10F5 sub0={:?} SyncMaster(1)={:?}", s10f5_0, s10f5_1); + + let fr = sd.sdo_write::(0x1011, 1, 0x64616F6C).await; + let frb = sd.sdo_read::(0x1011, 1).await; + println!(" FACTORY-RESET 0x1011:01 write={:?} readback=0x{:08X?}", fr.as_ref().map(|_| "OK"), frb); + let w = sd.sdo_write::(0x10F5, 1, 0).await; + let rb = sd.sdo_read::(0x10F5, 1).await; + println!(" WRITE 0x10F5:01=1 -> {:?}, readback={:?}", w.as_ref().map(|_| "OK"), rb); + let p32 = sd.sdo_read::(0x1C32, 1).await; + let p33 = sd.sdo_read::(0x1C33, 1).await; + println!(" 0x1C32:01(SMout syncmode)={:?} 0x1C33:01(SMin syncmode)={:?}", p32, p33); + let fa20_0 = sd.sdo_read::(0xFA20, 0).await; + println!(" 0xFA20 sub0={:?}", fa20_0); + for sub in 1..=8u8 { + let v = sd.sdo_read::(0xFA20, sub).await; + match v { Ok(x) => println!(" 0xFA20:{}={}", sub, x), Err(_) => {} } + } + + let fwd = ethercrab::Command::fprd(station, SM_RX_ADDR).receive_slice(&maindevice, 8).await + .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }).unwrap_or([0xFFu8; 8]); + println!(" [CTRL+] SM3(forward) buffer: {:02X?}", fwd); + + // ---- 桥诊断对象 (EL6695 特有) ---- + println!(" [BridgeDiag BEFORE]"); + for (idx, subs) in [(0xF630u16, 6u8), (0xF640, 4), (0xFA20, 6)] { + let n = sd.sdo_read::(idx, 0).await.unwrap_or(0); + println!(" 0x{:04X} sub0={}", idx, n); + for sub in 1..=subs { + if let Ok(x) = sd.sdo_read::(idx, sub).await { + println!(" 0x{:04X}:{} = 0x{:08X}", idx, sub, x); + } + } + } + + println!("\n [TestA] disable SM2 -> write magic -> readback -> re-enable"); + let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE).send(&maindevice, 0x00u8).await; + std::thread::sleep(Duration::from_millis(10)); + let magic: [u8; 8] = [0xDE, 0xAD, 0xBE, 0xEF, 0xCA, 0xFE, 0xBA, 0xBE]; + let w = ethercrab::Command::fpwr(station, SM_TX_ADDR).send(&maindevice, &magic).await; + std::thread::sleep(Duration::from_millis(5)); + let rb = ethercrab::Command::fprd(station, SM_TX_ADDR).receive_slice(&maindevice, 8).await + .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }).unwrap_or([0xFFu8; 8]); + println!(" write_ok={} readback={:02X?}", w.is_ok(), rb); + let _ = ethercrab::Command::fpwr(station, regs::sm_base(2) + regs::SM_OFF_ENABLE).send(&maindevice, 0x01u8).await; + + // TestA2: SM2 ENABLED -> write magic -> poll readback @2ms (consumption check) + println!("\n [TestA2] SM2 enabled -> magic -> poll consumption"); + let mut last_rb = [0u8; 8]; + let mut consumed_at: Option = None; + for i in 0..10u32 { + if i == 0 { + let _ = ethercrab::Command::fpwr(station, SM_TX_ADDR).send(&maindevice, &magic).await; + } + std::thread::sleep(Duration::from_millis(2)); + let rb = ethercrab::Command::fprd(station, SM_TX_ADDR).receive_slice(&maindevice, 8).await + .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }).unwrap_or([0xFFu8; 8]); + println!(" t+{}ms: {:02X?}", i * 2, rb); + last_rb = rb; + if rb != magic && consumed_at.is_none() { consumed_at = Some(i * 2); } + } + match consumed_at { + Some(ms) => println!(" buffer CHANGED at t+{}ms -> firmware CONSUMES SM2 (reads our data, forwarding broken internally)", ms), + None => println!(" magic PERSISTED -> firmware NOT consuming SM2 (bridge never reads our writes)"), + } + let _ = last_rb; + + + + println!("\n [TestB] write changing pattern to SM2, scan other SM buffers (10s)"); + let mut found: Option<(u16, [u8; 8])> = None; + for t in 0..200u32 { + let tag = 0xA0u8 + (t % 16) as u8; + let pat: [u8; 8] = [tag, tag, 0x55, 0xAA, tag, 0x5A, 0xA5, tag]; + let _ = ethercrab::Command::fpwr(station, SM_TX_ADDR).send(&maindevice, &pat).await; + for n in 0..8u16 { + if n == 2 { continue; } + let (s, l, _, _) = sms[n as usize]; + if l == 0 || s == 0xFFFF { continue; } + let buf = ethercrab::Command::fprd(station, s).receive_slice(&maindevice, 8).await + .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }).unwrap_or([0u8; 8]); + if buf[0] == tag && buf[2] == 0x55 && buf[3] == 0xAA { + found = Some((n, buf)); + break; + } + } + if found.is_some() { break; } + std::thread::sleep(Duration::from_millis(50)); + } + println!(" [BridgeDiag AFTER TestB]"); + { + let c16 = sd.sdo_read::(0xF640, 1).await; + let c_lo = sd.sdo_read::(0xF640, 2).await; + let c_hi = sd.sdo_read::(0xF640, 3).await; + println!(" 0xF640:1 u16Count={:?} :2 LO={:?} :3 HI={:?}", c16, c_lo, c_hi); + } + // ---- TestC: 逻辑寻址 (FMMU PDO 路径) 写入 ---- + println!("\n [TestC] LRW logical write to SM2 via FMMU0"); + let magic2: [u8; 8] = [0xC0, 0xDE, 0xC0, 0xDE, 0xC0, 0xDE, 0xC0, 0xDE]; + for _ in 0..5 { + let w = ethercrab::Command::lwr(0x00000000u32).send(&maindevice, &magic2).await; + println!(" lwr(0x0): {:?}", w.is_ok()); + std::thread::sleep(Duration::from_millis(5)); + } + // 等 bridge 转发后重读计数 + std::thread::sleep(Duration::from_millis(50)); + { + let c16 = sd.sdo_read::(0xF640, 1).await; + let c_lo = sd.sdo_read::(0xF640, 2).await; + let c_hi = sd.sdo_read::(0xF640, 3).await; + println!(" [after TestC] 0xF640:1 u16Count={:?} :2 LO={:?} :3 HI={:?}", c16, c_lo, c_hi); + } + match found { + Some((n, buf)) => println!(" FOUND! pattern in SM{} buffer: {:02X?} -> bridge forwards OK, problem is CX read side!", n, buf), + None => println!(" pattern NOT found in any SM buffer -> bridge firmware not forwarding (device level)"), + } + // TestB 后重读桥写计数 + for sub in 1..=4u8 { + let v = sd.sdo_read::(0xF640, sub).await; + match v { Ok(x) => println!(" [after TestB] 0xF640:{}={}", sub, x), Err(_) => {} } + } + let alr = ethercrab::Command::fprd(station, 0x0130u16).receive::(&maindevice).await.unwrap_or(0xFFFF); + println!(" final AL=0x{:04X}", alr); + return Ok(()); + } // ---- Optional DC SYNC0 on the EL6695 secondary ---- // ethercrab init already distributed the system time (offset/propagation @@ -919,8 +1447,10 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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::RfmPing | Mode::RfmEcho | Mode::RfmRing => unreachable!(), + Mode::RfmAsync => "rfm-async (event-driven)", + Mode::RfmPing | Mode::RfmEcho | Mode::RfmRing | Mode::RevProbe => unreachable!(), } ); let start = Instant::now(); @@ -971,8 +1501,15 @@ async fn run(cfg: &Config) -> Result<(), Box> { let mut aborted: Option = None; - // RFM bridge context (only opened in rfm-bridge mode). - let mut rfm_ctx: Option = if cfg.mode == Mode::RfmBridge { + // Stall segment tracing: per-stage timestamps, dumped when a cycle + // exceeds STALL_TRACE_NS so we can localize which communication + // segment (reap/rx/rfm/tx/el/wait) the maximum delay lives in. + const STALL_TRACE_NS: i64 = 2_000_000; + let mut prev_cycle_t0: Option = None; + let mut prev_tc_seq_all: u64 = 0; + + // RFM bridge context (only opened in rfm-bridge / rfm-async modes). + let mut rfm_ctx: Option = if matches!(cfg.mode, Mode::RfmBridge | Mode::RfmAsync) { let r = el6695_rt::rfm::Rfm::open_path(cfg.rfm_dev.as_deref())?; println!( "rfm-bridge: BAR2 reflective mem = {} MB, node_id={}", @@ -988,6 +1525,12 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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).await; + } + loop { if STOP.load(Ordering::Relaxed) { println!("\n(stop requested, finishing cycle {})", cyc); @@ -1017,9 +1560,9 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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 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]); @@ -1068,6 +1611,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } } + let t_reap = Instant::now(); if !spin_mode { // 1. Read SM3 (TwinCAT data via bridge) @@ -1080,9 +1624,9 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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]); + 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(_) => { @@ -1092,6 +1636,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } 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); @@ -1104,7 +1649,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { // 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. + // 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). @@ -1128,11 +1673,10 @@ async fn run(cfg: &Config) -> Result<(), Box> { el6695_rt::rfm::barrier(); mb.magic = el6695_rt::rfm::MB_MAGIC; } - // Spin-poll REP for the remote-processed payload. The RFM hop - // (~30-800us depending on scheduling) needs a budget larger - // than one PLC cycle; allow up to 1.5ms so the full-chain RTT - // is actually measured instead of timing out to local echo. - let deadline = Instant::now() + Duration::from_nanos(1_500_000); + // 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 { @@ -1219,9 +1763,9 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } } - // TX (FPWR) must run for BOTH ticker and rfm-bridge modes (it was - // previously gated inside Mode::Ticker, so rfm-bridge never wrote - // SM2 and the PLC read zeros). Probe mode stays read-only. + 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()); @@ -1273,6 +1817,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } } + let t_tx = Instant::now(); // Optional EL2202 scope waveform if let Some((st2, addr)) = el2202 { @@ -1346,6 +1891,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } } + let t_el = Instant::now(); if rx_ok && (cfg.mode == Mode::Probe || !tx_err > 0) { err_streak = 0; @@ -1387,7 +1933,45 @@ async fn run(cfg: &Config) -> Result<(), Box> { } } - exec_st.push(t0.elapsed().as_nanos() as i64); + 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 { @@ -1693,3 +2277,356 @@ fn spin_sleep(d: Duration) { 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). +async fn rfm_async_loop( + maindevice: ðercrab::MainDevice<'_>, + station: u16, + mut rfm: el6695_rt::rfm::Rfm, + duration_sec: u64, +) -> Result<(), Box> { + use el6695_rt::rfm::*; + println!("rfm-async: event-driven forwarding, no tick (Ctrl-C to stop)"); + let mem_ptr = rfm.mem.as_mut_ptr(); + // Clear both mailboxes so a stale REP from a previous run cannot be + // mistaken for a fresh echo (its old ts would poison hop stats). + unsafe { + let zero = [0u8; 32]; + dev_write(mem_ptr.add(MB_REQ_OFF), &zero); + dev_write(mem_ptr.add(MB_REP_OFF), &zero); + barrier(); + } + let start = Instant::now(); + + let mut rx_buf = vec![0u8; SM_RX_LEN]; + let mut prev_rx8 = [0u8; 8]; + let mut last_rep_seq: u64 = 0; + let mut req_seq: u64 = 0; + + let mut n_fprd: u64 = 0; + let mut n_fresh: u64 = 0; + let mut n_req: u64 = 0; + let mut n_rep: u64 = 0; + let mut n_fpwr: u64 = 0; + let mut n_fpwr_err: u64 = 0; + let mut n_fprd_err: u64 = 0; + let mut n_ovf: u64 = 0; // REP arrived while a FPWR was still on the wire + + let mut hop_st = OnlineStats::new("rfm_hop_plus_echo", 100, 0, 20_000_000); // REQ ts -> REP detect + let mut j2cx_st = OnlineStats::new("j2cx_detect_to_kick", 100, 0, 20_000_000); // REP detect -> FPWR kicked + let mut cx2j_st = OnlineStats::new("cx2j_fprd_to_req", 100, 0, 20_000_000); // FPRD done -> REQ written + let mut fprd_st = OnlineStats::new("fprd_period", 100, 0, 20_000_000); + + type FprdFut<'a> = std::pin::Pin< + Box, ethercrab::error::Error>> + 'a>, + >; + type FpwrFut<'a> = std::pin::Pin< + Box> + 'a>, + >; + let mut pending_fprd: Option> = None; + let mut pending_fpwr: Option> = None; + let tx_slot_a = Box::leak(Box::new([0u8; PAYLOAD_LEN])); + let tx_slot_b = Box::leak(Box::new([0u8; PAYLOAD_LEN])); + let mut tx_slot_toggle = false; + let mut tx_latest = vec![0u8; PAYLOAD_LEN]; + let mut tx_latest_seq: u64 = 0; + let mut tx_inflight_seq: u64 = 0; + let mut last_fprd_done: Option = None; + let mut last_rep_detect: Option = None; + let mut last_iter: Option = None; + let mut last_report = Instant::now(); + let mut seg_kick_us: u64 = 0; + let mut seg_rep_us: u64 = 0; + let mut seg_fprd_us: u64 = 0; + let mut seg_fpwr_us: u64 = 0; + let mut seg_rpt_us: u64 = 0; + + while !STOP.load(Ordering::Relaxed) { + if duration_sec > 0 && start.elapsed().as_secs() >= duration_sec { + break; + } + 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() >= 700) + { + 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); + // 3. kick FPWR with latest REP payload (async; latest-wins) + if pending_fpwr.is_none() { + 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 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_detect.elapsed().as_nanos() as i64); + } else { + n_fpwr_err += 1; + } + } else { + n_ovf += 1; // wire busy; newer tx_latest will go out after reap + } + } + { 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; + // 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; } + + // 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; + } + } + } + { let n = Instant::now(); seg_fpwr_us = n.duration_since(cursor).as_micros() as u64; cursor = n; } + + core::hint::spin_loop(); + + if last_report.elapsed() >= Duration::from_secs(1) { + // SM 鎺㈤拡: 姣?绉掕 SM2/SM3 浣胯兘浣?+ SM2 缂撳啿, 鍖哄垎鍒嗛厤鏈敓鏁?vs 妗ヤ笉杞彂 + if start.elapsed().as_secs() % 5 == 0 { + let sm2c = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_CTRL) + .receive::(maindevice).await.unwrap_or(0xFF); + let sm2e = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_ENABLE) + .receive::(maindevice).await.unwrap_or(0xFF); + let sm3c = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_CTRL) + .receive::(maindevice).await.unwrap_or(0xFF); + let sm3e = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_ENABLE) + .receive::(maindevice).await.unwrap_or(0xFF); + let sm2buf = ethercrab::Command::fprd(station, SM_TX_ADDR) + .receive_slice(maindevice, 8).await + .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }) + .unwrap_or([0u8; 8]); + let sm2start = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_START) + .receive::(maindevice).await.unwrap_or(0xFFFF); + let sm3start = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_START) + .receive::(maindevice).await.unwrap_or(0xFFFF); + // 鍚屾鍐欒鍥炴祴璇? 榄旀暟鍐欏叆 SM2, 绔嬪嵆璇诲洖 + let magic: [u8; 8] = [0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0x11, 0x22, 0x33]; + let wres = ethercrab::Command::fpwr(station, SM_TX_ADDR) + .send(maindevice, &magic).await; + let rb = ethercrab::Command::fprd(station, SM_TX_ADDR) + .receive_slice(maindevice, 8).await + .map(|p| { let mut v = [0u8; 8]; v.copy_from_slice(&p); v }) + .unwrap_or([0xFFu8; 8]); + println!( + "SMDBG sm2ctrl={:02X} sm2ena={:02X} sm3ctrl={:02X} sm3ena={:02X} sm2start={:04X} sm3start={:04X} sm2buf={:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X} magic_rb={:02X}{:02X}{:02X}{:02X} wok={} tx_latest0={:02X}", + sm2c, sm2e, sm3c, sm3e, sm2start, sm3start, + sm2buf[0], sm2buf[1], sm2buf[2], sm2buf[3], sm2buf[4], sm2buf[5], sm2buf[6], sm2buf[7], + rb[0], rb[1], rb[2], rb[3], + wres.is_ok(), + tx_latest[0] + ); + } + println!( + "S tms={} fprd={} fresh={} req={} rep={} fpwr={} ferr={} ovf={} hop_avg={}us j2cx_avg={}us cx2j_avg={}us", + start.elapsed().as_millis(), + n_fprd, + n_fresh, + n_req, + n_rep, + n_fpwr, + n_fpwr_err, + n_ovf, + if n_rep > 0 { hop_st.mean() / 1000.0 } else { 0.0 }, + if n_rep > 0 { j2cx_st.mean() / 1000.0 } else { 0.0 }, + if n_fresh > 0 { cx2j_st.mean() / 1000.0 } else { 0.0 }, + ); + last_report = Instant::now(); + } + { let n = Instant::now(); seg_rpt_us = n.duration_since(cursor).as_micros() as u64; cursor = n; } + let _ = cursor; + } + + // drain + if let Some(f) = pending_fprd.take() { + let _ = f.await; + } + if let Some(f) = pending_fpwr.take() { + let _ = f.await; + } + + println!("\n=== rfm-async done: {:.1}s ===", start.elapsed().as_secs_f64()); + 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(()) +} +