//! EL6695 bridge bring-up (J1900/primary side). //! //! Hard-won firmware constraints (all verified on hardware in the reference //! project, kept identical here): //! - The EL6695 user OD is wiped by the firmware whenever the master changes, //! so configuration and the data loop must happen in the same session. //! - Mapping objects must be written with a Complete Access download in a //! single mailbox message; the standard initiate+segments flow is rejected //! with MBXERR_INVALIDSIZE. //! - The PDO layout must replicate the TwinCAT secondary-side OD byte for //! byte: 0x1608/0x1A08 with 35 entries = [real object @240bit] //! + 33 x [0x0000:00 @240bit continuation] + 1 x [0x0000:00 @32bit], //! i.e. 8192 bit hanging on a single object (0x7000:01 / 0x6000:01). //! - 0x1C12 = [0x1608]; 0x1C13 = [0x1A01 or 0x1A02, 0x1A08]. //! - SM registers and the AL state machine are driven with raw fpwr/fprd, //! bypassing ethercrab's CoE PDO parsing entirely. //! - After SAFEOP the mailbox dies: no SDO traffic inside the data loop. use ethercrab::{Command, MainDevice}; use std::time::Duration; 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]. pub fn twin_layout_blob(obj: u16) -> Vec { 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]); for _ in 0..33 { blob.extend_from_slice(&[240, 0, 0, 0]); } blob.extend_from_slice(&[32, 0, 0, 0]); blob } /// Size-parameterised TwinCAT line-format blob. /// /// For payloads of 1024 bytes this is byte-identical to `twin_layout_blob`. /// For small payloads (<= 30 bytes, fitting one 240-bit entry) a single /// entry references the real object at the exact bit length. pub fn twin_layout_blob_sized(obj: u16, payload_bytes: usize) -> Vec { if payload_bytes == 1024 { return twin_layout_blob(obj); } let bits = (payload_bytes * 8).min(240) as u8; let idx_lo = (obj & 0xFF) as u8; let idx_hi = (obj >> 8) as u8; let mut blob = Vec::with_capacity(6); blob.extend_from_slice(&1u16.to_le_bytes()); blob.extend_from_slice(&[bits, 1, idx_lo, idx_hi]); blob } /// Standard PDO mapping blob: 35 entries ALL referencing `obj:01`. /// /// Unlike `twin_layout_blob` which uses 0x0000:00 continuation entries /// (rejected by the EL6695 firmware when written from a non-TwinCAT /// master), this format has every entry point to the same valid CoE object. /// /// 34 entries × 240 bits + 1 entry × 32 bits = 8192 bits = 1024 bytes. pub fn standard_layout_blob(obj: u16) -> Vec { let n = 35usize; let mut blob = Vec::with_capacity(2 + n * 4); blob.extend_from_slice(&(n as u16).to_le_bytes()); let idx_lo = (obj & 0xFF) as u8; let idx_hi = (obj >> 8) as u8; for _ in 0..34 { blob.extend_from_slice(&[240, 1, idx_lo, idx_hi]); } blob.extend_from_slice(&[32, 1, idx_lo, idx_hi]); blob } /// TwinCAT 256-bit PDO mapping blob: 32 entries × 256 bits = 8192 bits = 1024 bytes. /// /// The EL6695 firmware decodes a zero bit-length field as 256 bits. /// Entry 1 references the real object; entries 2..32 are 0x0000:00 /// continuation markers (same concept as twin_layout_blob but uses the /// zero-length/256-bit quirk which may trigger object expansion). pub fn twin_256_layout_blob(obj: u16) -> Vec { let n = 32usize; // 32 × 256 bits = 8192 bits = 1024 bytes let mut blob = Vec::with_capacity(2 + n * 4); blob.extend_from_slice(&(n as u16).to_le_bytes()); let idx_lo = (obj & 0xFF) as u8; let idx_hi = (obj >> 8) as u8; // Entry 1: real object at 256 bits (zero-length encoded) blob.extend_from_slice(&[0, 1, idx_lo, idx_hi]); // Entries 2..32: continuation entries at 256 bits for _ in 0..(n - 1) { blob.extend_from_slice(&[0, 0, 0, 0]); } blob } /// SM assign blob: entry count (u16 LE) + mapped object indices. pub fn assign_blob(objs: &[u16]) -> Vec { 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 { blob.extend_from_slice(&o.to_le_bytes()); } blob } /// Complete Access write of a whole object, 3 attempts. async fn sdo_ca(sd: ðercrab::SubDeviceRef<'_, S>, idx: u16, blob: &[u8]) -> bool where S: std::ops::Deref, { for attempt in 0..3 { match sd.sdo_write_complete(idx, 0, blob).await { Ok(_) => return true, Err(e) => { if attempt == 2 { println!(" CA write 0x{:04X} ({}B) failed x3: {:?}", idx, blob.len(), e); return false; } std::thread::sleep(Duration::from_millis(100)); } } } false } async fn read_u8(sd: ðercrab::SubDeviceRef<'_, S>, idx: u16, sub: u8) -> u8 where S: std::ops::Deref, { sd.sdo_read(idx, sub).await.unwrap_or(0xFF) } /// Raw AL control request + poll until the state takes effect (max ~3 s). /// Returns (AL status, AL status code). /// /// The AL control register is a *command* register: the protocol requires /// writing the full target state, so read-modify-write does not apply. pub async fn drive_state(md: &MainDevice<'_>, station: u16, state: u16) -> (u16, u16) { let _ = Command::fpwr(station, regs::AL_CONTROL).send(md, state).await; for _ in 0..20 { std::thread::sleep(Duration::from_millis(150)); let al = Command::fprd(station, regs::AL_STATUS) .receive::(md) .await .unwrap_or(0xFFFF); if al & regs::AL_ERROR != 0 || al & regs::AL_STATE_MASK == state { let alc = Command::fprd(station, regs::AL_STATUS_CODE) .receive::(md) .await .unwrap_or(0xFFFF); return (al, alc); } } (0xFFFF, 0xFFFF) } pub fn verdict(al: u16, alc: u16) -> String { let state = match al & 0x0F { 1 => "INIT", 2 => "PREOP", 4 => "SAFEOP", 8 => "OP", _ => "?", }; let meaning = match alc { 0x0000 => "OK", 0x001D => "invalid OUTPUT config", 0x001E => "invalid INPUT config", 0x0003 => "invalid device setup", _ => "?", }; format!( "AL=0x{:04X} ({}{}) code=0x{:04X} {}", al, state, if al & 0x10 != 0 { "+ERR" } else { "" }, alc, meaning ) } /// Ensure the user OD holds the expected PDO layout; (re)configure if not. /// /// `txpdo_first` is the first TxPDO in 0x1C13: 0x1A01 (diagnostic word, /// plain mode) or 0x1A02 (22-byte SYNC timestamps, follow mode). /// /// If the mailbox is dead (SAFEOP-nuked from a previous session) the bridge /// is cycled INIT -> PREOP first. pub async fn ensure_od( sd: ðercrab::SubDeviceRef<'_, S>, md: &MainDevice<'_>, station: u16, txpdo_first: u16, ) -> Result<(), String> where S: std::ops::Deref, { let configured = match sd.sdo_read::(0x1C13, 1).await { Ok(v) if v == txpdo_first => { let rb = ( read_u8(sd, 0x1C12, 0).await, read_u8(sd, 0x1C13, 0).await, read_u8(sd, 0x1608, 0).await, read_u8(sd, 0x1A08, 0).await, ); if rb == (1, 2, 35, 35) { println!( "EL6695 OD already configured (0x1C13=[0x{:04X}, 0x1A08])", txpdo_first ); return Ok(()); } println!("EL6695 OD partial (rb={:?}); rewriting layout...", rb); true } Ok(v) => { println!( "EL6695 OD has 0x1C13:01=0x{:04X}, need 0x{:04X}; reconfiguring...", v, txpdo_first ); true } Err(e) => { println!("SDO read failed ({:?}); INIT->PREOP reset then configure", e); let (al, _) = drive_state(md, station, 0x0001).await; if al & 0x0F != 0x01 { return Err(format!("not INIT after reset ({})", verdict(al, 0))); } let (al, alc) = drive_state(md, station, 0x0002).await; if al & 0x0F != 0x02 { return Err(format!("not PREOP after reset ({})", verdict(al, alc))); } true } }; if !configured { return Ok(()); } let mut ok = true; ok &= sdo_ca(sd, 0x1608, &twin_layout_blob(0x7000)).await; ok &= sdo_ca(sd, 0x1A08, &twin_layout_blob(0x6000)).await; ok &= sdo_ca(sd, 0x1C12, &assign_blob(&[0x1608])).await; ok &= sdo_ca(sd, 0x1C13, &assign_blob(&[txpdo_first, 0x1A08])).await; let rb = ( read_u8(sd, 0x1C12, 0).await, read_u8(sd, 0x1C13, 0).await, read_u8(sd, 0x1608, 0).await, read_u8(sd, 0x1A08, 0).await, ); let a1 = sd.sdo_read::(0x1C13, 1).await.unwrap_or(0xFFFF); let a2 = sd.sdo_read::(0x1C13, 2).await.unwrap_or(0xFFFF); println!( " writes_ok={} rb={:?} 0x1C13=[0x{:04X}, 0x{:04X}]", ok, rb, a1, a2 ); if !ok || rb != (1, 2, 35, 35) || (a1, a2) != (txpdo_first, 0x1A08) { return Err("PDO config write/readback mismatch".into()); } Ok(()) } /// Raw SM0/SM1 register config for the secondary side (probed from EEPROM). /// /// SM0: outputs (J1900 writes data to TwinCAT), start=0x1000, len=1024, ctrl=0x26 /// 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(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( sd: ðercrab::SubDeviceRef<'_, S>, md: &MainDevice<'_>, ) -> Result<(), String> where S: std::ops::Deref, { 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: SM2 outputs (J1900 writes data to bridge), /// logical 0→physical 0x1C00, write_enable /// FMMU1: SM3 inputs (J1900 reads data from bridge), /// logical {sm2_len}→physical 0x8E00, read_enable pub async fn write_fmmus_secondary(md: &MainDevice<'_>, station: u16, sm3_len: u16, sm2_len: u16) { // FMMU0: SM2 Outputs (master→slave, write_enable) per ESI let out: [u8; 16] = [ 0x00, 0x00, 0x00, 0x00, // logical_start=0 (sm2_len & 0xFF) as u8, ((sm2_len >> 8) & 0xFF) as u8, // length 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; // Logical start for SM3 = sm2_len (immediately after SM2 in process data image) let logical_start = sm2_len as u32; let inp: [u8; 16] = [ (logical_start & 0xFF) as u8, ((logical_start >> 8) & 0xFF) as u8, ((logical_start >> 16) & 0xFF) as u8, ((logical_start >> 24) & 0xFF) as u8, (sm3_len & 0xFF) as u8, ((sm3_len >> 8) & 0xFF) as u8, 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; } // ---- PDO layout inspection (read, don't write) ---- /// Read PDO mapping entries from a PDO mapping object (0x1608 or 0x1A08). /// Returns vec of (index, sub_index, bit_length) for each entry. pub async fn read_pdo_entries( sd: ðercrab::SubDeviceRef<'_, S>, pdo_idx: u16, ) -> Vec<(u16, u8, u8)> where S: std::ops::Deref, { let mut entries = Vec::new(); let count = match sd.sdo_read::(pdo_idx, 0).await { Ok(n) => n, Err(_) => return entries, }; for i in 1..=count { if let Ok(v) = sd.sdo_read::(pdo_idx, i).await { entries.push(( (v & 0xFFFF) as u16, ((v >> 16) & 0xFF) as u8, (v >> 24) as u8, )); } } entries } /// Read PDO assignment list from 0x1C12 or 0x1C13. pub async fn read_pdo_assign( sd: ðercrab::SubDeviceRef<'_, S>, assign_idx: u16, ) -> Vec where S: std::ops::Deref, { let mut pdos = Vec::new(); let count = match sd.sdo_read::(assign_idx, 0).await { Ok(n) => n, Err(_) => return pdos, }; for i in 1..=count { if let Ok(v) = sd.sdo_read::(assign_idx, i).await { pdos.push(v); } } pdos } /// Sum bit lengths of all entries. Unlike pdo_entry_bit_total_owned it /// includes continuation entries (0x0000:00 at non-zero bitlen) — the /// firmware counts every entry's declared length regardless of index. pub fn pdo_entry_bit_total(entries: &[(u16, u8, u8)]) -> u32 { entries.iter().map(|&(_, _, bl)| bl as u32).sum() } /// Expected total bit length = 34 × 240 + 32 = 8192 bit = 1024 byte. pub const EXPECTED_TOTAL_BITS: u32 = 34 * 240 + 32; /// The expected PDO mapping objects used by the secondary side. pub const TXPDO_IDX: u16 = 0x1608; pub const RXPDO_IDX: u16 = 0x1A08; // ---- SDO write whitelist ---- /// Allowed CoE indices for J1900/consumer SDO writes. /// Per spec §0 item 3: only PDO map/assign, reset, and NoCoeStorage. const SDO_WRITE_WHITELIST: &[std::ops::RangeInclusive] = &[ 0x1608..=0x1608, 0x1A08..=0x1A08, 0x1C12..=0x1C13, 0xF008..=0xF008, 0xF800..=0xF800, ]; /// Validate a CoE index against the SDO write whitelist. /// Returns Ok(()) if allowed, Err with message if denied. pub fn check_sdo_write_whitelist(idx: u16) -> Result<(), String> { if SDO_WRITE_WHITELIST.iter().any(|r| r.contains(&idx)) { Ok(()) } else { Err(format!( "SDO write 0x{:04X} denied by whitelist (allowed: {:?})", idx, SDO_WRITE_WHITELIST )) } } /// Whitelisted Complete Access write. Refuses indices outside the whitelist. pub async fn sdo_write_ca_checked( sd: ðercrab::SubDeviceRef<'_, S>, idx: u16, sub: u8, data: &[u8], ) -> Result<(), String> where S: std::ops::Deref, { check_sdo_write_whitelist(idx)?; sd.sdo_write_complete(idx, sub, data).await .map_err(|e| format!("SDO CA write 0x{:04X}:{:02X} failed: {:?}", idx, sub, e)) } // ---- Layout snapshot & hash ---- /// Full PDO layout snapshot used for hash computation. #[derive(Debug, Clone)] pub struct PdoLayoutSnapshot { pub tx_assign: Vec, pub rx_assign: Vec, pub tx_entries: Vec<(u16, u8, u8)>, pub rx_entries: Vec<(u16, u8, u8)>, pub sm2_len: u16, pub sm3_len: u16, /// Total PDO output bit length (0x1608). pub tx_total_bits: u32, /// Total PDO input bit length (0x1A08). pub rx_total_bits: u32, } /// Read SM2/SM3 lengths from ESC registers via FPRD. pub async fn read_sm_lengths(md: &MainDevice<'_>, station: u16) -> (u16, u16) { let sm2 = ethercrab::Command::fprd(station, crate::regs::sm_base(2) + crate::regs::SM_OFF_LEN) .receive::(md).await.unwrap_or(0); let sm3 = ethercrab::Command::fprd(station, crate::regs::sm_base(3) + crate::regs::SM_OFF_LEN) .receive::(md).await.unwrap_or(0); (sm2, sm3) } /// Read the full PDO layout from the bridge and return a snapshot. pub async fn snapshot_layout( sd: ðercrab::SubDeviceRef<'_, S>, md: &MainDevice<'_>, station: u16, ) -> PdoLayoutSnapshot where S: std::ops::Deref, { let tx_assign = read_pdo_assign(sd, 0x1C12).await; let rx_assign = read_pdo_assign(sd, 0x1C13).await; let tx_entries = read_pdo_entries(sd, TXPDO_IDX).await; let rx_entries = read_pdo_entries(sd, RXPDO_IDX).await; let (sm2_len, sm3_len) = read_sm_lengths(md, station).await; let tx_total_bits = pdo_entry_bit_total(&tx_entries); let rx_total_bits = pdo_entry_bit_total(&rx_entries); PdoLayoutSnapshot { tx_assign, rx_assign, tx_entries, rx_entries, sm2_len, sm3_len, tx_total_bits, rx_total_bits, } } /// Compute CRC32 hash of the full PDO layout. /// Digest order per spec §4: 0x1C12 subentries, 0x1C13 subentries, /// 0x1608 (index,sub,bitlen) × N, 0x1A08 (index,sub,bitlen) × N, /// SM2 length, SM3 length. pub fn compute_layout_hash(snap: &PdoLayoutSnapshot) -> u32 { use crc32fast::Hasher; let mut h = Hasher::new(); for &v in &snap.tx_assign { h.update(&v.to_le_bytes()); } for &v in &snap.rx_assign { h.update(&v.to_le_bytes()); } for &(idx, sub, bl) in &snap.tx_entries { h.update(&idx.to_le_bytes()); h.update(&[sub]); h.update(&[bl]); } for &(idx, sub, bl) in &snap.rx_entries { h.update(&idx.to_le_bytes()); h.update(&[sub]); h.update(&[bl]); } h.update(&snap.sm2_len.to_le_bytes()); h.update(&snap.sm3_len.to_le_bytes()); h.finalize() } /// Expected layout hash (crc32). /// Set after TwinCAT persistence test (spec §2). Default to 0 = unknown. pub static EXPECTED_LAYOUT_HASH: std::sync::atomic::AtomicU32 = std::sync::atomic::AtomicU32::new(0); /// Read the current PDO layout and compare with the expected layout. /// /// Returns `Ok(true)` if assignments and mapping entry counts + bit totals /// match the expected twin_layout_blob configuration. pub async fn layout_matches( sd: ðercrab::SubDeviceRef<'_, S>, md: &MainDevice<'_>, station: u16, ) -> Result where S: std::ops::Deref, { let snap = snapshot_layout(sd, md, station).await; let hash = compute_layout_hash(&snap); let expected = EXPECTED_LAYOUT_HASH.load(std::sync::atomic::Ordering::Relaxed); if expected == 0 { // Not yet initialized; fall back to field-by-field check let ok = snap.tx_assign.len() == 1 && snap.tx_assign.first() == Some(&TXPDO_IDX) && !snap.rx_assign.is_empty() && snap.rx_assign.last() == Some(&RXPDO_IDX) && snap.tx_entries.len() == 35 && snap.rx_entries.len() == 35 && snap.tx_total_bits == EXPECTED_TOTAL_BITS && snap.rx_total_bits == EXPECTED_TOTAL_BITS; if ok { // First valid layout seen — store it as expected EXPECTED_LAYOUT_HASH.store(hash, std::sync::atomic::Ordering::Relaxed); } return Ok(ok); } Ok(hash == expected) } /// 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::*; 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; 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; let _ = rmw_u8(md, station, sm_base(3) + SM_OFF_ENABLE, SM_ENABLE, 0).await; }