Fix message framing (backtracking parser)

mavlink-core/Cargo.toml

@@ -1,7 +1,14 @@
 [package]
 name = "mavlink-core"
 version = "0.12.2"
-authors = ["Todd Stellanova", "Michal Podhradsky", "Kevin Mehall", "Tim Ryan", "Patrick José Pereira", "Ibiyemi Abiodun"]
+authors = [
+    "Todd Stellanova",
+    "Michal Podhradsky",
+    "Kevin Mehall",
+    "Tim Ryan",
+    "Patrick José Pereira",
+    "Ibiyemi Abiodun",
+]
 description = "Implements the MAVLink data interchange format for UAVs."
 readme = "README.md"
 license = "MIT/Apache-2.0"

mavlink-core/src/connection/direct_serial.rs

@@ -1,10 +1,12 @@
 use crate::connection::MavConnection;
+use crate::peek_reader::PeekReader;
 use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message};
+use core::ops::DerefMut;
 use std::io;
 use std::sync::Mutex;
 
 use crate::error::{MessageReadError, MessageWriteError};
-use serial::prelude::*;
+use serial::{prelude::*, SystemPort};
 
 /// Serial MAVLINK connection
 
@@ -40,24 +42,23 @@ pub fn open(settings: &str) -> io::Result<SerialConnection> {
     port.configure(&settings)?;
 
     Ok(SerialConnection {
-        port: Mutex::new(port),
+        port: Mutex::new(PeekReader::new(port)),
         sequence: Mutex::new(0),
         protocol_version: MavlinkVersion::V2,
     })
 }
 
 pub struct SerialConnection {
-    port: Mutex<serial::SystemPort>,
+    port: Mutex<PeekReader<SystemPort>>,
     sequence: Mutex<u8>,
     protocol_version: MavlinkVersion,
 }
 
 impl<M: Message> MavConnection<M> for SerialConnection {
     fn recv(&self) -> Result<(MavHeader, M), MessageReadError> {
         let mut port = self.port.lock().unwrap();
-
         loop {
-            match read_versioned_msg(&mut *port, self.protocol_version) {
+            match read_versioned_msg(port.deref_mut(), self.protocol_version) {
                 ok @ Ok(..) => {
                     return ok;
                 }
@@ -83,7 +84,7 @@ impl<M: Message> MavConnection<M> for SerialConnection {
 
         *sequence = sequence.wrapping_add(1);
 
-        write_versioned_msg(&mut *port, self.protocol_version, header, data)
+        write_versioned_msg(port.reader_mut(), self.protocol_version, header, data)
     }
 
     fn set_protocol_version(&mut self, version: MavlinkVersion) {

mavlink-core/src/connection/file.rs

@@ -1,8 +1,10 @@
 use crate::connection::MavConnection;
 use crate::error::{MessageReadError, MessageWriteError};
+use crate::peek_reader::PeekReader;
 use crate::{read_versioned_msg, MavHeader, MavlinkVersion, Message};
+use core::ops::DerefMut;
 use std::fs::File;
-use std::io::{self};
+use std::io;
 use std::sync::Mutex;
 
 /// File MAVLINK connection
@@ -11,13 +13,13 @@ pub fn open(file_path: &str) -> io::Result<FileConnection> {
     let file = File::open(file_path)?;
 
     Ok(FileConnection {
-        file: Mutex::new(file),
+        file: Mutex::new(PeekReader::new(file)),
         protocol_version: MavlinkVersion::V2,
     })
 }
 
 pub struct FileConnection {
-    file: Mutex<std::fs::File>,
+    file: Mutex<PeekReader<File>>,
     protocol_version: MavlinkVersion,
 }
 
@@ -28,7 +30,7 @@ impl<M: Message> MavConnection<M> for FileConnection {
         let mut file = self.file.lock().unwrap();
 
         loop {
-            match read_versioned_msg(&mut *file, self.protocol_version) {
+            match read_versioned_msg(file.deref_mut(), self.protocol_version) {
                 ok @ Ok(..) => {
                     return ok;
                 }

mavlink-core/src/connection/tcp.rs

@@ -1,6 +1,8 @@
 use crate::connection::MavConnection;
+use crate::peek_reader::PeekReader;
 use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message};
-use std::io::{self};
+use core::ops::DerefMut;
+use std::io;
 use std::net::ToSocketAddrs;
 use std::net::{TcpListener, TcpStream};
 use std::sync::Mutex;
@@ -34,7 +36,7 @@ pub fn tcpout<T: ToSocketAddrs>(address: T) -> io::Result<TcpConnection> {
     socket.set_read_timeout(Some(Duration::from_millis(100)))?;
 
     Ok(TcpConnection {
-        reader: Mutex::new(socket.try_clone()?),
+        reader: Mutex::new(PeekReader::new(socket.try_clone()?)),
         writer: Mutex::new(TcpWrite {
             socket,
             sequence: 0,
@@ -52,7 +54,7 @@ pub fn tcpin<T: ToSocketAddrs>(address: T) -> io::Result<TcpConnection> {
         match incoming {
             Ok(socket) => {
                 return Ok(TcpConnection {
-                    reader: Mutex::new(socket.try_clone()?),
+                    reader: Mutex::new(PeekReader::new(socket.try_clone()?)),
                     writer: Mutex::new(TcpWrite {
                         socket,
                         sequence: 0,
@@ -73,7 +75,7 @@ pub fn tcpin<T: ToSocketAddrs>(address: T) -> io::Result<TcpConnection> {
 }
 
 pub struct TcpConnection {
-    reader: Mutex<TcpStream>,
+    reader: Mutex<PeekReader<TcpStream>>,
     writer: Mutex<TcpWrite>,
     protocol_version: MavlinkVersion,
 }
@@ -85,8 +87,8 @@ struct TcpWrite {
 
 impl<M: Message> MavConnection<M> for TcpConnection {
     fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> {
-        let mut lock = self.reader.lock().expect("tcp read failure");
-        read_versioned_msg(&mut *lock, self.protocol_version)
+        let mut reader = self.reader.lock().unwrap();
+        read_versioned_msg(reader.deref_mut(), self.protocol_version)
     }
 
     fn send(&self, header: &MavHeader, data: &M) -> Result<usize, crate::error::MessageWriteError> {

mavlink-core/src/connection/udp.rs

@@ -1,7 +1,8 @@
 use crate::connection::MavConnection;
+use crate::peek_reader::PeekReader;
 use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message};
-use std::io::Read;
-use std::io::{self};
+use core::ops::DerefMut;
+use std::io::{self, Read};
 use std::net::ToSocketAddrs;
 use std::net::{SocketAddr, UdpSocket};
 use std::sync::Mutex;
@@ -31,7 +32,7 @@ pub fn select_protocol<M: Message>(
 
 pub fn udpbcast<T: ToSocketAddrs>(address: T) -> io::Result<UdpConnection> {
     let addr = get_socket_addr(address)?;
-    let socket = UdpSocket::bind("0.0.0.0:0")?;
+    let socket = UdpSocket::bind("0.0.0.0:0").unwrap();
     socket
         .set_broadcast(true)
         .expect("Couldn't bind to broadcast address.");
@@ -45,67 +46,37 @@ pub fn udpout<T: ToSocketAddrs>(address: T) -> io::Result<UdpConnection> {
 }
 
 pub fn udpin<T: ToSocketAddrs>(address: T) -> io::Result<UdpConnection> {
-    let addr = get_socket_addr(address)?;
+    let addr = address
+        .to_socket_addrs()
+        .unwrap()
+        .next()
+        .expect("Invalid address");
     let socket = UdpSocket::bind(addr)?;
     UdpConnection::new(socket, true, None)
 }
 
-struct UdpWrite {
+struct UdpRead {
     socket: UdpSocket,
-    dest: Option<SocketAddr>,
-    sequence: u8,
-}
-
-struct PacketBuf {
-    buf: Vec<u8>,
-    start: usize,
-    end: usize,
-}
-
-impl PacketBuf {
-    pub fn new() -> Self {
-        let v = vec![0; 65536];
-        Self {
-            buf: v,
-            start: 0,
-            end: 0,
-        }
-    }
-
-    pub fn reset(&mut self) -> &mut [u8] {
-        self.start = 0;
-        self.end = 0;
-        &mut self.buf
-    }
-
-    pub fn set_len(&mut self, size: usize) {
-        self.end = size;
-    }
-
-    pub fn slice(&self) -> &[u8] {
-        &self.buf[self.start..self.end]
-    }
-
-    pub fn len(&self) -> usize {
-        self.slice().len()
-    }
+    last_recv_address: Option<SocketAddr>,
 }
 
-impl Read for PacketBuf {
+impl Read for UdpRead {
     fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
-        let n = Read::read(&mut self.slice(), buf)?;
-        self.start += n;
-        Ok(n)
+        self.socket.recv_from(buf).map(|(n, addr)| {
+            self.last_recv_address = Some(addr);
+            n
+        })
     }
 }
 
-struct UdpRead {
+struct UdpWrite {
     socket: UdpSocket,
-    recv_buf: PacketBuf,
+    dest: Option<SocketAddr>,
+    sequence: u8,
 }
 
 pub struct UdpConnection {
-    reader: Mutex<UdpRead>,
+    reader: Mutex<PeekReader<UdpRead>>,
     writer: Mutex<UdpWrite>,
     protocol_version: MavlinkVersion,
     server: bool,
@@ -115,10 +86,10 @@ impl UdpConnection {
     fn new(socket: UdpSocket, server: bool, dest: Option<SocketAddr>) -> io::Result<Self> {
         Ok(Self {
             server,
-            reader: Mutex::new(UdpRead {
+            reader: Mutex::new(PeekReader::new(UdpRead {
                 socket: socket.try_clone()?,
-                recv_buf: PacketBuf::new(),
-            }),
+                last_recv_address: None,
+            })),
             writer: Mutex::new(UdpWrite {
                 socket,
                 dest,
@@ -131,19 +102,16 @@ impl UdpConnection {
 
 impl<M: Message> MavConnection<M> for UdpConnection {
     fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> {
-        let mut guard = self.reader.lock().unwrap();
-        let state = &mut *guard;
-        loop {
-            if state.recv_buf.len() == 0 {
-                let (len, src) = state.socket.recv_from(state.recv_buf.reset())?;
-                state.recv_buf.set_len(len);
+        let mut reader = self.reader.lock().unwrap();
 
-                if self.server {
-                    self.writer.lock().unwrap().dest = Some(src);
+        loop {
+            let result = read_versioned_msg(reader.deref_mut(), self.protocol_version);
+            if self.server {
+                if let addr @ Some(_) = reader.reader_ref().last_recv_address {
+                    self.writer.lock().unwrap().dest = addr;
                 }
             }
-
-            if let ok @ Ok(..) = read_versioned_msg(&mut state.recv_buf, self.protocol_version) {
+            if let ok @ Ok(..) = result {
                 return ok;
             }
         }