mavlink_core/connection/udp/
sync.rs

1//! UDP MAVLink connection
2
3use super::config::{UdpConfig, UdpMode};
4
5use crate::Dialect;
6use crate::MavlinkReader;
7use crate::connection::get_socket_addr;
8use crate::connection::sync::{Connectable, DialectConnection, SyncTransport, Transport};
9use std::io::{self, Read, Write};
10use std::net::{SocketAddr, UdpSocket};
11use std::sync::{Arc, Mutex};
12
13pub(crate) struct UdpRead {
14    socket: UdpSocket,
15    reply_destination: Option<Arc<Mutex<Option<SocketAddr>>>>,
16}
17
18impl Read for UdpRead {
19    fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
20        let (len, address) = self.socket.recv_from(buf)?;
21        if let Some(reply_destination) = &self.reply_destination {
22            *reply_destination.lock().unwrap() = Some(address);
23        }
24        Ok(len)
25    }
26}
27
28pub(crate) struct UdpWrite {
29    socket: UdpSocket,
30    destination: Arc<Mutex<Option<SocketAddr>>>,
31    connected: bool,
32}
33
34impl Write for UdpWrite {
35    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
36        if self.connected {
37            return self.socket.send(buf);
38        }
39        self.socket.send_to(
40            buf,
41            self.destination
42                .lock()
43                .unwrap()
44                .expect("destination checked before write"),
45        )
46    }
47
48    fn write_all(&mut self, buf: &[u8]) -> io::Result<()> {
49        if self.write(buf)? == buf.len() {
50            Ok(())
51        } else {
52            Err(io::ErrorKind::WriteZero.into())
53        }
54    }
55
56    fn flush(&mut self) -> io::Result<()> {
57        Ok(())
58    }
59}
60
61pub struct UdpConnection {
62    reader: Mutex<MavlinkReader<UdpRead>>,
63    writer: Mutex<UdpWrite>,
64}
65
66impl UdpConnection {
67    fn new(
68        socket: UdpSocket,
69        server: bool,
70        dest: Option<SocketAddr>,
71        connected: bool,
72    ) -> io::Result<Self> {
73        let destination = Arc::new(Mutex::new(dest));
74        Ok(Self {
75            reader: Mutex::new(MavlinkReader::new(UdpRead {
76                socket: socket.try_clone()?,
77                reply_destination: server.then(|| destination.clone()),
78            })),
79            writer: Mutex::new(UdpWrite {
80                socket,
81                destination,
82                connected,
83            }),
84        })
85    }
86}
87
88impl SyncTransport for UdpConnection {
89    type Reader = UdpRead;
90    type Writer = UdpWrite;
91
92    fn reader(&self) -> std::sync::MutexGuard<'_, MavlinkReader<Self::Reader>> {
93        self.reader.lock().unwrap()
94    }
95
96    fn lock_writer(&self) -> Option<std::sync::MutexGuard<'_, Self::Writer>> {
97        let writer = self.writer.lock().unwrap();
98        let has_destination = writer.destination.lock().unwrap().is_some();
99        has_destination.then_some(writer)
100    }
101
102    fn set_nonblocking(
103        &self,
104        reader: &mut MavlinkReader<Self::Reader>,
105        enabled: bool,
106    ) -> io::Result<()> {
107        reader.get_mut().socket.set_nonblocking(enabled)
108    }
109}
110
111impl UdpConfig {
112    pub(crate) fn open(&self) -> io::Result<UdpConnection> {
113        let (address, server, dest): (&str, _, _) = match self.mode {
114            UdpMode::Udpin => (&self.address, true, None),
115            _ => ("0.0.0.0:0", false, Some(get_socket_addr(&self.address)?)),
116        };
117        let (socket, connected) = match self.take_socket()? {
118            Some(socket) => (socket, !server),
119            None => (UdpSocket::bind(address)?, false),
120        };
121        if let Some(timeout) = self.read_timeout {
122            socket.set_read_timeout(Some(timeout))?;
123        }
124        if matches!(self.mode, UdpMode::UdpBroadcast) {
125            socket.set_broadcast(true)?;
126        }
127        UdpConnection::new(socket, server, dest, connected)
128    }
129}
130
131impl Connectable for UdpConfig {
132    fn connect_with_dialect<D: Dialect + Send + Sync>(
133        &self,
134        dialect: D,
135    ) -> io::Result<DialectConnection<D>> {
136        Ok(DialectConnection::new(
137            Transport::Udp(self.open()?),
138            dialect,
139        ))
140    }
141}
142
143#[cfg(test)]
144mod tests {
145    use super::*;
146
147    #[test]
148    fn test_datagram_reading() {
149        let receiver_socket = UdpSocket::bind("127.0.0.1:5000").unwrap();
150        let mut udp_reader = UdpRead {
151            socket: receiver_socket.try_clone().unwrap(),
152            reply_destination: None,
153        };
154        let sender_socket = UdpSocket::bind("0.0.0.0:0").unwrap();
155        sender_socket.connect("127.0.0.1:5000").unwrap();
156
157        let datagram: Vec<u8> = (0..50).collect::<Vec<_>>();
158
159        let mut n_sent = sender_socket.send(&datagram).unwrap();
160        assert_eq!(n_sent, datagram.len());
161        n_sent = sender_socket.send(&datagram).unwrap();
162        assert_eq!(n_sent, datagram.len());
163
164        let mut buf = [0u8; 50];
165
166        let mut n_read = udp_reader.read(&mut buf).unwrap();
167        assert_eq!(n_read, 50);
168        assert_eq!(&buf[0..n_read], datagram.as_slice());
169
170        n_read = udp_reader.read(&mut buf).unwrap();
171        assert_eq!(n_read, 50);
172        assert_eq!(&buf[0..n_read], datagram.as_slice());
173    }
174}
mavlink_core/connection/udp/sync.rs

mavlink_core/connection/udp/
sync.rs
