Debian Codegraph @

libs/gst/helpers/ptp/parse.rs

raw
1 // GStreamer
2 //
3 // Copyright (C) 2015-2023 Sebastian Dröge <[email protected]>
4 //
5 // This Source Code Form is subject to the terms of the Mozilla Public License, v2.0.
6 // If a copy of the MPL was not distributed with this file, You can obtain one at
7 // <https://mozilla.org/MPL/2.0/>.
8 //
9 // SPDX-License-Identifier: MPL-2.0
10
11 #![allow(unused)]
12
13 use std::io;
14
15 pub trait ReadBytesBEExt: io::Read {
16 #[inline]
17 fn read_u8(&mut self) -> io::Result<u8> {
18 let mut buf = [0u8; 1];
19 self.read_exact(&mut buf)?;
20 Ok(buf[0])
21 }
22
23 #[inline]
24 fn read_i8(&mut self) -> io::Result<i8> {
25 let mut buf = [0u8; 1];
26 self.read_exact(&mut buf)?;
27 Ok(buf[0] as i8)
28 }
29
30 #[inline]
31 fn read_u16be(&mut self) -> io::Result<u16> {
32 let mut buf = [0u8; 2];
33 self.read_exact(&mut buf)?;
34 Ok(u16::from_be_bytes(buf))
35 }
36
37 #[inline]
38 fn read_i16be(&mut self) -> io::Result<i16> {
39 let mut buf = [0u8; 2];
40 self.read_exact(&mut buf)?;
41 Ok(u16::from_be_bytes(buf) as i16)
42 }
43
44 #[inline]
45 fn read_u32be(&mut self) -> io::Result<u32> {
46 let mut buf = [0u8; 4];
47 self.read_exact(&mut buf)?;
48 Ok(u32::from_be_bytes(buf))
49 }
50
51 #[inline]
52 fn read_i32be(&mut self) -> io::Result<i32> {
53 let mut buf = [0u8; 4];
54 self.read_exact(&mut buf)?;
55 Ok(u32::from_be_bytes(buf) as i32)
56 }
57
58 #[inline]
59 fn read_u64be(&mut self) -> io::Result<u64> {
60 let mut buf = [0u8; 8];
61 self.read_exact(&mut buf)?;
62 Ok(u64::from_be_bytes(buf))
63 }
64
65 #[inline]
66 fn read_i64be(&mut self) -> io::Result<i64> {
67 let mut buf = [0u8; 8];
68 self.read_exact(&mut buf)?;
69 Ok(u64::from_be_bytes(buf) as i64)
70 }
71 }
72
73 impl<T> ReadBytesBEExt for T where T: io::Read {}
74
75 pub trait WriteBytesBEExt: io::Write {
76 #[inline]
77 fn write_u8(&mut self, v: u8) -> io::Result<()> {
78 self.write_all(&[v])?;
79 Ok(())
80 }
81
82 #[inline]
83 fn write_i8(&mut self, v: i8) -> io::Result<()> {
84 self.write_all(&[v as u8])?;
85 Ok(())
86 }
87
88 #[inline]
89 fn write_u16be(&mut self, v: u16) -> io::Result<()> {
90 self.write_all(&v.to_be_bytes())?;
91 Ok(())
92 }
93
94 #[inline]
95 fn write_i16be(&mut self, v: i16) -> io::Result<()> {
96 self.write_all(&v.to_be_bytes())?;
97 Ok(())
98 }
99
100 #[inline]
101 fn write_u32be(&mut self, v: u32) -> io::Result<()> {
102 self.write_all(&v.to_be_bytes())?;
103 Ok(())
104 }
105
106 #[inline]
107 fn write_i32be(&mut self, v: i32) -> io::Result<()> {
108 self.write_all(&v.to_be_bytes())?;
109 Ok(())
110 }
111
112 #[inline]
113 fn write_u64be(&mut self, v: u64) -> io::Result<()> {
114 self.write_all(&v.to_be_bytes())?;
115 Ok(())
116 }
117
118 #[inline]
119 fn write_i64be(&mut self, v: i64) -> io::Result<()> {
120 self.write_all(&v.to_be_bytes())?;
121 Ok(())
122 }
123 }
124
125 impl<T> WriteBytesBEExt for T where T: io::Write {}
126
127 #[derive(Debug)]
128 pub struct PtpMessage {
129 pub transport_specific: u8,
130 pub message_type: PtpMessageType,
131 pub version_ptp: u8,
132 pub domain_number: u8,
133 pub flag_field: u16,
134 pub correction_field: i64,
135 pub source_port_identity: PtpClockIdentity,
136 pub sequence_id: u16,
137 pub control_field: u8,
138 pub log_message_interval: i8,
139 pub message_payload: PtpMessagePayload,
140 }
141
142 impl PtpMessage {
143 pub fn parse(mut data: &[u8]) -> io::Result<PtpMessage> {
144 if data.len() < 34 {
145 return Err(io::Error::new(
146 io::ErrorKind::InvalidInput,
147 format!("Too short PTP message {} < 34", data.len()),
148 ));
149 }
150
151 let b = data.read_u8()?;
152 let transport_specific = b >> 4;
153 let message_type = PtpMessageType(b & 0x0f);
154
155 let b = data.read_u8()?;
156 let version_ptp = b & 0x0f;
157 if version_ptp != 2 {
158 return Err(io::Error::new(
159 io::ErrorKind::InvalidData,
160 format!("Unsupported PTP version {}", version_ptp),
161 ));
162 }
163
164 let message_length = data.read_u16be()?;
165 if data.len() + 4 < message_length as usize {
166 return Err(io::Error::new(
167 io::ErrorKind::InvalidInput,
168 format!(
169 "Too short PTP message {} < {}",
170 data.len() + 4,
171 message_length
172 ),
173 ));
174 }
175
176 let domain_number = data.read_u8()?;
177 data = &data[1..];
178
179 let flag_field = data.read_u16be()?;
180 let correction_field = data.read_i64be()?;
181 data = &data[4..];
182
183 let clock_identity = data.read_u64be()?;
184 let port_number = data.read_u16be()?;
185
186 let sequence_id = data.read_u16be()?;
187 let control_field = data.read_u8()?;
188 let log_message_interval = data.read_i8()?;
189
190 let message_payload = match message_type {
191 PtpMessageType::ANNOUNCE => {
192 if data.len() < 30 {
193 return Err(io::Error::new(
194 io::ErrorKind::InvalidInput,
195 format!("Too short PTP ANNOUNCE message {} < 20", data.len(),),
196 ));
197 }
198
199 let origin_timestamp = PtpTimestamp {
200 seconds_field: ((data.read_u32be()? as u64) << 16)
201 | (data.read_u16be()? as u64),
202 nanoseconds_field: data.read_u32be()?,
203 };
204
205 let current_utc_offset = data.read_i16be()?;
206 data = &data[1..];
207
208 let grandmaster_priority_1 = data.read_u8()?;
209 let grandmaster_clock_quality = PtpClockQuality {
210 clock_class: data.read_u8()?,
211 clock_accuracy: data.read_u8()?,
212 offset_scaled_log_variance: data.read_u16be()?,
213 };
214 let grandmaster_priority_2 = data.read_u8()?;
215
216 let grandmaster_identity = data.read_u64be()?;
217 let steps_removed = data.read_u16be()?;
218 let time_source = data.read_u8()?;
219
220 PtpMessagePayload::Announce {
221 origin_timestamp,
222 current_utc_offset,
223 grandmaster_priority_1,
224 grandmaster_clock_quality,
225 grandmaster_priority_2,
226 grandmaster_identity,
227 steps_removed,
228 time_source,
229 }
230 }
231 PtpMessageType::SYNC => {
232 if data.len() < 10 {
233 return Err(io::Error::new(
234 io::ErrorKind::InvalidInput,
235 format!("Too short PTP SYNC message {} < 20", data.len(),),
236 ));
237 }
238
239 let origin_timestamp = PtpTimestamp {
240 seconds_field: ((data.read_u32be()? as u64) << 16)
241 | (data.read_u16be()? as u64),
242 nanoseconds_field: data.read_u32be()?,
243 };
244
245 PtpMessagePayload::Sync { origin_timestamp }
246 }
247 PtpMessageType::FOLLOW_UP => {
248 if data.len() < 10 {
249 return Err(io::Error::new(
250 io::ErrorKind::InvalidInput,
251 format!("Too short PTP FOLLOW_UP message {} < 20", data.len(),),
252 ));
253 }
254
255 let precise_origin_timestamp = PtpTimestamp {
256 seconds_field: ((data.read_u32be()? as u64) << 16)
257 | (data.read_u16be()? as u64),
258 nanoseconds_field: data.read_u32be()?,
259 };
260
261 PtpMessagePayload::FollowUp {
262 precise_origin_timestamp,
263 }
264 }
265 PtpMessageType::DELAY_REQ => {
266 if data.len() < 10 {
267 return Err(io::Error::new(
268 io::ErrorKind::InvalidInput,
269 format!("Too short PTP DELAY_REQ message {} < 20", data.len(),),
270 ));
271 }
272
273 let origin_timestamp = PtpTimestamp {
274 seconds_field: ((data.read_u32be()? as u64) << 16)
275 | (data.read_u16be()? as u64),
276 nanoseconds_field: data.read_u32be()?,
277 };
278
279 PtpMessagePayload::DelayReq { origin_timestamp }
280 }
281 PtpMessageType::DELAY_RESP => {
282 if data.len() < 20 {
283 return Err(io::Error::new(
284 io::ErrorKind::InvalidInput,
285 format!("Too short PTP DELAY_RESP message {} < 20", data.len(),),
286 ));
287 }
288
289 let receive_timestamp = PtpTimestamp {
290 seconds_field: ((data.read_u32be()? as u64) << 16)
291 | (data.read_u16be()? as u64),
292 nanoseconds_field: data.read_u32be()?,
293 };
294 let clock_identity = data.read_u64be()?;
295 let port_number = data.read_u16be()?;
296
297 PtpMessagePayload::DelayResp {
298 receive_timestamp,
299 requesting_port_identity: PtpClockIdentity {
300 clock_identity,
301 port_number,
302 },
303 }
304 }
305 _ => PtpMessagePayload::Other(message_type.0),
306 };
307
308 Ok(PtpMessage {
309 transport_specific,
310 message_type,
311 version_ptp,
312 domain_number,
313 flag_field,
314 correction_field,
315 source_port_identity: PtpClockIdentity {
316 clock_identity,
317 port_number,
318 },
319 sequence_id,
320 control_field,
321 log_message_interval,
322 message_payload,
323 })
324 }
325 }
326
327 #[derive(Debug, Clone, Copy, PartialEq, Eq)]
328 pub struct PtpMessageType(u8);
329
330 impl PtpMessageType {
331 pub const SYNC: PtpMessageType = PtpMessageType(0x0);
332 pub const DELAY_REQ: PtpMessageType = PtpMessageType(0x1);
333 pub const P_DELAY_REQ: PtpMessageType = PtpMessageType(0x2);
334 pub const P_DELAY_RESP: PtpMessageType = PtpMessageType(0x3);
335 pub const FOLLOW_UP: PtpMessageType = PtpMessageType(0x8);
336 pub const DELAY_RESP: PtpMessageType = PtpMessageType(0x9);
337 pub const P_DELAY_RESP_FOLLOW_UP: PtpMessageType = PtpMessageType(0xA);
338 pub const ANNOUNCE: PtpMessageType = PtpMessageType(0xB);
339 pub const SIGNALING: PtpMessageType = PtpMessageType(0xC);
340 pub const MANAGEMENT: PtpMessageType = PtpMessageType(0xD);
341 }
342
343 impl From<u8> for PtpMessageType {
344 fn from(v: u8) -> PtpMessageType {
345 PtpMessageType(v)
346 }
347 }
348
349 impl From<PtpMessageType> for u8 {
350 fn from(v: PtpMessageType) -> u8 {
351 v.0
352 }
353 }
354
355 #[derive(Debug)]
356 pub enum PtpMessagePayload {
357 Announce {
358 origin_timestamp: PtpTimestamp,
359 current_utc_offset: i16,
360 grandmaster_priority_1: u8,
361 grandmaster_clock_quality: PtpClockQuality,
362 grandmaster_priority_2: u8,
363 grandmaster_identity: u64,
364 steps_removed: u16,
365 time_source: u8,
366 },
367 Sync {
368 origin_timestamp: PtpTimestamp,
369 },
370 FollowUp {
371 precise_origin_timestamp: PtpTimestamp,
372 },
373 DelayReq {
374 origin_timestamp: PtpTimestamp,
375 },
376 DelayResp {
377 receive_timestamp: PtpTimestamp,
378 requesting_port_identity: PtpClockIdentity,
379 },
380 Other(u8),
381 }
382
383 impl PtpMessagePayload {
384 #[allow(unused)]
385 pub fn type_(&self) -> PtpMessageType {
386 match self {
387 PtpMessagePayload::Sync { .. } => PtpMessageType::SYNC,
388 PtpMessagePayload::Announce { .. } => PtpMessageType::ANNOUNCE,
389 PtpMessagePayload::FollowUp { .. } => PtpMessageType::FOLLOW_UP,
390 PtpMessagePayload::DelayReq { .. } => PtpMessageType::DELAY_REQ,
391 PtpMessagePayload::DelayResp { .. } => PtpMessageType::DELAY_RESP,
392 PtpMessagePayload::Other(v) => PtpMessageType(*v),
393 }
394 }
395 }
396
397 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
398 pub struct PtpClockIdentity {
399 pub clock_identity: u64,
400 pub port_number: u16,
401 }
402
403 #[derive(Debug, Copy, Clone, PartialEq, Eq, PartialOrd, Ord)]
404 pub struct PtpTimestamp {
405 pub seconds_field: u64,
406 pub nanoseconds_field: u32,
407 }
408
409 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
410 pub struct PtpClockQuality {
411 pub clock_class: u8,
412 pub clock_accuracy: u8,
413 pub offset_scaled_log_variance: u16,
414 }
415
416 #[cfg(test)]
417 mod test {
418 use super::*;
419
420 #[test]
421 fn test_parse_sync() {
422 let sync = [
423 0x00, 0x02, 0x00, 0x2c, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
424 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x56, 0x80, 0xff, 0xfe, 0x05, 0x7e, 0x77,
425 0x00, 0x01, 0x00, 0x4c, 0x00, 0x00, 0x00, 0x00, 0x64, 0x6b, 0x39, 0x5b, 0x06, 0xee,
426 0x6e, 0xf3,
427 ];
428
429 let msg = PtpMessage::parse(&sync).unwrap();
430 assert_eq!(msg.transport_specific, 0);
431 assert_eq!(msg.message_type, PtpMessageType::SYNC);
432 assert_eq!(msg.version_ptp, 2);
433 assert_eq!(msg.domain_number, 0);
434 assert_eq!(msg.flag_field, 0x0200);
435 assert_eq!(msg.correction_field, 0);
436 assert_eq!(msg.source_port_identity.clock_identity, 1753730941874175607);
437 assert_eq!(msg.source_port_identity.port_number, 1);
438 assert_eq!(msg.sequence_id, 76);
439 assert_eq!(msg.control_field, 0x00);
440 assert_eq!(msg.log_message_interval, 0);
441
442 match msg.message_payload {
443 PtpMessagePayload::Sync { origin_timestamp } => {
444 assert_eq!(origin_timestamp.seconds_field, 1684748635);
445 assert_eq!(origin_timestamp.nanoseconds_field, 116289267);
446 }
447 _ => unreachable!(),
448 }
449 }
450
451 #[test]
452 fn test_parse_follow_up() {
453 let follow_up = [
454 0x08, 0x02, 0x00, 0x2c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
455 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x56, 0x80, 0xff, 0xfe, 0x05, 0x7e, 0x77,
456 0x00, 0x01, 0x00, 0x4c, 0x02, 0x00, 0x00, 0x00, 0x64, 0x6b, 0x39, 0x5b, 0x06, 0xef,
457 0x0d, 0x58,
458 ];
459
460 let msg = PtpMessage::parse(&follow_up).unwrap();
461 assert_eq!(msg.transport_specific, 0);
462 assert_eq!(msg.message_type, PtpMessageType::FOLLOW_UP);
463 assert_eq!(msg.version_ptp, 2);
464 assert_eq!(msg.domain_number, 0);
465 assert_eq!(msg.flag_field, 0x0000);
466 assert_eq!(msg.correction_field, 0);
467 assert_eq!(msg.source_port_identity.clock_identity, 1753730941874175607);
468 assert_eq!(msg.source_port_identity.port_number, 1);
469 assert_eq!(msg.sequence_id, 76);
470 assert_eq!(msg.control_field, 0x02);
471 assert_eq!(msg.log_message_interval, 0);
472
473 match msg.message_payload {
474 PtpMessagePayload::FollowUp {
475 precise_origin_timestamp,
476 } => {
477 assert_eq!(precise_origin_timestamp.seconds_field, 1684748635);
478 assert_eq!(precise_origin_timestamp.nanoseconds_field, 116329816);
479 }
480 _ => unreachable!(),
481 }
482 }
483
484 #[test]
485 fn test_parse_announce() {
486 let announce = [
487 0x0b, 0x02, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
488 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x56, 0x80, 0xff, 0xfe, 0x05, 0x7e, 0x77,
489 0x00, 0x01, 0x00, 0x25, 0x05, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
490 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x0d, 0xfe, 0xff, 0xff, 0x80, 0x18, 0x56, 0x80,
491 0xff, 0xfe, 0x05, 0x7e, 0x77, 0x00, 0x00, 0xa0,
492 ];
493
494 let msg = PtpMessage::parse(&announce).unwrap();
495 assert_eq!(msg.transport_specific, 0);
496 assert_eq!(msg.message_type, PtpMessageType::ANNOUNCE);
497 assert_eq!(msg.version_ptp, 2);
498 assert_eq!(msg.domain_number, 0);
499 assert_eq!(msg.flag_field, 0x0000);
500 assert_eq!(msg.correction_field, 0);
501 assert_eq!(msg.source_port_identity.clock_identity, 1753730941874175607);
502 assert_eq!(msg.source_port_identity.port_number, 1);
503 assert_eq!(msg.sequence_id, 37);
504 assert_eq!(msg.control_field, 0x05);
505 assert_eq!(msg.log_message_interval, 1);
506
507 match msg.message_payload {
508 PtpMessagePayload::Announce {
509 origin_timestamp,
510 current_utc_offset,
511 grandmaster_priority_1,
512 grandmaster_clock_quality,
513 grandmaster_priority_2,
514 grandmaster_identity,
515 steps_removed,
516 time_source,
517 } => {
518 assert_eq!(origin_timestamp.seconds_field, 0);
519 assert_eq!(origin_timestamp.nanoseconds_field, 0);
520 assert_eq!(current_utc_offset, 0);
521 assert_eq!(grandmaster_priority_1, 128);
522 assert_eq!(grandmaster_clock_quality.clock_class, 13);
523 assert_eq!(grandmaster_clock_quality.clock_accuracy, 254);
524 assert_eq!(grandmaster_clock_quality.offset_scaled_log_variance, 65535);
525 assert_eq!(grandmaster_priority_2, 128);
526 assert_eq!(grandmaster_identity, 1753730941874175607);
527 assert_eq!(steps_removed, 0);
528 assert_eq!(time_source, 160);
529 }
530 _ => unreachable!(),
531 }
532 }
533
534 #[test]
535 fn test_parse_delay_req() {
536 let delay_req = [
537 0x01, 0x02, 0x00, 0x2c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
538 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd8, 0x5e, 0xd3, 0xff, 0xfe, 0xe5, 0x88, 0xd6,
539 0xbb, 0x60, 0x00, 0x01, 0x01, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
540 0x00, 0x00,
541 ];
542
543 let msg = PtpMessage::parse(&delay_req).unwrap();
544 assert_eq!(msg.transport_specific, 0);
545 assert_eq!(msg.message_type, PtpMessageType::DELAY_REQ);
546 assert_eq!(msg.version_ptp, 2);
547 assert_eq!(msg.domain_number, 0);
548 assert_eq!(msg.flag_field, 0x0000);
549 assert_eq!(msg.correction_field, 0);
550 assert_eq!(
551 msg.source_port_identity.clock_identity,
552 15591132056449812694,
553 );
554 assert_eq!(msg.source_port_identity.port_number, 47968);
555 assert_eq!(msg.sequence_id, 1);
556 assert_eq!(msg.control_field, 0x01);
557 assert_eq!(msg.log_message_interval, 0x7F);
558
559 match msg.message_payload {
560 PtpMessagePayload::DelayReq { origin_timestamp } => {
561 assert_eq!(origin_timestamp.seconds_field, 0);
562 assert_eq!(origin_timestamp.nanoseconds_field, 0);
563 }
564 _ => unreachable!(),
565 }
566 }
567 #[test]
568 fn test_parse_delay_resp() {
569 let delay_resp = [
570 0x09, 0x02, 0x00, 0x36, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
571 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x56, 0x80, 0xff, 0xfe, 0x05, 0x7e, 0x77,
572 0x00, 0x01, 0x00, 0x01, 0x03, 0x00, 0x00, 0x00, 0x64, 0x6b, 0x39, 0x5a, 0x07, 0x9f,
573 0x83, 0x65, 0xd8, 0x5e, 0xd3, 0xff, 0xfe, 0xe5, 0x88, 0xd6, 0xbb, 0x60,
574 ];
575
576 let msg = PtpMessage::parse(&delay_resp).unwrap();
577 assert_eq!(msg.transport_specific, 0);
578 assert_eq!(msg.message_type, PtpMessageType::DELAY_RESP);
579 assert_eq!(msg.version_ptp, 2);
580 assert_eq!(msg.domain_number, 0);
581 assert_eq!(msg.flag_field, 0x0000);
582 assert_eq!(msg.correction_field, 0);
583 assert_eq!(msg.source_port_identity.clock_identity, 1753730941874175607,);
584 assert_eq!(msg.source_port_identity.port_number, 1);
585 assert_eq!(msg.sequence_id, 1);
586 assert_eq!(msg.control_field, 0x03);
587 assert_eq!(msg.log_message_interval, 0);
588
589 match msg.message_payload {
590 PtpMessagePayload::DelayResp {
591 receive_timestamp,
592 requesting_port_identity,
593 } => {
594 assert_eq!(receive_timestamp.seconds_field, 1684748634);
595 assert_eq!(receive_timestamp.nanoseconds_field, 127894373,);
596 assert_eq!(
597 requesting_port_identity.clock_identity,
598 15591132056449812694
599 );
600 assert_eq!(requesting_port_identity.port_number, 47968);
601 }
602 _ => unreachable!(),
603 }
604 }
605 }