Debian Codegraph @

libs/gst/helpers/ptp/main.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 //! Helper process that runs setuid root or with appropriate privileges to
12 //! listen on ports < 1024, do multicast operations and get MAC addresses of
13 //! interfaces. Privileges are dropped after these operations are done.
14 //!
15 //! It listens on the PTP multicast group on port 319 and 320 and forwards
16 //! everything received there to stdout, while forwarding everything received
17 //! on stdin to those sockets.
18 //! Additionally it provides the MAC address of a network interface via stdout
19
20 use std::{
21 io::{Read, Write},
22 net::{Ipv4Addr, UdpSocket},
23 };
24
25 #[macro_use]
26 mod log;
27
28 mod args;
29 mod clock;
30 mod error;
31 mod ffi;
32 mod io;
33 mod net;
34 mod parse;
35 mod privileges;
36 mod rand;
37 mod thread;
38
39 use error::{Context, Error};
40 use parse::{PtpClockIdentity, PtpMessagePayload, PtpMessageType, ReadBytesBEExt, WriteBytesBEExt};
41 use rand::rand;
42
43 /// PTP Multicast group.
44 const PTP_MULTICAST_ADDR: Ipv4Addr = Ipv4Addr::new(224, 0, 1, 129);
45 /// PTP Event message port.
46 const PTP_EVENT_PORT: u16 = 319;
47 /// PTP General message port.
48 const PTP_GENERAL_PORT: u16 = 320;
49
50 /// StdIO Message Types.
51 /// PTP message for the event socket.
52 const MSG_TYPE_EVENT: u8 = 0;
53 /// PTP message for the general socket.
54 const MSG_TYPE_GENERAL: u8 = 1;
55 /// Clock ID message
56 const MSG_TYPE_CLOCK_ID: u8 = 2;
57 /// Send time ACK message
58 const MSG_TYPE_SEND_TIME_ACK: u8 = 3;
59
60 /// Create a new `UdpSocket` for the given port and configure it for PTP.
61 fn create_socket(port: u16, iface: &net::InterfaceInfo, ttl: u32) -> Result<UdpSocket, Error> {
62 let socket = net::create_udp_socket(&Ipv4Addr::UNSPECIFIED, port, Some(iface))
63 .with_context(|| format!("Failed to bind socket to port {}", port))?;
64
65 socket
66 .set_nonblocking(true)
67 .context("Failed setting socket non-blocking")?;
68 socket
69 .set_ttl(ttl)
70 .context("Failed setting TTL on socket")?;
71 socket
72 .set_multicast_ttl_v4(ttl)
73 .context("Failed to set multicast TTL on socket")?;
74
75 Ok(socket)
76 }
77
78 /// Retrieve the list of interfaces based on the available ones and the arguments.
79 fn list_interfaces(args: &args::Args) -> Result<Vec<net::InterfaceInfo>, Error> {
80 let mut ifaces = net::query_interfaces().context("Failed to query network interfaces")?;
81 if ifaces.is_empty() {
82 bail!("No suitable network interfaces for PTP found");
83 }
84
85 if !args.interfaces.is_empty() {
86 ifaces.retain(|iface| {
87 for filter_iface in &args.interfaces {
88 if &iface.name == filter_iface {
89 return true;
90 }
91 if let Some(ref other_name) = iface.other_name {
92 if other_name == filter_iface {
93 return true;
94 }
95 }
96 if let Ok(addr) = filter_iface.parse::<Ipv4Addr>() {
97 if addr == iface.ip_addr {
98 return true;
99 }
100 }
101 }
102
103 info!("Interface {} filtered out", iface.name);
104
105 false
106 });
107
108 if ifaces.is_empty() {
109 bail!("None of the selected network interfaces found");
110 }
111 if ifaces.len() != args.interfaces.len() {
112 bail!("Not all selected network interfaces found");
113 }
114 }
115
116 Ok(ifaces)
117 }
118
119 fn run() -> Result<(), Error> {
120 let args = args::parse_args().context("Failed parsing commandline parameters")?;
121
122 let ifaces = list_interfaces(&args).context("Failed listing interfaces")?;
123
124 let mut sockets = vec![];
125 for iface in &ifaces {
126 info!("Binding to interface {}", iface.name);
127
128 let event_socket = create_socket(PTP_EVENT_PORT, iface, args.ttl)
129 .context("Failed creating event socket")?;
130 let general_socket = create_socket(PTP_GENERAL_PORT, iface, args.ttl)
131 .context("Failed creating general socket")?;
132
133 for socket in [&event_socket, &general_socket].iter() {
134 net::join_multicast_v4(socket, &PTP_MULTICAST_ADDR, iface)
135 .context("Failed to join multicast group")?;
136 }
137
138 sockets.push((event_socket, general_socket));
139 }
140
141 let clock_id = if args.clock_id == 0 {
142 ifaces
143 .iter()
144 .find_map(|iface| iface.hw_addr)
145 .map(|hw_addr| {
146 [
147 hw_addr[0], hw_addr[1], hw_addr[2], 0xff, 0xfe, hw_addr[3], hw_addr[4],
148 hw_addr[5],
149 ]
150 })
151 .unwrap_or_else(rand)
152 } else {
153 args.clock_id.to_be_bytes()
154 };
155 info!("Using clock ID {:?}", clock_id);
156
157 thread::set_priority().context("Failed to set thread priority")?;
158
159 privileges::drop().context("Failed dropping privileges")?;
160
161 let mut poll = io::Poll::new(sockets).context("Failed creating poller")?;
162
163 // Write clock ID first
164 {
165 let mut clock_id_data = [0u8; 3 + 8];
166 {
167 let mut buf = &mut clock_id_data[..];
168 buf.write_u16be(8).expect("Too small clock ID buffer");
169 buf.write_u8(MSG_TYPE_CLOCK_ID)
170 .expect("Too small clock ID buffer");
171 buf.write_all(&clock_id).expect("Too small clock ID buffer");
172 assert!(buf.is_empty(), "Too big clock ID buffer");
173 }
174
175 poll.stdout()
176 .write_all(&clock_id_data)
177 .context("Failed writing to stdout")?;
178 }
179
180 // Now read-write from stdin/stdout and the sockets
181 //
182 // We assume that stdout never blocks and stdin receives a complete valid packet whenever it is
183 // ready and never blocks in the middle of a packet.
184 let mut socket_buffer = [0u8; 8192];
185 let mut stdinout_buffer = [0u8; 8192 + 4 + 8];
186
187 loop {
188 let poll_res = poll.poll().context("Failed polling")?;
189
190 // If any of the sockets are ready, continue reading packets from them until no more
191 // packets are left and directly forward them to stdout.
192 'next_socket: for (idx, type_, socket) in poll_res.ready_sockets() {
193 let idx = *idx;
194 let type_ = *type_;
195
196 // Read all available packets from the socket before going to the next socket.
197 'next_packet: loop {
198 let res = socket.recv_from(&mut socket_buffer);
199
200 let (read, addr) = match res {
201 Err(err) if err.kind() == std::io::ErrorKind::WouldBlock => {
202 continue 'next_socket;
203 }
204 Err(err) => {
205 bail!(
206 source: err,
207 "Failed reading from {:?} socket for interface {}",
208 type_,
209 idx,
210 );
211 }
212 Ok((read, addr)) => (read, addr),
213 };
214
215 let recv_time = clock::time();
216 if args.verbose {
217 trace!(
218 "Received {} bytes from {:?} socket for interface {} from {} at {}",
219 read,
220 type_,
221 idx,
222 addr,
223 recv_time,
224 );
225 }
226
227 let buf = &socket_buffer[..read];
228
229 // Check if this is a valid PTP message, that it is not a PTP message sent from
230 // our own clock ID and in case of a DELAY_RESP that it is for our clock id.
231 let ptp_message = match parse::PtpMessage::parse(buf) {
232 Ok(msg) => msg,
233 Err(err) => {
234 warn!("Received invalid PTP message: {}", err);
235 continue 'next_packet;
236 }
237 };
238
239 if args.verbose {
240 trace!("Received PTP message {:#?}", ptp_message);
241 }
242
243 // The delay request is the only message that is sent
244 // from PTP clock implementation, if others are added
245 // additional match arms should be added.
246 if [PtpMessageType::DELAY_REQ].contains(&ptp_message.message_type) {
247 if args.verbose {
248 trace!("Ignoring our own PTP message");
249 }
250 continue 'next_packet;
251 }
252
253 if let PtpMessagePayload::DelayResp {
254 requesting_port_identity: PtpClockIdentity { clock_identity, .. },
255 ..
256 } = ptp_message.message_payload
257 {
258 if clock_identity != u64::from_be_bytes(clock_id) {
259 if args.verbose {
260 trace!("Ignoring PTP DELAY_RESP message for a different clock");
261 }
262 continue 'next_packet;
263 }
264 }
265
266 {
267 let mut buf = &mut stdinout_buffer[..(read + 4 + 8)];
268 buf.write_u16be(read as u16 + 1 + 8)
269 .expect("Too small stdout buffer");
270 buf.write_u8(if type_ == io::SocketType::EventSocket {
271 MSG_TYPE_EVENT
272 } else {
273 MSG_TYPE_GENERAL
274 })
275 .expect("Too small stdout buffer");
276 buf.write_u8(idx as u8).expect("Too small stdout buffer");
277 buf.write_u64be(recv_time).expect("Too small stdout buffer");
278 buf.write_all(&socket_buffer[..read])
279 .expect("Too small stdout buffer");
280 assert!(buf.is_empty(), "Too big stdout buffer",);
281 }
282
283 let buf = &stdinout_buffer[..(read + 4 + 8)];
284 poll_res
285 .stdout()
286 .write_all(buf)
287 .context("Failed writing to stdout")?;
288 }
289 }
290
291 // After handling the sockets check if a packet is available on stdin, read it and forward
292 // it to the corresponding socket.
293 if let Some(ref mut stdin) = poll_res.stdin() {
294 stdin
295 .read_exact(&mut stdinout_buffer[0..3])
296 .context("Failed reading packet header from stdin")?;
297
298 let size = u16::from_be_bytes([stdinout_buffer[0], stdinout_buffer[1]]);
299 if size as usize > stdinout_buffer.len() {
300 bail!("Invalid packet size on stdin {}", size);
301 }
302 let type_ = stdinout_buffer[2];
303
304 stdin
305 .read_exact(&mut stdinout_buffer[0..size as usize])
306 .context("Failed reading packet body from stdin")?;
307
308 if type_ != MSG_TYPE_EVENT && type_ != MSG_TYPE_GENERAL {
309 warn!("Unexpected stdin message type {}", type_);
310 continue;
311 }
312
313 if size < 1 + 8 + 34 {
314 bail!("Invalid packet body size");
315 }
316
317 let buf = &mut &stdinout_buffer[..(size as usize)];
318
319 let idx = buf.read_u8().expect("Too small stdin buffer");
320 if idx as usize >= ifaces.len() {
321 warn!("Unexpected stdin message interface index {}", idx);
322 continue;
323 }
324
325 if args.verbose {
326 trace!(
327 "Received {} bytes for {} socket for interface {} from stdin",
328 size,
329 if type_ == MSG_TYPE_EVENT {
330 "event"
331 } else {
332 "general"
333 },
334 idx,
335 );
336 }
337
338 let main_send_time = buf.read_u64be().expect("Too small stdin buffer");
339
340 // We require that the main process only ever sends valid PTP messages with the clock
341 // ID assigned by this process.
342 let ptp_message =
343 parse::PtpMessage::parse(buf).context("Parsing PTP message from main process")?;
344 if ptp_message.source_port_identity.clock_identity != u64::from_be_bytes(clock_id) {
345 bail!("PTP message with unexpected clock identity on stdin");
346 }
347
348 if args.verbose {
349 trace!("Received PTP message from stdin {:#?}", ptp_message);
350 }
351
352 let send_time = clock::time();
353 match type_ {
354 MSG_TYPE_EVENT => poll_res
355 .event_socket(idx as usize)
356 .send_to(buf, (PTP_MULTICAST_ADDR, PTP_EVENT_PORT)),
357 MSG_TYPE_GENERAL => poll_res
358 .general_socket(idx as usize)
359 .send_to(buf, (PTP_MULTICAST_ADDR, PTP_GENERAL_PORT)),
360 _ => unreachable!(),
361 }
362 .with_context(|| {
363 format!(
364 "Failed sending to {} socket",
365 if type_ == MSG_TYPE_EVENT {
366 "event"
367 } else {
368 "general"
369 }
370 )
371 })?;
372
373 if args.verbose {
374 trace!(
375 "Sending SEND_TIME_ACK for message type {}, domain number {}, seqnum {} received at {} at {}",
376 u8::from(ptp_message.message_type),
377 ptp_message.domain_number,
378 ptp_message.sequence_id,
379 main_send_time,
380 send_time,
381 );
382 }
383
384 {
385 let mut buf = &mut stdinout_buffer[..(3 + 12)];
386 buf.write_u16be(12).expect("Too small stdout buffer");
387 buf.write_u8(MSG_TYPE_SEND_TIME_ACK)
388 .expect("Too small stdout buffer");
389 buf.write_u64be(send_time).expect("Too small stdout buffer");
390 buf.write_u8(ptp_message.message_type.into())
391 .expect("Too small stdout buffer");
392 buf.write_u8(ptp_message.domain_number)
393 .expect("Too small stdout buffer");
394 buf.write_u16be(ptp_message.sequence_id)
395 .expect("Too small stdout buffer");
396 assert!(buf.is_empty(), "Too big stdout buffer",);
397 }
398
399 let buf = &stdinout_buffer[..(3 + 12)];
400 poll_res
401 .stdout()
402 .write_all(buf)
403 .context("Failed writing to stdout")?;
404 }
405 }
406 }
407
408 /// Custom panic hook so we can print them to stderr in a format the main process understands
409 // `PanicHookInfo` is the new API and only stable since Rust 1.81.
410 #[allow(deprecated)]
411 fn panic_hook(info: &std::panic::PanicInfo) {
412 error!("Panicked. {}", info);
413 }
414
415 fn main() {
416 std::panic::set_hook(Box::new(panic_hook));
417
418 if let Err(err) = run() {
419 error!("Exited with error: {:?}", err);
420 }
421 }