1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * This program demonstrates how the various time stamping features in
4  * the Linux kernel work. It emulates the behavior of a PTP
5  * implementation in stand-alone master mode by sending PTPv1 Sync
6  * multicasts once every second. It looks for similar packets, but
7  * beyond that doesn't actually implement PTP.
8  *
9  * Outgoing packets are time stamped with SO_TIMESTAMPING with or
10  * without hardware support.
11  *
12  * Incoming packets are time stamped with SO_TIMESTAMPING with or
13  * without hardware support, SIOCGSTAMP[NS] (per-socket time stamp) and
14  * SO_TIMESTAMP[NS].
15  *
16  * Copyright (C) 2009 Intel Corporation.
17  * Author: Patrick Ohly <patrick.ohly@intel.com>
18  */
19 
20 #include <stdio.h>
21 #include <stdlib.h>
22 #include <errno.h>
23 #include <string.h>
24 
25 #include <sys/time.h>
26 #include <sys/socket.h>
27 #include <sys/select.h>
28 #include <sys/ioctl.h>
29 #include <arpa/inet.h>
30 #include <net/if.h>
31 
32 #include <asm/types.h>
33 #include <linux/net_tstamp.h>
34 #include <linux/errqueue.h>
35 #include <linux/sockios.h>
36 
37 #ifndef SO_TIMESTAMPING
38 # define SO_TIMESTAMPING         37
39 # define SCM_TIMESTAMPING        SO_TIMESTAMPING
40 #endif
41 
42 #ifndef SO_TIMESTAMPNS
43 # define SO_TIMESTAMPNS 35
44 #endif
45 
46 static void usage(const char *error)
47 {
48 	if (error)
49 		printf("invalid option: %s\n", error);
50 	printf("timestamping interface option*\n\n"
51 	       "Options:\n"
52 	       "  IP_MULTICAST_LOOP - looping outgoing multicasts\n"
53 	       "  SO_TIMESTAMP - normal software time stamping, ms resolution\n"
54 	       "  SO_TIMESTAMPNS - more accurate software time stamping\n"
55 	       "  SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n"
56 	       "  SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n"
57 	       "  SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n"
58 	       "  SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n"
59 	       "  SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n"
60 	       "  SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n"
61 	       "  SIOCGSTAMP - check last socket time stamp\n"
62 	       "  SIOCGSTAMPNS - more accurate socket time stamp\n"
63 	       "  PTPV2 - use PTPv2 messages\n");
64 	exit(1);
65 }
66 
67 static void bail(const char *error)
68 {
69 	printf("%s: %s\n", error, strerror(errno));
70 	exit(1);
71 }
72 
73 static const unsigned char sync[] = {
74 	0x00, 0x01, 0x00, 0x01,
75 	0x5f, 0x44, 0x46, 0x4c,
76 	0x54, 0x00, 0x00, 0x00,
77 	0x00, 0x00, 0x00, 0x00,
78 	0x00, 0x00, 0x00, 0x00,
79 	0x01, 0x01,
80 
81 	/* fake uuid */
82 	0x00, 0x01,
83 	0x02, 0x03, 0x04, 0x05,
84 
85 	0x00, 0x01, 0x00, 0x37,
86 	0x00, 0x00, 0x00, 0x08,
87 	0x00, 0x00, 0x00, 0x00,
88 	0x49, 0x05, 0xcd, 0x01,
89 	0x29, 0xb1, 0x8d, 0xb0,
90 	0x00, 0x00, 0x00, 0x00,
91 	0x00, 0x01,
92 
93 	/* fake uuid */
94 	0x00, 0x01,
95 	0x02, 0x03, 0x04, 0x05,
96 
97 	0x00, 0x00, 0x00, 0x37,
98 	0x00, 0x00, 0x00, 0x04,
99 	0x44, 0x46, 0x4c, 0x54,
100 	0x00, 0x00, 0xf0, 0x60,
101 	0x00, 0x01, 0x00, 0x00,
102 	0x00, 0x00, 0x00, 0x01,
103 	0x00, 0x00, 0xf0, 0x60,
104 	0x00, 0x00, 0x00, 0x00,
105 	0x00, 0x00, 0x00, 0x04,
106 	0x44, 0x46, 0x4c, 0x54,
107 	0x00, 0x01,
108 
109 	/* fake uuid */
110 	0x00, 0x01,
111 	0x02, 0x03, 0x04, 0x05,
112 
113 	0x00, 0x00, 0x00, 0x00,
114 	0x00, 0x00, 0x00, 0x00,
115 	0x00, 0x00, 0x00, 0x00,
116 	0x00, 0x00, 0x00, 0x00
117 };
118 
119 static const unsigned char sync_v2[] = {
120 	0x00, 0x02, 0x00, 0x2C,
121 	0x00, 0x00, 0x02, 0x00,
122 	0x00, 0x00, 0x00, 0x00,
123 	0x00, 0x00, 0x00, 0x00,
124 	0x00, 0x00, 0x00, 0x00,
125 	0x00, 0x00, 0x00, 0xFF,
126 	0xFE, 0x00, 0x00, 0x00,
127 	0x00, 0x01, 0x00, 0x01,
128 	0x00, 0x00, 0x00, 0x00,
129 	0x00, 0x00, 0x00, 0x00,
130 	0x00, 0x00, 0x00, 0x00,
131 };
132 
133 static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len, int ptpv2)
134 {
135 	size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
136 	const void *sync_p = ptpv2 ? sync_v2 : sync;
137 	struct timeval now;
138 	int res;
139 
140 	res = sendto(sock, sync_p, sync_len, 0, addr, addr_len);
141 	gettimeofday(&now, 0);
142 	if (res < 0)
143 		printf("%s: %s\n", "send", strerror(errno));
144 	else
145 		printf("%ld.%06ld: sent %d bytes\n",
146 		       (long)now.tv_sec, (long)now.tv_usec,
147 		       res);
148 }
149 
150 static void printpacket(struct msghdr *msg, int res,
151 			char *data,
152 			int sock, int recvmsg_flags,
153 			int siocgstamp, int siocgstampns, int ptpv2)
154 {
155 	struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
156 	size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
157 	const void *sync_p = ptpv2 ? sync_v2 : sync;
158 	struct cmsghdr *cmsg;
159 	struct timeval tv;
160 	struct timespec ts;
161 	struct timeval now;
162 
163 	gettimeofday(&now, 0);
164 
165 	printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n",
166 	       (long)now.tv_sec, (long)now.tv_usec,
167 	       (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
168 	       res,
169 	       inet_ntoa(from_addr->sin_addr),
170 	       msg->msg_controllen);
171 	for (cmsg = CMSG_FIRSTHDR(msg);
172 	     cmsg;
173 	     cmsg = CMSG_NXTHDR(msg, cmsg)) {
174 		printf("   cmsg len %zu: ", cmsg->cmsg_len);
175 		switch (cmsg->cmsg_level) {
176 		case SOL_SOCKET:
177 			printf("SOL_SOCKET ");
178 			switch (cmsg->cmsg_type) {
179 			case SO_TIMESTAMP: {
180 				struct timeval *stamp =
181 					(struct timeval *)CMSG_DATA(cmsg);
182 				printf("SO_TIMESTAMP %ld.%06ld",
183 				       (long)stamp->tv_sec,
184 				       (long)stamp->tv_usec);
185 				break;
186 			}
187 			case SO_TIMESTAMPNS: {
188 				struct timespec *stamp =
189 					(struct timespec *)CMSG_DATA(cmsg);
190 				printf("SO_TIMESTAMPNS %ld.%09ld",
191 				       (long)stamp->tv_sec,
192 				       (long)stamp->tv_nsec);
193 				break;
194 			}
195 			case SO_TIMESTAMPING: {
196 				struct timespec *stamp =
197 					(struct timespec *)CMSG_DATA(cmsg);
198 				printf("SO_TIMESTAMPING ");
199 				printf("SW %ld.%09ld ",
200 				       (long)stamp->tv_sec,
201 				       (long)stamp->tv_nsec);
202 				stamp++;
203 				/* skip deprecated HW transformed */
204 				stamp++;
205 				printf("HW raw %ld.%09ld",
206 				       (long)stamp->tv_sec,
207 				       (long)stamp->tv_nsec);
208 				break;
209 			}
210 			default:
211 				printf("type %d", cmsg->cmsg_type);
212 				break;
213 			}
214 			break;
215 		case IPPROTO_IP:
216 			printf("IPPROTO_IP ");
217 			switch (cmsg->cmsg_type) {
218 			case IP_RECVERR: {
219 				struct sock_extended_err *err =
220 					(struct sock_extended_err *)CMSG_DATA(cmsg);
221 				printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s",
222 					strerror(err->ee_errno),
223 					err->ee_origin,
224 #ifdef SO_EE_ORIGIN_TIMESTAMPING
225 					err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ?
226 					"bounced packet" : "unexpected origin"
227 #else
228 					"probably SO_EE_ORIGIN_TIMESTAMPING"
229 #endif
230 					);
231 				if (res < sync_len)
232 					printf(" => truncated data?!");
233 				else if (!memcmp(sync_p, data + res - sync_len, sync_len))
234 					printf(" => GOT OUR DATA BACK (HURRAY!)");
235 				break;
236 			}
237 			case IP_PKTINFO: {
238 				struct in_pktinfo *pktinfo =
239 					(struct in_pktinfo *)CMSG_DATA(cmsg);
240 				printf("IP_PKTINFO interface index %u",
241 					pktinfo->ipi_ifindex);
242 				break;
243 			}
244 			default:
245 				printf("type %d", cmsg->cmsg_type);
246 				break;
247 			}
248 			break;
249 		default:
250 			printf("level %d type %d",
251 				cmsg->cmsg_level,
252 				cmsg->cmsg_type);
253 			break;
254 		}
255 		printf("\n");
256 	}
257 
258 	if (siocgstamp) {
259 		if (ioctl(sock, SIOCGSTAMP, &tv))
260 			printf("   %s: %s\n", "SIOCGSTAMP", strerror(errno));
261 		else
262 			printf("SIOCGSTAMP %ld.%06ld\n",
263 			       (long)tv.tv_sec,
264 			       (long)tv.tv_usec);
265 	}
266 	if (siocgstampns) {
267 		if (ioctl(sock, SIOCGSTAMPNS, &ts))
268 			printf("   %s: %s\n", "SIOCGSTAMPNS", strerror(errno));
269 		else
270 			printf("SIOCGSTAMPNS %ld.%09ld\n",
271 			       (long)ts.tv_sec,
272 			       (long)ts.tv_nsec);
273 	}
274 }
275 
276 static void recvpacket(int sock, int recvmsg_flags,
277 		       int siocgstamp, int siocgstampns, int ptpv2)
278 {
279 	char data[256];
280 	struct msghdr msg;
281 	struct iovec entry;
282 	struct sockaddr_in from_addr;
283 	struct {
284 		struct cmsghdr cm;
285 		char control[512];
286 	} control;
287 	int res;
288 
289 	memset(&msg, 0, sizeof(msg));
290 	msg.msg_iov = &entry;
291 	msg.msg_iovlen = 1;
292 	entry.iov_base = data;
293 	entry.iov_len = sizeof(data);
294 	msg.msg_name = (caddr_t)&from_addr;
295 	msg.msg_namelen = sizeof(from_addr);
296 	msg.msg_control = &control;
297 	msg.msg_controllen = sizeof(control);
298 
299 	res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);
300 	if (res < 0) {
301 		printf("%s %s: %s\n",
302 		       "recvmsg",
303 		       (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
304 		       strerror(errno));
305 	} else {
306 		printpacket(&msg, res, data,
307 			    sock, recvmsg_flags,
308 			    siocgstamp, siocgstampns, ptpv2);
309 	}
310 }
311 
312 int main(int argc, char **argv)
313 {
314 	int so_timestamping_flags = 0;
315 	int so_timestamp = 0;
316 	int so_timestampns = 0;
317 	int siocgstamp = 0;
318 	int siocgstampns = 0;
319 	int ip_multicast_loop = 0;
320 	int ptpv2 = 0;
321 	char *interface;
322 	int i;
323 	int enabled = 1;
324 	int sock;
325 	struct ifreq device;
326 	struct ifreq hwtstamp;
327 	struct hwtstamp_config hwconfig, hwconfig_requested;
328 	struct sockaddr_in addr;
329 	struct ip_mreq imr;
330 	struct in_addr iaddr;
331 	int val;
332 	socklen_t len;
333 	struct timeval next;
334 	size_t if_len;
335 
336 	if (argc < 2)
337 		usage(0);
338 	interface = argv[1];
339 	if_len = strlen(interface);
340 	if (if_len >= IFNAMSIZ) {
341 		printf("interface name exceeds IFNAMSIZ\n");
342 		exit(1);
343 	}
344 
345 	for (i = 2; i < argc; i++) {
346 		if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
347 			so_timestamp = 1;
348 		else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
349 			so_timestampns = 1;
350 		else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
351 			siocgstamp = 1;
352 		else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
353 			siocgstampns = 1;
354 		else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
355 			ip_multicast_loop = 1;
356 		else if (!strcasecmp(argv[i], "PTPV2"))
357 			ptpv2 = 1;
358 		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
359 			so_timestamping_flags |= SOF_TIMESTAMPING_TX_HARDWARE;
360 		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
361 			so_timestamping_flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
362 		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
363 			so_timestamping_flags |= SOF_TIMESTAMPING_RX_HARDWARE;
364 		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
365 			so_timestamping_flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
366 		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
367 			so_timestamping_flags |= SOF_TIMESTAMPING_SOFTWARE;
368 		else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
369 			so_timestamping_flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
370 		else
371 			usage(argv[i]);
372 	}
373 
374 	sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
375 	if (sock < 0)
376 		bail("socket");
377 
378 	memset(&device, 0, sizeof(device));
379 	memcpy(device.ifr_name, interface, if_len + 1);
380 	if (ioctl(sock, SIOCGIFADDR, &device) < 0)
381 		bail("getting interface IP address");
382 
383 	memset(&hwtstamp, 0, sizeof(hwtstamp));
384 	memcpy(hwtstamp.ifr_name, interface, if_len + 1);
385 	hwtstamp.ifr_data = (void *)&hwconfig;
386 	memset(&hwconfig, 0, sizeof(hwconfig));
387 	hwconfig.tx_type =
388 		(so_timestamping_flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
389 		HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
390 	hwconfig.rx_filter =
391 		(so_timestamping_flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
392 		ptpv2 ? HWTSTAMP_FILTER_PTP_V2_L4_SYNC :
393 		HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
394 	hwconfig_requested = hwconfig;
395 	if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
396 		if ((errno == EINVAL || errno == ENOTSUP) &&
397 		    hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
398 		    hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
399 			printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
400 		else
401 			bail("SIOCSHWTSTAMP");
402 	}
403 	printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
404 	       hwconfig_requested.tx_type, hwconfig.tx_type,
405 	       hwconfig_requested.rx_filter, hwconfig.rx_filter);
406 
407 	/* bind to PTP port */
408 	addr.sin_family = AF_INET;
409 	addr.sin_addr.s_addr = htonl(INADDR_ANY);
410 	addr.sin_port = htons(319 /* PTP event port */);
411 	if (bind(sock,
412 		 (struct sockaddr *)&addr,
413 		 sizeof(struct sockaddr_in)) < 0)
414 		bail("bind");
415 
416 	/* set multicast group for outgoing packets */
417 	inet_aton("224.0.1.130", &iaddr); /* alternate PTP domain 1 */
418 	addr.sin_addr = iaddr;
419 	imr.imr_multiaddr.s_addr = iaddr.s_addr;
420 	imr.imr_interface.s_addr =
421 		((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;
422 	if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
423 		       &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)
424 		bail("set multicast");
425 
426 	/* join multicast group, loop our own packet */
427 	if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
428 		       &imr, sizeof(struct ip_mreq)) < 0)
429 		bail("join multicast group");
430 
431 	if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP,
432 		       &ip_multicast_loop, sizeof(enabled)) < 0) {
433 		bail("loop multicast");
434 	}
435 
436 	/* set socket options for time stamping */
437 	if (so_timestamp &&
438 		setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
439 			   &enabled, sizeof(enabled)) < 0)
440 		bail("setsockopt SO_TIMESTAMP");
441 
442 	if (so_timestampns &&
443 		setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
444 			   &enabled, sizeof(enabled)) < 0)
445 		bail("setsockopt SO_TIMESTAMPNS");
446 
447 	if (so_timestamping_flags &&
448 		setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING,
449 			   &so_timestamping_flags,
450 			   sizeof(so_timestamping_flags)) < 0)
451 		bail("setsockopt SO_TIMESTAMPING");
452 
453 	/* request IP_PKTINFO for debugging purposes */
454 	if (setsockopt(sock, SOL_IP, IP_PKTINFO,
455 		       &enabled, sizeof(enabled)) < 0)
456 		printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));
457 
458 	/* verify socket options */
459 	len = sizeof(val);
460 	if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
461 		printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
462 	else
463 		printf("SO_TIMESTAMP %d\n", val);
464 
465 	if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
466 		printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
467 		       strerror(errno));
468 	else
469 		printf("SO_TIMESTAMPNS %d\n", val);
470 
471 	if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, &len) < 0) {
472 		printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
473 		       strerror(errno));
474 	} else {
475 		printf("SO_TIMESTAMPING %d\n", val);
476 		if (val != so_timestamping_flags)
477 			printf("   not the expected value %d\n",
478 			       so_timestamping_flags);
479 	}
480 
481 	/* send packets forever every five seconds */
482 	gettimeofday(&next, 0);
483 	next.tv_sec = (next.tv_sec + 1) / 5 * 5;
484 	next.tv_usec = 0;
485 	while (1) {
486 		struct timeval now;
487 		struct timeval delta;
488 		long delta_us;
489 		int res;
490 		fd_set readfs, errorfs;
491 
492 		gettimeofday(&now, 0);
493 		delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
494 			(long)(next.tv_usec - now.tv_usec);
495 		if (delta_us > 0) {
496 			/* continue waiting for timeout or data */
497 			delta.tv_sec = delta_us / 1000000;
498 			delta.tv_usec = delta_us % 1000000;
499 
500 			FD_ZERO(&readfs);
501 			FD_ZERO(&errorfs);
502 			FD_SET(sock, &readfs);
503 			FD_SET(sock, &errorfs);
504 			printf("%ld.%06ld: select %ldus\n",
505 			       (long)now.tv_sec, (long)now.tv_usec,
506 			       delta_us);
507 			res = select(sock + 1, &readfs, 0, &errorfs, &delta);
508 			gettimeofday(&now, 0);
509 			printf("%ld.%06ld: select returned: %d, %s\n",
510 			       (long)now.tv_sec, (long)now.tv_usec,
511 			       res,
512 			       res < 0 ? strerror(errno) : "success");
513 			if (res > 0) {
514 				if (FD_ISSET(sock, &readfs))
515 					printf("ready for reading\n");
516 				if (FD_ISSET(sock, &errorfs))
517 					printf("has error\n");
518 				recvpacket(sock, 0,
519 					   siocgstamp,
520 					   siocgstampns, ptpv2);
521 				recvpacket(sock, MSG_ERRQUEUE,
522 					   siocgstamp,
523 					   siocgstampns, ptpv2);
524 			}
525 		} else {
526 			/* write one packet */
527 			sendpacket(sock,
528 				   (struct sockaddr *)&addr,
529 				   sizeof(addr), ptpv2);
530 			next.tv_sec += 5;
531 			continue;
532 		}
533 	}
534 
535 	return 0;
536 }
537