From 3fa96320e0eb61e901c97ee185d29e11008373e4 Mon Sep 17 00:00:00 2001 From: unizippro Date: Thu, 4 Nov 2021 17:26:51 +0100 Subject: [PATCH] UDP full stack initial commit --- ublox-short-range/src/client.rs | 22 +++- ublox-short-range/src/wifi/udp_stack.rs | 150 +++++++++++++++++++++++- 2 files changed, 168 insertions(+), 4 deletions(-) diff --git a/ublox-short-range/src/client.rs b/ublox-short-range/src/client.rs index 099a081..cc55ecc 100644 --- a/ublox-short-range/src/client.rs +++ b/ublox-short-range/src/client.rs @@ -26,7 +26,7 @@ use embedded_time::duration::{Generic, Milliseconds}; use embedded_time::Clock; use ublox_sockets::{ tcp_listener::TcpListener, AnySocket, SocketSet, SocketType, TcpSocket, TcpState, UdpSocket, - UdpState, + udp_listener::UdpListener, UdpState, }; #[derive(PartialEq, Copy, Clone)] @@ -69,6 +69,7 @@ where pub(crate) timer: CLK, pub(crate) edm_mapping: EdmMap, pub(crate) tcp_listener: TcpListener<3, N>, + pub(crate) udp_listener: UdpListener<3, N>, } impl UbloxClient @@ -93,6 +94,7 @@ where timer, edm_mapping: EdmMap::new(), tcp_listener: TcpListener::new(), + udp_listener: UdpListener::new(), } } @@ -232,6 +234,7 @@ where if let Some(ref mut sockets) = self.sockets.as_deref_mut() { let dns_state = &mut self.dns_state; let tcp_listener = &mut self.tcp_listener; + let udp_listener = &mut self.udp_listener; let edm_mapping = &mut self.edm_mapping; let wifi_connection = self.wifi_connection.as_mut(); let ts = self.timer.try_now().map_err(|_| Error::Timer)?; @@ -259,6 +262,9 @@ where if let Some(queue) = tcp_listener.incoming(event.local_port) { queue.enqueue((event.handle, remote)).unwrap(); return true; + } else if let Some(queue) = udp_listener.incoming(event.local_port) { + queue.enqueue((event.handle, remote)).unwrap(); + return true; } else { match event.protocol { IPProtocol::TCP => { @@ -438,6 +444,13 @@ where } _ => {} } + } else if let Some(queue) = udp_listener.incoming(event.local_port) { + match queue.peek() { + Some((h, remote)) if remote == &endpoint => { + return edm_mapping.insert(event.channel_id, *h).is_ok(); + } + _ => {} + } } sockets @@ -491,6 +504,13 @@ where } _ => {} } + } else if let Some(queue) = udp_listener.incoming(event.local_port) { + match queue.peek() { + Some((h, remote)) if remote == &endpoint => { + return edm_mapping.insert(event.channel_id, *h).is_ok(); + } + _ => {} + } } sockets diff --git a/ublox-short-range/src/wifi/udp_stack.rs b/ublox-short-range/src/wifi/udp_stack.rs index 333c56f..2cbcb86 100644 --- a/ublox-short-range/src/wifi/udp_stack.rs +++ b/ublox-short-range/src/wifi/udp_stack.rs @@ -1,6 +1,9 @@ use crate::{ command::data_mode::*, - command::edm::{EdmAtCmdWrapper, EdmDataCommand}, + command::{ + data_mode::types::{UDPBehaviour, ServerConfig, IPVersion}, + edm::{EdmAtCmdWrapper, EdmDataCommand}, + }, wifi::peer_builder::PeerUrlBuilder, UbloxClient, }; @@ -12,7 +15,7 @@ use embedded_time::{ Clock, }; -use embedded_nal::UdpClientStack; +use embedded_nal::{UdpClientStack, UdpFullStack}; use ublox_sockets::{Error, SocketHandle, UdpSocket, UdpState}; use super::EGRESS_CHUNK_SIZE; @@ -37,6 +40,7 @@ where if let Ok(ts) = self.timer.try_now() { // Check if there are any sockets closed by remote, and close it // if it has exceeded its timeout, in order to recycle it. + // TODO Is this connect? if sockets.recycle(&ts) { return Err(Error::SocketSetFull); } @@ -165,7 +169,37 @@ where socket: &mut Self::UdpSocket, buffer: &mut [u8], ) -> nb::Result<(usize, SocketAddr), Self::Error> { - if let Some(ref mut sockets) = self.sockets { + + // Handle reciving for udp server ports + if self.udp_listener.is_bound(*socket){ + if !self + .udp_listener + .available(*socket) + .unwrap_or(false) + { + return Err(nb::Error::WouldBlock); + } + + let (handle, remote) = self + .udp_listener + .accept(*socket) + .map_err(|_| Error::NotBound)?; + + + if let Some(ref mut sockets) = self.sockets { + let mut udp = sockets + .get::>(handle) + .map_err(Self::Error::from)?; + + let bytes = udp.recv_slice(buffer).map_err(Self::Error::from)?; + self.udp_listener.outgoing_connection(handle, remote).map_err(|_| Error::Illegal)?; + Ok((bytes, remote)) + } else { + Err(Error::Illegal.into()) + } + + // Handle reciving for udp normal ports + } else if let Some(ref mut sockets) = self.sockets { let mut udp = sockets .get::>(*socket) .map_err(Self::Error::from)?; @@ -198,3 +232,113 @@ where } } } + +/// UDP Full Stack +/// +/// Limitations: +/// One can only send to Socket addresses that have send data first. +/// One can only use send_to once after reciving data once. +/// +impl UdpFullStack for UbloxClient +where + C: atat::AtatClient, + CLK: Clock, + RST: OutputPin, + Generic: TryInto, +{ + fn bind(&mut self, socket: &mut Self::UdpSocket, local_port: u16) -> Result<(), Self::Error> { + if self.sockets.is_none() { + return Err(Error::Illegal); + } + + defmt::debug!("[UDP] bind socket: {:?}", socket); + if let Some(ref con) = self.wifi_connection { + if !self.initialized || !con.is_connected() { + return Err(Error::Illegal); + } + } else { + return Err(Error::Illegal); + } + + self.send_internal( + &EdmAtCmdWrapper(ServerConfiguration { + id: 1, + server_config: ServerConfig::UDP(local_port, UDPBehaviour::AutoConnect, IPVersion::IPv4), + }), + false, + ) + .map_err(|_| Error::Unaddressable)?; + + self.udp_listener + .bind(*socket, local_port) + .map_err(|_| Error::Illegal)?; + + Ok(()) + } + + fn send_to( + &mut self, + socket: &mut Self::UdpSocket, + remote: SocketAddr, + buffer: &[u8], + ) -> nb::Result<(), Self::Error>{ + + if let Ok(Some(socket)) = self.udp_listener.get_outgoing(remote){ + if let Some(ref mut sockets) = self.sockets { + if let Some(ref con) = self.wifi_connection { + if !self.initialized || !con.is_connected() { + return Err(Error::Illegal.into()); + } + } else { + return Err(Error::Illegal.into()); + } + + let udp = sockets + .get::>(socket) + .map_err(Self::Error::from)?; + + if !udp.is_open() { + return Err(Error::SocketClosed.into()); + } + + self.spin().map_err(|_| nb::Error::Other(Error::Illegal))?; + + let channel = *self + .edm_mapping + .channel_id(&socket) + .ok_or(nb::Error::WouldBlock)?; + + for chunk in buffer.chunks(EGRESS_CHUNK_SIZE) { + self.send_internal( + &EdmDataCommand { + channel, + data: chunk, + }, + true, + ) + .map_err(|_| nb::Error::Other(Error::Unaddressable))?; + } + self.close(socket).unwrap(); + Ok(()) + } else { + Err(Error::Illegal.into()) + } + } else { + Err(Error::Illegal.into()) + } + + + ////// Do with URC + // Crate a new SocketBuffer allocation for the incoming connection + // let mut tcp = self + // .sockets + // .as_mut() + // .ok_or(Error::Illegal)? + // .get::>(data_socket) + // .map_err(Self::Error::from)?; + + // tcp.update_handle(handle); + // tcp.set_state(TcpState::Connected(remote.clone())); + + } +} \ No newline at end of file