mavlink_core/
reader.rs

1//! Incremental blocking MAVLink reader.
2
3use crate::{
4    MAVLinkMessageRaw, MAVLinkV1MessageRaw, MAVLinkV2MessageRaw, MavHeader, MavlinkVersion,
5    Message, SigningData,
6    error::MessageReadError,
7    frame_decoder::{FrameDecoder, FrameRef, VersionFilter},
8};
9
10#[cfg(all(feature = "embedded", not(feature = "std")))]
11use embedded_io::Read;
12#[cfg(feature = "std")]
13use std::io::Read;
14
15/// Incrementally reads complete MAVLink frames from a blocking byte stream.
16///
17/// The reader keeps partial and read-ahead data between calls. Use the same
18/// MAVLink dialect `M` for the lifetime of a stream, because CRC validation is
19/// dialect-specific.
20pub struct MavlinkReader<R> {
21    source: R,
22    decoder: FrameDecoder,
23}
24
25impl<R> MavlinkReader<R> {
26    /// Creates a reader around `source` with the default read-ahead capacity.
27    ///
28    /// The buffer is allocated once and reused for the reader's lifetime.
29    #[cfg(feature = "std")]
30    pub fn new(source: R) -> Self
31    where
32        R: Read,
33    {
34        Self {
35            source,
36            decoder: FrameDecoder::new(),
37        }
38    }
39
40    /// Creates an allocation-free reader around `source`.
41    #[cfg(all(feature = "embedded", not(feature = "std")))]
42    pub const fn new(source: R) -> Self
43    where
44        R: Read,
45    {
46        Self {
47            source,
48            decoder: FrameDecoder::new(),
49        }
50    }
51
52    /// Creates a reader with at least `capacity` bytes of read-ahead space.
53    ///
54    /// Capacities smaller than one maximum-size MAVLink frame are raised to
55    /// [`consts::MAX_FRAME_SIZE`](crate::consts::MAX_FRAME_SIZE). The buffer is
56    /// allocated once and reused for the lifetime of the reader.
57    #[cfg(feature = "std")]
58    pub fn with_capacity(capacity: usize, source: R) -> Self
59    where
60        R: Read,
61    {
62        Self {
63            source,
64            decoder: FrameDecoder::with_capacity(capacity),
65        }
66    }
67
68    /// Returns a shared reference to the underlying source.
69    pub const fn get_ref(&self) -> &R {
70        &self.source
71    }
72
73    /// Returns a mutable reference to the underlying source.
74    ///
75    /// Reading directly from the source can skip bytes already buffered by
76    /// this reader and should therefore be avoided.
77    pub const fn get_mut(&mut self) -> &mut R {
78        &mut self.source
79    }
80
81    /// Returns the underlying source.
82    ///
83    /// Any bytes read ahead by this reader are discarded.
84    pub fn into_inner(self) -> R {
85        self.source
86    }
87}
88
89#[cfg(any(feature = "std", all(feature = "embedded", not(feature = "std"))))]
90impl<R: Read> MavlinkReader<R> {
91    /// Reads and parses the next CRC-valid message accepted by `version`.
92    pub fn read_message<M: Message>(
93        &mut self,
94        version: MavlinkVersion,
95    ) -> Result<(MavHeader, M), MessageReadError> {
96        self.read_message_inner(VersionFilter::Exact(version), None)
97    }
98
99    /// Reads and parses the next CRC-valid MAVLink 1 or MAVLink 2 message.
100    pub fn read_any_message<M: Message>(&mut self) -> Result<(MavHeader, M), MessageReadError> {
101        self.read_message_inner(VersionFilter::Any, None)
102    }
103
104    /// Reads the next CRC-valid raw message accepted by `version`.
105    pub fn read_raw_message<M: Message>(
106        &mut self,
107        version: MavlinkVersion,
108    ) -> Result<MAVLinkMessageRaw, MessageReadError> {
109        self.read_raw_message_inner::<M>(VersionFilter::Exact(version), None)
110    }
111
112    /// Reads the next CRC-valid MAVLink 1 or MAVLink 2 raw message.
113    pub fn read_any_raw_message<M: Message>(
114        &mut self,
115    ) -> Result<MAVLinkMessageRaw, MessageReadError> {
116        self.read_raw_message_inner::<M>(VersionFilter::Any, None)
117    }
118
119    /// Reads, verifies, and parses the next message accepted by `version`.
120    ///
121    /// With signing data, MAVLink 1 and unsigned MAVLink 2 frames require
122    /// `SigningConfig::allow_unsigned`; signed MAVLink 2 frames must have a valid
123    /// signature. With `None`, signature verification is skipped.
124    #[cfg(feature = "mav2-message-signing")]
125    pub fn read_message_signed<M: Message>(
126        &mut self,
127        version: MavlinkVersion,
128        signing_data: Option<&SigningData>,
129    ) -> Result<(MavHeader, M), MessageReadError> {
130        self.read_message_inner(VersionFilter::Exact(version), signing_data)
131    }
132
133    /// Reads, verifies, and parses the next MAVLink 1 or MAVLink 2 message.
134    /// With signing data, MAVLink 1 frames follow `SigningConfig::allow_unsigned`.
135    /// With `None`, signature verification is skipped.
136    #[cfg(feature = "mav2-message-signing")]
137    pub fn read_any_message_signed<M: Message>(
138        &mut self,
139        signing_data: Option<&SigningData>,
140    ) -> Result<(MavHeader, M), MessageReadError> {
141        self.read_message_inner(VersionFilter::Any, signing_data)
142    }
143
144    /// Reads and verifies the next raw message accepted by `version`.
145    ///
146    /// With signing data, MAVLink 1 and unsigned MAVLink 2 frames require
147    /// `SigningConfig::allow_unsigned`; signed MAVLink 2 frames must have a valid
148    /// signature. With `None`, signature verification is skipped.
149    #[cfg(feature = "mav2-message-signing")]
150    pub fn read_raw_message_signed<M: Message>(
151        &mut self,
152        version: MavlinkVersion,
153        signing_data: Option<&SigningData>,
154    ) -> Result<MAVLinkMessageRaw, MessageReadError> {
155        self.read_raw_message_inner::<M>(VersionFilter::Exact(version), signing_data)
156    }
157
158    /// Reads and verifies the next MAVLink 1 or MAVLink 2 raw message.
159    /// With signing data, MAVLink 1 frames follow `SigningConfig::allow_unsigned`.
160    /// With `None`, signature verification is skipped.
161    #[cfg(feature = "mav2-message-signing")]
162    pub fn read_any_raw_message_signed<M: Message>(
163        &mut self,
164        signing_data: Option<&SigningData>,
165    ) -> Result<MAVLinkMessageRaw, MessageReadError> {
166        self.read_raw_message_inner::<M>(VersionFilter::Any, signing_data)
167    }
168
169    #[inline]
170    pub(crate) fn read_message_inner<M: Message>(
171        &mut self,
172        filter: VersionFilter,
173        signing_data: Option<&SigningData>,
174    ) -> Result<(MavHeader, M), MessageReadError> {
175        loop {
176            if let Some(result) = try_decode_message::<M>(&mut self.decoder, filter, signing_data) {
177                return result;
178            }
179            self.read_more()?;
180        }
181    }
182
183    #[inline]
184    pub(crate) fn read_raw_message_inner<M: Message>(
185        &mut self,
186        filter: VersionFilter,
187        signing_data: Option<&SigningData>,
188    ) -> Result<MAVLinkMessageRaw, MessageReadError> {
189        loop {
190            if let Some(message) =
191                try_decode_raw_message::<M>(&mut self.decoder, filter, signing_data)
192            {
193                return Ok(message);
194            }
195            self.read_more()?;
196        }
197    }
198
199    fn read_more(&mut self) -> Result<(), MessageReadError> {
200        let destination = self.decoder.spare_capacity_mut();
201        assert!(
202            !destination.is_empty(),
203            "a pending MAVLink frame must fit in the decoder buffer"
204        );
205
206        #[cfg(feature = "std")]
207        let count = loop {
208            match self.source.read(destination) {
209                Err(error) if error.kind() == std::io::ErrorKind::Interrupted => continue,
210                result => break result?,
211            }
212        };
213
214        #[cfg(all(feature = "embedded", not(feature = "std")))]
215        let count = self
216            .source
217            .read(destination)
218            .map_err(|_| MessageReadError::Io)?;
219
220        if count == 0 {
221            return Err(MessageReadError::eof());
222        }
223        self.decoder.commit(count);
224        Ok(())
225    }
226}
227
228#[inline]
229pub(crate) fn try_decode_message<M: Message>(
230    decoder: &mut FrameDecoder,
231    filter: VersionFilter,
232    signing_data: Option<&SigningData>,
233) -> Option<Result<(MavHeader, M), MessageReadError>> {
234    loop {
235        let meta = decoder.next_frame::<M>(filter)?;
236
237        #[cfg(feature = "mav2-message-signing")]
238        if let Some(signing_data) = signing_data {
239            if !signature_is_valid(decoder.frame(meta), signing_data) {
240                decoder.advance(meta);
241                continue;
242            }
243        }
244
245        #[cfg(not(feature = "mav2-message-signing"))]
246        let _ = signing_data;
247
248        let (header, parsed) = {
249            let frame = decoder.frame(meta);
250            (
251                frame_header(frame),
252                M::parse(frame.version(), frame.message_id(), frame.payload()),
253            )
254        };
255        decoder.advance(meta);
256        return Some(parsed.map(|message| (header, message)).map_err(Into::into));
257    }
258}
259
260#[inline]
261pub(crate) fn try_decode_raw_message<M: Message>(
262    decoder: &mut FrameDecoder,
263    filter: VersionFilter,
264    signing_data: Option<&SigningData>,
265) -> Option<MAVLinkMessageRaw> {
266    loop {
267        let meta = decoder.next_frame::<M>(filter)?;
268        let frame = decoder.frame(meta);
269
270        let message = match frame.version() {
271            MavlinkVersion::V1 => MAVLinkMessageRaw::V1(MAVLinkV1MessageRaw::from(frame)),
272            MavlinkVersion::V2 => MAVLinkMessageRaw::V2(MAVLinkV2MessageRaw::from(frame)),
273        };
274
275        #[cfg(feature = "mav2-message-signing")]
276        if let Some(signing_data) = signing_data {
277            if !raw_signature_is_valid(&message, signing_data) {
278                decoder.advance(meta);
279                continue;
280            }
281        }
282
283        #[cfg(not(feature = "mav2-message-signing"))]
284        let _ = signing_data;
285
286        decoder.advance(meta);
287        return Some(message);
288    }
289}
290
291#[inline]
292fn frame_header(frame: FrameRef<'_>) -> MavHeader {
293    MavHeader {
294        sequence: frame.sequence(),
295        system_id: frame.system_id(),
296        component_id: frame.component_id(),
297    }
298}
299
300#[cfg(feature = "mav2-message-signing")]
301fn signature_is_valid(frame: FrameRef<'_>, signing_data: &SigningData) -> bool {
302    match frame.version() {
303        MavlinkVersion::V1 => signing_data.config.allow_unsigned,
304        MavlinkVersion::V2 => signing_data.verify_signature(&MAVLinkV2MessageRaw::from(frame)),
305    }
306}
307
308#[cfg(feature = "mav2-message-signing")]
309fn raw_signature_is_valid(message: &MAVLinkMessageRaw, signing_data: &SigningData) -> bool {
310    match message {
311        MAVLinkMessageRaw::V1(_) => signing_data.config.allow_unsigned,
312        MAVLinkMessageRaw::V2(message) => signing_data.verify_signature(message),
313    }
314}
315
316#[cfg(all(test, feature = "std"))]
317mod tests {
318    use super::*;
319    use std::io;
320
321    #[derive(Debug, PartialEq, Eq)]
322    struct TestMessage(u8);
323
324    impl Message for TestMessage {
325        fn message_id(&self) -> u32 {
326            0
327        }
328
329        fn message_name(&self) -> &'static str {
330            "TEST"
331        }
332
333        fn target_system_id(&self) -> Option<u8> {
334            None
335        }
336
337        fn target_component_id(&self) -> Option<u8> {
338            None
339        }
340
341        fn ser(&self, _: MavlinkVersion, bytes: &mut [u8]) -> usize {
342            bytes[0] = self.0;
343            1
344        }
345
346        fn parse(_: MavlinkVersion, _: u32, payload: &[u8]) -> Result<Self, crate::ParserError> {
347            Ok(Self(payload[0]))
348        }
349
350        fn message_id_from_name(_: &str) -> Option<u32> {
351            None
352        }
353
354        fn default_message_from_id(_: u32) -> Option<Self> {
355            None
356        }
357
358        #[cfg(feature = "arbitrary")]
359        fn random_message_from_id<R: rand::TryRng<Error = core::convert::Infallible>>(
360            _: u32,
361            _: &mut R,
362        ) -> Option<Self> {
363            None
364        }
365
366        fn extra_crc(_: u32) -> u8 {
367            77
368        }
369    }
370
371    struct CountingReader {
372        bytes: std::vec::Vec<u8>,
373        offset: usize,
374        reads: usize,
375    }
376
377    impl Read for CountingReader {
378        fn read(&mut self, destination: &mut [u8]) -> io::Result<usize> {
379            self.reads += 1;
380            let count = destination.len().min(self.bytes.len() - self.offset);
381            destination[..count].copy_from_slice(&self.bytes[self.offset..self.offset + count]);
382            self.offset += count;
383            Ok(count)
384        }
385    }
386
387    #[test]
388    fn standard_reader_capacity_is_configurable_and_frame_safe() {
389        let mut default_reader = MavlinkReader::new(io::empty());
390        assert_eq!(
391            default_reader.decoder.spare_capacity_mut().len(),
392            crate::consts::DEFAULT_READ_BUFFER_CAPACITY
393        );
394
395        let mut minimum_reader = MavlinkReader::with_capacity(1, io::empty());
396        assert_eq!(
397            minimum_reader.decoder.spare_capacity_mut().len(),
398            crate::consts::MAX_FRAME_SIZE
399        );
400    }
401
402    #[test]
403    fn reads_multiple_frames_per_source_read() {
404        let header = MavHeader {
405            sequence: 1,
406            system_id: 2,
407            component_id: 3,
408        };
409        let mut bytes = std::vec::Vec::new();
410        for value in 0..16 {
411            let mut raw = MAVLinkV2MessageRaw::new();
412            raw.serialize_message(header, &TestMessage(value));
413            bytes.extend_from_slice(raw.raw_bytes());
414        }
415
416        let mut reader = MavlinkReader::new(CountingReader {
417            bytes,
418            offset: 0,
419            reads: 0,
420        });
421        for value in 0..16 {
422            let (_, message) = reader.read_any_message::<TestMessage>().unwrap();
423            assert_eq!(message, TestMessage(value));
424        }
425        assert_eq!(reader.get_ref().reads, 1);
426    }
427}
mavlink_core/reader.rs

mavlink_core/
reader.rs
