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