wip: add ptp impl to embassy-net

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]
 

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<crate::SocketHandle, crate::TimeEntry>,
 }
 
 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));
     }
 }

embassy-net/src/lib.rs

@@ -101,6 +101,8 @@ pub struct StackResources<const SOCK: usize> {
     // 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<heapless::LinearMap<SocketHandle, TimeEntry, SOCK>>,
 }
 
 #[cfg(feature = "dhcpv4-hostname")]
@@ -124,6 +126,8 @@ impl<const SOCK: usize> StackResources<SOCK> {
             },
             #[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<SocketHandle, TimeEntry>,
+    #[cfg(feature = "ptp")]
+    next_id: u16,
+}
+
+#[cfg(feature = "ptp")]
+struct TimeEntry {
+    #[allow(unused)]
+    id: u16,
+    did: Option<u8>,
+    time: Option<embassy_net_driver::Timestamp>,
+    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<K, V> {
+    #[allow(dead_code)]
+    fn insert(&mut self, key: K, val: V) -> Result<Option<V>, ()>;
+    #[allow(dead_code)]
+    fn get_mut(&mut self, key: &K) -> Option<&mut V>;
+    #[allow(dead_code)]
+    fn remove(&mut self, key: &K) -> Option<V>;
+    #[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<K: Eq + Copy, V, const N: usize> LinearMap<K, V> for heapless::LinearMap<K, V, N> {
+    fn insert(&mut self, key: K, val: V) -> Result<Option<V>, ()> {
+        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<V> {
+        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<T: ?Sized>(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;

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<T>(
+        &self,
+        buf: &[u8],
+        remote_endpoint: T,
+    ) -> Result<embassy_net_driver::Timestamp, SendError>
+    where
+        T: Into<UdpMetadata>,
+    {
+        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.