tonycao/ethercat-linux
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(el6695): deploy 1400B PDO with LRW and frame diagnostics

Browse Source
This commit is contained in:
Tony Cao 2026-11-03 17:48:19 +08:00
parent 010fef0b5d
commit 3720737ab3
2 changed files with 203 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

243
src/main.rs
View File

@ -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<Config, String> {
"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<dyn std::error::Error>> {
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<dyn std::error::Error>> {
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<dyn std::error::Error>> {
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<dyn std::error::Error>> {
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);
}
}
@ -1551,7 +1562,8 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
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<dyn std::error::Error>> {
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) {
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={}",
@ -1632,6 +1644,12 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
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: &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,
@ -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<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 {
@ -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::<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 {

2
src/rfm.rs
View File

@ -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 {