tools/testing/selftests/net/timestamping.c

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