About Kernel Documentation Linux Kernel Contact Linux Resources Linux Blog

Documentation / networking / timestamping / timestamping.c




Custom Search

Based on kernel version 3.16. Page generated on 2014-08-06 21:40 EST.

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

Information is copyright its respective author. All material is available from the Linux Kernel Source distributed under a GPL License. This page is provided as a free service by mjmwired.net.