1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
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
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
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
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
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, -1 };
329 struct so_timestamping so_timestamping = { 0, -1 };
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
417 addr.sin_family = AF_INET;
418 addr.sin_addr.s_addr = htonl(INADDR_ANY);
419 addr.sin_port = htons(319 );
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
429 inet_aton("224.0.1.130", &iaddr);
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
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
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
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
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
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
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
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
