1#[cfg(feature = "alloc")]
22extern crate alloc;
23#[cfg(feature = "alloc")]
24use alloc::vec::Vec;
25
26use crate::endianness::Endianness;
27use crate::error::DecodeError;
28#[cfg(feature = "alloc")]
29use crate::error::EncodeError;
30
31#[cfg(feature = "alloc")]
39#[derive(Debug, Clone)]
40pub struct BufferWriter {
41 bytes: Vec<u8>,
42 endianness: Endianness,
43 max_alignment: usize,
50 align_origin: usize,
62}
63
64const XCDR1_MAX_ALIGNMENT: usize = 8;
66pub(crate) const XCDR2_MAX_ALIGNMENT: usize = 4;
68
69#[cfg(feature = "alloc")]
70impl BufferWriter {
71 #[must_use]
74 pub fn new(endianness: Endianness) -> Self {
75 Self {
76 bytes: Vec::new(),
77 endianness,
78 max_alignment: XCDR1_MAX_ALIGNMENT,
79 align_origin: 0,
80 }
81 }
82
83 #[must_use]
85 pub fn with_capacity(endianness: Endianness, cap: usize) -> Self {
86 Self {
87 bytes: Vec::with_capacity(cap),
88 endianness,
89 max_alignment: XCDR1_MAX_ALIGNMENT,
90 align_origin: 0,
91 }
92 }
93
94 #[must_use]
97 pub fn with_align_origin(mut self, origin: usize) -> Self {
98 self.align_origin = origin;
99 self
100 }
101
102 #[must_use]
105 pub fn with_max_alignment(mut self, max_alignment: usize) -> Self {
106 debug_assert!(
107 max_alignment.is_power_of_two(),
108 "max_alignment must be a power of two"
109 );
110 self.max_alignment = max_alignment;
111 self
112 }
113
114 #[must_use]
116 pub fn xcdr2(self) -> Self {
117 self.with_max_alignment(XCDR2_MAX_ALIGNMENT)
118 }
119
120 #[must_use]
122 pub fn max_alignment(&self) -> usize {
123 self.max_alignment
124 }
125
126 #[must_use]
128 pub fn endianness(&self) -> Endianness {
129 self.endianness
130 }
131
132 #[must_use]
134 pub fn position(&self) -> usize {
135 self.bytes.len()
136 }
137
138 #[must_use]
140 pub fn into_bytes(self) -> Vec<u8> {
141 self.bytes
142 }
143
144 #[must_use]
146 pub fn as_bytes(&self) -> &[u8] {
147 &self.bytes
148 }
149
150 pub fn align(&mut self, alignment: usize) {
154 debug_assert!(
155 alignment.is_power_of_two(),
156 "alignment must be a power of two"
157 );
158 let effective = alignment.min(self.max_alignment);
162 let pos = self.bytes.len() + self.align_origin;
163 let pad = padding_for(pos, effective);
164 for _ in 0..pad {
165 self.bytes.push(0);
166 }
167 }
168
169 pub fn write_bytes(&mut self, data: &[u8]) -> Result<(), EncodeError> {
175 self.bytes.extend_from_slice(data);
176 Ok(())
177 }
178
179 pub fn write_u8(&mut self, value: u8) -> Result<(), EncodeError> {
184 self.bytes.push(value);
185 Ok(())
186 }
187
188 pub fn write_u16(&mut self, value: u16) -> Result<(), EncodeError> {
193 self.align(2);
194 self.write_bytes(&self.endianness.write_u16(value))
195 }
196
197 pub fn write_u32(&mut self, value: u32) -> Result<(), EncodeError> {
202 self.align(4);
203 self.write_bytes(&self.endianness.write_u32(value))
204 }
205
206 pub fn write_u64(&mut self, value: u64) -> Result<(), EncodeError> {
211 self.align(8);
212 self.write_bytes(&self.endianness.write_u64(value))
213 }
214
215 pub fn write_string(&mut self, s: &str) -> Result<(), EncodeError> {
222 let bytes = s.as_bytes();
223 let len = u32::try_from(bytes.len().saturating_add(1)).map_err(|_| {
225 EncodeError::ValueOutOfRange {
226 message: "CDR string length exceeds u32::MAX",
227 }
228 })?;
229 self.write_u32(len)?;
230 self.write_bytes(bytes)?;
231 self.write_u8(0)
232 }
233}
234
235#[derive(Debug, Clone)]
241pub struct BufferReader<'a> {
242 bytes: &'a [u8],
243 pos: usize,
244 endianness: Endianness,
245 max_alignment: usize,
249 align_origin: usize,
254}
255
256impl<'a> BufferReader<'a> {
257 #[must_use]
259 pub fn new(bytes: &'a [u8], endianness: Endianness) -> Self {
260 Self {
261 bytes,
262 pos: 0,
263 endianness,
264 max_alignment: XCDR1_MAX_ALIGNMENT,
265 align_origin: 0,
266 }
267 }
268
269 #[must_use]
272 pub fn with_align_origin(mut self, origin: usize) -> Self {
273 self.align_origin = origin;
274 self
275 }
276
277 #[must_use]
280 pub fn with_max_alignment(mut self, max_alignment: usize) -> Self {
281 debug_assert!(
282 max_alignment.is_power_of_two(),
283 "max_alignment must be a power of two"
284 );
285 self.max_alignment = max_alignment;
286 self
287 }
288
289 #[must_use]
291 pub fn xcdr2(self) -> Self {
292 self.with_max_alignment(XCDR2_MAX_ALIGNMENT)
293 }
294
295 #[must_use]
297 pub fn max_alignment(&self) -> usize {
298 self.max_alignment
299 }
300
301 #[must_use]
303 pub fn endianness(&self) -> Endianness {
304 self.endianness
305 }
306
307 #[must_use]
309 pub fn position(&self) -> usize {
310 self.pos
311 }
312
313 #[must_use]
315 pub fn remaining(&self) -> usize {
316 self.bytes.len().saturating_sub(self.pos)
317 }
318
319 pub fn align(&mut self, alignment: usize) -> Result<(), DecodeError> {
324 debug_assert!(
325 alignment.is_power_of_two(),
326 "alignment must be power of two"
327 );
328 let alignment = alignment.min(self.max_alignment);
330 let pad = padding_for(self.pos + self.align_origin, alignment);
331 if self.remaining() < pad {
332 return Err(DecodeError::UnexpectedEof {
333 needed: pad,
334 offset: self.pos,
335 });
336 }
337 self.pos += pad;
338 Ok(())
339 }
340
341 pub fn read_bytes(&mut self, n: usize) -> Result<&'a [u8], DecodeError> {
346 if self.remaining() < n {
347 return Err(DecodeError::UnexpectedEof {
348 needed: n,
349 offset: self.pos,
350 });
351 }
352 let slice = &self.bytes[self.pos..self.pos + n];
353 self.pos += n;
354 Ok(slice)
355 }
356
357 pub fn read_u8(&mut self) -> Result<u8, DecodeError> {
362 let slice = self.read_bytes(1)?;
363 Ok(slice[0])
364 }
365
366 pub fn read_u16(&mut self) -> Result<u16, DecodeError> {
371 self.align(2)?;
372 let slice = self.read_bytes(2)?;
373 let mut buf = [0u8; 2];
374 buf.copy_from_slice(slice);
375 Ok(self.endianness.read_u16(buf))
376 }
377
378 pub fn read_u32(&mut self) -> Result<u32, DecodeError> {
383 self.align(4)?;
384 let slice = self.read_bytes(4)?;
385 let mut buf = [0u8; 4];
386 buf.copy_from_slice(slice);
387 Ok(self.endianness.read_u32(buf))
388 }
389
390 pub fn peek_u32(&self) -> Result<u32, DecodeError> {
400 if self.remaining() < 4 {
401 return Err(DecodeError::UnexpectedEof {
402 needed: 4,
403 offset: self.pos,
404 });
405 }
406 let mut buf = [0u8; 4];
407 buf.copy_from_slice(&self.bytes[self.pos..self.pos + 4]);
408 Ok(self.endianness.read_u32(buf))
409 }
410
411 pub fn read_u64(&mut self) -> Result<u64, DecodeError> {
416 self.align(8)?;
417 let slice = self.read_bytes(8)?;
418 let mut buf = [0u8; 8];
419 buf.copy_from_slice(slice);
420 Ok(self.endianness.read_u64(buf))
421 }
422
423 #[cfg(feature = "alloc")]
431 pub fn read_string(&mut self) -> Result<alloc::string::String, DecodeError> {
432 use alloc::string::String;
433 let start = self.pos;
434 let len = self.read_u32()? as usize;
435 if len == 0 {
436 return Err(DecodeError::InvalidString {
437 offset: start,
438 reason: "length must be > 0 (null terminator required)",
439 });
440 }
441 let raw = self.read_bytes(len)?;
442 if raw[len - 1] != 0 {
444 return Err(DecodeError::InvalidString {
445 offset: start,
446 reason: "missing null terminator",
447 });
448 }
449 String::from_utf8(raw[..len - 1].to_vec())
450 .map_err(|_| DecodeError::InvalidUtf8 { offset: start + 4 })
451 }
452}
453
454#[must_use]
461pub fn padding_for(pos: usize, alignment: usize) -> usize {
462 let mask = alignment - 1;
463 (alignment - (pos & mask)) & mask
464}
465
466#[cfg(test)]
467mod tests {
468 #![allow(clippy::expect_used, clippy::panic, clippy::unwrap_used)]
469 use super::*;
470
471 #[cfg(feature = "alloc")]
472 extern crate alloc;
473 #[cfg(feature = "alloc")]
474 use alloc::vec;
475
476 #[test]
477 fn padding_for_zero_position_is_zero() {
478 assert_eq!(padding_for(0, 4), 0);
479 assert_eq!(padding_for(0, 8), 0);
480 }
481
482 #[cfg(feature = "alloc")]
483 #[test]
484 fn xcdr2_caps_u64_alignment_at_4_not_8() {
485 let mut w = BufferWriter::new(Endianness::Little).xcdr2();
490 w.write_u8(1).unwrap();
491 w.write_u64(0x4242_4242_4242_4242).unwrap();
492 assert_eq!(w.position(), 4 + 8, "u64 at offset 4, not 8");
493 assert_eq!(&w.as_bytes()[1..4], &[0, 0, 0], "exactly 3 pad bytes");
494
495 let bytes = w.into_bytes();
497 let mut r = BufferReader::new(&bytes, Endianness::Little).xcdr2();
498 assert_eq!(r.read_u8().unwrap(), 1);
499 assert_eq!(r.read_u64().unwrap(), 0x4242_4242_4242_4242);
500 assert_eq!(r.position(), 12);
501 }
502
503 #[cfg(feature = "alloc")]
504 #[test]
505 fn xcdr1_default_keeps_u64_alignment_at_8() {
506 let mut w = BufferWriter::new(Endianness::Little);
509 w.write_u8(1).unwrap();
510 w.write_u64(7).unwrap();
511 assert_eq!(w.position(), 8 + 8, "XCDR1: u64 at offset 8");
512 }
513
514 #[test]
515 fn padding_for_already_aligned_is_zero() {
516 assert_eq!(padding_for(8, 4), 0);
517 assert_eq!(padding_for(16, 8), 0);
518 }
519
520 #[test]
521 fn padding_for_one_byte_to_4_align_is_three() {
522 assert_eq!(padding_for(1, 4), 3);
523 }
524
525 #[test]
526 fn padding_for_three_bytes_to_8_align_is_five() {
527 assert_eq!(padding_for(3, 8), 5);
528 }
529
530 #[cfg(feature = "alloc")]
531 #[test]
532 fn writer_writes_u8_without_padding() {
533 let mut w = BufferWriter::new(Endianness::Little);
534 w.write_u8(0xAB).unwrap();
535 assert_eq!(w.as_bytes(), &[0xAB]);
536 assert_eq!(w.position(), 1);
537 }
538
539 #[cfg(feature = "alloc")]
540 #[test]
541 fn writer_aligns_u32_after_u8() {
542 let mut w = BufferWriter::new(Endianness::Little);
543 w.write_u8(0xAB).unwrap();
544 w.write_u32(0xDEAD_BEEF).unwrap();
545 assert_eq!(w.as_bytes(), &[0xAB, 0, 0, 0, 0xEF, 0xBE, 0xAD, 0xDE]);
547 }
548
549 #[cfg(feature = "alloc")]
550 #[test]
551 fn writer_aligns_u64_after_u8() {
552 let mut w = BufferWriter::new(Endianness::Big);
553 w.write_u8(0x01).unwrap();
554 w.write_u64(0x0203_0405_0607_0809).unwrap();
555 assert_eq!(
557 w.as_bytes(),
558 &[0x01, 0, 0, 0, 0, 0, 0, 0, 2, 3, 4, 5, 6, 7, 8, 9]
559 );
560 }
561
562 #[cfg(feature = "alloc")]
563 #[test]
564 fn writer_with_capacity_preserves_endianness() {
565 let w = BufferWriter::with_capacity(Endianness::Big, 64);
566 assert_eq!(w.endianness(), Endianness::Big);
567 assert_eq!(w.position(), 0);
568 }
569
570 #[cfg(feature = "alloc")]
571 #[test]
572 fn writer_into_bytes_returns_full_buffer() {
573 let mut w = BufferWriter::new(Endianness::Little);
574 w.write_u32(0xCAFE_BABE).unwrap();
575 let bytes = w.into_bytes();
576 assert_eq!(bytes, vec![0xBE, 0xBA, 0xFE, 0xCA]);
577 }
578
579 #[test]
580 fn reader_reads_u8() {
581 let bytes = [0xAB, 0xCD];
582 let mut r = BufferReader::new(&bytes, Endianness::Little);
583 assert_eq!(r.read_u8().unwrap(), 0xAB);
584 assert_eq!(r.position(), 1);
585 }
586
587 #[test]
588 fn reader_aligns_before_u32() {
589 let bytes = [0xAB, 0, 0, 0, 0xEF, 0xBE, 0xAD, 0xDE];
590 let mut r = BufferReader::new(&bytes, Endianness::Little);
591 assert_eq!(r.read_u8().unwrap(), 0xAB);
592 assert_eq!(r.read_u32().unwrap(), 0xDEAD_BEEF);
593 assert_eq!(r.remaining(), 0);
594 }
595
596 #[test]
597 fn reader_aligns_before_u64_be() {
598 let bytes = [0x01, 0, 0, 0, 0, 0, 0, 0, 2, 3, 4, 5, 6, 7, 8, 9];
599 let mut r = BufferReader::new(&bytes, Endianness::Big);
600 assert_eq!(r.read_u8().unwrap(), 0x01);
601 assert_eq!(r.read_u64().unwrap(), 0x0203_0405_0607_0809);
602 }
603
604 #[test]
605 fn reader_unexpected_eof_on_short_read() {
606 let bytes = [0u8; 2];
607 let mut r = BufferReader::new(&bytes, Endianness::Little);
608 let res = r.read_u32();
609 match res {
611 Err(DecodeError::UnexpectedEof {
612 needed: 4,
613 offset: 0,
614 }) => {}
615 other => panic!("expected UnexpectedEof, got {other:?}"),
616 }
617 }
618
619 #[test]
620 fn reader_eof_on_align_overflow() {
621 let bytes = [0u8; 1];
622 let mut r = BufferReader::new(&bytes, Endianness::Little);
623 let _ = r.read_u8().unwrap();
624 let res = r.align(8);
625 assert!(matches!(res, Err(DecodeError::UnexpectedEof { .. })));
626 }
627
628 #[cfg(feature = "alloc")]
629 #[test]
630 fn writer_reader_roundtrip_mixed_primitives() {
631 let mut w = BufferWriter::new(Endianness::Little);
632 w.write_u8(1).unwrap();
633 w.write_u16(0x1234).unwrap();
634 w.write_u32(0x5678_9ABC).unwrap();
635 w.write_u64(0x0102_0304_0506_0708).unwrap();
636 let bytes = w.into_bytes();
637 let mut r = BufferReader::new(&bytes, Endianness::Little);
638 assert_eq!(r.read_u8().unwrap(), 1);
639 assert_eq!(r.read_u16().unwrap(), 0x1234);
640 assert_eq!(r.read_u32().unwrap(), 0x5678_9ABC);
641 assert_eq!(r.read_u64().unwrap(), 0x0102_0304_0506_0708);
642 assert_eq!(r.remaining(), 0);
643 }
644
645 #[cfg(feature = "alloc")]
646 #[test]
647 fn write_read_string_roundtrip_ascii() {
648 let mut w = BufferWriter::new(Endianness::Little);
649 w.write_string("ChatterTopic").unwrap();
650 let bytes = w.into_bytes();
651 assert_eq!(&bytes[0..4], &[13, 0, 0, 0]);
653 assert_eq!(&bytes[4..16], b"ChatterTopic");
654 assert_eq!(bytes[16], 0);
655 let mut r = BufferReader::new(&bytes, Endianness::Little);
656 assert_eq!(r.read_string().unwrap(), "ChatterTopic");
657 }
658
659 #[cfg(feature = "alloc")]
660 #[test]
661 fn write_read_string_empty() {
662 let mut w = BufferWriter::new(Endianness::Little);
664 w.write_string("").unwrap();
665 let bytes = w.into_bytes();
666 assert_eq!(&bytes[..], &[1, 0, 0, 0, 0]);
667 let mut r = BufferReader::new(&bytes, Endianness::Little);
668 assert_eq!(r.read_string().unwrap(), "");
669 }
670
671 #[cfg(feature = "alloc")]
672 #[test]
673 fn write_read_string_utf8_multibyte() {
674 let mut w = BufferWriter::new(Endianness::Little);
675 w.write_string("Zähler").unwrap(); let bytes = w.into_bytes();
677 let mut r = BufferReader::new(&bytes, Endianness::Little);
678 assert_eq!(r.read_string().unwrap(), "Zähler");
679 }
680
681 #[cfg(feature = "alloc")]
682 #[test]
683 fn read_string_rejects_length_zero() {
684 let bytes = [0u8, 0, 0, 0];
685 let mut r = BufferReader::new(&bytes, Endianness::Little);
686 assert!(matches!(
687 r.read_string(),
688 Err(DecodeError::InvalidString { .. })
689 ));
690 }
691
692 #[cfg(feature = "alloc")]
693 #[test]
694 fn read_string_rejects_missing_null_terminator() {
695 let bytes = [4u8, 0, 0, 0, b'A', b'B', b'C', b'D'];
697 let mut r = BufferReader::new(&bytes, Endianness::Little);
698 assert!(matches!(
699 r.read_string(),
700 Err(DecodeError::InvalidString { .. })
701 ));
702 }
703
704 #[cfg(feature = "alloc")]
705 #[test]
706 fn read_string_rejects_invalid_utf8() {
707 let bytes = [3u8, 0, 0, 0, 0xFF, 0xFE, 0];
709 let mut r = BufferReader::new(&bytes, Endianness::Little);
710 assert!(matches!(
711 r.read_string(),
712 Err(DecodeError::InvalidUtf8 { .. })
713 ));
714 }
715
716 #[test]
722 fn reader_endianness_getter_returns_construction_value() {
723 let r_be = BufferReader::new(&[0u8; 4], Endianness::Big);
724 assert_eq!(r_be.endianness(), Endianness::Big);
725 let r_le = BufferReader::new(&[0u8; 4], Endianness::Little);
726 assert_eq!(r_le.endianness(), Endianness::Little);
727 }
728
729 #[test]
732 fn reader_align_succeeds_when_remaining_equals_pad() {
733 let bytes = [0xAA, 0, 0, 0];
739 let mut r = BufferReader::new(&bytes, Endianness::Little);
740 r.read_u8().unwrap();
741 assert!(r.align(4).is_ok());
742 assert_eq!(r.position(), 4);
743 }
744
745 #[test]
748 fn reader_align_advances_position_strictly() {
749 let bytes = [0xAA, 0, 0, 0, 1, 2, 3, 4];
750 let mut r = BufferReader::new(&bytes, Endianness::Little);
751 r.read_u8().unwrap();
752 assert_eq!(r.position(), 1);
753 r.align(4).unwrap();
754 assert_eq!(r.position(), 4);
756 }
757
758 #[test]
761 fn reader_read_bytes_advances_position_strictly() {
762 let bytes = [1, 2, 3, 4, 5, 6, 7, 8];
763 let mut r = BufferReader::new(&bytes, Endianness::Little);
764 let _ = r.read_bytes(3).unwrap();
765 assert_eq!(r.position(), 3);
767 let _ = r.read_bytes(2).unwrap();
768 assert_eq!(r.position(), 5);
770 }
771
772 #[test]
775 fn reader_read_u16_returns_actual_bytes_not_zero() {
776 let bytes = [0x12, 0x34];
777 let mut r = BufferReader::new(&bytes, Endianness::Little);
778 assert_eq!(r.read_u16().unwrap(), 0x3412);
779 let mut r_be = BufferReader::new(&bytes, Endianness::Big);
780 assert_eq!(r_be.read_u16().unwrap(), 0x1234);
781 }
782
783 #[test]
791 fn reader_read_string_invalid_utf8_offset_is_start_plus_four() {
792 let mut bytes = vec![0xAB]; bytes.extend_from_slice(&[0, 0, 0]);
797 bytes.extend_from_slice(&3u32.to_le_bytes());
799 bytes.extend_from_slice(&[0xFF, 0xFE, 0]);
801
802 let mut r = BufferReader::new(&bytes, Endianness::Little);
803 r.read_u8().unwrap();
804 let err = r.read_string().unwrap_err();
805 match err {
808 DecodeError::InvalidUtf8 { offset } => assert_eq!(offset, 5),
809 other => panic!("expected InvalidUtf8, got {other:?}"),
810 }
811 }
812
813 #[test]
816 fn reader_read_string_invalid_utf8_offset_with_start_two() {
817 let mut bytes = vec![0xAB, 0xCD]; bytes.extend_from_slice(&[0, 0]);
820 bytes.extend_from_slice(&3u32.to_le_bytes());
821 bytes.extend_from_slice(&[0xFF, 0xFE, 0]);
822
823 let mut r = BufferReader::new(&bytes, Endianness::Little);
824 r.read_u8().unwrap();
825 r.read_u8().unwrap();
826 let err = r.read_string().unwrap_err();
827 match err {
828 DecodeError::InvalidUtf8 { offset } => assert_eq!(offset, 6),
829 other => panic!("expected InvalidUtf8, got {other:?}"),
830 }
831 }
832}