PTP(Precision Time Protocol)高精度时间同步协议+CS模式测试代码

Precision Time Protocol （PTP）
一、什么是PTP
PTP 是一种高精度时间同步协议,可以到达亚微秒级精度,有资料说可达到30纳秒左右的偏差精度,但需要网络的节点（交换机）支持PTP协议，才能实现纳秒量级的同步。
一般在实际使用中，现有的NTP可以达到5ms以内的精度，对一般的应用都是满足的；非超高精度设备，不建议使用PTP设备。

与NTP主要区别:PTP是在硬件级实现的,NTP是在应用层级别实现的.

PTP 是主从同步系统，一般采用硬件时间戳，并配合一些对NTP更高精度的延时测量算法。
PTP 最常用的是直接在 MAC 层进行 PTP 协议包分析 , 这样可以不经过UDP 协议栈 , 减少PTP 在协议栈中驻留时间 , 提高同步的精确度。
PTP 也可以承载在 UDP 上时 , 软件可以采用 SOCKET 进行收发 UDP包 , 事件消息的 UDP 端口号 319 , 普通消息的组播端口号为 320 ，但其精度就大大降低。
在物理硬件要求主从端都是PTP设备，且网络不能太大，其中间经过的交换机设备也必须支持PTP协议，并且主从时间网络链路唯一，不存在交替的PTP通道。

PTPv2 采用相对时间同步机制。一个参与者被选作主时间钟，其将发送同步信息到从站。主站将发送同步报文到网络。所有的从站计算时间延迟。

Fig. 39.1 PTP Synchronization Protocol

The PTP synchronization in the sample application works as follows:

Master sends Sync message - the slave saves it as T2.
Master sends Follow Up message and sends time of T1.
Slave sends Delay Request frame to PTP Master and stores T3.
Master sends Delay Response T4 time which is time of received T3.
The adjustment for slave can be represented as:

adj = -[(T2-T1) - (T4-T3)]/2

从钟根据 t1 、 t2 、 t3 、 t4 计算时间偏移 (offset) 以及传输延时 ( delay) ,即
t2 -t1 = offset + delay
t4 - t3 = delay - offset
计算出
delay = ( t4 - t3 + t2 - t1) / 2
offset = ( t2 - t1 - t4 + t3) / 2
从钟根据 offset 从钟可以调整自己的时钟。

二、PTP的一些名词
PTP域中的节点称为时钟节点，PTP协议定义了以下三种类型的基本时钟节点：
OC（Ordinary Clock，普通时钟）：只有一个PTP通信端口的时钟是普通时钟。
BC（Boundary Clock，边界时钟）：有一个以上PTP通信端口的时钟。
TC（Transparentclock，透明时钟）：与BC/OC相比，BC/OC需要与其它时钟节点保持时间同步，而TC则不与其它时钟节点保持时间同步。TC有多个PTP端口，但它只在这些端口间转发PTP协议报文并对其进行转发延时校正，而不会通过任何一个端口同步时间。TC包括以下两种类型：
E2ETC（End-to-End TransparentClock，端到端透明时钟）：直接转发网络中非P2P（Peer-to-Peer，点到点）类型的协议报文，并参与计算整条链路的延时。
P2PTC（Peer-to-PeerTransparent Clock，点到点透明时钟）：只直接转发Sync报文、Follow_Up报文和Announce报文，而终结其它PTP协议报文，并参与计算整条链路上每一段链路的延时。
一般链式的P2P网络选择E2E-TC，而从钟节点较多的网络考虑P2P-TC。因在 P2P 延时测量机制中，延时报文交互是在每条链路的两个端口间进行的，主钟只与直接相连的网络交换设备有延时报文交互，因此在 P2P TC 的延时测量机制中，没有对从钟数量的限制。
主时钟：一个PTP通信子网中只能有一个主时钟。

三、PTP报文
PTP协议定义了4种多点传送的报文类型和管理报文，包括同步报文（Sync），跟随报文（Follow_up），延迟请求报文（Delay_Req），延迟应答报文（Delay_Resp）和管理报文。
报文有一般报文和事件报文两种类型。跟随报文和延迟应答报文属于一般报文，一般报文本身不进行时戳处理，它可以携带事件报文的准确发送或接收时刻值信息。同步报文和延迟请求报文属于事件报文，事件报文是时间敏感消息，需要加盖精确的时间戳。

同步报文是从主时钟周期性发出的(一般为每两秒一次)，它包含了主时钟算法所需的时钟属性，它包含了一个时间戳，精确地描述了数据包发出的预计时间。

(1) Sync: 同步消息 , 由主设备发送给从设备 , 消息中可以包含 Sync 发送时间标签 , 也可以在后续的Follow UP 消息中包含 ;
(2) Delay Req: 请求对端返回接收到 Delay Req消息时的时间标签 , 时间标签嵌入在响应消息Delay Resp ;
(3) Pdelay req: 用于发起链路延时测量请求 , 带发送时间标签。
普通消息没有时间标签 , 主要用于传递其他消息的发送时间标签、系统状态以及管理信息 , 包括 :
(4) Announce: 广播发送节点和高级主钟的状态和特征信息 ;
(5) Follow Up : 用于传送Sync 消息的发送时间 ;
(6) Delay Resp : 对 Pdelayreq 的响应 , 可以带发送时间标签 , 如果没有带由随后的 Pdelay RespFollow Up 传送 ;
(7) Pdelay Resp Follow Up : 用于传送 DelayResp 的发送时间 ;
(8) Management : 传输用于管理时钟设备的的信息以及命令 ;Signaling: 在不同时钟之间传送信息、请求以及命令。
(9) Signaling: 在不同时钟之间传送信息、请求以及命令。
由于Sync包发送前,无法直接获取到硬件发送Sync包的时间; Sync发送后,可以获取到硬件发送Sync时间
ptpd源代码[2]net.c中的实现:
  netSendPcapEther -> sendto或pcap_inject发包
  getTxTimestamp 获取精确发送时间

四、局域网中实验
ubuntu下安装
$ sudo apt instal ptpd
server ip 192.168.37.68
$ sudo ptpd -M -i eno1 -C
指定了“仅主控”模式 向外组播数据
client
$ sudo ptpd -g -i eno1 -C
等一会就会看到输出

2018-08-30 10:05:23.271647 ptpd2[27616].eno1 (notice)    (lstn_reset) Now in state: PTP_LISTENING
2018-08-30 10:05:54.732606 ptpd2[27616].eno1 (info)      (lstn_reset) New best master selected: 180373fffed4ca44(unknown)/1
2018-08-30 10:05:54.732676 ptpd2[27616].eno1 (notice)    (slv) Now in state: PTP_SLAVE, Best master: 180373fffed4ca44(unknown)/1 (IPv4:192.168.37.68)
2018-08-30 10:05:55.732189 ptpd2[27616].eno1 (notice)    (slv) Received first Sync from Master
2018-08-30 10:05:56.732758 ptpd2[27616].eno1 (notice)    (slv) Received first Delay Response from Master
1
2
3
4
5
修改server的系统时间,client也会跟着同步.如果client开启了网络时间同步,系统时间会不停的在网络同步的时间和主服务器的时间之间进行切换
wireshark抓包看了一下,组播地址224.0.1.129,使用的是319和320端口
单播模式
服务器端 -u 指定单点广播模式 向指定IP发送数据
$ sudo ptpd -u 192.168.92.153 -M -i eno1 -V
客户端 接受指定IP的数据
$ sudo ptpd -u 192.168.52.190 -i eno1 -V

五、CS模式测试代码
使用CS模式实现如下伪PTP协议:

ID    方向    动作    动作    客户端状态    服务器状态    说明
1    c -> s    t1 send()    t2 recv    c:t1    s:t2    t1时刻,客户端向服务器发请求,服务器收包时间为t2,
此时客户端知道t1, 服务器端知道t2
2    c <- s    t4 recv    t3 send(t2)    c:t1,t2,t4    s:t2,t3    t3时刻,服务器把t2发向客户端, 客户端收包时间为t4,
此时客户端知道t1,t2,t4, 服务器端知道t2,t3
3    c -> s    send(t1,t4)        c:t1,t2,t4    s:t1,t2,t3,t4    
4    c <- s    send(t3)        c:t1,t2,t3,t4    s:t1,t2,t3,t4    
offset: t1端比t2端慢多少 负数表示t1端比t2端时间要快
delay: 延迟
delay = ( t4 - t3 + t2 - t1 ) / 2
offset = ( t2 - t1 - t4 + t3 ) / 2

官方文档timestamping.txt里的部分说明:
三个socket选项,都设置一下
SO_TIMESTAMP
SO_TIMESTAMPNS
SO_TIMESTAMPING
时间戳生成的几个标志位:
SOF_TIMESTAMPING_TX_SOFTWARE 发包的软件时间戳.离开内核时的时间戳,将数据包传递到网络接口之前。需要设备驱动支持,在设备驱动程序中生成
SOF_TIMESTAMPING_TX_HARDWARE 网卡生成的发包硬件时间戳.
SOF_TIMESTAMPING_RX_SOFTWARE 收到的包到达内核栈的时间戳.
SOF_TIMESTAMPING_RX_HARDWARE 网卡生成的收包硬件时间戳.

时间戳报告的几个标志位: 控制哪些时间戳将报告生成的控制消息。
SOF_TIMESTAMPING_SOFTWARE: 发包和收包的软件时间戳
  Report any software timestamps when available.
SOF_TIMESTAMPING_SYS_HARDWARE:
  This option is deprecated and ignored.
SOF_TIMESTAMPING_RAW_HARDWARE: 发包硬件时间戳 收包硬件时间戳呢???实际测试确实只有发包的时间戳,没收包的时间戳
  Report hardware timestamps as generated by SOF_TIMESTAMPING_TX_HARDWARE when available.

recvmsg函数返回时间戳时,收到数据的结构体
These timestamps are returned in a control message with cmsg_level SOL_SOCKET, cmsg_type SCM_TIMESTAMPING, and payload of type
struct scm_timestamping {
struct timespec ts[3];
};
Most timestamps are passed in ts[0]. Hardware timestamps are passed in ts[2].ts[1] used to hold hardware timestamps converted to system time.

坑:
给自己发消息,无法获取到时间戳,偶然向其他IP发送,获取到了发送时间戳. 猜想原因:没有出网卡,没有硬件时间戳
测试接收数据的时间戳和发送数据的时间戳,跳进这个坑里两次…
虚拟机中测试不支持加时间戳

源码:

/*
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
 * version 2, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. * See the GNU General Public License for
 * more details.
 */

#include
#include
#include
#include

#include
#include
#include
#include
#include
#include

#include "asm/types.h"
#include "linux/net_tstamp.h"
#include "linux/errqueue.h"

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

#ifndef SO_TIMESTAMPNS
# define SO_TIMESTAMPNS 35
#endif

#ifndef SIOCGSTAMPNS
# define SIOCGSTAMPNS 0x8907
#endif

#ifndef SIOCSHWTSTAMP
# define SIOCSHWTSTAMP 0x89b0
#endif

#define __out

static const unsigned char g_sync[] = {
    0x00, 0x02, 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 bail(const char *error)
{
    printf("[!] %s: %s\n", error, strerror(errno));
    exit(1);
}

static int send_packet(int sock, struct sockaddr *addr, socklen_t addr_len, char *buf, int len)
{
    struct timeval now;
    int res;

    res = sendto(sock, buf, len, 0, addr, addr_len);
    gettimeofday(&now, 0);
    if (res < 0)
        printf("[!] %s: %s\n", "sendto", strerror(errno));
    else
        printf("  %ld.%06ld: sendto %d bytes\n", (long)now.tv_sec, (long)now.tv_usec, res);
    return res;
}

static int recv_packet(int sock, struct sockaddr *addr, socklen_t *addr_len, char *buf, int len)
{
    struct timeval now;
    int res;

    res = recvfrom(sock, buf, len, 0, addr, addr_len);
    gettimeofday(&now, 0);
    if (res < 0)
        printf("[!] %s: %s\n", "recvfrom", strerror(errno));
    else
        printf("  %ld.%06ld: recvfrom %d bytes\n", (long)now.tv_sec, (long)now.tv_usec, res);
    return res;
}

static int print_packet(struct msghdr *msg, int res,    char *data, int sock, int recvmsg_flags,
        __out struct timespec *stamp_ns)
{
    struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
    struct cmsghdr *cmsg;
    struct timeval tv;
    struct timeval now;
    int     ret = -1;

    gettimeofday(&now, 0);

    printf("  %ld.%06ld: received %s data, %d bytes from %s, %ld 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 %ld: ", cmsg->cmsg_len);
        switch (cmsg->cmsg_level) {
        case 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);
                memcpy(stamp_ns, stamp, sizeof(timespec));
                ret = 0;
                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);
                memcpy(stamp_ns, stamp, sizeof(timespec));
                ret = 0;
                printf("SO_TIMESTAMPING ");
                printf("SW %ld.%09ld ", (long) stamp->tv_sec, (long) stamp->tv_nsec);
                stamp++;
                printf("HW transformed %ld.%09ld ", (long) stamp->tv_sec, (long) stamp->tv_nsec);
                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 < sizeof(g_sync))
                    printf(" => truncated data?!");
                else if (!memcmp(g_sync, data + res - sizeof(g_sync), sizeof(g_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");
    }
    return ret;
}

static int recv_packet_and_timestamp_ns(int sock, char *buf, int len, struct sockaddr_in *from_addr,
        int recvmsg_flags,    struct timespec *stamp_ns)
{
    //char data[1024] = {0};
    struct msghdr msg;
    struct iovec entry;
    struct {
        struct cmsghdr cm;
        char control[512];
    } control;
    int res;
    fd_set tmpSet;

    FD_ZERO(&tmpSet);
    FD_SET(sock, &tmpSet);
    struct timeval timeOut = {5,0};

    if(select(sock + 1, &tmpSet, NULL, NULL, &timeOut) > 0) {
        if (!FD_ISSET(sock, &tmpSet)){
            printf("[!] recvpacket timeout\n");
            return -2;
        }

        entry.iov_base = buf;
        entry.iov_len = len;//sizeof(data);
        //memset(&from_addr, 0, sizeof(from_addr));
        memset(&control, 0, sizeof(control));

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

        res = recvmsg(sock, &msg, recvmsg_flags | MSG_DONTWAIT);
        if (res < 0) {
            printf("[!] %s %d %s: %s\n", "recvmsg", res,
                    (recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
                    strerror(errno));
        } else {
            return print_packet(&msg, res, buf, sock, recvmsg_flags, stamp_ns);
        }
    }
    return -1;
}

void timespec_add(timespec &t1, timespec &t2, timespec &ret) {
    ret.tv_sec = t1.tv_sec + t2.tv_sec;
    ret.tv_nsec = t1.tv_nsec + t2.tv_nsec;
    if (ret.tv_nsec >= 1000000000) {
        ret.tv_nsec -= 1000000000;
        ret.tv_sec += 1;
    }
}

void timespec_dec(timespec &t1, timespec &t2, timespec &ret) {
    ret.tv_sec = t1.tv_sec - t2.tv_sec;
    ret.tv_nsec = t1.tv_nsec - t2.tv_nsec;
    if (ret.tv_nsec < 0) {
        ret.tv_nsec += 1000000000;
        ret.tv_sec -= 1;
    }
}

void timespec_div_int(timespec &t1, int i) {
    t1.tv_sec /= 2;
    t1.tv_nsec /= 2;
    if (t1.tv_sec % 2) {
        t1.tv_nsec += 500000000;
    }
}

void calc_offset_delay(timespec &offset, timespec &delay, timespec &t1, timespec &t2, timespec &t3, timespec &t4) {
    // delay  = ( t4 - t3 + t2 - t1 ) / 2
    // offset = ( t2 - t1 - t4 + t3 ) / 2
    timespec tt1, tt2;
    timespec_dec(t2, t1, tt1);
    timespec_dec(t4, t3, tt2);
    timespec_add(tt1, tt2, delay);
    timespec_dec(tt1, tt2,offset);
    timespec_div_int(delay, 2);
    timespec_div_int(offset, 2);
}

int ptp_client(char *if_name, char *ip_server, timespec &offset, timespec &delay)
{
    int sock;
    char *interface;
//    struct ifreq device;
    struct ifreq hwtstamp;
    struct hwtstamp_config hwconfig, hwconfig_requested;
    struct sockaddr_in addr;
    struct sockaddr_in addr_dst;
    struct in_addr iaddr;
    int val = 1;

    interface = strdup(if_name);

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

    memset(&hwtstamp, 0, sizeof(hwtstamp));
    strncpy(hwtstamp.ifr_name, interface, sizeof(hwtstamp.ifr_name));
    hwtstamp.ifr_data = (char*)&hwconfig;
    memset(&hwconfig, 0, sizeof(hwconfig));
    hwconfig.tx_type = HWTSTAMP_TX_ON;
    hwconfig.rx_filter = HWTSTAMP_FILTER_NONE;
    hwconfig_requested = hwconfig;
    if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0)
        bail("SIOCSHWTSTAMP");

    if (setsockopt(sock, SOL_SOCKET, SO_NO_CHECK, &val, sizeof(int)) < 0 )
    {
        printf("[!] Could not disable UDP checksum validation\n");
    }
    /* set socket options for time stamping */
    if ( setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, sizeof(int)) < 0)
        bail("setsockopt SO_TIMESTAMP");

    if ( setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, sizeof(int)) < 0)
        bail("setsockopt SO_TIMESTAMPNS");
    val = SOF_TIMESTAMPING_TX_HARDWARE| SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_RAW_HARDWARE
            | SOF_TIMESTAMPING_SOFTWARE;
    if ( setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, sizeof(int)) < 0)
        bail("setsockopt SO_TIMESTAMPING");

    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr(ip_server);//htonl(INADDR_ANY);
    addr.sin_port = htons(3200); /* PTP event port */

    struct timeval now;
    struct timeval delta;
    struct timespec t1, t4, t2, t3; //ns
    long delta_us;
    char buf[256];
    socklen_t len = sizeof(sockaddr);

    gettimeofday(&now, 0);

    //c -> s      t1 send()  t2 recv         c:t1              s:t2
    //c <- s      t4 recv  t3 send(t2)      c:t1,t2,t4         s:t2,t3
    //c -> s      send(t1,t4)                      c:t1,t2,t4        s:t1,t2,t3,t4
    //c <- s      send(t3)                        c:t1,t2,t3,t4    s:t1,t2,t3,t4
    printf("[*] 1. prepare send packet\n");
    if (send_packet(sock, (struct sockaddr *) &addr, sizeof(addr), (char*) g_sync, sizeof(g_sync)) <= 0)
        return -12;
    if (recv_packet_and_timestamp_ns(sock, buf, sizeof(buf), &addr_dst, MSG_ERRQUEUE, &t1))
        return -13;
    printf("[*] t1 = %ld.%09ld\n", (long) t1.tv_sec, (long) t1.tv_nsec);

    printf("[*] 2. prepare recv packet\n");
    //if( recv_packet(sock, (struct sockaddr *)&addr_dst, &len, (char*)&t2, sizeof(timespec)) <= 0)
    //return -22;
    if (recv_packet_and_timestamp_ns(sock, (char*) &t2, sizeof(timespec), &addr_dst, 0, &t4))
        return -23;
    printf("[*] t2 = %ld.%09ld\n", (long) t2.tv_sec, (long) t2.tv_nsec);
    printf("[*] t4 = %ld.%09ld\n", (long) t4.tv_sec, (long) t4.tv_nsec);

    printf("[*] 3. prepare send packet\n");
    if (send_packet(sock, (struct sockaddr *) &addr, sizeof(addr), (char*) &t1, sizeof(timespec) * 2) <= 0)
        return -32;

    printf("[*] 4. prepare recv packet\n");
    if (recv_packet(sock, (struct sockaddr *) &addr_dst, &len, (char*) &t3, sizeof(timespec)) <= 0)
        return -42;
    printf("[*] t3 = %ld.%09ld\n", (long) t3.tv_sec, (long) t3.tv_nsec);

    calc_offset_delay(offset, delay, t1, t2, t3, t4);
    printf("[*] offset = %ld.%09ld\n", (long) offset.tv_sec, (long) offset.tv_nsec);
    printf("[*] delay  = %ld.%09ld\n", (long) delay.tv_sec, (long) delay.tv_nsec);

    return 0;
}

int ptp_server(char *if_name)
{
    int sock;
    char *interface;
    struct ifreq device;
    struct ifreq hwtstamp;
    struct hwtstamp_config hwconfig, hwconfig_requested;
    struct sockaddr_in addr;
    struct sockaddr_in addr_dst;
    struct in_addr iaddr;
    int val = 1;

    interface = strdup(if_name);

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

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

    memset(&hwtstamp, 0, sizeof(hwtstamp));
    strncpy(hwtstamp.ifr_name, interface, sizeof(hwtstamp.ifr_name));
    hwtstamp.ifr_data = (char*)&hwconfig;
    memset(&hwconfig, 0, sizeof(&hwconfig));
    hwconfig.tx_type = HWTSTAMP_TX_ON;
    hwconfig.rx_filter = HWTSTAMP_FILTER_NONE;
    hwconfig_requested = hwconfig;
    if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
            bail("SIOCSHWTSTAMP");
    }

    val = 1;
    if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(int)) < 0) {
        printf("failed to set socket reuse\n");
    }
    if (setsockopt(sock, SOL_SOCKET, SO_NO_CHECK, &val, sizeof(int)) < 0) {
        printf("Could not disable UDP checksum validation\n");
    }
    /* bind to PTP port */
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = htonl(INADDR_ANY);
    addr.sin_port = htons(3200 /* PTP event port */);
    if (bind(sock, (struct sockaddr *) &addr, sizeof(struct sockaddr_in)) < 0)
        bail("bind");

    /* set socket options for time stamping */
    val = SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_RAW_HARDWARE
            | SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_OPT_ID | SOF_TIMESTAMPING_OPT_CMSG;
    if (setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, sizeof(int)) < 0)
        bail("setsockopt SO_TIMESTAMP");
    if (setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, sizeof(int)) < 0)
        bail("setsockopt SO_TIMESTAMPNS");
    if (setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, sizeof(int)) < 0)
        bail("setsockopt SO_TIMESTAMPING");

    while (1) {
        struct timeval now;
        struct timeval delta;
        struct timespec t1, t4, t2, t3, tmp; //ns
        long delta_us;
        char buf[256];
        socklen_t len = sizeof(sockaddr);

        //c -> s      t1 send()  t2 recv       c:t1              s:t2
        //c <- s      t4 recv  t3 send(t2)      c:t1,t2,t4         s:t2,t3
        //c -> s      send(t1,t4)                c:t1,t2,t4        s:t1,t2,t3,t4
        //c <- s      send(t3)                c:t1,t2,t3,t4    s:t1,t2,t3,t4
        printf("\n================================\n");
        printf("[*] 1. prepare recv packet, get t2\n");
        //if( recv_packet(sock, (struct sockaddr *)&addr_dst, &len, buf, sizeof(buf)) <= 0 )
            //return -12;
        if( recv_packet_and_timestamp_ns(sock, buf, sizeof(buf), &addr_dst, 0, &t2))
            continue;
        printf("t2 = %ld.%09ld\n", (long) t2.tv_sec, (long) t2.tv_nsec);

        printf("[*] 2. prepare send t2, get t3\n");
        if( send_packet(sock, (struct sockaddr *) &addr_dst, sizeof(sockaddr), (char*)&t2, sizeof(timespec)) <= 0 )
            return -22;
        if( recv_packet_and_timestamp_ns(sock, buf, sizeof(buf), &addr_dst, MSG_ERRQUEUE, &t3) )
            return -23;
        printf("t3 = %ld.%09ld\n", (long) t3.tv_sec, (long) t3.tv_nsec);

        printf("[*] 3. prepare recv t1,t4\n");
        //if( recv_packet(sock, (struct sockaddr *)&addr_dst, &len, (char*)&t1, sizeof(timespec) * 2) <= 0 )
        if( recv_packet_and_timestamp_ns(sock, buf, sizeof(buf), &addr_dst, 0, &tmp) )
            return -32;
        memcpy(&t1, buf, sizeof(timespec) * 2);
        printf("t1 = %ld.%09ld\n", (long) t1.tv_sec, (long) t1.tv_nsec);
        printf("t4 = %ld.%09ld\n", (long) t4.tv_sec, (long) t4.tv_nsec);

        printf("[*] 4. prepare send t3\n");
        if( send_packet(sock, (struct sockaddr *) &addr_dst, sizeof(sockaddr), (char*)&t3, sizeof(timespec)) <= 0 )
            return -42;
        if( recv_packet_and_timestamp_ns(sock, buf, sizeof(buf), &addr_dst, MSG_ERRQUEUE, &tmp) )
            return -32;

        struct timespec offset, delay;
        calc_offset_delay(offset, delay, t1, t2, t3, t4);
        printf("[*] offset = %ld.%09ld\n", (long) offset.tv_sec, (long) offset.tv_nsec);
        printf("[*] delay  = %ld.%09ld\n", (long) delay.tv_sec, (long) delay.tv_nsec);
    }

    return 0;
}

#ifdef PTP_SERVER
int main(int argc, char **argv)
{
    ptp_server(argv[1]);
    return 0;
}
#endif

#ifdef PTP_CLIENT
int main(int argc, char *argv[])
{
    timespec offset;
    timespec delay;
    ptp_client(argv[1], argv[2], offset, delay);
    return 0;
}
#endif

服务器端

$ gcc -DPTP_SERVER ptp.cpp -o ptpd
$ sudo ./ptpd eno1
================================
[*] 1. prepare recv packet, get t2
  1536484923.385697: received regular data, 124 bytes from 192.168.37.68, 96 bytes control messages
  cmsg len 32: SO_TIMESTAMPNS 1536484923.385564578
  cmsg len 64: SO_TIMESTAMPING SW 1536484923.385564578 HW transformed 0.000000000 HW raw 0.000000000
t2 = 1536484923.385564578
[*] 2. prepare send t2, get t3
  1536484923.385810: sendto 16 bytes
  1536484923.385886: received error data, 58 bytes from 192.168.37.68, 144 bytes control messages
  cmsg len 32: SO_TIMESTAMPNS 1536484923.385884974
  cmsg len 64: SO_TIMESTAMPING SW 1536484923.385884974 HW transformed 0.000000000 HW raw 1536484924.098158819
  cmsg len 48:   IPPROTO_IP  IP_RECVERR ee_errno 'No message of desired type' ee_origin 4 => bounced packet => truncated data?!
t3 = 1536484923.385884974
[*] 3. prepare recv t1,t4
  1536484923.386207: received regular data, 32 bytes from 192.168.37.68, 96 bytes control messages
  cmsg len 32: SO_TIMESTAMPNS 1536484923.386185423
  cmsg len 64: SO_TIMESTAMPING SW 1536484923.386185423 HW transformed 0.000000000 HW raw 0.000000000
t1 = 1536484923.418924231
t4 = 1536484923.419359084
[*] 4. prepare send t3
  1536484923.386238: sendto 16 bytes
  1536484923.386269: received error data, 58 bytes from 192.168.37.68, 144 bytes control messages
  cmsg len 32: SO_TIMESTAMPNS 1536484923.386268056
  cmsg len 64: SO_TIMESTAMPING SW 1536484923.386268056 HW transformed 0.000000000 HW raw 1536484924.098587007
  cmsg len 48:   IPPROTO_IP  IP_RECVERR ee_errno 'No message of desired type' ee_origin 4 => bounced packet => truncated data?!
[*] offset = 0.466583118
[*] delay  = 0.000057228

客户端

$ gcc -DPTP_CLIENT ptp.cpp -o ptp
$ sudo ./ptp eno1 192.168.20.153
[*] 1. prepare send packet
  1536484923.418825: sendto 124 bytes
  1536484923.418927: received error data, 166 bytes from 252.127.0.0, 144 bytes control messages
  cmsg len 32: SO_TIMESTAMPNS 1536484923.418924231
  cmsg len 64: SO_TIMESTAMPING SW 1536484923.418924231 HW transformed 0.000000000 HW raw 1536484923.400539922
  cmsg len 48:   IPPROTO_IP  IP_RECVERR ee_errno 'No message of desired type' ee_origin 4 => bounced packet => GOT OUR DATA BACK (HURRAY!)
[*] t1 = 1536484923.418924231
[*] 2. prepare recv packet
  1536484923.419397: received regular data, 16 bytes from 192.168.20.153, 96 bytes control messages
  cmsg len 32: SO_TIMESTAMPNS 1536484923.419359084
  cmsg len 64: SO_TIMESTAMPING SW 1536484923.419359084 HW transformed 0.000000000 HW raw 0.000000000
[*] t2 = 1536484923.385564578
[*] t4 = 1536484923.419359084
[*] 3. prepare send packet
  1536484923.419473: sendto 32 bytes
[*] 4. prepare recv packet
  1536484923.419741: recvfrom 16 bytes
[*] t3 = 1536484923.385884974
[*] offset = 0.466583118
[*] delay  = 0.000057228

————————————————
版权声明：本文为CSDN博主「woswod」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/woswod/article/details/82345380