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         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 }