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