darra-ethercat-master 2.0.0

//! MDP (Modular Device Profile) 模块发现
//!
//! 通过 SDO 读取 0xF050 对象发现从站支持的 MDP 模块，
//! 返回每个模块的标识与分类信息。
//!
//! 参考：ETG.5001 MDP 规范，0xF050:sub 为模块描述列表。

use crate::data::error::{DarraError, Result};
use crate::utils::ffi;
use std::fmt;
use std::os::raw::c_int;

// 本地 native MDP API 绑定 (轻量, 不放 ffi.rs 公共层)
// 对应 Core.h: MDPGetConfigModuleList / MDPGetDetectedModuleList / MDPCheckModuleMatch
extern "C" {
    #[link_name = "D_1259"]
    fn mdp_get_config_module_list(mi: u16, si: u16, out_idents: *mut u32, max: c_int) -> c_int;
    #[link_name = "D_1260"]
    fn mdp_get_detected_module_list(mi: u16, si: u16, out_idents: *mut u32, max: c_int) -> c_int;
    #[link_name = "D_1261"]
    fn mdp_check_module_match(mi: u16, si: u16, out_first_mismatch: *mut c_int) -> c_int;
}

// ===================== 模块分类 =====================

/// MDP 模块分类代码（0xF050:sub 高 16 位的分类字段）
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum MdpModuleClass {
    /// 数字量输入
    DigitalInput  = 0x0001,
    /// 数字量输出
    DigitalOutput = 0x0002,
    /// 模拟量输入
    AnalogInput   = 0x0003,
    /// 模拟量输出
    AnalogOutput  = 0x0004,
    /// 计数器输入
    CounterInput  = 0x0005,
    /// 驱动器轴
    DriveAxis     = 0x0006,
    /// 功能安全 (FSoE)
    FunctionalSafety = 0x0007,
    /// 编码器接口
    EncoderInterface = 0x0008,
    /// 通信桥接
    CommunicationBridge = 0x0009,
    /// 未知分类
    Unknown = 0xFFFF,
}

impl MdpModuleClass {
    fn from_raw(val: u16) -> Self {
        match val {
            0x0001 => Self::DigitalInput,
            0x0002 => Self::DigitalOutput,
            0x0003 => Self::AnalogInput,
            0x0004 => Self::AnalogOutput,
            0x0005 => Self::CounterInput,
            0x0006 => Self::DriveAxis,
            0x0007 => Self::FunctionalSafety,
            0x0008 => Self::EncoderInterface,
            0x0009 => Self::CommunicationBridge,
            _ => Self::Unknown,
        }
    }
}

impl fmt::Display for MdpModuleClass {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let s = match self {
            Self::DigitalInput         => "数字量输入",
            Self::DigitalOutput        => "数字量输出",
            Self::AnalogInput          => "模拟量输入",
            Self::AnalogOutput         => "模拟量输出",
            Self::CounterInput         => "计数器输入",
            Self::DriveAxis            => "驱动器轴",
            Self::FunctionalSafety     => "功能安全",
            Self::EncoderInterface     => "编码器接口",
            Self::CommunicationBridge  => "通信桥接",
            Self::Unknown              => "未知",
        };
        write!(f, "{}", s)
    }
}

// ===================== MDP 模块信息 =====================

/// MDP 模块描述（一个从站可包含多个模块）
#[derive(Debug, Clone)]
pub struct MdpModule {
    /// 模块在从站中的编号 (1-based, 对应 0xF050 子索引)
    pub module_number: u8,
    /// 厂商 ID
    pub vendor_id: u32,
    /// 产品代码
    pub product_code: u32,
    /// 修订号
    pub revision_no: u32,
    /// 模块分类
    pub module_class: MdpModuleClass,
    /// 原始模块标识符 (0xF050:sub 的完整 u32 值)
    pub raw_ident: u32,
}

impl MdpModule {
    /// 检查是否为 FSoE 功能安全模块
    pub fn is_safety(&self) -> bool {
        self.module_class == MdpModuleClass::FunctionalSafety
    }

    /// 检查是否为驱动器轴模块
    pub fn is_drive(&self) -> bool {
        self.module_class == MdpModuleClass::DriveAxis
    }
}

impl fmt::Display for MdpModule {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f,
            "模块[{:02}] 分类:{:<12} VID:0x{:08X} PC:0x{:08X} Rev:0x{:08X}",
            self.module_number, self.module_class.to_string(),
            self.vendor_id, self.product_code, self.revision_no
        )
    }
}

// ===================== MDP 发现函数 =====================

/// 发现从站所有 MDP 模块
///
/// 通过 SDO 读取 0xF050 对象（MDP 模块列表），
/// 子索引 0 = 模块数量，子索引 1..n = 各模块 32 位标识。
/// 再通过 0xF000:01 (VendorID), 0xF000:02 (ProductCode) 等读取详细信息。
///
/// 该函数仅在从站处于 PreOp 及以上状态时才能成功。
pub fn mdp_discover(master_index: u16, slave_index: u16) -> Result<Vec<MdpModule>> {
    // 读取模块数量 (0xF050:00)
    let module_count = read_u8_sdo(master_index, slave_index, 0xF050, 0)?;
    if module_count == 0 {
        return Ok(Vec::new());
    }

    let mut modules = Vec::with_capacity(module_count as usize);

    for sub in 1..=module_count {
        // 读取模块标识 (0xF050:sub) 32位，包含分类信息
        let raw_ident = match read_u32_sdo(master_index, slave_index, 0xF050, sub) {
            Ok(v) => v,
            Err(_) => continue,
        };

        // 高16位 = 分类代码，低16位 = 厂商特定
        let class_raw = ((raw_ident >> 16) & 0xFFFF) as u16;
        let module_class = MdpModuleClass::from_raw(class_raw);

        // 尝试读取模块特定的 VendorID / ProductCode / RevisionNo
        // 这些对象地址依模块编号而定 (0xF00x:xx 系列)
        // 使用 0xF000 + (sub-1)*0x800 基地址模式（简化处理）
        let vendor_id = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x01)
            .unwrap_or(0);
        let product_code = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x02)
            .unwrap_or(0);
        let revision_no = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x03)
            .unwrap_or(0);

        modules.push(MdpModule {
            module_number: sub,
            vendor_id,
            product_code,
            revision_no,
            module_class,
            raw_ident,
        });
    }

    Ok(modules)
}

/// 仅发现支持 FSoE 的模块
pub fn mdp_discover_safety(master_index: u16, slave_index: u16) -> Result<Vec<MdpModule>> {
    let all = mdp_discover(master_index, slave_index)?;
    Ok(all.into_iter().filter(|m| m.is_safety()).collect())
}

/// 检查从站是否为 MDP 设备（通过 0xF000:00 可读性判断）
pub fn mdp_is_mdp_device(master_index: u16, slave_index: u16) -> bool {
    read_u8_sdo(master_index, slave_index, 0xF000, 0).is_ok()
}

// ===================== MDP 模块 Profile =====================

/// MDP 模块 Profile 信息 (ETG.5001, 0xF010)
#[derive(Debug, Clone)]
pub struct MdpModuleProfile {
    /// 槽位索引 (子索引, 1-based)
    pub slot_index: u8,
    /// Profile Number (0xF010:sub 的 u32 值)
    pub profile_number: u32,
    /// 模块标识码 (从 0xF050 读取的对应值)
    pub module_ident: u32,
}

impl fmt::Display for MdpModuleProfile {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f, "Slot[{:02}] Profile:0x{:08X} Ident:0x{:08X}",
            self.slot_index, self.profile_number, self.module_ident)
    }
}

// ===================== 扩展对象读取 (ETG.5001) =====================

/// 读取已配置模块地址列表 (0xF030, 转调 native MDPGetConfigModuleList).
///
/// 返回各槽位的已配置模块标识列表 (ETG.5001 Configured Module Ident List)。
/// 直接走 native, 不再走 SDORead 0xF030.
pub fn read_configured_address_list(master_index: u16, slave_index: u16) -> Result<Vec<u32>> {
    Ok(native_get_module_list(master_index, slave_index, true))
}

/// 读取已检测模块地址列表 (0xF050, 转调 native MDPGetDetectedModuleList).
///
/// 返回各槽位的已检测模块标识列表 (ETG.5001 Detected Module Ident List)。
/// 直接走 native, 不再走 SDORead 0xF050.
pub fn read_detected_address_list(master_index: u16, slave_index: u16) -> Result<Vec<u32>> {
    Ok(native_get_module_list(master_index, slave_index, false))
}

/// 通用: 调用 native MDPGet{Config,Detected}ModuleList 直读 ident 数组.
fn native_get_module_list(mi: u16, si: u16, configured: bool) -> Vec<u32> {
    let mut buf = [0u32; 32];
    let n = unsafe {
        if configured {
            mdp_get_config_module_list(mi, si, buf.as_mut_ptr(), buf.len() as c_int)
        } else {
            mdp_get_detected_module_list(mi, si, buf.as_mut_ptr(), buf.len() as c_int)
        }
    };
    if n <= 0 { return Vec::new(); }
    buf[..(n as usize)].to_vec()
}

/// 读取模块 Profile 列表 (0xF010)
///
/// 返回各槽位的模块 Profile Number, 同时从 0xF050 读取对应的模块标识码。
/// 需要从站处于 PreOp 及以上状态。
pub fn read_module_profile_list(master_index: u16, slave_index: u16) -> Result<Vec<MdpModuleProfile>> {
    // 读取 Profile 列表 (0xF010)
    let count = read_u8_sdo(master_index, slave_index, 0xF010, 0)?;
    if count == 0 {
        return Ok(Vec::new());
    }

    let mut profiles = Vec::with_capacity(count as usize);

    for sub in 1..=count {
        // 读取 Profile Number (0xF010:sub)
        let profile_number = match read_u32_sdo(master_index, slave_index, 0xF010, sub) {
            Ok(v) => v,
            Err(_) => continue,
        };

        // 读取对应的模块标识码 (0xF050:sub)
        let module_ident = read_u32_sdo(master_index, slave_index, 0xF050, sub)
            .unwrap_or(0);

        profiles.push(MdpModuleProfile {
            slot_index: sub,
            profile_number,
            module_ident,
        });
    }

    Ok(profiles)
}

/// 检查已配置模块与已检测模块是否一致 (转调 native MDPCheckModuleMatch).
///
/// 直接走 native, 不再走 SDORead. 等价于 ETG.5001 ModuleIdListMismatch (AL 0x0035) 校验语义.
pub fn is_module_config_consistent(master_index: u16, slave_index: u16) -> bool {
    let mut first_mismatch: c_int = -1;
    let rc = unsafe { mdp_check_module_match(master_index, slave_index, &mut first_mismatch) };
    rc == 1
}

/// 校验模块列表, 返回 (is_match, first_mismatch_slot). 不匹配时 first_mismatch_slot 为 0-based.
pub fn check_module_match(master_index: u16, slave_index: u16) -> (bool, i32) {
    let mut first_mismatch: c_int = -1;
    let rc = unsafe { mdp_check_module_match(master_index, slave_index, &mut first_mismatch) };
    (rc == 1, first_mismatch as i32)
}

/// 根据检测到的模块自动配置 - 将检测到的模块列表写入已配置列表 (0xF030)
///
/// 读取 0xF050 (已检测模块), 然后逐个写入 0xF030 (已配置模块),
/// 使已配置模块与已检测模块保持一致。
///
/// 返回成功写入的模块数量。
/// 需要从站处于 PreOp 状态, 且从站支持 0xF030 写入。
pub fn auto_configure_from_detected_modules(master_index: u16, slave_index: u16) -> Result<u8> {
    // 读取已检测模块列表
    let detected = read_detected_address_list(master_index, slave_index)?;
    if detected.is_empty() {
        return Ok(0);
    }

    let count = detected.len() as u8;

    // 写入已配置模块数量 (0xF030:00)
    write_u8_sdo(master_index, slave_index, 0xF030, 0, count)?;

    // 逐个写入模块标识到 0xF030
    let mut written: u8 = 0;
    for (i, ident) in detected.iter().enumerate() {
        let sub = (i + 1) as u8;
        if write_u32_sdo(master_index, slave_index, 0xF030, sub, *ident).is_ok() {
            written += 1;
        }
    }

    Ok(written)
}

// ===================== MDP 模块 PDO 布局 =====================

/// MDP 模块 PDO 布局信息 (对齐 C# MdpModulePdoInfo)
#[derive(Debug, Clone)]
pub struct MdpModulePdoInfo {
    /// 槽位索引
    pub slot_index: u8,
    /// 输入 PDO 偏移 (相对于 slave.Ioffset)
    pub input_offset: u32,
    /// 输入 PDO 字节数
    pub input_size: u16,
    /// 输出 PDO 偏移 (相对于 slave.Ooffset)
    pub output_offset: u32,
    /// 输出 PDO 字节数
    pub output_size: u16,
}

/// 获取各模块在从站 IOmap 中的 PDO 布局
///
/// 通过 CoE SDORead 读取 PDO Assignment (0x1C12/0x1C13) 和 PDO Mapping 条目，
/// 累积计算各模块在 IOmap 中的字节偏移。
/// 返回值中的偏移是相对于 slave.Ioffset/slave.Ooffset 的偏移。
pub fn get_module_pdo_layout(master_index: u16, slave_index: u16) -> Result<Vec<MdpModulePdoInfo>> {
    let detected = read_detected_address_list(master_index, slave_index)?;
    if detected.is_empty() {
        return Ok(Vec::new());
    }

    let input_pdo_sizes = read_pdo_assignment_sizes(master_index, slave_index, 0x1C13)?;   // TxPDO → 主站输入
    let output_pdo_sizes = read_pdo_assignment_sizes(master_index, slave_index, 0x1C12)?;  // RxPDO → 主站输出

    if input_pdo_sizes.is_empty() && output_pdo_sizes.is_empty() {
        return Ok(Vec::new());
    }

    let mut result = Vec::with_capacity(detected.len());
    let mut input_cursor: u32 = 0;
    let mut output_cursor: u32 = 0;

    let input_per_module = if !input_pdo_sizes.is_empty() {
        std::cmp::max(1, input_pdo_sizes.len() / detected.len())
    } else { 0 };
    let output_per_module = if !output_pdo_sizes.is_empty() {
        std::cmp::max(1, output_pdo_sizes.len() / detected.len())
    } else { 0 };

    for (i, _ident) in detected.iter().enumerate() {
        let mut input_bytes: u16 = 0;
        let mut output_bytes: u16 = 0;

        if input_per_module > 0 {
            let start = i * input_per_module;
            for j in start..std::cmp::min(start + input_per_module, input_pdo_sizes.len()) {
                input_bytes += input_pdo_sizes[j];
            }
        }

        if output_per_module > 0 {
            let start = i * output_per_module;
            for j in start..std::cmp::min(start + output_per_module, output_pdo_sizes.len()) {
                output_bytes += output_pdo_sizes[j];
            }
        }

        result.push(MdpModulePdoInfo {
            slot_index: (i + 1) as u8,
            input_offset: input_cursor,
            input_size: input_bytes,
            output_offset: output_cursor,
            output_size: output_bytes,
        });

        input_cursor += input_bytes as u32;
        output_cursor += output_bytes as u32;
    }

    Ok(result)
}

/// 读取 PDO Assignment 中各 PDO 的字节大小列表
fn read_pdo_assignment_sizes(master: u16, slave: u16, assignment_index: u16) -> Result<Vec<u16>> {
    let mut sizes = Vec::new();
    let count = match read_u8_sdo(master, slave, assignment_index, 0) {
        Ok(c) => c,
        Err(_) => return Ok(sizes),
    };

    for i in 1..=count {
        let pdo_data = read_u16_sdo(master, slave, assignment_index, i);
        let pdo_mapping_index = match pdo_data {
            Ok(v) if v != 0 => v,
            _ => continue,
        };

        let pdo_bytes = read_pdo_mapping_size(master, slave, pdo_mapping_index);
        sizes.push(pdo_bytes);
    }

    Ok(sizes)
}

/// 读取单个 PDO Mapping 对象的总字节数
fn read_pdo_mapping_size(master: u16, slave: u16, pdo_mapping_index: u16) -> u16 {
    let mut total_bits: u32 = 0;
    let entry_count = match read_u8_sdo(master, slave, pdo_mapping_index, 0) {
        Ok(c) => c,
        Err(_) => return 0,
    };

    for i in 1..=entry_count {
        if let Ok(entry_val) = read_u32_sdo(master, slave, pdo_mapping_index, i) {
            // Byte[0] = BitLength (32位 PDO Mapping Entry 的低8位)
            total_bits += (entry_val & 0xFF) as u32;
        }
    }

    ((total_bits + 7) / 8) as u16
}

/// 读取 u16 值 SDO
fn read_u16_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u16> {
    let mut size: c_int = 0;
    let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
    if ptr.is_null() || size < 2 {
        if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
        return Err(DarraError::SdoReadFailed { index, subindex: sub });
    }
    let val = unsafe {
        u16::from_le_bytes([*ptr, *ptr.add(1)])
    };
    unsafe { ffi::FreeMemory(ptr as *mut _) };
    Ok(val)
}

// ===================== 内部辅助 =====================

/// 通用: 读取一个子索引为 u32 值的对象列表
#[allow(dead_code)]
fn read_u32_object_list(master: u16, slave: u16, od_index: u16) -> Result<Vec<u32>> {
    let count = read_u8_sdo(master, slave, od_index, 0)?;
    if count == 0 {
        return Ok(Vec::new());
    }

    let mut list = Vec::with_capacity(count as usize);
    for sub in 1..=count {
        match read_u32_sdo(master, slave, od_index, sub) {
            Ok(v) => list.push(v),
            Err(_) => continue,
        }
    }
    Ok(list)
}

fn read_u8_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u8> {
    let mut size: c_int = 0;
    let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
    if ptr.is_null() || size < 1 {
        if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
        return Err(DarraError::SdoReadFailed { index, subindex: sub });
    }
    let val = unsafe { *ptr };
    unsafe { ffi::FreeMemory(ptr as *mut _) };
    Ok(val)
}

fn read_u32_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u32> {
    let mut size: c_int = 0;
    let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
    if ptr.is_null() || size < 4 {
        if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
        return Err(DarraError::SdoReadFailed { index, subindex: sub });
    }
    let val = unsafe {
        u32::from_le_bytes([*ptr, *ptr.add(1), *ptr.add(2), *ptr.add(3)])
    };
    unsafe { ffi::FreeMemory(ptr as *mut _) };
    Ok(val)
}

/// 写入 u8 值到 SDO
fn write_u8_sdo(master: u16, slave: u16, index: u16, sub: u8, value: u8) -> Result<()> {
    let data = [value];
    let ret = unsafe {
        ffi::SDOwrite_raw(master, slave, index, sub, 0, data.as_ptr(), 1)
    };
    if ret != 0 {
        Err(DarraError::SdoWriteFailed { index, subindex: sub })
    } else {
        Ok(())
    }
}

/// 写入 u32 值到 SDO (小端)
fn write_u32_sdo(master: u16, slave: u16, index: u16, sub: u8, value: u32) -> Result<()> {
    let data = value.to_le_bytes();
    let ret = unsafe {
        ffi::SDOwrite_raw(master, slave, index, sub, 0, data.as_ptr(), 4)
    };
    if ret != 0 {
        Err(DarraError::SdoWriteFailed { index, subindex: sub })
    } else {
        Ok(())
    }
}