tools/testing/selftests/net/timestamping.c

   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                "  PTPV2 - use PTPv2 messages\n");
  64         exit(1);
  65 }
  66
  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
 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
 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
 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
 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 }