EL6695 secondary side: SAFEOP/OP bring-up + 1kHz data loop

- Fix SM ctrl bytes for SM2 (0x64) and SM3 (0x20) per ESI
- Add SM2/SM3 register config + FMMU + PDO assignment via SDO
- Change SM_TX_ADDR/RX_ADDR to SM2(0x1C00)/SM3(0x8E00)
- Add ensure_od_secondary for PDO assignment
- Add secondary-side diagnostics (CoE object probe)
- Update SM2/SM3 enable/disable sequence for clean config
- Add full bring-up sequence: INIT->PREOP->SAFEOP->OP

docs/verification-progress.md

@@ -0,0 +1,132 @@
+# EL6695 1 kHz 通讯验证实施进度
+
+- 创建日期：2026-07-22
+- 当前状态：**TwinCAT 待切回 Run，PLC 波形逻辑已部署**
+- 关联文档：`docs/scope-waveform-design.md`
+
+---
+
+## 1. 目标
+
+在 TwinCAT Scope View 中直观观测 J1900 ↔ TwinCAT（经 EL6695 桥）1 kHz 通讯的周期与抖动，并验证其稳定性。
+
+---
+
+## 2. 已完成
+
+### 2.1 J1900 侧（`src/main.rs`）
+
+- 在回显帧 `tx_buf` 的 **[264..272]** 追加诊断字段：
+  - `tx_buf[264..268]`：`jitter_ns`（i32 LE）—— el6695_rt 内部 PLL/唤醒抖动
+  - `tx_buf[268..272]`：`last_phase_err_ns`（i32 LE）—— 与 TwinCAT DC 网格的相位误差
+- 已在 J1900 上构建并部署到 `/root/gateway/el6695_rt`
+- 协议兼容性：诊断区位于图案校验区 [16..255] 之后、时间戳 [256..263] 之后，不触发 PLC 的 `nMismatch`
+
+### 2.2 TwinCAT PLC 侧（`free/TwinCATProject1/PlcSync`）
+
+`GVL_Sync.TcGVL` 新增变量：
+
+| 变量 | 类型 | 含义 |
+|---|---|---|
+| `nRttUs` | `DINT` | 端到端往返时间（µs） |
+| `nInSeqDelta` | `DINT` | `nReturnedSeq` 周期差分 |
+| `nJ1900Delta` | `DINT` | `nJ1900Cycle` 周期差分 |
+| `nJ1900JitterNs` | `DINT` | J1900 自测抖动（ns） |
+| `nJ1900PhaseErrNs` | `DINT` | J1900 相位误差（ns） |
+| `nPrevRetSeq` | `ULINT` | 差分寄存器 |
+| `nPrevJ1900` | `ULINT` | 差分寄存器 |
+
+`MAIN.TcPOU` 末尾追加：
+
+- 通道 1：`nRttUs = (nEchoTimestamp - nSendTimestamp) / 10`
+- 通道 2/3：两个序列号的周期差分
+- 通道 4/5：两条 `MEMCPY` 从 `aIn1[264..271]` 拷贝到 DINT
+
+### 2.3 TwinCAT 工程激活
+
+- 运行 `free/rt-activate2.ps1`
+- `ActivateConfiguration()` 返回成功
+- 但后续系统进入 **Config** 模式，未自动进入 Run
+
+---
+
+## 3. 当前阻塞
+
+**TwinCAT 系统服务停留在 Config 模式，PLC 端口 851 未启动。**
+
+验证命令：
+
+```powershell
+powershell -ExecutionPolicy Bypass -File C:\Users\tonycao\work\free\ads-state.ps1
+```
+
+输出：
+
+```text
+port10000: ads=Config device=1
+port851 err: Target port could not be found. (AdsErrorCode: 6, 0x6)
+```
+
+已尝试但未成功的方法：
+
+- `ads-set-run.ps1`：`WriteControl` 重载问题失败
+- 直接 `TcAdsClient.WriteControl([AdsState]::Run)`：状态仍为 Config
+- `rt-activate2.ps1` 中的 `StartRestartTwinCAT()`：进入 Config 后未切 Run
+
+---
+
+## 4. 下一步
+
+1. **切 TwinCAT 到 Run 模式**
+   - 首选：`free/switch-to-run.ps1`（通过 DTE/COM 打开工程后调用 `StartRestartTwinCAT` 并轮询）
+   - 备选：`free/activate-and-run.ps1`（完整流程：ActivateConfiguration + StartRestartTwinCAT + 轮询）
+   - 最坏情况：若 ADS/DTE 均无法切 Run，考虑重启 Windows 后再试
+
+2. **确认端口 851 可达**
+   - `ads-state.ps1` 应输出 `port10000: ads=Run` 且 `port851: ads=Run`
+
+3. **启动 J1900 侧**
+
+   ```bash
+   ssh root@<j1900-ip> /root/gateway/el6695_rt eth1 --mode follow --duration 120
+   ```
+
+4. **运行波形快照验证**
+
+   ```powershell
+   powershell -ExecutionPolicy Bypass -File C:\Users\tonycao\work\free\ads-waveform-snapshot.ps1
+   ```
+
+   预期值：
+
+   | 字段 | 预期 |
+   |---|---|
+   | `nRttUs` | ≈ 4000 µs |
+   | `nInSeqDelta` | 1 |
+   | `nJ1900Delta` | 1 |
+   | `nJ1900JitterNs` | < 1 µs（follow 模式锁定后） |
+   | `nJ1900PhaseErrNs` | 稳定在某一直流值（约 165 µs） |
+
+5. **在 TwinCAT Scope View 中添加 YT Chart**
+   - 数据源：ADS 符号
+   - 通道：上述 5 个变量
+   - 采样率：挂到 PLC 任务 1 kHz
+
+6. **提交 git**
+   - 当前 `src/main.rs` 有未提交修改
+   - 提交信息：`feat: add jitter/phase_err diagnostics for TwinCAT Scope waveform`
+
+---
+
+## 5. 相关文件
+
+- `C:/Users/tonycao/work/ethercat-linux/src/main.rs`
+- `C:/Users/tonycao/work/ethercat-linux/docs/scope-waveform-design.md`
+- `C:/Users/tonycao/work/ethercat-linux/docs/verification-progress.md`
+- `C:/Users/tonycao/work/free/TwinCATProject1/PlcSync/GVLs/GVL_Sync.TcGVL`
+- `C:/Users/tonycao/work/free/TwinCATProject1/PlcSync/POUs/MAIN.TcPOU`
+- `C:/Users/tonycao/work/free/rt-activate2.ps1`
+- `C:/Users/tonycao/work/free/ads-state.ps1`
+- `C:/Users/tonycao/work/free/ads-waveform-snapshot.ps1`
+- `C:/Users/tonycao/work/free/switch-to-run.ps1`
+- `C:/Users/tonycao/work/free/activate-and-run.ps1`

src/bridge.rs

@@ -24,7 +24,7 @@ use crate::regs;
 /// TwinCAT line-format 35-entry blob: first entry is the real object at
 /// 240 bit, then 33 continuation entries at 240 bit, final entry 32 bit.
 /// Entry format = [len_bits: u8, sub: u8, idx_lo: u8, idx_hi: u8].
-fn twin_layout_blob(obj: u16) -> Vec<u8> {
+pub fn twin_layout_blob(obj: u16) -> Vec<u8> {
     let mut blob = Vec::with_capacity(2 + 35 * 4);
     blob.extend_from_slice(&35u16.to_le_bytes());
     blob.extend_from_slice(&[240, 1, (obj & 0xFF) as u8, (obj >> 8) as u8]);
@@ -36,7 +36,7 @@ fn twin_layout_blob(obj: u16) -> Vec<u8> {
 }
 
 /// SM assign blob: entry count (u16 LE) + mapped object indices.
-fn assign_blob(objs: &[u16]) -> Vec<u8> {
+pub fn assign_blob(objs: &[u16]) -> Vec<u8> {
     let mut blob = Vec::with_capacity(2 + objs.len() * 2);
     blob.extend_from_slice(&(objs.len() as u16).to_le_bytes());
     for &o in objs {
@@ -203,18 +203,109 @@ where
     Ok(())
 }
 
-/// Raw SM2/SM3 register config (start/len/ctrl/enable per the ESI).
+/// Raw SM0/SM1 register config for the secondary side (probed from EEPROM).
 ///
-/// Start/length/ctrl are complete configuration values written wholesale;
+/// SM0: outputs  (J1900 writes data to TwinCAT), start=0x1000, len=1024, ctrl=0x26
-/// the enable byte is applied read-modify-write so unrelated bits survive.
+/// SM1: inputs   (J1900 reads data from TwinCAT),  start=0x1600, len=1024, ctrl=0x22
+///
+/// Must disable SM before writing ctrl, otherwise ESC ignores the change.
+pub async fn write_sms_secondary(md: &MainDevice<'_>, station: u16) -> Result<(), String> {
+    use crate::regs::*;
+    fn w<E: std::fmt::Debug>(r: Result<(), E>, label: &str) -> Result<(), String> {
+        r.map_err(|e| format!("{}: {:?}", label, e))
+    }
+    // Disable SM0, set ctrl, re-enable
+    w(Command::fpwr(station, sm_base(0) + SM_OFF_ENABLE)
+        .send(md, 0u8).await, "SM0 disable")?;
+    w(Command::fpwr(station, sm_base(0) + SM_OFF_CTRL)
+        .send(md, SM_CTRL_OUTPUTS_SEC).await, "SM0 ctrl")?;
+    w(Command::fpwr(station, sm_base(0) + SM_OFF_START)
+        .send(md, 0x1000u16).await, "SM0 start")?;
+    w(Command::fpwr(station, sm_base(0) + SM_OFF_LEN)
+        .send(md, 1024u16).await, "SM0 len")?;
+    w(Command::fpwr(station, sm_base(0) + SM_OFF_ENABLE)
+        .send(md, SM_ENABLE).await, "SM0 enable")?;
+    // Disable SM1, set ctrl, re-enable
+    w(Command::fpwr(station, sm_base(1) + SM_OFF_ENABLE)
+        .send(md, 0u8).await, "SM1 disable")?;
+    w(Command::fpwr(station, sm_base(1) + SM_OFF_CTRL)
+        .send(md, SM_CTRL_INPUTS_SEC).await, "SM1 ctrl")?;
+    w(Command::fpwr(station, sm_base(1) + SM_OFF_START)
+        .send(md, 0x1600u16).await, "SM1 start")?;
+    w(Command::fpwr(station, sm_base(1) + SM_OFF_LEN)
+        .send(md, 1024u16).await, "SM1 len")?;
+    w(Command::fpwr(station, sm_base(1) + SM_OFF_ENABLE)
+        .send(md, SM_ENABLE).await, "SM1 enable")?;
+    Ok(())
+}
+
+/// Ensure the secondary-side PDO assignment (0x1C10 = SM0, 0x1C11 = SM1).
+pub async fn ensure_od_secondary<S>(
+    sd: &ethercrab::SubDeviceRef<'_, S>,
+    md: &MainDevice<'_>,
+) -> Result<(), String>
+where
+    S: std::ops::Deref<Target = ethercrab::SubDevice>,
+{
+    let _ = md; // used implicitly via sd
+    // On the secondary side the EEPROM should already configure the PDO
+    // assignment. If not, write the standard layout:
+    // 0x1C10 = [0x1608] (output PDO assigned to SM0)
+    // 0x1C11 = [0x1A08] (input PDO assigned to SM1)
+    let ok = sdo_ca(sd, 0x1C10, &assign_blob(&[0x1608])).await
+        && sdo_ca(sd, 0x1C11, &assign_blob(&[0x1A08])).await;
+    if !ok {
+        return Err("secondary PDO assignment write failed".into());
+    }
+    let rb0 = read_u8(sd, 0x1C10, 0).await;
+    let rb1 = read_u8(sd, 0x1C11, 0).await;
+    println!("  secondary PDO assignment: 0x1C10:00={} 0x1C11:00={}", rb0, rb1);
+    if rb0 != 1 || rb1 != 1 {
+        return Err("secondary PDO assignment readback mismatch".into());
+    }
+    Ok(())
+}
+
+/// Write FMMU entries for the secondary side.
+///
+/// FMMU0: SM0 outputs (J1900 writes data), logical 0→physical 0x1000, write_enable
+/// FMMU1: SM1 inputs (J1900 reads data),  logical 1024→physical 0x1600, read_enable
+pub async fn write_fmmus_secondary(md: &MainDevice<'_>, station: u16) {
+    // FMMU0: SM2 Outputs (master→slave, write_enable) per ESI
+    let out: [u8; 16] = [
+        0x00, 0x00, 0x00, 0x00, // logical_start=0
+        0x00, 0x04,             // length=1024
+        0x00,                   // logical_start_bit=0
+        0x07,                   // logical_end_bit=7
+        0x00, 0x1C,             // physical_start=0x1C00
+        0x00,                   // physical_start_bit=0
+        0x02,                   // read_enable=0, write_enable=1
+        0x01,                   // enable=1
+        0x00, 0x00, 0x00,       // padding
+    ];
+    let _ = Command::fpwr(station, 0x0600).send(md, out).await;
+    // FMMU1: SM3 Inputs (slave→master, read_enable) at 0x2000
+    let inp: [u8; 16] = [
+        0x00, 0x04, 0x00, 0x00, // logical_start=1024
+        0x00, 0x04,             // length=1024
+        0x00,                   // logical_start_bit=0
+        0x07,                   // logical_end_bit=7
+        0x00, 0x8E,             // physical_start=0x8E00
+        0x00,                   // physical_start_bit=0
+        0x01,                   // read_enable=1, write_enable=0
+        0x01,                   // enable=1
+        0x00, 0x00, 0x00,       // padding
+    ];
+    let _ = Command::fpwr(station, 0x0610).send(md, inp).await;
+}
+
+/// Raw SM2/SM3 register config (primary side, kept for reference).
 pub async fn write_sms(md: &MainDevice<'_>, station: u16, sm2_addr: u16, sm2_len: u16, sm3_addr: u16, sm3_len: u16) {
     use crate::regs::*;
-    // SM2: outputs (master -> slave), ESI ctrl byte
     let _ = Command::fpwr(station, sm_base(2) + SM_OFF_START).send(md, sm2_addr).await;
     let _ = Command::fpwr(station, sm_base(2) + SM_OFF_LEN).send(md, sm2_len).await;
     let _ = Command::fpwr(station, sm_base(2) + SM_OFF_CTRL).send(md, SM_CTRL_OUTPUTS_ESI).await;
     let _ = rmw_u8(md, station, sm_base(2) + SM_OFF_ENABLE, SM_ENABLE, 0).await;
-    // SM3: inputs (slave -> master), ESI ctrl byte
     let _ = Command::fpwr(station, sm_base(3) + SM_OFF_START).send(md, sm3_addr).await;
     let _ = Command::fpwr(station, sm_base(3) + SM_OFF_LEN).send(md, sm3_len).await;
     let _ = Command::fpwr(station, sm_base(3) + SM_OFF_CTRL).send(md, SM_CTRL_INPUTS_ESI).await;

src/main.rs

@@ -1,32 +1,18 @@
 //! J1900 <-> TwinCAT 1 kHz communication verification over an EL6695 bridge.
 //!
-//! Re-implementation of the reference project's `ecat_el6695_*` examples as a
+//! SECONDARY-SIDE build: J1900 talks to the EL6695 secondary port. The
-//! single binary with two pacing modes:
+//! 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.
 //!
-//! - `ticker`: free-running 1 kHz absolute-time ticker
+//! Two modes:
-//!   (`clock_nanosleep(TIMER_ABSTIME)`, drift-free grid), SM3 = [0x1A01 diag
+//! - `ticker` (default): echo TwinCAT's payload back on SM0 with the local
-//!   word (2B), 0x1A08 user data (1024B)].
+//!   cycle number stamped at bytes 8..16.
-//! - `follow` (default): software PLL locked to the TwinCAT DC clock via the
+//! - `probe`: read-only; dump the first bytes of SM1 every N cycles and do
-//!   EL6695 external timestamp in 0x1A02, SM3 = [0x1A02 (22B), 0x1A08 (1024B)].
+//!   NOT write to SM0 (avoids disturbing TwinCAT's view).
 //!
-//! Data plane per cycle: fprd SM3 -> echo payload back -> fpwr SM2, with the
+//! Graceful SIGINT/SIGTERM shutdown with full statistics.
-//! local cycle number stamped at bytes 8..16 (TwinCAT compares its own
-//! sequence in bytes 0..8 to compute round trips).
-//!
-//! Improvements over the reference implementation:
-//! - no duplicated bring-up code (single `bridge` module)
-//! - graceful SIGINT/SIGTERM shutdown with full statistics (long stability
-//!   runs can be stopped any time without losing results)
-//! - bounded-memory online statistics (histograms, no per-sample Vecs)
-//! - absolute-grid pacing in ticker mode (no cumulative drift from spin padding)
-//! - plausibility filter on the external timestamp (one bad sample can no
-//!   longer yank the PLL offset)
-//! - error-streak detection: aborts loudly instead of silently accumulating
-//!   rx/tx errors on a dead link
-//!
-//! Usage:
-//!   el6695_rt [iface] [--mode ticker|follow] [--duration SEC] [--phase-us N]
-//!             [--cycle-us N] [--core N] [--prio N] [--quiet]
 
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::time::{Duration, Instant};
@@ -43,27 +29,16 @@ const MAX_PDU_DATA: usize = 1100;
 const MAX_FRAMES: usize = 16;
 const PDI_LEN: usize = 3072;
 
-const SM2_ADDR: u16 = 0x1C00;
+// Secondary side SM layout (ESI-defined process data SMs)
-const SM2_LEN: usize = 1024;
+const SM_TX_ADDR: u16 = 0x1C00;  // SM2: outputs (J1900 writes data to TwinCAT)
-const SM3_ADDR: u16 = 0x8E00;
+const SM_TX_LEN: usize = 1024;
-const SM3_LEN_TICKER: usize = 1026; // 2B diag + 1024B user
+const SM_RX_ADDR: u16 = 0x8E00;  // SM3: inputs (J1900 reads data from TwinCAT)
-const SM3_LEN_FOLLOW: usize = 1046; // 22B timestamps + 1024B user
+const SM_RX_LEN: usize = 1024;
-const SM3_CAP: usize = SM3_LEN_FOLLOW;
 
 const PAYLOAD_LEN: usize = 1024;
 
-// SM3 offsets in follow mode (0x1A02 layout)
-const TS_EXT_OFF: usize = 10;
-const DATA_OFF_TICKER: usize = 2;
-const DATA_OFF_FOLLOW: usize = 22;
-
 /// Abort after this many consecutive cycles with a TX or RX error.
 const MAX_ERR_STREAK: u64 = 1000;
-/// Reject external timestamps that jump forward more than this from the last
-/// accepted sample (ns). Must be generous: the ext timestamp steps at the
-/// *secondary-side* bus cycle, which may be 10 ms or slower. Identical
-/// consecutive values are stale samples, not corruption.
-const TS_MAX_GAP_NS: u64 = 200_000_000;
 
 static STOP: AtomicBool = AtomicBool::new(false);
 static PDU_STORAGE: std::sync::OnceLock<PduStorage<MAX_FRAMES, MAX_PDU_DATA>> =
@@ -76,10 +51,6 @@ extern "C" fn on_signal(_sig: libc::c_int) {
 #[derive(Clone, Copy, PartialEq)]
 enum Mode {
     Ticker,
-    Follow,
-    /// Free-running 1 kHz pacing with the follow-mode SM3 layout (0x1A02
-    /// timestamps) but NO PLL feedback: dumps raw ext_ts phase so the
-    /// timestamp latch behaviour can be observed without the loop reacting.
     Probe,
 }
 
@@ -87,12 +58,10 @@ struct Config {
     iface: String,
     mode: Mode,
     duration_sec: u64,
-    phase_us: u64,
     cycle_us: u64,
     core: usize,
     prio: u8,
     quiet: bool,
-    debug_ts: u32,
     /// Toggle an EL2202 digital output every cycle (oscilloscope jitter
     /// waveform). Off by default: the outputs may drive real actuators.
     el2202: bool,
@@ -114,14 +83,12 @@ struct Config {
 fn parse_args() -> Result<Config, String> {
     let mut cfg = Config {
         iface: "eth1".into(),
-        mode: Mode::Follow,
+        mode: Mode::Ticker,
         duration_sec: 60,
-        phase_us: 500,
         cycle_us: 1000,
         core: 2,
         prio: 80,
         quiet: false,
-        debug_ts: 0,
         el2202: false,
         el2202_dual: false,
         el1252: false,
@@ -129,7 +96,6 @@ fn parse_args() -> Result<Config, String> {
         el1252_every: 1,
     };
     let mut args = std::env::args().skip(1).peekable();
-    // Optional positional iface (anything not starting with '--').
     if let Some(a) = args.peek() {
         if !a.starts_with("--") {
             cfg.iface = args.next().unwrap();
@@ -143,7 +109,6 @@ fn parse_args() -> Result<Config, String> {
             "--mode" => {
                 cfg.mode = match val("--mode")?.as_str() {
                     "ticker" => Mode::Ticker,
-                    "follow" => Mode::Follow,
                     "probe" => Mode::Probe,
                     other => return Err(format!("unknown mode '{}'", other)),
                 }
@@ -153,7 +118,6 @@ fn parse_args() -> Result<Config, String> {
                     .parse()
                     .map_err(|_| "bad --duration")?
             }
-            "--phase-us" => cfg.phase_us = val("--phase-us")?.parse().map_err(|_| "bad --phase-us")?,
             "--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")?,
@@ -168,9 +132,6 @@ fn parse_args() -> Result<Config, String> {
             "--el1252-every" => {
                 cfg.el1252_every = val("--el1252-every")?.parse().map_err(|_| "bad --el1252-every")?
             }
-            "--debug-ts" => {
-                cfg.debug_ts = val("--debug-ts")?.parse().map_err(|_| "bad --debug-ts")?
-            }
             other => return Err(format!("unknown argument '{}'", other)),
         }
     }
@@ -180,135 +141,23 @@ fn parse_args() -> Result<Config, String> {
     Ok(cfg)
 }
 
-/// Software PLL locking the local wake point to the TwinCAT DC 1 ms grid
-/// plus a phase offset (`floor(ext/cycle)+cycle+phase`), with slew-rate
-/// limiting on the anchor.
-///
-/// Why slew limiting: the ext timestamp proves to be a *feedback* signal —
-/// TwinCAT's send phase couples to the timing of our frames, so a deadbeat
-/// anchor (`next = floor(ext)+cycle+phase`, as in the reference
-/// implementation) forms a delayed feedback loop with multiple periodic
-/// orbits. Observed on hardware: depending on the (slowly drifting) phase
-/// relationship, the loop either locks cleanly or falls into a period-2
-/// hunt (cycle times alternating ~0.4/1.6 ms, phase error bimodal over
-/// >1 ms). The reference binary merely happens to sit in a stable orbit for
-/// its particular timing. Limiting anchor movement to +-SLEW per cycle
-/// damps the loop: a stale/fresh flip can no longer yank the deadline by
-/// +-1 ms, while the slew (50 us/cycle = 50 ms/s) still tracks clock drift
-/// (~155 ppm = ns/s scale) with huge margin.
-///
-/// Offset tracking: a fresher (smaller) offset sample takes effect
-/// immediately; otherwise the offset creeps +200 ns/cycle to absorb clock
-/// drift between the two sides.
-struct Follow {
-    offset_ns: i64,
-    next_target_tc: u64,
-    primed: bool,
-    cycle_ns: u64,
-    phase_ns: u64,
-    last_wait_ns: i64,
-    /// Consecutive cycles with the anchor error pinned at a clamp bound.
-    stuck: u32,
-}
-
-/// Max anchor movement per cycle beyond the nominal 1 ms advance.
-const ANCHOR_SLEW_NS: i64 = 50_000;
-
-impl Follow {
-    fn new(cycle_ns: u64, phase_ns: u64) -> Self {
-        Self {
-            offset_ns: 0,
-            next_target_tc: 0,
-            primed: false,
-            cycle_ns,
-            phase_ns,
-            last_wait_ns: 0,
-            stuck: 0,
-        }
-    }
-
-    fn update(&mut self, ext_ts: u64, local_read_ns: u64) {
-        let off = local_read_ns as i64 - ext_ts as i64;
-        let raw = ((ext_ts / self.cycle_ns) + 1) * self.cycle_ns + self.phase_ns;
-        if !self.primed {
-            self.offset_ns = off;
-            self.next_target_tc = raw;
-            self.primed = true;
-            self.stuck = 0;
-            return;
-        }
-        if off < self.offset_ns {
-            self.offset_ns = off;
-        } else {
-            self.offset_ns += 200;
-        }
-        // Slew-limited tracking of the raw grid anchor.
-        let predicted = self.next_target_tc + self.cycle_ns;
-        let err = raw as i64 - predicted as i64;
-        let clamped = err.clamp(-ANCHOR_SLEW_NS, ANCHOR_SLEW_NS);
-        self.next_target_tc = (predicted as i64 + clamped) as u64;
-        // The raw signal is bistable: reading the SM3 buffer just before the
-        // bridge forwards a fresh frame yields an ext one grid cell older
-        // than reading just after, and each regime self-reinforces under
-        // slew limiting. If the error has been pinned at the same clamp
-        // bound for many cycles we are trapped in the wrong regime: snap
-        // directly to the raw anchor (one bounded jump) to escape.
-        if clamped == err {
-            self.stuck = 0;
-        } else {
-            self.stuck += 1;
-            if self.stuck > 100 {
-                self.next_target_tc = raw;
-                self.stuck = 0;
-            }
-        }
-    }
-
-    /// Current boundary's target read point in local time (for phase-error
-    /// accounting after a read).
-    fn current_target_local(&self, ext_ts: u64) -> u64 {
-        let boundary = (ext_ts / self.cycle_ns) * self.cycle_ns + self.phase_ns;
-        (boundary as i64 + self.offset_ns) as u64
-    }
-
-    fn wait_next(&mut self) -> i64 {
-        if !self.primed {
-            std::thread::sleep(Duration::from_nanos(self.cycle_ns));
-            return 0;
-        }
-        let local_deadline = (self.next_target_tc as i64 + self.offset_ns) as u64;
-        let now = rt::now_ns();
-        self.last_wait_ns = local_deadline as i64 - now as i64;
-        if self.last_wait_ns < -2 * self.cycle_ns as i64 {
-            // Target is hopelessly stale: re-prime so the next update snaps
-            // directly to the raw anchor (also discards a poisoned offset).
-            self.primed = false;
-            std::thread::sleep(Duration::from_nanos(self.cycle_ns));
-            return 0;
-        }
-        rt::hybrid_sleep_until(local_deadline, 100_000);
-        rt::now_ns() as i64 - local_deadline as i64
-    }
-}
-
 fn main() {
     let cfg = match parse_args() {
         Ok(c) => c,
         Err(e) => {
             eprintln!("error: {}", e);
             eprintln!(
-                "usage: el6695_rt [iface] [--mode ticker|follow|probe] [--duration SEC] \
+                "usage: el6695_rt [iface] [--mode ticker|probe] [--duration SEC] \
-                 [--phase-us N] [--cycle-us N] [--core N] [--prio N] [--quiet] [--el2202]"
+                 [--cycle-us N] [--core N] [--prio N] [--quiet] [--el2202]"
             );
             std::process::exit(2);
         }
     };
 
-    // Graceful shutdown: finish the current cycle, print full statistics.
     unsafe {
         let mut sa: libc::sigaction = core::mem::zeroed();
         sa.sa_sigaction = on_signal as usize;
-        sa.sa_flags = 0; // no SA_RESTART: blocking sleeps return EINTR, we retry
+        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());
@@ -325,30 +174,22 @@ fn main() {
 }
 
 async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
-    let follow = cfg.mode == Mode::Follow;
-    // SM3 layout with 0x1A02 timestamps (follow and probe) vs 0x1A01 diag.
-    let ts_layout = cfg.mode != Mode::Ticker;
     let cycle_ns = cfg.cycle_us * 1000;
-    let sm3_len = if ts_layout { SM3_LEN_FOLLOW } else { SM3_LEN_TICKER };
-    let data_off = if ts_layout { DATA_OFF_FOLLOW } else { DATA_OFF_TICKER };
 
-    println!("el6695-rt: J1900 <-> TwinCAT 1 kHz verification (EL6695 bridge)");
+    println!("el6695-rt: J1900 <-> TwinCAT 1 kHz verification (EL6695 bridge, secondary side)");
     println!(
-        "  iface={} mode={} duration={}s cycle={}us phase={}us core={} prio={}",
+        "  iface={} mode={} duration={}s cycle={}us core={} prio={}",
         cfg.iface,
         match cfg.mode {
-            Mode::Follow => "follow",
             Mode::Ticker => "ticker",
             Mode::Probe => "probe",
         },
         cfg.duration_sec,
         cfg.cycle_us,
-        cfg.phase_us,
         cfg.core,
         cfg.prio
     );
 
-    // ---- EtherCAT master init ----
     let pdu_storage = PDU_STORAGE.get_or_init(PduStorage::new);
     let (tx, rx, pdu_loop) = pdu_storage.try_split().map_err(|_| "PDU storage already in use")?;
 
@@ -388,47 +229,150 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
     let station = sd.configured_address();
     println!("EL6695 station address: 0x{:04X}", station);
 
-    // Follow/probe: enable external sync forwarding (0x10F5:01=1), best effort.
+    // ---- Probe SM registers ----
-    if ts_layout {
+    async fn read_sm(md: &MainDevice<'_>, st: u16, n: u8) -> (u16, u16, u8, u8) {
-        match sd.sdo_write_complete(0x10F5, 1, &[1u8]).await {
+        let start = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_START).receive::<u16>(md).await.unwrap_or(0xFFFF);
-            Ok(_) => println!("0x10F5:01 external sync enabled"),
+        let len = ethercrab::Command::fprd(st, regs::sm_base(n as u16) + regs::SM_OFF_LEN).receive::<u16>(md).await.unwrap_or(0xFFFF);
-            Err(e) => println!("0x10F5:01 write failed (continue anyway): {:?}", e),
+        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),
         }
     }
 
-    // ---- OD self-check / self-config ----
+    // ---- Configure SM2/SM3 for process data (SM0/SM1 are EEPROM-fixed mailbox) ----
-    let txpdo_first: u16 = if ts_layout { 0x1A02 } else { 0x1A01 };
+    // 0x1C12/0x1C13 exist (empty), 0x1C10/0x1C11 do not.
-    bridge::ensure_od(&sd, &maindevice, station, txpdo_first)
+    // 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
-        .map_err(|e| -> Box<dyn std::error::Error> { e.into() })?;
+    {
+        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),
+    }
     
-    // ---- Raw SM config + AL state machine (no SDO beyond this point) ----
+    // Remove SM2/SM3 config (was causing SAFEOP 0x001D)
-    bridge::write_sms(&maindevice, station, SM2_ADDR, SM2_LEN as u16, SM3_ADDR, sm3_len as u16).await;
+    // 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;
     
-    println!("Requesting SAFEOP...");
+    // ---- Reconfigure + SAFEOP after TwinCAT activation ----
+    // TwinCAT was just re-activated (writes PDO config to terminal flash).
+    // Now configure SM2/SM3 + PDO assignment and try SAFEOP.
+    println!("\nReconfiguring + SAFEOP after TwinCAT activation...");
+    
+    // Check if PDO assignment was pre-configured by TwinCAT
+    for idx in [0x1C12u16, 0x1C13u16] {
+        match sd.sdo_read::<u8>(idx, 0).await {
+            Ok(n) => {
+                println!("  0x{:04X} sub0={} (TwinCAT config)", idx, n);
+                for i in 1..=n {
+                    if let Ok(v) = sd.sdo_read::<u16>(idx, i as u8).await {
+                        println!("    [{:02}] = 0x{:04X}", i, v);
+                    }
+                }
+            }
+            Err(e) => println!("  0x{:04X} error: {:?}", idx, e),
+        }
+    }
+    
+    // Write PDO definitions and assignment (override TwinCAT if needed)
+    let blob = bridge::twin_layout_blob(0x7000);
+    let _ = sd.sdo_write_complete(0x1608, 0, &blob).await;
+    let blob = bridge::twin_layout_blob(0x6000);
+    let _ = sd.sdo_write_complete(0x1A08, 0, &blob).await;
+    let blob = bridge::assign_blob(&[0x1608]);
+    let _ = sd.sdo_write_complete(0x1C12, 0, &blob).await;
+    let blob = bridge::assign_blob(&[0x1A08]);
+    let _ = sd.sdo_write_complete(0x1C13, 0, &blob).await;
+    
+    // Configure SM2/SM3 with ESI values + 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, 1024u16).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, 1024u16).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).await;
+    
+    // SAFEOP
     let (al, alc) = bridge::drive_state(&maindevice, station, 0x0004).await;
-    println!("  {}", bridge::verdict(al, alc));
+    println!("  SAFEOP: {}", bridge::verdict(al, alc));
     if al & 0x0F != 0x04 || al & 0x10 != 0 {
-        return Err(format!("SAFEOP rejected: {}", bridge::verdict(al, alc)).into());
+        return Err(format!("Cannot reach SAFEOP: {}", bridge::verdict(al, alc)).into());
     }
-
+    println!("  >>> SAFEOP achieved! <<<");
-    println!("Requesting OP...");
+    let (al2, alc2) = bridge::drive_state(&maindevice, station, 0x0008).await;
-    let (al, alc) = bridge::drive_state(&maindevice, station, 0x0008).await;
+    println!("  OP: {}", bridge::verdict(al2, alc2));
-    println!("  {}", bridge::verdict(al, alc));
+    if al2 & 0x0F != 0x08 || al2 & 0x10 != 0 {
-    if al & 0x0F != 0x08 || al & 0x10 != 0 {
+        return Err(format!("OP rejected: {}", bridge::verdict(al2, alc2)).into());
-        return Err(format!("OP rejected: {}", bridge::verdict(al, alc)).into());
     }
+    println!("  >>> OP achieved! <<<");
 
     // ---- Optional EL2202 oscilloscope waveform (toggle one DO per cycle) ----
-    // Simple DO terminals have no mailbox; their single output SyncManager
-    // (SM0) is NOT pre-programmed (ethercrab programs SMs/FMMUs only during
-    // the group PREOP->SAFEOP transition, which we deliberately skip for the
-    // bridge). Program SM0 manually per the EL2xxx family layout (outputs at
-    // physical 0x0F00, 1 byte, buffered ECAT-write ctrl 0x44), drive the
-    // terminal to OP, and verify the path with a write/readback before
-    // enabling the per-cycle toggle.
     const EL2202_OUT_ADDR: u16 = 0x0F00;
-    let mut el2202: Option<(u16, u16)> = None; // (station, output physical addr)
+    let mut el2202: Option<(u16, u16)> = None;
     if cfg.el2202 {
         let mut found = false;
         for (idx, subdevice) in group.iter(&maindevice).enumerate() {
@@ -437,10 +381,6 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
                 let sd2 = group.subdevice(&maindevice, idx)?;
                 let st2 = sd2.configured_address();
                 println!("[{}] Found EL2202, station 0x{:04X}", idx, st2);
-                // SM0: start/len/ctrl are complete config values; the
-                // enable byte is read-modify-write (set enable + the family
-                // bit per EEPROM — without the family bit the output stage
-                // stays inert and the pin does not switch).
                 let _ = ethercrab::Command::fpwr(st2, regs::sm_base(0) + regs::SM_OFF_START)
                     .send(&maindevice, EL2202_OUT_ADDR)
                     .await;
@@ -465,7 +405,6 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
                 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());
                 }
-                // Verify the output path: write a byte, read it back.
                 let _ = ethercrab::Command::fpwr(st2, EL2202_OUT_ADDR)
                     .send(&maindevice, regs::EL2202_CH1_DATA)
                     .await;
@@ -483,10 +422,7 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
                         .await;
                     el2202 = Some((st2, EL2202_OUT_ADDR));
                 } else {
-                    println!(
+                    println!("EL2202 output readback failed (0x{:02X}); scope output disabled", rb);
-                        "EL2202 output readback failed (0x{:02X}); scope output disabled",
-                        rb
-                    );
                 }
             }
         }
@@ -499,14 +435,8 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
     }
 
     // ---- Optional EL2262 DC-locked reference output ----
-    // The XFC terminal applies process data at its SYNC events, so with the
-    // DC sync unit programmed to the 1 ms cycle the output edges land on the
-    // DC grid regardless of frame arrival jitter — an oscilloscope (or the
-    // EL1252 below) can compare them against the loop-driven EL2202 edges.
-    // SM0 (outputs @0x1000, ctrl 0x64, enable 0x09 per EEPROM) is programmed
-    // manually for the same reason as the EL2202.
     const EL2262_OUT_ADDR: u16 = 0x1000;
-    let mut el2262: Option<u16> = None; // station
+    let mut el2262: Option<u16> = None;
     if cfg.el2262 {
         let mut found = false;
         for (idx, subdevice) in group.iter(&maindevice).enumerate() {
@@ -515,10 +445,6 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
                 let sd3 = group.subdevice(&maindevice, idx)?;
                 let st3 = sd3.configured_address();
                 println!("[{}] Found EL2262, station 0x{:04X}", idx, st3);
-                // DC sync unit: SYNC0+SYNC1 at the configured cycle, first
-                // pulse 100 ms in the future (DC system time is already
-                // running on these terminals). Start/cycle times are full
-                // values; the activation byte is read-modify-write.
                 let t = ethercrab::Command::fprd(st3, regs::DC_SYSTEM_TIME)
                     .receive::<u64>(&maindevice)
                     .await
@@ -541,12 +467,7 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
                     0,
                 )
                 .await;
-                println!(
+                println!("EL2262 DC sync: sync0/1 cycle={}ns start=+100ms (t={})", cycle_ns, t);
-                    "EL2262 DC sync: sync0/1 cycle={}ns start=+100ms (t={})",
-                    cycle_ns, t
-                );
-                // SM0: outputs @0x1000, 1 byte, ESI ctrl; enable via RMW
-                // (enable + family bit per EEPROM).
                 let _ = ethercrab::Command::fpwr(st3, regs::sm_base(0) + regs::SM_OFF_START)
                     .send(&maindevice, EL2262_OUT_ADDR)
                     .await;
@@ -580,12 +501,7 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
     }
 
     // ---- Optional EL1252 DC latch timestamp readback ----
-    // The latch status/timestamps live in plain ESC registers (0x09AE
+    let mut el1252: Option<u16> = None;
-    // status, 0x09B0 Latch0Pos, 0x09B8 Latch0Neg for channel 1). Edge
-    // latching only runs while the terminal is in OP, so configure its
-    // input SM0 (@0x1000, 1 byte, per EEPROM ctrl 0x22/enable 0x01) and
-    // drive it up — this also makes the raw input level byte readable.
-    let mut el1252: Option<u16> = None; // station
     if cfg.el1252 {
         let mut found = false;
         for (idx, subdevice) in group.iter(&maindevice).enumerate() {
@@ -627,14 +543,12 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
     }
 
     // ---- Pre-allocate everything, then promote to RT ----
-    let mut tx_buf = vec![0u8; SM2_LEN];
+    let mut tx_buf = vec![0u8; SM_TX_LEN];
-    let mut rx_buf = vec![0u8; SM3_CAP];
+    let mut rx_buf = vec![0u8; SM_RX_LEN];
-    // Jitter: 100 ns bins, window +-5 ms. Exec/RTT: 1 us bins up to 20 ms.
     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);
-    let mut phase_st = OnlineStats::new("phase_err", 100, -5_000_000, 5_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) {
@@ -645,21 +559,19 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
     // ---- Real-time data loop ----
     println!(
         "\n>>> running: {} loop, echo payload, stats (Ctrl-C for graceful stop)",
-        if follow { "follow-TwinCAT" } else { "ticker" }
+        match cfg.mode {
+            Mode::Ticker => "ticker",
+            Mode::Probe => "probe (read-only, no echo)",
+        }
     );
     let start = Instant::now();
     let mut ticker = rt::Ticker::new(cycle_ns);
-    let mut pll = Follow::new(cycle_ns, cfg.phase_us * 1000);
-    let mut last_ext_ts: Option<u64> = None;
-    let mut debug_ts = cfg.debug_ts;
 
     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 ts_stale: u64 = 0;
-    let mut ts_rejected: u64 = 0;
     let mut el2202_err: u64 = 0;
     let mut el2262_err: u64 = 0;
     let mut el1252_err: u64 = 0;
@@ -672,8 +584,6 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
     let mut pos1_edges: u64 = 0;
     let mut el1252_st: [u8; 2] = [0, 0];
     let mut el1252_in: u8 = 0;
-    // EL1252 edge stats per channel: interval between rising edges (1 us
-    // bins to 20 ms), and edge phase within the 1 ms cycle (100 ns bins).
     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);
@@ -690,24 +600,21 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
             break;
         }
 
-        let jitter_ns = match cfg.mode {
+        let jitter_ns = ticker.wait_next_period();
-            Mode::Ticker | Mode::Probe => ticker.wait_next_period(),
-            Mode::Follow => pll.wait_next(),
-        };
         let t0 = Instant::now();
         cyc += 1;
         jitter_st.push(jitter_ns);
 
-        // 1. Read SM3 (TwinCAT outputs, plus diag/timestamps)
+        // 1. Read SM1 (TwinCAT data)
         let t_rx0 = Instant::now();
         let mut rx_ok = true;
-        match ethercrab::Command::fprd(station, SM3_ADDR)
+        match ethercrab::Command::fprd(station, SM_RX_ADDR)
-            .receive_slice(&maindevice, sm3_len as u16)
+            .receive_slice(&maindevice, SM_RX_LEN as u16)
             .await
         {
             Ok(pdu) => {
                 let raw: &[u8] = &pdu;
-                let n = raw.len().min(sm3_len);
+                let n = raw.len().min(SM_RX_LEN);
                 rx_buf[..n].copy_from_slice(&raw[..n]);
             }
             Err(_) => {
@@ -716,104 +623,29 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
             }
         }
         rx_rtt_st.push(t_rx0.elapsed().as_nanos() as i64);
-        let read_ns = rt::now_ns();
 
-        // 2. Timestamp handling.
+        // 2. In ticker mode: echo payload back and stamp cycle number.
-        if rx_ok && ts_layout {
+        //    In probe mode: skip TX (read-only observation).
-            let ext_ts = u64::from_le_bytes(
+        if cfg.mode == Mode::Ticker {
-                rx_buf[TS_EXT_OFF..TS_EXT_OFF + 8].try_into().unwrap_or([0; 8]),
+            if rx_ok {
-            );
+                let n = PAYLOAD_LEN.min(rx_buf.len());
-            if cfg.mode == Mode::Probe {
+                tx_buf[..n].copy_from_slice(&rx_buf[..n]);
-                // No feedback: just dump the ext phase and inter-sample delta.
+            }
-                if ext_ts == 0 {
+            tx_buf[8..16].copy_from_slice(&cyc.to_le_bytes());
-                    ts_stale += 1;
+            tx_buf[264..268].copy_from_slice(&(jitter_ns as i32).to_le_bytes());
-                } else {
+
-                    let d = last_ext_ts.map(|l| ext_ts.wrapping_sub(l));
+            let t_tx0 = Instant::now();
-                    if Some(ext_ts) != last_ext_ts {
+            let tx_ok = ethercrab::Command::fpwr(station, SM_TX_ADDR)
-                        if debug_ts > 0 && cyc > 2000 {
+                .send(&maindevice, &tx_buf[..PAYLOAD_LEN])
-                            println!(
+                .await
-                                "TS cyc={} ext_mod_us={} d={} read_mod_us={}",
+                .is_ok();
-                                cyc,
+            tx_rtt_st.push(t_tx0.elapsed().as_nanos() as i64);
-                                ext_ts % cycle_ns / 1000,
+            if !tx_ok {
-                                d.unwrap_or(0) as i64,
+                tx_err += 1;
-                                read_ns % cycle_ns / 1000,
-                            );
-                            debug_ts -= 1;
-                        }
-                        last_ext_ts = Some(ext_ts);
-                    } else {
-                        ts_stale += 1;
-                    }
-                }
-            } else if ext_ts == 0 {
-                ts_stale += 1;
-            } else {
-                // Plausibility is judged against the previous *raw* sample,
-                // so a single bad sample causes at most two rejects and the
-                // filter re-syncs itself (comparing against the last accepted
-                // sample instead would deadlock after one startup outlier).
-                let d = last_ext_ts.map(|l| ext_ts.wrapping_sub(l));
-                last_ext_ts = Some(ext_ts);
-                let verdict = match d {
-                    None => "accept",
-                    Some(0) => "stale",
-                    Some(dd) if dd <= TS_MAX_GAP_NS => "accept",
-                    Some(_) => "reject",
-                };
-                if debug_ts > 0 && cyc > 2000 {
-                    println!(
-                        "TS cyc={} ext_mod_us={} d={} off={} wait={} anchor={} dl_mod_us={} {}",
-                        cyc,
-                        ext_ts % cycle_ns / 1000,
-                        d.unwrap_or(0) as i64,
-                        pll.offset_ns,
-                        pll.last_wait_ns,
-                        pll.next_target_tc,
-                        (pll.next_target_tc as i64 + pll.offset_ns)
-                            .rem_euclid(cycle_ns as i64)
-                            / 1000,
-                        verdict
-                    );
-                    debug_ts -= 1;
-                }
-                match verdict {
-                    "accept" => {
-                        pll.update(ext_ts, read_ns);
-                        let target = pll.current_target_local(ext_ts);
-                        phase_st.push(read_ns as i64 - target as i64);
-                    }
-                    "stale" => ts_stale += 1,
-                    _ => ts_rejected += 1,
-                }
             }
         }
 
-        // 3. Echo TwinCAT's payload straight back, stamp our cycle number at
+        // Optional EL2202 scope waveform
-        //    bytes 8..16 (TwinCAT round-trip check uses bytes 0..8).
-        if rx_ok {
-            let data = &rx_buf[data_off..];
-            let n = PAYLOAD_LEN.min(data.len());
-            tx_buf[..n].copy_from_slice(&data[..n]);
-        }
-        tx_buf[8..16].copy_from_slice(&cyc.to_le_bytes());
-
-        let t_tx0 = Instant::now();
-        let tx_ok = ethercrab::Command::fpwr(station, SM2_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;
-        }
-
-        // Optional EL2202 scope waveform: toggle one output every cycle
-        // (dual mode: both channels). The EL2202 process byte is
-        // [bit0=ch1 data, bit1=ch1 TRISTATE, bit2=ch2 data, bit3=ch2
-        // TRISTATE] (PDO 0x1600/0x1601, verified from EEPROM) — writing
-        // ch1 data + ch1 tristate puts channel 1 into high-resistance, so
-        // dual mode must only set the two DATA bits. This is a process-data
-        // payload (the whole byte is ours), not a register bit modification.
         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)
@@ -829,8 +661,7 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
             }
         }
 
-        // Optional EL2262 DC-locked reference output: toggle every cycle;
+        // Optional EL2262 DC-locked reference output
-        // the terminal applies the new value at its next SYNC event.
         if let Some(st3) = el2262 {
             let v: u8 = (cyc & 1) as u8;
             if ethercrab::Command::fpwr(st3, EL2262_OUT_ADDR)
@@ -842,12 +673,7 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
             }
         }
 
-        // Optional EL1252 latch readback: 34 bytes @0x09AE =
+        // Optional EL1252 latch readback
-        // [status0, status1, Latch0Pos u64, Latch0Neg u64,
-        //  Latch1Pos u64, Latch1Neg u64] (PDO 0x1A13 extent, ESC registers
-        //  0x09AE..0x09CF). A changed LatchXPos means a new rising edge was
-        //  captured on that channel; intervals between rising edges and
-        //  their phase within the cycle are the jitter statistics.
         if let Some(st4) = el1252 {
             if cyc % (cfg.el1252_every.max(1) as u64) == 0 {
                 match ethercrab::Command::fprd(st4, 0x09AEu16)
@@ -892,8 +718,7 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
             }
         }
 
-        // Error-streak guard: a dead link must abort loudly, not accumulate.
+        if rx_ok && (cfg.mode == Mode::Probe || !tx_err > 0) {
-        if rx_ok && tx_ok {
             err_streak = 0;
         } else {
             err_streak += 1;
@@ -906,7 +731,7 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
         }
 
         let tc_seq = u64::from_le_bytes(
-            rx_buf[data_off..data_off + 8].try_into().unwrap_or([0; 8]),
+            rx_buf[..8].try_into().unwrap_or([0; 8]),
         );
         if tc_seq == 0 {
             no_tc_seq += 1;
@@ -915,7 +740,6 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
         exec_st.push(t0.elapsed().as_nanos() as i64);
 
         if !cfg.quiet && last_report.elapsed() >= Duration::from_millis(1000) {
-            // Diagnostic: raw EL1252 input level byte (SM0 @0x1000), 1 Hz only.
             if let Some(st4) = el1252 {
                 if let Ok(b) = ethercrab::Command::fprd(st4, 0x1000u16)
                     .receive::<u8>(&maindevice)
@@ -924,20 +748,14 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
                     el1252_in = b;
                 }
             }
-            let diag = u16::from_le_bytes([rx_buf[0], rx_buf[1]]);
             println!(
-                "S tms={} cyc={} diag=0x{:04X} tc_seq={} off={} wait={} txe={} rxe={} notc={} ts0={} tsrej={} e0={} e1={} in=0x{:02X} lst={:02X}{:02X}",
+                "S tms={} cyc={} tc_seq={} txe={} rxe={} notc={} e0={} e1={} in=0x{:02X} lst={:02X}{:02X}",
                 start.elapsed().as_millis(),
                 cyc,
-                diag,
                 tc_seq,
-                pll.offset_ns,
-                pll.last_wait_ns,
                 tx_err,
                 rx_err,
                 no_tc_seq,
-                ts_stale,
-                ts_rejected,
                 pos_edges,
                 pos1_edges,
                 el1252_in,
@@ -958,10 +776,6 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
     exec_st.report(1000, "us");
     rx_rtt_st.report(1000, "us");
     tx_rtt_st.report(1000, "us");
-    if follow {
-        println!("=== phase error: SM3 read point vs (TC grid boundary + phase) ===");
-        phase_st.report(1000, "us");
-    }
     if el1252.is_some() {
         println!("=== EL1252 latch: rising-edge intervals and phase in cycle ===");
         latch_interval_st.report(1000, "us");
@@ -974,14 +788,12 @@ async fn run(cfg: &Config) -> Result<(), Box<dyn std::error::Error>> {
         );
     }
     println!(
-        "\n=== done: {} cycles in {:.1}s, tx_err={}, rx_err={}, no_tc_seq={}, ts_stale={}, ts_rejected={}, el2202_err={}, el2262_err={} ===",
+        "\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,
-        ts_stale,
-        ts_rejected,
         el2202_err,
         el2262_err
     );

src/regs.rs

@@ -42,8 +42,10 @@ pub const SM_CTRL_DIR_ECAT_WRITE: u8 = 0x04;
 // Full ctrl bytes verified from each device's EEPROM SM category. These are
 // complete configuration values (mode + direction + vendor bits), written
 // wholesale during initialization — not bit modifications.
-pub const SM_CTRL_OUTPUTS_ESI: u8 = 0x64; // EL6695 SM2, EL2262 SM0
+pub const SM_CTRL_OUTPUTS_ESI: u8 = 0x64; // EL6695 SM2 (primary), EL2262 SM0
-pub const SM_CTRL_INPUTS_ESI: u8 = 0x20; // EL6695 SM3
+pub const SM_CTRL_INPUTS_ESI: u8 = 0x20; // EL6695 SM3 (primary)
+pub const SM_CTRL_OUTPUTS_SEC: u8 = 0x70; // EL6695 SM0 (secondary, buffered WRITE output)
+pub const SM_CTRL_INPUTS_SEC: u8 = 0x20;  // EL6695 SM1 (secondary, buffered input, matching primary SM3)
 pub const SM_CTRL_EL2202_OUTPUTS: u8 = 0x44; // EL2202 SM0
 pub const SM_CTRL_EL1252_INPUTS: u8 = 0x22; // EL1252 SM0