Documentation/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
 336         if (argc < 2)
 337                 usage(0);
 338         interface = argv[1];
 339
 340         for (i = 2; i < argc; i++) {
 341                 if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
 342                         so_timestamp = 1;
 343                 else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
 344                         so_timestampns = 1;
 345                 else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
 346                         siocgstamp = 1;
 347                 else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
 348                         siocgstampns = 1;
 349                 else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
 350                         ip_multicast_loop = 1;
 351                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
 352                         so_timestamping_flags |= SOF_TIMESTAMPING_TX_HARDWARE;
 353                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
 354                         so_timestamping_flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
 355                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
 356                         so_timestamping_flags |= SOF_TIMESTAMPING_RX_HARDWARE;
 357                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
 358                         so_timestamping_flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
 359                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
 360                         so_timestamping_flags |= SOF_TIMESTAMPING_SOFTWARE;
 361                 else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
 362                         so_timestamping_flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
 363                 else
 364                         usage(argv[i]);
 365         }
 366
 367         sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
 368         if (sock < 0)
 369                 bail("socket");
 370
 371         memset(&device, 0, sizeof(device));
 372         strncpy(device.ifr_name, interface, sizeof(device.ifr_name));
 373         if (ioctl(sock, SIOCGIFADDR, &device) < 0)
 374                 bail("getting interface IP address");
 375
 376         memset(&hwtstamp, 0, sizeof(hwtstamp));
 377         strncpy(hwtstamp.ifr_name, interface, sizeof(hwtstamp.ifr_name));
 378         hwtstamp.ifr_data = (void *)&hwconfig;
 379         memset(&hwconfig, 0, sizeof(hwconfig));
 380         hwconfig.tx_type =
 381                 (so_timestamping_flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
 382                 HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
 383         hwconfig.rx_filter =
 384                 (so_timestamping_flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
 385                 HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
 386         hwconfig_requested = hwconfig;
 387         if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
 388                 if ((errno == EINVAL || errno == ENOTSUP) &&
 389                     hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
 390                     hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
 391                         printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
 392                 else
 393                         bail("SIOCSHWTSTAMP");
 394         }
 395         printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
 396                hwconfig_requested.tx_type, hwconfig.tx_type,
 397                hwconfig_requested.rx_filter, hwconfig.rx_filter);
 398
 399         /* bind to PTP port */
 400         addr.sin_family = AF_INET;
 401         addr.sin_addr.s_addr = htonl(INADDR_ANY);
 402         addr.sin_port = htons(319 /* PTP event port */);
 403         if (bind(sock,
 404                  (struct sockaddr *)&addr,
 405                  sizeof(struct sockaddr_in)) < 0)
 406                 bail("bind");
 407
 408         /* set multicast group for outgoing packets */
 409         inet_aton("224.0.1.130", &iaddr); /* alternate PTP domain 1 */
 410         addr.sin_addr = iaddr;
 411         imr.imr_multiaddr.s_addr = iaddr.s_addr;
 412         imr.imr_interface.s_addr =
 413                 ((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;
 414         if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
 415                        &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)
 416                 bail("set multicast");
 417
 418         /* join multicast group, loop our own packet */
 419         if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
 420                        &imr, sizeof(struct ip_mreq)) < 0)
 421                 bail("join multicast group");
 422
 423         if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP,
 424                        &ip_multicast_loop, sizeof(enabled)) < 0) {
 425                 bail("loop multicast");
 426         }
 427
 428         /* set socket options for time stamping */
 429         if (so_timestamp &&
 430                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
 431                            &enabled, sizeof(enabled)) < 0)
 432                 bail("setsockopt SO_TIMESTAMP");
 433
 434         if (so_timestampns &&
 435                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
 436                            &enabled, sizeof(enabled)) < 0)
 437                 bail("setsockopt SO_TIMESTAMPNS");
 438
 439         if (so_timestamping_flags &&
 440                 setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING,
 441                            &so_timestamping_flags,
 442                            sizeof(so_timestamping_flags)) < 0)
 443                 bail("setsockopt SO_TIMESTAMPING");
 444
 445         /* request IP_PKTINFO for debugging purposes */
 446         if (setsockopt(sock, SOL_IP, IP_PKTINFO,
 447                        &enabled, sizeof(enabled)) < 0)
 448                 printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));
 449
 450         /* verify socket options */
 451         len = sizeof(val);
 452         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
 453                 printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
 454         else
 455                 printf("SO_TIMESTAMP %d\n", val);
 456
 457         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
 458                 printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
 459                        strerror(errno));
 460         else
 461                 printf("SO_TIMESTAMPNS %d\n", val);
 462
 463         if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, &len) < 0) {
 464                 printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
 465                        strerror(errno));
 466         } else {
 467                 printf("SO_TIMESTAMPING %d\n", val);
 468                 if (val != so_timestamping_flags)
 469                         printf("   not the expected value %d\n",
 470                                so_timestamping_flags);
 471         }
 472
 473         /* send packets forever every five seconds */
 474         gettimeofday(&next, 0);
 475         next.tv_sec = (next.tv_sec + 1) / 5 * 5;
 476         next.tv_usec = 0;
 477         while (1) {
 478                 struct timeval now;
 479                 struct timeval delta;
 480                 long delta_us;
 481                 int res;
 482                 fd_set readfs, errorfs;
 483
 484                 gettimeofday(&now, 0);
 485                 delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
 486                         (long)(next.tv_usec - now.tv_usec);
 487                 if (delta_us > 0) {
 488                         /* continue waiting for timeout or data */
 489                         delta.tv_sec = delta_us / 1000000;
 490                         delta.tv_usec = delta_us % 1000000;
 491
 492                         FD_ZERO(&readfs);
 493                         FD_ZERO(&errorfs);
 494                         FD_SET(sock, &readfs);
 495                         FD_SET(sock, &errorfs);
 496                         printf("%ld.%06ld: select %ldus\n",
 497                                (long)now.tv_sec, (long)now.tv_usec,
 498                                delta_us);
 499                         res = select(sock + 1, &readfs, 0, &errorfs, &delta);
 500                         gettimeofday(&now, 0);
 501                         printf("%ld.%06ld: select returned: %d, %s\n",
 502                                (long)now.tv_sec, (long)now.tv_usec,
 503                                res,
 504                                res < 0 ? strerror(errno) : "success");
 505                         if (res > 0) {
 506                                 if (FD_ISSET(sock, &readfs))
 507                                         printf("ready for reading\n");
 508                                 if (FD_ISSET(sock, &errorfs))
 509                                         printf("has error\n");
 510                                 recvpacket(sock, 0,
 511                                            siocgstamp,
 512                                            siocgstampns);
 513                                 recvpacket(sock, MSG_ERRQUEUE,
 514                                            siocgstamp,
 515                                            siocgstampns);
 516                         }
 517                 } else {
 518                         /* write one packet */
 519                         sendpacket(sock,
 520                                    (struct sockaddr *)&addr,
 521                                    sizeof(addr));
 522                         next.tv_sec += 5;
 523                         continue;
 524                 }
 525         }
 526
 527         return 0;
 528 }