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