1use crate::{
58 cdr::{CdrReader, CdrWriter},
59 error::{DeserError, SerError},
60 schema::{Field, FieldType, Message},
61};
62
63#[derive(Debug, Clone, Copy, PartialEq, Eq)]
70pub enum SchemaError {
71 CdrRead(DeserError),
73 CdrWrite(SerError),
75 BufferTooSmall,
77 Truncated,
79 Malformed,
82 Unsupported(&'static str),
87}
88
89impl From<DeserError> for SchemaError {
90 fn from(e: DeserError) -> Self {
91 SchemaError::CdrRead(e)
92 }
93}
94
95impl From<SerError> for SchemaError {
96 fn from(e: SerError) -> Self {
97 SchemaError::CdrWrite(e)
98 }
99}
100
101impl core::fmt::Display for SchemaError {
102 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
103 match self {
104 SchemaError::CdrRead(e) => write!(f, "cdr read: {e}"),
105 SchemaError::CdrWrite(e) => write!(f, "cdr write: {e}"),
106 SchemaError::BufferTooSmall => write!(f, "output buffer too small"),
107 SchemaError::Truncated => write!(f, "input ended mid-value"),
108 SchemaError::Malformed => write!(f, "input disagrees with the schema"),
109 SchemaError::Unsupported(why) => write!(f, "unsupported schema shape: {why}"),
110 }
111 }
112}
113
114pub trait SchemaSink {
122 fn struct_begin(&mut self, type_name: &str) -> Result<(), SchemaError> {
124 let _ = type_name;
125 Ok(())
126 }
127 fn struct_end(&mut self) -> Result<(), SchemaError> {
129 Ok(())
130 }
131 fn field_begin(&mut self, field: &'static Field) -> Result<(), SchemaError> {
134 let _ = field;
135 Ok(())
136 }
137 fn field_end(&mut self, field: &'static Field) -> Result<(), SchemaError> {
139 let _ = field;
140 Ok(())
141 }
142 fn array_begin(&mut self, len: usize) -> Result<(), SchemaError> {
146 let _ = len;
147 Ok(())
148 }
149 fn array_end(&mut self) -> Result<(), SchemaError> {
151 Ok(())
152 }
153 fn seq_begin(&mut self, len: usize) -> Result<(), SchemaError>;
156 fn seq_end(&mut self) -> Result<(), SchemaError> {
158 Ok(())
159 }
160
161 fn put_bool(&mut self, v: bool) -> Result<(), SchemaError>;
163 fn put_u8(&mut self, v: u8) -> Result<(), SchemaError>;
165 fn put_i8(&mut self, v: i8) -> Result<(), SchemaError>;
167 fn put_u16(&mut self, v: u16) -> Result<(), SchemaError>;
169 fn put_i16(&mut self, v: i16) -> Result<(), SchemaError>;
171 fn put_u32(&mut self, v: u32) -> Result<(), SchemaError>;
173 fn put_i32(&mut self, v: i32) -> Result<(), SchemaError>;
175 fn put_u64(&mut self, v: u64) -> Result<(), SchemaError>;
177 fn put_i64(&mut self, v: i64) -> Result<(), SchemaError>;
179 fn put_f32(&mut self, v: f32) -> Result<(), SchemaError>;
181 fn put_f64(&mut self, v: f64) -> Result<(), SchemaError>;
183 fn put_str(&mut self, v: &str) -> Result<(), SchemaError>;
186}
187
188pub trait SchemaSource {
193 fn struct_begin(&mut self, type_name: &str) -> Result<(), SchemaError> {
195 let _ = type_name;
196 Ok(())
197 }
198 fn struct_end(&mut self) -> Result<(), SchemaError> {
200 Ok(())
201 }
202 fn field_begin(&mut self, field: &'static Field) -> Result<(), SchemaError> {
204 let _ = field;
205 Ok(())
206 }
207 fn field_end(&mut self, field: &'static Field) -> Result<(), SchemaError> {
209 let _ = field;
210 Ok(())
211 }
212 fn array_begin(&mut self, len: usize) -> Result<(), SchemaError> {
214 let _ = len;
215 Ok(())
216 }
217 fn array_end(&mut self) -> Result<(), SchemaError> {
219 Ok(())
220 }
221 fn seq_begin(&mut self) -> Result<usize, SchemaError>;
223 fn seq_end(&mut self) -> Result<(), SchemaError> {
225 Ok(())
226 }
227
228 fn take_bool(&mut self) -> Result<bool, SchemaError>;
230 fn take_u8(&mut self) -> Result<u8, SchemaError>;
232 fn take_i8(&mut self) -> Result<i8, SchemaError>;
234 fn take_u16(&mut self) -> Result<u16, SchemaError>;
236 fn take_i16(&mut self) -> Result<i16, SchemaError>;
238 fn take_u32(&mut self) -> Result<u32, SchemaError>;
240 fn take_i32(&mut self) -> Result<i32, SchemaError>;
242 fn take_u64(&mut self) -> Result<u64, SchemaError>;
244 fn take_i64(&mut self) -> Result<i64, SchemaError>;
246 fn take_f32(&mut self) -> Result<f32, SchemaError>;
248 fn take_f64(&mut self) -> Result<f64, SchemaError>;
250 fn take_str(&mut self) -> Result<&str, SchemaError>;
252}
253
254pub fn encode_from_cdr<S: SchemaSink + ?Sized>(
261 reader: &mut CdrReader<'_>,
262 type_name: &str,
263 schema: &'static [Field],
264 sink: &mut S,
265) -> Result<(), SchemaError> {
266 let scope = reader.begin_dheader()?;
267 sink.struct_begin(type_name)?;
268 for field in schema {
269 sink.field_begin(field)?;
270 encode_one(reader, &field.ty, sink)?;
271 sink.field_end(field)?;
272 }
273 sink.struct_end()?;
274 reader.end_dheader(scope)?;
275 Ok(())
276}
277
278fn encode_one<S: SchemaSink + ?Sized>(
279 r: &mut CdrReader<'_>,
280 ty: &'static FieldType,
281 sink: &mut S,
282) -> Result<(), SchemaError> {
283 match ty {
284 FieldType::Bool => sink.put_bool(r.read_bool()?),
285 FieldType::Uint8 => sink.put_u8(r.read_u8()?),
286 FieldType::Int8 => sink.put_i8(r.read_i8()?),
287 FieldType::Uint16 => sink.put_u16(r.read_u16()?),
288 FieldType::Int16 => sink.put_i16(r.read_i16()?),
289 FieldType::Uint32 => sink.put_u32(r.read_u32()?),
290 FieldType::Int32 => sink.put_i32(r.read_i32()?),
291 FieldType::Uint64 => sink.put_u64(r.read_u64()?),
292 FieldType::Int64 => sink.put_i64(r.read_i64()?),
293 FieldType::Float32 => sink.put_f32(r.read_f32()?),
294 FieldType::Float64 => sink.put_f64(r.read_f64()?),
295 FieldType::String => sink.put_str(r.read_string()?),
296 FieldType::BoundedString(n) => {
297 let s = r.read_string()?;
298 if s.len() > *n {
301 return Err(SchemaError::Malformed);
302 }
303 sink.put_str(s)
304 }
305 FieldType::WString | FieldType::BoundedWString(_) => Err(SchemaError::Unsupported(
306 "wstring: CdrReader has no wide-string primitive to transcode from",
307 )),
308 FieldType::Nested(nested) => encode_from_cdr(r, nested.type_name, nested.fields, sink),
309 FieldType::Array(n, inner) => {
310 sink.array_begin(*n)?;
311 for _ in 0..*n {
312 encode_one(r, inner, sink)?;
313 }
314 sink.array_end()
315 }
316 FieldType::Sequence(inner) => {
317 let n = r.read_sequence_len()?;
318 sink.seq_begin(n)?;
319 for _ in 0..n {
320 encode_one(r, inner, sink)?;
321 }
322 sink.seq_end()
323 }
324 FieldType::BoundedSequence(cap, inner) => {
325 let n = r.read_sequence_len()?;
326 if n > *cap {
327 return Err(SchemaError::Malformed);
328 }
329 sink.seq_begin(n)?;
330 for _ in 0..n {
331 encode_one(r, inner, sink)?;
332 }
333 sink.seq_end()
334 }
335 }
336}
337
338pub fn decode_to_cdr<S: SchemaSource + ?Sized>(
343 source: &mut S,
344 type_name: &str,
345 schema: &'static [Field],
346 writer: &mut CdrWriter<'_>,
347) -> Result<(), SchemaError> {
348 let mark = writer.begin_dheader()?;
349 source.struct_begin(type_name)?;
350 for field in schema {
351 source.field_begin(field)?;
352 decode_one(source, &field.ty, writer)?;
353 source.field_end(field)?;
354 }
355 source.struct_end()?;
356 writer.end_dheader(mark)?;
357 Ok(())
358}
359
360fn decode_one<S: SchemaSource + ?Sized>(
361 source: &mut S,
362 ty: &'static FieldType,
363 w: &mut CdrWriter<'_>,
364) -> Result<(), SchemaError> {
365 match ty {
366 FieldType::Bool => w.write_bool(source.take_bool()?)?,
367 FieldType::Uint8 => w.write_u8(source.take_u8()?)?,
368 FieldType::Int8 => w.write_i8(source.take_i8()?)?,
369 FieldType::Uint16 => w.write_u16(source.take_u16()?)?,
370 FieldType::Int16 => w.write_i16(source.take_i16()?)?,
371 FieldType::Uint32 => w.write_u32(source.take_u32()?)?,
372 FieldType::Int32 => w.write_i32(source.take_i32()?)?,
373 FieldType::Uint64 => w.write_u64(source.take_u64()?)?,
374 FieldType::Int64 => w.write_i64(source.take_i64()?)?,
375 FieldType::Float32 => w.write_f32(source.take_f32()?)?,
376 FieldType::Float64 => w.write_f64(source.take_f64()?)?,
377 FieldType::String => {
378 let s = source.take_str()?;
379 w.write_string(s)?;
380 }
381 FieldType::BoundedString(n) => {
382 let s = source.take_str()?;
383 if s.len() > *n {
384 return Err(SchemaError::Malformed);
385 }
386 w.write_string(s)?;
387 }
388 FieldType::WString | FieldType::BoundedWString(_) => {
389 return Err(SchemaError::Unsupported(
390 "wstring: CdrWriter has no wide-string primitive to transcode into",
391 ));
392 }
393 FieldType::Nested(nested) => {
394 decode_to_cdr(source, nested.type_name, nested.fields, w)?;
395 }
396 FieldType::Array(n, inner) => {
397 source.array_begin(*n)?;
398 for _ in 0..*n {
399 decode_one(source, inner, w)?;
400 }
401 source.array_end()?;
402 }
403 FieldType::Sequence(inner) => {
404 let n = source.seq_begin()?;
405 w.write_sequence_len(n)?;
406 for _ in 0..n {
407 decode_one(source, inner, w)?;
408 }
409 source.seq_end()?;
410 }
411 FieldType::BoundedSequence(cap, inner) => {
412 let n = source.seq_begin()?;
413 if n > *cap {
414 return Err(SchemaError::Malformed);
415 }
416 w.write_sequence_len(n)?;
417 for _ in 0..n {
418 decode_one(source, inner, w)?;
419 }
420 source.seq_end()?;
421 }
422 }
423 Ok(())
424}
425
426pub trait SchemaSerializer {
443 const FORMAT_NAME: &'static str;
446
447 const FORMAT_ID: u8;
453
454 fn serialize(
456 msg: &mut CdrReader<'_>,
457 type_name: &str,
458 schema: &'static [Field],
459 out: &mut [u8],
460 ) -> Result<usize, SchemaError>;
461
462 fn deserialize(
464 bytes: &[u8],
465 type_name: &str,
466 schema: &'static [Field],
467 msg: &mut CdrWriter<'_>,
468 ) -> Result<usize, SchemaError>;
469
470 fn serialize_message<M: Message>(
472 msg: &mut CdrReader<'_>,
473 out: &mut [u8],
474 ) -> Result<usize, SchemaError>
475 where
476 Self: Sized,
477 {
478 Self::serialize(msg, M::TYPE_NAME, M::FIELDS, out)
479 }
480
481 fn deserialize_message<M: Message>(
483 bytes: &[u8],
484 msg: &mut CdrWriter<'_>,
485 ) -> Result<usize, SchemaError>
486 where
487 Self: Sized,
488 {
489 Self::deserialize(bytes, M::TYPE_NAME, M::FIELDS, msg)
490 }
491}
492
493#[cfg(test)]
494mod tests {
495 use super::*;
496 use crate::schema::NestedType;
497
498 #[derive(Default)]
502 struct Trace {
503 out: heapless::String<512>,
504 }
505
506 impl Trace {
507 fn push(&mut self, s: &str) -> Result<(), SchemaError> {
508 self.out
509 .push_str(s)
510 .map_err(|_| SchemaError::BufferTooSmall)
511 }
512 }
513
514 impl SchemaSink for Trace {
515 fn struct_begin(&mut self, type_name: &str) -> Result<(), SchemaError> {
516 self.push("{")?;
517 self.push(type_name)
518 }
519 fn struct_end(&mut self) -> Result<(), SchemaError> {
520 self.push("}")
521 }
522 fn field_begin(&mut self, field: &'static Field) -> Result<(), SchemaError> {
523 self.push(" ")?;
524 self.push(field.name)?;
525 self.push("=")
526 }
527 fn seq_begin(&mut self, len: usize) -> Result<(), SchemaError> {
528 self.push(if len == 0 { "[0" } else { "[n" })
529 }
530 fn seq_end(&mut self) -> Result<(), SchemaError> {
531 self.push("]")
532 }
533 fn put_bool(&mut self, _: bool) -> Result<(), SchemaError> {
534 self.push("b")
535 }
536 fn put_u8(&mut self, _: u8) -> Result<(), SchemaError> {
537 self.push("u8")
538 }
539 fn put_i8(&mut self, _: i8) -> Result<(), SchemaError> {
540 self.push("i8")
541 }
542 fn put_u16(&mut self, _: u16) -> Result<(), SchemaError> {
543 self.push("u16")
544 }
545 fn put_i16(&mut self, _: i16) -> Result<(), SchemaError> {
546 self.push("i16")
547 }
548 fn put_u32(&mut self, _: u32) -> Result<(), SchemaError> {
549 self.push("u32")
550 }
551 fn put_i32(&mut self, _: i32) -> Result<(), SchemaError> {
552 self.push("i32")
553 }
554 fn put_u64(&mut self, _: u64) -> Result<(), SchemaError> {
555 self.push("u64")
556 }
557 fn put_i64(&mut self, _: i64) -> Result<(), SchemaError> {
558 self.push("i64")
559 }
560 fn put_f32(&mut self, _: f32) -> Result<(), SchemaError> {
561 self.push("f32")
562 }
563 fn put_f64(&mut self, _: f64) -> Result<(), SchemaError> {
564 self.push("f64")
565 }
566 fn put_str(&mut self, v: &str) -> Result<(), SchemaError> {
567 self.push("\"")?;
568 self.push(v)?;
569 self.push("\"")
570 }
571 }
572
573 const TIME_FIELDS: &[Field] = &[
574 Field {
575 name: "sec",
576 ty: FieldType::Int32,
577 offset: 0,
578 },
579 Field {
580 name: "nanosec",
581 ty: FieldType::Uint32,
582 offset: 4,
583 },
584 ];
585 const TIME: NestedType = NestedType {
586 type_name: "builtin_interfaces/msg/Time",
587 fields: TIME_FIELDS,
588 };
589 const HEADER_FIELDS: &[Field] = &[
590 Field {
591 name: "stamp",
592 ty: FieldType::Nested(&TIME),
593 offset: 0,
594 },
595 Field {
596 name: "frame_id",
597 ty: FieldType::String,
598 offset: 8,
599 },
600 ];
601
602 fn header_cdr(buf: &mut [u8]) -> usize {
603 let mut w = CdrWriter::new(buf);
604 let dh = w.begin_dheader().unwrap();
605 let inner = w.begin_dheader().unwrap();
606 w.write_i32(7).unwrap();
607 w.write_u32(8).unwrap();
608 w.end_dheader(inner).unwrap();
609 w.write_string("map").unwrap();
610 w.end_dheader(dh).unwrap();
611 w.position()
612 }
613
614 #[test]
615 fn the_walk_visits_nested_structs_in_declaration_order() {
616 let mut buf = [0u8; 64];
617 let len = header_cdr(&mut buf);
618 let mut r = CdrReader::new(&buf[..len]);
619 let mut trace = Trace::default();
620 encode_from_cdr(&mut r, "std_msgs/msg/Header", HEADER_FIELDS, &mut trace).unwrap();
621 assert_eq!(
622 trace.out.as_str(),
623 "{std_msgs/msg/Header stamp={builtin_interfaces/msg/Time sec=i32 nanosec=u32} \
624 frame_id=\"map\"}"
625 );
626 }
627
628 #[test]
629 fn a_wstring_is_refused_by_name_rather_than_skipped() {
630 const WIDE: &[Field] = &[Field {
631 name: "text",
632 ty: FieldType::WString,
633 offset: 0,
634 }];
635 let buf = [0u8; 16];
636 let mut r = CdrReader::new(&buf);
637 let mut trace = Trace::default();
638 let err = encode_from_cdr(&mut r, "t/msg/W", WIDE, &mut trace).unwrap_err();
639 match err {
640 SchemaError::Unsupported(why) => assert!(why.contains("wstring"), "got {why:?}"),
641 other => panic!("expected Unsupported, got {other:?}"),
642 }
643 }
644
645 #[test]
646 fn a_bounded_sequence_past_its_bound_is_malformed_not_accepted() {
647 const ELEM: FieldType = FieldType::Uint8;
648 const FIELDS: &[Field] = &[Field {
649 name: "data",
650 ty: FieldType::BoundedSequence(2, &ELEM),
651 offset: 0,
652 }];
653 let mut buf = [0u8; 32];
654 let len = {
655 let mut w = CdrWriter::new(&mut buf);
656 let dh = w.begin_dheader().unwrap();
657 w.write_sequence_len(5).unwrap();
658 for _ in 0..5 {
659 w.write_u8(1).unwrap();
660 }
661 w.end_dheader(dh).unwrap();
662 w.position()
663 };
664 let mut r = CdrReader::new(&buf[..len]);
665 let mut trace = Trace::default();
666 assert_eq!(
667 encode_from_cdr(&mut r, "t/msg/B", FIELDS, &mut trace),
668 Err(SchemaError::Malformed)
669 );
670 }
671}