zerodds_rtps/
datagram.rs

1// SPDX-License-Identifier: Apache-2.0
2// Copyright 2026 ZeroDDS Contributors
3//! Datagram encoder/decoder: combines the RTPS header and submessages
4//! into a finished wire datagram (W4).
5
6extern crate alloc;
7use alloc::vec::Vec;
8
9use crate::error::WireError;
10use crate::header::RtpsHeader;
11use crate::header_extension::{HeaderExtension, SUBMESSAGE_ID_HEADER_EXTENSION};
12use crate::submessage_header::{FLAG_E_LITTLE_ENDIAN, SubmessageHeader, SubmessageId};
13use crate::submessages::{
14    ACKNACK_FLAG_FINAL, AckNackSubmessage, DATA_FRAG_FLAG_HASH_KEY, DATA_FRAG_FLAG_INLINE_QOS,
15    DATA_FRAG_FLAG_KEY, DATA_FRAG_FLAG_NON_STANDARD, DataFragSubmessage, DataSubmessage,
16    GAP_FLAG_FILTERED_COUNT, GAP_FLAG_GROUP_INFO, GapSubmessage, HEARTBEAT_FLAG_FINAL,
17    HEARTBEAT_FLAG_GROUP_INFO, HEARTBEAT_FLAG_LIVELINESS, HeartbeatFragSubmessage,
18    HeartbeatSubmessage, INFO_REPLY_FLAG_MULTICAST, INFO_TIMESTAMP_FLAG_INVALIDATE,
19    InfoReplySubmessage, InfoSourceSubmessage, InfoTimestampSubmessage, NackFragSubmessage,
20};
21
22/// Encodes an RTPS datagram = `RtpsHeader` + a sequence of `DATA`
23/// submessages. Variant: all submessages are LE; a datagram carries a
24/// list of DATA bodies.
25pub fn encode_data_datagram(
26    header: RtpsHeader,
27    data_submessages: &[DataSubmessage],
28) -> Result<Vec<u8>, WireError> {
29    let mut out = Vec::new();
30    out.extend_from_slice(&header.to_bytes());
31    for d in data_submessages {
32        let (body, flags) = d.write_body(true);
33        let body_len = u16::try_from(body.len()).map_err(|_| WireError::ValueOutOfRange {
34            message: "DATA submessage body exceeds u16::MAX",
35        })?;
36        let sh = SubmessageHeader {
37            submessage_id: SubmessageId::Data,
38            flags,
39            octets_to_next_header: body_len,
40        };
41        out.extend_from_slice(&sh.to_bytes());
42        out.extend_from_slice(&body);
43    }
44    Ok(out)
45}
46
47/// Parsed datagram: header + all recognized submessages.
48#[derive(Debug, Clone, PartialEq, Eq)]
49pub struct ParsedDatagram {
50    /// RTPS header.
51    pub header: RtpsHeader,
52    /// All recognized submessages in order.
53    pub submessages: Vec<ParsedSubmessage>,
54}
55
56/// A recognized submessage. Supports DATA/HEARTBEAT/ACKNACK/GAP/DATA_FRAG/HEARTBEAT_FRAG/NACK_FRAG/INFO_*; others are skipped via `octets_to_next_header`
57/// and recorded as [`ParsedSubmessage::Unknown`].
58#[derive(Debug, Clone, PartialEq, Eq)]
59pub enum ParsedSubmessage {
60    /// DATA submessage.
61    Data(DataSubmessage),
62    /// DATA_FRAG submessage (fragmentation).
63    DataFrag(DataFragSubmessage),
64    /// HEARTBEAT submessage.
65    Heartbeat(HeartbeatSubmessage),
66    /// HEARTBEAT_FRAG submessage.
67    HeartbeatFrag(HeartbeatFragSubmessage),
68    /// ACKNACK submessage.
69    AckNack(AckNackSubmessage),
70    /// NACK_FRAG submessage.
71    NackFrag(NackFragSubmessage),
72    /// GAP submessage.
73    Gap(GapSubmessage),
74    /// HeaderExtension-Submessage (DDSI-RTPS 2.5 §8.3.3.2).
75    HeaderExtension(HeaderExtension),
76    /// InfoSource-Submessage (§8.3.8.9.4).
77    InfoSource(InfoSourceSubmessage),
78    /// InfoReply-Submessage (§8.3.8.10.4).
79    InfoReply(InfoReplySubmessage),
80    /// InfoTimestamp-Submessage (§8.3.8.5 / §8.3.7.5).
81    InfoTimestamp(InfoTimestampSubmessage),
82    /// Another submessage class (skipped). Carries id + flags for
83    /// diagnostics.
84    Unknown {
85        /// Submessage ID byte.
86        id: u8,
87        /// Flag byte.
88        flags: u8,
89    },
90}
91
92/// Submessage-header must-understand bit (bit 7 of the flag byte,
93/// DDSI-RTPS 2.5 §8.3.3.2). On an unknown submessage ID + set bit, the
94/// whole RTPS message MUST be discarded.
95pub const SUBMESSAGE_FLAG_MUST_UNDERSTAND: u8 = 0x80;
96
97/// Decodes an RTPS datagram into header + submessage list.
98///
99/// `octets_to_next_header == 0` (last-submessage marker, spec §8.3.4.2)
100/// is treated as: the submessage extends to the end of the datagram.
101///
102/// # Errors
103/// `InvalidMagic`, `UnexpectedEof`, or a sub-decoder error. Unknown
104/// submessage IDs are marked as `Unknown` (not an error).
105pub fn decode_datagram(bytes: &[u8]) -> Result<ParsedDatagram, WireError> {
106    let header = RtpsHeader::from_bytes(bytes)?;
107    let mut pos = RtpsHeader::WIRE_SIZE;
108    // Typical RTPS packets carry 1–3 submessages (DATA, DATA+HB,
109    // INFO_TS+DATA+HB). Pre-allocating to 4 avoids the Vec::push
110    // realloc step on the recv-thread hot path.
111    let mut submessages = Vec::with_capacity(4);
112
113    while pos < bytes.len() {
114        if bytes.len() < pos + SubmessageHeader::WIRE_SIZE {
115            return Err(WireError::UnexpectedEof {
116                needed: SubmessageHeader::WIRE_SIZE,
117                offset: pos,
118            });
119        }
120        // We read the submessage header first; on an unknown ID skip
121        // resiliently.
122        let id_byte = bytes[pos];
123        let flags = bytes[pos + 1];
124        let mut len_bytes = [0u8; 2];
125        len_bytes.copy_from_slice(&bytes[pos + 2..pos + 4]);
126        let little_endian = (flags & FLAG_E_LITTLE_ENDIAN) != 0;
127        let octets = if little_endian {
128            u16::from_le_bytes(len_bytes)
129        } else {
130            u16::from_be_bytes(len_bytes)
131        };
132        let body_start = pos + SubmessageHeader::WIRE_SIZE;
133        let body_end = if octets == 0 {
134            // Last-submessage marker: to the end of the datagram.
135            bytes.len()
136        } else {
137            body_start + octets as usize
138        };
139        if body_end > bytes.len() {
140            return Err(WireError::UnexpectedEof {
141                needed: body_end - bytes.len(),
142                offset: body_start,
143            });
144        }
145        let body = &bytes[body_start..body_end];
146        let sub = match SubmessageId::from_u8(id_byte) {
147            Ok(SubmessageId::Data) => {
148                let d = DataSubmessage::read_body_with_flags(body, little_endian, flags)?;
149                if let Some(pl) = &d.inline_qos {
150                    pl.validate_must_understand_in_data_pipeline()?;
151                }
152                ParsedSubmessage::Data(d)
153            }
154            Ok(SubmessageId::Heartbeat) => {
155                let final_flag = (flags & HEARTBEAT_FLAG_FINAL) != 0;
156                let liveliness_flag = (flags & HEARTBEAT_FLAG_LIVELINESS) != 0;
157                let group_info_flag = (flags & HEARTBEAT_FLAG_GROUP_INFO) != 0;
158                ParsedSubmessage::Heartbeat(HeartbeatSubmessage::read_body(
159                    body,
160                    little_endian,
161                    final_flag,
162                    liveliness_flag,
163                    group_info_flag,
164                )?)
165            }
166            Ok(SubmessageId::AckNack) => {
167                let final_flag = (flags & ACKNACK_FLAG_FINAL) != 0;
168                ParsedSubmessage::AckNack(AckNackSubmessage::read_body(
169                    body,
170                    little_endian,
171                    final_flag,
172                )?)
173            }
174            Ok(SubmessageId::Gap) => {
175                let group_info_flag = (flags & GAP_FLAG_GROUP_INFO) != 0;
176                let filtered_count_flag = (flags & GAP_FLAG_FILTERED_COUNT) != 0;
177                ParsedSubmessage::Gap(GapSubmessage::read_body(
178                    body,
179                    little_endian,
180                    group_info_flag,
181                    filtered_count_flag,
182                )?)
183            }
184            Ok(SubmessageId::DataFrag) => {
185                let inline_qos = (flags & DATA_FRAG_FLAG_INLINE_QOS) != 0;
186                let hash_key = (flags & DATA_FRAG_FLAG_HASH_KEY) != 0;
187                let key = (flags & DATA_FRAG_FLAG_KEY) != 0;
188                let non_standard = (flags & DATA_FRAG_FLAG_NON_STANDARD) != 0;
189                ParsedSubmessage::DataFrag(DataFragSubmessage::read_body(
190                    body,
191                    little_endian,
192                    inline_qos,
193                    hash_key,
194                    key,
195                    non_standard,
196                )?)
197            }
198            Ok(SubmessageId::HeartbeatFrag) => ParsedSubmessage::HeartbeatFrag(
199                HeartbeatFragSubmessage::read_body(body, little_endian)?,
200            ),
201            Ok(SubmessageId::NackFrag) => {
202                ParsedSubmessage::NackFrag(NackFragSubmessage::read_body(body, little_endian)?)
203            }
204            Ok(SubmessageId::InfoSrc) => {
205                ParsedSubmessage::InfoSource(InfoSourceSubmessage::read_body(body, little_endian)?)
206            }
207            Ok(SubmessageId::InfoTs) => {
208                let invalidate = (flags & INFO_TIMESTAMP_FLAG_INVALIDATE) != 0;
209                ParsedSubmessage::InfoTimestamp(InfoTimestampSubmessage::read_body(
210                    body,
211                    little_endian,
212                    invalidate,
213                )?)
214            }
215            Ok(SubmessageId::InfoReply) => {
216                let multicast_flag = (flags & INFO_REPLY_FLAG_MULTICAST) != 0;
217                ParsedSubmessage::InfoReply(InfoReplySubmessage::read_body(
218                    body,
219                    little_endian,
220                    multicast_flag,
221                )?)
222            }
223            // HeaderExtension (SubmessageId 0x80, outside the enum
224            // range — we match explicitly via the ID byte). Spec
225            // §8.3.7.3: HE MUST appear directly after the header (i.e.
226            // as the first submessage). Otherwise reject.
227            //
228            // Vendor compat: only RTPS >= 2.5 defines 0x80 as HE. For
229            // older vendors (e.g. Cyclone 2.1, FastDDS 2.x with
230            // protocol_version=2.1) 0x80 falls into the vendor-specific
231            // range [0x80, 0xFF] (spec §8.3.3.2). Such submessages are —
232            // unless the must-understand flag is set — simply marked as
233            // `Unknown` and skipped.
234            Ok(_) | Err(WireError::UnknownSubmessageId { .. })
235                if id_byte == SUBMESSAGE_ID_HEADER_EXTENSION
236                    && header.protocol_version
237                        >= crate::wire_types::ProtocolVersion { major: 2, minor: 5 } =>
238            {
239                if !submessages.is_empty() {
240                    return Err(WireError::ValueOutOfRange {
241                        message: "HeaderExtension must appear directly after the RTPS header",
242                    });
243                }
244                let he = HeaderExtension::decode_body(body, flags)?;
245                if let Some(pl) = &he.parameters {
246                    pl.validate_must_understand_in_data_pipeline()?;
247                }
248                ParsedSubmessage::HeaderExtension(he)
249            }
250            // Unknown submessage ID + must-understand bit:
251            // SPEC §8.3.3.2 / §9.4.5.1 — discard the whole RTPS
252            // message.
253            Ok(_) | Err(WireError::UnknownSubmessageId { .. })
254                if (flags & SUBMESSAGE_FLAG_MUST_UNDERSTAND) != 0 =>
255            {
256                return Err(WireError::ValueOutOfRange {
257                    message: "Unknown submessage id with must-understand flag",
258                });
259            }
260            // Other known submessage IDs without a decoder
261            // (PAD, InfoTs, …): skip and mark as Unknown.
262            Ok(_) | Err(WireError::UnknownSubmessageId { .. }) => {
263                ParsedSubmessage::Unknown { id: id_byte, flags }
264            }
265            Err(other) => return Err(other),
266        };
267        submessages.push(sub);
268        pos = body_end;
269    }
270
271    Ok(ParsedDatagram {
272        header,
273        submessages,
274    })
275}
276
277#[cfg(test)]
278mod tests {
279    #![allow(clippy::expect_used, clippy::unwrap_used, clippy::panic)]
280    use super::*;
281    use crate::wire_types::{EntityId, GuidPrefix, SequenceNumber, VendorId};
282    use alloc::vec;
283
284    fn header() -> RtpsHeader {
285        RtpsHeader::new(VendorId::ZERODDS, GuidPrefix::from_bytes([1; 12]))
286    }
287
288    fn data_msg(sn: i64, payload: &[u8]) -> DataSubmessage {
289        DataSubmessage {
290            extra_flags: 0,
291            reader_id: EntityId::user_reader_with_key([0xA, 0xB, 0xC]),
292            writer_id: EntityId::user_writer_with_key([0x1, 0x2, 0x3]),
293            writer_sn: SequenceNumber(sn),
294            inline_qos: None,
295            key_flag: false,
296            non_standard_flag: false,
297            serialized_payload: alloc::sync::Arc::from(payload),
298        }
299    }
300
301    #[test]
302    fn encode_decode_single_data_datagram() {
303        let h = header();
304        let d = data_msg(1, b"hello");
305        let bytes = encode_data_datagram(h, &[d.clone()]).unwrap();
306        let parsed = decode_datagram(&bytes).unwrap();
307        assert_eq!(parsed.header, h);
308        assert_eq!(parsed.submessages.len(), 1);
309        match &parsed.submessages[0] {
310            ParsedSubmessage::Data(decoded) => assert_eq!(decoded, &d),
311            other => panic!("expected Data, got {other:?}"),
312        }
313    }
314
315    #[test]
316    fn encode_decode_two_data_submessages() {
317        let h = header();
318        let d1 = data_msg(1, b"first");
319        let d2 = data_msg(2, b"second-payload");
320        let bytes = encode_data_datagram(h, &[d1.clone(), d2.clone()]).unwrap();
321        let parsed = decode_datagram(&bytes).unwrap();
322        assert_eq!(parsed.submessages.len(), 2);
323        match (&parsed.submessages[0], &parsed.submessages[1]) {
324            (ParsedSubmessage::Data(a), ParsedSubmessage::Data(b)) => {
325                assert_eq!(a, &d1);
326                assert_eq!(b, &d2);
327            }
328            other => panic!("unexpected: {other:?}"),
329        }
330    }
331
332    #[test]
333    fn encode_decode_empty_payload() {
334        let h = header();
335        let d = data_msg(42, b"");
336        let bytes = encode_data_datagram(h, &[d.clone()]).unwrap();
337        let parsed = decode_datagram(&bytes).unwrap();
338        assert_eq!(parsed.submessages.len(), 1);
339        match &parsed.submessages[0] {
340            ParsedSubmessage::Data(decoded) => {
341                assert!(decoded.serialized_payload.is_empty());
342                assert_eq!(decoded.writer_sn, SequenceNumber(42));
343            }
344            other => panic!("expected Data, got {other:?}"),
345        }
346    }
347
348    #[test]
349    fn decode_rejects_invalid_magic() {
350        let mut bytes = vec![0u8; 32];
351        bytes[..4].copy_from_slice(b"XXXX");
352        let res = decode_datagram(&bytes);
353        assert!(matches!(res, Err(WireError::InvalidMagic { .. })));
354    }
355
356    #[test]
357    fn decode_handles_last_submessage_zero_length() {
358        // Construct manually: header + DATA-SH with octets=0
359        // Body is DATA format (at least 20 bytes + payload).
360        let h = header();
361        let mut bytes = h.to_bytes().to_vec();
362        let d = data_msg(7, b"X");
363        let (body, flags) = d.write_body(true);
364        // Submessage header with octets=0 (last marker)
365        let sh = SubmessageHeader {
366            submessage_id: SubmessageId::Data,
367            flags,
368            octets_to_next_header: 0,
369        };
370        bytes.extend_from_slice(&sh.to_bytes());
371        bytes.extend_from_slice(&body);
372        let parsed = decode_datagram(&bytes).unwrap();
373        match &parsed.submessages[0] {
374            ParsedSubmessage::Data(decoded) => {
375                assert_eq!(decoded, &d);
376            }
377            other => panic!("expected Data, got {other:?}"),
378        }
379    }
380
381    #[test]
382    fn decode_marks_unknown_submessage_id_without_failing() {
383        // Header + sub-header with ID 0x01 (PAD submessage). We use
384        // PAD as a stand-in, because InfoTs is now typed-decoded (R3).
385        let h = header();
386        let mut bytes = h.to_bytes().to_vec();
387        let body = [0u8; 0]; // PAD has no body
388        let sh = SubmessageHeader {
389            submessage_id: SubmessageId::Pad,
390            flags: FLAG_E_LITTLE_ENDIAN,
391            octets_to_next_header: body.len() as u16,
392        };
393        bytes.extend_from_slice(&sh.to_bytes());
394        bytes.extend_from_slice(&body);
395        let parsed = decode_datagram(&bytes).unwrap();
396        assert_eq!(parsed.submessages.len(), 1);
397        match &parsed.submessages[0] {
398            ParsedSubmessage::Unknown { id, flags } => {
399                assert_eq!(*id, 0x01);
400                assert_eq!(*flags, FLAG_E_LITTLE_ENDIAN);
401            }
402            other => panic!("expected Unknown, got {other:?}"),
403        }
404    }
405
406    #[test]
407    fn decode_heartbeat_preserves_final_and_liveliness_flags() {
408        // Regression for the WP-1.1 finding: the F/L flag from the
409        // submessage header must be available in HeartbeatSubmessage.
410        let h = header();
411        let hb = HeartbeatSubmessage {
412            reader_id: crate::wire_types::EntityId::user_reader_with_key([1, 2, 3]),
413            writer_id: crate::wire_types::EntityId::user_writer_with_key([4, 5, 6]),
414            first_sn: SequenceNumber(1),
415            last_sn: SequenceNumber(7),
416            count: 42,
417            final_flag: true,
418            liveliness_flag: true,
419            group_info: None,
420        };
421        let (body, flags) = hb.write_body(true);
422        let mut bytes = h.to_bytes().to_vec();
423        let sh = SubmessageHeader {
424            submessage_id: SubmessageId::Heartbeat,
425            flags,
426            octets_to_next_header: body.len() as u16,
427        };
428        bytes.extend_from_slice(&sh.to_bytes());
429        bytes.extend_from_slice(&body);
430        let parsed = decode_datagram(&bytes).unwrap();
431        match &parsed.submessages[0] {
432            ParsedSubmessage::Heartbeat(decoded) => {
433                assert_eq!(decoded, &hb);
434                assert!(decoded.final_flag);
435                assert!(decoded.liveliness_flag);
436            }
437            other => panic!("expected Heartbeat, got {other:?}"),
438        }
439    }
440
441    #[test]
442    fn decode_acknack_preserves_final_flag() {
443        let h = header();
444        let ack = AckNackSubmessage {
445            reader_id: crate::wire_types::EntityId::user_reader_with_key([1, 2, 3]),
446            writer_id: crate::wire_types::EntityId::user_writer_with_key([4, 5, 6]),
447            reader_sn_state: crate::submessages::SequenceNumberSet {
448                bitmap_base: SequenceNumber(1),
449                num_bits: 0,
450                bitmap: vec![],
451            },
452            count: 3,
453            final_flag: true,
454        };
455        let (body, flags) = ack.write_body(true);
456        let mut bytes = h.to_bytes().to_vec();
457        let sh = SubmessageHeader {
458            submessage_id: SubmessageId::AckNack,
459            flags,
460            octets_to_next_header: body.len() as u16,
461        };
462        bytes.extend_from_slice(&sh.to_bytes());
463        bytes.extend_from_slice(&body);
464        let parsed = decode_datagram(&bytes).unwrap();
465        match &parsed.submessages[0] {
466            ParsedSubmessage::AckNack(decoded) => {
467                assert_eq!(decoded, &ack);
468                assert!(decoded.final_flag);
469            }
470            other => panic!("expected AckNack, got {other:?}"),
471        }
472    }
473
474    #[test]
475    fn decode_data_frag_preserves_flags_and_payload() {
476        let h = header();
477        let df = DataFragSubmessage {
478            extra_flags: 0,
479            reader_id: crate::wire_types::EntityId::user_reader_with_key([1, 2, 3]),
480            writer_id: crate::wire_types::EntityId::user_writer_with_key([4, 5, 6]),
481            writer_sn: SequenceNumber(7),
482            fragment_starting_num: crate::wire_types::FragmentNumber(1),
483            fragments_in_submessage: 1,
484            fragment_size: 4,
485            sample_size: 12,
486            serialized_payload: alloc::sync::Arc::<[u8]>::from([0xAA, 0xBB, 0xCC, 0xDD].as_slice()),
487            inline_qos_flag: false,
488            hash_key_flag: true,
489            key_flag: false,
490            non_standard_flag: false,
491        };
492        let (body, flags) = df.write_body(true);
493        let mut bytes = h.to_bytes().to_vec();
494        let sh = SubmessageHeader {
495            submessage_id: SubmessageId::DataFrag,
496            flags,
497            octets_to_next_header: body.len() as u16,
498        };
499        bytes.extend_from_slice(&sh.to_bytes());
500        bytes.extend_from_slice(&body);
501        let parsed = decode_datagram(&bytes).unwrap();
502        match &parsed.submessages[0] {
503            ParsedSubmessage::DataFrag(decoded) => {
504                assert_eq!(decoded, &df);
505                assert!(decoded.hash_key_flag);
506                assert!(!decoded.inline_qos_flag);
507            }
508            other => panic!("expected DataFrag, got {other:?}"),
509        }
510    }
511
512    #[test]
513    fn decode_heartbeat_frag_roundtrip() {
514        let h = header();
515        let hf = HeartbeatFragSubmessage {
516            reader_id: crate::wire_types::EntityId::user_reader_with_key([1, 2, 3]),
517            writer_id: crate::wire_types::EntityId::user_writer_with_key([4, 5, 6]),
518            writer_sn: SequenceNumber(42),
519            last_fragment_num: crate::wire_types::FragmentNumber(8),
520            count: 3,
521        };
522        let (body, flags) = hf.write_body(true);
523        let mut bytes = h.to_bytes().to_vec();
524        let sh = SubmessageHeader {
525            submessage_id: SubmessageId::HeartbeatFrag,
526            flags,
527            octets_to_next_header: body.len() as u16,
528        };
529        bytes.extend_from_slice(&sh.to_bytes());
530        bytes.extend_from_slice(&body);
531        let parsed = decode_datagram(&bytes).unwrap();
532        match &parsed.submessages[0] {
533            ParsedSubmessage::HeartbeatFrag(decoded) => assert_eq!(decoded, &hf),
534            other => panic!("expected HeartbeatFrag, got {other:?}"),
535        }
536    }
537
538    #[test]
539    fn decode_nack_frag_roundtrip() {
540        let h = header();
541        let nf = NackFragSubmessage {
542            reader_id: crate::wire_types::EntityId::user_reader_with_key([1, 2, 3]),
543            writer_id: crate::wire_types::EntityId::user_writer_with_key([4, 5, 6]),
544            writer_sn: SequenceNumber(5),
545            fragment_number_state: crate::submessages::FragmentNumberSet {
546                bitmap_base: crate::wire_types::FragmentNumber(1),
547                num_bits: 4,
548                bitmap: vec![0b1010_0000_0000_0000_0000_0000_0000_0000],
549            },
550            count: 1,
551        };
552        let (body, flags) = nf.write_body(true);
553        let mut bytes = h.to_bytes().to_vec();
554        let sh = SubmessageHeader {
555            submessage_id: SubmessageId::NackFrag,
556            flags,
557            octets_to_next_header: body.len() as u16,
558        };
559        bytes.extend_from_slice(&sh.to_bytes());
560        bytes.extend_from_slice(&body);
561        let parsed = decode_datagram(&bytes).unwrap();
562        match &parsed.submessages[0] {
563            ParsedSubmessage::NackFrag(decoded) => assert_eq!(decoded, &nf),
564            other => panic!("expected NackFrag, got {other:?}"),
565        }
566    }
567
568    // ---- WP 1.E stage E/F: InfoSource + InfoReply via datagram ----
569
570    #[test]
571    fn decode_info_source_via_datagram() {
572        use crate::wire_types::{GuidPrefix, ProtocolVersion as PV, VendorId};
573        let h = header();
574        let info = InfoSourceSubmessage {
575            unused: 0,
576            protocol_version: PV::V2_5,
577            vendor_id: VendorId([0xAB, 0xCD]),
578            guid_prefix: GuidPrefix::from_bytes([3; 12]),
579        };
580        let (body, flags) = info.write_body(true);
581        let mut bytes = h.to_bytes().to_vec();
582        let sh = SubmessageHeader {
583            submessage_id: SubmessageId::InfoSrc,
584            flags,
585            octets_to_next_header: body.len() as u16,
586        };
587        bytes.extend_from_slice(&sh.to_bytes());
588        bytes.extend_from_slice(&body);
589        let parsed = decode_datagram(&bytes).unwrap();
590        match &parsed.submessages[0] {
591            ParsedSubmessage::InfoSource(decoded) => assert_eq!(decoded, &info),
592            other => panic!("expected InfoSource, got {other:?}"),
593        }
594    }
595
596    #[test]
597    fn decode_info_reply_with_multicast_via_datagram() {
598        use crate::wire_types::Locator;
599        let h = header();
600        let info = InfoReplySubmessage {
601            unicast_locators: alloc::vec![Locator::udp_v4([10, 1, 2, 3], 7411)],
602            multicast_locators: Some(alloc::vec![Locator::udp_v4([239, 255, 0, 1], 7400)]),
603        };
604        let (body, flags) = info.write_body(true);
605        let mut bytes = h.to_bytes().to_vec();
606        let sh = SubmessageHeader {
607            submessage_id: SubmessageId::InfoReply,
608            flags,
609            octets_to_next_header: body.len() as u16,
610        };
611        bytes.extend_from_slice(&sh.to_bytes());
612        bytes.extend_from_slice(&body);
613        let parsed = decode_datagram(&bytes).unwrap();
614        match &parsed.submessages[0] {
615            ParsedSubmessage::InfoReply(decoded) => assert_eq!(decoded, &info),
616            other => panic!("expected InfoReply, got {other:?}"),
617        }
618    }
619
620    #[test]
621    fn decode_rejects_truncated_after_header() {
622        let h = header();
623        let mut bytes = h.to_bytes().to_vec();
624        bytes.extend_from_slice(&[0u8, 0, 0]); // nur 3 Byte Sub-Header statt 4
625        let res = decode_datagram(&bytes);
626        assert!(matches!(res, Err(WireError::UnexpectedEof { .. })));
627    }
628
629    #[test]
630    fn decode_header_extension_in_datagram() {
631        let h = header();
632        let he = crate::header_extension::HeaderExtension {
633            little_endian: true,
634            message_length: Some(123),
635            timestamp: Some(crate::header_extension::HeTimestamp {
636                seconds: 1,
637                fraction: 2,
638            }),
639            checksum: crate::header_extension::ChecksumValue::Crc32c(0xDEAD_BEEF),
640            ..crate::header_extension::HeaderExtension::default()
641        };
642        let mut bytes = h.to_bytes().to_vec();
643        bytes.extend_from_slice(&he.encode().unwrap());
644        let parsed = decode_datagram(&bytes).unwrap();
645        assert_eq!(parsed.submessages.len(), 1);
646        match &parsed.submessages[0] {
647            ParsedSubmessage::HeaderExtension(decoded) => assert_eq!(decoded, &he),
648            other => panic!("expected HE, got {other:?}"),
649        }
650    }
651
652    #[test]
653    fn decode_rejects_unknown_submessage_with_must_understand() {
654        // Submessage ID 0x7E with must-understand bit (0x80 in the flag byte)
655        // → ganze Message verwerfen.
656        let h = header();
657        let mut bytes = h.to_bytes().to_vec();
658        let body = [0u8; 4];
659        let sh = SubmessageHeader {
660            submessage_id: SubmessageId::Pad, // the ID value is overwritten shortly
661            flags: FLAG_E_LITTLE_ENDIAN | SUBMESSAGE_FLAG_MUST_UNDERSTAND,
662            octets_to_next_header: body.len() as u16,
663        };
664        let mut sh_bytes = sh.to_bytes();
665        sh_bytes[0] = 0x7E; // unknown ID, outside the 0x80 range
666        bytes.extend_from_slice(&sh_bytes);
667        bytes.extend_from_slice(&body);
668        let res = decode_datagram(&bytes);
669        assert!(matches!(
670            res,
671            Err(WireError::ValueOutOfRange { message: msg }) if msg.contains("must-understand")
672        ));
673    }
674
675    #[test]
676    fn decode_skips_unknown_submessage_without_must_understand() {
677        // Without the Must-Understand bit: skip + mark as Unknown.
678        let h = header();
679        let mut bytes = h.to_bytes().to_vec();
680        let body = [0u8; 4];
681        let sh = SubmessageHeader {
682            submessage_id: SubmessageId::Pad,
683            flags: FLAG_E_LITTLE_ENDIAN,
684            octets_to_next_header: body.len() as u16,
685        };
686        let mut sh_bytes = sh.to_bytes();
687        sh_bytes[0] = 0x7E;
688        bytes.extend_from_slice(&sh_bytes);
689        bytes.extend_from_slice(&body);
690        let parsed = decode_datagram(&bytes).unwrap();
691        assert_eq!(parsed.submessages.len(), 1);
692        match &parsed.submessages[0] {
693            ParsedSubmessage::Unknown { id, .. } => assert_eq!(*id, 0x7E),
694            other => panic!("expected Unknown, got {other:?}"),
695        }
696    }
697
698    #[test]
699    fn decode_data_after_header_extension() {
700        // Wire-Layout: RtpsHeader || HE || DATA.
701        let h = header();
702        let he = crate::header_extension::HeaderExtension {
703            little_endian: true,
704            message_length: Some(0),
705            ..crate::header_extension::HeaderExtension::default()
706        };
707        let d = data_msg(7, b"after-he");
708        let mut bytes = h.to_bytes().to_vec();
709        bytes.extend_from_slice(&he.encode().unwrap());
710        let (body, flags) = d.write_body(true);
711        let sh = SubmessageHeader {
712            submessage_id: SubmessageId::Data,
713            flags,
714            octets_to_next_header: body.len() as u16,
715        };
716        bytes.extend_from_slice(&sh.to_bytes());
717        bytes.extend_from_slice(&body);
718        let parsed = decode_datagram(&bytes).unwrap();
719        assert_eq!(parsed.submessages.len(), 2);
720        assert!(matches!(
721            &parsed.submessages[0],
722            ParsedSubmessage::HeaderExtension(_)
723        ));
724        assert!(matches!(&parsed.submessages[1], ParsedSubmessage::Data(_)));
725    }
726
727    #[test]
728    fn decode_rejects_header_extension_after_data_submessage() {
729        // Spec §8.3.7.3: HE MUST appear directly after the RTPS header.
730        // If another submessage was parsed before it, reject.
731        // Wire layout: RtpsHeader || DATA || HE.
732        let h = header();
733        let d = data_msg(7, b"first");
734        let he = crate::header_extension::HeaderExtension {
735            little_endian: true,
736            message_length: Some(0),
737            ..crate::header_extension::HeaderExtension::default()
738        };
739        let mut bytes = h.to_bytes().to_vec();
740        let (dbody, dflags) = d.write_body(true);
741        let dsh = SubmessageHeader {
742            submessage_id: SubmessageId::Data,
743            flags: dflags,
744            octets_to_next_header: dbody.len() as u16,
745        };
746        bytes.extend_from_slice(&dsh.to_bytes());
747        bytes.extend_from_slice(&dbody);
748        bytes.extend_from_slice(&he.encode().unwrap());
749        let res = decode_datagram(&bytes);
750        assert!(matches!(res, Err(WireError::ValueOutOfRange { .. })));
751    }
752
753    // ---- §9.4.2.11.2 Must-Understand-Bit reject path ----
754
755    #[test]
756    fn decode_rejects_data_with_unknown_must_understand_pid_in_inline_qos() {
757        use crate::parameter_list::{MUST_UNDERSTAND_BIT, Parameter, ParameterList};
758        let h = header();
759        // Inline QoS with an unknown MU PID 0x3500 (not a standard PID).
760        let mut pl = ParameterList::new();
761        pl.push(Parameter::new(
762            MUST_UNDERSTAND_BIT | 0x3500,
763            vec![1, 2, 3, 4],
764        ));
765        let d = DataSubmessage {
766            extra_flags: 0,
767            reader_id: EntityId::user_reader_with_key([0xA, 0xB, 0xC]),
768            writer_id: EntityId::user_writer_with_key([0x1, 0x2, 0x3]),
769            writer_sn: SequenceNumber(1),
770            inline_qos: Some(pl),
771            key_flag: false,
772            non_standard_flag: false,
773            serialized_payload: alloc::sync::Arc::from([] as [u8; 0]),
774        };
775        let mut bytes = h.to_bytes().to_vec();
776        let (body, flags) = d.write_body(true);
777        let sh = SubmessageHeader {
778            submessage_id: SubmessageId::Data,
779            flags,
780            octets_to_next_header: body.len() as u16,
781        };
782        bytes.extend_from_slice(&sh.to_bytes());
783        bytes.extend_from_slice(&body);
784        let res = decode_datagram(&bytes);
785        assert!(matches!(res, Err(WireError::ValueOutOfRange { .. })));
786    }
787
788    #[test]
789    fn decode_accepts_data_with_known_must_understand_pid_in_inline_qos() {
790        use crate::parameter_list::{MUST_UNDERSTAND_BIT, Parameter, ParameterList, pid};
791        let h = header();
792        let mut pl = ParameterList::new();
793        // KEY_HASH is a standard PID, MU bit allowed.
794        pl.push(Parameter::new(
795            MUST_UNDERSTAND_BIT | pid::KEY_HASH,
796            vec![0; 16],
797        ));
798        let d = DataSubmessage {
799            extra_flags: 0,
800            reader_id: EntityId::user_reader_with_key([0xA, 0xB, 0xC]),
801            writer_id: EntityId::user_writer_with_key([0x1, 0x2, 0x3]),
802            writer_sn: SequenceNumber(2),
803            inline_qos: Some(pl),
804            key_flag: false,
805            non_standard_flag: false,
806            serialized_payload: alloc::sync::Arc::from([] as [u8; 0]),
807        };
808        let mut bytes = h.to_bytes().to_vec();
809        let (body, flags) = d.write_body(true);
810        let sh = SubmessageHeader {
811            submessage_id: SubmessageId::Data,
812            flags,
813            octets_to_next_header: body.len() as u16,
814        };
815        bytes.extend_from_slice(&sh.to_bytes());
816        bytes.extend_from_slice(&body);
817        decode_datagram(&bytes).expect("known MU PID should pass");
818    }
819
820    #[test]
821    fn decode_accepts_vendor_specific_must_understand_pid() {
822        use crate::parameter_list::{
823            MUST_UNDERSTAND_BIT, Parameter, ParameterList, VENDOR_SPECIFIC_BIT,
824        };
825        let h = header();
826        let mut pl = ParameterList::new();
827        // Vendor-specific MU PID — Spec §9.6.2 allows ignoring.
828        pl.push(Parameter::new(
829            MUST_UNDERSTAND_BIT | VENDOR_SPECIFIC_BIT | 0x0050,
830            vec![0xCA, 0xFE, 0xBA, 0xBE],
831        ));
832        let d = DataSubmessage {
833            extra_flags: 0,
834            reader_id: EntityId::user_reader_with_key([0xA, 0xB, 0xC]),
835            writer_id: EntityId::user_writer_with_key([0x1, 0x2, 0x3]),
836            writer_sn: SequenceNumber(3),
837            inline_qos: Some(pl),
838            key_flag: false,
839            non_standard_flag: false,
840            serialized_payload: alloc::sync::Arc::from([] as [u8; 0]),
841        };
842        let mut bytes = h.to_bytes().to_vec();
843        let (body, flags) = d.write_body(true);
844        let sh = SubmessageHeader {
845            submessage_id: SubmessageId::Data,
846            flags,
847            octets_to_next_header: body.len() as u16,
848        };
849        bytes.extend_from_slice(&sh.to_bytes());
850        bytes.extend_from_slice(&body);
851        decode_datagram(&bytes).expect("vendor-specific MU PID should pass");
852    }
853
854    #[test]
855    fn rtps_2_1_treats_0x80_as_vendor_specific_not_header_extension() {
856        use crate::wire_types::ProtocolVersion;
857        // Spec §8.3.7.3: HeaderExtension (0x80) ist ein 2.5-Submessage.
858        // Older vendors (e.g. Cyclone 2.1) may use 0x80 as
859        // vendor-specific. We do NOT discard, but skip.
860        let mut h = header();
861        h.protocol_version = ProtocolVersion::V2_1;
862        let mut bytes = h.to_bytes().to_vec();
863        // First a real DATA submessage (so `submessages` becomes
864        // non-empty), then 0x80 as a vendor submessage appended.
865        let d = data_msg(1, b"x");
866        let (body, flags) = d.write_body(true);
867        let sh = SubmessageHeader {
868            submessage_id: SubmessageId::Data,
869            flags,
870            octets_to_next_header: body.len() as u16,
871        };
872        bytes.extend_from_slice(&sh.to_bytes());
873        bytes.extend_from_slice(&body);
874        // Vendor submessage 0x80 with a 4-byte body, no MU bit.
875        bytes.extend_from_slice(&[0x80, FLAG_E_LITTLE_ENDIAN, 4, 0]);
876        bytes.extend_from_slice(&[0xDE, 0xAD, 0xBE, 0xEF]);
877        let parsed = decode_datagram(&bytes).expect("0x80 under RTPS 2.1 must skip");
878        assert!(matches!(
879            parsed.submessages.last(),
880            Some(ParsedSubmessage::Unknown { id: 0x80, .. })
881        ));
882    }
883}
zerodds_rtps/datagram.rs

zerodds_rtps/
datagram.rs
