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