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