diff --git a/embassy-net/Cargo.toml b/embassy-net/Cargo.toml index d5544e3de2..383a6b2ba7 100644 --- a/embassy-net/Cargo.toml +++ b/embassy-net/Cargo.toml @@ -109,6 +109,8 @@ std = ["smoltcp/std"] alloc = ["smoltcp/alloc"] ## Enable smoltcp packetmeta-id feature packetmeta-id = [ "smoltcp/packetmeta-id", "embassy-net-driver/packetmeta-id" ] +## Enable ptp support +ptp = ["packetmeta-id"] [dependencies] diff --git a/embassy-net/src/driver_util.rs b/embassy-net/src/driver_util.rs index 4db57aa631..e689476ae5 100644 --- a/embassy-net/src/driver_util.rs +++ b/embassy-net/src/driver_util.rs @@ -13,6 +13,9 @@ where pub inner: &'d mut T, pub medium: Medium, pub tx_exhausted: bool, + #[cfg(feature = "ptp")] + #[allow(unused)] + pub times: &'d mut dyn crate::LinearMap, } impl<'d, 'c, T> phy::Device for DriverAdapter<'d, 'c, T> @@ -120,6 +123,7 @@ where } fn set_meta(&mut self, meta: phy::PacketMeta) { + // TODO: when called with a nonzero ID, associate the inner ID of this token with the ID if set. self.0.set_meta(into_embassy_net_meta(meta)); } } diff --git a/embassy-net/src/lib.rs b/embassy-net/src/lib.rs index a89662c1c4..42ebd9fe23 100644 --- a/embassy-net/src/lib.rs +++ b/embassy-net/src/lib.rs @@ -101,6 +101,8 @@ pub struct StackResources { // undersized buffer never corrupts the IP configuration, it only drops the extra options. #[cfg(feature = "dhcpv4-ntp")] dhcp_rx_buffer: MaybeUninit<[u8; DHCP_RX_BUFFER_SIZE]>, + #[cfg(feature = "ptp")] + times: MaybeUninit>, } #[cfg(feature = "dhcpv4-hostname")] @@ -124,6 +126,8 @@ impl StackResources { }, #[cfg(feature = "dhcpv4-ntp")] dhcp_rx_buffer: MaybeUninit::uninit(), + #[cfg(feature = "ptp")] + times: MaybeUninit::uninit(), } } } @@ -332,6 +336,68 @@ pub(crate) struct Inner { hostname: *mut HostnameResources, #[cfg(feature = "dhcpv4-ntp")] dhcp_rx_buffer: *mut [u8], + #[cfg(feature = "ptp")] + times: &'static mut dyn LinearMap, + #[cfg(feature = "ptp")] + next_id: u16, +} + +#[cfg(feature = "ptp")] +struct TimeEntry { + #[allow(unused)] + id: u16, + did: Option, + time: Option, + waker: WakerRegistration, +} + +#[cfg(feature = "ptp")] +impl TimeEntry { + pub const fn new(id: u16) -> Self { + Self { + id, + did: None, + time: None, + waker: WakerRegistration::new(), + } + } +} + +#[cfg(feature = "ptp")] +trait LinearMap { + #[allow(dead_code)] + fn insert(&mut self, key: K, val: V) -> Result, ()>; + #[allow(dead_code)] + fn get_mut(&mut self, key: &K) -> Option<&mut V>; + #[allow(dead_code)] + fn remove(&mut self, key: &K) -> Option; + #[allow(dead_code)] + fn iter(&self) -> heapless::linear_map::Iter<'_, K, V>; + #[allow(dead_code)] + fn iter_mut(&mut self) -> heapless::linear_map::IterMut<'_, K, V>; +} + +#[cfg(feature = "ptp")] +impl LinearMap for heapless::LinearMap { + fn insert(&mut self, key: K, val: V) -> Result, ()> { + self.insert(key, val).map_err(|_| ()) + } + + fn get_mut(&mut self, key: &K) -> Option<&mut V> { + self.get_mut(key) + } + + fn remove(&mut self, key: &K) -> Option { + self.remove(key) + } + + fn iter(&self) -> heapless::linear_map::Iter<'_, K, V> { + self.iter() + } + + fn iter_mut(&mut self) -> heapless::linear_map::IterMut<'_, K, V> { + self.iter_mut() + } } fn _assert_covariant<'a, 'b: 'a>(x: Stack<'b>) -> Stack<'a> { @@ -353,6 +419,9 @@ pub fn new<'d, D: Driver, const SOCK: usize>( iface_cfg.slaac = matches!(config.ipv6, ConfigV6::Slaac); } + #[cfg(feature = "ptp")] + let times = resources.times.write(heapless::LinearMap::new()); + #[allow(unused_mut)] let mut iface = Interface::new( iface_cfg, @@ -361,6 +430,8 @@ pub fn new<'d, D: Driver, const SOCK: usize>( cx: None, medium, tx_exhausted: false, + #[cfg(feature = "ptp")] + times, }, instant_to_smoltcp(Instant::now()), ); @@ -369,6 +440,11 @@ pub fn new<'d, D: Driver, const SOCK: usize>( core::mem::transmute(x) } + #[cfg(feature = "ptp")] + unsafe fn transmute_static(x: &mut T) -> &'static mut T { + core::mem::transmute(x) + } + let sockets = resources.sockets.write([SocketStorage::EMPTY; SOCK]); #[allow(unused_mut)] let mut sockets: SocketSet<'static> = SocketSet::new(unsafe { transmute_slice(sockets) }); @@ -407,6 +483,10 @@ pub fn new<'d, D: Driver, const SOCK: usize>( hostname: &mut resources.hostname, #[cfg(feature = "dhcpv4-ntp")] dhcp_rx_buffer: resources.dhcp_rx_buffer.write([0; DHCP_RX_BUFFER_SIZE]) as *mut [u8], + #[cfg(feature = "ptp")] + times: unsafe { transmute_static(times) }, + #[cfg(feature = "ptp")] + next_id: 0, }; #[cfg(feature = "proto-ipv4")] @@ -942,6 +1022,16 @@ impl Inner { } } + #[cfg(feature = "ptp")] + while let Some((id, timestamp)) = driver.poll_timestamp(cx) { + for (_handle, entry) in self.times.iter_mut() { + if entry.did.is_some_and(|did| id == did) && entry.time.is_none() { + entry.time = Some(timestamp); + entry.waker.wake(); + } + } + } + // Update link up let old_link_up = self.link_up; self.link_up = driver.link_state(cx) == LinkState::Up; @@ -967,6 +1057,8 @@ impl Inner { inner: driver, medium, tx_exhausted: false, + #[cfg(feature = "ptp")] + times: self.times, }; self.iface.poll(timestamp, &mut smoldev, &mut self.sockets); let tx_exhausted = smoldev.tx_exhausted; diff --git a/embassy-net/src/udp.rs b/embassy-net/src/udp.rs index 93f298ee1a..8abd5f0874 100644 --- a/embassy-net/src/udp.rs +++ b/embassy-net/src/udp.rs @@ -161,6 +161,17 @@ impl<'a> UdpSocket<'a> { }) } + #[cfg(feature = "ptp")] + fn with_id(&self, mut remote_endpoint: UdpMetadata) -> (UdpMetadata, u16) { + self.stack.with_mut(|i| { + i.next_id = i.next_id.wrapping_add(1); + remote_endpoint.meta.id = i.next_id as u32; + i.times.insert(self.handle, crate::TimeEntry::new(i.next_id)).unwrap(); + + (remote_endpoint, i.next_id) + }) + } + /// Wait until the socket becomes readable. /// /// A socket is readable when a packet has been received, or when there are queued packets in @@ -311,6 +322,32 @@ impl<'a> UdpSocket<'a> { }) } + #[cfg(feature = "ptp")] + /// Send a datagram to the specified remote endpoint and retreive the timestamp in which it has been sent. + /// + /// This method will wait until the datagram has been sent. + /// + /// If the socket's send buffer is too small to fit `buf`, this method will return `Err(SendError::PacketTooLarge)` + /// + /// When the remote endpoint is not reachable, this method will return `Err(SendError::NoRoute)` + pub async fn send_to_timed( + &self, + buf: &[u8], + remote_endpoint: T, + ) -> Result + where + T: Into, + { + let remote_endpoint: UdpMetadata = remote_endpoint.into(); + let (remote_endpoint, _id) = self.with_id(remote_endpoint); + + poll_fn(move |cx| self.poll_send_to(buf, remote_endpoint, cx)).await?; + + // TODO: wait for the timestamp to be entered + + todo!() + } + /// Send a datagram to the specified remote endpoint. /// /// This method will wait until the datagram has been sent.