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 
usage(const char * error)46 static 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 
bail(const char * error)68 static void bail(const char *error)
69 {
70 	printf("%s: %s\n", error, strerror(errno));
71 	exit(1);
72 }
73 
74 static 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 
120 static 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 
sendpacket(int sock,struct sockaddr * addr,socklen_t addr_len,int ptpv2)134 static 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 
printpacket(struct msghdr * msg,int res,char * data,int sock,int recvmsg_flags,int siocgstamp,int siocgstampns,int ptpv2)151 static 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 
recvpacket(int sock,int recvmsg_flags,int siocgstamp,int siocgstampns,int ptpv2)277 static 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 
main(int argc,char ** argv)313 int 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