ptp: Parse PTP messages in the helper process and filter earlier

This drops invalid, unknown or uninteresting PTP messages in the helper
process already instead of forwarding them to the main process.

Part-of: <https://gitlab.freedesktop.org/gstreamer/gstreamer/-/merge_requests/4665>

subprojects/gstreamer/libs/gst/helpers/ptp/main.rs

@@ -31,11 +31,13 @@ mod error;
 mod ffi;
 mod io;
 mod net;
+mod parse;
 mod privileges;
 mod rand;
 mod thread;
 
 use error::{Context, Error};
+use parse::{PtpClockIdentity, PtpMessagePayload, ReadBytesBEExt, WriteBytesBEExt};
 use rand::rand;
 
 /// PTP Multicast group.
@@ -55,27 +57,6 @@ const MSG_TYPE_CLOCK_ID: u8 = 2;
 /// Send time ACK message
 const MSG_TYPE_SEND_TIME_ACK: u8 = 3;
 
-/// Reads a big endian 64 bit integer from a slice.
-fn read_u64_be(s: &[u8]) -> u64 {
-    assert!(s.len() >= 8);
-
-    (s[7] as u64)
-        | ((s[6] as u64) << 8)
-        | ((s[5] as u64) << 16)
-        | ((s[4] as u64) << 24)
-        | ((s[3] as u64) << 32)
-        | ((s[2] as u64) << 40)
-        | ((s[1] as u64) << 48)
-        | ((s[0] as u64) << 56)
-}
-
-/// Reads a big endian 16 bit integer from a slice.
-fn read_u16_be(s: &[u8]) -> u16 {
-    assert!(s.len() >= 2);
-
-    (s[1] as u16) | ((s[0] as u16) << 8)
-}
-
 /// Create a new `UdpSocket` for the given port and configure it for PTP.
 fn create_socket(port: u16) -> Result<UdpSocket, Error> {
     let socket = UdpSocket::bind(SocketAddr::from((Ipv4Addr::UNSPECIFIED, port)))
@@ -186,9 +167,14 @@ fn run() -> Result<(), Error> {
     // Write clock ID first
     {
         let mut clock_id_data = [0u8; 3 + 8];
-        clock_id_data[0..2].copy_from_slice(&8u16.to_be_bytes());
-        clock_id_data[2] = MSG_TYPE_CLOCK_ID;
-        clock_id_data[3..].copy_from_slice(&clock_id);
+        {
+            let mut buf = &mut clock_id_data[..];
+            buf.write_u16be(8).expect("Too small clock ID buffer");
+            buf.write_u8(MSG_TYPE_CLOCK_ID)
+                .expect("Too small clock ID buffer");
+            buf.write_all(&clock_id).expect("Too small clock ID buffer");
+            assert!(buf.is_empty(), "Too big clock ID buffer");
+        }
 
         poll.stdout()
             .write_all(&clock_id_data)
@@ -213,52 +199,98 @@ fn run() -> Result<(), Error> {
             .filter_map(|(idx, r)| if *r { Some(idx) } else { None })
         {
             let idx = idx as u8;
-            let res = match idx {
-                MSG_TYPE_EVENT => poll.event_socket().recv_from(&mut socket_buffer),
-                MSG_TYPE_GENERAL => poll.general_socket().recv_from(&mut socket_buffer),
-                _ => unreachable!(),
-            };
 
-            match res {
-                Err(err) if err.kind() == std::io::ErrorKind::WouldBlock => {
-                    continue 'next_socket;
-                }
-                Err(err) => {
-                    bail!(
-                        source: err,
-                        "Failed reading from {} socket",
-                        if idx == MSG_TYPE_EVENT {
-                            "event"
-                        } else {
-                            "general"
-                        }
-                    );
-                }
-                Ok((read, addr)) => {
-                    let recv_time = clock::time();
-                    if args.verbose {
-                        trace!(
-                            "Received {} bytes from {} socket from {} at {}",
-                            read,
+            // Read all available packets from the socket before going to the next socket.
+            'next_packet: loop {
+                let res = match idx {
+                    MSG_TYPE_EVENT => poll.event_socket().recv_from(&mut socket_buffer),
+                    MSG_TYPE_GENERAL => poll.general_socket().recv_from(&mut socket_buffer),
+                    _ => unreachable!(),
+                };
+
+                let (read, addr) = match res {
+                    Err(err) if err.kind() == std::io::ErrorKind::WouldBlock => {
+                        continue 'next_socket;
+                    }
+                    Err(err) => {
+                        bail!(
+                            source: err,
+                            "Failed reading from {} socket",
                             if idx == MSG_TYPE_EVENT {
                                 "event"
                             } else {
                                 "general"
-                            },
-                            addr,
-                            recv_time,
+                            }
                         );
                     }
+                    Ok((read, addr)) => (read, addr),
+                };
 
-                    stdinout_buffer[0..2].copy_from_slice(&(read as u16 + 8).to_be_bytes());
-                    stdinout_buffer[2] = idx;
-                    stdinout_buffer[3..][..8].copy_from_slice(&recv_time.to_be_bytes());
-                    stdinout_buffer[11..][..read].copy_from_slice(&socket_buffer[..read]);
-
-                    poll.stdout()
-                        .write_all(&stdinout_buffer[..(read + 3 + 8)])
-                        .context("Failed writing to stdout")?;
+                let recv_time = clock::time();
+                if args.verbose {
+                    trace!(
+                        "Received {} bytes from {} socket from {} at {}",
+                        read,
+                        if idx == MSG_TYPE_EVENT {
+                            "event"
+                        } else {
+                            "general"
+                        },
+                        addr,
+                        recv_time,
+                    );
                 }
+
+                let buf = &socket_buffer[..read];
+
+                // Check if this is a valid PTP message, that it is not a PTP message sent from
+                // our own clock ID and in case of a DELAY_RESP that it is for our clock id.
+                let ptp_message = match parse::PtpMessage::parse(buf) {
+                    Ok(msg) => msg,
+                    Err(err) => {
+                        warn!("Received invalid PTP message: {}", err);
+                        continue 'next_packet;
+                    }
+                };
+
+                if args.verbose {
+                    trace!("Received PTP message {:#?}", ptp_message);
+                }
+
+                if ptp_message.source_port_identity.clock_identity == u64::from_be_bytes(clock_id) {
+                    if args.verbose {
+                        trace!("Ignoring our own PTP message");
+                    }
+                    continue 'next_packet;
+                }
+                if let PtpMessagePayload::DelayResp {
+                    requesting_port_identity: PtpClockIdentity { clock_identity, .. },
+                    ..
+                } = ptp_message.message_payload
+                {
+                    if clock_identity != u64::from_be_bytes(clock_id) {
+                        if args.verbose {
+                            trace!("Ignoring PTP DELAY_RESP message for a different clock");
+                        }
+                        continue 'next_packet;
+                    }
+                }
+
+                {
+                    let mut buf = &mut stdinout_buffer[..(read + 3 + 8)];
+                    buf.write_u16be(read as u16 + 8)
+                        .expect("Too small stdout buffer");
+                    buf.write_u8(idx).expect("Too small stdout buffer");
+                    buf.write_u64be(recv_time).expect("Too small stdout buffer");
+                    buf.write_all(&socket_buffer[..read])
+                        .expect("Too small stdout buffer");
+                    assert!(buf.is_empty(), "Too big stdout buffer",);
+                }
+
+                let buf = &stdinout_buffer[..(read + 3 + 8)];
+                poll.stdout()
+                    .write_all(buf)
+                    .context("Failed writing to stdout")?;
             }
         }
 
@@ -296,15 +328,24 @@ fn run() -> Result<(), Error> {
                 );
             }
 
-            if size < 8 + 32 {
+            if size < 8 + 34 {
                 bail!("Invalid packet body size");
             }
 
-            let main_send_time = read_u64_be(&stdinout_buffer[..8]);
-            let buf = &stdinout_buffer[8..size as usize];
-            let message_type = buf[0] & 0x0f;
-            let domain_number = buf[4];
-            let seqnum = read_u16_be(&buf[30..][..2]);
+            let buf = &mut &stdinout_buffer[..(size as usize)];
+            let main_send_time = buf.read_u64be().expect("Too small stdin buffer");
+
+            // We require that the main process only ever sends valid PTP messages with the clock
+            // ID assigned by this process.
+            let ptp_message =
+                parse::PtpMessage::parse(buf).context("Parsing PTP message from main process")?;
+            if ptp_message.source_port_identity.clock_identity != u64::from_be_bytes(clock_id) {
+                bail!("PTP message with unexpected clock identity on stdin");
+            }
+
+            if args.verbose {
+                trace!("Received PTP message from stdin {:#?}", ptp_message);
+            }
 
             let send_time = clock::time();
             match type_ {
@@ -330,24 +371,32 @@ fn run() -> Result<(), Error> {
             if args.verbose {
                 trace!(
                     "Sending SEND_TIME_ACK for message type {}, domain number {}, seqnum {} received at {} at {}",
-                    message_type,
-                    domain_number,
-                    seqnum,
+                    u8::from(ptp_message.message_type),
+                    ptp_message.domain_number,
+                    ptp_message.sequence_id,
                     main_send_time,
                     send_time,
                 );
             }
 
-            stdinout_buffer[0..2].copy_from_slice(&12u16.to_be_bytes());
-            stdinout_buffer[2] = MSG_TYPE_SEND_TIME_ACK;
-            let send_time_ack_msg = &mut stdinout_buffer[3..];
-            send_time_ack_msg[..8].copy_from_slice(&send_time.to_be_bytes());
-            send_time_ack_msg[8] = message_type;
-            send_time_ack_msg[9] = domain_number;
-            send_time_ack_msg[10..][..2].copy_from_slice(&seqnum.to_be_bytes());
+            {
+                let mut buf = &mut stdinout_buffer[..(3 + 12)];
+                buf.write_u16be(12).expect("Too small stdout buffer");
+                buf.write_u8(MSG_TYPE_SEND_TIME_ACK)
+                    .expect("Too small stdout buffer");
+                buf.write_u64be(send_time).expect("Too small stdout buffer");
+                buf.write_u8(ptp_message.message_type.into())
+                    .expect("Too small stdout buffer");
+                buf.write_u8(ptp_message.domain_number)
+                    .expect("Too small stdout buffer");
+                buf.write_u16be(ptp_message.sequence_id)
+                    .expect("Too small stdout buffer");
+                assert!(buf.is_empty(), "Too big stdout buffer",);
+            }
 
+            let buf = &stdinout_buffer[..(3 + 12)];
             poll.stdout()
-                .write_all(&stdinout_buffer[..(3 + 12)])
+                .write_all(buf)
                 .context("Failed writing to stdout")?;
         }
     }

subprojects/gstreamer/libs/gst/helpers/ptp/parse.rs

@@ -0,0 +1,603 @@
+// GStreamer
+//
+// Copyright (C) 2015-2023 Sebastian Dröge <sebastian@centricular.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla Public License, v2.0.
+// If a copy of the MPL was not distributed with this file, You can obtain one at
+// <https://mozilla.org/MPL/2.0/>.
+//
+// SPDX-License-Identifier: MPL-2.0
+
+use std::io;
+
+pub trait ReadBytesBEExt: io::Read {
+    #[inline]
+    fn read_u8(&mut self) -> io::Result<u8> {
+        let mut buf = [0u8; 1];
+        self.read_exact(&mut buf)?;
+        Ok(buf[0])
+    }
+
+    #[inline]
+    fn read_i8(&mut self) -> io::Result<i8> {
+        let mut buf = [0u8; 1];
+        self.read_exact(&mut buf)?;
+        Ok(buf[0] as i8)
+    }
+
+    #[inline]
+    fn read_u16be(&mut self) -> io::Result<u16> {
+        let mut buf = [0u8; 2];
+        self.read_exact(&mut buf)?;
+        Ok(u16::from_be_bytes(buf))
+    }
+
+    #[inline]
+    fn read_i16be(&mut self) -> io::Result<i16> {
+        let mut buf = [0u8; 2];
+        self.read_exact(&mut buf)?;
+        Ok(u16::from_be_bytes(buf) as i16)
+    }
+
+    #[inline]
+    fn read_u32be(&mut self) -> io::Result<u32> {
+        let mut buf = [0u8; 4];
+        self.read_exact(&mut buf)?;
+        Ok(u32::from_be_bytes(buf))
+    }
+
+    #[inline]
+    fn read_i32be(&mut self) -> io::Result<i32> {
+        let mut buf = [0u8; 4];
+        self.read_exact(&mut buf)?;
+        Ok(u32::from_be_bytes(buf) as i32)
+    }
+
+    #[inline]
+    fn read_u64be(&mut self) -> io::Result<u64> {
+        let mut buf = [0u8; 8];
+        self.read_exact(&mut buf)?;
+        Ok(u64::from_be_bytes(buf))
+    }
+
+    #[inline]
+    fn read_i64be(&mut self) -> io::Result<i64> {
+        let mut buf = [0u8; 8];
+        self.read_exact(&mut buf)?;
+        Ok(u64::from_be_bytes(buf) as i64)
+    }
+}
+
+impl<T> ReadBytesBEExt for T where T: io::Read {}
+
+pub trait WriteBytesBEExt: io::Write {
+    #[inline]
+    fn write_u8(&mut self, v: u8) -> io::Result<()> {
+        self.write_all(&[v])?;
+        Ok(())
+    }
+
+    #[inline]
+    fn write_i8(&mut self, v: i8) -> io::Result<()> {
+        self.write_all(&[v as u8])?;
+        Ok(())
+    }
+
+    #[inline]
+    fn write_u16be(&mut self, v: u16) -> io::Result<()> {
+        self.write_all(&v.to_be_bytes())?;
+        Ok(())
+    }
+
+    #[inline]
+    fn write_i16be(&mut self, v: i16) -> io::Result<()> {
+        self.write_all(&v.to_be_bytes())?;
+        Ok(())
+    }
+
+    #[inline]
+    fn write_u32be(&mut self, v: u32) -> io::Result<()> {
+        self.write_all(&v.to_be_bytes())?;
+        Ok(())
+    }
+
+    #[inline]
+    fn write_i32be(&mut self, v: i32) -> io::Result<()> {
+        self.write_all(&v.to_be_bytes())?;
+        Ok(())
+    }
+
+    #[inline]
+    fn write_u64be(&mut self, v: u64) -> io::Result<()> {
+        self.write_all(&v.to_be_bytes())?;
+        Ok(())
+    }
+
+    #[inline]
+    fn write_i64be(&mut self, v: i64) -> io::Result<()> {
+        self.write_all(&v.to_be_bytes())?;
+        Ok(())
+    }
+}
+
+impl<T> WriteBytesBEExt for T where T: io::Write {}
+
+#[derive(Debug)]
+pub struct PtpMessage {
+    pub transport_specific: u8,
+    pub message_type: PtpMessageType,
+    pub version_ptp: u8,
+    pub domain_number: u8,
+    pub flag_field: u16,
+    pub correction_field: i64,
+    pub source_port_identity: PtpClockIdentity,
+    pub sequence_id: u16,
+    pub control_field: u8,
+    pub log_message_interval: i8,
+    pub message_payload: PtpMessagePayload,
+}
+
+impl PtpMessage {
+    pub fn parse(mut data: &[u8]) -> io::Result<PtpMessage> {
+        if data.len() < 34 {
+            return Err(io::Error::new(
+                io::ErrorKind::InvalidInput,
+                format!("Too short PTP message {} < 34", data.len()),
+            ));
+        }
+
+        let b = data.read_u8()?;
+        let transport_specific = b >> 4;
+        let message_type = PtpMessageType(b & 0x0f);
+
+        let b = data.read_u8()?;
+        let version_ptp = b & 0x0f;
+        if version_ptp != 2 {
+            return Err(io::Error::new(
+                io::ErrorKind::InvalidData,
+                format!("Unsupported PTP version {}", version_ptp),
+            ));
+        }
+
+        let message_length = data.read_u16be()?;
+        if data.len() + 4 < message_length as usize {
+            return Err(io::Error::new(
+                io::ErrorKind::InvalidInput,
+                format!(
+                    "Too short PTP message {} < {}",
+                    data.len() + 4,
+                    message_length
+                ),
+            ));
+        }
+
+        let domain_number = data.read_u8()?;
+        data = &data[1..];
+
+        let flag_field = data.read_u16be()?;
+        let correction_field = data.read_i64be()?;
+        data = &data[4..];
+
+        let clock_identity = data.read_u64be()?;
+        let port_number = data.read_u16be()?;
+
+        let sequence_id = data.read_u16be()?;
+        let control_field = data.read_u8()?;
+        let log_message_interval = data.read_i8()?;
+
+        let message_payload = match message_type {
+            PtpMessageType::ANNOUNCE => {
+                if data.len() < 20 {
+                    return Err(io::Error::new(
+                        io::ErrorKind::InvalidInput,
+                        format!("Too short PTP SYNC message {} < 20", data.len(),),
+                    ));
+                }
+
+                let origin_timestamp = PtpTimestamp {
+                    seconds_field: ((data.read_u32be()? as u64) << 16)
+                        | (data.read_u16be()? as u64),
+                    nanoseconds_field: data.read_u32be()?,
+                };
+
+                let current_utc_offset = data.read_i16be()?;
+                data = &data[1..];
+
+                let grandmaster_priority_1 = data.read_u8()?;
+                let grandmaster_clock_quality = PtpClockQuality {
+                    clock_class: data.read_u8()?,
+                    clock_accuracy: data.read_u8()?,
+                    offset_scaled_log_variance: data.read_u16be()?,
+                };
+                let grandmaster_priority_2 = data.read_u8()?;
+
+                let grandmaster_identity = data.read_u64be()?;
+                let steps_removed = data.read_u16be()?;
+                let time_source = data.read_u8()?;
+
+                PtpMessagePayload::Announce {
+                    origin_timestamp,
+                    current_utc_offset,
+                    grandmaster_priority_1,
+                    grandmaster_clock_quality,
+                    grandmaster_priority_2,
+                    grandmaster_identity,
+                    steps_removed,
+                    time_source,
+                }
+            }
+            PtpMessageType::SYNC => {
+                if data.len() < 10 {
+                    return Err(io::Error::new(
+                        io::ErrorKind::InvalidInput,
+                        format!("Too short PTP SYNC message {} < 20", data.len(),),
+                    ));
+                }
+
+                let origin_timestamp = PtpTimestamp {
+                    seconds_field: ((data.read_u32be()? as u64) << 16)
+                        | (data.read_u16be()? as u64),
+                    nanoseconds_field: data.read_u32be()?,
+                };
+
+                PtpMessagePayload::Sync { origin_timestamp }
+            }
+            PtpMessageType::FOLLOW_UP => {
+                if data.len() < 10 {
+                    return Err(io::Error::new(
+                        io::ErrorKind::InvalidInput,
+                        format!("Too short PTP SYNC message {} < 20", data.len(),),
+                    ));
+                }
+
+                let precise_origin_timestamp = PtpTimestamp {
+                    seconds_field: ((data.read_u32be()? as u64) << 16)
+                        | (data.read_u16be()? as u64),
+                    nanoseconds_field: data.read_u32be()?,
+                };
+
+                PtpMessagePayload::FollowUp {
+                    precise_origin_timestamp,
+                }
+            }
+            PtpMessageType::DELAY_REQ => {
+                if data.len() < 10 {
+                    return Err(io::Error::new(
+                        io::ErrorKind::InvalidInput,
+                        format!("Too short PTP SYNC message {} < 20", data.len(),),
+                    ));
+                }
+
+                let origin_timestamp = PtpTimestamp {
+                    seconds_field: ((data.read_u32be()? as u64) << 16)
+                        | (data.read_u16be()? as u64),
+                    nanoseconds_field: data.read_u32be()?,
+                };
+
+                PtpMessagePayload::DelayReq { origin_timestamp }
+            }
+            PtpMessageType::DELAY_RESP => {
+                if data.len() < 20 {
+                    return Err(io::Error::new(
+                        io::ErrorKind::InvalidInput,
+                        format!("Too short PTP SYNC message {} < 20", data.len(),),
+                    ));
+                }
+
+                let receive_timestamp = PtpTimestamp {
+                    seconds_field: ((data.read_u32be()? as u64) << 16)
+                        | (data.read_u16be()? as u64),
+                    nanoseconds_field: data.read_u32be()?,
+                };
+                let clock_identity = data.read_u64be()?;
+                let port_number = data.read_u16be()?;
+
+                PtpMessagePayload::DelayResp {
+                    receive_timestamp,
+                    requesting_port_identity: PtpClockIdentity {
+                        clock_identity,
+                        port_number,
+                    },
+                }
+            }
+            _ => PtpMessagePayload::Other(message_type.0),
+        };
+
+        Ok(PtpMessage {
+            transport_specific,
+            message_type,
+            version_ptp,
+            domain_number,
+            flag_field,
+            correction_field,
+            source_port_identity: PtpClockIdentity {
+                clock_identity,
+                port_number,
+            },
+            sequence_id,
+            control_field,
+            log_message_interval,
+            message_payload,
+        })
+    }
+}
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct PtpMessageType(u8);
+
+impl PtpMessageType {
+    pub const SYNC: PtpMessageType = PtpMessageType(0x0);
+    pub const DELAY_REQ: PtpMessageType = PtpMessageType(0x1);
+    pub const P_DELAY_REQ: PtpMessageType = PtpMessageType(0x2);
+    pub const P_DELAY_RESP: PtpMessageType = PtpMessageType(0x3);
+    pub const FOLLOW_UP: PtpMessageType = PtpMessageType(0x8);
+    pub const DELAY_RESP: PtpMessageType = PtpMessageType(0x9);
+    pub const P_DELAY_RESP_FOLLOW_UP: PtpMessageType = PtpMessageType(0xA);
+    pub const ANNOUNCE: PtpMessageType = PtpMessageType(0xB);
+    pub const SIGNALING: PtpMessageType = PtpMessageType(0xC);
+    pub const MANAGEMENT: PtpMessageType = PtpMessageType(0xD);
+}
+
+impl From<u8> for PtpMessageType {
+    fn from(v: u8) -> PtpMessageType {
+        PtpMessageType(v)
+    }
+}
+
+impl From<PtpMessageType> for u8 {
+    fn from(v: PtpMessageType) -> u8 {
+        v.0
+    }
+}
+
+#[derive(Debug)]
+pub enum PtpMessagePayload {
+    Announce {
+        origin_timestamp: PtpTimestamp,
+        current_utc_offset: i16,
+        grandmaster_priority_1: u8,
+        grandmaster_clock_quality: PtpClockQuality,
+        grandmaster_priority_2: u8,
+        grandmaster_identity: u64,
+        steps_removed: u16,
+        time_source: u8,
+    },
+    Sync {
+        origin_timestamp: PtpTimestamp,
+    },
+    FollowUp {
+        precise_origin_timestamp: PtpTimestamp,
+    },
+    DelayReq {
+        origin_timestamp: PtpTimestamp,
+    },
+    DelayResp {
+        receive_timestamp: PtpTimestamp,
+        requesting_port_identity: PtpClockIdentity,
+    },
+    Other(u8),
+}
+
+impl PtpMessagePayload {
+    #[allow(unused)]
+    pub fn type_(&self) -> PtpMessageType {
+        match self {
+            PtpMessagePayload::Sync { .. } => PtpMessageType::SYNC,
+            PtpMessagePayload::Announce { .. } => PtpMessageType::ANNOUNCE,
+            PtpMessagePayload::FollowUp { .. } => PtpMessageType::FOLLOW_UP,
+            PtpMessagePayload::DelayReq { .. } => PtpMessageType::DELAY_REQ,
+            PtpMessagePayload::DelayResp { .. } => PtpMessageType::DELAY_RESP,
+            PtpMessagePayload::Other(v) => PtpMessageType(*v),
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub struct PtpClockIdentity {
+    pub clock_identity: u64,
+    pub port_number: u16,
+}
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq, PartialOrd, Ord)]
+pub struct PtpTimestamp {
+    pub seconds_field: u64,
+    pub nanoseconds_field: u32,
+}
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub struct PtpClockQuality {
+    pub clock_class: u8,
+    pub clock_accuracy: u8,
+    pub offset_scaled_log_variance: u16,
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    #[test]
+    fn test_parse_sync() {
+        let sync = [
+            0x00, 0x02, 0x00, 0x2c, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x56, 0x80, 0xff, 0xfe, 0x05, 0x7e, 0x77,
+            0x00, 0x01, 0x00, 0x4c, 0x00, 0x00, 0x00, 0x00, 0x64, 0x6b, 0x39, 0x5b, 0x06, 0xee,
+            0x6e, 0xf3,
+        ];
+
+        let msg = PtpMessage::parse(&sync).unwrap();
+        assert_eq!(msg.transport_specific, 0);
+        assert_eq!(msg.message_type, PtpMessageType::SYNC);
+        assert_eq!(msg.version_ptp, 2);
+        assert_eq!(msg.domain_number, 0);
+        assert_eq!(msg.flag_field, 0x0200);
+        assert_eq!(msg.correction_field, 0);
+        assert_eq!(msg.source_port_identity.clock_identity, 1753730941874175607);
+        assert_eq!(msg.source_port_identity.port_number, 1);
+        assert_eq!(msg.sequence_id, 76);
+        assert_eq!(msg.control_field, 0x00);
+        assert_eq!(msg.log_message_interval, 0);
+
+        match msg.message_payload {
+            PtpMessagePayload::Sync { origin_timestamp } => {
+                assert_eq!(origin_timestamp.seconds_field, 1684748635);
+                assert_eq!(origin_timestamp.nanoseconds_field, 116289267);
+            }
+            _ => unreachable!(),
+        }
+    }
+
+    #[test]
+    fn test_parse_follow_up() {
+        let follow_up = [
+            0x08, 0x02, 0x00, 0x2c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x56, 0x80, 0xff, 0xfe, 0x05, 0x7e, 0x77,
+            0x00, 0x01, 0x00, 0x4c, 0x02, 0x00, 0x00, 0x00, 0x64, 0x6b, 0x39, 0x5b, 0x06, 0xef,
+            0x0d, 0x58,
+        ];
+
+        let msg = PtpMessage::parse(&follow_up).unwrap();
+        assert_eq!(msg.transport_specific, 0);
+        assert_eq!(msg.message_type, PtpMessageType::FOLLOW_UP);
+        assert_eq!(msg.version_ptp, 2);
+        assert_eq!(msg.domain_number, 0);
+        assert_eq!(msg.flag_field, 0x0000);
+        assert_eq!(msg.correction_field, 0);
+        assert_eq!(msg.source_port_identity.clock_identity, 1753730941874175607);
+        assert_eq!(msg.source_port_identity.port_number, 1);
+        assert_eq!(msg.sequence_id, 76);
+        assert_eq!(msg.control_field, 0x02);
+        assert_eq!(msg.log_message_interval, 0);
+
+        match msg.message_payload {
+            PtpMessagePayload::FollowUp {
+                precise_origin_timestamp,
+            } => {
+                assert_eq!(precise_origin_timestamp.seconds_field, 1684748635);
+                assert_eq!(precise_origin_timestamp.nanoseconds_field, 116329816);
+            }
+            _ => unreachable!(),
+        }
+    }
+
+    #[test]
+    fn test_parse_announce() {
+        let announce = [
+            0x0b, 0x02, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x56, 0x80, 0xff, 0xfe, 0x05, 0x7e, 0x77,
+            0x00, 0x01, 0x00, 0x25, 0x05, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x0d, 0xfe, 0xff, 0xff, 0x80, 0x18, 0x56, 0x80,
+            0xff, 0xfe, 0x05, 0x7e, 0x77, 0x00, 0x00, 0xa0,
+        ];
+
+        let msg = PtpMessage::parse(&announce).unwrap();
+        assert_eq!(msg.transport_specific, 0);
+        assert_eq!(msg.message_type, PtpMessageType::ANNOUNCE);
+        assert_eq!(msg.version_ptp, 2);
+        assert_eq!(msg.domain_number, 0);
+        assert_eq!(msg.flag_field, 0x0000);
+        assert_eq!(msg.correction_field, 0);
+        assert_eq!(msg.source_port_identity.clock_identity, 1753730941874175607);
+        assert_eq!(msg.source_port_identity.port_number, 1);
+        assert_eq!(msg.sequence_id, 37);
+        assert_eq!(msg.control_field, 0x05);
+        assert_eq!(msg.log_message_interval, 1);
+
+        match msg.message_payload {
+            PtpMessagePayload::Announce {
+                origin_timestamp,
+                current_utc_offset,
+                grandmaster_priority_1,
+                grandmaster_clock_quality,
+                grandmaster_priority_2,
+                grandmaster_identity,
+                steps_removed,
+                time_source,
+            } => {
+                assert_eq!(origin_timestamp.seconds_field, 0);
+                assert_eq!(origin_timestamp.nanoseconds_field, 0);
+                assert_eq!(current_utc_offset, 0);
+                assert_eq!(grandmaster_priority_1, 128);
+                assert_eq!(grandmaster_clock_quality.clock_class, 13);
+                assert_eq!(grandmaster_clock_quality.clock_accuracy, 254);
+                assert_eq!(grandmaster_clock_quality.offset_scaled_log_variance, 65535);
+                assert_eq!(grandmaster_priority_2, 128);
+                assert_eq!(grandmaster_identity, 1753730941874175607);
+                assert_eq!(steps_removed, 0);
+                assert_eq!(time_source, 160);
+            }
+            _ => unreachable!(),
+        }
+    }
+
+    #[test]
+    fn test_parse_delay_req() {
+        let delay_req = [
+            0x01, 0x02, 0x00, 0x2c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd8, 0x5e, 0xd3, 0xff, 0xfe, 0xe5, 0x88, 0xd6,
+            0xbb, 0x60, 0x00, 0x01, 0x01, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00,
+        ];
+
+        let msg = PtpMessage::parse(&delay_req).unwrap();
+        assert_eq!(msg.transport_specific, 0);
+        assert_eq!(msg.message_type, PtpMessageType::DELAY_REQ);
+        assert_eq!(msg.version_ptp, 2);
+        assert_eq!(msg.domain_number, 0);
+        assert_eq!(msg.flag_field, 0x0000);
+        assert_eq!(msg.correction_field, 0);
+        assert_eq!(
+            msg.source_port_identity.clock_identity,
+            15591132056449812694,
+        );
+        assert_eq!(msg.source_port_identity.port_number, 47968);
+        assert_eq!(msg.sequence_id, 1);
+        assert_eq!(msg.control_field, 0x01);
+        assert_eq!(msg.log_message_interval, 0x7F);
+
+        match msg.message_payload {
+            PtpMessagePayload::DelayReq { origin_timestamp } => {
+                assert_eq!(origin_timestamp.seconds_field, 0);
+                assert_eq!(origin_timestamp.nanoseconds_field, 0);
+            }
+            _ => unreachable!(),
+        }
+    }
+    #[test]
+    fn test_parse_delay_resp() {
+        let delay_resp = [
+            0x09, 0x02, 0x00, 0x36, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x56, 0x80, 0xff, 0xfe, 0x05, 0x7e, 0x77,
+            0x00, 0x01, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x64, 0x6b, 0x39, 0x5a, 0x07, 0x9f,
+            0x83, 0x65, 0xd8, 0x5e, 0xd3, 0xff, 0xfe, 0xe5, 0x88, 0xd6, 0xbb, 0x60,
+        ];
+
+        let msg = PtpMessage::parse(&delay_resp).unwrap();
+        assert_eq!(msg.transport_specific, 0);
+        assert_eq!(msg.message_type, PtpMessageType::DELAY_RESP);
+        assert_eq!(msg.version_ptp, 2);
+        assert_eq!(msg.domain_number, 0);
+        assert_eq!(msg.flag_field, 0x0000);
+        assert_eq!(msg.correction_field, 0);
+        assert_eq!(msg.source_port_identity.clock_identity, 1753730941874175607,);
+        assert_eq!(msg.source_port_identity.port_number, 1);
+        assert_eq!(msg.sequence_id, 1);
+        assert_eq!(msg.control_field, 0x03);
+        assert_eq!(msg.log_message_interval, 0);
+
+        match msg.message_payload {
+            PtpMessagePayload::DelayResp {
+                receive_timestamp,
+                requesting_port_identity,
+            } => {
+                assert_eq!(receive_timestamp.seconds_field, 1684748634);
+                assert_eq!(receive_timestamp.nanoseconds_field, 127894373,);
+                assert_eq!(
+                    requesting_port_identity.clock_identity,
+                    15591132056449812694
+                );
+                assert_eq!(requesting_port_identity.port_number, 47968);
+            }
+            _ => unreachable!(),
+        }
+    }
+}