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

修改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 <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"

#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

參考資料:
[1] https://m.gpstime.com.cn/service_info-1199-20.html
[2] https://github.com/ptpd/ptpd
[3] https://www.kernel.org/doc/Documentation/networking/timestamping.txt