// SPDX-License-Identifier: GPL-2.0-only
/*
 * This program demonstrates how the various time stamping features in
 * the Linux kernel work. It emulates the behavior of a PTP
 * implementation in stand-alone master mode by sending PTPv1 Sync
 * multicasts once every second. It looks for similar packets, but
 * beyond that doesn't actually implement PTP.
 *
 * Outgoing packets are time stamped with SO_TIMESTAMPING with or
 * without hardware support.
 *
 * Incoming packets are time stamped with SO_TIMESTAMPING with or
 * without hardware support, SIOCGSTAMP[NS] (per-socket time stamp) and
 * SO_TIMESTAMP[NS].
 *
 * Copyright (C) 2009 Intel Corporation.
 * Author: Patrick Ohly <patrick.ohly@intel.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>

#include <asm/types.h>
#include <linux/net_tstamp.h>
#include <linux/errqueue.h>
#include <linux/sockios.h>

#ifndef SO_TIMESTAMPING
# define SO_TIMESTAMPING         37
# define SCM_TIMESTAMPING        SO_TIMESTAMPING
#endif

#ifndef SO_TIMESTAMPNS
# define SO_TIMESTAMPNS 35
#endif

static void usage(const char *error)
{
        if (error)
                printf("invalid option: %s\n", error);
        printf("timestamping <interface> [bind_phc_index] [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_RAW_HARDWARE - request reporting of raw HW time stamps\n"
               "  SOF_TIMESTAMPING_BIND_PHC - request to bind a PHC of PTP vclock\n"
               "  SIOCGSTAMP - check last socket time stamp\n"
               "  SIOCGSTAMPNS - more accurate socket time stamp\n"
               "  PTPV2 - use PTPv2 messages\n");
        exit(1);
}

static void bail(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 const unsigned char sync_v2[] = {
        0x00, 0x02, 0x00, 0x2C,
        0x00, 0x00, 0x02, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0xFF,
        0xFE, 0x00, 0x00, 0x00,
        0x00, 0x01, 0x00, 0x01,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00,
};

static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len, int ptpv2)
{
        size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
        const void *sync_p = ptpv2 ? sync_v2 : sync;
        struct timeval now;
        int res;

        res = sendto(sock, sync_p, sync_len, 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, int ptpv2)
{
        struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
        size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
        const void *sync_p = ptpv2 ? sync_v2 : sync;
        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",
               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: {
                                struct timeval *stamp =
                                        (struct timeval *)CMSG_DATA(cmsg);
                                printf("SO_TIMESTAMP %ld.%06ld",
                                       (long)stamp->tv_sec,
                                       (long)stamp->tv_usec);
                                break;
                        }
                        case SO_TIMESTAMPNS: {
                                struct timespec *stamp =
                                        (struct timespec *)CMSG_DATA(cmsg);
                                printf("SO_TIMESTAMPNS %ld.%09ld",
                                       (long)stamp->tv_sec,
                                       (long)stamp->tv_nsec);
                                break;
                        }
                        case SO_TIMESTAMPING: {
                                struct timespec *stamp =
                                        (struct timespec *)CMSG_DATA(cmsg);
                                printf("SO_TIMESTAMPING ");
                                printf("SW %ld.%09ld ",
                                       (long)stamp->tv_sec,
                                       (long)stamp->tv_nsec);
                                stamp++;
                                /* skip deprecated HW transformed */
                                stamp++;
                                printf("HW raw %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 < sync_len)
                                        printf(" => truncated data?!");
                                else if (!memcmp(sync_p, data + res - sync_len, sync_len))
                                        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, int ptpv2)
{
        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",
                       (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
                       strerror(errno));
        } else {
                printpacket(&msg, res, data,
                            sock, recvmsg_flags,
                            siocgstamp, siocgstampns, ptpv2);
        }
}

int main(int argc, char **argv)
{
        int so_timestamp = 0;
        int so_timestampns = 0;
        int siocgstamp = 0;
        int siocgstampns = 0;
        int ip_multicast_loop = 0;
        int ptpv2 = 0;
        char *interface;
        int i;
        int enabled = 1;
        int sock;
        struct ifreq device;
        struct ifreq hwtstamp;
        struct hwtstamp_config hwconfig, hwconfig_requested;
        struct so_timestamping so_timestamping_get = { 0, 0 };
        struct so_timestamping so_timestamping = { 0, 0 };
        struct sockaddr_in addr;
        struct ip_mreq imr;
        struct in_addr iaddr;
        int val;
        socklen_t len;
        struct timeval next;
        size_t if_len;

        if (argc < 2)
                usage(0);
        interface = argv[1];
        if_len = strlen(interface);
        if (if_len >= IFNAMSIZ) {
                printf("interface name exceeds IFNAMSIZ\n");
                exit(1);
        }

        if (argc >= 3 && sscanf(argv[2], "%d", &so_timestamping.bind_phc) == 1)
                val = 3;
        else
                val = 2;

        for (i = val; i < argc; i++) {
                if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
                        so_timestamp = 1;
                else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
                        so_timestampns = 1;
                else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
                        siocgstamp = 1;
                else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
                        siocgstampns = 1;
                else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
                        ip_multicast_loop = 1;
                else if (!strcasecmp(argv[i], "PTPV2"))
                        ptpv2 = 1;
                else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
                        so_timestamping.flags |= SOF_TIMESTAMPING_TX_HARDWARE;
                else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
                        so_timestamping.flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
                else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
                        so_timestamping.flags |= SOF_TIMESTAMPING_RX_HARDWARE;
                else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
                        so_timestamping.flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
                else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
                        so_timestamping.flags |= SOF_TIMESTAMPING_SOFTWARE;
                else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
                        so_timestamping.flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
                else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_BIND_PHC"))
                        so_timestamping.flags |= SOF_TIMESTAMPING_BIND_PHC;
                else
                        usage(argv[i]);
        }

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

        memset(&device, 0, sizeof(device));
        memcpy(device.ifr_name, interface, if_len + 1);
        if (ioctl(sock, SIOCGIFADDR, &device) < 0)
                bail("getting interface IP address");

        memset(&hwtstamp, 0, sizeof(hwtstamp));
        memcpy(hwtstamp.ifr_name, interface, if_len + 1);
        hwtstamp.ifr_data = (void *)&hwconfig;
        memset(&hwconfig, 0, sizeof(hwconfig));
        hwconfig.tx_type =
                (so_timestamping.flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
                HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
        hwconfig.rx_filter =
                (so_timestamping.flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
                ptpv2 ? HWTSTAMP_FILTER_PTP_V2_L4_SYNC :
                HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
        hwconfig_requested = hwconfig;
        if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
                if ((errno == EINVAL || errno == ENOTSUP) &&
                    hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
                    hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
                        printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
                else
                        bail("SIOCSHWTSTAMP");
        }
        printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
               hwconfig_requested.tx_type, hwconfig.tx_type,
               hwconfig_requested.rx_filter, hwconfig.rx_filter);

        /* 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)
                bail("bind");

        if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, interface, if_len))
                bail("bind device");

        /* 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)
                bail("set multicast");

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

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

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

        if (so_timestampns &&
                setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
                           &enabled, sizeof(enabled)) < 0)
                bail("setsockopt SO_TIMESTAMPNS");

        if (so_timestamping.flags &&
            setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping,
                       sizeof(so_timestamping)) < 0)
                bail("setsockopt SO_TIMESTAMPING");

        /* request IP_PKTINFO for debugging purposes */
        if (setsockopt(sock, SOL_IP, IP_PKTINFO,
                       &enabled, sizeof(enabled)) < 0)
                printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));

        /* verify socket options */
        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);

        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);

        len = sizeof(so_timestamping_get);
        if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping_get,
                       &len) < 0) {
                printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
                       strerror(errno));
        } else {
                printf("SO_TIMESTAMPING flags %d, bind phc %d\n",
                       so_timestamping_get.flags, so_timestamping_get.bind_phc);
                if (so_timestamping_get.flags != so_timestamping.flags ||
                    so_timestamping_get.bind_phc != so_timestamping.bind_phc)
                        printf("   not expected, flags %d, bind phc %d\n",
                               so_timestamping.flags, so_timestamping.bind_phc);
        }

        /* 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, ptpv2);
                                recvpacket(sock, MSG_ERRQUEUE,
                                           siocgstamp,
                                           siocgstampns, ptpv2);
                        }
                } else {
                        /* write one packet */
                        sendpacket(sock,
                                   (struct sockaddr *)&addr,
                                   sizeof(addr), ptpv2);
                        next.tv_sec += 5;
                        continue;
                }
        }

        return 0;
}