1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
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#include <linux/sockios.h>
48
49#ifndef SO_TIMESTAMPING
50# define SO_TIMESTAMPING 37
51# define SCM_TIMESTAMPING SO_TIMESTAMPING
52#endif
53
54#ifndef SO_TIMESTAMPNS
55# define SO_TIMESTAMPNS 35
56#endif
57
58static void usage(const char *error)
59{
60 if (error)
61 printf("invalid option: %s\n", error);
62 printf("timestamping interface option*\n\n"
63 "Options:\n"
64 " IP_MULTICAST_LOOP - looping outgoing multicasts\n"
65 " SO_TIMESTAMP - normal software time stamping, ms resolution\n"
66 " SO_TIMESTAMPNS - more accurate software time stamping\n"
67 " SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n"
68 " SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n"
69 " SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n"
70 " SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n"
71 " SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n"
72 " SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n"
73 " SIOCGSTAMP - check last socket time stamp\n"
74 " SIOCGSTAMPNS - more accurate socket time stamp\n"
75 " PTPV2 - use PTPv2 messages\n");
76 exit(1);
77}
78
79static void bail(const char *error)
80{
81 printf("%s: %s\n", error, strerror(errno));
82 exit(1);
83}
84
85static const unsigned char sync[] = {
86 0x00, 0x01, 0x00, 0x01,
87 0x5f, 0x44, 0x46, 0x4c,
88 0x54, 0x00, 0x00, 0x00,
89 0x00, 0x00, 0x00, 0x00,
90 0x00, 0x00, 0x00, 0x00,
91 0x01, 0x01,
92
93
94 0x00, 0x01,
95 0x02, 0x03, 0x04, 0x05,
96
97 0x00, 0x01, 0x00, 0x37,
98 0x00, 0x00, 0x00, 0x08,
99 0x00, 0x00, 0x00, 0x00,
100 0x49, 0x05, 0xcd, 0x01,
101 0x29, 0xb1, 0x8d, 0xb0,
102 0x00, 0x00, 0x00, 0x00,
103 0x00, 0x01,
104
105
106 0x00, 0x01,
107 0x02, 0x03, 0x04, 0x05,
108
109 0x00, 0x00, 0x00, 0x37,
110 0x00, 0x00, 0x00, 0x04,
111 0x44, 0x46, 0x4c, 0x54,
112 0x00, 0x00, 0xf0, 0x60,
113 0x00, 0x01, 0x00, 0x00,
114 0x00, 0x00, 0x00, 0x01,
115 0x00, 0x00, 0xf0, 0x60,
116 0x00, 0x00, 0x00, 0x00,
117 0x00, 0x00, 0x00, 0x04,
118 0x44, 0x46, 0x4c, 0x54,
119 0x00, 0x01,
120
121
122 0x00, 0x01,
123 0x02, 0x03, 0x04, 0x05,
124
125 0x00, 0x00, 0x00, 0x00,
126 0x00, 0x00, 0x00, 0x00,
127 0x00, 0x00, 0x00, 0x00,
128 0x00, 0x00, 0x00, 0x00
129};
130
131static const unsigned char sync_v2[] = {
132 0x00, 0x02, 0x00, 0x2C,
133 0x00, 0x00, 0x02, 0x00,
134 0x00, 0x00, 0x00, 0x00,
135 0x00, 0x00, 0x00, 0x00,
136 0x00, 0x00, 0x00, 0x00,
137 0x00, 0x00, 0x00, 0xFF,
138 0xFE, 0x00, 0x00, 0x00,
139 0x00, 0x01, 0x00, 0x01,
140 0x00, 0x00, 0x00, 0x00,
141 0x00, 0x00, 0x00, 0x00,
142 0x00, 0x00, 0x00, 0x00,
143};
144
145static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len, int ptpv2)
146{
147 size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
148 const void *sync_p = ptpv2 ? sync_v2 : sync;
149 struct timeval now;
150 int res;
151
152 res = sendto(sock, sync_p, sync_len, 0, addr, addr_len);
153 gettimeofday(&now, 0);
154 if (res < 0)
155 printf("%s: %s\n", "send", strerror(errno));
156 else
157 printf("%ld.%06ld: sent %d bytes\n",
158 (long)now.tv_sec, (long)now.tv_usec,
159 res);
160}
161
162static void printpacket(struct msghdr *msg, int res,
163 char *data,
164 int sock, int recvmsg_flags,
165 int siocgstamp, int siocgstampns, int ptpv2)
166{
167 struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
168 size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
169 const void *sync_p = ptpv2 ? sync_v2 : sync;
170 struct cmsghdr *cmsg;
171 struct timeval tv;
172 struct timespec ts;
173 struct timeval now;
174
175 gettimeofday(&now, 0);
176
177 printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n",
178 (long)now.tv_sec, (long)now.tv_usec,
179 (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
180 res,
181 inet_ntoa(from_addr->sin_addr),
182 msg->msg_controllen);
183 for (cmsg = CMSG_FIRSTHDR(msg);
184 cmsg;
185 cmsg = CMSG_NXTHDR(msg, cmsg)) {
186 printf(" cmsg len %zu: ", cmsg->cmsg_len);
187 switch (cmsg->cmsg_level) {
188 case SOL_SOCKET:
189 printf("SOL_SOCKET ");
190 switch (cmsg->cmsg_type) {
191 case SO_TIMESTAMP: {
192 struct timeval *stamp =
193 (struct timeval *)CMSG_DATA(cmsg);
194 printf("SO_TIMESTAMP %ld.%06ld",
195 (long)stamp->tv_sec,
196 (long)stamp->tv_usec);
197 break;
198 }
199 case SO_TIMESTAMPNS: {
200 struct timespec *stamp =
201 (struct timespec *)CMSG_DATA(cmsg);
202 printf("SO_TIMESTAMPNS %ld.%09ld",
203 (long)stamp->tv_sec,
204 (long)stamp->tv_nsec);
205 break;
206 }
207 case SO_TIMESTAMPING: {
208 struct timespec *stamp =
209 (struct timespec *)CMSG_DATA(cmsg);
210 printf("SO_TIMESTAMPING ");
211 printf("SW %ld.%09ld ",
212 (long)stamp->tv_sec,
213 (long)stamp->tv_nsec);
214 stamp++;
215
216 stamp++;
217 printf("HW raw %ld.%09ld",
218 (long)stamp->tv_sec,
219 (long)stamp->tv_nsec);
220 break;
221 }
222 default:
223 printf("type %d", cmsg->cmsg_type);
224 break;
225 }
226 break;
227 case IPPROTO_IP:
228 printf("IPPROTO_IP ");
229 switch (cmsg->cmsg_type) {
230 case IP_RECVERR: {
231 struct sock_extended_err *err =
232 (struct sock_extended_err *)CMSG_DATA(cmsg);
233 printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s",
234 strerror(err->ee_errno),
235 err->ee_origin,
236#ifdef SO_EE_ORIGIN_TIMESTAMPING
237 err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ?
238 "bounced packet" : "unexpected origin"
239#else
240 "probably SO_EE_ORIGIN_TIMESTAMPING"
241#endif
242 );
243 if (res < sync_len)
244 printf(" => truncated data?!");
245 else if (!memcmp(sync_p, data + res - sync_len, sync_len))
246 printf(" => GOT OUR DATA BACK (HURRAY!)");
247 break;
248 }
249 case IP_PKTINFO: {
250 struct in_pktinfo *pktinfo =
251 (struct in_pktinfo *)CMSG_DATA(cmsg);
252 printf("IP_PKTINFO interface index %u",
253 pktinfo->ipi_ifindex);
254 break;
255 }
256 default:
257 printf("type %d", cmsg->cmsg_type);
258 break;
259 }
260 break;
261 default:
262 printf("level %d type %d",
263 cmsg->cmsg_level,
264 cmsg->cmsg_type);
265 break;
266 }
267 printf("\n");
268 }
269
270 if (siocgstamp) {
271 if (ioctl(sock, SIOCGSTAMP, &tv))
272 printf(" %s: %s\n", "SIOCGSTAMP", strerror(errno));
273 else
274 printf("SIOCGSTAMP %ld.%06ld\n",
275 (long)tv.tv_sec,
276 (long)tv.tv_usec);
277 }
278 if (siocgstampns) {
279 if (ioctl(sock, SIOCGSTAMPNS, &ts))
280 printf(" %s: %s\n", "SIOCGSTAMPNS", strerror(errno));
281 else
282 printf("SIOCGSTAMPNS %ld.%09ld\n",
283 (long)ts.tv_sec,
284 (long)ts.tv_nsec);
285 }
286}
287
288static void recvpacket(int sock, int recvmsg_flags,
289 int siocgstamp, int siocgstampns, int ptpv2)
290{
291 char data[256];
292 struct msghdr msg;
293 struct iovec entry;
294 struct sockaddr_in from_addr;
295 struct {
296 struct cmsghdr cm;
297 char control[512];
298 } control;
299 int res;
300
301 memset(&msg, 0, sizeof(msg));
302 msg.msg_iov = &entry;
303 msg.msg_iovlen = 1;
304 entry.iov_base = data;
305 entry.iov_len = sizeof(data);
306 msg.msg_name = (caddr_t)&from_addr;
307 msg.msg_namelen = sizeof(from_addr);
308 msg.msg_control = &control;
309 msg.msg_controllen = sizeof(control);
310
311 res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);
312 if (res < 0) {
313 printf("%s %s: %s\n",
314 "recvmsg",
315 (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
316 strerror(errno));
317 } else {
318 printpacket(&msg, res, data,
319 sock, recvmsg_flags,
320 siocgstamp, siocgstampns, ptpv2);
321 }
322}
323
324int main(int argc, char **argv)
325{
326 int so_timestamping_flags = 0;
327 int so_timestamp = 0;
328 int so_timestampns = 0;
329 int siocgstamp = 0;
330 int siocgstampns = 0;
331 int ip_multicast_loop = 0;
332 int ptpv2 = 0;
333 char *interface;
334 int i;
335 int enabled = 1;
336 int sock;
337 struct ifreq device;
338 struct ifreq hwtstamp;
339 struct hwtstamp_config hwconfig, hwconfig_requested;
340 struct sockaddr_in addr;
341 struct ip_mreq imr;
342 struct in_addr iaddr;
343 int val;
344 socklen_t len;
345 struct timeval next;
346 size_t if_len;
347
348 if (argc < 2)
349 usage(0);
350 interface = argv[1];
351 if_len = strlen(interface);
352 if (if_len >= IFNAMSIZ) {
353 printf("interface name exceeds IFNAMSIZ\n");
354 exit(1);
355 }
356
357 for (i = 2; i < argc; i++) {
358 if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
359 so_timestamp = 1;
360 else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
361 so_timestampns = 1;
362 else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
363 siocgstamp = 1;
364 else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
365 siocgstampns = 1;
366 else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
367 ip_multicast_loop = 1;
368 else if (!strcasecmp(argv[i], "PTPV2"))
369 ptpv2 = 1;
370 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
371 so_timestamping_flags |= SOF_TIMESTAMPING_TX_HARDWARE;
372 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
373 so_timestamping_flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
374 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
375 so_timestamping_flags |= SOF_TIMESTAMPING_RX_HARDWARE;
376 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
377 so_timestamping_flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
378 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
379 so_timestamping_flags |= SOF_TIMESTAMPING_SOFTWARE;
380 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
381 so_timestamping_flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
382 else
383 usage(argv[i]);
384 }
385
386 sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
387 if (sock < 0)
388 bail("socket");
389
390 memset(&device, 0, sizeof(device));
391 memcpy(device.ifr_name, interface, if_len + 1);
392 if (ioctl(sock, SIOCGIFADDR, &device) < 0)
393 bail("getting interface IP address");
394
395 memset(&hwtstamp, 0, sizeof(hwtstamp));
396 memcpy(hwtstamp.ifr_name, interface, if_len + 1);
397 hwtstamp.ifr_data = (void *)&hwconfig;
398 memset(&hwconfig, 0, sizeof(hwconfig));
399 hwconfig.tx_type =
400 (so_timestamping_flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
401 HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
402 hwconfig.rx_filter =
403 (so_timestamping_flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
404 ptpv2 ? HWTSTAMP_FILTER_PTP_V2_L4_SYNC :
405 HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
406 hwconfig_requested = hwconfig;
407 if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
408 if ((errno == EINVAL || errno == ENOTSUP) &&
409 hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
410 hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
411 printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
412 else
413 bail("SIOCSHWTSTAMP");
414 }
415 printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
416 hwconfig_requested.tx_type, hwconfig.tx_type,
417 hwconfig_requested.rx_filter, hwconfig.rx_filter);
418
419
420 addr.sin_family = AF_INET;
421 addr.sin_addr.s_addr = htonl(INADDR_ANY);
422 addr.sin_port = htons(319 );
423 if (bind(sock,
424 (struct sockaddr *)&addr,
425 sizeof(struct sockaddr_in)) < 0)
426 bail("bind");
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,
461 &so_timestamping_flags,
462 sizeof(so_timestamping_flags)) < 0)
463 bail("setsockopt SO_TIMESTAMPING");
464
465
466 if (setsockopt(sock, SOL_IP, IP_PKTINFO,
467 &enabled, sizeof(enabled)) < 0)
468 printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));
469
470
471 len = sizeof(val);
472 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
473 printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
474 else
475 printf("SO_TIMESTAMP %d\n", val);
476
477 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
478 printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
479 strerror(errno));
480 else
481 printf("SO_TIMESTAMPNS %d\n", val);
482
483 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, &len) < 0) {
484 printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
485 strerror(errno));
486 } else {
487 printf("SO_TIMESTAMPING %d\n", val);
488 if (val != so_timestamping_flags)
489 printf(" not the expected value %d\n",
490 so_timestamping_flags);
491 }
492
493
494 gettimeofday(&next, 0);
495 next.tv_sec = (next.tv_sec + 1) / 5 * 5;
496 next.tv_usec = 0;
497 while (1) {
498 struct timeval now;
499 struct timeval delta;
500 long delta_us;
501 int res;
502 fd_set readfs, errorfs;
503
504 gettimeofday(&now, 0);
505 delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
506 (long)(next.tv_usec - now.tv_usec);
507 if (delta_us > 0) {
508
509 delta.tv_sec = delta_us / 1000000;
510 delta.tv_usec = delta_us % 1000000;
511
512 FD_ZERO(&readfs);
513 FD_ZERO(&errorfs);
514 FD_SET(sock, &readfs);
515 FD_SET(sock, &errorfs);
516 printf("%ld.%06ld: select %ldus\n",
517 (long)now.tv_sec, (long)now.tv_usec,
518 delta_us);
519 res = select(sock + 1, &readfs, 0, &errorfs, &delta);
520 gettimeofday(&now, 0);
521 printf("%ld.%06ld: select returned: %d, %s\n",
522 (long)now.tv_sec, (long)now.tv_usec,
523 res,
524 res < 0 ? strerror(errno) : "success");
525 if (res > 0) {
526 if (FD_ISSET(sock, &readfs))
527 printf("ready for reading\n");
528 if (FD_ISSET(sock, &errorfs))
529 printf("has error\n");
530 recvpacket(sock, 0,
531 siocgstamp,
532 siocgstampns, ptpv2);
533 recvpacket(sock, MSG_ERRQUEUE,
534 siocgstamp,
535 siocgstampns, ptpv2);
536 }
537 } else {
538
539 sendpacket(sock,
540 (struct sockaddr *)&addr,
541 sizeof(addr), ptpv2);
542 next.tv_sec += 5;
543 continue;
544 }
545 }
546
547 return 0;
548}
549
