linux/Documentation/networking/timestamping/timestamping.c
<<
>>
Prefs
   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
  65static 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
  86static void bail(const char *error)
  87{
  88        printf("%s: %s\n", error, strerror(errno));
  89        exit(1);
  90}
  91
  92static 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
 138static 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
 154static 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
 281static 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
 317int 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}
 534