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 #[expect(
274 clippy::cast_sign_loss,
275 reason = "a two's-complement reinterpretation before a little-endian write of exactly `n` octets"
276 )]
277 pub fn signed(&mut self, tag: Tag, value: i64) -> Result<()> {
278 let w = Width::for_signed(value);
279 self.head(tag, ElementType::SignedInt(w))?;
280 let n = w.octets();
281 let slot = self.room(n)?;
282 write_le(slot, value as u64, n)?;
283 self.advance(n)
284 }
285
286 pub fn unsigned(&mut self, tag: Tag, value: u64) -> Result<()> {
288 let w = Width::for_unsigned(value);
289 self.head(tag, ElementType::UnsignedInt(w))?;
290 let n = w.octets();
291 let slot = self.room(n)?;
292 write_le(slot, value, n)?;
293 self.advance(n)
294 }
295
296 pub fn bool(&mut self, tag: Tag, value: bool) -> Result<()> {
298 self.head(tag, ElementType::Bool(value))
299 }
300
301 pub fn null(&mut self, tag: Tag) -> Result<()> {
303 self.head(tag, ElementType::Null)
304 }
305
306 pub fn float(&mut self, tag: Tag, value: f32) -> Result<()> {
308 self.head(tag, ElementType::Float)?;
309 let slot = self.room(4)?;
310 write_le(slot, u64::from(value.to_bits()), 4)?;
311 self.advance(4)
312 }
313
314 pub fn double(&mut self, tag: Tag, value: f64) -> Result<()> {
316 self.head(tag, ElementType::Double)?;
317 let slot = self.room(8)?;
318 write_le(slot, value.to_bits(), 8)?;
319 self.advance(8)
320 }
321
322 pub fn utf8(&mut self, tag: Tag, value: &str) -> Result<()> {
324 self.string(tag, value.as_bytes(), true)
325 }
326
327 pub fn octets(&mut self, tag: Tag, value: &[u8]) -> Result<()> {
329 self.string(tag, value, false)
330 }
331
332 pub fn octets_fill(&mut self, tag: Tag, len: usize, byte: u8) -> Result<()> {
341 let length = len as u64;
342 let w = Width::for_unsigned(length);
343 self.head(tag, ElementType::Octets(w))?;
344 let n = w.octets();
345 let slot = self.room(n)?;
346 write_le(slot, length, n)?;
347 self.advance(n)?;
348 let dst = self.room(len)?;
349 let Some(dst) = dst.get_mut(..len) else {
350 bail!(BufferTooSmall)
351 };
352 dst.fill(byte);
353 self.advance(len)
354 }
355
356 fn string(&mut self, tag: Tag, value: &[u8], utf8: bool) -> Result<()> {
357 let len = value.len() as u64;
358 let w = Width::for_unsigned(len);
359 let ty = if utf8 {
360 ElementType::Utf8(w)
361 } else {
362 ElementType::Octets(w)
363 };
364 self.head(tag, ty)?;
365 let n = w.octets();
366 let slot = self.room(n)?;
367 write_le(slot, len, n)?;
368 self.advance(n)?;
369 let dst = self.room(value.len())?;
370 let Some(dst) = dst.get_mut(..value.len()) else {
371 bail!(BufferTooSmall)
372 };
373 dst.copy_from_slice(value);
374 self.advance(value.len())
375 }
376
377 pub fn start(&mut self, tag: Tag, kind: ContainerKind) -> Result<()> {
379 if self.depth >= MAX_DEPTH {
380 bail!(TlvDepthExceeded)
381 }
382 self.head(tag, kind.element_type())?;
383 let Some(slot) = self.stack.get_mut(self.depth) else {
384 bail!(TlvDepthExceeded)
385 };
386 *slot = kind;
387 self.depth = self
388 .depth
389 .checked_add(1)
390 .ok_or(Error::new(ErrorCode::TlvDepthExceeded))?;
391 self.top_level_done = false;
394 Ok(())
395 }
396
397 pub fn start_structure(&mut self, tag: Tag) -> Result<()> {
399 self.start(tag, ContainerKind::Structure)
400 }
401
402 pub fn start_array(&mut self, tag: Tag) -> Result<()> {
404 self.start(tag, ContainerKind::Array)
405 }
406
407 pub fn start_list(&mut self, tag: Tag) -> Result<()> {
409 self.start(tag, ContainerKind::List)
410 }
411
412 pub fn end_container(&mut self) -> Result<()> {
414 let Some(new_depth) = self.depth.checked_sub(1) else {
415 bail!(TlvContainerMismatch)
416 };
417 let slot = self.room(1)?;
418 let Some(b) = slot.first_mut() else {
419 bail!(BufferTooSmall)
420 };
421 *b = 0x18;
422 self.advance(1)?;
423 self.depth = new_depth;
424 if self.depth == 0 {
425 self.top_level_done = true;
426 }
427 Ok(())
428 }
429
430 pub fn raw_element_retagged(&mut self, bytes: &[u8], tag: Tag) -> Result<()> {
442 use super::reader::TlvReader;
443 let mut r = TlvReader::new_in(bytes, ContainerKind::List);
446 let Some(first) = r.next_element()? else {
447 bail!(InvalidArgument)
448 };
449 if first.value.container().is_some() {
450 r.skip_container()?;
451 }
452 r.finish()?;
453
454 let Some(&control) = bytes.first() else {
455 bail!(InvalidArgument)
456 };
457 let old_tag_len = first.tag.encoded_len();
460 let value_start = old_tag_len
461 .checked_add(1)
462 .ok_or(Error::new(ErrorCode::InvalidArgument))?;
463 let Some(value) = bytes.get(value_start..) else {
464 bail!(InvalidArgument)
465 };
466
467 self.check_tag(tag)?;
468 let new_tag_len = tag.encoded_len();
469 let head = new_tag_len
470 .checked_add(1)
471 .ok_or(Error::new(ErrorCode::BufferTooSmall))?;
472 let total = head
473 .checked_add(value.len())
474 .ok_or(Error::new(ErrorCode::BufferTooSmall))?;
475 let slot = self.room(total)?;
476 let Some(first_byte) = slot.first_mut() else {
477 bail!(BufferTooSmall)
478 };
479 *first_byte = tag.control_bits() | (control & 0x1F);
480 let Some(rest) = slot.get_mut(1..) else {
481 bail!(BufferTooSmall)
482 };
483 tag.encode(rest)?;
484 let Some(dst) = slot.get_mut(head..total) else {
485 bail!(BufferTooSmall)
486 };
487 dst.copy_from_slice(value);
488 self.advance(total)?;
489 if self.depth == 0 {
490 self.top_level_done = true;
491 }
492 Ok(())
493 }
494
495 pub fn raw_element(&mut self, bytes: &[u8]) -> Result<()> {
506 use super::reader::TlvReader;
507 let mut r = match self.current_container() {
508 Some(kind) => TlvReader::new_in(bytes, kind),
509 None => TlvReader::new(bytes),
510 };
511 let Some(first) = r.next_element()? else {
512 bail!(InvalidArgument)
513 };
514 if first.value.container().is_some() {
515 r.skip_container()?;
516 }
517 r.finish()?;
518 self.check_tag(first.tag)?;
519 let dst = self.room(bytes.len())?;
520 let Some(dst) = dst.get_mut(..bytes.len()) else {
521 bail!(BufferTooSmall)
522 };
523 dst.copy_from_slice(bytes);
524 self.advance(bytes.len())?;
525 if self.depth == 0 {
526 self.top_level_done = true;
527 }
528 Ok(())
529 }
530}
531
532#[cfg(test)]
533#[allow(clippy::arithmetic_side_effects, clippy::indexing_slicing)]
534mod tests {
535 use super::*;
536 use crate::tlv::TlvReader;
537
538 fn write_with(f: impl FnOnce(&mut TlvWriter<'_>) -> Result<()>) -> heapless::Vec<u8, 256> {
539 let mut buf = [0u8; 256];
540 let mut w = TlvWriter::new(&mut buf);
541 f(&mut w).expect("write");
542 let out = w.finish().expect("finish");
543 heapless::Vec::from_slice(out).expect("fits")
544 }
545
546 #[test]
547 fn primitives_match_table_127() {
548 assert_eq!(&write_with(|w| w.bool(Tag::Anonymous, false))[..], &[0x08]);
549 assert_eq!(&write_with(|w| w.bool(Tag::Anonymous, true))[..], &[0x09]);
550 assert_eq!(
551 &write_with(|w| w.signed(Tag::Anonymous, 42))[..],
552 &[0x00, 0x2a]
553 );
554 assert_eq!(
555 &write_with(|w| w.signed(Tag::Anonymous, -17))[..],
556 &[0x00, 0xef]
557 );
558 assert_eq!(
559 &write_with(|w| w.unsigned(Tag::Anonymous, 42))[..],
560 &[0x04, 0x2a]
561 );
562 assert_eq!(
563 &write_with(|w| w.signed(Tag::Anonymous, -170_000))[..],
564 &[0x02, 0xf0, 0x67, 0xfd, 0xff]
565 );
566 assert_eq!(
567 &write_with(|w| w.signed(Tag::Anonymous, 40_000_000_000))[..],
568 &[0x03, 0x00, 0x90, 0x2f, 0x50, 0x09, 0x00, 0x00, 0x00]
569 );
570 assert_eq!(
571 &write_with(|w| w.utf8(Tag::Anonymous, "Hello!"))[..],
572 &[0x0c, 0x06, 0x48, 0x65, 0x6c, 0x6c, 0x6f, 0x21]
573 );
574 assert_eq!(
575 &write_with(|w| w.utf8(Tag::Anonymous, "Tschüs"))[..],
576 &[0x0c, 0x07, 0x54, 0x73, 0x63, 0x68, 0xc3, 0xbc, 0x73]
577 );
578 assert_eq!(
579 &write_with(|w| w.octets(Tag::Anonymous, &[0, 1, 2, 3, 4]))[..],
580 &[0x10, 0x05, 0x00, 0x01, 0x02, 0x03, 0x04]
581 );
582 assert_eq!(&write_with(|w| w.null(Tag::Anonymous))[..], &[0x14]);
583 assert_eq!(
584 &write_with(|w| w.float(Tag::Anonymous, 17.9))[..],
585 &[0x0a, 0x33, 0x33, 0x8f, 0x41]
586 );
587 assert_eq!(
588 &write_with(|w| w.double(Tag::Anonymous, 17.9))[..],
589 &[0x0b, 0x66, 0x66, 0x66, 0x66, 0x66, 0xe6, 0x31, 0x40]
590 );
591 assert_eq!(
592 &write_with(|w| w.float(Tag::Anonymous, f32::NEG_INFINITY))[..],
593 &[0x0a, 0x00, 0x00, 0x80, 0xff]
594 );
595 }
596
597 #[test]
598 fn profile_tags_match_table_129() {
599 assert_eq!(
600 &write_with(|w| w.unsigned(Tag::Common(1), 42))[..],
601 &[0x44, 0x01, 0x00, 0x2a]
602 );
603 assert_eq!(
604 &write_with(|w| w.unsigned(Tag::Common(100_000), 42))[..],
605 &[0x64, 0xa0, 0x86, 0x01, 0x00, 0x2a]
606 );
607 assert_eq!(
608 &write_with(|w| w.unsigned(
609 Tag::FullyQualified {
610 vendor: 0xFFF1,
611 profile: 0xDEED,
612 number: 1
613 },
614 42
615 ))[..],
616 &[0xc4, 0xf1, 0xff, 0xed, 0xde, 0x01, 0x00, 0x2a]
617 );
618 assert_eq!(
619 &write_with(|w| w.unsigned(
620 Tag::FullyQualified {
621 vendor: 0xFFF1,
622 profile: 0xDEED,
623 number: 0xAA55_FEED
624 },
625 42
626 ))[..],
627 &[0xe4, 0xf1, 0xff, 0xed, 0xde, 0xed, 0xfe, 0x55, 0xaa, 0x2a]
628 );
629 }
630
631 #[test]
632 fn containers_match_table_128() {
633 assert_eq!(
634 &write_with(|w| {
635 w.start_structure(Tag::Anonymous)?;
636 w.end_container()
637 })[..],
638 &[0x15, 0x18]
639 );
640 assert_eq!(
641 &write_with(|w| {
642 w.start_structure(Tag::Anonymous)?;
643 w.signed(Tag::Context(0), 42)?;
644 w.signed(Tag::Context(1), -17)?;
645 w.end_container()
646 })[..],
647 &[0x15, 0x20, 0x00, 0x2a, 0x20, 0x01, 0xef, 0x18]
648 );
649 assert_eq!(
650 &write_with(|w| {
651 w.start_array(Tag::Anonymous)?;
652 for v in 0..5 {
653 w.signed(Tag::Anonymous, v)?;
654 }
655 w.end_container()
656 })[..],
657 &[
658 0x16, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x04, 0x18
659 ]
660 );
661 assert_eq!(
662 &write_with(|w| {
663 w.start_list(Tag::Anonymous)?;
664 w.signed(Tag::Anonymous, 1)?;
665 w.signed(Tag::Context(0), 42)?;
666 w.signed(Tag::Anonymous, 2)?;
667 w.signed(Tag::Anonymous, 3)?;
668 w.signed(Tag::Context(0), -17)?;
669 w.end_container()
670 })[..],
671 &[
672 0x17, 0x00, 0x01, 0x20, 0x00, 0x2a, 0x00, 0x02, 0x00, 0x03, 0x20, 0x00, 0xef, 0x18
673 ]
674 );
675 assert_eq!(
677 &write_with(|w| {
678 w.start_structure(Tag::FullyQualified {
679 vendor: 0xFFF1,
680 profile: 0xDEED,
681 number: 1,
682 })?;
683 w.unsigned(
684 Tag::FullyQualified {
685 vendor: 0xFFF1,
686 profile: 0xDEED,
687 number: 0xAA55,
688 },
689 42,
690 )?;
691 w.end_container()
692 })[..],
693 &[
694 0xd5, 0xf1, 0xff, 0xed, 0xde, 0x01, 0x00, 0xc4, 0xf1, 0xff, 0xed, 0xde, 0x55, 0xaa,
695 0x2a, 0x18
696 ]
697 );
698 }
699
700 #[test]
701 fn the_writer_refuses_to_emit_invalid_tlv() {
702 let mut buf = [0u8; 64];
703
704 let mut w = TlvWriter::new(&mut buf);
705 w.start_structure(Tag::Anonymous).unwrap();
706 assert_eq!(
707 w.unsigned(Tag::Anonymous, 1).unwrap_err().code(),
708 ErrorCode::TlvInvalidTag,
709 "anonymous member of a structure"
710 );
711
712 let mut w = TlvWriter::new(&mut buf);
713 w.start_array(Tag::Anonymous).unwrap();
714 assert_eq!(
715 w.unsigned(Tag::Context(1), 1).unwrap_err().code(),
716 ErrorCode::TlvInvalidTag,
717 "tagged member of an array"
718 );
719
720 let mut w = TlvWriter::new(&mut buf);
721 assert_eq!(
722 w.unsigned(Tag::Context(1), 1).unwrap_err().code(),
723 ErrorCode::TlvInvalidTag,
724 "context tag at the top level"
725 );
726
727 let mut w = TlvWriter::new(&mut buf);
728 w.unsigned(Tag::Anonymous, 1).unwrap();
729 assert_eq!(
730 w.unsigned(Tag::Anonymous, 2).unwrap_err().code(),
731 ErrorCode::InvalidState,
732 "second top-level element"
733 );
734
735 let mut w = TlvWriter::new(&mut buf);
736 w.start_structure(Tag::Anonymous).unwrap();
737 assert_eq!(
738 w.finish().unwrap_err().code(),
739 ErrorCode::TlvContainerMismatch,
740 "unclosed container"
741 );
742 }
743
744 #[test]
745 fn a_full_buffer_is_an_error_not_a_panic() {
746 let mut buf = [0u8; 3];
747 let mut w = TlvWriter::new(&mut buf);
748 assert_eq!(
749 w.utf8(Tag::Anonymous, "Hello!").unwrap_err().code(),
750 ErrorCode::BufferTooSmall
751 );
752 }
753
754 #[test]
755 fn everything_written_reads_back() {
756 let out = write_with(|w| {
757 w.start_structure(Tag::Anonymous)?;
758 w.unsigned(Tag::Context(0), 1)?;
759 w.utf8(Tag::Context(1), "matter")?;
760 w.start_array(Tag::Context(2))?;
761 w.unsigned(Tag::Anonymous, 7)?;
762 w.end_container()?;
763 w.end_container()
764 });
765 assert!(TlvReader::validate(&out).is_ok());
766 }
767
768 #[test]
769 fn raw_element_checks_what_it_copies() {
770 let mut buf = [0u8; 64];
771 let mut w = TlvWriter::new(&mut buf);
772 w.start_structure(Tag::Anonymous).unwrap();
773 w.raw_element(&[0x24, 0x01, 0x2a]).unwrap();
775 assert!(w.raw_element(&[0x15]).is_err());
777 assert_eq!(
779 w.raw_element(&[0x04, 0x2a]).unwrap_err().code(),
780 ErrorCode::TlvInvalidTag
781 );
782 w.end_container().unwrap();
783 let out = w.finish().unwrap();
784 assert_eq!(out, &[0x15, 0x24, 0x01, 0x2a, 0x18]);
785 }
786}