About Kernel Documentation Linux Kernel Contact Linux Resources Linux Blog

Documentation / networking / timestamping / timestamping.c




Custom Search

Based on kernel version 4.0. Page generated on 2015-04-14 21:25 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_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	
85	static void bail(const char *error)
86	{
87		printf("%s: %s\n", error, strerror(errno));
88		exit(1);
89	}
90	
91	static 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	
137	static 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	
153	static 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	
278	static 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	
314	int 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	}
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.