g2g_core/ptp/
wire.rs

1//! PTP-over-UDP (IEEE 1588-2008 / SMPTE ST 2059-2) wire format, the messages a
2//! SLAVE ordinary clock needs to parse and the Delay_Req it sends (M594).
3//!
4//! Only the subset a delay-request-response (E2E) slave uses: the common 34-byte
5//! header, plus Sync / Follow_Up / Delay_Resp bodies, plus a Delay_Req builder.
6//! Announce (BMCA), peer-delay (P2P) and unicast are out of scope (a SLAVE that
7//! just follows whatever master is sending on its domain); management messages
8//! live in the sibling `management` module.
9//!
10//! Every field is read from the network, so parsing is bounds-checked and returns
11//! `None` on a short or malformed buffer rather than panicking, per the parser
12//! rules in AGENTS.md. Sub-nanosecond correction bits are dropped (we time in ns).
13
14/// PTP common-header length in bytes.
15pub const HEADER_LEN: usize = 34;
16/// A PTP timestamp on the wire: 48-bit seconds + 32-bit nanoseconds = 10 bytes.
17pub const TIMESTAMP_LEN: usize = 10;
18/// Byte offset of the first message body (right after the common header).
19pub const BODY_OFFSET: usize = HEADER_LEN;
20/// PTP version this parser targets (IEEE 1588-2008).
21pub const PTP_VERSION: u8 = 2;
22/// Total length of a Delay_Req: header + originTimestamp.
23pub const DELAY_REQ_LEN: usize = HEADER_LEN + TIMESTAMP_LEN;
24
25/// The message types a delay-request-response SLAVE cares about (the low nibble
26/// of the first header octet); everything else is `Other`.
27#[derive(Clone, Copy, Debug, PartialEq, Eq)]
28pub enum PtpMessageType {
29    Sync,
30    DelayReq,
31    FollowUp,
32    DelayResp,
33    Announce,
34    Management,
35    Other(u8),
36}
37
38impl PtpMessageType {
39    fn from_nibble(n: u8) -> Self {
40        match n & 0x0f {
41            0x0 => Self::Sync,
42            0x1 => Self::DelayReq,
43            0x8 => Self::FollowUp,
44            0x9 => Self::DelayResp,
45            0xb => Self::Announce,
46            0xd => Self::Management,
47            other => Self::Other(other),
48        }
49    }
50
51    pub(crate) fn nibble(self) -> u8 {
52        match self {
53            Self::Sync => 0x0,
54            Self::DelayReq => 0x1,
55            Self::FollowUp => 0x8,
56            Self::DelayResp => 0x9,
57            Self::Announce => 0xb,
58            Self::Management => 0xd,
59            Self::Other(o) => o & 0x0f,
60        }
61    }
62}
63
64/// The parsed PTP common header (the first 34 bytes of every message).
65#[derive(Clone, Copy, Debug, PartialEq, Eq)]
66pub struct PtpHeader {
67    pub message_type: PtpMessageType,
68    pub version: u8,
69    pub message_length: u16,
70    pub domain: u8,
71    pub flags: u16,
72    /// correctionField in ns (the fractional-ns low 16 bits dropped).
73    pub correction_ns: i64,
74    pub source_clock_id: [u8; 8],
75    pub source_port: u16,
76    pub sequence_id: u16,
77}
78
79impl PtpHeader {
80    /// Parse the common header from the front of `buf`, or `None` if too short.
81    pub fn parse(buf: &[u8]) -> Option<Self> {
82        if buf.len() < HEADER_LEN {
83            return None;
84        }
85        let correction_raw = i64::from_be_bytes(buf[8..16].try_into().ok()?);
86        Some(Self {
87            message_type: PtpMessageType::from_nibble(buf[0]),
88            version: buf[1] & 0x0f,
89            message_length: u16::from_be_bytes([buf[2], buf[3]]),
90            domain: buf[4],
91            flags: u16::from_be_bytes([buf[6], buf[7]]),
92            // correctionField is a 64-bit fixed-point ns value scaled by 2^16.
93            correction_ns: correction_raw >> 16,
94            source_clock_id: buf[20..28].try_into().ok()?,
95            source_port: u16::from_be_bytes([buf[28], buf[29]]),
96            sequence_id: u16::from_be_bytes([buf[30], buf[31]]),
97        })
98    }
99
100    /// twoStepFlag (bit 1 of flagField octet 0): the accurate Sync TX time comes
101    /// in a following Follow_Up rather than in the Sync itself.
102    pub fn two_step(&self) -> bool {
103        self.flags & 0x0200 != 0
104    }
105}
106
107/// Read a 10-byte PTP timestamp (48-bit seconds + 32-bit ns) at `buf[off..]` as
108/// total nanoseconds, or `None` if out of range / overflowing.
109pub fn parse_timestamp(buf: &[u8], off: usize) -> Option<u64> {
110    let b = buf.get(off..off + TIMESTAMP_LEN)?;
111    let secs = (u64::from(b[0]) << 40)
112        | (u64::from(b[1]) << 32)
113        | (u64::from(b[2]) << 24)
114        | (u64::from(b[3]) << 16)
115        | (u64::from(b[4]) << 8)
116        | u64::from(b[5]);
117    let nanos = u64::from(u32::from_be_bytes([b[6], b[7], b[8], b[9]]));
118    secs.checked_mul(1_000_000_000)?.checked_add(nanos)
119}
120
121/// The originTimestamp of a Sync (meaningful only for a one-step master; a
122/// two-step master sends it as zero and the real value in the Follow_Up).
123pub fn parse_sync_origin(buf: &[u8]) -> Option<u64> {
124    parse_timestamp(buf, BODY_OFFSET)
125}
126
127/// The preciseOriginTimestamp of a Follow_Up (the accurate Sync TX time).
128pub fn parse_follow_up_origin(buf: &[u8]) -> Option<u64> {
129    parse_timestamp(buf, BODY_OFFSET)
130}
131
132/// A parsed Delay_Resp body: the master's receiveTimestamp of our Delay_Req plus
133/// the requestingPortIdentity it echoes, so a slave can match its own request.
134#[derive(Clone, Copy, Debug, PartialEq, Eq)]
135pub struct DelayResp {
136    pub receive_ts_ns: u64,
137    pub requesting_clock_id: [u8; 8],
138    pub requesting_port: u16,
139}
140
141impl DelayResp {
142    /// Parse a Delay_Resp body (receiveTimestamp + requestingPortIdentity) from a
143    /// full message buffer.
144    pub fn parse(buf: &[u8]) -> Option<Self> {
145        let receive_ts_ns = parse_timestamp(buf, BODY_OFFSET)?;
146        // requestingPortIdentity follows the 10-byte timestamp.
147        let id_off = BODY_OFFSET + TIMESTAMP_LEN;
148        let clock = buf.get(id_off..id_off + 8)?;
149        let port = buf.get(id_off + 8..id_off + 10)?;
150        Some(Self {
151            receive_ts_ns,
152            requesting_clock_id: clock.try_into().ok()?,
153            requesting_port: u16::from_be_bytes([port[0], port[1]]),
154        })
155    }
156}
157
158/// Build a Delay_Req message a SLAVE multicasts to the master. The
159/// originTimestamp is left zero (a software slave times its own TX on send and
160/// carries t3 locally, not in the message).
161pub fn build_delay_req(
162    domain: u8,
163    clock_id: [u8; 8],
164    port: u16,
165    sequence_id: u16,
166) -> [u8; DELAY_REQ_LEN] {
167    let mut m = [0u8; DELAY_REQ_LEN];
168    m[0] = PtpMessageType::DelayReq.nibble(); // majorSdoId 0 | messageType
169    m[1] = PTP_VERSION; // minorVersion 0 | versionPTP 2
170    let len = DELAY_REQ_LEN as u16;
171    m[2..4].copy_from_slice(&len.to_be_bytes());
172    m[4] = domain;
173    // flagField 0, correctionField 0 (bytes 6..16 already zero).
174    m[20..28].copy_from_slice(&clock_id);
175    m[28..30].copy_from_slice(&port.to_be_bytes());
176    m[30..32].copy_from_slice(&sequence_id.to_be_bytes());
177    m[32] = 0x01; // controlField: Delay_Req (legacy but still set)
178    m[33] = 0x7f; // logMessageInterval: 0x7f = "not set" for an event message
179    m
180}
181
182#[cfg(test)]
183mod tests {
184    use super::*;
185
186    /// Hand-build a common header into `buf` for tests.
187    fn put_header(buf: &mut [u8], mtype: u8, two_step: bool, domain: u8, seq: u16, corr_ns: i64) {
188        buf[0] = mtype & 0x0f;
189        buf[1] = PTP_VERSION;
190        let len = buf.len() as u16;
191        buf[2..4].copy_from_slice(&len.to_be_bytes());
192        buf[4] = domain;
193        let flags: u16 = if two_step { 0x0200 } else { 0 };
194        buf[6..8].copy_from_slice(&flags.to_be_bytes());
195        buf[8..16].copy_from_slice(&(corr_ns << 16).to_be_bytes());
196        buf[20..28].copy_from_slice(&[1, 2, 3, 4, 5, 6, 7, 8]);
197        buf[28..30].copy_from_slice(&1u16.to_be_bytes());
198        buf[30..32].copy_from_slice(&seq.to_be_bytes());
199    }
200
201    fn put_timestamp(buf: &mut [u8], off: usize, secs: u64, nanos: u32) {
202        buf[off] = (secs >> 40) as u8;
203        buf[off + 1] = (secs >> 32) as u8;
204        buf[off + 2] = (secs >> 24) as u8;
205        buf[off + 3] = (secs >> 16) as u8;
206        buf[off + 4] = (secs >> 8) as u8;
207        buf[off + 5] = secs as u8;
208        buf[off + 6..off + 10].copy_from_slice(&nanos.to_be_bytes());
209    }
210
211    #[test]
212    fn rejects_short_buffers() {
213        assert!(PtpHeader::parse(&[0u8; 10]).is_none());
214        assert!(parse_timestamp(&[0u8; 5], 0).is_none());
215        assert!(
216            DelayResp::parse(&[0u8; HEADER_LEN]).is_none(),
217            "no room for the body"
218        );
219    }
220
221    #[test]
222    fn parses_a_two_step_sync_header() {
223        let mut buf = [0u8; HEADER_LEN + TIMESTAMP_LEN];
224        put_header(&mut buf, 0x0, true, 0, 42, 0);
225        let h = PtpHeader::parse(&buf).unwrap();
226        assert_eq!(h.message_type, PtpMessageType::Sync);
227        assert_eq!(h.version, PTP_VERSION);
228        assert_eq!(h.domain, 0);
229        assert_eq!(h.sequence_id, 42);
230        assert_eq!(h.source_clock_id, [1, 2, 3, 4, 5, 6, 7, 8]);
231        assert!(h.two_step(), "twoStepFlag set");
232    }
233
234    #[test]
235    fn parses_timestamps_and_correction() {
236        let mut buf = [0u8; HEADER_LEN + TIMESTAMP_LEN];
237        put_header(&mut buf, 0x8, false, 0, 7, 1234); // Follow_Up, correction 1234 ns
238        put_timestamp(&mut buf, BODY_OFFSET, 1_700_000_000, 500_000_000);
239        let h = PtpHeader::parse(&buf).unwrap();
240        assert_eq!(h.message_type, PtpMessageType::FollowUp);
241        assert_eq!(h.correction_ns, 1234);
242        assert_eq!(
243            parse_follow_up_origin(&buf),
244            Some(1_700_000_000_500_000_000)
245        );
246    }
247
248    #[test]
249    fn parses_a_delay_resp_body() {
250        let mut buf = [0u8; HEADER_LEN + TIMESTAMP_LEN + 10];
251        put_header(&mut buf, 0x9, false, 0, 7, 0);
252        put_timestamp(&mut buf, BODY_OFFSET, 1_700_000_001, 250);
253        // requestingPortIdentity
254        buf[BODY_OFFSET + TIMESTAMP_LEN..BODY_OFFSET + TIMESTAMP_LEN + 8]
255            .copy_from_slice(&[9, 9, 9, 9, 9, 9, 9, 9]);
256        buf[BODY_OFFSET + TIMESTAMP_LEN + 8..BODY_OFFSET + TIMESTAMP_LEN + 10]
257            .copy_from_slice(&3u16.to_be_bytes());
258        let r = DelayResp::parse(&buf).unwrap();
259        assert_eq!(r.receive_ts_ns, 1_700_000_001_000_000_250);
260        assert_eq!(r.requesting_clock_id, [9; 8]);
261        assert_eq!(r.requesting_port, 3);
262    }
263
264    #[test]
265    fn builds_a_parseable_delay_req() {
266        let m = build_delay_req(0, [1, 2, 3, 4, 5, 6, 7, 8], 1, 99);
267        assert_eq!(m.len(), DELAY_REQ_LEN);
268        let h = PtpHeader::parse(&m).unwrap();
269        assert_eq!(h.message_type, PtpMessageType::DelayReq);
270        assert_eq!(h.version, PTP_VERSION);
271        assert_eq!(h.sequence_id, 99);
272        assert_eq!(h.source_clock_id, [1, 2, 3, 4, 5, 6, 7, 8]);
273        assert_eq!(h.source_port, 1);
274        assert!(!h.two_step());
275    }
276}
g2g_core/ptp/wire.rs

g2g_core/ptp/
wire.rs
