1use super::reader::MAX_DEPTH;
15use super::types::{ContainerKind, ElementType, Tag, Width, write_le};
16use crate::error::{Error, ErrorCode, Result, bail};
17
18#[derive(Debug, Clone, Copy, PartialEq, Eq)]
23pub struct Checkpoint {
24 pos: usize,
25 stack: [ContainerKind; MAX_DEPTH],
26 depth: usize,
27 top_level_done: bool,
28}
29
30#[derive(Debug)]
32pub struct TlvWriter<'a> {
33 buf: &'a mut [u8],
34 pos: usize,
35 stack: [ContainerKind; MAX_DEPTH],
36 depth: usize,
37 top_level_done: bool,
38 started_inside: bool,
41}
42
43impl<'a> TlvWriter<'a> {
44 #[must_use]
46 pub fn new(buf: &'a mut [u8]) -> Self {
47 Self {
48 buf,
49 pos: 0,
50 stack: [ContainerKind::Structure; MAX_DEPTH],
51 depth: 0,
52 top_level_done: false,
53 started_inside: false,
54 }
55 }
56
57 #[must_use]
81 pub fn new_in(buf: &'a mut [u8], container: ContainerKind) -> Self {
82 Self {
83 buf,
84 pos: 0,
85 stack: [container; MAX_DEPTH],
86 depth: 1,
87 top_level_done: false,
88 started_inside: true,
89 }
90 }
91
92 #[must_use]
94 pub const fn len(&self) -> usize {
95 self.pos
96 }
97
98 #[must_use]
100 pub const fn is_empty(&self) -> bool {
101 self.pos == 0
102 }
103
104 #[must_use]
106 pub const fn depth(&self) -> usize {
107 self.depth
108 }
109
110 #[must_use]
114 pub fn written(&self) -> &[u8] {
115 self.buf.get(..self.pos).unwrap_or(&[])
116 }
117
118 #[must_use]
143 pub const fn checkpoint(&self) -> Checkpoint {
144 Checkpoint {
145 pos: self.pos,
146 stack: self.stack,
147 depth: self.depth,
148 top_level_done: self.top_level_done,
149 }
150 }
151
152 pub const fn rollback(&mut self, checkpoint: &Checkpoint) {
159 self.pos = checkpoint.pos;
160 self.stack = checkpoint.stack;
161 self.depth = checkpoint.depth;
162 self.top_level_done = checkpoint.top_level_done;
163 }
164
165 #[must_use]
170 pub const fn remaining(&self) -> usize {
171 self.buf.len().saturating_sub(self.pos)
172 }
173
174 pub fn finish(self) -> Result<&'a [u8]> {
177 let base = usize::from(self.started_inside);
178 if self.depth != base {
179 bail!(TlvContainerMismatch)
180 }
181 if self.started_inside {
185 if self.pos == 0 {
186 bail!(InvalidState)
187 }
188 } else if !self.top_level_done {
189 bail!(InvalidState)
190 }
191 self.buf
192 .get(..self.pos)
193 .ok_or(Error::new(ErrorCode::BufferTooSmall))
194 }
195
196 fn room(&mut self, n: usize) -> Result<&mut [u8]> {
197 let end = self
198 .pos
199 .checked_add(n)
200 .ok_or(Error::new(ErrorCode::BufferTooSmall))?;
201 let Some(slice) = self.buf.get_mut(self.pos..end) else {
202 bail!(BufferTooSmall)
203 };
204 Ok(slice)
205 }
206
207 fn advance(&mut self, n: usize) -> Result<()> {
208 self.pos = self
209 .pos
210 .checked_add(n)
211 .ok_or(Error::new(ErrorCode::BufferTooSmall))?;
212 Ok(())
213 }
214
215 fn current_container(&self) -> Option<ContainerKind> {
216 self.depth
217 .checked_sub(1)
218 .and_then(|i| self.stack.get(i))
219 .copied()
220 }
221
222 fn check_tag(&self, tag: Tag) -> Result<()> {
224 if self.depth == 0 && self.top_level_done {
225 bail!(InvalidState)
227 }
228 match self.current_container() {
229 None => {
230 if tag.context().is_some() {
231 bail!(TlvInvalidTag)
232 }
233 }
234 Some(ContainerKind::Structure) => {
235 if tag.is_anonymous() {
236 bail!(TlvInvalidTag)
237 }
238 }
239 Some(ContainerKind::Array) => {
240 if !tag.is_anonymous() {
241 bail!(TlvInvalidTag)
242 }
243 }
244 Some(ContainerKind::List) => {}
245 }
246 Ok(())
247 }
248
249 fn head(&mut self, tag: Tag, ty: ElementType) -> Result<()> {
251 self.check_tag(tag)?;
252 let tag_len = tag.encoded_len();
253 let total = tag_len
254 .checked_add(1)
255 .ok_or(Error::new(ErrorCode::BufferTooSmall))?;
256 let slot = self.room(total)?;
257 let Some(control) = slot.first_mut() else {
258 bail!(BufferTooSmall)
259 };
260 *control = tag.control_bits() | ty.bits();
261 let Some(rest) = slot.get_mut(1..) else {
262 bail!(BufferTooSmall)
263 };
264 tag.encode(rest)?;
265 self.advance(total)?;
266 if self.depth == 0 {
267 self.top_level_done = true;
268 }
269 Ok(())
270 }
271
272 pub fn signed(&mut self, tag: Tag, value: i64) -> Result<()> {
274 let w = Width::for_signed(value);
275 self.head(tag, ElementType::SignedInt(w))?;
276 let n = w.octets();
277 let slot = self.room(n)?;
278 write_le(slot, value as u64, n)?;
279 self.advance(n)
280 }
281
282 pub fn unsigned(&mut self, tag: Tag, value: u64) -> Result<()> {
284 let w = Width::for_unsigned(value);
285 self.head(tag, ElementType::UnsignedInt(w))?;
286 let n = w.octets();
287 let slot = self.room(n)?;
288 write_le(slot, value, n)?;
289 self.advance(n)
290 }
291
292 pub fn bool(&mut self, tag: Tag, value: bool) -> Result<()> {
294 self.head(tag, ElementType::Bool(value))
295 }
296
297 pub fn null(&mut self, tag: Tag) -> Result<()> {
299 self.head(tag, ElementType::Null)
300 }
301
302 pub fn float(&mut self, tag: Tag, value: f32) -> Result<()> {
304 self.head(tag, ElementType::Float)?;
305 let slot = self.room(4)?;
306 write_le(slot, u64::from(value.to_bits()), 4)?;
307 self.advance(4)
308 }
309
310 pub fn double(&mut self, tag: Tag, value: f64) -> Result<()> {
312 self.head(tag, ElementType::Double)?;
313 let slot = self.room(8)?;
314 write_le(slot, value.to_bits(), 8)?;
315 self.advance(8)
316 }
317
318 pub fn utf8(&mut self, tag: Tag, value: &str) -> Result<()> {
320 self.string(tag, value.as_bytes(), true)
321 }
322
323 pub fn octets(&mut self, tag: Tag, value: &[u8]) -> Result<()> {
325 self.string(tag, value, false)
326 }
327
328 pub fn octets_fill(&mut self, tag: Tag, len: usize, byte: u8) -> Result<()> {
337 let length = len as u64;
338 let w = Width::for_unsigned(length);
339 self.head(tag, ElementType::Octets(w))?;
340 let n = w.octets();
341 let slot = self.room(n)?;
342 write_le(slot, length, n)?;
343 self.advance(n)?;
344 let dst = self.room(len)?;
345 let Some(dst) = dst.get_mut(..len) else {
346 bail!(BufferTooSmall)
347 };
348 dst.fill(byte);
349 self.advance(len)
350 }
351
352 fn string(&mut self, tag: Tag, value: &[u8], utf8: bool) -> Result<()> {
353 let len = value.len() as u64;
354 let w = Width::for_unsigned(len);
355 let ty = if utf8 {
356 ElementType::Utf8(w)
357 } else {
358 ElementType::Octets(w)
359 };
360 self.head(tag, ty)?;
361 let n = w.octets();
362 let slot = self.room(n)?;
363 write_le(slot, len, n)?;
364 self.advance(n)?;
365 let dst = self.room(value.len())?;
366 let Some(dst) = dst.get_mut(..value.len()) else {
367 bail!(BufferTooSmall)
368 };
369 dst.copy_from_slice(value);
370 self.advance(value.len())
371 }
372
373 pub fn start(&mut self, tag: Tag, kind: ContainerKind) -> Result<()> {
375 if self.depth >= MAX_DEPTH {
376 bail!(TlvDepthExceeded)
377 }
378 self.head(tag, kind.element_type())?;
379 let Some(slot) = self.stack.get_mut(self.depth) else {
380 bail!(TlvDepthExceeded)
381 };
382 *slot = kind;
383 self.depth = self
384 .depth
385 .checked_add(1)
386 .ok_or(Error::new(ErrorCode::TlvDepthExceeded))?;
387 self.top_level_done = false;
390 Ok(())
391 }
392
393 pub fn start_structure(&mut self, tag: Tag) -> Result<()> {
395 self.start(tag, ContainerKind::Structure)
396 }
397
398 pub fn start_array(&mut self, tag: Tag) -> Result<()> {
400 self.start(tag, ContainerKind::Array)
401 }
402
403 pub fn start_list(&mut self, tag: Tag) -> Result<()> {
405 self.start(tag, ContainerKind::List)
406 }
407
408 pub fn end_container(&mut self) -> Result<()> {
410 let Some(new_depth) = self.depth.checked_sub(1) else {
411 bail!(TlvContainerMismatch)
412 };
413 let slot = self.room(1)?;
414 let Some(b) = slot.first_mut() else {
415 bail!(BufferTooSmall)
416 };
417 *b = 0x18;
418 self.advance(1)?;
419 self.depth = new_depth;
420 if self.depth == 0 {
421 self.top_level_done = true;
422 }
423 Ok(())
424 }
425
426 pub fn raw_element_retagged(&mut self, bytes: &[u8], tag: Tag) -> Result<()> {
438 use super::reader::TlvReader;
439 let mut r = TlvReader::new_in(bytes, ContainerKind::List);
442 let Some(first) = r.next_element()? else {
443 bail!(InvalidArgument)
444 };
445 if first.value.container().is_some() {
446 r.skip_container()?;
447 }
448 r.finish()?;
449
450 let Some(&control) = bytes.first() else {
451 bail!(InvalidArgument)
452 };
453 let old_tag_len = first.tag.encoded_len();
456 let value_start = old_tag_len
457 .checked_add(1)
458 .ok_or(Error::new(ErrorCode::InvalidArgument))?;
459 let Some(value) = bytes.get(value_start..) else {
460 bail!(InvalidArgument)
461 };
462
463 self.check_tag(tag)?;
464 let new_tag_len = tag.encoded_len();
465 let head = new_tag_len
466 .checked_add(1)
467 .ok_or(Error::new(ErrorCode::BufferTooSmall))?;
468 let total = head
469 .checked_add(value.len())
470 .ok_or(Error::new(ErrorCode::BufferTooSmall))?;
471 let slot = self.room(total)?;
472 let Some(first_byte) = slot.first_mut() else {
473 bail!(BufferTooSmall)
474 };
475 *first_byte = tag.control_bits() | (control & 0x1F);
476 let Some(rest) = slot.get_mut(1..) else {
477 bail!(BufferTooSmall)
478 };
479 tag.encode(rest)?;
480 let Some(dst) = slot.get_mut(head..total) else {
481 bail!(BufferTooSmall)
482 };
483 dst.copy_from_slice(value);
484 self.advance(total)?;
485 if self.depth == 0 {
486 self.top_level_done = true;
487 }
488 Ok(())
489 }
490
491 pub fn raw_element(&mut self, bytes: &[u8]) -> Result<()> {
502 use super::reader::TlvReader;
503 let mut r = match self.current_container() {
504 Some(kind) => TlvReader::new_in(bytes, kind),
505 None => TlvReader::new(bytes),
506 };
507 let Some(first) = r.next_element()? else {
508 bail!(InvalidArgument)
509 };
510 if first.value.container().is_some() {
511 r.skip_container()?;
512 }
513 r.finish()?;
514 self.check_tag(first.tag)?;
515 let dst = self.room(bytes.len())?;
516 let Some(dst) = dst.get_mut(..bytes.len()) else {
517 bail!(BufferTooSmall)
518 };
519 dst.copy_from_slice(bytes);
520 self.advance(bytes.len())?;
521 if self.depth == 0 {
522 self.top_level_done = true;
523 }
524 Ok(())
525 }
526}
527
528#[cfg(test)]
529#[allow(clippy::arithmetic_side_effects, clippy::indexing_slicing)]
530mod tests {
531 use super::*;
532 use crate::tlv::TlvReader;
533
534 fn write_with(f: impl FnOnce(&mut TlvWriter<'_>) -> Result<()>) -> heapless::Vec<u8, 256> {
535 let mut buf = [0u8; 256];
536 let mut w = TlvWriter::new(&mut buf);
537 f(&mut w).expect("write");
538 let out = w.finish().expect("finish");
539 heapless::Vec::from_slice(out).expect("fits")
540 }
541
542 #[test]
543 fn primitives_match_table_127() {
544 assert_eq!(&write_with(|w| w.bool(Tag::Anonymous, false))[..], &[0x08]);
545 assert_eq!(&write_with(|w| w.bool(Tag::Anonymous, true))[..], &[0x09]);
546 assert_eq!(
547 &write_with(|w| w.signed(Tag::Anonymous, 42))[..],
548 &[0x00, 0x2a]
549 );
550 assert_eq!(
551 &write_with(|w| w.signed(Tag::Anonymous, -17))[..],
552 &[0x00, 0xef]
553 );
554 assert_eq!(
555 &write_with(|w| w.unsigned(Tag::Anonymous, 42))[..],
556 &[0x04, 0x2a]
557 );
558 assert_eq!(
559 &write_with(|w| w.signed(Tag::Anonymous, -170_000))[..],
560 &[0x02, 0xf0, 0x67, 0xfd, 0xff]
561 );
562 assert_eq!(
563 &write_with(|w| w.signed(Tag::Anonymous, 40_000_000_000))[..],
564 &[0x03, 0x00, 0x90, 0x2f, 0x50, 0x09, 0x00, 0x00, 0x00]
565 );
566 assert_eq!(
567 &write_with(|w| w.utf8(Tag::Anonymous, "Hello!"))[..],
568 &[0x0c, 0x06, 0x48, 0x65, 0x6c, 0x6c, 0x6f, 0x21]
569 );
570 assert_eq!(
571 &write_with(|w| w.utf8(Tag::Anonymous, "Tschüs"))[..],
572 &[0x0c, 0x07, 0x54, 0x73, 0x63, 0x68, 0xc3, 0xbc, 0x73]
573 );
574 assert_eq!(
575 &write_with(|w| w.octets(Tag::Anonymous, &[0, 1, 2, 3, 4]))[..],
576 &[0x10, 0x05, 0x00, 0x01, 0x02, 0x03, 0x04]
577 );
578 assert_eq!(&write_with(|w| w.null(Tag::Anonymous))[..], &[0x14]);
579 assert_eq!(
580 &write_with(|w| w.float(Tag::Anonymous, 17.9))[..],
581 &[0x0a, 0x33, 0x33, 0x8f, 0x41]
582 );
583 assert_eq!(
584 &write_with(|w| w.double(Tag::Anonymous, 17.9))[..],
585 &[0x0b, 0x66, 0x66, 0x66, 0x66, 0x66, 0xe6, 0x31, 0x40]
586 );
587 assert_eq!(
588 &write_with(|w| w.float(Tag::Anonymous, f32::NEG_INFINITY))[..],
589 &[0x0a, 0x00, 0x00, 0x80, 0xff]
590 );
591 }
592
593 #[test]
594 fn profile_tags_match_table_129() {
595 assert_eq!(
596 &write_with(|w| w.unsigned(Tag::Common(1), 42))[..],
597 &[0x44, 0x01, 0x00, 0x2a]
598 );
599 assert_eq!(
600 &write_with(|w| w.unsigned(Tag::Common(100_000), 42))[..],
601 &[0x64, 0xa0, 0x86, 0x01, 0x00, 0x2a]
602 );
603 assert_eq!(
604 &write_with(|w| w.unsigned(
605 Tag::FullyQualified {
606 vendor: 0xFFF1,
607 profile: 0xDEED,
608 number: 1
609 },
610 42
611 ))[..],
612 &[0xc4, 0xf1, 0xff, 0xed, 0xde, 0x01, 0x00, 0x2a]
613 );
614 assert_eq!(
615 &write_with(|w| w.unsigned(
616 Tag::FullyQualified {
617 vendor: 0xFFF1,
618 profile: 0xDEED,
619 number: 0xAA55_FEED
620 },
621 42
622 ))[..],
623 &[0xe4, 0xf1, 0xff, 0xed, 0xde, 0xed, 0xfe, 0x55, 0xaa, 0x2a]
624 );
625 }
626
627 #[test]
628 fn containers_match_table_128() {
629 assert_eq!(
630 &write_with(|w| {
631 w.start_structure(Tag::Anonymous)?;
632 w.end_container()
633 })[..],
634 &[0x15, 0x18]
635 );
636 assert_eq!(
637 &write_with(|w| {
638 w.start_structure(Tag::Anonymous)?;
639 w.signed(Tag::Context(0), 42)?;
640 w.signed(Tag::Context(1), -17)?;
641 w.end_container()
642 })[..],
643 &[0x15, 0x20, 0x00, 0x2a, 0x20, 0x01, 0xef, 0x18]
644 );
645 assert_eq!(
646 &write_with(|w| {
647 w.start_array(Tag::Anonymous)?;
648 for v in 0..5 {
649 w.signed(Tag::Anonymous, v)?;
650 }
651 w.end_container()
652 })[..],
653 &[
654 0x16, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x04, 0x18
655 ]
656 );
657 assert_eq!(
658 &write_with(|w| {
659 w.start_list(Tag::Anonymous)?;
660 w.signed(Tag::Anonymous, 1)?;
661 w.signed(Tag::Context(0), 42)?;
662 w.signed(Tag::Anonymous, 2)?;
663 w.signed(Tag::Anonymous, 3)?;
664 w.signed(Tag::Context(0), -17)?;
665 w.end_container()
666 })[..],
667 &[
668 0x17, 0x00, 0x01, 0x20, 0x00, 0x2a, 0x00, 0x02, 0x00, 0x03, 0x20, 0x00, 0xef, 0x18
669 ]
670 );
671 assert_eq!(
673 &write_with(|w| {
674 w.start_structure(Tag::FullyQualified {
675 vendor: 0xFFF1,
676 profile: 0xDEED,
677 number: 1,
678 })?;
679 w.unsigned(
680 Tag::FullyQualified {
681 vendor: 0xFFF1,
682 profile: 0xDEED,
683 number: 0xAA55,
684 },
685 42,
686 )?;
687 w.end_container()
688 })[..],
689 &[
690 0xd5, 0xf1, 0xff, 0xed, 0xde, 0x01, 0x00, 0xc4, 0xf1, 0xff, 0xed, 0xde, 0x55, 0xaa,
691 0x2a, 0x18
692 ]
693 );
694 }
695
696 #[test]
697 fn the_writer_refuses_to_emit_invalid_tlv() {
698 let mut buf = [0u8; 64];
699
700 let mut w = TlvWriter::new(&mut buf);
701 w.start_structure(Tag::Anonymous).unwrap();
702 assert_eq!(
703 w.unsigned(Tag::Anonymous, 1).unwrap_err().code(),
704 ErrorCode::TlvInvalidTag,
705 "anonymous member of a structure"
706 );
707
708 let mut w = TlvWriter::new(&mut buf);
709 w.start_array(Tag::Anonymous).unwrap();
710 assert_eq!(
711 w.unsigned(Tag::Context(1), 1).unwrap_err().code(),
712 ErrorCode::TlvInvalidTag,
713 "tagged member of an array"
714 );
715
716 let mut w = TlvWriter::new(&mut buf);
717 assert_eq!(
718 w.unsigned(Tag::Context(1), 1).unwrap_err().code(),
719 ErrorCode::TlvInvalidTag,
720 "context tag at the top level"
721 );
722
723 let mut w = TlvWriter::new(&mut buf);
724 w.unsigned(Tag::Anonymous, 1).unwrap();
725 assert_eq!(
726 w.unsigned(Tag::Anonymous, 2).unwrap_err().code(),
727 ErrorCode::InvalidState,
728 "second top-level element"
729 );
730
731 let mut w = TlvWriter::new(&mut buf);
732 w.start_structure(Tag::Anonymous).unwrap();
733 assert_eq!(
734 w.finish().unwrap_err().code(),
735 ErrorCode::TlvContainerMismatch,
736 "unclosed container"
737 );
738 }
739
740 #[test]
741 fn a_full_buffer_is_an_error_not_a_panic() {
742 let mut buf = [0u8; 3];
743 let mut w = TlvWriter::new(&mut buf);
744 assert_eq!(
745 w.utf8(Tag::Anonymous, "Hello!").unwrap_err().code(),
746 ErrorCode::BufferTooSmall
747 );
748 }
749
750 #[test]
751 fn everything_written_reads_back() {
752 let out = write_with(|w| {
753 w.start_structure(Tag::Anonymous)?;
754 w.unsigned(Tag::Context(0), 1)?;
755 w.utf8(Tag::Context(1), "matter")?;
756 w.start_array(Tag::Context(2))?;
757 w.unsigned(Tag::Anonymous, 7)?;
758 w.end_container()?;
759 w.end_container()
760 });
761 assert!(TlvReader::validate(&out).is_ok());
762 }
763
764 #[test]
765 fn raw_element_checks_what_it_copies() {
766 let mut buf = [0u8; 64];
767 let mut w = TlvWriter::new(&mut buf);
768 w.start_structure(Tag::Anonymous).unwrap();
769 w.raw_element(&[0x24, 0x01, 0x2a]).unwrap();
771 assert!(w.raw_element(&[0x15]).is_err());
773 assert_eq!(
775 w.raw_element(&[0x04, 0x2a]).unwrap_err().code(),
776 ErrorCode::TlvInvalidTag
777 );
778 w.end_container().unwrap();
779 let out = w.finish().unwrap();
780 assert_eq!(out, &[0x15, 0x24, 0x01, 0x2a, 0x18]);
781 }
782}