tools/testing/selftests/networking/timestamping/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
 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 }