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
  36 #ifndef SO_TIMESTAMPING
  37 # define SO_TIMESTAMPING         37
  38 # define SCM_TIMESTAMPING        SO_TIMESTAMPING
  39 #endif
  40
  41 #ifndef SO_TIMESTAMPNS
  42 # define SO_TIMESTAMPNS 35
  43 #endif
  44
  45 #ifndef SIOCGSTAMPNS
  46 # define SIOCGSTAMPNS 0x8907
  47 #endif
  48
  49 #ifndef SIOCSHWTSTAMP
  50 # define SIOCSHWTSTAMP 0x89b0
  51 #endif
  52
  53 static void usage(const char *error)
  54 {
  55         if (error)
  56                 printf("invalid option: %s\n", error);
  57         printf("timestamping interface option*\n\n"
  58                "Options:\n"
  59                "  IP_MULTICAST_LOOP - looping outgoing multicasts\n"
  60                "  SO_TIMESTAMP - normal software time stamping, ms resolution\n"
  61                "  SO_TIMESTAMPNS - more accurate software time stamping\n"
  62                "  SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n"
  63                "  SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n"
  64                "  SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n"
  65                "  SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n"
  66                "  SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n"
  67                "  SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n"
  68                "  SIOCGSTAMP - check last socket time stamp\n"
  69                "  SIOCGSTAMPNS - more accurate socket time stamp\n");
  70         exit(1);
  71 }
  72
  73 static void bail(const char *error)
  74 {
  75         printf("%s: %s\n", error, strerror(errno));
  76         exit(1);
  77 }
  78
  79 static const unsigned char sync[] = {
  80         0x00, 0x01, 0x00, 0x01,
  81         0x5f, 0x44, 0x46, 0x4c,
  82         0x54, 0x00, 0x00, 0x00,
  83         0x00, 0x00, 0x00, 0x00,
  84         0x00, 0x00, 0x00, 0x00,
  85         0x01, 0x01,
  86
  87         /* fake uuid */
  88         0x00, 0x01,
  89         0x02, 0x03, 0x04, 0x05,
  90
  91         0x00, 0x01, 0x00, 0x37,
  92         0x00, 0x00, 0x00, 0x08,
  93         0x00, 0x00, 0x00, 0x00,
  94         0x49, 0x05, 0xcd, 0x01,
  95         0x29, 0xb1, 0x8d, 0xb0,
  96         0x00, 0x00, 0x00, 0x00,
  97         0x00, 0x01,
  98
  99         /* fake uuid */
 100         0x00, 0x01,
 101         0x02, 0x03, 0x04, 0x05,
 102
 103         0x00, 0x00, 0x00, 0x37,
 104         0x00, 0x00, 0x00, 0x04,
 105         0x44, 0x46, 0x4c, 0x54,
 106         0x00, 0x00, 0xf0, 0x60,
 107         0x00, 0x01, 0x00, 0x00,
 108         0x00, 0x00, 0x00, 0x01,
 109         0x00, 0x00, 0xf0, 0x60,
 110         0x00, 0x00, 0x00, 0x00,
 111         0x00, 0x00, 0x00, 0x04,
 112         0x44, 0x46, 0x4c, 0x54,
 113         0x00, 0x01,
 114
 115         /* fake uuid */
 116         0x00, 0x01,
 117         0x02, 0x03, 0x04, 0x05,
 118
 119         0x00, 0x00, 0x00, 0x00,
 120         0x00, 0x00, 0x00, 0x00,
 121         0x00, 0x00, 0x00, 0x00,
 122         0x00, 0x00, 0x00, 0x00
 123 };
 124
 125 static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len)
 126 {
 127         struct timeval now;
 128         int res;
 129
 130         res = sendto(sock, sync, sizeof(sync), 0,
 131                 addr, addr_len);
 132         gettimeofday(&now, 0);
 133         if (res < 0)
 134                 printf("%s: %s\n", "send", strerror(errno));
 135         else
 136                 printf("%ld.%06ld: sent %d bytes\n",
 137                        (long)now.tv_sec, (long)now.tv_usec,
 138                        res);
 139 }
 140
 141 static void printpacket(struct msghdr *msg, int res,
 142                         char *data,
 143                         int sock, int recvmsg_flags,
 144                         int siocgstamp, int siocgstampns)
 145 {
 146         struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
 147         struct cmsghdr *cmsg;
 148         struct timeval tv;
 149         struct timespec ts;
 150         struct timeval now;
 151
 152         gettimeofday(&now, 0);
 153
 154         printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n",
 155                (long)now.tv_sec, (long)now.tv_usec,
 156                (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
 157                res,
 158                inet_ntoa(from_addr->sin_addr),
 159                msg->msg_controllen);
 160         for (cmsg = CMSG_FIRSTHDR(msg);
 161              cmsg;
 162              cmsg = CMSG_NXTHDR(msg, cmsg)) {
 163                 printf("   cmsg len %zu: ", cmsg->cmsg_len);
 164                 switch (cmsg->cmsg_level) {
 165                 case SOL_SOCKET:
 166                         printf("SOL_SOCKET ");
 167                         switch (cmsg->cmsg_type) {
 168                         case SO_TIMESTAMP: {
 169                                 struct timeval *stamp =
 170                                         (struct timeval *)CMSG_DATA(cmsg);
 171                                 printf("SO_TIMESTAMP %ld.%06ld",
 172                                        (long)stamp->tv_sec,
 173                                        (long)stamp->tv_usec);
 174                                 break;
 175                         }
 176                         case SO_TIMESTAMPNS: {
 177                                 struct timespec *stamp =
 178                                         (struct timespec *)CMSG_DATA(cmsg);
 179                                 printf("SO_TIMESTAMPNS %ld.%09ld",
 180                                        (long)stamp->tv_sec,
 181                                        (long)stamp->tv_nsec);
 182                                 break;
 183                         }
 184                         case SO_TIMESTAMPING: {
 185                                 struct timespec *stamp =
 186                                         (struct timespec *)CMSG_DATA(cmsg);
 187                                 printf("SO_TIMESTAMPING ");
 188                                 printf("SW %ld.%09ld ",
 189                                        (long)stamp->tv_sec,
 190                                        (long)stamp->tv_nsec);
 191                                 stamp++;
 192                                 /* skip deprecated HW transformed */
 193                                 stamp++;
 194                                 printf("HW raw %ld.%09ld",
 195                                        (long)stamp->tv_sec,
 196                                        (long)stamp->tv_nsec);
 197                                 break;
 198                         }
 199                         default:
 200                                 printf("type %d", cmsg->cmsg_type);
 201                                 break;
 202                         }
 203                         break;
 204                 case IPPROTO_IP:
 205                         printf("IPPROTO_IP ");
 206                         switch (cmsg->cmsg_type) {
 207                         case IP_RECVERR: {
 208                                 struct sock_extended_err *err =
 209                                         (struct sock_extended_err *)CMSG_DATA(cmsg);
 210                                 printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s",
 211                                         strerror(err->ee_errno),
 212                                         err->ee_origin,
 213 #ifdef SO_EE_ORIGIN_TIMESTAMPING
 214                                         err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ?
 215                                         "bounced packet" : "unexpected origin"
 216 #else
 217                                         "probably SO_EE_ORIGIN_TIMESTAMPING"
 218 #endif
 219                                         );
 220                                 if (res < sizeof(sync))
 221                                         printf(" => truncated data?!");
 222                                 else if (!memcmp(sync, data + res - sizeof(sync),
 223                                                         sizeof(sync)))
 224                                         printf(" => GOT OUR DATA BACK (HURRAY!)");
 225                                 break;
 226                         }
 227                         case IP_PKTINFO: {
 228                                 struct in_pktinfo *pktinfo =
 229                                         (struct in_pktinfo *)CMSG_DATA(cmsg);
 230                                 printf("IP_PKTINFO interface index %u",
 231                                         pktinfo->ipi_ifindex);
 232                                 break;
 233                         }
 234                         default:
 235                                 printf("type %d", cmsg->cmsg_type);
 236                                 break;
 237                         }
 238                         break;
 239                 default:
 240                         printf("level %d type %d",
 241                                 cmsg->cmsg_level,
 242                                 cmsg->cmsg_type);
 243                         break;
 244                 }
 245                 printf("\n");
 246         }
 247
 248         if (siocgstamp) {
 249                 if (ioctl(sock, SIOCGSTAMP, &tv))
 250                         printf("   %s: %s\n", "SIOCGSTAMP", strerror(errno));
 251                 else
 252                         printf("SIOCGSTAMP %ld.%06ld\n",
 253                                (long)tv.tv_sec,
 254                                (long)tv.tv_usec);
 255         }
 256         if (siocgstampns) {
 257                 if (ioctl(sock, SIOCGSTAMPNS, &ts))
 258                         printf("   %s: %s\n", "SIOCGSTAMPNS", strerror(errno));
 259                 else
 260                         printf("SIOCGSTAMPNS %ld.%09ld\n",
 261                                (long)ts.tv_sec,
 262                                (long)ts.tv_nsec);
 263         }
 264 }
 265
 266 static void recvpacket(int sock, int recvmsg_flags,
 267                        int siocgstamp, int siocgstampns)
 268 {
 269         char data[256];
 270         struct msghdr msg;
 271         struct iovec entry;
 272         struct sockaddr_in from_addr;
 273         struct {
 274                 struct cmsghdr cm;
 275                 char control[512];
 276         } control;
 277         int res;
 278
 279         memset(&msg, 0, sizeof(msg));
 280         msg.msg_iov = &entry;
 281         msg.msg_iovlen = 1;
 282         entry.iov_base = data;
 283         entry.iov_len = sizeof(data);
 284         msg.msg_name = (caddr_t)&from_addr;
 285         msg.msg_namelen = sizeof(from_addr);
 286         msg.msg_control = &control;
 287         msg.msg_controllen = sizeof(control);
 288
 289         res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);
 290         if (res < 0) {
 291                 printf("%s %s: %s\n",
 292                        "recvmsg",
 293                        (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
 294                        strerror(errno));
 295         } else {
 296                 printpacket(&msg, res, data,
 297                             sock, recvmsg_flags,
 298                             siocgstamp, siocgstampns);
 299         }
 300 }
 301
 302 int main(int argc, char **argv)
 303 {
 304         int so_timestamping_flags = 0;
 305         int so_timestamp = 0;
 306         int so_timestampns = 0;
 307         int siocgstamp = 0;
 308         int siocgstampns = 0;
 309         int ip_multicast_loop = 0;
 310         char *interface;
 311         int i;
 312         int enabled = 1;
 313         int sock;
 314         struct ifreq device;
 315         struct ifreq hwtstamp;
 316         struct hwtstamp_config hwconfig, hwconfig_requested;
 317         struct sockaddr_in addr;
 318         struct ip_mreq imr;
 319         struct in_addr iaddr;
 320         int val;
 321         socklen_t len;
 322         struct timeval next;
 323
 324         if (argc < 2)
 325                 usage(0);
 326         interface = argv[1];
 327
 328         for (i = 2; i < argc; i++) {
 329                 if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
 330                         so_timestamp = 1;
 331                 else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
 332                         so_timestampns = 1;
 333                 else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
 334                         siocgstamp = 1;
 335                 else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
 336                         siocgstampns = 1;
 337                 else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
 338                         ip_multicast_loop = 1;
 339                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
 340                         so_timestamping_flags |= SOF_TIMESTAMPING_TX_HARDWARE;
 341                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
 342                         so_timestamping_flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
 343                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
 344                         so_timestamping_flags |= SOF_TIMESTAMPING_RX_HARDWARE;
 345                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
 346                         so_timestamping_flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
 347                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
 348                         so_timestamping_flags |= SOF_TIMESTAMPING_SOFTWARE;
 349                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
 350                         so_timestamping_flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
 351                 else
 352                         usage(argv[i]);
 353         }
 354
 355         sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
 356         if (sock < 0)
 357                 bail("socket");
 358
 359         memset(&device, 0, sizeof(device));
 360         strncpy(device.ifr_name, interface, sizeof(device.ifr_name));
 361         if (ioctl(sock, SIOCGIFADDR, &device) < 0)
 362                 bail("getting interface IP address");
 363
 364         memset(&hwtstamp, 0, sizeof(hwtstamp));
 365         strncpy(hwtstamp.ifr_name, interface, sizeof(hwtstamp.ifr_name));
 366         hwtstamp.ifr_data = (void *)&hwconfig;
 367         memset(&hwconfig, 0, sizeof(hwconfig));
 368         hwconfig.tx_type =
 369                 (so_timestamping_flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
 370                 HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
 371         hwconfig.rx_filter =
 372                 (so_timestamping_flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
 373                 HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
 374         hwconfig_requested = hwconfig;
 375         if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
 376                 if ((errno == EINVAL || errno == ENOTSUP) &&
 377                     hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
 378                     hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
 379                         printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
 380                 else
 381                         bail("SIOCSHWTSTAMP");
 382         }
 383         printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
 384                hwconfig_requested.tx_type, hwconfig.tx_type,
 385                hwconfig_requested.rx_filter, hwconfig.rx_filter);
 386
 387         /* bind to PTP port */
 388         addr.sin_family = AF_INET;
 389         addr.sin_addr.s_addr = htonl(INADDR_ANY);
 390         addr.sin_port = htons(319 /* PTP event port */);
 391         if (bind(sock,
 392                  (struct sockaddr *)&addr,
 393                  sizeof(struct sockaddr_in)) < 0)
 394                 bail("bind");
 395
 396         /* set multicast group for outgoing packets */
 397         inet_aton("224.0.1.130", &iaddr); /* alternate PTP domain 1 */
 398         addr.sin_addr = iaddr;
 399         imr.imr_multiaddr.s_addr = iaddr.s_addr;
 400         imr.imr_interface.s_addr =
 401                 ((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;
 402         if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
 403                        &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)
 404                 bail("set multicast");
 405
 406         /* join multicast group, loop our own packet */
 407         if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
 408                        &imr, sizeof(struct ip_mreq)) < 0)
 409                 bail("join multicast group");
 410
 411         if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP,
 412                        &ip_multicast_loop, sizeof(enabled)) < 0) {
 413                 bail("loop multicast");
 414         }
 415
 416         /* set socket options for time stamping */
 417         if (so_timestamp &&
 418                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
 419                            &enabled, sizeof(enabled)) < 0)
 420                 bail("setsockopt SO_TIMESTAMP");
 421
 422         if (so_timestampns &&
 423                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
 424                            &enabled, sizeof(enabled)) < 0)
 425                 bail("setsockopt SO_TIMESTAMPNS");
 426
 427         if (so_timestamping_flags &&
 428                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING,
 429                            &so_timestamping_flags,
 430                            sizeof(so_timestamping_flags)) < 0)
 431                 bail("setsockopt SO_TIMESTAMPING");
 432
 433         /* request IP_PKTINFO for debugging purposes */
 434         if (setsockopt(sock, SOL_IP, IP_PKTINFO,
 435                        &enabled, sizeof(enabled)) < 0)
 436                 printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));
 437
 438         /* verify socket options */
 439         len = sizeof(val);
 440         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
 441                 printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
 442         else
 443                 printf("SO_TIMESTAMP %d\n", val);
 444
 445         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
 446                 printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
 447                        strerror(errno));
 448         else
 449                 printf("SO_TIMESTAMPNS %d\n", val);
 450
 451         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, &len) < 0) {
 452                 printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
 453                        strerror(errno));
 454         } else {
 455                 printf("SO_TIMESTAMPING %d\n", val);
 456                 if (val != so_timestamping_flags)
 457                         printf("   not the expected value %d\n",
 458                                so_timestamping_flags);
 459         }
 460
 461         /* send packets forever every five seconds */
 462         gettimeofday(&next, 0);
 463         next.tv_sec = (next.tv_sec + 1) / 5 * 5;
 464         next.tv_usec = 0;
 465         while (1) {
 466                 struct timeval now;
 467                 struct timeval delta;
 468                 long delta_us;
 469                 int res;
 470                 fd_set readfs, errorfs;
 471
 472                 gettimeofday(&now, 0);
 473                 delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
 474                         (long)(next.tv_usec - now.tv_usec);
 475                 if (delta_us > 0) {
 476                         /* continue waiting for timeout or data */
 477                         delta.tv_sec = delta_us / 1000000;
 478                         delta.tv_usec = delta_us % 1000000;
 479
 480                         FD_ZERO(&readfs);
 481                         FD_ZERO(&errorfs);
 482                         FD_SET(sock, &readfs);
 483                         FD_SET(sock, &errorfs);
 484                         printf("%ld.%06ld: select %ldus\n",
 485                                (long)now.tv_sec, (long)now.tv_usec,
 486                                delta_us);
 487                         res = select(sock + 1, &readfs, 0, &errorfs, &delta);
 488                         gettimeofday(&now, 0);
 489                         printf("%ld.%06ld: select returned: %d, %s\n",
 490                                (long)now.tv_sec, (long)now.tv_usec,
 491                                res,
 492                                res < 0 ? strerror(errno) : "success");
 493                         if (res > 0) {
 494                                 if (FD_ISSET(sock, &readfs))
 495                                         printf("ready for reading\n");
 496                                 if (FD_ISSET(sock, &errorfs))
 497                                         printf("has error\n");
 498                                 recvpacket(sock, 0,
 499                                            siocgstamp,
 500                                            siocgstampns);
 501                                 recvpacket(sock, MSG_ERRQUEUE,
 502                                            siocgstamp,
 503                                            siocgstampns);
 504                         }
 505                 } else {
 506                         /* write one packet */
 507                         sendpacket(sock,
 508                                    (struct sockaddr *)&addr,
 509                                    sizeof(addr));
 510                         next.tv_sec += 5;
 511                         continue;
 512                 }
 513         }
 514
 515         return 0;
 516 }