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