tools/testing/selftests/networking/timestamping/timestamping.c

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