From 3720737ab3083139bad72e68e2167d8a5c741f4a Mon Sep 17 00:00:00 2001 From: Tony Cao Date: Tue, 3 Nov 2026 17:48:19 +0800 Subject: [PATCH] feat(el6695): deploy 1400B PDO with LRW and frame diagnostics --- src/main.rs | 243 +++++++++++++++++++++++++++++++++++++++++++--------- src/rfm.rs | 2 +- 2 files changed, 203 insertions(+), 42 deletions(-) diff --git a/src/main.rs b/src/main.rs index 8f26923..009dedb 100644 --- a/src/main.rs +++ b/src/main.rs @@ -26,21 +26,21 @@ use ethercrab::{ use stats::OnlineStats; const MAX_SUBDEVICES: usize = 16; -const MAX_PDU_DATA: usize = 1100; +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 = 800; +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 = 800; // SM3 buffer (no diag word, pure 8B payload) -const SM_RX_PLD_LEN: usize = 800; // actual payload after stripping diagnostic word +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 = 800; +const PAYLOAD_LEN: usize = 1400; /// Abort after this many consecutive cycles with a TX or RX error. const MAX_ERR_STREAK: u64 = 1000; @@ -76,6 +76,9 @@ enum Mode { /// 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, } @@ -188,6 +191,7 @@ fn parse_args() -> Result { "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)), } @@ -285,6 +289,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { Mode::RfmBridge => "rfm-bridge", Mode::RfmRing => "rfm-ring", Mode::RfmAsync => "rfm-async", + Mode::RfmLrw => "rfm-lrw", Mode::RevProbe => "revprobe", }, cfg.duration_sec, @@ -359,6 +364,8 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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) { @@ -401,12 +408,13 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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={} otherAddr={}) FPRD={}", n_fpwr, n_sm2, n_sm2_wkc1, n_sm2_wkc0, n_other_addr, n_fprd); + 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(()); } @@ -876,6 +884,9 @@ async fn run(cfg: &Config) -> Result<(), Box> { let _ = ethercrab::Command::fpwr(station, regs::sm_base(3) + regs::SM_OFF_ENABLE) .send(&maindevice, 0x01u8).await; bridge::write_fmmus_secondary(&maindevice, station, (PAYLOAD_LEN + sm3_prefix) as u16, PAYLOAD_LEN as u16).await; + let sm2_len_rb = ethercrab::Command::fprd(station, regs::sm_base(2) + regs::SM_OFF_LEN).receive::(&maindevice).await; + let sm3_len_rb = ethercrab::Command::fprd(station, regs::sm_base(3) + regs::SM_OFF_LEN).receive::(&maindevice).await; + println!(" SM LEN readback: SM2={:?} SM3={:?}", sm2_len_rb, sm3_len_rb); } } @@ -1551,7 +1562,8 @@ async fn run(cfg: &Config) -> Result<(), Box> { Mode::Sniff => "sniff (raw frame diagnostics)", Mode::RfmBridge => "rfm-bridge (EL6695 <-> RFM <-> remote)", Mode::RfmAsync => "rfm-async (event-driven)", - Mode::RfmPing | Mode::RfmEcho | Mode::RfmRing | Mode::RevProbe => unreachable!(), + Mode::RfmLrw => "rfm-lrw (single-frame read+write)", + Mode::RfmPing | Mode::RfmEcho | Mode::RfmRing | Mode::RevProbe | Mode::RfmLrw => unreachable!(), } ); let start = Instant::now(); @@ -1610,7 +1622,7 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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 mut rfm_ctx: Option = if matches!(cfg.mode, Mode::RfmBridge | Mode::RfmAsync | Mode::RfmLrw) { let r = el6695_rt::rfm::Rfm::open_path(cfg.rfm_dev.as_deref())?; println!( "rfm-bridge: BAR2 reflective mem = {} MB, node_id={}", @@ -1632,6 +1644,12 @@ async fn run(cfg: &Config) -> Result<(), Box> { 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); @@ -2386,6 +2404,154 @@ fn print_stats(s: &OnlineStats) { // Fresh CX data is written to the RFM REQ mailbox immediately after each // FPRD completes; fresh REP data is FPWR'd to SM2 immediately on detection // (async pipelined, latest-wins if the wire is busy). +/// rfm-lrw: one LRW frame per cycle does SM2 write + SM3 read. +/// The FMMU maps logical [0..PAYLOAD]=SM2(write) and [PAYLOAD..2P]=SM3(read), +/// so a single frame RTT covers both directions. No concurrent PDU futures. +async fn rfm_lrw_loop( + maindevice: ðercrab::MainDevice<'_>, + station: u16, + mut rfm: el6695_rt::rfm::Rfm, + duration_sec: u64, +) -> Result<(), Box> { + use el6695_rt::rfm::*; + println!("rfm-lrw: single-frame read+write per cycle (no tick, Ctrl-C to stop)"); + { + let f0 = ethercrab::Command::fprd(station, 0x0600u16).receive_slice(maindevice, 16).await; + let f1 = ethercrab::Command::fprd(station, 0x0610u16).receive_slice(maindevice, 16).await; + match (&f0, &f1) { + (Ok(p0), Ok(p1)) => { + let r0: &[u8] = p0; let r1: &[u8] = p1; + let len0 = u16::from_le_bytes([r0[4], r0[5]]); + let len1 = u16::from_le_bytes([r1[4], r1[5]]); + let en0 = r0[11]; let en1 = r1[11]; + println!(" FMMU0: len={} enable={} | FMMU1: len={} enable={}", len0, en0, len1, en1); + } + _ => println!(" FMMU readback failed"), + } + let f0f = ethercrab::Command::fprd(station, 0x0600u16).receive_slice(maindevice, 16).await; + let f1f = ethercrab::Command::fprd(station, 0x0610u16).receive_slice(maindevice, 16).await; + if let Ok(p) = &f0f { let r: &[u8] = p; println!(" FMMU0 raw: {:02X?}", &r[..16]); } + if let Ok(p) = &f1f { let r: &[u8] = p; println!(" FMMU1 raw: {:02X?}", &r[..16]); } + } + let mem_ptr = rfm.mem.as_mut_ptr(); + unsafe { + let zero = [0u8; 32]; + dev_write(mem_ptr.add(MB_REQ_OFF), &zero); + dev_write(mem_ptr.add(MB_REP_OFF), &zero); + barrier(); + } + let start = Instant::now(); + let mut prev_rx8 = [0u8; 8]; + let mut last_rep_seq: u64 = 0; + let mut req_seq: u64 = 0; + let mut tx_latest = vec![0u8; PAYLOAD_LEN]; + let mut frame = vec![0u8; 1344usize]; + let (mut n_iter, mut n_fresh, mut n_req, mut n_rep, mut n_err) = (0u64, 0u64, 0u64, 0u64, 0u64); + let (mut lrw_sum, mut hop_sum, mut cx2j_sum) = (0u64, 0u64, 0u64); + let mut lrw_st = OnlineStats::new("lrw_rtt", 100, 0, 20_000_000); + let mut hop_st = OnlineStats::new("rfm_hop_plus_echo", 100, 0, 20_000_000); + let mut cx2j_st = OnlineStats::new("fresh_to_req", 100, 0, 20_000_000); + let mut last_report = Instant::now(); + while !STOP.load(Ordering::Relaxed) { + if duration_sec > 0 && start.elapsed().as_secs() >= duration_sec { + break; + } + n_iter += 1; + let pace_t0 = Instant::now(); + // 1. poll REP mailbox -> update tx_latest (apply the single-point transform) + let (magic, rseq, rts) = unsafe { + let rep = Mailbox::at(mem_ptr, MB_REP_OFF); + barrier(); + (rep.magic, rep.seq, rep.ts_ns) + }; + if magic == MB_MAGIC && rseq != last_rep_seq { + last_rep_seq = rseq; + let n = unsafe { + let rep = Mailbox::at(mem_ptr, MB_REP_OFF); + (rep.len as usize).min(PAYLOAD_LEN).min(MB_MAX_PAYLOAD) + }; + unsafe { + let rep = Mailbox::at(mem_ptr, MB_REP_OFF); + dev_read(&mut tx_latest[..n], rep.payload.as_ptr()); + } + if n >= 8 { + let rx_seq = u64::from_le_bytes(tx_latest[..8].try_into().unwrap()); + let processed = rx_seq.wrapping_mul(7).wrapping_add(3); + tx_latest[..8].copy_from_slice(&processed.to_le_bytes()); + } + n_rep += 1; + hop_sum += now_ns().saturating_sub(rts); + hop_st.push(now_ns().saturating_sub(rts) as i64); + } + // 2. single LRW: [0..P] write SM2 (latest REP), [P..2P] read SM3 (fresh CX data) + frame[..672].copy_from_slice(&tx_latest[..672]); + let t0 = Instant::now(); + match ethercrab::Command::lrw(0) + .ignore_wkc() + .send_receive_slice(maindevice, &frame[..]) + .await + { + Ok(pdu) => { + let raw: &[u8] = &pdu; + let off = 672usize; + lrw_st.push((t0.elapsed().as_micros() * 1000) as i64); + lrw_sum += t0.elapsed().as_micros() as u64; + if raw.len() >= off + 672 { + let cur: [u8; 8] = raw[off..off + 8].try_into().unwrap(); + if cur != prev_rx8 { + prev_rx8 = cur; + n_fresh += 1; + let t1 = Instant::now(); + let n = 672usize.min(MB_MAX_PAYLOAD); + req_seq = req_seq.wrapping_add(1); + unsafe { + let mb = Mailbox::at(mem_ptr, MB_REQ_OFF); + dev_write(mb.payload.as_mut_ptr(), &raw[off..off + n]); + mb.len = n as u32; + mb.ts_ns = now_ns(); + barrier(); + mb.seq = req_seq; + barrier(); + mb.magic = MB_MAGIC; + } + n_req += 1; + cx2j_sum += t1.elapsed().as_nanos() as u64; + cx2j_st.push(t1.elapsed().as_nanos() as i64); + } + } + } + Err(e) => { + n_err += 1; + if n_err <= 8 { println!(" LRW ERR n={}: {:?}", n_err, e); } + } + } + if n_iter % 2000 == 0 { + println!(" TXDBG tx[0..16]={:02X?}", &tx_latest[..16]); + } + if last_report.elapsed() >= Duration::from_secs(1) { + last_report = Instant::now(); + println!( + "S tms={} iter={} fresh={} req={} rep={} err={} lrw_avg={}us hop_avg={}us cx2j_avg={}us", + start.elapsed().as_millis(), + n_iter, + n_fresh, + n_req, + n_rep, + n_err, + if n_iter > 0 { lrw_sum / n_iter } else { 0 }, + if n_rep > 0 { hop_sum / n_rep / 1000 } else { 0 }, + if n_req > 0 { cx2j_sum / n_req / 1000 } else { 0 }, + ); + } + } + println!("\n=== rfm-lrw done: {:.1}s ===", start.elapsed().as_secs_f64()); + println!("iter={} fresh={} req={} rep={} err={}", n_iter, n_fresh, n_req, n_rep, n_err); + print_stats(&lrw_st); + print_stats(&hop_st); + print_stats(&cx2j_st); + Ok(()) +} + async fn rfm_async_loop( maindevice: ðercrab::MainDevice<'_>, station: u16, @@ -2462,6 +2628,7 @@ async fn rfm_async_loop( let mut wphase_st = OnlineStats::new("write_phase_us", 200, -1_500_000, 1_500_000); let mut j2cx_pending: Option = None; let mut last_f640_print: u64 = 0; + let mut rep_new = false; while !STOP.load(Ordering::Relaxed) { if duration_sec > 0 && start.elapsed().as_secs() >= duration_sec { @@ -2523,7 +2690,7 @@ async fn rfm_async_loop( // 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) + && last_fprd_done.map_or(true, |p| p.elapsed().as_micros() >= 350) { let fut = async move { ethercrab::Command::fprd(station, SM_RX_ADDR) @@ -2605,38 +2772,7 @@ async fn rfm_async_loop( 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; - if false && dc_discipline && phase_est != 0 { - let now_w = rt::now_ns() as i64; - let mut target = now_w - phase_est + 1_000_000 + 500_000; - while target < now_w + 50_000 { target += 1_000_000; } - while (rt::now_ns() as i64) < target { core::hint::spin_loop(); } - wphase_st.push(((rt::now_ns() % 1_000_000) / 1000) as i64); - } - 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 - } + 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; } @@ -2709,6 +2845,31 @@ async fn rfm_async_loop( let c = sd.sdo_read::(0xF640, 1).await; println!(" F640CNT n={} count={:?}", n_fpwr, c); } + // 5.5 serial write when wire is free (fprd completed this iteration) + if rep_new && pending_fprd.is_none() { + rep_new = false; + let p = if tx_slot_toggle { + tx_slot_b.as_mut_ptr() + } else { + tx_slot_a.as_mut_ptr() + }; + tx_slot_toggle = !tx_slot_toggle; + let slot: &'static mut [u8] = + unsafe { std::slice::from_raw_parts_mut(p, PAYLOAD_LEN) }; + slot.copy_from_slice(&tx_latest[..PAYLOAD_LEN]); + tx_inflight_seq = tx_latest_seq; + let t_w = Instant::now(); + let tx_ok = ethercrab::Command::fpwr(station, SM_TX_ADDR) + .send(maindevice, &slot[..PAYLOAD_LEN]) + .await + .is_ok(); + n_fpwr += 1; + if tx_ok { + j2cx_st.push(t_w.elapsed().as_nanos() as i64); + } else { + n_fpwr_err += 1; + } + } // 6. reap FPWR completion if let Some(f) = pending_fpwr.as_mut() { if let Some(res) = futures_lite::future::poll_once(f.as_mut()).await { diff --git a/src/rfm.rs b/src/rfm.rs index 3001c3d..50733ab 100644 --- a/src/rfm.rs +++ b/src/rfm.rs @@ -290,7 +290,7 @@ use std::os::unix::io::FromRawFd; pub const MB_REQ_OFF: usize = 0x0000; pub const MB_REP_OFF: usize = 0x1000; pub const MB_MAGIC: u32 = 0x5246_4D32; // "RFM2" -pub const MB_MAX_PAYLOAD: usize = 1024; +pub const MB_MAX_PAYLOAD: usize = 1400; #[repr(C)] pub struct Mailbox {