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