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 read_u64(&mut self) -> Result<u64, DecodeError> {
395 self.align(8)?;
396 let slice = self.read_bytes(8)?;
397 let mut buf = [0u8; 8];
398 buf.copy_from_slice(slice);
399 Ok(self.endianness.read_u64(buf))
400 }
401
402 #[cfg(feature = "alloc")]
410 pub fn read_string(&mut self) -> Result<alloc::string::String, DecodeError> {
411 use alloc::string::String;
412 let start = self.pos;
413 let len = self.read_u32()? as usize;
414 if len == 0 {
415 return Err(DecodeError::InvalidString {
416 offset: start,
417 reason: "length must be > 0 (null terminator required)",
418 });
419 }
420 let raw = self.read_bytes(len)?;
421 if raw[len - 1] != 0 {
423 return Err(DecodeError::InvalidString {
424 offset: start,
425 reason: "missing null terminator",
426 });
427 }
428 String::from_utf8(raw[..len - 1].to_vec())
429 .map_err(|_| DecodeError::InvalidUtf8 { offset: start + 4 })
430 }
431}
432
433#[must_use]
440pub fn padding_for(pos: usize, alignment: usize) -> usize {
441 let mask = alignment - 1;
442 (alignment - (pos & mask)) & mask
443}
444
445#[cfg(test)]
446mod tests {
447 #![allow(clippy::expect_used, clippy::panic, clippy::unwrap_used)]
448 use super::*;
449
450 #[cfg(feature = "alloc")]
451 extern crate alloc;
452 #[cfg(feature = "alloc")]
453 use alloc::vec;
454
455 #[test]
456 fn padding_for_zero_position_is_zero() {
457 assert_eq!(padding_for(0, 4), 0);
458 assert_eq!(padding_for(0, 8), 0);
459 }
460
461 #[cfg(feature = "alloc")]
462 #[test]
463 fn xcdr2_caps_u64_alignment_at_4_not_8() {
464 let mut w = BufferWriter::new(Endianness::Little).xcdr2();
469 w.write_u8(1).unwrap();
470 w.write_u64(0x4242_4242_4242_4242).unwrap();
471 assert_eq!(w.position(), 4 + 8, "u64 at offset 4, not 8");
472 assert_eq!(&w.as_bytes()[1..4], &[0, 0, 0], "exactly 3 pad bytes");
473
474 let bytes = w.into_bytes();
476 let mut r = BufferReader::new(&bytes, Endianness::Little).xcdr2();
477 assert_eq!(r.read_u8().unwrap(), 1);
478 assert_eq!(r.read_u64().unwrap(), 0x4242_4242_4242_4242);
479 assert_eq!(r.position(), 12);
480 }
481
482 #[cfg(feature = "alloc")]
483 #[test]
484 fn xcdr1_default_keeps_u64_alignment_at_8() {
485 let mut w = BufferWriter::new(Endianness::Little);
488 w.write_u8(1).unwrap();
489 w.write_u64(7).unwrap();
490 assert_eq!(w.position(), 8 + 8, "XCDR1: u64 at offset 8");
491 }
492
493 #[test]
494 fn padding_for_already_aligned_is_zero() {
495 assert_eq!(padding_for(8, 4), 0);
496 assert_eq!(padding_for(16, 8), 0);
497 }
498
499 #[test]
500 fn padding_for_one_byte_to_4_align_is_three() {
501 assert_eq!(padding_for(1, 4), 3);
502 }
503
504 #[test]
505 fn padding_for_three_bytes_to_8_align_is_five() {
506 assert_eq!(padding_for(3, 8), 5);
507 }
508
509 #[cfg(feature = "alloc")]
510 #[test]
511 fn writer_writes_u8_without_padding() {
512 let mut w = BufferWriter::new(Endianness::Little);
513 w.write_u8(0xAB).unwrap();
514 assert_eq!(w.as_bytes(), &[0xAB]);
515 assert_eq!(w.position(), 1);
516 }
517
518 #[cfg(feature = "alloc")]
519 #[test]
520 fn writer_aligns_u32_after_u8() {
521 let mut w = BufferWriter::new(Endianness::Little);
522 w.write_u8(0xAB).unwrap();
523 w.write_u32(0xDEAD_BEEF).unwrap();
524 assert_eq!(w.as_bytes(), &[0xAB, 0, 0, 0, 0xEF, 0xBE, 0xAD, 0xDE]);
526 }
527
528 #[cfg(feature = "alloc")]
529 #[test]
530 fn writer_aligns_u64_after_u8() {
531 let mut w = BufferWriter::new(Endianness::Big);
532 w.write_u8(0x01).unwrap();
533 w.write_u64(0x0203_0405_0607_0809).unwrap();
534 assert_eq!(
536 w.as_bytes(),
537 &[0x01, 0, 0, 0, 0, 0, 0, 0, 2, 3, 4, 5, 6, 7, 8, 9]
538 );
539 }
540
541 #[cfg(feature = "alloc")]
542 #[test]
543 fn writer_with_capacity_preserves_endianness() {
544 let w = BufferWriter::with_capacity(Endianness::Big, 64);
545 assert_eq!(w.endianness(), Endianness::Big);
546 assert_eq!(w.position(), 0);
547 }
548
549 #[cfg(feature = "alloc")]
550 #[test]
551 fn writer_into_bytes_returns_full_buffer() {
552 let mut w = BufferWriter::new(Endianness::Little);
553 w.write_u32(0xCAFE_BABE).unwrap();
554 let bytes = w.into_bytes();
555 assert_eq!(bytes, vec![0xBE, 0xBA, 0xFE, 0xCA]);
556 }
557
558 #[test]
559 fn reader_reads_u8() {
560 let bytes = [0xAB, 0xCD];
561 let mut r = BufferReader::new(&bytes, Endianness::Little);
562 assert_eq!(r.read_u8().unwrap(), 0xAB);
563 assert_eq!(r.position(), 1);
564 }
565
566 #[test]
567 fn reader_aligns_before_u32() {
568 let bytes = [0xAB, 0, 0, 0, 0xEF, 0xBE, 0xAD, 0xDE];
569 let mut r = BufferReader::new(&bytes, Endianness::Little);
570 assert_eq!(r.read_u8().unwrap(), 0xAB);
571 assert_eq!(r.read_u32().unwrap(), 0xDEAD_BEEF);
572 assert_eq!(r.remaining(), 0);
573 }
574
575 #[test]
576 fn reader_aligns_before_u64_be() {
577 let bytes = [0x01, 0, 0, 0, 0, 0, 0, 0, 2, 3, 4, 5, 6, 7, 8, 9];
578 let mut r = BufferReader::new(&bytes, Endianness::Big);
579 assert_eq!(r.read_u8().unwrap(), 0x01);
580 assert_eq!(r.read_u64().unwrap(), 0x0203_0405_0607_0809);
581 }
582
583 #[test]
584 fn reader_unexpected_eof_on_short_read() {
585 let bytes = [0u8; 2];
586 let mut r = BufferReader::new(&bytes, Endianness::Little);
587 let res = r.read_u32();
588 match res {
590 Err(DecodeError::UnexpectedEof {
591 needed: 4,
592 offset: 0,
593 }) => {}
594 other => panic!("expected UnexpectedEof, got {other:?}"),
595 }
596 }
597
598 #[test]
599 fn reader_eof_on_align_overflow() {
600 let bytes = [0u8; 1];
601 let mut r = BufferReader::new(&bytes, Endianness::Little);
602 let _ = r.read_u8().unwrap();
603 let res = r.align(8);
604 assert!(matches!(res, Err(DecodeError::UnexpectedEof { .. })));
605 }
606
607 #[cfg(feature = "alloc")]
608 #[test]
609 fn writer_reader_roundtrip_mixed_primitives() {
610 let mut w = BufferWriter::new(Endianness::Little);
611 w.write_u8(1).unwrap();
612 w.write_u16(0x1234).unwrap();
613 w.write_u32(0x5678_9ABC).unwrap();
614 w.write_u64(0x0102_0304_0506_0708).unwrap();
615 let bytes = w.into_bytes();
616 let mut r = BufferReader::new(&bytes, Endianness::Little);
617 assert_eq!(r.read_u8().unwrap(), 1);
618 assert_eq!(r.read_u16().unwrap(), 0x1234);
619 assert_eq!(r.read_u32().unwrap(), 0x5678_9ABC);
620 assert_eq!(r.read_u64().unwrap(), 0x0102_0304_0506_0708);
621 assert_eq!(r.remaining(), 0);
622 }
623
624 #[cfg(feature = "alloc")]
625 #[test]
626 fn write_read_string_roundtrip_ascii() {
627 let mut w = BufferWriter::new(Endianness::Little);
628 w.write_string("ChatterTopic").unwrap();
629 let bytes = w.into_bytes();
630 assert_eq!(&bytes[0..4], &[13, 0, 0, 0]);
632 assert_eq!(&bytes[4..16], b"ChatterTopic");
633 assert_eq!(bytes[16], 0);
634 let mut r = BufferReader::new(&bytes, Endianness::Little);
635 assert_eq!(r.read_string().unwrap(), "ChatterTopic");
636 }
637
638 #[cfg(feature = "alloc")]
639 #[test]
640 fn write_read_string_empty() {
641 let mut w = BufferWriter::new(Endianness::Little);
643 w.write_string("").unwrap();
644 let bytes = w.into_bytes();
645 assert_eq!(&bytes[..], &[1, 0, 0, 0, 0]);
646 let mut r = BufferReader::new(&bytes, Endianness::Little);
647 assert_eq!(r.read_string().unwrap(), "");
648 }
649
650 #[cfg(feature = "alloc")]
651 #[test]
652 fn write_read_string_utf8_multibyte() {
653 let mut w = BufferWriter::new(Endianness::Little);
654 w.write_string("Zähler").unwrap(); let bytes = w.into_bytes();
656 let mut r = BufferReader::new(&bytes, Endianness::Little);
657 assert_eq!(r.read_string().unwrap(), "Zähler");
658 }
659
660 #[cfg(feature = "alloc")]
661 #[test]
662 fn read_string_rejects_length_zero() {
663 let bytes = [0u8, 0, 0, 0];
664 let mut r = BufferReader::new(&bytes, Endianness::Little);
665 assert!(matches!(
666 r.read_string(),
667 Err(DecodeError::InvalidString { .. })
668 ));
669 }
670
671 #[cfg(feature = "alloc")]
672 #[test]
673 fn read_string_rejects_missing_null_terminator() {
674 let bytes = [4u8, 0, 0, 0, b'A', b'B', b'C', b'D'];
676 let mut r = BufferReader::new(&bytes, Endianness::Little);
677 assert!(matches!(
678 r.read_string(),
679 Err(DecodeError::InvalidString { .. })
680 ));
681 }
682
683 #[cfg(feature = "alloc")]
684 #[test]
685 fn read_string_rejects_invalid_utf8() {
686 let bytes = [3u8, 0, 0, 0, 0xFF, 0xFE, 0];
688 let mut r = BufferReader::new(&bytes, Endianness::Little);
689 assert!(matches!(
690 r.read_string(),
691 Err(DecodeError::InvalidUtf8 { .. })
692 ));
693 }
694
695 #[test]
701 fn reader_endianness_getter_returns_construction_value() {
702 let r_be = BufferReader::new(&[0u8; 4], Endianness::Big);
703 assert_eq!(r_be.endianness(), Endianness::Big);
704 let r_le = BufferReader::new(&[0u8; 4], Endianness::Little);
705 assert_eq!(r_le.endianness(), Endianness::Little);
706 }
707
708 #[test]
711 fn reader_align_succeeds_when_remaining_equals_pad() {
712 let bytes = [0xAA, 0, 0, 0];
718 let mut r = BufferReader::new(&bytes, Endianness::Little);
719 r.read_u8().unwrap();
720 assert!(r.align(4).is_ok());
721 assert_eq!(r.position(), 4);
722 }
723
724 #[test]
727 fn reader_align_advances_position_strictly() {
728 let bytes = [0xAA, 0, 0, 0, 1, 2, 3, 4];
729 let mut r = BufferReader::new(&bytes, Endianness::Little);
730 r.read_u8().unwrap();
731 assert_eq!(r.position(), 1);
732 r.align(4).unwrap();
733 assert_eq!(r.position(), 4);
735 }
736
737 #[test]
740 fn reader_read_bytes_advances_position_strictly() {
741 let bytes = [1, 2, 3, 4, 5, 6, 7, 8];
742 let mut r = BufferReader::new(&bytes, Endianness::Little);
743 let _ = r.read_bytes(3).unwrap();
744 assert_eq!(r.position(), 3);
746 let _ = r.read_bytes(2).unwrap();
747 assert_eq!(r.position(), 5);
749 }
750
751 #[test]
754 fn reader_read_u16_returns_actual_bytes_not_zero() {
755 let bytes = [0x12, 0x34];
756 let mut r = BufferReader::new(&bytes, Endianness::Little);
757 assert_eq!(r.read_u16().unwrap(), 0x3412);
758 let mut r_be = BufferReader::new(&bytes, Endianness::Big);
759 assert_eq!(r_be.read_u16().unwrap(), 0x1234);
760 }
761
762 #[test]
770 fn reader_read_string_invalid_utf8_offset_is_start_plus_four() {
771 let mut bytes = vec![0xAB]; bytes.extend_from_slice(&[0, 0, 0]);
776 bytes.extend_from_slice(&3u32.to_le_bytes());
778 bytes.extend_from_slice(&[0xFF, 0xFE, 0]);
780
781 let mut r = BufferReader::new(&bytes, Endianness::Little);
782 r.read_u8().unwrap();
783 let err = r.read_string().unwrap_err();
784 match err {
787 DecodeError::InvalidUtf8 { offset } => assert_eq!(offset, 5),
788 other => panic!("expected InvalidUtf8, got {other:?}"),
789 }
790 }
791
792 #[test]
795 fn reader_read_string_invalid_utf8_offset_with_start_two() {
796 let mut bytes = vec![0xAB, 0xCD]; bytes.extend_from_slice(&[0, 0]);
799 bytes.extend_from_slice(&3u32.to_le_bytes());
800 bytes.extend_from_slice(&[0xFF, 0xFE, 0]);
801
802 let mut r = BufferReader::new(&bytes, Endianness::Little);
803 r.read_u8().unwrap();
804 r.read_u8().unwrap();
805 let err = r.read_string().unwrap_err();
806 match err {
807 DecodeError::InvalidUtf8 { offset } => assert_eq!(offset, 6),
808 other => panic!("expected InvalidUtf8, got {other:?}"),
809 }
810 }
811}