//! 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]. 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 } /// SM assign blob: entry count (u16 LE) + mapped object indices. 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 SM2/SM3 register config (start/len/ctrl/enable per the ESI). /// /// Start/length/ctrl are complete configuration values written wholesale; /// the enable byte is applied read-modify-write so unrelated bits survive. 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; let _ = rmw_u8(md, station, sm_base(3) + SM_OFF_ENABLE, SM_ENABLE, 0).await; }