ethercat-linux/src/bridge.rs

//! 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<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]);
    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<u8> {
    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<u8> {
    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<u8> {
    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<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 {
        blob.extend_from_slice(&o.to_le_bytes());
    }
    blob
}

/// Complete Access write of a whole object, 3 attempts.
async fn sdo_ca<S>(sd: &ethercrab::SubDeviceRef<'_, S>, idx: u16, blob: &[u8]) -> bool
where
    S: std::ops::Deref<Target = ethercrab::SubDevice>,
{
    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<S>(sd: &ethercrab::SubDeviceRef<'_, S>, idx: u16, sub: u8) -> u8
where
    S: std::ops::Deref<Target = ethercrab::SubDevice>,
{
    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::<u16>(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::<u16>(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<S>(
    sd: &ethercrab::SubDeviceRef<'_, S>,
    md: &MainDevice<'_>,
    station: u16,
    txpdo_first: u16,
) -> Result<(), String>
where
    S: std::ops::Deref<Target = ethercrab::SubDevice>,
{
    let configured = match sd.sdo_read::<u16>(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::<u16>(0x1C13, 1).await.unwrap_or(0xFFFF);
    let a2 = sd.sdo_read::<u16>(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<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: 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<S>(
    sd: &ethercrab::SubDeviceRef<'_, S>,
    pdo_idx: u16,
) -> Vec<(u16, u8, u8)>
where
    S: std::ops::Deref<Target = ethercrab::SubDevice>,
{
    let mut entries = Vec::new();
    let count = match sd.sdo_read::<u8>(pdo_idx, 0).await {
        Ok(n) => n,
        Err(_) => return entries,
    };
    for i in 1..=count {
        if let Ok(v) = sd.sdo_read::<u32>(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<S>(
    sd: &ethercrab::SubDeviceRef<'_, S>,
    assign_idx: u16,
) -> Vec<u16>
where
    S: std::ops::Deref<Target = ethercrab::SubDevice>,
{
    let mut pdos = Vec::new();
    let count = match sd.sdo_read::<u8>(assign_idx, 0).await {
        Ok(n) => n,
        Err(_) => return pdos,
    };
    for i in 1..=count {
        if let Ok(v) = sd.sdo_read::<u16>(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<u16>] = &[
    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<S>(
    sd: &ethercrab::SubDeviceRef<'_, S>,
    idx: u16,
    sub: u8,
    data: &[u8],
) -> Result<(), String>
where
    S: std::ops::Deref<Target = ethercrab::SubDevice>,
{
    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<u16>,
    pub rx_assign: Vec<u16>,
    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::<u16>(md).await.unwrap_or(0);
    let sm3 = ethercrab::Command::fprd(station, crate::regs::sm_base(3) + crate::regs::SM_OFF_LEN)
        .receive::<u16>(md).await.unwrap_or(0);
    (sm2, sm3)
}

/// Read the full PDO layout from the bridge and return a snapshot.
pub async fn snapshot_layout<S>(
    sd: &ethercrab::SubDeviceRef<'_, S>,
    md: &MainDevice<'_>,
    station: u16,
) -> PdoLayoutSnapshot
where
    S: std::ops::Deref<Target = ethercrab::SubDevice>,
{
    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<S>(
    sd: &ethercrab::SubDeviceRef<'_, S>,
    md: &MainDevice<'_>,
    station: u16,
) -> Result<bool, String>
where
    S: std::ops::Deref<Target = ethercrab::SubDevice>,
{
    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;
}