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