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
317 if (argc < 2)
318 usage(0);
319 interface = argv[1];
320
321 for (i = 2; i < argc; i++) {
322 if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
323 so_timestamp = 1;
324 else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
325 so_timestampns = 1;
326 else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
327 siocgstamp = 1;
328 else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
329 siocgstampns = 1;
330 else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
331 ip_multicast_loop = 1;
332 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
333 so_timestamping_flags |= SOF_TIMESTAMPING_TX_HARDWARE;
334 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
335 so_timestamping_flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
336 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
337 so_timestamping_flags |= SOF_TIMESTAMPING_RX_HARDWARE;
338 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
339 so_timestamping_flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
340 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
341 so_timestamping_flags |= SOF_TIMESTAMPING_SOFTWARE;
342 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
343 so_timestamping_flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
344 else
345 usage(argv[i]);
346 }
347
348 sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
349 if (sock < 0)
350 bail("socket");
351
352 memset(&device, 0, sizeof(device));
353 strncpy(device.ifr_name, interface, sizeof(device.ifr_name));
354 if (ioctl(sock, SIOCGIFADDR, &device) < 0)
355 bail("getting interface IP address");
356
357 memset(&hwtstamp, 0, sizeof(hwtstamp));
358 strncpy(hwtstamp.ifr_name, interface, sizeof(hwtstamp.ifr_name));
359 hwtstamp.ifr_data = (void *)&hwconfig;
360 memset(&hwconfig, 0, sizeof(hwconfig));
361 hwconfig.tx_type =
362 (so_timestamping_flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
363 HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
364 hwconfig.rx_filter =
365 (so_timestamping_flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
366 HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
367 hwconfig_requested = hwconfig;
368 if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
369 if ((errno == EINVAL || errno == ENOTSUP) &&
370 hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
371 hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
372 printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
373 else
374 bail("SIOCSHWTSTAMP");
375 }
376 printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
377 hwconfig_requested.tx_type, hwconfig.tx_type,
378 hwconfig_requested.rx_filter, hwconfig.rx_filter);
379
380
381 addr.sin_family = AF_INET;
382 addr.sin_addr.s_addr = htonl(INADDR_ANY);
383 addr.sin_port = htons(319 );
384 if (bind(sock,
385 (struct sockaddr *)&addr,
386 sizeof(struct sockaddr_in)) < 0)
387 bail("bind");
388
389
390 inet_aton("224.0.1.130", &iaddr);
391 addr.sin_addr = iaddr;
392 imr.imr_multiaddr.s_addr = iaddr.s_addr;
393 imr.imr_interface.s_addr =
394 ((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;
395 if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
396 &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)
397 bail("set multicast");
398
399
400 if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
401 &imr, sizeof(struct ip_mreq)) < 0)
402 bail("join multicast group");
403
404 if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP,
405 &ip_multicast_loop, sizeof(enabled)) < 0) {
406 bail("loop multicast");
407 }
408
409
410 if (so_timestamp &&
411 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
412 &enabled, sizeof(enabled)) < 0)
413 bail("setsockopt SO_TIMESTAMP");
414
415 if (so_timestampns &&
416 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
417 &enabled, sizeof(enabled)) < 0)
418 bail("setsockopt SO_TIMESTAMPNS");
419
420 if (so_timestamping_flags &&
421 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING,
422 &so_timestamping_flags,
423 sizeof(so_timestamping_flags)) < 0)
424 bail("setsockopt SO_TIMESTAMPING");
425
426
427 if (setsockopt(sock, SOL_IP, IP_PKTINFO,
428 &enabled, sizeof(enabled)) < 0)
429 printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));
430
431
432 len = sizeof(val);
433 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
434 printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
435 else
436 printf("SO_TIMESTAMP %d\n", val);
437
438 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
439 printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
440 strerror(errno));
441 else
442 printf("SO_TIMESTAMPNS %d\n", val);
443
444 if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, &len) < 0) {
445 printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
446 strerror(errno));
447 } else {
448 printf("SO_TIMESTAMPING %d\n", val);
449 if (val != so_timestamping_flags)
450 printf(" not the expected value %d\n",
451 so_timestamping_flags);
452 }
453
454
455 gettimeofday(&next, 0);
456 next.tv_sec = (next.tv_sec + 1) / 5 * 5;
457 next.tv_usec = 0;
458 while (1) {
459 struct timeval now;
460 struct timeval delta;
461 long delta_us;
462 int res;
463 fd_set readfs, errorfs;
464
465 gettimeofday(&now, 0);
466 delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
467 (long)(next.tv_usec - now.tv_usec);
468 if (delta_us > 0) {
469
470 delta.tv_sec = delta_us / 1000000;
471 delta.tv_usec = delta_us % 1000000;
472
473 FD_ZERO(&readfs);
474 FD_ZERO(&errorfs);
475 FD_SET(sock, &readfs);
476 FD_SET(sock, &errorfs);
477 printf("%ld.%06ld: select %ldus\n",
478 (long)now.tv_sec, (long)now.tv_usec,
479 delta_us);
480 res = select(sock + 1, &readfs, 0, &errorfs, &delta);
481 gettimeofday(&now, 0);
482 printf("%ld.%06ld: select returned: %d, %s\n",
483 (long)now.tv_sec, (long)now.tv_usec,
484 res,
485 res < 0 ? strerror(errno) : "success");
486 if (res > 0) {
487 if (FD_ISSET(sock, &readfs))
488 printf("ready for reading\n");
489 if (FD_ISSET(sock, &errorfs))
490 printf("has error\n");
491 recvpacket(sock, 0,
492 siocgstamp,
493 siocgstampns);
494 recvpacket(sock, MSG_ERRQUEUE,
495 siocgstamp,
496 siocgstampns);
497 }
498 } else {
499
500 sendpacket(sock,
501 (struct sockaddr *)&addr,
502 sizeof(addr));
503 next.tv_sec += 5;
504 continue;
505 }
506 }
507
508 return 0;
509}
510
