diff --git a/subprojects/gstreamer/libs/gst/helpers/ptp/main.rs b/subprojects/gstreamer/libs/gst/helpers/ptp/main.rs index e6694875e9..f2b0fb4831 100644 --- a/subprojects/gstreamer/libs/gst/helpers/ptp/main.rs +++ b/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 { 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")?; } } diff --git a/subprojects/gstreamer/libs/gst/helpers/ptp/parse.rs b/subprojects/gstreamer/libs/gst/helpers/ptp/parse.rs new file mode 100644 index 0000000000..20925d20ab --- /dev/null +++ b/subprojects/gstreamer/libs/gst/helpers/ptp/parse.rs @@ -0,0 +1,603 @@ +// GStreamer +// +// Copyright (C) 2015-2023 Sebastian Dröge +// +// 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 +// . +// +// SPDX-License-Identifier: MPL-2.0 + +use std::io; + +pub trait ReadBytesBEExt: io::Read { + #[inline] + fn read_u8(&mut self) -> io::Result { + let mut buf = [0u8; 1]; + self.read_exact(&mut buf)?; + Ok(buf[0]) + } + + #[inline] + fn read_i8(&mut self) -> io::Result { + let mut buf = [0u8; 1]; + self.read_exact(&mut buf)?; + Ok(buf[0] as i8) + } + + #[inline] + fn read_u16be(&mut self) -> io::Result { + let mut buf = [0u8; 2]; + self.read_exact(&mut buf)?; + Ok(u16::from_be_bytes(buf)) + } + + #[inline] + fn read_i16be(&mut self) -> io::Result { + 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 { + let mut buf = [0u8; 4]; + self.read_exact(&mut buf)?; + Ok(u32::from_be_bytes(buf)) + } + + #[inline] + fn read_i32be(&mut self) -> io::Result { + 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 { + let mut buf = [0u8; 8]; + self.read_exact(&mut buf)?; + Ok(u64::from_be_bytes(buf)) + } + + #[inline] + fn read_i64be(&mut self) -> io::Result { + let mut buf = [0u8; 8]; + self.read_exact(&mut buf)?; + Ok(u64::from_be_bytes(buf) as i64) + } +} + +impl 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 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 { + 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 for PtpMessageType { + fn from(v: u8) -> PtpMessageType { + PtpMessageType(v) + } +} + +impl From 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!(), + } + } +}