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