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