diff --git a/Cargo.toml b/Cargo.toml index 050a57eb..03ba7b99 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -48,7 +48,7 @@ fdcan = { version = "0.2", optional = true } embedded-storage = "0.3" [dependencies.smoltcp] -version = "0.10.0" +version = "0.11.0" default-features = false features = ["medium-ethernet", "proto-ipv4", "socket-raw"] optional = true @@ -83,7 +83,7 @@ tinybmp = "0.5" embedded-graphics = "0.8" [dev-dependencies.smoltcp] -version = "0.10.0" +version = "0.11.0" default-features = false features = ["medium-ethernet", "proto-ipv4", "proto-ipv6", "socket-raw"] @@ -100,6 +100,7 @@ gpio-h72 = [] gpio-h747 = [] gpio-h7a2 = [] +ptp = ["smoltcp/packetmeta-id"] dsi = [] cm4 = [] cm7 = [] diff --git a/src/ethernet/eth.rs b/src/ethernet/eth.rs index d786dac0..a419499a 100644 --- a/src/ethernet/eth.rs +++ b/src/ethernet/eth.rs @@ -23,6 +23,8 @@ //! [notes]: https://github.com/quartiq/stabilizer/commit/ab1735950b2108eaa8d51eb63efadcd2e25c35c4 use core::ptr; +#[cfg(feature = "ptp")] +use core::task::Poll; use crate::rcc::{rec, CoreClocks, ResetEnable}; use crate::stm32; @@ -52,7 +54,10 @@ mod emac_consts { pub const EMAC_DES3_LD: u32 = 0x1000_0000; pub const EMAC_DES3_ES: u32 = 0x0000_8000; pub const EMAC_TDES2_IOC: u32 = 0x8000_0000; + pub const EMAC_TDES2_TTSE: u32 = 0x4000_0000; + pub const EMAC_TDES3_TTSS: u32 = 0x0002_0000; pub const EMAC_RDES3_IOC: u32 = 0x4000_0000; + pub const EMAC_RDES1_TSA: u32 = 0x0000_4000; pub const EMAC_RDES3_PL: u32 = 0x0000_7FFF; pub const EMAC_RDES3_BUF1V: u32 = 0x0100_0000; pub const EMAC_TDES2_B1L: u32 = 0x0000_3FFF; @@ -60,6 +65,75 @@ mod emac_consts { } use self::emac_consts::*; +#[cfg(feature = "ptp")] +use crate::ptp::{Timestamp, MAX_PTP_FOLLOWERS, PTP_MAX_SIZE}; + +/// A struct to store the PTP frame and clock_identity +#[derive(Clone, Copy)] +#[cfg(feature = "ptp")] +pub struct PtpFrame { + pub buffer: [u8; PTP_MAX_SIZE], + pub clock_identity: u64, + pub meta_option: Option, +} + +#[cfg(feature = "ptp")] +impl PtpFrame{ + pub const fn new() -> Self { + Self { + buffer: [0u8; PTP_MAX_SIZE], + clock_identity: 0, + meta_option: None, + } + } +} + +/// A struct to store the packet meta and the timestamp +#[derive(Clone, Copy)] +#[repr(C, packed)] +#[cfg(feature = "ptp")] +pub struct PacketInfo { + packet_meta: Option, + timestamp: Option, +} + +#[derive(Clone, Copy, PartialEq)] +#[cfg(feature = "ptp")] +pub struct PacketMetaNotFound; + +#[cfg(feature = "ptp")] +impl Default for PacketInfo { + fn default() -> Self { + Self::new() + } +} + +#[cfg(feature = "ptp")] +impl PacketInfo { + pub const fn new() -> Self { + Self { + packet_meta: None, + timestamp: None, + } + } + + pub fn set_meta_and_clear_ts(&mut self, packet_meta: Option) { + self.packet_meta = packet_meta; + self.timestamp = None; + } + + pub fn meta(&self) -> Option { + self.packet_meta + } + + pub fn ts(&self) -> Option { + self.timestamp + } + + pub fn set_ts(&mut self, timestamp: Option) { + self.timestamp = timestamp; + } +} /// Transmit Descriptor representation /// /// * tdes0: transmit buffer address @@ -99,6 +173,8 @@ impl TDes { struct TDesRing { td: [TDes; TD], tbuf: [[u32; ETH_BUF_SIZE / 4]; TD], + #[cfg(feature = "ptp")] + tinfo: [PacketInfo; TD], tdidx: usize, } @@ -112,6 +188,8 @@ impl TDesRing { tdes3: 0, }; TD], tbuf: [[0; ETH_BUF_SIZE / 4]; TD], + #[cfg(feature = "ptp")] + tinfo: [PacketInfo::new(); TD], tdidx: 0, } } @@ -154,7 +232,7 @@ impl TDesRing { // Read format self.td[x].tdes0 = address; // Buffer 1 self.td[x].tdes1 = 0; // Not used - assert!(self.td[x].tdes2 & !EMAC_TDES2_B1L == 0); // Not used + assert!(self.td[x].tdes2 & !(EMAC_TDES2_B1L | EMAC_TDES2_TTSE)== 0); // Not used assert!(self.td[x].tdes2 & EMAC_TDES2_B1L > 0); // Length must be valid self.td[x].tdes3 = 0; self.td[x].tdes3 |= EMAC_DES3_FD; // FD: Contains first buffer of packet @@ -192,6 +270,50 @@ impl TDesRing { let addr = ptr::addr_of_mut!(self.tbuf[x]) as *mut _; core::slice::from_raw_parts_mut(addr, len) } + + #[cfg(feature = "ptp")] + pub fn poll_timestamp( + &self, + packet_meta: &smoltcp::phy::PacketMeta, + ) -> Poll, PacketMetaNotFound>> { + for (idx, info) in self.tinfo.into_iter().enumerate() { + if match info.meta() { + Some(smoltcp::phy::PacketMeta {id, ..}) => id == packet_meta.id, + _ => false, + } { + if self.td[idx].available() { + let timestamp = self.get_timestamp(idx); + return Poll::Ready(Ok(timestamp)) + } else { + return Poll::Pending + } + } + } + Poll::Ready(Err(PacketMetaNotFound)) + } + + #[cfg(feature = "ptp")] + pub fn enable_ptp_with_id(&mut self, packet_meta: Option) { + if packet_meta.is_some() { + self.td[self.tdidx].tdes2 |= EMAC_TDES2_TTSE; + } + self.tinfo[self.tdidx].set_meta_and_clear_ts(packet_meta); + } + + #[cfg(feature = "ptp")] + pub fn get_timestamp(&self, idx: usize) -> Option { + let contains_timestamp = + (self.td[idx].tdes3 & EMAC_TDES3_TTSS) == EMAC_TDES3_TTSS; + let owned = (self.td[idx].tdes3 & EMAC_DES3_OWN) == EMAC_DES3_OWN; + let is_last = (self.td[idx].tdes3 & EMAC_DES3_LD) == EMAC_DES3_LD; + + if !owned && contains_timestamp && is_last { + let (low, high) = (self.td[idx].tdes0, self.td[idx].tdes1); + Some(Timestamp::from_parts(high, low)) + } else { + None + } + } } /// Receive Descriptor representation @@ -244,6 +366,8 @@ impl RDes { struct RDesRing { rd: [RDes; RD], rbuf: [[u32; ETH_BUF_SIZE / 4]; RD], + #[cfg(feature = "ptp")] + rinfo: [PacketInfo; RD], rdidx: usize, } @@ -257,6 +381,8 @@ impl RDesRing { rdes3: 0, }; RD], rbuf: [[0; ETH_BUF_SIZE / 4]; RD], + #[cfg(feature = "ptp")] + rinfo: [PacketInfo::new(); RD], rdidx: 0, } } @@ -344,6 +470,76 @@ impl RDesRing { let len = core::cmp::min(len, ETH_BUF_SIZE); core::slice::from_raw_parts_mut(addr, len) } + + #[cfg(feature = "ptp")] + pub fn timestamp(&self, packet_meta: &smoltcp::phy::PacketMeta) -> Result, PacketMetaNotFound> { + for info in self.rinfo { + if match info.meta() { + Some(smoltcp::phy::PacketMeta {id, ..}) => id == packet_meta.id, + _ => false, + } { + return Ok(info.ts()) + } + } + Err(PacketMetaNotFound) + } + + #[cfg(feature = "ptp")] + pub fn set_meta_and_clear_ts(&mut self, packet_meta: Option) { + self.rinfo[self.rdidx].set_meta_and_clear_ts(packet_meta) + } + + #[cfg(feature = "ptp")] + pub fn was_timestamped(&self) -> bool { + ((self.rd[self.rdidx + 1].rdes3 & EMAC_DES3_CTXT) == EMAC_DES3_CTXT) // next one is context! + && ((self.rd[self.rdidx].rdes1 & EMAC_RDES1_TSA) == EMAC_RDES1_TSA) // + && (self.rd[self.rdidx].rdes3 & EMAC_DES3_LD) == EMAC_DES3_LD // is last + } + + #[cfg(feature = "ptp")] + pub fn read_timestamp_from_next(&self) -> Option { + if !((self.rd[self.rdidx + 1].rdes3 & EMAC_DES3_OWN) == EMAC_DES3_OWN) { // if not is owned + let (high, low) = (self.rd[self.rdidx + 1].rdes1, self.rd[self.rdidx+1].rdes0); + Some(Timestamp::from_parts(high, low)) + } else { + None + } + } + + #[cfg(feature = "ptp")] + pub fn attach_timestamp(&mut self, timestamp: Option) { + self.rinfo[self.rdidx].set_ts(timestamp); + } + + #[cfg(feature = "ptp")] + pub fn release_timestamp_desc(&mut self) { + let x = self.rdidx + 1; + + assert!(self.rd[x].rdes3 & EMAC_DES3_OWN == 0); // Owned by us + + let address = ptr::addr_of!(self.rbuf[x]) as u32; + + // Read format + self.rd[x].rdes0 = address; // Buffer 1 + self.rd[x].rdes1 = 0; // Reserved + self.rd[x].rdes2 = 0; // Marked as invalid + self.rd[x].rdes3 = 0; + self.rd[x].rdes3 |= EMAC_DES3_OWN; // Give the DMA engine ownership + self.rd[x].rdes3 |= EMAC_RDES3_BUF1V; // BUF1V: 1st buffer address is valid + self.rd[x].rdes3 |= EMAC_RDES3_IOC; // IOC: Interrupt on complete + + // Ensure changes to the descriptor are committed before + // DMA engine sees tail pointer store + // cortex_m::asm::dsb(); + + // // Move the tail pointer (TPR) to this descriptor + // unsafe { + // let dma = &*stm32::ETHERNET_DMA::ptr(); + // dma.dmacrx_dtpr + // .write(|w| w.bits(&(self.rd[x]) as *const _ as u32)); + // } + + } } pub struct DesRing { @@ -370,6 +566,38 @@ impl Default for DesRing { pub struct EthernetDMA { ring: &'static mut DesRing, eth_dma: stm32::ETHERNET_DMA, + + #[cfg(feature = "ptp")] + packet_meta_counter: u32, + #[cfg(feature = "ptp")] + pub ptp_frame_buffer: [PtpFrame; MAX_PTP_FOLLOWERS], + #[cfg(feature = "ptp")] + pub write_pos: usize, +} + +#[cfg(feature = "ptp")] +impl EthernetDMA { + pub fn next_packet_meta(&mut self) -> smoltcp::phy::PacketMeta { + let mut meta = smoltcp::phy::PacketMeta::default(); + meta.id = self.packet_meta_counter; + self.packet_meta_counter += 1; + meta + } + + pub fn rx_timestamp( + &self, + packet_meta: &smoltcp::phy::PacketMeta, + ) -> Result, PacketMetaNotFound> { + self.ring.rx.timestamp(packet_meta) + } + + pub fn poll_tx_timestamp( + &self, + packet_meta: &smoltcp::phy::PacketMeta, + ) -> Poll, PacketMetaNotFound>> { + self.ring.tx.poll_timestamp(packet_meta) + } + } /// @@ -539,6 +767,12 @@ pub unsafe fn new_unchecked( | (u32::from(mac_addr.0[3]) << 24), ) }); + + #[cfg(feature = "ptp")] + let pm_value = true; + #[cfg(not(feature = "ptp"))] + let pm_value = false; + // frame filter register eth_mac.macpfr.modify(|_, w| { w.dntu() @@ -558,7 +792,7 @@ pub unsafe fn new_unchecked( .dbf() .clear_bit() .pm() - .clear_bit() + .bit(pm_value) .daif() .clear_bit() .hmc() @@ -643,9 +877,9 @@ pub unsafe fn new_unchecked( eth_dma.dmamr.modify(|_, w| { w.intm() .bits(0b00) - // Rx Tx priority ratio 1:1 + // Rx Tx priority ratio 2:1 .pr() - .bits(0b000) + .bits(0b001) .txpr() .clear_bit() .da() @@ -736,7 +970,16 @@ pub unsafe fn new_unchecked( clock_range: csr_clock_range, }; - let dma = EthernetDMA { ring, eth_dma }; + let dma = EthernetDMA { + ring, + eth_dma, + #[cfg(feature = "ptp")] + packet_meta_counter: 0, + #[cfg(feature = "ptp")] + ptp_frame_buffer: [PtpFrame::new(); MAX_PTP_FOLLOWERS], + #[cfg(feature = "ptp")] + write_pos: 0, + }; (dma, mac) } @@ -796,7 +1039,11 @@ impl StationManagement for EthernetMAC { } /// Define TxToken type and implement consume method -pub struct TxToken<'a, const TD: usize>(&'a mut TDesRing); +pub struct TxToken<'a, const TD: usize> { + des_ring: &'a mut TDesRing, + #[cfg(feature = "ptp")] + packet_meta: Option, +} impl<'a, const TD: usize> phy::TxToken for TxToken<'a, TD> { fn consume(self, len: usize, f: F) -> R @@ -804,25 +1051,74 @@ impl<'a, const TD: usize> phy::TxToken for TxToken<'a, TD> { F: FnOnce(&mut [u8]) -> R, { assert!(len <= ETH_BUF_SIZE); - - let result = f(unsafe { self.0.buf_as_slice_mut(len) }); - self.0.release(); + let result = f(unsafe { self.des_ring.buf_as_slice_mut(len) }); + #[cfg(feature = "ptp")] + self.des_ring.enable_ptp_with_id(self.packet_meta); + self.des_ring.release(); result } + + #[cfg(feature = "ptp")] + fn set_meta(&mut self, packet_meta: smoltcp::phy::PacketMeta) { + self.packet_meta = Some(packet_meta) + } } /// Define RxToken type and implement consume method -pub struct RxToken<'a, const RD: usize>(&'a mut RDesRing); +pub struct RxToken<'a, const RD: usize> { + des_ring: &'a mut RDesRing, + #[cfg(feature = "ptp")] + packet_meta: smoltcp::phy::PacketMeta, + #[cfg(feature = "ptp")] + ptp_frame: &'a mut PtpFrame, +} impl<'a, const RD: usize> phy::RxToken for RxToken<'a, RD> { fn consume(self, f: F) -> R where F: FnOnce(&mut [u8]) -> R, { - let result = f(unsafe { self.0.buf_as_slice_mut() }); - self.0.release(); + #[cfg(feature = "ptp")] + { + // if running state is NOT RUNNING + // demand poll!!!! otherwise itll stop when timestamp blocking and not start again?!?! + let eth_dma = unsafe{&*stm32::ETHERNET_DMA::ptr()}; + let status = eth_dma.dmadsr.read().rps0().bits(); + if status == 0b100 || status == 0b000 { //receive descriptor unavailable! + // demand poll!!! + eth_dma + .dmacrx_dtpr + .modify(|r, w| unsafe { w.bits(r.bits()) }); + } + self.des_ring.set_meta_and_clear_ts(Some(self.packet_meta)); + if self.des_ring.was_timestamped() { + let timestamp = self.des_ring.read_timestamp_from_next(); + self.des_ring.attach_timestamp(timestamp); + // self.des_ring.release_timestamp_desc(); + } + } + + let buf = unsafe { self.des_ring.buf_as_slice_mut() }; + #[cfg(feature = "ptp")] + { + let ethertype = u16::from_be_bytes(buf[12..14].try_into().unwrap()); + if ethertype == 0x88F7 { + let packet_buf = &buf[14..]; + let clock_identity = u64::from_be_bytes(packet_buf[20..28].try_into().unwrap()); + self.ptp_frame.buffer[0..core::cmp::min(packet_buf.len(), PTP_MAX_SIZE)].copy_from_slice(&packet_buf[0..core::cmp::min(packet_buf.len(), PTP_MAX_SIZE)]); + self.ptp_frame.meta_option = Some(self.packet_meta); + self.ptp_frame.clock_identity = clock_identity; + } + } + let result = f(buf); + self.des_ring.release(); result } + + #[cfg(feature = "ptp")] + fn meta(&self) -> smoltcp::phy::PacketMeta { + self.packet_meta + } } /// Implement the smoltcp Device interface @@ -851,7 +1147,27 @@ impl phy::Device for EthernetDMA { } if self.ring.rx.available() && self.ring.tx.available() { - Some((RxToken(&mut self.ring.rx), TxToken(&mut self.ring.tx))) + #[cfg(feature = "ptp")] + let rx_packet_meta = self.next_packet_meta(); + let tokens = Some(( + RxToken { + des_ring: &mut self.ring.rx, + #[cfg(feature = "ptp")] + packet_meta: rx_packet_meta, + #[cfg(feature = "ptp")] + ptp_frame: &mut self.ptp_frame_buffer[self.write_pos], + }, + TxToken { + des_ring: &mut self.ring.tx, + #[cfg(feature = "ptp")] + packet_meta: None + } + )); + #[cfg(feature = "ptp")] + { + self.write_pos = (self.write_pos + 1) % MAX_PTP_FOLLOWERS; + } + tokens } else { None } @@ -859,7 +1175,15 @@ impl phy::Device for EthernetDMA { fn transmit(&mut self, _timestamp: Instant) -> Option> { if self.ring.tx.available() { - Some(TxToken(&mut self.ring.tx)) + #[cfg(feature = "ptp")] + let tx_packet_meta = Some(self.next_packet_meta()); + Some( + TxToken { + des_ring: &mut self.ring.tx, + #[cfg(feature = "ptp")] + packet_meta: tx_packet_meta, + } + ) } else { None } @@ -884,6 +1208,7 @@ pub unsafe fn interrupt_handler() { eth_dma .dmacsr .write(|w| w.nis().set_bit().ri().set_bit().ti().set_bit()); + // .tbu().set_bit().rbu().set_bit().ais().set_bit() let _ = eth_dma.dmacsr.read(); let _ = eth_dma.dmacsr.read(); // Delay 2 peripheral clocks } @@ -902,4 +1227,5 @@ pub unsafe fn enable_interrupt() { eth_dma .dmacier .modify(|_, w| w.nie().set_bit().rie().set_bit().tie().set_bit()); + // .aie().set_bit().rbue().set_bit().tbue().set_bit() } diff --git a/src/ethernet/mod.rs b/src/ethernet/mod.rs index 12240d5f..14566c6c 100644 --- a/src/ethernet/mod.rs +++ b/src/ethernet/mod.rs @@ -41,7 +41,9 @@ pub mod phy { mod eth; pub use eth::{enable_interrupt, interrupt_handler, new, new_unchecked}; -pub use eth::{DesRing, EthernetDMA, EthernetMAC}; +pub use eth::{DesRing, EthernetDMA, EthernetMAC, TxToken}; +#[cfg(feature = "ptp")] +pub use eth::PtpFrame; /// Marks a set of pins used to communciate to a PHY with a Reduced Media /// Independent Interface (RMII) diff --git a/src/lib.rs b/src/lib.rs index 054a7f65..1904f8b7 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -142,6 +142,15 @@ pub use crate::stm32 as device; #[cfg_attr(docsrs, doc(cfg(feature = "rt")))] pub use crate::stm32::interrupt; +#[cfg(all( + feature = "device-selected", + feature = "ethernet", + not(feature = "rm0455"), + feature = "ptp" +))] +#[cfg_attr(docsrs, doc(cfg(feature = "ethernet")))] +pub mod ptp; + #[cfg(feature = "device-selected")] pub mod adc; #[cfg(all(feature = "device-selected", feature = "can"))] diff --git a/src/ptp/mod.rs b/src/ptp/mod.rs new file mode 100644 index 00000000..5d6b14c6 --- /dev/null +++ b/src/ptp/mod.rs @@ -0,0 +1,405 @@ +//! PTP access and configuration. +//! +//! See [`EthernetPTP`] for a more details. +//! This implementation is derived from 0BSD-relicensed work done by +//! Johannes Draaijer for the +//! [`stm32-eth`](https://github.com/stm32-rs/stm32-eth) project + +use crate::rcc::CoreClocks; +use smoltcp::phy::TxToken as smoltcp_TxToken; +use crate::ethernet::{EthernetDMA, TxToken, PtpFrame}; + +mod timestamp; +pub use timestamp::Timestamp; + +mod subseconds; +pub use subseconds::{ + Subseconds, NANOS_PER_SECOND, SUBSECONDS_PER_SECOND, SUBSECONDS_TO_SECONDS, +}; + +pub const MAX_PTP_FOLLOWERS: usize = 8; +pub const PTP_MAX_SIZE: usize = 76; + +/// Access to the IEEE 1508v2 PTP peripheral present on the ethernet peripheral. +/// +/// On STM32FXXX's, the PTP peripheral has/uses the following important parts: +/// * HCLK (the chip's high speed clock, configured externally). +/// * The global timestamp (`global_time`, a [`Timestamp`]). +/// * A subsecond increment register (`subsecond_increment`, a [`Subseconds`] with a value of 0 to 255, see [`EthernetPTP::subsecond_increment`]). +/// * An accumulator register (`accumulator`, an [`u32`]). +/// * An addend register (`addend`, an [`u32`], see [`EthernetPTP::addend`] and [`EthernetPTP::set_addend`]). +/// +/// To ensure that `global_time` advances at the correct rate, the system performs the following steps: +/// 1. On every clock of HCLK, `addend` is added to `accumulator`. +/// 2. If `accumulator` overflows during step 1, add `subsecond_increment` to `global_time`. +/// +/// When a new [`EthernetPTP`] is created, it is assumed that the frequency of HCLK is exactly correct. +/// Using HCLK, values for `subsecond_increment` and `addend` are calculated so that `global_time` represents +/// real-time. +/// +/// Subsequently, `addend` can be adjusted to compensate for possible errors in HCLK, using [`EthernetPTP::addend`] and [`EthernetPTP::set_addend`] +/// +/// To assess the correctness of the current speed at which `global_time` is running, one can use the +/// following equation: +/// +/// ```no_compile +/// clock_ratio = ((2^31 / subsecond_increment) / (HCLK_HZ * (addend / 2^32))) +/// ``` +/// Values greater than 1 indicate that the provided `HCLK_HZ` is less than the actual frequency of HCLK, which should +/// be compensated by increasing `addend`. Values less than 1 indicate that the provided `HCLK_HZ` is greater than the +/// actual frequency of HCLK, which should be compensated by decreasing `addend`. +/// +/// [`NonZeroU8`]: core::num::NonZeroU8 +pub struct EthernetPTP {} + +impl EthernetPTP { + /// # Safety + /// The reference to the registerblock obtained using this function + /// must _only_ be used to change strictly PTP related registers. + unsafe fn mac() -> &'static crate::stm32::ethernet_mac::RegisterBlock { + &*crate::stm32::ETHERNET_MAC::ptr() + } + + // Calculate the `addend` required for running `global_time` at + // the correct rate + const fn calculate_regs(hclk: u32) -> (Subseconds, u32) { + let half_hclk = hclk / 2; + + // Calculate the closest `subsecond_increment` we can use if we want to update at a + // frequency of `half_hclk` + let stssi = Subseconds::nearest_increment(half_hclk); + let half_rate_subsec_increment_hz = stssi.hertz(); + + // Calculate the `addend` required for running `global_time` at + // the correct rate, given that we increment `global_time` by `stssi` every + // time `accumulator` overflows. + let tsa = ((half_rate_subsec_increment_hz as u64 * u32::MAX as u64) + / hclk as u64) as u32; + (stssi, tsa) + } + + pub fn new( + clocks: CoreClocks, + // Note(_dma): this field exists to ensure that the PTP is not + // initialized before the DMA. If PTP is started before the DMA, + // it doesn't work. + _dma: &impl smoltcp::phy::Device, + ) -> Self { + let hclk = clocks.hclk().to_Hz(); + + let (stssi, tsa) = Self::calculate_regs(hclk); + + let mut me = { + let me = Self {}; + + // SAFETY: we only write to `mactscr` (timestamp control register) + let mac = unsafe { Self::mac() }; + + mac.mactscr.modify(|_, w| { + w + // Enable timestamp snapshots for all frames + .tsenall() + .set_bit() + // Enable fine-grain update mode + .tscfupdt() + .set_bit() + // Enable all timestamps + .tsena() + .set_bit() + // Tell MAC to overwrite non-read timestamps + .txtsstsm() + .set_bit() + }); + + // Set up the subsecond increment + mac.macssir + .write(|w| unsafe { w.ssinc().bits(stssi.raw() as u8) }); + + me + }; + + me.set_addend(tsa); + me.set_time(0, 0); + + me + } + + /// Get the configured subsecond increment. + pub fn subsecond_increment(&self) -> Subseconds { + // SAFETY: we only read `macssir` (subsecond register). + return Subseconds::new_unchecked(unsafe { + Self::mac().macssir.read().ssinc().bits() as u32 + }); + } + + /// Get the currently configured PTP clock addend. + pub fn addend(&self) -> u32 { + // SAFETY: we only read `mactsar` (timestamp addend register). + return unsafe { Self::mac().mactsar.read().bits() }; + } + + /// Set the PTP clock addend. + #[inline(always)] + pub fn set_addend(&mut self, rate: u32) { + { + // SAFETY: we only write to `mactsar` (timestamp addend register) + // and `mactscr` (timestamp control register) + let (mactsar, mactscr) = unsafe { + let mac = Self::mac(); + (&mac.mactsar, &mac.mactscr) + }; + + mactsar.write(|w| unsafe { w.tsar().bits(rate) }); + + while mactscr.read().tsaddreg().bit_is_set() {} + mactscr.modify(|_, w| w.tsaddreg().set_bit()); + while mactscr.read().tsaddreg().bit_is_set() {} + } + } + + /// Set the current time. + pub fn set_time(&mut self, seconds: u32, nanoseconds: u32) { + { + // SAFETY: we only write to `mactscr` (timestamp control register), `macstsur` + // (timestamp update seconds register) and `macstnur` (timestmap update subsecond/nanosecond + // register) + let (mactscr, macstsur, macstnur) = unsafe { + let mac = Self::mac(); + (&mac.mactscr, &mac.macstsur, &mac.macstnur) + }; + + macstsur.write(|w| unsafe { w.bits(seconds) }); + macstnur.write(|w| unsafe { w.bits(nanoseconds) }); + + while mactscr.read().tsinit().bit_is_set() {} + mactscr.modify(|_, w| w.tsinit().set_bit()); + while mactscr.read().tsinit().bit_is_set() {} + } + } + + /// Add the provided time to the current time, atomically. + /// + /// If `time` is negative, it will instead be subtracted from the + /// system time. + pub fn update_time(&mut self, time: Timestamp) { + let seconds = time.seconds(); + let subseconds = time.subseconds_signed(); + + { + // SAFETY: we only write to `mactscr` (timestamp control register), `macstsur` + // (timestamp update seconds register) and `macstnur` (timestmap update subsecond/nanosecond + // register) + let (mactscr, macstsur, macstnur) = unsafe { + let mac = Self::mac(); + (&mac.mactscr, &mac.macstsur, &mac.macstnur) + }; + + macstsur.write(|w| unsafe { w.bits(seconds) }); + macstnur.write(|w| unsafe { w.bits(subseconds) }); + + while mactscr.read().tsupdt().bit_is_set() {} + mactscr.modify(|_, w| w.tsupdt().set_bit()); + while mactscr.read().tsupdt().bit_is_set() {} + } + } + + /// Get the current time + pub fn now() -> Timestamp { + Self::get_time() + } + + /// Get the current time. + pub fn get_time() -> Timestamp { + let try_read_time = || { + let (seconds, subseconds, seconds_after) = { + // SAFETY: we only atomically read PTP registers. + let (macstsr, macstnr) = unsafe { + let mac = Self::mac(); + (&mac.macstsr, &mac.macstnr) + }; + + let seconds = macstsr.read().bits(); + let subseconds = macstnr.read().bits(); + let seconds2 = macstsr.read().bits(); + (seconds, subseconds, seconds2) + }; + + if seconds == seconds_after { + Ok(Timestamp::from_parts(seconds, subseconds)) + } else { + Err(()) + } + }; + + loop { + if let Ok(res) = try_read_time() { + return res; + } + } + } + + // /// Enable the PPS output on the provided pin. + // pub fn enable_pps

(&mut self, pin: P) -> P::Output + // where + // P: PPSPin, + // { + // pin.enable() + // } +} + +/// Setting and configuring target time interrupts on the STM32F107 does not +/// make any sense: we can generate the interrupt, but it is impossible to +/// clear the flag as the register required to do so does not exist. +impl EthernetPTP { + /// Configure the target time. + fn set_target_time(&mut self, timestamp: Timestamp) { + let (high, low) = (timestamp.seconds(), timestamp.subseconds_signed()); + + { + // SAFETY: we only write to `ppsttsr` (PPS target time seconds register) and + // `ppsttnr` (PPS target time subseconds register) + let (ppsttsr, ppsttnr) = unsafe { + let mac = Self::mac(); + (&mac.macppsttsr, &mac.macppsttnr) + }; + + ppsttsr.write(|w| unsafe { w.bits(high) }); + ppsttnr.write(|w| unsafe { w.bits(low) }); + } + } + + /// Configure the target time interrupt. + /// + /// You must call [`EthernetPTP::interrupt_handler`] in the `ETH` + /// interrupt to detect (and clear) the correct status bits. + pub fn configure_target_time_interrupt(&mut self, timestamp: Timestamp) { + self.set_target_time(timestamp); + } + + #[inline(always)] + fn read_and_clear_interrupt_flag() -> bool { + { + // SAFETY: we only read the ethernet ptp status register. + + let mac = unsafe { Self::mac() }; + + // Reading the register clears all of the bits in + // that register. + let tssr = mac.mactssr.read(); + let tstargt0 = tssr.tstargt0().bit_is_set(); + let tstrgterr0 = tssr.tstrgterr0().bit_is_set(); + + tstargt0 || tstrgterr0 + } + } + + /// Handle the PTP parts of the `ETH` interrupt. + /// + /// Returns a boolean indicating whether or not the interrupt + /// was caused by a Timestamp trigger and clears the interrupt + /// flag. + pub fn interrupt_handler() -> bool { + let is_tsint = { + // SAFETY: we only read from `macisr` (Interrupt status register) + let macisr = unsafe { &Self::mac().macisr }; + macisr.read().tsis().bit_is_set() + }; + + EthernetPTP::read_and_clear_interrupt_flag(); + + is_tsint + } + + /// Configure the PPS output frequency. + /// + /// The PPS output frequency becomes `2 ^ pps_freq`. `pps_freq` is + /// clamped to `[0..31]`. + pub fn set_pps_freq(&mut self, pps_freq: u8) { + let pps_freq = pps_freq.max(31); + + // SAFETY: we atomically write to the PTPPPSCR register, which is + // not read or written to anywhere else. The SVD files are incorrectly + // saying that the bits in this register are read-only. + { + // SAFETY: we only access and modify the `macppscr` (PPS Control register) + let macppscr = unsafe { &Self::mac().macppscr }; + + macppscr.modify(|_, w| w.ppsctrl().variant(pps_freq)); + } + } +} + +impl<'a> EthernetPTP { + pub fn get_frame_from(dma: &'a EthernetDMA, clock_identity: u64) -> Option { + let mut i = dma.write_pos; + loop { + if dma.ptp_frame_buffer[i].meta_option.is_some() { + if dma.ptp_frame_buffer[i].clock_identity == clock_identity { + return Some(dma.ptp_frame_buffer[i]); + } + } + i = (i + 1) % MAX_PTP_FOLLOWERS; + + if i == dma.write_pos { + break; + } + } + return None; + } + pub fn invalidate_frame_from(dma: &'a mut EthernetDMA, clock_identity: u64) { + let mut i = dma.write_pos; + loop { + if dma.ptp_frame_buffer[i].meta_option.is_some() { + if dma.ptp_frame_buffer[i].clock_identity == clock_identity { + dma.ptp_frame_buffer[i].meta_option = None; + return; + } + } + i = (i + 1) % MAX_PTP_FOLLOWERS; + + if i == dma.write_pos { + break; + } + } + } + + pub fn send_ptp_frame( + frame: &[u8], + tx_option: Option>, + meta: smoltcp::phy::PacketMeta, + ) { + if let Some(mut tx_token) = tx_option { + tx_token.set_meta(meta); + tx_token.consume(frame.len(), |buf| { + buf[..frame.len()].copy_from_slice(frame); + }); + } + } + +} + +#[cfg(all(test, not(target_os = "none")))] +mod test { + + use super::*; + + // Test that we get accurate addend and subsecond_increment values + // with the provided clock speeds. + #[test] + fn hclk_to_regs() { + for hclk_hz in (25..180).map(|v| v * 1_000_000) { + let (stssi, tsa) = EthernetPTP::calculate_regs(hclk_hz); + + let stssi = stssi.raw() as f64; + let tsa = tsa as f64; + + // calculate the clock ratio + let clock_ratio = (SUBSECONDS_PER_SECOND as f64 / stssi) + / (hclk_hz as f64 * (tsa / 0xFFFF_FFFFu32 as f64)); + + let ppm = (clock_ratio - 1f64) * 1_000_000f64; + + assert!(ppm <= 0.06, "{} at {}", ppm, hclk_hz); + } + } +} diff --git a/src/ptp/subseconds.rs b/src/ptp/subseconds.rs new file mode 100644 index 00000000..74df5e93 --- /dev/null +++ b/src/ptp/subseconds.rs @@ -0,0 +1,174 @@ +//! This implementation is derived from 0BSD-relicensed work done by +//! Johannes Draaijer for the +//! [`stm32-eth`](https://github.com/stm32-rs/stm32-eth) project + +/// The amount of nanoseconds per second. +pub const NANOS_PER_SECOND: u32 = 1_000_000_000; + +/// The amount of subseconds per second. +pub const SUBSECONDS_PER_SECOND: u32 = 0x7FFF_FFFF; + +/// The ratio to use to convert subseconds to seconds. +pub const SUBSECONDS_TO_SECONDS: f32 = 1.0 / (SUBSECONDS_PER_SECOND as f32); + +const NS_PER_S: u64 = NANOS_PER_SECOND as u64; +const SUBS_PER_S: u64 = SUBSECONDS_PER_SECOND as u64; + +/// A subsecond value as produced by the PTP peripheral +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)] +pub struct Subseconds(u32); + +impl Subseconds { + /// The maximum possible value for [`Subseconds`] + pub const MAX_VALUE: u32 = SUBSECONDS_PER_SECOND; + + /// The maximum possible [`Subseconds`] + pub const MAX: Self = Self(SUBSECONDS_PER_SECOND); + + /// Zero [`Subseconds`] + pub const ZERO: Self = Self(0); + + /// Create a new [`Subseconds`] from the provided value. + /// + /// The returned [`Subseconds`] represents a time of `value / 2^31` seconds. + /// + /// To obtain that representation in nanoseconds, see [`Subseconds::nanos`]. + /// + /// To approximate a [`Subseconds`] from nanoseconds, see [`Subseconds::new_from_nanos`]. + /// + /// Returns `None` if `value > SUBSECONDS_PER_SECOND`. (See [`SUBSECONDS_PER_SECOND`]). + pub const fn new(value: u32) -> Option { + if value > SUBSECONDS_PER_SECOND { + None + } else { + Some(Self(value)) + } + } + + /// Create a new [`Subseconds`] from the provided value, without verifying that `value` + /// is less than or equal to [`Self::MAX_VALUE`]). + /// + /// The returned [`Subseconds`] represents a time of `value / 2^31` seconds. + /// + /// To obtain that representation in nanoseconds, see [`Subseconds::nanos`]. + /// + /// To approximate a [`Subseconds`] from nanoseconds, see [`Subseconds::new_from_nanos`] + pub(crate) const fn new_unchecked(value: u32) -> Self { + Self(value) + } + + /// Create a new [`Subseconds`] from the given amount of nanoseconds, + /// using a round-to-nearest method. + /// + /// Returns [`None`] if `nanos >= NANOS_PER_SECOND`. (See [`NANOS_PER_SECOND`]) + pub const fn new_from_nanos(nanos: u32) -> Option { + if nanos >= NANOS_PER_SECOND { + return None; + } + + let subseconds = + ((nanos as u64 * SUBS_PER_S) + (NS_PER_S / 2)) / NS_PER_S; + + Some(Subseconds::new_unchecked(subseconds as u32)) + } + + /// Convert this [`Subseconds`] to nanoseconds, using a round-to-nearest method. + pub const fn nanos(&self) -> u32 { + let nanos = + ((self.0 as u64 * NS_PER_S) + (SUBS_PER_S / 2)) / SUBS_PER_S; + + nanos as u32 + } + + /// Get the raw value of this [`Subseconds`] + pub const fn raw(&self) -> u32 { + self.0 + } + + #[allow(unused)] + /// Convert this [`Subseconds`] to Hertz + pub(crate) const fn hertz(&self) -> u32 { + SUBSECONDS_PER_SECOND / self.0 + } + + #[allow(unused)] + pub(crate) const fn nearest_increment(input_clk_hz: u32) -> Subseconds { + let hclk_half_subs = + (SUBSECONDS_PER_SECOND + (input_clk_hz / 2)) / input_clk_hz; + + Self::new_unchecked(hclk_half_subs) + } +} + +impl core::ops::Add for Subseconds { + type Output = Self; + + fn add(self, rhs: Subseconds) -> Self::Output { + Self(self.0.wrapping_add(rhs.0) % (SUBSECONDS_PER_SECOND + 1)) + } +} + +impl core::ops::AddAssign for Subseconds { + fn add_assign(&mut self, rhs: Subseconds) { + *self = *self + rhs; + } +} + +impl core::ops::Sub for Subseconds { + type Output = Self; + + fn sub(self, rhs: Subseconds) -> Self::Output { + Self(self.0.wrapping_sub(rhs.0) % (SUBSECONDS_PER_SECOND + 1)) + } +} + +impl core::ops::SubAssign for Subseconds { + fn sub_assign(&mut self, rhs: Subseconds) { + *self = *self - rhs; + } +} + +#[cfg(all(test, not(target_os = "none")))] +mod test { + + use super::*; + + // Assert that values produced by [`Subseconds::nearest_increment`] for some + // valid frequencies are within the correct span for `stssi` + #[test] + fn correct_subsecond_increment() { + for i in (25_000..180_000).map(|v| v * 1_000) { + let subs = Subseconds::nearest_increment(i).raw(); + assert!(subs > 0 && subs <= 255); + } + } + + #[test] + fn from_nanos() { + for i in [0, 1, 2, 3, NANOS_PER_SECOND - 1] { + let subseconds = Subseconds::new_from_nanos(i).unwrap(); + assert!(subseconds.raw() < SUBSECONDS_PER_SECOND); + } + + assert!(Subseconds::new_from_nanos(NANOS_PER_SECOND).is_none()); + assert!(Subseconds::new_from_nanos(u32::MAX).is_none()); + } + + #[test] + fn subsecond_math() { + let one = Subseconds::new(1).unwrap(); + let two = Subseconds::new(2).unwrap(); + let three = Subseconds::new(3).unwrap(); + let max = Subseconds::new(SUBSECONDS_PER_SECOND).unwrap(); + let zero = Subseconds::new(0).unwrap(); + + assert_eq!(one + two, three); + assert_eq!(two - one, one); + + assert_eq!(one - max + max, one); + assert_eq!(one - two, max); + assert_eq!(one + max, zero); + assert_eq!(two + max, one); + } +} diff --git a/src/ptp/timestamp.rs b/src/ptp/timestamp.rs new file mode 100644 index 00000000..0bca05c5 --- /dev/null +++ b/src/ptp/timestamp.rs @@ -0,0 +1,239 @@ +//! This implementation is derived from 0BSD-relicensed work done by +//! Johannes Draaijer for the +//! [`stm32-eth`](https://github.com/stm32-rs/stm32-eth) project + +use super::{Subseconds, NANOS_PER_SECOND}; + +/// A timestamp produced by the PTP periperhal +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub struct Timestamp(i64); + +#[cfg(feature = "defmt")] +impl defmt::Format for Timestamp { + fn format(&self, fmt: defmt::Formatter) { + if self.is_positive() { + defmt::write!(fmt, "{}.{:09}", self.seconds(), self.nanos()); + } else { + defmt::write!(fmt, "-{}.{:09}", self.seconds(), self.nanos()); + } + } +} + +impl Timestamp { + // The bit that represents the signedness of the timestamp in the + // subseconds value. + const SIGN_BIT: u32 = 0x8000_0000; + + /// Create a new [`Timestamp`] + pub const fn new( + negative: bool, + seconds: u32, + subseconds: Subseconds, + ) -> Self { + Self::new_unchecked(negative, seconds, subseconds.raw()) + } + + /// Create a new [`Timestamp`] from the given raw value. + pub const fn new_raw(value: i64) -> Self { + Self(value) + } + + /// Get the raw value of this [`Timestamp`] + pub const fn raw(&self) -> i64 { + self.0 + } + + /// Check whether this timestamp is negative or not. + pub const fn is_negative(&self) -> bool { + self.0.is_negative() + } + + /// Check whether this timestamp is positive or not. + pub const fn is_positive(&self) -> bool { + !self.is_negative() + } + + pub(crate) const fn new_unchecked( + negative: bool, + seconds: u32, + subseconds: u32, + ) -> Self { + let seconds: i64 = (seconds as i64) << 31; + let subseconds: i64 = subseconds as i64; + + let mut total = seconds + subseconds; + + if negative { + total = -total; + }; + + Self(total) + } + + /// Get the second component of this timestamp + pub const fn seconds(&self) -> u32 { + (self.0.abs() >> 31) as u32 + } + + /// Get the raw subsecond value of this timestamp. + pub const fn subseconds(&self) -> Subseconds { + Subseconds::new_unchecked( + self.0.unsigned_abs() as u32 & Subseconds::MAX_VALUE, + ) + } + + /// Get the signed subsecond value of this timestamp. + /// + /// Note that this is _not_ an i32: it is, technically, + /// a u31 with a leading sign bit. + pub const fn subseconds_signed(&self) -> u32 { + let mut subseconds = self.subseconds().raw(); + + if self.0.is_negative() { + subseconds |= Self::SIGN_BIT; + } + + subseconds + } + + /// Get the nanosecond component of this timestamp + pub const fn nanos(&self) -> u32 { + self.subseconds().nanos() + } + + /// Get the total amount of nanoseconds in this [`Timestamp`]. + /// + /// Example: + /// ```rust + /// # use stm32_eth::ptp::{Subseconds, Timestamp}; + /// let timestamp = Timestamp::new(false, 500, Subseconds::new_from_nanos(500_000).unwrap()); + /// assert_eq!(timestamp.total_nanos(), 500 * 1_000_000_000 + 500_000); + /// + /// + /// let timestamp_neg = Timestamp::new(true, 500, Subseconds::new_from_nanos(500_000).unwrap()); + /// assert_eq!(timestamp_neg.total_nanos(), -1 * (500 * 1_000_000_000 + 500_000)); + /// ``` + pub const fn total_abs_nanos(&self) -> u64 { + self.seconds() as u64 * NANOS_PER_SECOND as u64 + self.nanos() as u64 + } + + /// Create a new timestamp from the provided register values. + pub const fn from_parts(high: u32, low: u32) -> Timestamp { + let negative = (low & Self::SIGN_BIT) == Self::SIGN_BIT; + let subseconds = low & !(Self::SIGN_BIT); + + Timestamp::new_unchecked(negative, high, subseconds) + } +} + +impl core::ops::Add for Timestamp { + type Output = Self; + + fn add(self, rhs: Timestamp) -> Self::Output { + Self(self.0.saturating_add(rhs.0)) + } +} + +impl core::ops::AddAssign for Timestamp { + fn add_assign(&mut self, rhs: Timestamp) { + self.0 += rhs.0; + } +} + +impl core::ops::Sub for Timestamp { + type Output = Self; + + fn sub(self, rhs: Timestamp) -> Self::Output { + Self(self.0.saturating_sub(rhs.0)) + } +} + +impl core::ops::SubAssign for Timestamp { + fn sub_assign(&mut self, rhs: Timestamp) { + self.0 -= rhs.0 + } +} + +#[cfg(all(test, not(target_os = "none")))] +mod test { + use crate::ptp::SUBSECONDS_PER_SECOND; + + use super::{Subseconds, Timestamp}; + + fn subs(val: u32) -> Subseconds { + Subseconds::new(val).unwrap() + } + + #[test] + fn timestamp_add() { + let one = Timestamp::new(false, 1, subs(1)); + let one_big = Timestamp::new(false, 1, subs(SUBSECONDS_PER_SECOND - 1)); + let two = Timestamp::new(false, 2, subs(2)); + let three = Timestamp::new(false, 3, subs(3)); + + let one_neg = Timestamp::new(true, 1, subs(1)); + let one_big_neg = + Timestamp::new(true, 1, subs(SUBSECONDS_PER_SECOND - 1)); + let two_neg = Timestamp::new(true, 2, subs(2)); + let three_neg = Timestamp::new(true, 3, subs(3)); + + let one_minus_two = Timestamp::new(true, 1, subs(1)); + let one_big_plus_two = Timestamp::new(false, 4, subs(0)); + let two_minus_one_big = Timestamp::new(false, 0, subs(4)); + let one_big_neg_plus_two_neg = Timestamp::new(true, 4, subs(0)); + + // +self + +rhs + assert_eq!(one + two, three); + assert_eq!(two + one, three); + assert_eq!(one_big + two, one_big_plus_two); + assert_eq!(two + one_big, one_big_plus_two); + + // +self + -rhs + assert_eq!(one + two_neg, one_minus_two); + assert_eq!(two + one_big_neg, two_minus_one_big); + + // -self + rhs + assert_eq!(one_neg + two, one); + assert_eq!(two + one_neg, one); + + // -self + -rhs + assert_eq!(one_neg + two_neg, three_neg); + assert_eq!(two_neg + one_neg, three_neg); + assert_eq!(one_big_neg + two_neg, one_big_neg_plus_two_neg); + assert_eq!(two_neg + one_big_neg, one_big_neg_plus_two_neg); + } + + #[test] + fn timestamp_sub() { + let one = Timestamp::new(false, 1, subs(1)); + let one_big = Timestamp::new(false, 1, subs(SUBSECONDS_PER_SECOND - 1)); + let two = Timestamp::new(false, 2, subs(2)); + let three = Timestamp::new(false, 3, subs(3)); + + let one_neg = Timestamp::new(true, 1, subs(1)); + let two_neg = Timestamp::new(true, 2, subs(2)); + let three_neg = Timestamp::new(true, 3, subs(3)); + + let one_minus_two = Timestamp::new(true, 1, subs(1)); + let one_minus_one_big = + Timestamp::new(true, 0, subs(SUBSECONDS_PER_SECOND - 2)); + + assert_eq!(one - one_big, one_minus_one_big); + + // +self - +rhs + assert_eq!(two - one, one); + assert_eq!(one - two, one_minus_two); + + // +self - -rhs + assert_eq!(two - one_neg, three); + assert_eq!(one_neg - two, three_neg); + + // -self - +rhs + assert_eq!(one_neg - two, three_neg); + assert_eq!(two - one_neg, three); + + // -self - -rhs + assert_eq!(one_neg - two_neg, one); + assert_eq!(two_neg - one_neg, one_minus_two); + } +}