1/* 2 * This program demonstrates how the various time stamping features in 3 * the Linux kernel work. It emulates the behavior of a PTP 4 * implementation in stand-alone master mode by sending PTPv1 Sync 5 * multicasts once every second. It looks for similar packets, but 6 * beyond that doesn't actually implement PTP. 7 * 8 * Outgoing packets are time stamped with SO_TIMESTAMPING with or 9 * without hardware support. 10 * 11 * Incoming packets are time stamped with SO_TIMESTAMPING with or 12 * without hardware support, SIOCGSTAMP[NS] (per-socket time stamp) and 13 * SO_TIMESTAMP[NS]. 14 * 15 * Copyright (C) 2009 Intel Corporation. 16 * Author: Patrick Ohly <patrick.ohly@intel.com> 17 * 18 * This program is free software; you can redistribute it and/or modify it 19 * under the terms and conditions of the GNU General Public License, 20 * version 2, as published by the Free Software Foundation. 21 * 22 * This program is distributed in the hope it will be useful, but WITHOUT 23 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 24 * FITNESS FOR A PARTICULAR PURPOSE. * See the GNU General Public License for 25 * more details. 26 * 27 * You should have received a copy of the GNU General Public License along with 28 * this program; if not, write to the Free Software Foundation, Inc., 29 * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. 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 /* fake uuid */ 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 /* fake uuid */ 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 /* fake uuid */ 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, %zu 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 %zu: ", 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 (sock < 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 /* bind to PTP port */ 405 addr.sin_family = AF_INET; 406 addr.sin_addr.s_addr = htonl(INADDR_ANY); 407 addr.sin_port = htons(319 /* PTP event port */); 408 if (bind(sock, 409 (struct sockaddr *)&addr, 410 sizeof(struct sockaddr_in)) < 0) 411 bail("bind"); 412 413 /* set multicast group for outgoing packets */ 414 inet_aton("224.0.1.130", &iaddr); /* alternate PTP domain 1 */ 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 /* join multicast group, loop our own packet */ 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 /* set socket options for time stamping */ 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 /* request IP_PKTINFO for debugging purposes */ 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 /* verify socket options */ 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 /* send packets forever every five seconds */ 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 /* continue waiting for timeout or data */ 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 /* write one packet */ 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