use std::collections::HashMap;
use crate::data::error::{DarraError, Result};
use crate::utils::ffi;
use std::os::raw::c_int;
use std::ptr::NonNull;
#[derive(Debug, Clone)]
pub struct WatchPoint {
pub name: String,
pub byte_offset: u32,
pub byte_count: u32,
snapshot: Vec<u8>,
}
impl WatchPoint {
fn new(name: impl Into<String>, byte_offset: u32, byte_count: u32) -> Self {
Self {
name: name.into(),
byte_offset,
byte_count,
snapshot: vec![0u8; byte_count as usize],
}
}
}
#[derive(Debug, Clone)]
pub struct PdoChange {
pub watch_name: String,
pub slave_index: u16,
pub old_data: Vec<u8>,
pub new_data: Vec<u8>,
}
impl PdoChange {
pub fn format_hex(&self) -> String {
let old_hex: String = self.old_data.iter().map(|b| format!("{:02X}", b)).collect::<Vec<_>>().join(" ");
let new_hex: String = self.new_data.iter().map(|b| format!("{:02X}", b)).collect::<Vec<_>>().join(" ");
format!("[{}] 从站 {}: {} -> {}", self.watch_name, self.slave_index, old_hex, new_hex)
}
}
pub struct PdoMonitor {
master_index: u16,
watches: HashMap<(u16, String), WatchPoint>,
pub total_changes: u64,
}
impl PdoMonitor {
pub fn new(master_index: u16) -> Self {
Self {
master_index,
watches: HashMap::new(),
total_changes: 0,
}
}
pub fn watch(
&mut self,
slave_index: u16,
name: impl Into<String>,
byte_offset: u32,
byte_count: u32,
) {
let name = name.into();
let initial = self.read_input_bytes(slave_index, byte_offset, byte_count)
.unwrap_or_else(|_| vec![0u8; byte_count as usize]);
let mut wp = WatchPoint::new(name.clone(), byte_offset, byte_count);
wp.snapshot = initial;
self.watches.insert((slave_index, name), wp);
}
pub fn unwatch(&mut self, slave_index: u16, name: &str) {
self.watches.remove(&(slave_index, name.to_string()));
}
pub fn unwatch_slave(&mut self, slave_index: u16) {
self.watches.retain(|(s, _), _| *s != slave_index);
}
pub fn check(&mut self) -> Vec<PdoChange> {
let mut changes = Vec::new();
let keys: Vec<(u16, String)> = self.watches.keys().cloned().collect();
for key in keys {
let (byte_offset, byte_count) = match self.watches.get(&key) {
Some(v) => (v.byte_offset, v.byte_count),
None => continue,
};
let slave_index = key.0;
let current = match self.read_input_bytes(slave_index, byte_offset, byte_count) {
Ok(v) => v,
Err(_) => continue,
};
let wp = match self.watches.get_mut(&key) {
Some(v) => v,
None => continue,
};
if current != wp.snapshot {
let change = PdoChange {
watch_name: wp.name.clone(),
slave_index,
old_data: wp.snapshot.clone(),
new_data: current.clone(),
};
wp.snapshot = current;
self.total_changes += 1;
changes.push(change);
}
}
changes
}
pub fn check_slave(&mut self, slave_index: u16) -> Vec<PdoChange> {
self.check()
.into_iter()
.filter(|c| c.slave_index == slave_index)
.collect()
}
pub fn reset_snapshots(&mut self) {
let keys: Vec<(u16, String)> = self.watches.keys().cloned().collect();
for key in keys {
let slave_index = key.0;
let (byte_offset, byte_count) = match self.watches.get(&key) {
Some(v) => (v.byte_offset, v.byte_count),
None => continue,
};
if let Ok(data) = self.read_input_bytes(slave_index, byte_offset, byte_count) {
if let Some(wp) = self.watches.get_mut(&key) {
wp.snapshot = data;
}
}
}
}
pub fn watch_count(&self) -> usize {
self.watches.len()
}
pub fn watch_count_for_slave(&self, slave_index: u16) -> usize {
self.watches.keys().filter(|(s, _)| *s == slave_index).count()
}
pub fn watch_list(&self) -> Vec<(u16, &str)> {
self.watches.keys()
.map(|(s, n)| (*s, n.as_str()))
.collect()
}
fn read_input_bytes(&self, slave_index: u16, offset: u32, count: u32) -> Result<Vec<u8>> {
let mut out_size: c_int = 0;
let mut out_ptr: *mut u8 = std::ptr::null_mut();
let mut in_size: c_int = 0;
let mut in_ptr: *mut u8 = std::ptr::null_mut();
let ok = unsafe {
ffi::GetIO(self.master_index, slave_index,
&mut out_size, &mut out_ptr,
&mut in_size, &mut in_ptr)
};
if ok == 0 || in_ptr.is_null() || in_size <= 0 {
return Err(crate::data::error::DarraError::NullPointer);
}
let available = in_size as u32;
let end = offset.saturating_add(count).min(available);
if offset >= end {
return Ok(vec![0u8; count as usize]);
}
let actual_count = (end - offset) as usize;
let mut buf = vec![0u8; count as usize];
unsafe {
std::ptr::copy_nonoverlapping(
in_ptr.add(offset as usize),
buf.as_mut_ptr(),
actual_count,
);
}
Ok(buf)
}
}
impl std::fmt::Debug for PdoMonitor {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
f.debug_struct("PdoMonitor")
.field("master_index", &self.master_index)
.field("watch_count", &self.watch_count())
.field("total_changes", &self.total_changes)
.finish()
}
}
pub fn read_le_u16(data: &[u8], offset: usize) -> u16 {
if offset + 2 > data.len() { return 0; }
u16::from_le_bytes([data[offset], data[offset + 1]])
}
pub fn read_le_u32(data: &[u8], offset: usize) -> u32 {
if offset + 4 > data.len() { return 0; }
u32::from_le_bytes([data[offset], data[offset + 1], data[offset + 2], data[offset + 3]])
}
pub fn read_le_i16(data: &[u8], offset: usize) -> i16 {
read_le_u16(data, offset) as i16
}
pub fn read_le_i32(data: &[u8], offset: usize) -> i32 {
read_le_u32(data, offset) as i32
}
pub struct PdoDataItem {
base_ptr: *mut u8,
offset: usize,
is_input: bool,
}
impl PdoDataItem {
pub fn new(base_ptr: *mut u8, offset: usize, is_input: bool) -> Self {
Self { base_ptr, offset, is_input }
}
fn validate(&self) -> *mut u8 {
assert!(!self.base_ptr.is_null(), "基础指针为空");
unsafe { self.base_ptr.add(self.offset) }
}
fn check_writable(&self) {
assert!(!self.is_input, "不能写入输入 PDO");
}
pub fn content(&self) -> u8 {
unsafe { *self.validate() }
}
pub fn set_content(&self, value: u8) {
self.check_writable();
unsafe { *self.validate() = value; }
}
pub fn as_int16(&self) -> i16 {
let ptr = self.validate();
unsafe { (ptr as *const i16).read_unaligned() }
}
pub fn set_int16(&self, value: i16) {
self.check_writable();
let ptr = self.validate();
unsafe { (ptr as *mut i16).write_unaligned(value); }
}
pub fn as_uint16(&self) -> u16 {
let ptr = self.validate();
unsafe { (ptr as *const u16).read_unaligned() }
}
pub fn set_uint16(&self, value: u16) {
self.check_writable();
let ptr = self.validate();
unsafe { (ptr as *mut u16).write_unaligned(value); }
}
pub fn as_int32(&self) -> i32 {
let ptr = self.validate();
unsafe { (ptr as *const i32).read_unaligned() }
}
pub fn set_int32(&self, value: i32) {
self.check_writable();
let ptr = self.validate();
unsafe { (ptr as *mut i32).write_unaligned(value); }
}
pub fn as_uint32(&self) -> u32 {
let ptr = self.validate();
unsafe { (ptr as *const u32).read_unaligned() }
}
pub fn set_uint32(&self, value: u32) {
self.check_writable();
let ptr = self.validate();
unsafe { (ptr as *mut u32).write_unaligned(value); }
}
pub fn as_float(&self) -> f32 {
let ptr = self.validate();
unsafe { (ptr as *const f32).read_unaligned() }
}
pub fn set_float(&self, value: f32) {
self.check_writable();
let ptr = self.validate();
unsafe { (ptr as *mut f32).write_unaligned(value); }
}
pub fn as_double(&self) -> f64 {
let ptr = self.validate();
unsafe { (ptr as *const f64).read_unaligned() }
}
pub fn set_double(&self, value: f64) {
self.check_writable();
let ptr = self.validate();
unsafe { (ptr as *mut f64).write_unaligned(value); }
}
pub fn get_bit(&self, bit_index: u8) -> bool {
assert!(bit_index <= 7, "位索引必须在 0-7 之间");
(self.content() & (1 << bit_index)) != 0
}
pub fn set_bit(&self, bit_index: u8, value: bool) {
self.check_writable();
assert!(bit_index <= 7, "位索引必须在 0-7 之间");
let mut current = self.content();
if value {
current |= 1 << bit_index;
} else {
current &= !(1 << bit_index);
}
self.set_content(current);
}
}
pub struct PdoArrayInstance {
data_ptr: *mut u8,
length: usize,
is_input: bool,
}
impl PdoArrayInstance {
pub fn new(data_ptr: *mut u8, length: usize, is_input: bool) -> Self {
Self { data_ptr, length, is_input }
}
pub fn get(&self, index: usize) -> PdoDataItem {
assert!(index < self.length, "索引 {} 超出范围 [0, {})", index, self.length);
PdoDataItem::new(self.data_ptr, index, self.is_input)
}
pub fn set(&self, index: usize, value: u8) {
assert!(index < self.length, "索引 {} 超出范围 [0, {})", index, self.length);
assert!(!self.is_input, "不能写入输入 PDO");
let item = PdoDataItem::new(self.data_ptr, index, self.is_input);
item.set_content(value);
}
pub fn length(&self) -> usize {
self.length
}
}
#[derive(Debug, Clone, Copy)]
pub struct PdoInputView {
ptr: NonNull<u8>,
len: usize,
}
impl PdoInputView {
pub fn len(&self) -> usize {
self.len
}
pub fn is_empty(&self) -> bool {
self.len == 0
}
pub fn as_ptr(&self) -> *const u8 {
self.ptr.as_ptr()
}
pub unsafe fn as_slice<'a>(&self) -> &'a [u8] {
std::slice::from_raw_parts(self.ptr.as_ptr(), self.len)
}
}
#[derive(Debug, Clone, Copy)]
pub struct PdoOutputView {
ptr: NonNull<u8>,
len: usize,
}
impl PdoOutputView {
pub fn len(&self) -> usize {
self.len
}
pub fn is_empty(&self) -> bool {
self.len == 0
}
pub fn as_ptr(&self) -> *mut u8 {
self.ptr.as_ptr()
}
pub unsafe fn as_mut_slice<'a>(&self) -> &'a mut [u8] {
std::slice::from_raw_parts_mut(self.ptr.as_ptr(), self.len)
}
}
fn io_pointers(master_index: u16, slave_index: u16) -> Option<(usize, *mut u8, usize, *mut u8)> {
let mut out_size: c_int = 0;
let mut out_ptr: *mut u8 = std::ptr::null_mut();
let mut in_size: c_int = 0;
let mut in_ptr: *mut u8 = std::ptr::null_mut();
let ok = unsafe {
ffi::GetIO(
master_index,
slave_index,
&mut out_size,
&mut out_ptr,
&mut in_size,
&mut in_ptr,
)
};
if ok != 1 {
return None;
}
let out_len = if out_size > 0 { out_size as usize } else { 0 };
let in_len = if in_size > 0 { in_size as usize } else { 0 };
Some((out_len, out_ptr, in_len, in_ptr))
}
fn copy_len(total: usize, offset: usize, requested: usize, user_len: usize) -> usize {
let Some(available) = total.checked_sub(offset) else {
return 0;
};
let requested = if requested == 0 {
available
} else {
requested.min(available)
};
requested.min(user_len)
}
pub fn input_data_view(master_index: u16, slave_index: u16) -> Option<PdoInputView> {
let (_, _, in_len, in_ptr) = io_pointers(master_index, slave_index)?;
if in_len == 0 {
return None;
}
NonNull::new(in_ptr).map(|ptr| PdoInputView { ptr, len: in_len })
}
pub fn output_data_view(master_index: u16, slave_index: u16) -> Option<PdoOutputView> {
let (out_len, out_ptr, _, _) = io_pointers(master_index, slave_index)?;
if out_len == 0 {
return None;
}
NonNull::new(out_ptr).map(|ptr| PdoOutputView { ptr, len: out_len })
}
pub fn get_input_data_pointer(master_index: u16, slave_index: u16) -> *mut u8 {
input_data_view(master_index, slave_index)
.map(|view| view.as_ptr() as *mut u8)
.unwrap_or(std::ptr::null_mut())
}
pub fn get_output_data_pointer(master_index: u16, slave_index: u16) -> *mut u8 {
output_data_view(master_index, slave_index)
.map(|view| view.as_ptr())
.unwrap_or(std::ptr::null_mut())
}
pub fn read_input_data_direct(master_index: u16, slave_index: u16,
buffer: &mut [u8], offset: usize, length: usize) -> usize {
try_read_input_data_direct(master_index, slave_index, buffer, offset, length).unwrap_or(0)
}
pub fn try_read_input_data_direct(
master_index: u16,
slave_index: u16,
buffer: &mut [u8],
offset: usize,
length: usize,
) -> Result<usize> {
let view = input_data_view(master_index, slave_index).ok_or(DarraError::NullPointer)?;
let to_read = copy_len(view.len(), offset, length, buffer.len());
if to_read == 0 {
return Ok(0);
}
unsafe {
std::ptr::copy_nonoverlapping(view.as_ptr().add(offset), buffer.as_mut_ptr(), to_read);
}
Ok(to_read)
}
pub fn write_output_data_direct(master_index: u16, slave_index: u16,
data: &[u8], offset: usize, length: usize) -> usize {
try_write_output_data_direct(master_index, slave_index, data, offset, length).unwrap_or(0)
}
pub fn try_write_output_data_direct(
master_index: u16,
slave_index: u16,
data: &[u8],
offset: usize,
length: usize,
) -> Result<usize> {
let view = output_data_view(master_index, slave_index).ok_or(DarraError::NullPointer)?;
let to_write = copy_len(view.len(), offset, length, data.len());
if to_write == 0 {
return Ok(0);
}
unsafe {
std::ptr::copy_nonoverlapping(data.as_ptr(), view.as_ptr().add(offset), to_write);
}
Ok(to_write)
}
pub fn batch_read(master_index: u16, slave_index: u16,
offsets: &[usize], sizes: &[usize]) -> Vec<Vec<u8>> {
assert_eq!(offsets.len(), sizes.len(), "offsets 和 sizes 长度必须一致");
let mut results: Vec<Vec<u8>> = vec![Vec::new(); offsets.len()];
let Some(view) = input_data_view(master_index, slave_index) else {
return results;
};
let total = view.len();
for (i, (&offset, &size)) in offsets.iter().zip(sizes.iter()).enumerate() {
if offset.checked_add(size).is_some_and(|end| end <= total) {
let slice = unsafe { std::slice::from_raw_parts(view.as_ptr().add(offset), size) };
results[i] = slice.to_vec();
}
}
results
}
pub fn batch_write(master_index: u16, slave_index: u16,
offsets: &[usize], values: &[&[u8]]) -> usize {
assert_eq!(offsets.len(), values.len(), "offsets 和 values 长度必须一致");
let Some(view) = output_data_view(master_index, slave_index) else {
return 0;
};
let total = view.len();
let mut written = 0usize;
for (&offset, data) in offsets.iter().zip(values.iter()) {
if offset.checked_add(data.len()).is_some_and(|end| end <= total) {
unsafe {
std::ptr::copy_nonoverlapping(data.as_ptr(), view.as_ptr().add(offset), data.len());
}
written += 1;
}
}
written
}
pub unsafe fn input_slice(master_index: u16, slave_index: u16) -> &'static [u8] {
match input_data_view(master_index, slave_index) {
Some(view) => view.as_slice(),
None => &[],
}
}
pub unsafe fn output_slice(master_index: u16, slave_index: u16) -> &'static mut [u8] {
match output_data_view(master_index, slave_index) {
Some(view) => view.as_mut_slice(),
None => &mut [],
}
}
#[derive(Debug, Clone, Default)]
pub struct PdoPerformanceStats {
pub read_count: u64,
pub write_count: u64,
pub error_count: u64,
pub total_bytes_read: u64,
pub total_bytes_written: u64,
pub last_cycle_ns: u64,
pub min_cycle_ns: u64,
pub max_cycle_ns: u64,
pub avg_cycle_ns: u64,
}
pub fn performance_stats(master_index: u16, slave_index: u16) -> Option<PdoPerformanceStats> {
unsafe {
let ptr = ffi::GetPDOStats(master_index, slave_index);
if ptr.is_null() {
return None;
}
let stats = std::ptr::read_unaligned(ptr as *const crate::data::structures::PdoStats);
Some(PdoPerformanceStats {
read_count: stats.read_count as u64,
write_count: stats.write_count as u64,
error_count: stats.error_count as u64,
total_bytes_read: stats.total_bytes_read as u64,
total_bytes_written: stats.total_bytes_written as u64,
last_cycle_ns: stats.last_cycle_time_ns,
min_cycle_ns: stats.min_cycle_time_ns,
max_cycle_ns: stats.max_cycle_time_ns,
avg_cycle_ns: stats.avg_cycle_time_ns,
})
}
}
pub fn reset_performance_stats(master_index: u16, slave_index: u16) {
unsafe { ffi::ResetPDOStats(master_index, slave_index); }
}
pub fn batch_read_pooled(master_index: u16, slave_indices: &[u16],
expected_sizes: &[usize]) -> Vec<Option<Vec<u8>>> {
assert_eq!(slave_indices.len(), expected_sizes.len());
let mut results: Vec<Option<Vec<u8>>> = vec![None; slave_indices.len()];
for (i, (&si, &sz)) in slave_indices.iter().zip(expected_sizes.iter()).enumerate() {
if let Some(view) = input_data_view(master_index, si) {
let to_read = sz.min(view.len());
let mut buf = vec![0u8; to_read];
unsafe { std::ptr::copy_nonoverlapping(view.as_ptr(), buf.as_mut_ptr(), to_read); }
results[i] = Some(buf);
}
}
results
}
pub unsafe fn as_slice(master_index: u16, slave_index: u16, is_input: bool) -> &'static [u8] {
if is_input {
input_slice(master_index, slave_index)
} else {
output_slice(master_index, slave_index)
}
}
pub fn read_input_to_buffer(master_index: u16, slave_index: u16, buf: &mut [u8]) -> usize {
read_input_data_direct(master_index, slave_index, buf, 0, buf.len())
}
pub fn write_output_from_buffer(master_index: u16, slave_index: u16, buf: &[u8]) -> usize {
write_output_data_direct(master_index, slave_index, buf, 0, buf.len())
}
pub unsafe fn write_from_typed<T: Copy>(master_index: u16, slave_index: u16, value: &T, byte_offset: usize) -> bool {
let size = std::mem::size_of::<T>();
let data = std::slice::from_raw_parts(value as *const T as *const u8, size);
let mut out_size: c_int = 0;
let mut out_ptr: *mut u8 = std::ptr::null_mut();
let mut in_size: c_int = 0;
let mut in_ptr: *mut u8 = std::ptr::null_mut();
let ok = ffi::GetIO(master_index, slave_index,
&mut out_size, &mut out_ptr, &mut in_size, &mut in_ptr);
if ok != 1 || out_ptr.is_null() || out_size <= 0 {
return false;
}
if byte_offset + size > out_size as usize {
return false;
}
std::ptr::copy_nonoverlapping(data.as_ptr(), out_ptr.add(byte_offset), size);
true
}
pub fn ensure_capacity(buf: &mut Vec<u8>, required_size: usize) {
if buf.len() < required_size {
buf.resize(required_size, 0);
}
}
pub fn batch_write_pooled(master_index: u16, slave_indices: &[u16],
data_list: &[&[u8]]) -> usize {
assert_eq!(slave_indices.len(), data_list.len());
let mut success = 0usize;
for (&si, data) in slave_indices.iter().zip(data_list.iter()) {
if let Some(view) = output_data_view(master_index, si) {
let to_write = data.len().min(view.len());
unsafe { std::ptr::copy_nonoverlapping(data.as_ptr(), view.as_ptr(), to_write); }
success += 1;
}
}
success
}