1
2use crate::slave::coe::EcDataType;
3use std::sync::Mutex;
4
5#[derive(Debug, Clone)]
6pub enum EcValue {
7
8 Bool(bool),
9
10 I8(i8),
11
12 U8(u8),
13
14 I16(i16),
15
16 U16(u16),
17
18 I32(i32),
19
20 U32(u32),
21
22 I64(i64),
23
24 U64(u64),
25
26 F32(f32),
27
28 F64(f64),
29
30 String(String),
31
32 Bytes(Vec<u8>),
33}
34
35impl std::fmt::Display for EcValue {
36 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
37 match self {
38 EcValue::Bool(v) => write!(f, "{}", v),
39 EcValue::I8(v) => write!(f, "{}", v),
40 EcValue::U8(v) => write!(f, "{}", v),
41 EcValue::I16(v) => write!(f, "{}", v),
42 EcValue::U16(v) => write!(f, "{}", v),
43 EcValue::I32(v) => write!(f, "{}", v),
44 EcValue::U32(v) => write!(f, "{}", v),
45 EcValue::I64(v) => write!(f, "{}", v),
46 EcValue::U64(v) => write!(f, "{}", v),
47 EcValue::F32(v) => write!(f, "{}", v),
48 EcValue::F64(v) => write!(f, "{}", v),
49 EcValue::String(v) => write!(f, "{}", v),
50 EcValue::Bytes(v) => write!(f, "[{} bytes]", v.len()),
51 }
52 }
53}
54
55pub struct BaseData {
56
57 data_type: EcDataType,
58
59 raw_data: Mutex<Vec<u8>>,
60}
61
62impl BaseData {
63
64 pub fn new(data_type: EcDataType) -> Self {
65 let size = type_size(data_type);
66 Self {
67 data_type,
68 raw_data: Mutex::new(vec![0u8; size]),
69 }
70 }
71
72 pub fn with_value(data_type: EcDataType, value: &EcValue) -> Self {
73 let bd = Self::new(data_type);
74 let _ = bd.write(value);
75 bd
76 }
77
78 pub fn data_type(&self) -> EcDataType {
79 self.data_type
80 }
81
82 pub fn read(&self) -> EcValue {
83 let guard = self.raw_data.lock().unwrap();
84 convert_from_bytes(&guard, self.data_type)
85 }
86
87 pub fn write(&self, value: &EcValue) -> bool {
88 match convert_to_bytes(value, self.data_type) {
89 Some(bytes) => {
90 let mut guard = self.raw_data.lock().unwrap();
91 *guard = bytes;
92 true
93 }
94 None => false,
95 }
96 }
97
98 pub fn write_raw(&self, data: &[u8]) -> bool {
99
100 match self.data_type {
101 EcDataType::OctetString | EcDataType::UnicodeString
102 | EcDataType::VisibleString | EcDataType::Domain => {
103 let mut guard = self.raw_data.lock().unwrap();
104 guard.clear();
105 guard.extend_from_slice(data);
106 return true;
107 }
108 _ => {}
109 }
110
111 let expected = type_size(self.data_type);
112 if data.len() != expected {
113 return false;
114 }
115 let mut guard = self.raw_data.lock().unwrap();
116 guard.clear();
117 guard.extend_from_slice(data);
118 true
119 }
120
121 pub fn raw_data(&self) -> Vec<u8> {
122 self.raw_data.lock().unwrap().clone()
123 }
124
125 pub fn get_bit(&self, bit_index: usize) -> Option<bool> {
126 let guard = self.raw_data.lock().unwrap();
127 let byte_index = bit_index / 8;
128 let bit_offset = bit_index % 8;
129 if byte_index >= guard.len() {
130 return None;
131 }
132 Some((guard[byte_index] & (1 << bit_offset)) != 0)
133 }
134
135 pub fn set_bit(&self, bit_index: usize, value: bool) -> bool {
136 let mut guard = self.raw_data.lock().unwrap();
137 let byte_index = bit_index / 8;
138 let bit_offset = bit_index % 8;
139 if byte_index >= guard.len() {
140 return false;
141 }
142 if value {
143 guard[byte_index] |= 1 << bit_offset;
144 } else {
145 guard[byte_index] &= !(1 << bit_offset);
146 }
147 true
148 }
149}
150
151impl std::fmt::Display for BaseData {
152 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
153 write!(f, "{}", self.read())
154 }
155}
156
157impl std::fmt::Debug for BaseData {
158 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
159 f.debug_struct("BaseData")
160 .field("type", &self.data_type)
161 .field("value", &self.read())
162 .finish()
163 }
164}
165
166pub fn type_size(dt: EcDataType) -> usize {
167 match dt {
168 EcDataType::Boolean | EcDataType::Integer8 | EcDataType::Unsigned8 => 1,
169 EcDataType::Bit1 | EcDataType::Bit2 | EcDataType::Bit3 | EcDataType::Bit4 => 1,
170 EcDataType::Bit5 | EcDataType::Bit6 | EcDataType::Bit7 | EcDataType::Bit8 => 1,
171 EcDataType::Integer16 | EcDataType::Unsigned16 => 2,
172 EcDataType::Integer24 | EcDataType::Unsigned24 => 3,
173 EcDataType::Integer32 | EcDataType::Unsigned32 | EcDataType::Real32 => 4,
174 EcDataType::Integer64 | EcDataType::Unsigned64 | EcDataType::Real64 => 8,
175 EcDataType::TimeOfDay | EcDataType::TimeDifference => 8,
176 EcDataType::VisibleString | EcDataType::OctetString | EcDataType::Domain => 256,
177 EcDataType::UnicodeString => 512,
178 _ => 1,
179 }
180}
181
182pub fn convert_from_bytes(data: &[u8], dt: EcDataType) -> EcValue {
183 if data.is_empty() {
184 return default_value(dt);
185 }
186 match dt {
187
188 EcDataType::Boolean => EcValue::Bool(data[0] != 0),
189 EcDataType::Bit1 | EcDataType::Bit2 | EcDataType::Bit3
190 | EcDataType::Bit4 | EcDataType::Bit5 | EcDataType::Bit6 | EcDataType::Bit7
191 | EcDataType::Bit8 => EcValue::Bool((data[0] & 1) != 0),
192
193 EcDataType::Integer8 => EcValue::I8(data[0] as i8),
194 EcDataType::Unsigned8 => EcValue::U8(data[0]),
195
196 EcDataType::Integer16 if data.len() >= 2 => {
197 EcValue::I16(i16::from_le_bytes([data[0], data[1]]))
198 }
199 EcDataType::Unsigned16 if data.len() >= 2 => {
200 EcValue::U16(u16::from_le_bytes([data[0], data[1]]))
201 }
202
203 EcDataType::Integer24 if data.len() >= 3 => {
204 let v = (data[0] as i32) | ((data[1] as i32) << 8) | ((data[2] as i32) << 16);
205
206 let v = if v & 0x800000 != 0 { v | !0xFFFFFF_i32 } else { v };
207 EcValue::I32(v)
208 }
209 EcDataType::Unsigned24 if data.len() >= 3 => {
210 let v = (data[0] as u32) | ((data[1] as u32) << 8) | ((data[2] as u32) << 16);
211 EcValue::U32(v)
212 }
213
214 EcDataType::Integer32 if data.len() >= 4 => {
215 EcValue::I32(i32::from_le_bytes([data[0], data[1], data[2], data[3]]))
216 }
217 EcDataType::Unsigned32 if data.len() >= 4 => {
218 EcValue::U32(u32::from_le_bytes([data[0], data[1], data[2], data[3]]))
219 }
220
221 EcDataType::Integer64 if data.len() >= 8 => {
222 let mut buf = [0u8; 8];
223 buf.copy_from_slice(&data[..8]);
224 EcValue::I64(i64::from_le_bytes(buf))
225 }
226 EcDataType::Unsigned64 if data.len() >= 8 => {
227 let mut buf = [0u8; 8];
228 buf.copy_from_slice(&data[..8]);
229 EcValue::U64(u64::from_le_bytes(buf))
230 }
231
232 EcDataType::Real32 if data.len() >= 4 => {
233 EcValue::F32(f32::from_le_bytes([data[0], data[1], data[2], data[3]]))
234 }
235 EcDataType::Real64 if data.len() >= 8 => {
236 let mut buf = [0u8; 8];
237 buf.copy_from_slice(&data[..8]);
238 EcValue::F64(f64::from_le_bytes(buf))
239 }
240
241 EcDataType::VisibleString => {
242 let null_pos = data.iter().position(|&b| b == 0).unwrap_or(data.len());
243
244 EcValue::String(crate::utils::help::decode_ethercat_string(&data[..null_pos]))
245 }
246 EcDataType::UnicodeString => {
247
248 let null_pos = data.chunks(2)
249 .position(|c| c.len() == 2 && c[0] == 0 && c[1] == 0)
250 .unwrap_or(data.len() / 2);
251 let u16s: Vec<u16> = data[..null_pos * 2].chunks(2)
252 .filter(|c| c.len() == 2)
253 .map(|c| u16::from_le_bytes([c[0], c[1]]))
254 .collect();
255 EcValue::String(String::from_utf16_lossy(&u16s))
256 }
257
258 EcDataType::OctetString | EcDataType::Domain => {
259 EcValue::Bytes(data.to_vec())
260 }
261
262 EcDataType::TimeOfDay | EcDataType::TimeDifference if data.len() >= 8 => {
263 let mut buf = [0u8; 8];
264 buf.copy_from_slice(&data[..8]);
265 EcValue::I64(i64::from_le_bytes(buf))
266 }
267
268 _ => default_value(dt),
269 }
270}
271
272pub fn convert_to_bytes(value: &EcValue, dt: EcDataType) -> Option<Vec<u8>> {
273 match (value, dt) {
274 (EcValue::Bool(v), EcDataType::Boolean)
275 | (EcValue::Bool(v), EcDataType::Bit1)
276 | (EcValue::Bool(v), EcDataType::Bit2)
277 | (EcValue::Bool(v), EcDataType::Bit3)
278 | (EcValue::Bool(v), EcDataType::Bit4)
279 | (EcValue::Bool(v), EcDataType::Bit5)
280 | (EcValue::Bool(v), EcDataType::Bit6)
281 | (EcValue::Bool(v), EcDataType::Bit7)
282 | (EcValue::Bool(v), EcDataType::Bit8) => {
283 Some(vec![if *v { 1 } else { 0 }])
284 }
285
286 (EcValue::I8(v), EcDataType::Integer8) => Some(vec![*v as u8]),
287 (EcValue::U8(v), EcDataType::Unsigned8) => Some(vec![*v]),
288
289 (EcValue::I16(v), EcDataType::Integer16) => Some(v.to_le_bytes().to_vec()),
290 (EcValue::U16(v), EcDataType::Unsigned16) => Some(v.to_le_bytes().to_vec()),
291
292 (EcValue::I32(v), EcDataType::Integer24) => {
293 let bytes = v.to_le_bytes();
294 Some(vec![bytes[0], bytes[1], bytes[2]])
295 }
296 (EcValue::U32(v), EcDataType::Unsigned24) => {
297 let bytes = v.to_le_bytes();
298 Some(vec![bytes[0], bytes[1], bytes[2]])
299 }
300
301 (EcValue::I32(v), EcDataType::Integer32) => Some(v.to_le_bytes().to_vec()),
302 (EcValue::U32(v), EcDataType::Unsigned32) => Some(v.to_le_bytes().to_vec()),
303
304 (EcValue::I64(v), EcDataType::Integer64) => Some(v.to_le_bytes().to_vec()),
305 (EcValue::U64(v), EcDataType::Unsigned64) => Some(v.to_le_bytes().to_vec()),
306
307 (EcValue::F32(v), EcDataType::Real32) => Some(v.to_le_bytes().to_vec()),
308 (EcValue::F64(v), EcDataType::Real64) => Some(v.to_le_bytes().to_vec()),
309
310 (EcValue::String(v), EcDataType::VisibleString) => {
311 let mut buf = vec![0u8; 256];
312 let bytes = v.as_bytes();
313 let len = bytes.len().min(255);
314 buf[..len].copy_from_slice(&bytes[..len]);
315 Some(buf)
316 }
317 (EcValue::String(v), EcDataType::UnicodeString) => {
318 let mut buf = vec![0u8; 512];
319 let u16s: Vec<u16> = v.encode_utf16().collect();
320 let len = u16s.len().min(255);
321 for (i, &c) in u16s[..len].iter().enumerate() {
322 let bytes = c.to_le_bytes();
323 buf[i * 2] = bytes[0];
324 buf[i * 2 + 1] = bytes[1];
325 }
326 Some(buf)
327 }
328
329 (EcValue::Bytes(v), EcDataType::OctetString)
330 | (EcValue::Bytes(v), EcDataType::Domain) => Some(v.clone()),
331
332 (EcValue::I64(v), EcDataType::TimeOfDay)
333 | (EcValue::I64(v), EcDataType::TimeDifference) => Some(v.to_le_bytes().to_vec()),
334
335 _ => None,
336 }
337}
338
339pub fn default_value(dt: EcDataType) -> EcValue {
340 match dt {
341 EcDataType::Boolean | EcDataType::Bit1 | EcDataType::Bit2 | EcDataType::Bit3
342 | EcDataType::Bit4 | EcDataType::Bit5 | EcDataType::Bit6 | EcDataType::Bit7
343 | EcDataType::Bit8 => EcValue::Bool(false),
344 EcDataType::Integer8 => EcValue::I8(0),
345 EcDataType::Unsigned8 => EcValue::U8(0),
346 EcDataType::Integer16 => EcValue::I16(0),
347 EcDataType::Unsigned16 => EcValue::U16(0),
348 EcDataType::Integer24 | EcDataType::Integer32 => EcValue::I32(0),
349 EcDataType::Unsigned24 | EcDataType::Unsigned32 => EcValue::U32(0),
350 EcDataType::Integer64 | EcDataType::TimeOfDay | EcDataType::TimeDifference => EcValue::I64(0),
351 EcDataType::Unsigned64 => EcValue::U64(0),
352 EcDataType::Real32 => EcValue::F32(0.0),
353 EcDataType::Real64 => EcValue::F64(0.0),
354 EcDataType::VisibleString | EcDataType::UnicodeString => EcValue::String(String::new()),
355 EcDataType::OctetString | EcDataType::Domain => EcValue::Bytes(Vec::new()),
356 _ => EcValue::U8(0),
357 }
358}