darra-ethercat-master 1.99.7

//! ETG.8200 Mailbox Gateway 服务
//!
//! 允许外部诊断工具通过 UDP/IP 访问 EtherCAT 从站和主站对象字典。
//! 默认 UDP 端口: 0x88A4 (34980) - ETG.8200 标准端口。
//! 当 Address 字段为 0x0000 时，访问主站对象字典 (ETG.1510)。
//!
//! 通过 master.mailbox_gateway() 获取实例。

use crate::utils::ffi;

use std::ffi::CString;
use std::os::raw::{c_void, c_int};
use std::net::UdpSocket;
use std::sync::atomic::{AtomicBool, AtomicU64, Ordering};
use std::sync::Arc;
use std::thread;

/// 默认超时: 5秒 (微秒)
const DEFAULT_TIMEOUT: c_int = 5_000_000;

/// ETG.8200 标准端口
pub const DEFAULT_PORT: u16 = 0x88A4; // 34980

/// Mailbox Gateway 统计信息 (与 C++ MailboxGatewayStats / Java Stats 8 字段对齐)
#[derive(Debug, Default, Clone, Copy)]
pub struct MailboxGatewayStats {
    /// 收到的 UDP 包数
    pub rx_packets: u64,
    /// 发送的响应包数
    pub tx_packets: u64,
    /// 收到的字节数
    pub rx_bytes: u64,
    /// 发送的字节数
    pub tx_bytes: u64,
    /// 路由到主站 OD 的请求数
    pub master_od_requests: u64,
    /// 路由到从站邮箱的请求数
    pub slave_requests: u64,
    /// 帧格式错误丢弃数
    pub drop_malformed: u64,
    /// 处理超时丢弃数
    pub drop_timeout: u64,
}

/// 内部用原子统计 (跨线程共享)
#[derive(Default)]
struct AtomicStats {
    rx_packets: AtomicU64,
    tx_packets: AtomicU64,
    rx_bytes: AtomicU64,
    tx_bytes: AtomicU64,
    master_od_requests: AtomicU64,
    slave_requests: AtomicU64,
    drop_malformed: AtomicU64,
    drop_timeout: AtomicU64,
}

impl AtomicStats {
    fn snapshot(&self) -> MailboxGatewayStats {
        MailboxGatewayStats {
            rx_packets: self.rx_packets.load(Ordering::Relaxed),
            tx_packets: self.tx_packets.load(Ordering::Relaxed),
            rx_bytes: self.rx_bytes.load(Ordering::Relaxed),
            tx_bytes: self.tx_bytes.load(Ordering::Relaxed),
            master_od_requests: self.master_od_requests.load(Ordering::Relaxed),
            slave_requests: self.slave_requests.load(Ordering::Relaxed),
            drop_malformed: self.drop_malformed.load(Ordering::Relaxed),
            drop_timeout: self.drop_timeout.load(Ordering::Relaxed),
        }
    }

    fn reset(&self) {
        self.rx_packets.store(0, Ordering::Relaxed);
        self.tx_packets.store(0, Ordering::Relaxed);
        self.rx_bytes.store(0, Ordering::Relaxed);
        self.tx_bytes.store(0, Ordering::Relaxed);
        self.master_od_requests.store(0, Ordering::Relaxed);
        self.slave_requests.store(0, Ordering::Relaxed);
        self.drop_malformed.store(0, Ordering::Relaxed);
        self.drop_timeout.store(0, Ordering::Relaxed);
    }
}

/// ETG.8200 Mailbox Gateway 服务
///
/// 提供 UDP 服务器，允许外部诊断工具通过 ETG.8200 协议访问
/// EtherCAT 从站邮箱和主站对象字典 (ETG.1510)。
///
/// # 示例
/// ```no_run
/// use std::sync::Arc;
/// let master = Arc::new(EtherCATMaster::new().unwrap());
/// let mut gw = MailboxGatewayService::new(master);
/// gw.start().unwrap();
/// // ... 诊断工具可通过 UDP 34980 端口访问
/// gw.stop();
/// ```
pub struct MailboxGatewayService {
    /// 主站引用 (Arc 以支持多线程)
    master_index: u16,
    /// UDP 端口号
    port: u16,
    /// 是否正在运行
    running: Arc<AtomicBool>,
    /// 监听线程句柄
    listener_thread: Option<thread::JoinHandle<()>>,
    /// 用于停止的 socket (关闭以中断阻塞接收)
    stop_socket: Option<Arc<UdpSocket>>,
    /// 邮箱计数器 (1-7, 原子操作)
    _mailbox_counter: std::sync::atomic::AtomicU8,
    /// 统计信息 (原子, 跨线程共享)
    stats: Arc<AtomicStats>,
}

impl MailboxGatewayService {
    /// 创建 Mailbox Gateway 服务实例
    ///
    /// # 参数
    /// - `master_index`: 主站索引
    pub fn new(master_index: u16) -> Self {
        Self {
            master_index,
            port: DEFAULT_PORT + master_index.saturating_sub(1),
            running: Arc::new(AtomicBool::new(false)),
            listener_thread: None,
            stop_socket: None,
            _mailbox_counter: std::sync::atomic::AtomicU8::new(0),
            stats: Arc::new(AtomicStats::default()),
        }
    }

    /// 获取统计信息快照 (对应 C++ GetStatistics / Java getStatistics)
    pub fn get_statistics(&self) -> MailboxGatewayStats {
        self.stats.snapshot()
    }

    /// 重置所有统计计数 (对应 Java resetStatistics)
    pub fn reset_statistics(&self) {
        self.stats.reset();
    }

    /// 启动 Mailbox Gateway 服务
    pub fn start(&mut self) -> Result<(), String> {
        if self.running.load(Ordering::SeqCst) {
            return Ok(()); // 已在运行
        }

        // 停止旧实例
        self.stop_internal();

        // 绑定 UDP 端口: 优先期望端口 -> 10个备用端口 -> 系统分配
        let socket = self.bind_udp_port(self.port)?;
        let actual_port = socket.local_addr()
            .map(|a| a.port())
            .unwrap_or(self.port);
        self.port = actual_port;

        let socket = Arc::new(socket);
        self.stop_socket = Some(socket.clone());
        self.running.store(true, Ordering::SeqCst);

        let running = self.running.clone();
        let master_index = self.master_index;
        let stats = self.stats.clone();

        // 启动监听线程
        self.listener_thread = Some(thread::spawn(move || {
            listen_loop(socket, running, master_index, stats);
        }));

        Ok(())
    }

    /// 停止 Mailbox Gateway 服务
    pub fn stop(&mut self) {
        self.stop_internal();
    }

    /// 是否正在运行
    pub fn is_running(&self) -> bool {
        self.running.load(Ordering::SeqCst)
    }

    /// 获取 UDP 端口号
    pub fn port(&self) -> u16 {
        self.port
    }

    /// 设置 UDP 端口号 (仅在停止状态下可设置)
    pub fn set_port(&mut self, port: u16) {
        if !self.running.load(Ordering::SeqCst) {
            self.port = port;
        }
    }

    /// 内部停止方法
    fn stop_internal(&mut self) {
        if !self.running.load(Ordering::SeqCst) && self.listener_thread.is_none() {
            return;
        }

        self.running.store(false, Ordering::SeqCst);

        // 关闭 socket 以中断阻塞接收
        if let Some(ref socket) = self.stop_socket {
            // 发送空数据到自身以中断 recv_from 阻塞
            let _ = UdpSocket::bind("0.0.0.0:0")
                .and_then(|s| s.send_to(&[0], socket.local_addr().unwrap_or_else(|_| {
                    std::net::SocketAddr::from(([127, 0, 0, 1], self.port))
                })));
        }
        self.stop_socket = None;

        // 等待线程结束
        if let Some(handle) = self.listener_thread.take() {
            let _ = handle.join();
        }
    }

    /// 绑定 UDP 端口
    fn bind_udp_port(&self, preferred_port: u16) -> Result<UdpSocket, String> {
        // 尝试标准端口和 10 个备用端口
        for i in 0..10u16 {
            let port = preferred_port.wrapping_add(i);
            match UdpSocket::bind(format!("0.0.0.0:{}", port)) {
                Ok(socket) => return Ok(socket),
                Err(_) => continue,
            }
        }

        // 所有端口都不可用，让系统分配
        UdpSocket::bind("0.0.0.0:0")
            .map_err(|e| format!("无法绑定 UDP 端口: {}", e))
    }
}

impl Drop for MailboxGatewayService {
    fn drop(&mut self) {
        self.stop_internal();
    }
}

// ===================== 监听循环 =====================

/// UDP 监听循环 (在独立线程中运行)
fn listen_loop(
    socket: Arc<UdpSocket>,
    running: Arc<AtomicBool>,
    master_index: u16,
    stats: Arc<AtomicStats>,
) {
    let mut buf = [0u8; 2048];

    // 设置读超时，避免永久阻塞
    let _ = socket.set_read_timeout(Some(std::time::Duration::from_millis(500)));

    while running.load(Ordering::SeqCst) {
        match socket.recv_from(&mut buf) {
            Ok((len, remote_addr)) => {
                if !running.load(Ordering::SeqCst) { break; }
                stats.rx_packets.fetch_add(1, Ordering::Relaxed);
                stats.rx_bytes.fetch_add(len as u64, Ordering::Relaxed);

                if len < 9 {
                    // ETG.8200 最小帧长度 = ECAT(2) + MBX(6) + 至少 1 字节
                    stats.drop_malformed.fetch_add(1, Ordering::Relaxed);
                    continue;
                }

                // 处理请求并发送响应 (None = 协议解析失败或处理无响应, 已在内部按需计入 drop_malformed)
                if let Some(response) = process_request(&buf[..len], master_index, &stats) {
                    match socket.send_to(&response, remote_addr) {
                        Ok(sent) => {
                            stats.tx_packets.fetch_add(1, Ordering::Relaxed);
                            stats.tx_bytes.fetch_add(sent as u64, Ordering::Relaxed);
                        }
                        Err(_) => {
                            stats.drop_timeout.fetch_add(1, Ordering::Relaxed);
                        }
                    }
                }
            }
            Err(ref e) if e.kind() == std::io::ErrorKind::TimedOut
                || e.kind() == std::io::ErrorKind::WouldBlock => {
                continue; // 超时，继续循环检查 running 标志
            }
            Err(_) => {
                if running.load(Ordering::SeqCst) {
                    // 非预期错误，短暂等待后重试
                    thread::sleep(std::time::Duration::from_millis(10));
                } else {
                    break;
                }
            }
        }
    }
}

// ===================== 请求处理 =====================

/// 处理 ETG.8200 请求
fn process_request(data: &[u8], master_index: u16, stats: &Arc<AtomicStats>) -> Option<Vec<u8>> {
    // ECAT Header(2) + MBX Header(6) = 至少 8 字节
    if data.len() < 8 {
        stats.drop_malformed.fetch_add(1, Ordering::Relaxed);
        return None;
    }

    // 解析 EtherCAT Header (2 bytes)
    let ecat_header = u16::from_le_bytes([data[0], data[1]]);
    let ecat_data_type = ((ecat_header >> 12) & 0x0F) as u8;

    // 验证数据类型为邮箱通信 (0x05)
    if ecat_data_type != 0x05 {
        stats.drop_malformed.fetch_add(1, Ordering::Relaxed);
        return None;
    }

    // 解析 Mailbox Header (6 bytes)
    let mb_length = u16::from_le_bytes([data[2], data[3]]) as usize;
    let address = u16::from_le_bytes([data[4], data[5]]);
    let _channel_prio = data[6];
    let type_cnt_res = data[7];

    let mailbox_type = (type_cnt_res >> 4) & 0x0F;
    let _mailbox_cnt = (type_cnt_res >> 1) & 0x07;

    // 提取邮箱数据 (容错: 截断到实际长度而非直接 None, 与 C++ 对齐)
    let mb_data_start = 8;
    let avail = data.len().saturating_sub(mb_data_start);
    let actual_len = mb_length.min(avail);
    let mailbox_data = &data[mb_data_start..mb_data_start + actual_len];

    let response_data = if address == 0x0000 {
        // 访问主站对象字典 (ETG.1510)
        stats.master_od_requests.fetch_add(1, Ordering::Relaxed);
        process_master_request(master_index, mailbox_type, mailbox_data)
    } else {
        // 访问从站邮箱
        stats.slave_requests.fetch_add(1, Ordering::Relaxed);
        process_slave_request(master_index, address, mailbox_type, mailbox_data)
    };

    // 构建响应帧
    response_data.map(|resp| build_response_frame(address, mailbox_type, &resp))
}

/// 处理主站对象字典请求 (ETG.1510)
fn process_master_request(master_index: u16, mailbox_type: u8, mailbox_data: &[u8]) -> Option<Vec<u8>> {
    // 仅支持 CoE (0x03)
    if mailbox_type != 0x03 {
        return Some(build_coe_abort_response(0x06010000)); // Unsupported access
    }

    if mailbox_data.len() < 6 { return None; }

    // 解析 CoE SDO 请求
    let coe_type = (mailbox_data[1] >> 4) & 0x0F;
    if coe_type != 0x02 { return None; } // 不是 SDO

    let sdo_command = mailbox_data[2];
    let index = u16::from_le_bytes([mailbox_data[3], mailbox_data[4]]);
    let subindex = mailbox_data[5];

    let is_upload = (sdo_command & 0x40) != 0;
    let is_download = (sdo_command & 0x20) != 0 && (sdo_command & 0x40) == 0;

    // 临时创建 MasterObjectDictionary (无需持久化)
    // 注意: 这里无法安全获取 EtherCATMaster 引用, 使用 FFI 直接操作
    if is_upload {
        // SDO Upload (Read) - 通过构造临时 OD 实例
        let data = read_master_od_object(master_index, index, subindex);
        match data {
            Some(d) => Some(build_coe_upload_response(index, subindex, &d)),
            None => Some(build_coe_abort_response(0x06020000)), // Object does not exist
        }
    } else if is_download {
        // SDO Download (Write)
        let data_offset = 6;
        if mailbox_data.len() > data_offset {
            let write_data = &mailbox_data[data_offset..];
            write_master_od_object(master_index, index, subindex, write_data)
        } else {
            Some(build_coe_abort_response(0x05040001))
        }
    } else {
        Some(build_coe_abort_response(0x05040001)) // Command not implemented
    }
}

/// 直接读取主站 OD 对象 (不依赖 EtherCATMaster 引用)
fn read_master_od_object(master_index: u16, index: u16, subindex: u8) -> Option<Vec<u8>> {
    match index {
        0x1000 => Some(0x000011A4u32.to_le_bytes().to_vec()),
        0x1008 => Some(b"Darra EtherCAT Master".to_vec()),
        0x1009 => Some(b"1.0".to_vec()),
        0x100A => {
            let ptr = unsafe { ffi::GetDllVersionInfo() };
            if ptr.is_null() { return Some(b"0.0.0.0".to_vec()); }
            let info = unsafe { std::ptr::read_unaligned(ptr) };
            // packed struct 字段需要用 {} 强制按值拷贝 (避免未对齐引用)
            let ver = format!("{}.{}.{}.{}", {info.major}, {info.minor}, {info.patch}, {info.build});
            Some(ver.into_bytes())
        }
        0x1018 => {
            match subindex {
                0 => Some(vec![4]),
                1 => Some(0x00001164u32.to_le_bytes().to_vec()),
                2 => Some(0x00000001u32.to_le_bytes().to_vec()),
                3 => {
                    let ptr = unsafe { ffi::GetDllVersionInfo() };
                    if ptr.is_null() { return Some(vec![0; 4]); }
                    let info = unsafe { std::ptr::read_unaligned(ptr) };
                    let rev = ((info.major as u32) << 16) | (info.minor as u32);
                    Some(rev.to_le_bytes().to_vec())
                }
                4 => Some(0u32.to_le_bytes().to_vec()),
                _ => None,
            }
        }
        0xF120 => {
            // Master Diag Data
            let mut diag = ffi::MasterDiagData {
                cyclic_lost_frames: 0, acyclic_lost_frames: 0,
                cyclic_frames_per_sec: 0, acyclic_frames_per_sec: 0,
                master_state: 0,
            };
            unsafe { ffi::GetMasterDiagData(master_index, &mut diag) };
            match subindex {
                0 => Some(vec![16]),
                1 => Some(diag.cyclic_lost_frames.to_le_bytes().to_vec()),
                2 => Some(diag.acyclic_lost_frames.to_le_bytes().to_vec()),
                3 => Some(diag.cyclic_frames_per_sec.to_le_bytes().to_vec()),
                4 => Some(diag.acyclic_frames_per_sec.to_le_bytes().to_vec()),
                5 => Some(diag.master_state.to_le_bytes().to_vec()),
                _ => Some(vec![0; 4]),
            }
        }
        _ => None,
    }
}

/// 写入主站 OD 对象
///
/// 对已知可写索引调用对应 DLL 功能, 未知索引返回中止码
fn write_master_od_object(master_index: u16, index: u16, subindex: u8, data: &[u8]) -> Option<Vec<u8>> {
    match index {
        0xF120 => {
            // 诊断控制: subindex 决定具体操作
            match subindex {
                // subindex 5 = master_state 控制 (启用/禁用诊断)
                5 => {
                    if data.len() >= 4 {
                        let val = u32::from_le_bytes([data[0], data[1], data[2], data[3]]);
                        unsafe { ffi::SetDiagnosticsEnabled(master_index, val as i32) };
                        Some(build_coe_download_response(index, subindex))
                    } else {
                        Some(build_coe_abort_response(0x06070010)) // 数据长度不匹配
                    }
                }
                _ => Some(build_coe_abort_response(0x06010002)), // 只读子索引
            }
        }
        // 只读对象
        0x1000 | 0x1008 | 0x1009 | 0x100A | 0x1018 => {
            Some(build_coe_abort_response(0x06010002)) // 写保护
        }
        _ => Some(build_coe_abort_response(0x06010001)), // 对象不存在
    }
}

/// 处理从站邮箱请求
fn process_slave_request(
    master_index: u16,
    address: u16,
    mailbox_type: u8,
    mailbox_data: &[u8],
) -> Option<Vec<u8>> {
    match mailbox_type {
        0x03 => process_slave_coe_request(master_index, address, mailbox_data),
        0x04 => process_slave_foe_request(master_index, address, mailbox_data),
        0x05 => process_slave_soe_request(master_index, address, mailbox_data),
        0x0F => process_slave_voe_request(master_index, address, mailbox_data),
        _ => Some(build_coe_abort_response(0x06010000)), // 不支持的邮箱类型
    }
}

/// 处理从站 CoE (SDO) 请求
fn process_slave_coe_request(
    master_index: u16,
    address: u16,
    mailbox_data: &[u8],
) -> Option<Vec<u8>> {
    if mailbox_data.len() < 6 { return None; }

    let sdo_command = mailbox_data[2];
    let index = u16::from_le_bytes([mailbox_data[3], mailbox_data[4]]);
    let subindex = mailbox_data[5];

    let is_upload = (sdo_command & 0x40) != 0;

    if is_upload {
        // 通过 DLL SDO Read 转发
        let slave = crate::slave::Slave::new(master_index, address);
        match slave.sdo_read(index, subindex, false) {
            Ok(data) => Some(build_coe_upload_response(index, subindex, &data)),
            Err(_) => Some(build_coe_abort_response(0x06090011)), // 子索引不存在
        }
    } else {
        // SDO Download - 转发写入
        let data_offset = 6;
        if mailbox_data.len() > data_offset {
            let write_data = &mailbox_data[data_offset..];
            let slave = crate::slave::Slave::new(master_index, address);
            match slave.sdo_write(index, subindex, false, write_data) {
                Ok(_) => Some(build_coe_download_response(index, subindex)),
                Err(_) => Some(build_coe_abort_response(0x06010002)), // 写保护
            }
        } else {
            Some(build_coe_abort_response(0x05040001))
        }
    }
}

// ===================== SoE 请求处理 =====================

/// 处理从站 SoE (Servo over EtherCAT) 请求
///
/// SoE 请求帧格式:
///   [0]: op_code (1=读取, 2=写入)
///   [1]: drive_no (高4位) + element_flags (低4位)
///   [2..4]: IDN (小端序)
///   [4..]: 写入数据 (仅写入操作)
fn process_slave_soe_request(
    master_index: u16,
    address: u16,
    mailbox_data: &[u8],
) -> Option<Vec<u8>> {
    if mailbox_data.len() < 4 { return Some(build_soe_error_response(0, 0, 0x7001)); }

    // 解析 SoE 请求头
    let op_code = mailbox_data[0] & 0x07;
    let drive_no = (mailbox_data[1] >> 4) & 0x0F;
    let element_flags = mailbox_data[1] & 0x0F;
    let idn = u16::from_le_bytes([mailbox_data[2], mailbox_data[3]]);

    match op_code {
        1 => {
            // SoE 读取
            let mut data_ptr: *mut c_void = std::ptr::null_mut();
            let mut data_size: c_int = 0;

            let ret = unsafe {
                ffi::SoERead(
                    master_index, address, drive_no, element_flags, idn,
                    &mut data_ptr, &mut data_size, DEFAULT_TIMEOUT,
                )
            };

            if ret == 0 && !data_ptr.is_null() && data_size > 0 {
                // 从 DLL 分配的内存中复制数据
                let data = unsafe {
                    std::slice::from_raw_parts(data_ptr as *const u8, data_size as usize)
                }.to_vec();
                unsafe { ffi::FreeMemory(data_ptr); }
                Some(build_soe_response(drive_no, element_flags, idn, &data))
            } else {
                // 读取失败, 释放可能分配的内存
                if !data_ptr.is_null() {
                    unsafe { ffi::FreeMemory(data_ptr); }
                }
                Some(build_soe_error_response(drive_no, idn, 0x7002))
            }
        }
        2 => {
            // SoE 写入
            if mailbox_data.len() <= 4 {
                return Some(build_soe_error_response(drive_no, idn, 0x7001));
            }
            let write_data = &mailbox_data[4..];

            let ret = unsafe {
                ffi::SoEWrite(
                    master_index, address, drive_no, element_flags, idn,
                    write_data.as_ptr(), write_data.len() as c_int, DEFAULT_TIMEOUT,
                )
            };

            if ret == 0 {
                // 写入成功, 返回空数据的确认响应
                Some(build_soe_response(drive_no, element_flags, idn, &[]))
            } else {
                Some(build_soe_error_response(drive_no, idn, 0x7002))
            }
        }
        _ => Some(build_soe_error_response(drive_no, idn, 0x7001)), // 不支持的操作码
    }
}

// ===================== FoE 请求处理 =====================

/// 处理从站 FoE (File over EtherCAT) 请求
///
/// FoE 请求帧格式:
///   [0]: op_code (1=读取/下载请求, 2=写入/上传数据)
///   [1]: 保留
///   [2..6]: password (小端序, 4字节)
///   [6..]: filename (以 null 结尾的字符串, 读取时) 或文件数据 (写入时)
fn process_slave_foe_request(
    master_index: u16,
    address: u16,
    mailbox_data: &[u8],
) -> Option<Vec<u8>> {
    if mailbox_data.len() < 6 { return Some(build_foe_error_response(0x8001)); }

    let op_code = mailbox_data[0];
    let password = u32::from_le_bytes([
        mailbox_data[2], mailbox_data[3], mailbox_data[4], mailbox_data[5],
    ]);

    match op_code {
        1 => {
            // FoE 读取 (下载请求): 从从站读取文件
            let filename_bytes = &mailbox_data[6..];
            // 查找 null 结尾或使用全部字节
            let name_end = filename_bytes.iter().position(|&b| b == 0)
                .unwrap_or(filename_bytes.len());
            let filename_str = std::str::from_utf8(&filename_bytes[..name_end])
                .unwrap_or("");
            let c_filename = match CString::new(filename_str) {
                Ok(s) => s,
                Err(_) => return Some(build_foe_error_response(0x8002)),
            };

            let mut file_data: *mut c_void = std::ptr::null_mut();
            let mut file_size: c_int = 0;

            let ret = unsafe {
                ffi::FOERead(
                    master_index, address, c_filename.as_ptr(),
                    password, &mut file_data, &mut file_size, DEFAULT_TIMEOUT,
                )
            };

            if ret == 0 && !file_data.is_null() && file_size > 0 {
                let data = unsafe {
                    std::slice::from_raw_parts(file_data as *const u8, file_size as usize)
                }.to_vec();
                unsafe { ffi::FreeMemory(file_data); }
                Some(build_foe_data_response(&data))
            } else {
                if !file_data.is_null() {
                    unsafe { ffi::FreeMemory(file_data); }
                }
                Some(build_foe_error_response(0x8001))
            }
        }
        2 => {
            // FoE 写入 (上传数据): 向从站写入文件
            // 写入请求中, [6..] 前面包含文件名, 后面是数据
            // 简化处理: 从 [6..] 提取 null 结尾文件名 + 剩余数据
            let payload = &mailbox_data[6..];
            let name_end = payload.iter().position(|&b| b == 0)
                .unwrap_or(0);

            let filename_str = std::str::from_utf8(&payload[..name_end])
                .unwrap_or("");
            let c_filename = match CString::new(filename_str) {
                Ok(s) => s,
                Err(_) => return Some(build_foe_error_response(0x8002)),
            };

            // 数据在文件名 null 结尾之后
            let data_start = if name_end < payload.len() { name_end + 1 } else { payload.len() };
            let file_data = &payload[data_start..];

            let ret = unsafe {
                ffi::FOEWrite(
                    master_index, address, c_filename.as_ptr(),
                    password, file_data.as_ptr() as *const c_void,
                    file_data.len() as c_int, DEFAULT_TIMEOUT,
                )
            };

            if ret == 0 {
                Some(build_foe_ack_response())
            } else {
                Some(build_foe_error_response(0x8001))
            }
        }
        _ => Some(build_foe_error_response(0x8003)), // 不支持的操作码
    }
}

// ===================== VoE 请求处理 =====================

/// 处理从站 VoE (Vendor over EtherCAT) 请求
///
/// VoE 请求帧格式:
///   [0..4]: vendor_id (小端序, 4字节)
///   [4..6]: vendor_type (小端序, 2字节)
///   [6..]: 厂商特定数据
fn process_slave_voe_request(
    master_index: u16,
    address: u16,
    mailbox_data: &[u8],
) -> Option<Vec<u8>> {
    if mailbox_data.len() < 6 { return Some(build_voe_error_response()); }

    // 解析 VoE 头: vendor_id + vendor_type
    let _vendor_id = u32::from_le_bytes([
        mailbox_data[0], mailbox_data[1], mailbox_data[2], mailbox_data[3],
    ]);
    let _vendor_type = u16::from_le_bytes([mailbox_data[4], mailbox_data[5]]);

    // 发送原始 VoE 帧 (包含完整 VoE 头 + 数据)
    let ret_send = unsafe {
        ffi::VOESendRaw(
            master_index, address,
            mailbox_data.as_ptr(), mailbox_data.len() as c_int,
            DEFAULT_TIMEOUT,
        )
    };

    if ret_send != 0 {
        return Some(build_voe_error_response());
    }

    // 接收 VoE 响应
    let mut recv_data: *mut c_void = std::ptr::null_mut();
    let mut recv_size: c_int = 0;

    let ret_recv = unsafe {
        ffi::VOEReceiveRaw(
            master_index, address,
            &mut recv_data, &mut recv_size,
            DEFAULT_TIMEOUT,
        )
    };

    if ret_recv == 0 && !recv_data.is_null() && recv_size > 0 {
        let data = unsafe {
            std::slice::from_raw_parts(recv_data as *const u8, recv_size as usize)
        }.to_vec();
        unsafe { ffi::FreeMemory(recv_data); }
        // 返回接收到的原始 VoE 帧作为响应
        Some(data)
    } else {
        if !recv_data.is_null() {
            unsafe { ffi::FreeMemory(recv_data); }
        }
        Some(build_voe_error_response())
    }
}

// ===================== 响应帧构建 =====================

/// 构建 ETG.8200 响应帧
fn build_response_frame(address: u16, mailbox_type: u8, data: &[u8]) -> Vec<u8> {
    let mb_length = data.len() as u16;
    // EtherCAT Header: length(11bits) | reserved(1bit) | type(4bits)
    let ecat_header: u16 = (mb_length + 6) & 0x07FF | (0x05 << 12);

    let mut frame = Vec::with_capacity(8 + data.len());
    // EtherCAT Header
    frame.extend_from_slice(&ecat_header.to_le_bytes());
    // Mailbox Header
    frame.extend_from_slice(&mb_length.to_le_bytes()); // Length
    frame.extend_from_slice(&address.to_le_bytes());    // Address
    frame.push(0x00);                                   // Channel + Priority
    frame.push((mailbox_type << 4) | 0x02);            // Type + Cnt
    // Mailbox Data
    frame.extend_from_slice(data);

    frame
}

/// 构建 CoE SDO Upload 响应
fn build_coe_upload_response(index: u16, subindex: u8, data: &[u8]) -> Vec<u8> {
    let mut resp = Vec::with_capacity(6 + data.len());
    // CoE Header (2 bytes): Number=0, Type=SDO(0x02)
    resp.push(0x00);
    resp.push(0x20); // SDO type
    // SDO Upload Response: command=0x43 (expedited, 4 bytes) 或 0x41 (normal)
    if data.len() <= 4 {
        // Expedited transfer
        let size_indicator = (4 - data.len()) as u8;
        resp.push(0x43 | (size_indicator << 2));
    } else {
        // Normal transfer
        resp.push(0x41);
    }
    resp.extend_from_slice(&index.to_le_bytes());
    resp.push(subindex);
    if data.len() <= 4 {
        // 填充到 4 字节
        resp.extend_from_slice(data);
        for _ in data.len()..4 {
            resp.push(0x00);
        }
    } else {
        // 先写长度再写数据
        resp.extend_from_slice(&(data.len() as u32).to_le_bytes());
        resp.extend_from_slice(data);
    }
    resp
}

/// 构建 CoE SDO Download 响应 (确认)
fn build_coe_download_response(index: u16, subindex: u8) -> Vec<u8> {
    let mut resp = Vec::with_capacity(6);
    // CoE Header
    resp.push(0x00);
    resp.push(0x20); // SDO type
    // SDO Download Response: command=0x60
    resp.push(0x60);
    resp.extend_from_slice(&index.to_le_bytes());
    resp.push(subindex);
    resp
}

/// 构建 CoE SDO Abort 响应
fn build_coe_abort_response(abort_code: u32) -> Vec<u8> {
    let mut resp = Vec::with_capacity(10);
    // CoE Header
    resp.push(0x00);
    resp.push(0x20); // SDO type
    // SDO Abort: command=0x80
    resp.push(0x80);
    resp.push(0x00); // index low
    resp.push(0x00); // index high
    resp.push(0x00); // subindex
    resp.extend_from_slice(&abort_code.to_le_bytes());
    resp
}

// ===================== SoE 响应帧构建 =====================

/// 构建 SoE 正常响应
///
/// 响应帧格式:
///   [0]: op_code=3 (读取响应) 或 op_code=0 (写入确认)
///   [1]: drive_no(高4位) + element_flags(低4位)
///   [2..4]: IDN (小端序)
///   [4..]: 数据 (仅读取响应)
fn build_soe_response(drive_no: u8, element_flags: u8, idn: u16, data: &[u8]) -> Vec<u8> {
    let op_code: u8 = if data.is_empty() { 0x00 } else { 0x03 }; // 写入确认 / 读取响应
    let mut resp = Vec::with_capacity(4 + data.len());
    resp.push(op_code);
    resp.push((drive_no << 4) | (element_flags & 0x0F));
    resp.extend_from_slice(&idn.to_le_bytes());
    if !data.is_empty() {
        resp.extend_from_slice(data);
    }
    resp
}

/// 构建 SoE 错误响应
///
/// 错误帧格式:
///   [0]: op_code=4 (错误)
///   [1]: drive_no(高4位)
///   [2..4]: IDN
///   [4..6]: error_code (小端序)
fn build_soe_error_response(drive_no: u8, idn: u16, error_code: u16) -> Vec<u8> {
    let mut resp = Vec::with_capacity(6);
    resp.push(0x04); // 错误操作码
    resp.push(drive_no << 4);
    resp.extend_from_slice(&idn.to_le_bytes());
    resp.extend_from_slice(&error_code.to_le_bytes());
    resp
}

// ===================== FoE 响应帧构建 =====================

/// 构建 FoE 数据响应 (读取成功)
///
/// 响应帧格式:
///   [0]: op_code=3 (数据)
///   [1]: 保留
///   [2..6]: packet_number (小端序, 4字节)
///   [6..]: 文件数据
fn build_foe_data_response(data: &[u8]) -> Vec<u8> {
    let mut resp = Vec::with_capacity(6 + data.len());
    resp.push(0x03); // op_code = DATA
    resp.push(0x00); // 保留
    resp.extend_from_slice(&1u32.to_le_bytes()); // packet_number = 1 (单包传输)
    resp.extend_from_slice(data);
    resp
}

/// 构建 FoE 确认响应 (写入成功)
///
/// 响应帧格式:
///   [0]: op_code=4 (ACK)
///   [1]: 保留
///   [2..6]: packet_number (小端序, 4字节)
fn build_foe_ack_response() -> Vec<u8> {
    let mut resp = Vec::with_capacity(6);
    resp.push(0x04); // op_code = ACK
    resp.push(0x00); // 保留
    resp.extend_from_slice(&1u32.to_le_bytes()); // packet_number = 1
    resp
}

/// 构建 FoE 错误响应
///
/// 错误帧格式:
///   [0]: op_code=5 (ERROR)
///   [1]: 保留
///   [2..6]: error_code (小端序, 4字节)
fn build_foe_error_response(error_code: u32) -> Vec<u8> {
    let mut resp = Vec::with_capacity(6);
    resp.push(0x05); // op_code = ERROR
    resp.push(0x00); // 保留
    resp.extend_from_slice(&error_code.to_le_bytes());
    resp
}

// ===================== VoE 响应帧构建 =====================

/// 构建 VoE 错误响应
///
/// 返回一个空 VoE 帧表示错误 (VoE 协议无标准错误格式)
/// 帧格式:
///   [0..4]: vendor_id = 0
///   [4..6]: vendor_type = 0xFFFF (错误标志)
fn build_voe_error_response() -> Vec<u8> {
    let mut resp = Vec::with_capacity(6);
    resp.extend_from_slice(&0u32.to_le_bytes());    // vendor_id = 0
    resp.extend_from_slice(&0xFFFFu16.to_le_bytes()); // vendor_type = 0xFFFF (错误)
    resp
}