1use core::cell::UnsafeCell;
4
5use zencan_common::{
6 i24, sdo::AbortCode, traits::ReadSize, u24, AtomicCell, TimeDifference, TimeOfDay,
7};
8
9pub trait SubObjectAccess: Sync + Send {
11 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode>;
30
31 fn read_size(&self) -> usize;
43
44 fn write(&self, data: &[u8]) -> Result<(), AbortCode>;
66
67 fn begin_partial(&self) -> Result<(), AbortCode> {
87 Err(AbortCode::UnsupportedAccess)
88 }
89
90 fn write_partial(&self, _buf: &[u8]) -> Result<(), AbortCode> {
98 Err(AbortCode::UnsupportedAccess)
99 }
100
101 fn end_partial(&self) -> Result<(), AbortCode> {
103 Err(AbortCode::UnsupportedAccess)
104 }
105}
106
107#[allow(missing_debug_implementations)]
109pub struct ScalarField<T: Copy> {
110 value: AtomicCell<T>,
111}
112
113impl<T: Send + Copy + PartialEq> ScalarField<T> {
114 pub fn load(&self) -> T {
116 self.value.load()
117 }
118
119 pub fn store(&self, value: T) {
121 self.value.store(value);
122 }
123}
124
125impl<T: Copy + Default> Default for ScalarField<T> {
126 fn default() -> Self {
127 Self {
128 value: AtomicCell::default(),
129 }
130 }
131}
132
133macro_rules! impl_scalar_field {
134 ($rust_type: ty) => {
135 impl ScalarField<$rust_type> {
136 pub const fn new(value: $rust_type) -> Self {
138 Self {
139 value: AtomicCell::new(value),
140 }
141 }
142 }
143 impl SubObjectAccess for ScalarField<$rust_type> {
144 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
145 let bytes = self.value.load().to_le_bytes();
146 if offset < bytes.len() {
147 let read_len = buf.len().min(bytes.len() - offset);
148 buf[0..read_len].copy_from_slice(&bytes[offset..offset + read_len]);
149 Ok(read_len)
150 } else {
151 Ok(0)
152 }
153 }
154
155 fn read_size(&self) -> usize {
156 <$rust_type as ReadSize>::READ_SIZE
157 }
158
159 fn write(&self, data: &[u8]) -> Result<(), AbortCode> {
160 let value = <$rust_type>::from_le_bytes(data.try_into().map_err(|_| {
161 if data.len() < size_of::<$rust_type>() {
162 AbortCode::DataTypeMismatchLengthLow
163 } else {
164 AbortCode::DataTypeMismatchLengthHigh
165 }
166 })?);
167 self.value.store(value);
168 Ok(())
169 }
170 }
171 };
172}
173
174impl_scalar_field!(u8);
175impl_scalar_field!(u16);
176impl_scalar_field!(u24);
177impl_scalar_field!(u32);
178impl_scalar_field!(u64);
179impl_scalar_field!(i8);
180impl_scalar_field!(i16);
181impl_scalar_field!(i24);
182impl_scalar_field!(i32);
183impl_scalar_field!(i64);
184impl_scalar_field!(f32);
185impl_scalar_field!(f64);
186
187impl SubObjectAccess for ScalarField<bool> {
189 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
190 let value = self.value.load();
191 if offset != 0 || buf.len() > 1 {
192 return Err(AbortCode::DataTypeMismatchLengthHigh);
193 }
194 buf[0] = if value { 1 } else { 0 };
195 Ok(1)
196 }
197
198 fn read_size(&self) -> usize {
199 1
200 }
201
202 fn write(&self, data: &[u8]) -> Result<(), AbortCode> {
203 if data.len() != 1 {
204 return Err(AbortCode::DataTypeMismatchLengthHigh);
205 }
206 let value = data[0] != 0;
207 self.value.store(value);
208 Ok(())
209 }
210}
211
212impl SubObjectAccess for ScalarField<TimeDifference> {
213 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
214 let value = self.value.load();
215 let bytes = value.to_le_bytes();
216 if offset < bytes.len() {
217 let read_len = buf.len().min(bytes.len() - offset);
218 buf[0..read_len].copy_from_slice(&bytes[offset..offset + read_len]);
219 Ok(read_len)
220 } else {
221 Ok(0)
222 }
223 }
224
225 fn read_size(&self) -> usize {
226 6
227 }
228
229 fn write(&self, data: &[u8]) -> Result<(), AbortCode> {
230 let value = TimeDifference::from_le_bytes(data.try_into().map_err(|_| {
231 if data.len() < 6 {
232 AbortCode::DataTypeMismatchLengthLow
233 } else {
234 AbortCode::DataTypeMismatchLengthHigh
235 }
236 })?);
237 self.value.store(value);
238 Ok(())
239 }
240}
241
242impl ScalarField<TimeDifference> {
243 pub const fn new(value: TimeDifference) -> Self {
245 Self {
246 value: AtomicCell::new(value),
247 }
248 }
249}
250
251impl SubObjectAccess for ScalarField<TimeOfDay> {
252 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
253 let value = self.value.load();
254 let bytes = value.to_le_bytes();
255 if offset < bytes.len() {
256 let read_len = buf.len().min(bytes.len() - offset);
257 buf[0..read_len].copy_from_slice(&bytes[offset..offset + read_len]);
258 Ok(read_len)
259 } else {
260 Ok(0)
261 }
262 }
263
264 fn read_size(&self) -> usize {
265 6
266 }
267
268 fn write(&self, data: &[u8]) -> Result<(), AbortCode> {
269 let value = TimeOfDay::from_le_bytes(data.try_into().map_err(|_| {
270 if data.len() < 6 {
271 AbortCode::DataTypeMismatchLengthLow
272 } else {
273 AbortCode::DataTypeMismatchLengthHigh
274 }
275 })?);
276 self.value.store(value);
277 Ok(())
278 }
279}
280
281impl ScalarField<TimeOfDay> {
282 pub const fn new(value: TimeOfDay) -> Self {
284 Self {
285 value: AtomicCell::new(value),
286 }
287 }
288}
289
290#[allow(clippy::len_without_is_empty, missing_debug_implementations)]
294pub struct ByteField<const N: usize> {
295 value: UnsafeCell<[u8; N]>,
296 write_offset: AtomicCell<Option<usize>>,
297}
298
299unsafe impl<const N: usize> Sync for ByteField<N> {}
300
301impl<const N: usize> ByteField<N> {
302 pub const fn new(value: [u8; N]) -> Self {
304 Self {
305 value: UnsafeCell::new(value),
306 write_offset: AtomicCell::new(None),
307 }
308 }
309
310 pub fn len(&self) -> usize {
312 N
313 }
314
315 pub fn store(&self, value: [u8; N]) {
317 self.write_offset.store(None);
319 critical_section::with(|_| {
320 let bytes = unsafe { &mut *self.value.get() };
321 bytes.copy_from_slice(&value);
322 });
323 }
324
325 pub fn load(&self) -> [u8; N] {
327 critical_section::with(|_| unsafe { *self.value.get() })
328 }
329}
330
331impl<const N: usize> Default for ByteField<N> {
332 fn default() -> Self {
333 Self {
334 value: UnsafeCell::new([0; N]),
335 write_offset: AtomicCell::new(None),
336 }
337 }
338}
339
340impl<const N: usize> SubObjectAccess for ByteField<N> {
341 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
342 critical_section::with(|_| {
343 let bytes = unsafe { &*self.value.get() };
344 if bytes.len() > offset {
345 let read_len = buf.len().min(bytes.len() - offset);
346 buf[..read_len].copy_from_slice(&bytes[offset..offset + read_len]);
347 Ok(read_len)
348 } else {
349 Ok(0)
350 }
351 })
352 }
353
354 fn read_size(&self) -> usize {
355 N
356 }
357
358 fn write(&self, data: &[u8]) -> Result<(), AbortCode> {
359 critical_section::with(|_| {
360 let bytes = unsafe { &mut *self.value.get() };
361 if data.len() > bytes.len() {
362 return Err(AbortCode::DataTypeMismatchLengthHigh);
363 }
364 bytes[..data.len()].copy_from_slice(data);
365 Ok(())
366 })
367 }
368
369 fn begin_partial(&self) -> Result<(), AbortCode> {
370 self.write_offset.store(Some(0));
371 Ok(())
372 }
373
374 fn write_partial(&self, buf: &[u8]) -> Result<(), AbortCode> {
375 let offset = self
377 .write_offset
378 .fetch_update(|old| Some(old.map(|x| x + buf.len())))
379 .unwrap();
380 if offset.is_none() {
381 return Err(AbortCode::GeneralError);
382 }
383 let offset = offset.unwrap();
384 if offset + buf.len() > N {
385 return Err(AbortCode::DataTypeMismatchLengthHigh);
386 }
387 critical_section::with(|_| {
388 let bytes = unsafe { &mut *self.value.get() };
389 bytes[offset..offset + buf.len()].copy_from_slice(buf);
390 });
391 Ok(())
392 }
393
394 fn end_partial(&self) -> Result<(), AbortCode> {
395 self.write_offset.store(None);
397 Ok(())
398 }
399}
400
401#[allow(clippy::len_without_is_empty, missing_debug_implementations)]
405pub struct NullTermByteField<const N: usize>(ByteField<N>);
406
407impl<const N: usize> NullTermByteField<N> {
408 pub const fn new(value: [u8; N]) -> Self {
410 Self(ByteField::new(value))
411 }
412
413 pub fn len(&self) -> usize {
415 N
416 }
417
418 pub fn load(&self) -> [u8; N] {
423 self.0.load()
424 }
425
426 pub fn store(&self, value: [u8; N]) {
428 self.0.store(value);
429 }
430
431 pub fn set_str(&self, value: &[u8]) -> Result<(), AbortCode> {
436 self.0.begin_partial()?;
437 self.0.write_partial(value)?;
438 if value.len() < N {
439 self.0.write_partial(&[0])?;
440 }
441 self.end_partial()?;
442 Ok(())
443 }
444}
445
446impl<const N: usize> Default for NullTermByteField<N> {
447 fn default() -> Self {
448 Self(ByteField::default())
449 }
450}
451
452impl<const N: usize> SubObjectAccess for NullTermByteField<N> {
453 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
454 let size = self.0.read(offset, buf)?;
455 let size = buf[0..size].iter().position(|b| *b == 0).unwrap_or(size);
456 Ok(size)
457 }
458
459 fn read_size(&self) -> usize {
460 critical_section::with(|_| {
461 let bytes = unsafe { &*self.0.value.get() };
462 bytes.iter().position(|b| *b == 0).unwrap_or(bytes.len())
464 })
465 }
466
467 fn write(&self, data: &[u8]) -> Result<(), AbortCode> {
468 self.0.begin_partial()?;
469 self.0.write_partial(data)?;
470 if data.len() < N {
471 self.0.write_partial(&[0])?;
472 }
473 self.0.end_partial()?;
474 Ok(())
475 }
476
477 fn begin_partial(&self) -> Result<(), AbortCode> {
478 self.0.begin_partial()
479 }
480
481 fn write_partial(&self, data: &[u8]) -> Result<(), AbortCode> {
482 self.0.write_partial(data)
483 }
484
485 fn end_partial(&self) -> Result<(), AbortCode> {
486 if self.0.write_offset.load().unwrap_or(0) < N {
488 self.0.write_partial(&[0])?;
489 }
490 self.0.end_partial()
491 }
492}
493
494#[derive(Clone, Copy, Debug)]
496pub struct ConstByteRefField {
497 value: &'static [u8],
498}
499
500impl ConstByteRefField {
501 pub const fn new(value: &'static [u8]) -> Self {
503 Self { value }
504 }
505}
506
507impl SubObjectAccess for ConstByteRefField {
508 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
509 let read_len = buf.len().min(self.value.len() - offset);
510 buf[..read_len].copy_from_slice(&self.value[offset..offset + read_len]);
511 Ok(read_len)
512 }
513
514 fn read_size(&self) -> usize {
515 self.value.len()
516 }
517
518 fn write(&self, _data: &[u8]) -> Result<(), AbortCode> {
519 Err(AbortCode::ReadOnly)
520 }
521}
522
523#[derive(Debug)]
524pub struct ConstField<const N: usize> {
529 bytes: [u8; N],
530}
531
532impl<const N: usize> ConstField<N> {
533 pub const fn new(bytes: [u8; N]) -> Self {
535 Self { bytes }
536 }
537}
538
539impl<const N: usize> SubObjectAccess for ConstField<N> {
540 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
541 if offset < self.bytes.len() {
542 let read_len = buf.len().min(self.bytes.len() - offset);
543 buf[..read_len].copy_from_slice(&self.bytes[offset..offset + read_len]);
544 Ok(read_len)
545 } else {
546 Ok(0)
547 }
548 }
549
550 fn read_size(&self) -> usize {
551 N
552 }
553
554 fn write(&self, _data: &[u8]) -> Result<(), AbortCode> {
555 Err(AbortCode::ReadOnly)
556 }
557}
558
559#[allow(missing_debug_implementations)]
561pub struct CallbackSubObject {
562 handler: AtomicCell<Option<&'static dyn SubObjectAccess>>,
563}
564
565impl Default for CallbackSubObject {
566 fn default() -> Self {
567 Self::new()
568 }
569}
570
571impl CallbackSubObject {
572 pub const fn new() -> Self {
574 Self {
575 handler: AtomicCell::new(None),
576 }
577 }
578
579 pub fn register_handler(&self, handler: &'static dyn SubObjectAccess) {
581 self.handler.store(Some(handler));
582 }
583}
584
585impl SubObjectAccess for CallbackSubObject {
586 fn read(&self, offset: usize, buf: &mut [u8]) -> Result<usize, AbortCode> {
587 if let Some(handler) = self.handler.load() {
588 handler.read(offset, buf)
589 } else {
590 Err(AbortCode::ResourceNotAvailable)
591 }
592 }
593
594 fn read_size(&self) -> usize {
595 if let Some(handler) = self.handler.load() {
596 handler.read_size()
597 } else {
598 0
599 }
600 }
601
602 fn write(&self, data: &[u8]) -> Result<(), AbortCode> {
603 if let Some(handler) = self.handler.load() {
604 handler.write(data)
605 } else {
606 Err(AbortCode::ResourceNotAvailable)
607 }
608 }
609
610 fn begin_partial(&self) -> Result<(), AbortCode> {
611 if let Some(handler) = self.handler.load() {
612 handler.begin_partial()
613 } else {
614 Err(AbortCode::ResourceNotAvailable)
615 }
616 }
617
618 fn write_partial(&self, buf: &[u8]) -> Result<(), AbortCode> {
619 if let Some(handler) = self.handler.load() {
620 handler.write_partial(buf)
621 } else {
622 Err(AbortCode::ResourceNotAvailable)
623 }
624 }
625
626 fn end_partial(&self) -> Result<(), AbortCode> {
627 if let Some(handler) = self.handler.load() {
628 handler.end_partial()
629 } else {
630 Err(AbortCode::ResourceNotAvailable)
631 }
632 }
633}
634
635#[cfg(test)]
636mod tests {
637 use zencan_common::objects::{ObjectCode, SubInfo};
638
639 use crate::object_dict::{ObjectAccess, ProvidesSubObjects};
640
641 use super::*;
642
643 #[derive(Default)]
644 struct ExampleRecord {
645 val1: ScalarField<u32>,
646 val2: ScalarField<bool>,
647 val3: NullTermByteField<10>,
648 }
649
650 impl ProvidesSubObjects for ExampleRecord {
651 fn get_sub_object(&self, sub: u8) -> Option<(SubInfo, &dyn SubObjectAccess)> {
652 match sub {
653 0 => Some((
654 SubInfo::MAX_SUB_NUMBER,
655 const { &ConstField::new(3u8.to_le_bytes()) },
656 )),
657 1 => Some((SubInfo::new_u32().rw_access(), &self.val1)),
658 2 => Some((SubInfo::new_u8().rw_access(), &self.val2)),
659 3 => Some((
660 SubInfo::new_visibile_str(self.val3.len()).rw_access(),
661 &self.val3,
662 )),
663 _ => None,
664 }
665 }
666
667 fn object_code(&self) -> ObjectCode {
668 ObjectCode::Record
669 }
670 }
671
672 #[test]
673 fn test_record_with_provides_sub_objects() {
674 let record = ExampleRecord::default();
675
676 assert_eq!(3, record.read_u8(0).unwrap());
677 record.write(1, &42u32.to_le_bytes()).unwrap();
678 assert_eq!(42, record.read_u32(1).unwrap());
679
680 record.begin_partial(3).unwrap();
681 record
683 .write_partial(3, &[0, 1, 2, 3, 4, 5, 6, 7, 8, 9])
684 .unwrap();
685 let mut buf = [0; 10];
686 record.read(3, 0, &mut buf).unwrap();
687 assert_eq!([0, 1, 2, 3, 4, 5, 6, 7, 8, 9], buf);
688 record.begin_partial(3).unwrap();
690 record.write_partial(3, &[0, 1, 2, 3]).unwrap();
691 record.write_partial(3, &[4, 5, 6, 7]).unwrap();
692 record.end_partial(3).unwrap();
693 let mut buf = [0; 9];
694 record.read(3, 0, &mut buf).unwrap();
695 assert_eq!([0u8, 1, 2, 3, 4, 5, 6, 7, 0], buf)
696 }
697
698 fn sub_read_test_helper(field: &dyn SubObjectAccess, expected_bytes: &[u8]) {
699 let n = expected_bytes.len();
700
701 assert_eq!(n, field.read_size());
702
703 let mut read_buf = vec![0xffu8; n + 10];
705 let read_size = field.read(0, &mut read_buf).unwrap();
706 assert_eq!(n, read_size);
707 assert_eq!(expected_bytes, &read_buf[0..n]);
708
709 let mut read_buf = vec![0xffu8; n + 10];
711 let read_size = field.read(0, &mut read_buf).unwrap();
712 assert_eq!(n, read_size);
713 assert_eq!(expected_bytes, &read_buf[0..n]);
714
715 if n > 2 {
716 let mut read_buf = vec![0xffu8; n + 10];
718 let read_size = field.read(2, &mut read_buf).unwrap();
719 assert_eq!(n - 2, read_size);
720 assert_eq!(&expected_bytes[2..], &read_buf[0..n - 2]);
721
722 let mut read_buf = vec![0xffu8; n - 2];
724 let read_size = field.read(1, &mut read_buf).unwrap();
725 assert_eq!(n - 2, read_size);
726 assert_eq!(expected_bytes[1..n - 1], read_buf);
727 } else {
728 let mut read_buf = vec![0xffu8; n + 10];
729 let read_size = field.read(1, &mut read_buf).unwrap();
730 assert_eq!(n.saturating_sub(1), read_size);
731 assert_eq!(&expected_bytes[1..], &read_buf[0..read_size]);
732 }
733 }
734
735 #[test]
736 fn test_scalar_field_bool() {
737 let field = ScalarField::<bool>::default();
738 field.store(true);
739 assert_eq!(1, field.read_size());
740
741 let mut read_buf = [0xffu8; 1];
742 let read_size = field.read(0, &mut read_buf).unwrap();
743 assert_eq!(1, read_size);
744 assert_eq!([1], read_buf);
745 }
746
747 #[test]
748 fn test_scalar_field_u8() {
749 let field = ScalarField::<u8>::new(42u8);
750 let exp_bytes = 42u8.to_le_bytes();
751 sub_read_test_helper(&field, &exp_bytes);
752 }
753
754 #[test]
755 fn test_scalar_field_u32() {
756 let field = ScalarField::<u32>::new(42u32);
757 let exp_bytes = 42u32.to_le_bytes();
758 sub_read_test_helper(&field, &exp_bytes);
759 }
760
761 #[test]
762 fn test_scalar_field_u16() {
763 let field = ScalarField::<u16>::new(42u16);
764 let exp_bytes = 42u16.to_le_bytes();
765 sub_read_test_helper(&field, &exp_bytes);
766 }
767
768 #[test]
769 fn test_scalar_field_u24() {
770 let field = ScalarField::<u24>::new(u24::new(42));
771 let exp_bytes = u24::new(42).to_le_bytes();
772 sub_read_test_helper(&field, &exp_bytes);
773 }
774
775 #[test]
776 fn test_scalar_field_u64() {
777 let field = ScalarField::<u64>::new(42u64);
778 let exp_bytes = 42u64.to_le_bytes();
779 sub_read_test_helper(&field, &exp_bytes);
780 }
781
782 #[test]
783 fn test_scalar_field_i8() {
784 let field = ScalarField::<i8>::new(-42i8);
785 let exp_bytes = (-42i8).to_le_bytes();
786 sub_read_test_helper(&field, &exp_bytes);
787 }
788
789 #[test]
790 fn test_scalar_field_i16() {
791 let field = ScalarField::<i16>::new(-42i16);
792 let exp_bytes = (-42i16).to_le_bytes();
793 sub_read_test_helper(&field, &exp_bytes);
794 }
795
796 #[test]
797 fn test_scalar_field_i24() {
798 let field = ScalarField::<i24>::new(i24::new(-42));
799 let exp_bytes = i24::new(-42).to_le_bytes();
800 sub_read_test_helper(&field, &exp_bytes);
801 }
802
803 #[test]
804 fn test_scalar_field_i32() {
805 let field = ScalarField::<i32>::new(-42i32);
806 let exp_bytes = (-42i32).to_le_bytes();
807 sub_read_test_helper(&field, &exp_bytes);
808 }
809
810 #[test]
811 fn test_scalar_field_i64() {
812 let field = ScalarField::<i64>::new(-42i64);
813 let exp_bytes = (-42i64).to_le_bytes();
814 sub_read_test_helper(&field, &exp_bytes);
815 }
816
817 #[test]
818 fn test_scalar_field_f32() {
819 let field = ScalarField::<f32>::new(42.5f32);
820 let exp_bytes = 42.5f32.to_le_bytes();
821 sub_read_test_helper(&field, &exp_bytes);
822 }
823
824 #[test]
825 fn test_scalar_field_f64() {
826 let field = ScalarField::<f64>::new(42.5f64);
827 let exp_bytes = 42.5f64.to_le_bytes();
828 sub_read_test_helper(&field, &exp_bytes);
829 }
830
831 #[test]
832 fn test_scalar_field_time_difference() {
833 let value = TimeDifference::new(42, 1234);
834 let field = ScalarField::<TimeDifference>::new(value);
835 let exp_bytes = value.to_le_bytes();
836 sub_read_test_helper(&field, &exp_bytes);
837 }
838
839 #[test]
840 fn test_scalar_field_time_of_day() {
841 let value = TimeOfDay::new(42, 1234);
842 let field = ScalarField::<TimeOfDay>::new(value);
843 let exp_bytes = value.to_le_bytes();
844 sub_read_test_helper(&field, &exp_bytes);
845 }
846
847 #[test]
848 fn test_byte_field() {
849 const N: usize = 10;
850 let field = ByteField::new([0; N]);
851
852 let write_data = Vec::from_iter(0u8..N as u8);
853 field.write(&write_data).unwrap();
854
855 sub_read_test_helper(&field, &write_data);
856 }
857
858 #[test]
859 fn test_null_term_byte_field() {
860 let field = NullTermByteField::new([0; 10]);
861 field.write(&[1, 2, 3, 4, 5, 6, 7, 8, 9, 10]).unwrap();
863 sub_read_test_helper(&field, &[1, 2, 3, 4, 5, 6, 7, 8, 9, 10]);
864 field.write(&[1, 2, 3, 4]).unwrap();
866 sub_read_test_helper(&field, &[1, 2, 3, 4]);
867 }
868
869 #[test]
870 fn test_const_field() {
871 let field = ConstField::new([1, 2, 3, 4, 5]);
872 sub_read_test_helper(&field, &[1, 2, 3, 4, 5]);
873 }
874
875 #[test]
876 fn test_const_byte_ref_field() {
877 let field = ConstByteRefField::new(&[1, 2, 3, 4, 5]);
878 sub_read_test_helper(&field, &[1, 2, 3, 4, 5]);
879 }
880}