NetBSD timestamp testing

/*
  Demonstration of NetBSD timestamping features

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1. Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.
  2. Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.
  3. Neither the name of the project nor the names of its contributors
  may be used to endorse or promote products derived from this software
  without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED.  IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

  Author: 2011 Dmitry Cherkassov <dcherkassov@gmail.com>
*/

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>

#include <sys/time.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <sys/ioctl.h>
#include <arpa/inet.h>
#include <net/if.h>

#ifndef SO_TIMESTAMPNS
#define SO_TIMESTAMPNS 0x4000
#endif

#ifndef SCM_TIMESTAMPNS
#define SCM_TIMESTAMPNS 0x10
#endif

static void fail(const char *error)
{
    printf("%s: %s\n", error, strerror(errno));
    exit(1);
}

static const unsigned char sync[] = {
    0x00, 0x01, 0x00, 0x01,
    0x5f, 0x44, 0x46, 0x4c,
    0x54, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00,
    0x01, 0x01,

    /* fake uuid */
    0x00, 0x01,
    0x02, 0x03, 0x04, 0x05,

    0x00, 0x01, 0x00, 0x37,
    0x00, 0x00, 0x00, 0x08,
    0x00, 0x00, 0x00, 0x00,
    0x49, 0x05, 0xcd, 0x01,
    0x29, 0xb1, 0x8d, 0xb0,
    0x00, 0x00, 0x00, 0x00,
    0x00, 0x01,

    /* fake uuid */
    0x00, 0x01,
    0x02, 0x03, 0x04, 0x05,

    0x00, 0x00, 0x00, 0x37,
    0x00, 0x00, 0x00, 0x04,
    0x44, 0x46, 0x4c, 0x54,
    0x00, 0x00, 0xf0, 0x60,
    0x00, 0x01, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x01,
    0x00, 0x00, 0xf0, 0x60,
    0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x04,
    0x44, 0x46, 0x4c, 0x54,
    0x00, 0x01,

    /* fake uuid */
    0x00, 0x01,
    0x02, 0x03, 0x04, 0x05,

    0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00
};

static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len)
{
    struct timeval now;
    int res;

    res = sendto(sock, sync, sizeof(sync), 0,
                 addr, addr_len);
    gettimeofday(&now, 0);
    if (res < 0)
        printf("%s: %s\n", "send", strerror(errno));
    else
        printf("%ld.%06ld: sent %d bytes\n",
               (long)now.tv_sec, (long)now.tv_usec,
               res);
}

static void printpacket(struct msghdr *msg, int res,
                        char *data,
                        int sock, int recvmsg_flags,
                        int siocgstamp, int siocgstampns)
{
    struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
    struct cmsghdr *cmsg;
    struct timeval tv;
    struct timespec ts;
    struct timeval now;

    gettimeofday(&now, 0);

    printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n",
           (long)now.tv_sec, (long)now.tv_usec,
           /* (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular", */
           "regular",
           res,
           inet_ntoa(from_addr->sin_addr),
           msg->msg_controllen);
    for (cmsg = CMSG_FIRSTHDR(msg);
         cmsg;
         cmsg = CMSG_NXTHDR(msg, cmsg)) {
        printf("   cmsg len %zu: ", cmsg->cmsg_len);
        switch (cmsg->cmsg_level) {
        case SOL_SOCKET:
            printf("SOL_SOCKET ");
            switch (cmsg->cmsg_type) {
            /* case SO_TIMESTAMP: { */
            case SCM_TIMESTAMP: {
                struct timeval *stamp =
                    (struct timeval *)CMSG_DATA(cmsg);
                printf("TIMESTAMP %ld.%06ld",
                       (long)stamp->tv_sec,
                       (long)stamp->tv_usec);
                break;
            }
            case SCM_TIMESTAMPNS: {
                struct timespec *stamp =
                    (struct timespec *)CMSG_DATA(cmsg);

                if (stamp->tv_nsec == 0)
                    printf ("[zeronsecs] ");

                printf("TIMESTAMPNS %ld.%09ld",
                       (long)stamp->tv_sec,
                       (long)stamp->tv_nsec);
                break;
            }

            default: {
                printf("type %d", cmsg->cmsg_type);
                break;
            }
            }
            break;
        case IPPROTO_IP:
            printf("IPPROTO_IP ");
            switch (cmsg->cmsg_type) {
/*          case IP_RECVERR: { */
/*              struct sock_extended_err *err = */
/*                  (struct sock_extended_err *)CMSG_DATA(cmsg); */
/*              printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s", */
/*                     strerror(err->ee_errno), */
/*                     err->ee_origin, */
/* #ifdef SO_EE_ORIGIN_TIMESTAMPING */
/*                     err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ? */
/*                     "bounced packet" : "unexpected origin" */
/* #else */
/*                     "probably SO_EE_ORIGIN_TIMESTAMPING" */
/* #endif */
/*                  ); */
/*              if (res < sizeof(sync)) */
/*                  printf(" => truncated data?!"); */
/*              else if (!memcmp(sync, data + res - sizeof(sync), */
/*                               sizeof(sync))) */
/*                  printf(" => GOT OUR DATA BACK (HURRAY!)"); */
/*              break; */
/*          } */
            /* case IP_PKTINFO: { */
            /*  struct in_pktinfo *pktinfo = */
            /*      (struct in_pktinfo *)CMSG_DATA(cmsg); */
            /*  printf("IP_PKTINFO interface index %u", */
            /*         pktinfo->ipi_ifindex); */
            /*  break; */
            /* } */
            default:
                printf("type %d", cmsg->cmsg_type);
                break;
            }
            break;
        default:
            printf("level %d type %d",
                   cmsg->cmsg_level,
                   cmsg->cmsg_type);
            break;
        }
        printf("\n");
    }

    /* if (siocgstamp) { */
    /*  if (ioctl(sock, SIOCGSTAMP, &tv)) */
    /*      printf("   %s: %s\n", "SIOCGSTAMP", strerror(errno)); */
    /*  else */
    /*      printf("SIOCGSTAMP %ld.%06ld\n", */
    /*             (long)tv.tv_sec, */
    /*             (long)tv.tv_usec); */
    /* } */
    /* if (siocgstampns) { */
    /*  if (ioctl(sock, SIOCGSTAMPNS, &ts)) */
    /*      printf("   %s: %s\n", "SIOCGSTAMPNS", strerror(errno)); */
    /*  else */
    /*      printf("SIOCGSTAMPNS %ld.%09ld\n", */
    /*             (long)ts.tv_sec, */
    /*             (long)ts.tv_nsec); */
    /* } */
}


static void recvpacket(int sock, int recvmsg_flags,
                       int siocgstamp, int siocgstampns)
{
    char data[256];
    struct msghdr msg;
    struct iovec entry;
    struct sockaddr_in from_addr;
    struct {
        struct cmsghdr cm;
        char control[512];
    } control;
    int res;

    memset(&msg, 0, sizeof(msg));
    msg.msg_iov = &entry;
    msg.msg_iovlen = 1;
    entry.iov_base = data;
    entry.iov_len = sizeof(data);
    msg.msg_name = (caddr_t)&from_addr;
    msg.msg_namelen = sizeof(from_addr);
    msg.msg_control = &control;
    msg.msg_controllen = sizeof(control);

    res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);

    if (res < 0) {
        printf("%s %s: %s\n",
               "recvmsg",
               "regular",
               strerror(errno));
    } else {
        printpacket(&msg, res, data,
                    sock, recvmsg_flags,
                    siocgstamp, siocgstampns);
    }
}

static void usage(const char *error)
{
    if (error)
        printf("invalid option: %s\n", error);
    printf("timestamping interface option*\n\n"
           "Options:\n"
           /* "  IP_MULTICAST_LOOP - looping outgoing multicasts\n" */
           "  SO_TIMESTAMP - normal software time stamping, ms resolution\n"
           "  SO_TIMESTAMPNS - more accurate software time stamping\n"
           /* "  SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n" */
           /* "  SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n" */
           /* "  SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n" */
           /* "  SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n" */
           /* "  SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n" */
           /* "  SOF_TIMESTAMPING_SYS_HARDWARE - request reporting of transformed HW time stamps\n" */
           /* "  SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n" */
           /* "  SIOCGSTAMP - check last socket time stamp\n" */
           /* "  SIOCGSTAMPNS - more accurate socket time stamp\n" */);
    exit(1);
}


int main(int argc, char *argv[])
{
    int sock;
    struct in_addr iaddr;
    int val;
    socklen_t len;
    struct timeval next;
    struct ifreq device;
    struct ifreq hwtstamp;
    struct sockaddr_in addr;
    struct ip_mreq imr;
    int enabled = 1;
    int i;
    int ip_multicast_loop = 0;
    char *interface;
    /* char *interface = "lo0"; */
    int siocgstamp = 0;

    int so_timestamp = 0;
    int so_timestampns = 0;

    int siocgstampns = 0;

    if (argc < 2)
        usage(0);
    interface = argv[1];


    for (i = 2; i < argc; i++) {
        if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
            so_timestamp = 1;
        else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
            so_timestampns = 1;
    }


    sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (sock < 0)
        fail("socket");

    memset(&device, 0, sizeof(device));
    strncpy(device.ifr_name, interface, sizeof(device.ifr_name));
    if (ioctl(sock, SIOCGIFADDR, &device) < 0)
        fail("getting interface IP address");

    /* bind to PTP port */
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = htonl(INADDR_ANY);
    addr.sin_port = htons(319 /* PTP event port */);
    if (bind(sock,
             (struct sockaddr *)&addr,
             sizeof(struct sockaddr_in)) < 0)
        fail("bind");

    /* set multicast group for outgoing packets */
    inet_aton("224.0.1.130", &iaddr); /* alternate PTP domain 1 */
    addr.sin_addr = iaddr;
    imr.imr_multiaddr.s_addr = iaddr.s_addr;
    imr.imr_interface.s_addr =
        ((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;
    if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
                   &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)
        fail("set multicast");

    /* join multicast group, loop our own packet */
    if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
                   &imr, sizeof(struct ip_mreq)) < 0)
        fail("join multicast group");

    /* if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP, */
    /*             &ip_multicast_loop, sizeof(enabled)) < 0) { */
    /*  fail("loop multicast"); */
    /* } */

    /* set socket options for time stamping */
    if (so_timestamp &&
        setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
                   &enabled, sizeof(enabled)) < 0)
        fail("setsockopt SO_TIMESTAMP");

    /* verify socket options */

    printf("Socket options: \n");
    len = sizeof(val);
    if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
        printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
    else
        printf("SO_TIMESTAMP = %d\n", val);


    /* set socket options for time stamping */
    if (so_timestampns &&
        setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
                   &enabled, sizeof(enabled)) < 0)
        fail("setsockopt SO_TIMESTAMPNS");

    /* verify socket options */

    printf("Socket options: \n");
    len = sizeof(val);
    if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
        printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS", strerror(errno));
    else
        printf("SO_TIMESTAMPNS = %d\n", val);

    /* send packets forever every five seconds */
    gettimeofday(&next, 0);
    next.tv_sec = (next.tv_sec + 1) / 5 * 5;
    next.tv_usec = 0;

    while (1) {
        struct timeval now;
        struct timeval delta;
        long delta_us;
        int res;
        fd_set readfs, errorfs;

        gettimeofday(&now, 0);
        delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
            (long)(next.tv_usec - now.tv_usec);
        if (delta_us > 0) {
            /* continue waiting for timeout or data */
            delta.tv_sec = delta_us / 1000000;
            delta.tv_usec = delta_us % 1000000;

            FD_ZERO(&readfs);
            FD_ZERO(&errorfs);
            FD_SET(sock, &readfs);
            FD_SET(sock, &errorfs);
            printf("%ld.%06ld: select %ldus\n",
                   (long)now.tv_sec, (long)now.tv_usec,
                   delta_us);
            res = select(sock+1, &readfs, 0, &errorfs, &delta);
            gettimeofday(&now, 0);
            printf("%ld.%06ld: select returned: %d, %s\n",
                   (long)now.tv_sec, (long)now.tv_usec,
                   res,
                   res < 0 ? strerror(errno) : "success");
            if (res > 0) {
                if (FD_ISSET(sock, &readfs))
                    printf("ready for reading\n");
                if (FD_ISSET(sock, &errorfs))
                    printf("has error\n");
                recvpacket(sock, 0,
                           siocgstamp,
                           siocgstampns);
                /* recvpacket(sock, MSG_ERRQUEUE, */
                /*         siocgstamp, */
                /*         siocgstampns); */
            }
        } else {
            /* write one packet */
            sendpacket(sock,
                       (struct sockaddr *)&addr,
                       sizeof(addr));
            next.tv_sec += 5;
            continue;
        }
    }

    return 0;
}