[PATCH 0/3] add frequency adjustment support for PTP

[PATCH 0/3] add frequency adjustment support for PTP

[Mingjin Ye]

[1/3] ethdev: add frequency adjustment API
[2/3] net/ice: add frequency adjustment support for PTP
[3/3] examples/ptpclient: add frequency adjustment support

Mingjin Ye (3):
  ethdev: add frequency adjustment API
  net/ice: add frequency adjustment support for PTP
  examples/ptpclient: add frequency adjustment support

 doc/guides/nics/features.rst     |   4 +-
 doc/guides/nics/ice.rst          |  15 ++
 drivers/net/ice/ice_ethdev.c     | 177 +++++++++++++-----
 drivers/net/ice/ice_ethdev.h     |   2 +
 drivers/net/ice/ice_rxtx.c       |   4 +-
 examples/ptpclient/ptpclient.c   | 300 +++++++++++++++++++++++++++----
 lib/ethdev/ethdev_driver.h       |   5 +
 lib/ethdev/ethdev_trace.h        |   9 +
 lib/ethdev/ethdev_trace_points.c |   3 +
 lib/ethdev/rte_ethdev.c          |  18 ++
 lib/ethdev/rte_ethdev.h          |  19 ++
 lib/ethdev/version.map           |   3 +
 12 files changed, 478 insertions(+), 81 deletions(-)

-- 
2.25.1

[PATCH 1/3] ethdev: add frequency adjustment API

[Mingjin Ye]

This patch adds freq adjustment API for PTP high accuracy.

Signed-off-by: Simei Su <simei.su@intel.com>
Signed-off-by: Mingjin Ye <mingjinx.ye@intel.com>
---
 doc/guides/nics/features.rst     |  4 +++-
 lib/ethdev/ethdev_driver.h       |  5 +++++
 lib/ethdev/ethdev_trace.h        |  9 +++++++++
 lib/ethdev/ethdev_trace_points.c |  3 +++
 lib/ethdev/rte_ethdev.c          | 18 ++++++++++++++++++
 lib/ethdev/rte_ethdev.h          | 19 +++++++++++++++++++
 lib/ethdev/version.map           |  3 +++
 7 files changed, 60 insertions(+), 1 deletion(-)

diff --git a/doc/guides/nics/features.rst b/doc/guides/nics/features.rst
index cd0115ffb3..0508f118fe 100644
--- a/doc/guides/nics/features.rst
+++ b/doc/guides/nics/features.rst
@@ -677,10 +677,12 @@ Supports IEEE1588/802.1AS timestamping.
 
 * **[implements] eth_dev_ops**: ``timesync_enable``, ``timesync_disable``
   ``timesync_read_rx_timestamp``, ``timesync_read_tx_timestamp``,
-  ``timesync_adjust_time``, ``timesync_read_time``, ``timesync_write_time``.
+  ``timesync_adjust_time``, ``timesync_adjust_freq``,
+  ``timesync_read_time``, ``timesync_write_time``.
 * **[related]    API**: ``rte_eth_timesync_enable()``, ``rte_eth_timesync_disable()``,
   ``rte_eth_timesync_read_rx_timestamp()``,
   ``rte_eth_timesync_read_tx_timestamp``, ``rte_eth_timesync_adjust_time()``,
+  ``rte_eth_timesync_adjust_freq()``,
   ``rte_eth_timesync_read_time()``, ``rte_eth_timesync_write_time()``.
 
 
diff --git a/lib/ethdev/ethdev_driver.h b/lib/ethdev/ethdev_driver.h
index 883e59a927..68cadc14a5 100644
--- a/lib/ethdev/ethdev_driver.h
+++ b/lib/ethdev/ethdev_driver.h
@@ -664,6 +664,9 @@ typedef int (*eth_timesync_read_tx_timestamp_t)(struct rte_eth_dev *dev,
 /** @internal Function used to adjust the device clock. */
 typedef int (*eth_timesync_adjust_time)(struct rte_eth_dev *dev, int64_t);
 
+/** @internal Function used to adjust the clock frequency. */
+typedef int (*eth_timesync_adjust_freq)(struct rte_eth_dev *dev, int64_t);
+
 /** @internal Function used to get time from the device clock. */
 typedef int (*eth_timesync_read_time)(struct rte_eth_dev *dev,
 				      struct timespec *timestamp);
@@ -1378,6 +1381,8 @@ struct eth_dev_ops {
 	eth_timesync_read_tx_timestamp_t timesync_read_tx_timestamp;
 	/** Adjust the device clock */
 	eth_timesync_adjust_time   timesync_adjust_time;
+	/** Adjust the clock frequency */
+	eth_timesync_adjust_freq   timesync_adjust_freq;
 	/** Get the device clock time */
 	eth_timesync_read_time     timesync_read_time;
 	/** Set the device clock time */
diff --git a/lib/ethdev/ethdev_trace.h b/lib/ethdev/ethdev_trace.h
index 3bec87bfdb..e273d5853e 100644
--- a/lib/ethdev/ethdev_trace.h
+++ b/lib/ethdev/ethdev_trace.h
@@ -2183,6 +2183,15 @@ RTE_TRACE_POINT_FP(
 	rte_trace_point_emit_int(ret);
 )
 
+/* Called in loop in examples/ptpclient */
+RTE_TRACE_POINT_FP(
+	rte_eth_trace_timesync_adjust_freq,
+	RTE_TRACE_POINT_ARGS(uint16_t port_id, int64_t ppm, int ret),
+	rte_trace_point_emit_u16(port_id);
+	rte_trace_point_emit_i64(ppm);
+	rte_trace_point_emit_int(ret);
+)
+
 /* Called in loop in app/test-flow-perf */
 RTE_TRACE_POINT_FP(
 	rte_flow_trace_create,
diff --git a/lib/ethdev/ethdev_trace_points.c b/lib/ethdev/ethdev_trace_points.c
index 99e04f5893..a99fec0c1e 100644
--- a/lib/ethdev/ethdev_trace_points.c
+++ b/lib/ethdev/ethdev_trace_points.c
@@ -409,6 +409,9 @@ RTE_TRACE_POINT_REGISTER(rte_eth_trace_timesync_read_tx_timestamp,
 RTE_TRACE_POINT_REGISTER(rte_eth_trace_timesync_adjust_time,
 	lib.ethdev.timesync_adjust_time)
 
+RTE_TRACE_POINT_REGISTER(rte_eth_trace_timesync_adjust_freq,
+	lib.ethdev.timesync_adjust_freq)
+
 RTE_TRACE_POINT_REGISTER(rte_eth_trace_timesync_read_time,
 	lib.ethdev.timesync_read_time)
 
diff --git a/lib/ethdev/rte_ethdev.c b/lib/ethdev/rte_ethdev.c
index f1c658f49e..660eab2f1e 100644
--- a/lib/ethdev/rte_ethdev.c
+++ b/lib/ethdev/rte_ethdev.c
@@ -6310,6 +6310,24 @@ rte_eth_timesync_adjust_time(uint16_t port_id, int64_t delta)
 	return ret;
 }
 
+int
+rte_eth_timesync_adjust_freq(uint16_t port_id, int64_t ppm)
+{
+	struct rte_eth_dev *dev;
+	int ret;
+
+	RTE_ETH_VALID_PORTID_OR_ERR_RET(port_id, -ENODEV);
+	dev = &rte_eth_devices[port_id];
+
+	if (*dev->dev_ops->timesync_adjust_freq == NULL)
+		return -ENOTSUP;
+	ret = eth_err(port_id, (*dev->dev_ops->timesync_adjust_freq)(dev, ppm));
+
+	rte_eth_trace_timesync_adjust_freq(port_id, ppm, ret);
+
+	return ret;
+}
+
 int
 rte_eth_timesync_read_time(uint16_t port_id, struct timespec *timestamp)
 {
diff --git a/lib/ethdev/rte_ethdev.h b/lib/ethdev/rte_ethdev.h
index 548fada1c7..42fa2ca39c 100644
--- a/lib/ethdev/rte_ethdev.h
+++ b/lib/ethdev/rte_ethdev.h
@@ -5297,6 +5297,25 @@ int rte_eth_timesync_read_tx_timestamp(uint16_t port_id,
  */
 int rte_eth_timesync_adjust_time(uint16_t port_id, int64_t delta);
 
+/**
+ * Adjust the clock increment rate on an Ethernet device.
+ *
+ * This is usually used in conjunction with other Ethdev timesync functions to
+ * synchronize the device time using the IEEE1588/802.1AS protocol.
+ *
+ * @param port_id
+ *  The port identifier of the Ethernet device.
+ * @param ppm
+ *  Parts per million with 16-bit fractional field
+ *
+ * @return
+ *   - 0: Success.
+ *   - -ENODEV: The port ID is invalid.
+ *   - -EIO: if device is removed.
+ *   - -ENOTSUP: The function is not supported by the Ethernet driver.
+ */
+int rte_eth_timesync_adjust_freq(uint16_t port_id, int64_t ppm);
+
 /**
  * Read the time from the timesync clock on an Ethernet device.
  *
diff --git a/lib/ethdev/version.map b/lib/ethdev/version.map
index 1669055ca5..199480ef07 100644
--- a/lib/ethdev/version.map
+++ b/lib/ethdev/version.map
@@ -325,6 +325,9 @@ EXPERIMENTAL {
 	rte_flow_template_table_resizable;
 	rte_flow_template_table_resize;
 	rte_flow_template_table_resize_complete;
+
+	# added in 24.11
+	rte_eth_timesync_adjust_freq;
 };
 
 INTERNAL {
-- 
2.25.1

[PATCH 2/3] net/ice: add frequency adjustment support for PTP

[Mingjin Ye]

Add ice support for new ethdev API to adjust frequency for IEEE1588
PTP. Also, this patch reworks code for converting software update
to hardware update.

Signed-off-by: Simei Su <simei.su@intel.com>
Signed-off-by: Mingjin Ye <mingjinx.ye@intel.com>
---
 doc/guides/nics/ice.rst      |  15 +++
 drivers/net/ice/ice_ethdev.c | 177 ++++++++++++++++++++++++++---------
 drivers/net/ice/ice_ethdev.h |   2 +
 drivers/net/ice/ice_rxtx.c   |   4 +-
 4 files changed, 153 insertions(+), 45 deletions(-)

diff --git a/doc/guides/nics/ice.rst b/doc/guides/nics/ice.rst
index ae975d19ad..dec4cc2434 100644
--- a/doc/guides/nics/ice.rst
+++ b/doc/guides/nics/ice.rst
@@ -328,6 +328,21 @@ Forward Error Correction (FEC)
 
 Supports get/set FEC mode and get FEC capability.
 
+Time Synchronisation
+~~~~~~~~~~~~~~~~~~~~
+
+The system operator can run a PTP (Precision Time Protocol) client application
+to synchronise the time on the network card (and optionally the time on the
+system) to the PTP master.
+
+ICE PMD supports PTP client applications that use the DPDK IEEE1588 API to
+communicate with the PTP master clock. Note that the PTP client application
+needs to run on the PF and vector mode needs to be disabled.
+
+.. code-block:: console
+
+    examples/dpdk-ptpclient -c f -n 3 -a 0000:ec:00.1 -- -T 1 -p 0x1 --controller=pi
+
 Generic Flow Support
 ~~~~~~~~~~~~~~~~~~~~
 
diff --git a/drivers/net/ice/ice_ethdev.c b/drivers/net/ice/ice_ethdev.c
index 304f959b7e..33293a917f 100644
--- a/drivers/net/ice/ice_ethdev.c
+++ b/drivers/net/ice/ice_ethdev.c
@@ -10,6 +10,7 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <unistd.h>
+#include <math.h>
 
 #include <rte_tailq.h>
 #include <rte_os_shim.h>
@@ -176,6 +177,7 @@ static int ice_timesync_read_rx_timestamp(struct rte_eth_dev *dev,
 static int ice_timesync_read_tx_timestamp(struct rte_eth_dev *dev,
 					  struct timespec *timestamp);
 static int ice_timesync_adjust_time(struct rte_eth_dev *dev, int64_t delta);
+static int ice_timesync_adjust_freq(struct rte_eth_dev *dev, int64_t ppm);
 static int ice_timesync_read_time(struct rte_eth_dev *dev,
 				  struct timespec *timestamp);
 static int ice_timesync_write_time(struct rte_eth_dev *dev,
@@ -307,6 +309,7 @@ static const struct eth_dev_ops ice_eth_dev_ops = {
 	.timesync_read_rx_timestamp   = ice_timesync_read_rx_timestamp,
 	.timesync_read_tx_timestamp   = ice_timesync_read_tx_timestamp,
 	.timesync_adjust_time         = ice_timesync_adjust_time,
+	.timesync_adjust_freq         = ice_timesync_adjust_freq,
 	.timesync_read_time           = ice_timesync_read_time,
 	.timesync_write_time          = ice_timesync_write_time,
 	.timesync_disable             = ice_timesync_disable,
@@ -2332,6 +2335,34 @@ ice_get_supported_rxdid(struct ice_hw *hw)
 	return supported_rxdid;
 }
 
+static void ice_ptp_init_info(struct rte_eth_dev *dev)
+{
+	struct ice_hw *hw = ICE_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+	struct ice_adapter *ad =
+		ICE_DEV_PRIVATE_TO_ADAPTER(dev->data->dev_private);
+
+	switch (hw->phy_model) {
+	case ICE_PHY_ETH56G:
+		ad->ptp_tx_block = hw->pf_id;
+		ad->ptp_tx_index = 0;
+		break;
+	case ICE_PHY_E810:
+	/* fallthrough */
+	case ICE_PHY_E830:
+		ad->ptp_tx_block = hw->port_info->lport;
+		ad->ptp_tx_index = 0;
+		break;
+	case ICE_PHY_E822:
+		ad->ptp_tx_block = hw->pf_id / ICE_PORTS_PER_QUAD;
+		ad->ptp_tx_index = (hw->pf_id % ICE_PORTS_PER_QUAD) *
+				ICE_PORTS_PER_PHY_E822 * ICE_QUADS_PER_PHY_E822;
+		break;
+	default:
+		PMD_DRV_LOG(WARNING, "Unsupported PHY model");
+		break;
+	}
+}
+
 static int
 ice_dev_init(struct rte_eth_dev *dev)
 {
@@ -2499,6 +2530,9 @@ ice_dev_init(struct rte_eth_dev *dev)
 	/* Initialize PHY model */
 	ice_ptp_init_phy_model(hw);
 
+	/* Initialize PTP info */
+	ice_ptp_init_info(dev);
+
 	if (hw->phy_model == ICE_PHY_E822) {
 		ret = ice_start_phy_timer_e822(hw, hw->pf_id);
 		if (ret)
@@ -6466,23 +6500,6 @@ ice_timesync_enable(struct rte_eth_dev *dev)
 		}
 	}
 
-	/* Initialize cycle counters for system time/RX/TX timestamp */
-	memset(&ad->systime_tc, 0, sizeof(struct rte_timecounter));
-	memset(&ad->rx_tstamp_tc, 0, sizeof(struct rte_timecounter));
-	memset(&ad->tx_tstamp_tc, 0, sizeof(struct rte_timecounter));
-
-	ad->systime_tc.cc_mask = ICE_CYCLECOUNTER_MASK;
-	ad->systime_tc.cc_shift = 0;
-	ad->systime_tc.nsec_mask = 0;
-
-	ad->rx_tstamp_tc.cc_mask = ICE_CYCLECOUNTER_MASK;
-	ad->rx_tstamp_tc.cc_shift = 0;
-	ad->rx_tstamp_tc.nsec_mask = 0;
-
-	ad->tx_tstamp_tc.cc_mask = ICE_CYCLECOUNTER_MASK;
-	ad->tx_tstamp_tc.cc_shift = 0;
-	ad->tx_tstamp_tc.nsec_mask = 0;
-
 	ad->ptp_ena = 1;
 
 	return 0;
@@ -6497,14 +6514,13 @@ ice_timesync_read_rx_timestamp(struct rte_eth_dev *dev,
 			ICE_DEV_PRIVATE_TO_ADAPTER(dev->data->dev_private);
 	struct ice_rx_queue *rxq;
 	uint32_t ts_high;
-	uint64_t ts_ns, ns;
+	uint64_t ts_ns;
 
 	rxq = dev->data->rx_queues[flags];
 
 	ts_high = rxq->time_high;
 	ts_ns = ice_tstamp_convert_32b_64b(hw, ad, 1, ts_high);
-	ns = rte_timecounter_update(&ad->rx_tstamp_tc, ts_ns);
-	*timestamp = rte_ns_to_timespec(ns);
+	*timestamp = rte_ns_to_timespec(ts_ns);
 
 	return 0;
 }
@@ -6516,22 +6532,18 @@ ice_timesync_read_tx_timestamp(struct rte_eth_dev *dev,
 	struct ice_hw *hw = ICE_DEV_PRIVATE_TO_HW(dev->data->dev_private);
 	struct ice_adapter *ad =
 			ICE_DEV_PRIVATE_TO_ADAPTER(dev->data->dev_private);
-	uint8_t lport;
-	uint64_t ts_ns, ns, tstamp;
+	uint64_t ts_ns, tstamp;
 	const uint64_t mask = 0xFFFFFFFF;
 	int ret;
 
-	lport = hw->port_info->lport;
-
-	ret = ice_read_phy_tstamp(hw, lport, 0, &tstamp);
-	if (ret) {
+	ret = ice_read_phy_tstamp(hw, ad->ptp_tx_block, ad->ptp_tx_index, &tstamp);
+	if (ret || tstamp == 0) {
 		PMD_DRV_LOG(ERR, "Failed to read phy timestamp");
 		return -1;
 	}
 
 	ts_ns = ice_tstamp_convert_32b_64b(hw, ad, 1, (tstamp >> 8) & mask);
-	ns = rte_timecounter_update(&ad->tx_tstamp_tc, ts_ns);
-	*timestamp = rte_ns_to_timespec(ns);
+	*timestamp = rte_ns_to_timespec(ts_ns);
 
 	return 0;
 }
@@ -6539,28 +6551,108 @@ ice_timesync_read_tx_timestamp(struct rte_eth_dev *dev,
 static int
 ice_timesync_adjust_time(struct rte_eth_dev *dev, int64_t delta)
 {
-	struct ice_adapter *ad =
-			ICE_DEV_PRIVATE_TO_ADAPTER(dev->data->dev_private);
+	struct ice_hw *hw = ICE_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+	uint8_t tmr_idx = hw->func_caps.ts_func_info.tmr_index_assoc;
+	uint32_t lo, lo2, hi;
+	uint64_t time, ns;
+	int ret;
+
+	if (delta > INT32_MAX || delta < INT32_MIN) {
+		lo = ICE_READ_REG(hw, GLTSYN_TIME_L(tmr_idx));
+		hi = ICE_READ_REG(hw, GLTSYN_TIME_H(tmr_idx));
+		lo2 = ICE_READ_REG(hw, GLTSYN_TIME_L(tmr_idx));
+
+		if (lo2 < lo) {
+			lo = ICE_READ_REG(hw, GLTSYN_TIME_L(tmr_idx));
+			hi = ICE_READ_REG(hw, GLTSYN_TIME_H(tmr_idx));
+		}
+
+		time = ((uint64_t)hi << 32) | lo;
+		ns = time + delta;
+
+		wr32(hw, GLTSYN_SHTIME_L(tmr_idx), ICE_LO_DWORD(ns));
+		wr32(hw, GLTSYN_SHTIME_H(tmr_idx), ICE_HI_DWORD(ns));
+		wr32(hw, GLTSYN_SHTIME_0(tmr_idx), 0);
+
+		ret = ice_ptp_init_time(hw, ns, true);
+		if (ret) {
+			PMD_DRV_LOG(ERR, "PTP init time failed, err %d", ret);
+			return -1;
+		}
+		return 0;
+	}
+
+	ret = ice_ptp_adj_clock(hw, delta, true);
+	if (ret) {
+		PMD_DRV_LOG(ERR, "PTP adj clock failed, err %d", ret);
+		return -1;
+	}
+
+	return 0;
+}
+
+static int
+ice_timesync_adjust_freq(struct rte_eth_dev *dev, int64_t ppm)
+{
+	struct ice_hw *hw = ICE_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+	int64_t incval, diff = 0;
+	bool negative = false;
+	uint64_t div, rem;
+	uint64_t divisor = 1000000ULL << 16;
+	int shift;
+	int ret;
+
+	incval = ice_get_base_incval(hw, ICE_SRC_TMR_MODE_NANOSECONDS);
+
+	if (ppm < 0) {
+		negative = true;
+		ppm = -ppm;
+	}
+
+	/* can incval * ppm overflow ? */
+	if (log2(incval) + log2(ppm) > 62) {
+		rem = ppm % divisor;
+		div = ppm / divisor;
+		diff = div * incval;
+		ppm = rem;
 
-	ad->systime_tc.nsec += delta;
-	ad->rx_tstamp_tc.nsec += delta;
-	ad->tx_tstamp_tc.nsec += delta;
+		shift = log2(incval) + log2(ppm) - 62;
+		if (shift > 0) {
+			/* drop precision */
+			ppm >>= shift;
+			divisor >>= shift;
+		}
+	}
+
+	if (divisor)
+		diff = diff + incval * ppm / divisor;
+
+	if (negative)
+		incval -= diff;
+	else
+		incval += diff;
 
+	ret = ice_ptp_write_incval_locked(hw, incval, true);
+	if (ret) {
+		PMD_DRV_LOG(ERR, "PTP failed to set incval, err %d", ret);
+		return -1;
+	}
 	return 0;
 }
 
 static int
 ice_timesync_write_time(struct rte_eth_dev *dev, const struct timespec *ts)
 {
-	struct ice_adapter *ad =
-			ICE_DEV_PRIVATE_TO_ADAPTER(dev->data->dev_private);
+	struct ice_hw *hw = ICE_DEV_PRIVATE_TO_HW(dev->data->dev_private);
 	uint64_t ns;
+	int ret;
 
 	ns = rte_timespec_to_ns(ts);
-
-	ad->systime_tc.nsec = ns;
-	ad->rx_tstamp_tc.nsec = ns;
-	ad->tx_tstamp_tc.nsec = ns;
+	ret = ice_ptp_init_time(hw, ns, true);
+	if (ret) {
+		PMD_DRV_LOG(ERR, "PTP init time failed, err %d", ret);
+		return -1;
+	}
 
 	return 0;
 }
@@ -6569,11 +6661,9 @@ static int
 ice_timesync_read_time(struct rte_eth_dev *dev, struct timespec *ts)
 {
 	struct ice_hw *hw = ICE_DEV_PRIVATE_TO_HW(dev->data->dev_private);
-	struct ice_adapter *ad =
-			ICE_DEV_PRIVATE_TO_ADAPTER(dev->data->dev_private);
 	uint8_t tmr_idx = hw->func_caps.ts_func_info.tmr_index_assoc;
 	uint32_t hi, lo, lo2;
-	uint64_t time, ns;
+	uint64_t time;
 
 	lo = ICE_READ_REG(hw, GLTSYN_TIME_L(tmr_idx));
 	hi = ICE_READ_REG(hw, GLTSYN_TIME_H(tmr_idx));
@@ -6585,8 +6675,7 @@ ice_timesync_read_time(struct rte_eth_dev *dev, struct timespec *ts)
 	}
 
 	time = ((uint64_t)hi << 32) | lo;
-	ns = rte_timecounter_update(&ad->systime_tc, time);
-	*ts = rte_ns_to_timespec(ns);
+	*ts = rte_ns_to_timespec(time);
 
 	return 0;
 }
diff --git a/drivers/net/ice/ice_ethdev.h b/drivers/net/ice/ice_ethdev.h
index 3ea9f37dc8..db192e76fe 100644
--- a/drivers/net/ice/ice_ethdev.h
+++ b/drivers/net/ice/ice_ethdev.h
@@ -614,6 +614,8 @@ struct ice_adapter {
 	struct rte_timecounter systime_tc;
 	struct rte_timecounter rx_tstamp_tc;
 	struct rte_timecounter tx_tstamp_tc;
+	uint8_t ptp_tx_block;
+	uint8_t ptp_tx_index;
 	bool ptp_ena;
 	uint64_t time_hw;
 	struct ice_fdir_prof_info fdir_prof_info[ICE_MAX_PTGS];
diff --git a/drivers/net/ice/ice_rxtx.c b/drivers/net/ice/ice_rxtx.c
index f270498ed1..5f399e8aae 100644
--- a/drivers/net/ice/ice_rxtx.c
+++ b/drivers/net/ice/ice_rxtx.c
@@ -3066,7 +3066,9 @@ ice_xmit_pkts(void *tx_queue, struct rte_mbuf **tx_pkts, uint16_t nb_pkts)
 			else if (ol_flags & RTE_MBUF_F_TX_IEEE1588_TMST)
 				cd_type_cmd_tso_mss |=
 					((uint64_t)ICE_TX_CTX_DESC_TSYN <<
-					ICE_TXD_CTX_QW1_CMD_S);
+					ICE_TXD_CTX_QW1_CMD_S) |
+					 (((uint64_t)txq->vsi->adapter->ptp_tx_index <<
+					 ICE_TXD_CTX_QW1_TSYN_S) & ICE_TXD_CTX_QW1_TSYN_M);
 
 			ctx_txd->tunneling_params =
 				rte_cpu_to_le_32(cd_tunneling_params);
-- 
2.25.1

[PATCH 3/3] examples/ptpclient: add frequency adjustment support

[Mingjin Ye]

This patch adds PI servo algorithm to support frequency
adjustment API for IEEE1588 PTP.

For example, the command for starting ptpclient with PI algorithm is:
./build/examples/dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p 0x1
--controller=pi

Signed-off-by: Simei Su <simei.su@intel.com>
Signed-off-by: Wenjun Wu <wenjun1.wu@intel.com>
Signed-off-by: Mingjin Ye <mingjinx.ye@intel.com>
---
 examples/ptpclient/ptpclient.c | 300 +++++++++++++++++++++++++++++----
 1 file changed, 265 insertions(+), 35 deletions(-)

diff --git a/examples/ptpclient/ptpclient.c b/examples/ptpclient/ptpclient.c
index afb61bba51..b97b858310 100644
--- a/examples/ptpclient/ptpclient.c
+++ b/examples/ptpclient/ptpclient.c
@@ -46,6 +46,35 @@ static volatile bool force_quit;
 #define KERNEL_TIME_ADJUST_LIMIT  20000
 #define PTP_PROTOCOL             0x88F7
 
+#define KP 0.7
+#define KI 0.3
+#define FREQ_EST_MARGIN 0.001
+
+enum servo_state {
+	SERVO_UNLOCKED,
+	SERVO_JUMP,
+	SERVO_LOCKED,
+};
+
+struct pi_servo {
+	double offset[2];
+	double local[2];
+	double drift;
+	double last_freq;
+	int count;
+
+	double max_frequency;
+	double step_threshold;
+	double first_step_threshold;
+	int first_update;
+};
+
+enum controller_mode {
+	MODE_NONE,
+	MODE_PI,
+	MAX_ALL
+} mode = MODE_PI;
+
 struct rte_mempool *mbuf_pool;
 uint32_t ptp_enabled_port_mask;
 uint8_t ptp_enabled_port_nb;
@@ -135,6 +164,9 @@ struct ptpv2_data_slave_ordinary {
 	uint8_t ptpset;
 	uint8_t kernel_time_set;
 	uint16_t current_ptp_port;
+	int64_t master_offset;
+	int64_t path_delay;
+	struct pi_servo *servo;
 };
 
 static struct ptpv2_data_slave_ordinary ptp_data;
@@ -293,36 +325,44 @@ print_clock_info(struct ptpv2_data_slave_ordinary *ptp_data)
 			ptp_data->tstamp3.tv_sec,
 			(ptp_data->tstamp3.tv_nsec));
 
-	printf("\nT4 - Master Clock.  %lds %ldns ",
+	printf("\nT4 - Master Clock.  %lds %ldns\n",
 			ptp_data->tstamp4.tv_sec,
 			(ptp_data->tstamp4.tv_nsec));
 
-	printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
+	if (mode == MODE_NONE) {
+		printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
 			ptp_data->delta);
 
-	clock_gettime(CLOCK_REALTIME, &sys_time);
-	rte_eth_timesync_read_time(ptp_data->current_ptp_port, &net_time);
+		clock_gettime(CLOCK_REALTIME, &sys_time);
+		rte_eth_timesync_read_time(ptp_data->current_ptp_port,
+					   &net_time);
 
-	time_t ts = net_time.tv_sec;
+		time_t ts = net_time.tv_sec;
 
-	printf("\n\nComparison between Linux kernel Time and PTP:");
+		printf("\n\nComparison between Linux kernel Time and PTP:");
 
-	printf("\nCurrent PTP Time: %.24s %.9ld ns",
+		printf("\nCurrent PTP Time: %.24s %.9ld ns",
 			ctime(&ts), net_time.tv_nsec);
 
-	nsec = (int64_t)timespec64_to_ns(&net_time) -
+		nsec = (int64_t)timespec64_to_ns(&net_time) -
 			(int64_t)timespec64_to_ns(&sys_time);
-	ptp_data->new_adj = ns_to_timeval(nsec);
+		ptp_data->new_adj = ns_to_timeval(nsec);
 
-	gettimeofday(&ptp_data->new_adj, NULL);
+		gettimeofday(&ptp_data->new_adj, NULL);
 
-	time_t tp = ptp_data->new_adj.tv_sec;
+		time_t tp = ptp_data->new_adj.tv_sec;
 
-	printf("\nCurrent SYS Time: %.24s %.6ld ns",
-				ctime(&tp), ptp_data->new_adj.tv_usec);
+		printf("\nCurrent SYS Time: %.24s %.6ld ns",
+			ctime(&tp), ptp_data->new_adj.tv_usec);
 
-	printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
-				nsec);
+		printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
+			nsec);
+	}
+
+	if (mode == MODE_PI) {
+		printf("path delay: %"PRId64"ns\n", ptp_data->path_delay);
+		printf("master offset: %"PRId64"ns\n", ptp_data->master_offset);
+	}
 
 	printf("[Ctrl+C to quit]\n");
 
@@ -385,21 +425,11 @@ parse_sync(struct ptpv2_data_slave_ordinary *ptp_data, uint16_t rx_tstamp_idx)
 static void
 parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 {
-	struct rte_ether_hdr *eth_hdr;
-	struct rte_ether_addr eth_addr;
 	struct ptp_header *ptp_hdr;
-	struct clock_id *client_clkid;
 	struct ptp_message *ptp_msg;
-	struct delay_req_msg *req_msg;
-	struct rte_mbuf *created_pkt;
 	struct tstamp *origin_tstamp;
-	struct rte_ether_addr eth_multicast = ether_multicast;
-	size_t pkt_size;
-	int wait_us;
 	struct rte_mbuf *m = ptp_data->m;
-	int ret;
 
-	eth_hdr = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
 	ptp_hdr = rte_pktmbuf_mtod_offset(m, struct ptp_header *,
 					  sizeof(struct rte_ether_hdr));
 	if (memcmp(&ptp_data->master_clock_id,
@@ -416,6 +446,150 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 	ptp_data->tstamp1.tv_sec =
 		((uint64_t)ntohl(origin_tstamp->sec_lsb)) |
 		(((uint64_t)ntohs(origin_tstamp->sec_msb)) << 32);
+}
+
+static double
+pi_sample(struct pi_servo *s, int64_t offset, double local_ts,
+	  enum servo_state *state)
+{
+	double ki_term, ppb = s->last_freq;
+	double freq_est_interval, localdiff;
+
+	switch (s->count) {
+	case 0:
+		s->offset[0] = offset;
+		s->local[0] = local_ts;
+		*state = SERVO_UNLOCKED;
+		s->count = 1;
+		break;
+	case 1:
+		s->offset[1] = offset;
+		s->local[1] = local_ts;
+
+		/* Make sure the first sample is older than the second. */
+		if (s->local[0] >= s->local[1]) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
+		/* Wait long enough before estimating the frequency offset. */
+		localdiff = (s->local[1] - s->local[0]) / 1e9;
+		localdiff += localdiff * FREQ_EST_MARGIN;
+		freq_est_interval = 0.016 / KI;
+		if (freq_est_interval > 1000.0)
+			freq_est_interval = 1000.0;
+
+		if (localdiff < freq_est_interval) {
+			*state = SERVO_UNLOCKED;
+			break;
+		}
+
+		/* Adjust drift by the measured frequency offset. */
+		s->drift += (1e9 - s->drift) * (s->offset[1] - s->offset[0]) /
+						(s->local[1] - s->local[0]);
+
+		if (s->drift < -s->max_frequency)
+			s->drift = -s->max_frequency;
+		else if (s->drift > s->max_frequency)
+			s->drift = s->max_frequency;
+
+		if ((s->first_update &&
+		     s->first_step_threshold &&
+		     s->first_step_threshold < llabs(offset)) ||
+		    (s->step_threshold &&
+		     s->step_threshold < llabs(offset)))
+			*state = SERVO_JUMP;
+		else
+			*state = SERVO_LOCKED;
+
+		ppb = s->drift;
+		s->count = 2;
+		break;
+	case 2:
+		/*
+		 * reset the clock servo when offset is greater than the max
+		 * offset value. Note that the clock jump will be performed in
+		 * step 1, so it is not necessary to have clock jump
+		 * immediately. This allows re-calculating drift as in initial
+		 * clock startup.
+		 */
+		if (s->step_threshold &&
+		    s->step_threshold < llabs(offset)) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
+		ki_term = KI * offset;
+		ppb = KP * offset + s->drift + ki_term;
+		if (ppb < -s->max_frequency)
+			ppb = -s->max_frequency;
+		else if (ppb > s->max_frequency)
+			ppb = s->max_frequency;
+		else
+			s->drift += ki_term;
+
+		*state = SERVO_LOCKED;
+		break;
+	}
+
+	s->last_freq = ppb;
+	return ppb;
+}
+
+static void
+ptp_adjust_freq(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns;
+	double adj_freq;
+	enum servo_state state = SERVO_UNLOCKED;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	ptp_data->master_offset = t2_ns - t1_ns - ptp_data->path_delay;
+	if (!ptp_data->path_delay)
+		return;
+
+	adj_freq = pi_sample(ptp_data->servo, ptp_data->master_offset, t2_ns,
+		     &state);
+
+	switch (state) {
+	case SERVO_UNLOCKED:
+		break;
+	case SERVO_JUMP:
+		ptp_data->servo->first_update = 0;
+		rte_eth_timesync_adjust_freq(ptp_data->portid,
+						-(long)(adj_freq * 65.536));
+		rte_eth_timesync_adjust_time(ptp_data->portid,
+					     -ptp_data->master_offset);
+		break;
+	case SERVO_LOCKED:
+		ptp_data->servo->first_update = 0;
+		rte_eth_timesync_adjust_freq(ptp_data->portid,
+					     -(long)(adj_freq * 65.536));
+		break;
+	}
+}
+
+static void
+send_delay_request(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	struct rte_ether_hdr *eth_hdr;
+	struct rte_ether_addr eth_addr;
+	struct ptp_header *ptp_hdr;
+	struct clock_id *client_clkid;
+	struct delay_req_msg *req_msg;
+	struct rte_mbuf *created_pkt;
+	struct rte_ether_addr eth_multicast = ether_multicast;
+	size_t pkt_size;
+	int wait_us;
+	struct rte_mbuf *m = ptp_data->m;
+	int ret;
+
+	eth_hdr = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
+	ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
+			+ sizeof(struct rte_ether_hdr));
 
 	if (ptp_data->seqID_FOLLOWUP == ptp_data->seqID_SYNC) {
 		ret = rte_eth_macaddr_get(ptp_data->portid, &eth_addr);
@@ -488,11 +662,10 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 		ptp_data->tstamp3.tv_sec = 0;
 
 		/* Wait at least 1 us to read TX timestamp. */
-		while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
-				&ptp_data->tstamp3) < 0) && (wait_us < 1000)) {
+		do {
 			rte_delay_us(1);
-			wait_us++;
-		}
+		} while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
+				&ptp_data->tstamp3) < 0) && (wait_us++ < 10));
 	}
 }
 
@@ -529,6 +702,25 @@ update_kernel_time(void)
 
 }
 
+static void
+clock_path_delay(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns, t3_ns, t4_ns;
+	int64_t pd, diff;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	t3_ns = timespec64_to_ns(&ptp_data->tstamp3);
+	t4_ns = timespec64_to_ns(&ptp_data->tstamp4);
+
+	pd = (t2_ns - t3_ns) + (t4_ns - t1_ns);
+	diff = t3_ns - t2_ns;
+	if (diff <= INT32_MAX && diff >= INT32_MIN)
+		ptp_data->path_delay = pd / 2;
+	else
+		ptp_data->path_delay = 0;
+}
+
 /*
  * Parse the DELAY_RESP message.
  */
@@ -541,7 +733,7 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 	uint16_t seq_id;
 
 	ptp_msg = rte_pktmbuf_mtod_offset(m, struct ptp_message *,
-					  sizeof(struct rte_ether_hdr));
+					sizeof(struct rte_ether_hdr));
 	seq_id = rte_be_to_cpu_16(ptp_msg->delay_resp.hdr.seq_id);
 	if (memcmp(&ptp_data->client_clock_id,
 		   &ptp_msg->delay_resp.req_port_id.clock_id,
@@ -553,11 +745,15 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 				((uint64_t)ntohl(rx_tstamp->sec_lsb)) |
 				(((uint64_t)ntohs(rx_tstamp->sec_msb)) << 32);
 
-			/* Evaluate the delta for adjustment. */
-			ptp_data->delta = delta_eval(ptp_data);
+			if (mode == MODE_PI) {
+				clock_path_delay(ptp_data);
+			} else {
+				/* Evaluate the delta for adjustment. */
+				ptp_data->delta = delta_eval(ptp_data);
 
-			rte_eth_timesync_adjust_time(ptp_data->portid,
-						     ptp_data->delta);
+				rte_eth_timesync_adjust_time(ptp_data->portid,
+								ptp_data->delta);
+			}
 
 			ptp_data->current_ptp_port = ptp_data->portid;
 
@@ -597,6 +793,9 @@ parse_ptp_frames(uint16_t portid, struct rte_mbuf *m) {
 			break;
 		case FOLLOW_UP:
 			parse_fup(&ptp_data);
+			if (mode == MODE_PI)
+				ptp_adjust_freq(&ptp_data);
+			send_delay_request(&ptp_data);
 			break;
 		case DELAY_RESP:
 			parse_drsp(&ptp_data);
@@ -688,6 +887,18 @@ parse_ptp_kernel(const char *param)
 	return 1;
 }
 
+static void
+servo_init(struct pi_servo *servo)
+{
+	memset(servo, 0x00, sizeof(*servo));
+
+	servo->drift = 100000000;
+	servo->last_freq = 100000000;
+	servo->count = 0;
+	servo->max_frequency = 100000000;
+	servo->step_threshold = 0.1 * NSEC_PER_SEC;
+}
+
 /* Parse the commandline arguments. */
 static int
 ptp_parse_args(int argc, char **argv)
@@ -696,7 +907,10 @@ ptp_parse_args(int argc, char **argv)
 	char **argvopt;
 	int option_index;
 	char *prgname = argv[0];
-	static struct option lgopts[] = { {NULL, 0, 0, 0} };
+	static struct option lgopts[] = {
+		{"controller", 1, 0, 0},
+		{NULL, 0, 0, 0}
+	};
 
 	argvopt = argv;
 
@@ -724,6 +938,11 @@ ptp_parse_args(int argc, char **argv)
 
 			ptp_data.kernel_time_set = ret;
 			break;
+		case 0:
+			if (!strcmp(lgopts[option_index].name, "controller"))
+				if (!strcmp(optarg, "pi"))
+					mode = MODE_PI;
+			break;
 
 		default:
 			print_usage(prgname);
@@ -778,6 +997,14 @@ main(int argc, char *argv[])
 		rte_exit(EXIT_FAILURE, "Error with PTP initialization\n");
 	/* >8 End of parsing specific arguments. */
 
+	if (mode == MODE_PI) {
+		ptp_data.servo = malloc(sizeof(*(ptp_data.servo)));
+		if (!ptp_data.servo)
+			rte_exit(EXIT_FAILURE, "no memory for servo\n");
+
+		servo_init(ptp_data.servo);
+	}
+
 	/* Check that there is an even number of ports to send/receive on. */
 	nb_ports = rte_eth_dev_count_avail();
 
@@ -831,6 +1058,9 @@ main(int argc, char *argv[])
 		rte_eth_dev_close(portid);
 	}
 
+	if (mode == MODE_PI)
+		free(ptp_data.servo);
+
 	/* clean up the EAL */
 	rte_eal_cleanup();
 
-- 
2.25.1

Re: [PATCH 0/3] add frequency adjustment support for PTP

[Ferruh Yigit]

On 9/5/2024 10:26 AM, Mingjin Ye wrote:
> [1/3] ethdev: add frequency adjustment API
> [2/3] net/ice: add frequency adjustment support for PTP
> [3/3] examples/ptpclient: add frequency adjustment support
> 
> Mingjin Ye (3):
>   ethdev: add frequency adjustment API
>   net/ice: add frequency adjustment support for PTP
>   examples/ptpclient: add frequency adjustment support
>

Hi Mingjin,

There are multiple instance of same patchset, can you please increase
the version information each time you send a new version and keep them
in same email thread?

Thanks,
ferruh

[PATCH 3/3] examples/ptpclient: add frequency adjustment support

[Mingjin Ye]

This patch adds PI servo algorithm to support frequency
adjustment API for IEEE1588 PTP.

For example, the command for starting ptpclient with PI algorithm is:
./build/examples/dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p 0x1
--controller=pi

Signed-off-by: Simei Su <simei.su@intel.com>
Signed-off-by: Wenjun Wu <wenjun1.wu@intel.com>
Signed-off-by: Mingjin Ye <mingjinx.ye@intel.com>
---
 examples/ptpclient/ptpclient.c | 300 +++++++++++++++++++++++++++++----
 1 file changed, 265 insertions(+), 35 deletions(-)

diff --git a/examples/ptpclient/ptpclient.c b/examples/ptpclient/ptpclient.c
index afb61bba51..b97b858310 100644
--- a/examples/ptpclient/ptpclient.c
+++ b/examples/ptpclient/ptpclient.c
@@ -46,6 +46,35 @@ static volatile bool force_quit;
 #define KERNEL_TIME_ADJUST_LIMIT  20000
 #define PTP_PROTOCOL             0x88F7
 
+#define KP 0.7
+#define KI 0.3
+#define FREQ_EST_MARGIN 0.001
+
+enum servo_state {
+	SERVO_UNLOCKED,
+	SERVO_JUMP,
+	SERVO_LOCKED,
+};
+
+struct pi_servo {
+	double offset[2];
+	double local[2];
+	double drift;
+	double last_freq;
+	int count;
+
+	double max_frequency;
+	double step_threshold;
+	double first_step_threshold;
+	int first_update;
+};
+
+enum controller_mode {
+	MODE_NONE,
+	MODE_PI,
+	MAX_ALL
+} mode = MODE_PI;
+
 struct rte_mempool *mbuf_pool;
 uint32_t ptp_enabled_port_mask;
 uint8_t ptp_enabled_port_nb;
@@ -135,6 +164,9 @@ struct ptpv2_data_slave_ordinary {
 	uint8_t ptpset;
 	uint8_t kernel_time_set;
 	uint16_t current_ptp_port;
+	int64_t master_offset;
+	int64_t path_delay;
+	struct pi_servo *servo;
 };
 
 static struct ptpv2_data_slave_ordinary ptp_data;
@@ -293,36 +325,44 @@ print_clock_info(struct ptpv2_data_slave_ordinary *ptp_data)
 			ptp_data->tstamp3.tv_sec,
 			(ptp_data->tstamp3.tv_nsec));
 
-	printf("\nT4 - Master Clock.  %lds %ldns ",
+	printf("\nT4 - Master Clock.  %lds %ldns\n",
 			ptp_data->tstamp4.tv_sec,
 			(ptp_data->tstamp4.tv_nsec));
 
-	printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
+	if (mode == MODE_NONE) {
+		printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
 			ptp_data->delta);
 
-	clock_gettime(CLOCK_REALTIME, &sys_time);
-	rte_eth_timesync_read_time(ptp_data->current_ptp_port, &net_time);
+		clock_gettime(CLOCK_REALTIME, &sys_time);
+		rte_eth_timesync_read_time(ptp_data->current_ptp_port,
+					   &net_time);
 
-	time_t ts = net_time.tv_sec;
+		time_t ts = net_time.tv_sec;
 
-	printf("\n\nComparison between Linux kernel Time and PTP:");
+		printf("\n\nComparison between Linux kernel Time and PTP:");
 
-	printf("\nCurrent PTP Time: %.24s %.9ld ns",
+		printf("\nCurrent PTP Time: %.24s %.9ld ns",
 			ctime(&ts), net_time.tv_nsec);
 
-	nsec = (int64_t)timespec64_to_ns(&net_time) -
+		nsec = (int64_t)timespec64_to_ns(&net_time) -
 			(int64_t)timespec64_to_ns(&sys_time);
-	ptp_data->new_adj = ns_to_timeval(nsec);
+		ptp_data->new_adj = ns_to_timeval(nsec);
 
-	gettimeofday(&ptp_data->new_adj, NULL);
+		gettimeofday(&ptp_data->new_adj, NULL);
 
-	time_t tp = ptp_data->new_adj.tv_sec;
+		time_t tp = ptp_data->new_adj.tv_sec;
 
-	printf("\nCurrent SYS Time: %.24s %.6ld ns",
-				ctime(&tp), ptp_data->new_adj.tv_usec);
+		printf("\nCurrent SYS Time: %.24s %.6ld ns",
+			ctime(&tp), ptp_data->new_adj.tv_usec);
 
-	printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
-				nsec);
+		printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
+			nsec);
+	}
+
+	if (mode == MODE_PI) {
+		printf("path delay: %"PRId64"ns\n", ptp_data->path_delay);
+		printf("master offset: %"PRId64"ns\n", ptp_data->master_offset);
+	}
 
 	printf("[Ctrl+C to quit]\n");
 
@@ -385,21 +425,11 @@ parse_sync(struct ptpv2_data_slave_ordinary *ptp_data, uint16_t rx_tstamp_idx)
 static void
 parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 {
-	struct rte_ether_hdr *eth_hdr;
-	struct rte_ether_addr eth_addr;
 	struct ptp_header *ptp_hdr;
-	struct clock_id *client_clkid;
 	struct ptp_message *ptp_msg;
-	struct delay_req_msg *req_msg;
-	struct rte_mbuf *created_pkt;
 	struct tstamp *origin_tstamp;
-	struct rte_ether_addr eth_multicast = ether_multicast;
-	size_t pkt_size;
-	int wait_us;
 	struct rte_mbuf *m = ptp_data->m;
-	int ret;
 
-	eth_hdr = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
 	ptp_hdr = rte_pktmbuf_mtod_offset(m, struct ptp_header *,
 					  sizeof(struct rte_ether_hdr));
 	if (memcmp(&ptp_data->master_clock_id,
@@ -416,6 +446,150 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 	ptp_data->tstamp1.tv_sec =
 		((uint64_t)ntohl(origin_tstamp->sec_lsb)) |
 		(((uint64_t)ntohs(origin_tstamp->sec_msb)) << 32);
+}
+
+static double
+pi_sample(struct pi_servo *s, int64_t offset, double local_ts,
+	  enum servo_state *state)
+{
+	double ki_term, ppb = s->last_freq;
+	double freq_est_interval, localdiff;
+
+	switch (s->count) {
+	case 0:
+		s->offset[0] = offset;
+		s->local[0] = local_ts;
+		*state = SERVO_UNLOCKED;
+		s->count = 1;
+		break;
+	case 1:
+		s->offset[1] = offset;
+		s->local[1] = local_ts;
+
+		/* Make sure the first sample is older than the second. */
+		if (s->local[0] >= s->local[1]) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
+		/* Wait long enough before estimating the frequency offset. */
+		localdiff = (s->local[1] - s->local[0]) / 1e9;
+		localdiff += localdiff * FREQ_EST_MARGIN;
+		freq_est_interval = 0.016 / KI;
+		if (freq_est_interval > 1000.0)
+			freq_est_interval = 1000.0;
+
+		if (localdiff < freq_est_interval) {
+			*state = SERVO_UNLOCKED;
+			break;
+		}
+
+		/* Adjust drift by the measured frequency offset. */
+		s->drift += (1e9 - s->drift) * (s->offset[1] - s->offset[0]) /
+						(s->local[1] - s->local[0]);
+
+		if (s->drift < -s->max_frequency)
+			s->drift = -s->max_frequency;
+		else if (s->drift > s->max_frequency)
+			s->drift = s->max_frequency;
+
+		if ((s->first_update &&
+		     s->first_step_threshold &&
+		     s->first_step_threshold < llabs(offset)) ||
+		    (s->step_threshold &&
+		     s->step_threshold < llabs(offset)))
+			*state = SERVO_JUMP;
+		else
+			*state = SERVO_LOCKED;
+
+		ppb = s->drift;
+		s->count = 2;
+		break;
+	case 2:
+		/*
+		 * reset the clock servo when offset is greater than the max
+		 * offset value. Note that the clock jump will be performed in
+		 * step 1, so it is not necessary to have clock jump
+		 * immediately. This allows re-calculating drift as in initial
+		 * clock startup.
+		 */
+		if (s->step_threshold &&
+		    s->step_threshold < llabs(offset)) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
+		ki_term = KI * offset;
+		ppb = KP * offset + s->drift + ki_term;
+		if (ppb < -s->max_frequency)
+			ppb = -s->max_frequency;
+		else if (ppb > s->max_frequency)
+			ppb = s->max_frequency;
+		else
+			s->drift += ki_term;
+
+		*state = SERVO_LOCKED;
+		break;
+	}
+
+	s->last_freq = ppb;
+	return ppb;
+}
+
+static void
+ptp_adjust_freq(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns;
+	double adj_freq;
+	enum servo_state state = SERVO_UNLOCKED;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	ptp_data->master_offset = t2_ns - t1_ns - ptp_data->path_delay;
+	if (!ptp_data->path_delay)
+		return;
+
+	adj_freq = pi_sample(ptp_data->servo, ptp_data->master_offset, t2_ns,
+		     &state);
+
+	switch (state) {
+	case SERVO_UNLOCKED:
+		break;
+	case SERVO_JUMP:
+		ptp_data->servo->first_update = 0;
+		rte_eth_timesync_adjust_freq(ptp_data->portid,
+						-(long)(adj_freq * 65.536));
+		rte_eth_timesync_adjust_time(ptp_data->portid,
+					     -ptp_data->master_offset);
+		break;
+	case SERVO_LOCKED:
+		ptp_data->servo->first_update = 0;
+		rte_eth_timesync_adjust_freq(ptp_data->portid,
+					     -(long)(adj_freq * 65.536));
+		break;
+	}
+}
+
+static void
+send_delay_request(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	struct rte_ether_hdr *eth_hdr;
+	struct rte_ether_addr eth_addr;
+	struct ptp_header *ptp_hdr;
+	struct clock_id *client_clkid;
+	struct delay_req_msg *req_msg;
+	struct rte_mbuf *created_pkt;
+	struct rte_ether_addr eth_multicast = ether_multicast;
+	size_t pkt_size;
+	int wait_us;
+	struct rte_mbuf *m = ptp_data->m;
+	int ret;
+
+	eth_hdr = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
+	ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
+			+ sizeof(struct rte_ether_hdr));
 
 	if (ptp_data->seqID_FOLLOWUP == ptp_data->seqID_SYNC) {
 		ret = rte_eth_macaddr_get(ptp_data->portid, &eth_addr);
@@ -488,11 +662,10 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 		ptp_data->tstamp3.tv_sec = 0;
 
 		/* Wait at least 1 us to read TX timestamp. */
-		while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
-				&ptp_data->tstamp3) < 0) && (wait_us < 1000)) {
+		do {
 			rte_delay_us(1);
-			wait_us++;
-		}
+		} while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
+				&ptp_data->tstamp3) < 0) && (wait_us++ < 10));
 	}
 }
 
@@ -529,6 +702,25 @@ update_kernel_time(void)
 
 }
 
+static void
+clock_path_delay(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns, t3_ns, t4_ns;
+	int64_t pd, diff;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	t3_ns = timespec64_to_ns(&ptp_data->tstamp3);
+	t4_ns = timespec64_to_ns(&ptp_data->tstamp4);
+
+	pd = (t2_ns - t3_ns) + (t4_ns - t1_ns);
+	diff = t3_ns - t2_ns;
+	if (diff <= INT32_MAX && diff >= INT32_MIN)
+		ptp_data->path_delay = pd / 2;
+	else
+		ptp_data->path_delay = 0;
+}
+
 /*
  * Parse the DELAY_RESP message.
  */
@@ -541,7 +733,7 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 	uint16_t seq_id;
 
 	ptp_msg = rte_pktmbuf_mtod_offset(m, struct ptp_message *,
-					  sizeof(struct rte_ether_hdr));
+					sizeof(struct rte_ether_hdr));
 	seq_id = rte_be_to_cpu_16(ptp_msg->delay_resp.hdr.seq_id);
 	if (memcmp(&ptp_data->client_clock_id,
 		   &ptp_msg->delay_resp.req_port_id.clock_id,
@@ -553,11 +745,15 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 				((uint64_t)ntohl(rx_tstamp->sec_lsb)) |
 				(((uint64_t)ntohs(rx_tstamp->sec_msb)) << 32);
 
-			/* Evaluate the delta for adjustment. */
-			ptp_data->delta = delta_eval(ptp_data);
+			if (mode == MODE_PI) {
+				clock_path_delay(ptp_data);
+			} else {
+				/* Evaluate the delta for adjustment. */
+				ptp_data->delta = delta_eval(ptp_data);
 
-			rte_eth_timesync_adjust_time(ptp_data->portid,
-						     ptp_data->delta);
+				rte_eth_timesync_adjust_time(ptp_data->portid,
+								ptp_data->delta);
+			}
 
 			ptp_data->current_ptp_port = ptp_data->portid;
 
@@ -597,6 +793,9 @@ parse_ptp_frames(uint16_t portid, struct rte_mbuf *m) {
 			break;
 		case FOLLOW_UP:
 			parse_fup(&ptp_data);
+			if (mode == MODE_PI)
+				ptp_adjust_freq(&ptp_data);
+			send_delay_request(&ptp_data);
 			break;
 		case DELAY_RESP:
 			parse_drsp(&ptp_data);
@@ -688,6 +887,18 @@ parse_ptp_kernel(const char *param)
 	return 1;
 }
 
+static void
+servo_init(struct pi_servo *servo)
+{
+	memset(servo, 0x00, sizeof(*servo));
+
+	servo->drift = 100000000;
+	servo->last_freq = 100000000;
+	servo->count = 0;
+	servo->max_frequency = 100000000;
+	servo->step_threshold = 0.1 * NSEC_PER_SEC;
+}
+
 /* Parse the commandline arguments. */
 static int
 ptp_parse_args(int argc, char **argv)
@@ -696,7 +907,10 @@ ptp_parse_args(int argc, char **argv)
 	char **argvopt;
 	int option_index;
 	char *prgname = argv[0];
-	static struct option lgopts[] = { {NULL, 0, 0, 0} };
+	static struct option lgopts[] = {
+		{"controller", 1, 0, 0},
+		{NULL, 0, 0, 0}
+	};
 
 	argvopt = argv;
 
@@ -724,6 +938,11 @@ ptp_parse_args(int argc, char **argv)
 
 			ptp_data.kernel_time_set = ret;
 			break;
+		case 0:
+			if (!strcmp(lgopts[option_index].name, "controller"))
+				if (!strcmp(optarg, "pi"))
+					mode = MODE_PI;
+			break;
 
 		default:
 			print_usage(prgname);
@@ -778,6 +997,14 @@ main(int argc, char *argv[])
 		rte_exit(EXIT_FAILURE, "Error with PTP initialization\n");
 	/* >8 End of parsing specific arguments. */
 
+	if (mode == MODE_PI) {
+		ptp_data.servo = malloc(sizeof(*(ptp_data.servo)));
+		if (!ptp_data.servo)
+			rte_exit(EXIT_FAILURE, "no memory for servo\n");
+
+		servo_init(ptp_data.servo);
+	}
+
 	/* Check that there is an even number of ports to send/receive on. */
 	nb_ports = rte_eth_dev_count_avail();
 
@@ -831,6 +1058,9 @@ main(int argc, char *argv[])
 		rte_eth_dev_close(portid);
 	}
 
+	if (mode == MODE_PI)
+		free(ptp_data.servo);
+
 	/* clean up the EAL */
 	rte_eal_cleanup();
 
-- 
2.25.1

[PATCH 3/3] examples/ptpclient: add frequency adjustment support

[Mingjin Ye]

This patch adds PI servo algorithm to support frequency
adjustment API for IEEE1588 PTP.

For example, the command for starting ptpclient with PI algorithm is:
./build/examples/dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p 0x1
--controller=pi

Signed-off-by: Simei Su <simei.su@intel.com>
Signed-off-by: Wenjun Wu <wenjun1.wu@intel.com>
Signed-off-by: Mingjin Ye <mingjinx.ye@intel.com>
---
 examples/ptpclient/ptpclient.c | 300 +++++++++++++++++++++++++++++----
 1 file changed, 265 insertions(+), 35 deletions(-)

diff --git a/examples/ptpclient/ptpclient.c b/examples/ptpclient/ptpclient.c
index afb61bba51..b97b858310 100644
--- a/examples/ptpclient/ptpclient.c
+++ b/examples/ptpclient/ptpclient.c
@@ -46,6 +46,35 @@ static volatile bool force_quit;
 #define KERNEL_TIME_ADJUST_LIMIT  20000
 #define PTP_PROTOCOL             0x88F7
 
+#define KP 0.7
+#define KI 0.3
+#define FREQ_EST_MARGIN 0.001
+
+enum servo_state {
+	SERVO_UNLOCKED,
+	SERVO_JUMP,
+	SERVO_LOCKED,
+};
+
+struct pi_servo {
+	double offset[2];
+	double local[2];
+	double drift;
+	double last_freq;
+	int count;
+
+	double max_frequency;
+	double step_threshold;
+	double first_step_threshold;
+	int first_update;
+};
+
+enum controller_mode {
+	MODE_NONE,
+	MODE_PI,
+	MAX_ALL
+} mode = MODE_PI;
+
 struct rte_mempool *mbuf_pool;
 uint32_t ptp_enabled_port_mask;
 uint8_t ptp_enabled_port_nb;
@@ -135,6 +164,9 @@ struct ptpv2_data_slave_ordinary {
 	uint8_t ptpset;
 	uint8_t kernel_time_set;
 	uint16_t current_ptp_port;
+	int64_t master_offset;
+	int64_t path_delay;
+	struct pi_servo *servo;
 };
 
 static struct ptpv2_data_slave_ordinary ptp_data;
@@ -293,36 +325,44 @@ print_clock_info(struct ptpv2_data_slave_ordinary *ptp_data)
 			ptp_data->tstamp3.tv_sec,
 			(ptp_data->tstamp3.tv_nsec));
 
-	printf("\nT4 - Master Clock.  %lds %ldns ",
+	printf("\nT4 - Master Clock.  %lds %ldns\n",
 			ptp_data->tstamp4.tv_sec,
 			(ptp_data->tstamp4.tv_nsec));
 
-	printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
+	if (mode == MODE_NONE) {
+		printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
 			ptp_data->delta);
 
-	clock_gettime(CLOCK_REALTIME, &sys_time);
-	rte_eth_timesync_read_time(ptp_data->current_ptp_port, &net_time);
+		clock_gettime(CLOCK_REALTIME, &sys_time);
+		rte_eth_timesync_read_time(ptp_data->current_ptp_port,
+					   &net_time);
 
-	time_t ts = net_time.tv_sec;
+		time_t ts = net_time.tv_sec;
 
-	printf("\n\nComparison between Linux kernel Time and PTP:");
+		printf("\n\nComparison between Linux kernel Time and PTP:");
 
-	printf("\nCurrent PTP Time: %.24s %.9ld ns",
+		printf("\nCurrent PTP Time: %.24s %.9ld ns",
 			ctime(&ts), net_time.tv_nsec);
 
-	nsec = (int64_t)timespec64_to_ns(&net_time) -
+		nsec = (int64_t)timespec64_to_ns(&net_time) -
 			(int64_t)timespec64_to_ns(&sys_time);
-	ptp_data->new_adj = ns_to_timeval(nsec);
+		ptp_data->new_adj = ns_to_timeval(nsec);
 
-	gettimeofday(&ptp_data->new_adj, NULL);
+		gettimeofday(&ptp_data->new_adj, NULL);
 
-	time_t tp = ptp_data->new_adj.tv_sec;
+		time_t tp = ptp_data->new_adj.tv_sec;
 
-	printf("\nCurrent SYS Time: %.24s %.6ld ns",
-				ctime(&tp), ptp_data->new_adj.tv_usec);
+		printf("\nCurrent SYS Time: %.24s %.6ld ns",
+			ctime(&tp), ptp_data->new_adj.tv_usec);
 
-	printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
-				nsec);
+		printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
+			nsec);
+	}
+
+	if (mode == MODE_PI) {
+		printf("path delay: %"PRId64"ns\n", ptp_data->path_delay);
+		printf("master offset: %"PRId64"ns\n", ptp_data->master_offset);
+	}
 
 	printf("[Ctrl+C to quit]\n");
 
@@ -385,21 +425,11 @@ parse_sync(struct ptpv2_data_slave_ordinary *ptp_data, uint16_t rx_tstamp_idx)
 static void
 parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 {
-	struct rte_ether_hdr *eth_hdr;
-	struct rte_ether_addr eth_addr;
 	struct ptp_header *ptp_hdr;
-	struct clock_id *client_clkid;
 	struct ptp_message *ptp_msg;
-	struct delay_req_msg *req_msg;
-	struct rte_mbuf *created_pkt;
 	struct tstamp *origin_tstamp;
-	struct rte_ether_addr eth_multicast = ether_multicast;
-	size_t pkt_size;
-	int wait_us;
 	struct rte_mbuf *m = ptp_data->m;
-	int ret;
 
-	eth_hdr = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
 	ptp_hdr = rte_pktmbuf_mtod_offset(m, struct ptp_header *,
 					  sizeof(struct rte_ether_hdr));
 	if (memcmp(&ptp_data->master_clock_id,
@@ -416,6 +446,150 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 	ptp_data->tstamp1.tv_sec =
 		((uint64_t)ntohl(origin_tstamp->sec_lsb)) |
 		(((uint64_t)ntohs(origin_tstamp->sec_msb)) << 32);
+}
+
+static double
+pi_sample(struct pi_servo *s, int64_t offset, double local_ts,
+	  enum servo_state *state)
+{
+	double ki_term, ppb = s->last_freq;
+	double freq_est_interval, localdiff;
+
+	switch (s->count) {
+	case 0:
+		s->offset[0] = offset;
+		s->local[0] = local_ts;
+		*state = SERVO_UNLOCKED;
+		s->count = 1;
+		break;
+	case 1:
+		s->offset[1] = offset;
+		s->local[1] = local_ts;
+
+		/* Make sure the first sample is older than the second. */
+		if (s->local[0] >= s->local[1]) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
+		/* Wait long enough before estimating the frequency offset. */
+		localdiff = (s->local[1] - s->local[0]) / 1e9;
+		localdiff += localdiff * FREQ_EST_MARGIN;
+		freq_est_interval = 0.016 / KI;
+		if (freq_est_interval > 1000.0)
+			freq_est_interval = 1000.0;
+
+		if (localdiff < freq_est_interval) {
+			*state = SERVO_UNLOCKED;
+			break;
+		}
+
+		/* Adjust drift by the measured frequency offset. */
+		s->drift += (1e9 - s->drift) * (s->offset[1] - s->offset[0]) /
+						(s->local[1] - s->local[0]);
+
+		if (s->drift < -s->max_frequency)
+			s->drift = -s->max_frequency;
+		else if (s->drift > s->max_frequency)
+			s->drift = s->max_frequency;
+
+		if ((s->first_update &&
+		     s->first_step_threshold &&
+		     s->first_step_threshold < llabs(offset)) ||
+		    (s->step_threshold &&
+		     s->step_threshold < llabs(offset)))
+			*state = SERVO_JUMP;
+		else
+			*state = SERVO_LOCKED;
+
+		ppb = s->drift;
+		s->count = 2;
+		break;
+	case 2:
+		/*
+		 * reset the clock servo when offset is greater than the max
+		 * offset value. Note that the clock jump will be performed in
+		 * step 1, so it is not necessary to have clock jump
+		 * immediately. This allows re-calculating drift as in initial
+		 * clock startup.
+		 */
+		if (s->step_threshold &&
+		    s->step_threshold < llabs(offset)) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
+		ki_term = KI * offset;
+		ppb = KP * offset + s->drift + ki_term;
+		if (ppb < -s->max_frequency)
+			ppb = -s->max_frequency;
+		else if (ppb > s->max_frequency)
+			ppb = s->max_frequency;
+		else
+			s->drift += ki_term;
+
+		*state = SERVO_LOCKED;
+		break;
+	}
+
+	s->last_freq = ppb;
+	return ppb;
+}
+
+static void
+ptp_adjust_freq(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns;
+	double adj_freq;
+	enum servo_state state = SERVO_UNLOCKED;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	ptp_data->master_offset = t2_ns - t1_ns - ptp_data->path_delay;
+	if (!ptp_data->path_delay)
+		return;
+
+	adj_freq = pi_sample(ptp_data->servo, ptp_data->master_offset, t2_ns,
+		     &state);
+
+	switch (state) {
+	case SERVO_UNLOCKED:
+		break;
+	case SERVO_JUMP:
+		ptp_data->servo->first_update = 0;
+		rte_eth_timesync_adjust_freq(ptp_data->portid,
+						-(long)(adj_freq * 65.536));
+		rte_eth_timesync_adjust_time(ptp_data->portid,
+					     -ptp_data->master_offset);
+		break;
+	case SERVO_LOCKED:
+		ptp_data->servo->first_update = 0;
+		rte_eth_timesync_adjust_freq(ptp_data->portid,
+					     -(long)(adj_freq * 65.536));
+		break;
+	}
+}
+
+static void
+send_delay_request(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	struct rte_ether_hdr *eth_hdr;
+	struct rte_ether_addr eth_addr;
+	struct ptp_header *ptp_hdr;
+	struct clock_id *client_clkid;
+	struct delay_req_msg *req_msg;
+	struct rte_mbuf *created_pkt;
+	struct rte_ether_addr eth_multicast = ether_multicast;
+	size_t pkt_size;
+	int wait_us;
+	struct rte_mbuf *m = ptp_data->m;
+	int ret;
+
+	eth_hdr = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
+	ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
+			+ sizeof(struct rte_ether_hdr));
 
 	if (ptp_data->seqID_FOLLOWUP == ptp_data->seqID_SYNC) {
 		ret = rte_eth_macaddr_get(ptp_data->portid, &eth_addr);
@@ -488,11 +662,10 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 		ptp_data->tstamp3.tv_sec = 0;
 
 		/* Wait at least 1 us to read TX timestamp. */
-		while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
-				&ptp_data->tstamp3) < 0) && (wait_us < 1000)) {
+		do {
 			rte_delay_us(1);
-			wait_us++;
-		}
+		} while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
+				&ptp_data->tstamp3) < 0) && (wait_us++ < 10));
 	}
 }
 
@@ -529,6 +702,25 @@ update_kernel_time(void)
 
 }
 
+static void
+clock_path_delay(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns, t3_ns, t4_ns;
+	int64_t pd, diff;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	t3_ns = timespec64_to_ns(&ptp_data->tstamp3);
+	t4_ns = timespec64_to_ns(&ptp_data->tstamp4);
+
+	pd = (t2_ns - t3_ns) + (t4_ns - t1_ns);
+	diff = t3_ns - t2_ns;
+	if (diff <= INT32_MAX && diff >= INT32_MIN)
+		ptp_data->path_delay = pd / 2;
+	else
+		ptp_data->path_delay = 0;
+}
+
 /*
  * Parse the DELAY_RESP message.
  */
@@ -541,7 +733,7 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 	uint16_t seq_id;
 
 	ptp_msg = rte_pktmbuf_mtod_offset(m, struct ptp_message *,
-					  sizeof(struct rte_ether_hdr));
+					sizeof(struct rte_ether_hdr));
 	seq_id = rte_be_to_cpu_16(ptp_msg->delay_resp.hdr.seq_id);
 	if (memcmp(&ptp_data->client_clock_id,
 		   &ptp_msg->delay_resp.req_port_id.clock_id,
@@ -553,11 +745,15 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 				((uint64_t)ntohl(rx_tstamp->sec_lsb)) |
 				(((uint64_t)ntohs(rx_tstamp->sec_msb)) << 32);
 
-			/* Evaluate the delta for adjustment. */
-			ptp_data->delta = delta_eval(ptp_data);
+			if (mode == MODE_PI) {
+				clock_path_delay(ptp_data);
+			} else {
+				/* Evaluate the delta for adjustment. */
+				ptp_data->delta = delta_eval(ptp_data);
 
-			rte_eth_timesync_adjust_time(ptp_data->portid,
-						     ptp_data->delta);
+				rte_eth_timesync_adjust_time(ptp_data->portid,
+								ptp_data->delta);
+			}
 
 			ptp_data->current_ptp_port = ptp_data->portid;
 
@@ -597,6 +793,9 @@ parse_ptp_frames(uint16_t portid, struct rte_mbuf *m) {
 			break;
 		case FOLLOW_UP:
 			parse_fup(&ptp_data);
+			if (mode == MODE_PI)
+				ptp_adjust_freq(&ptp_data);
+			send_delay_request(&ptp_data);
 			break;
 		case DELAY_RESP:
 			parse_drsp(&ptp_data);
@@ -688,6 +887,18 @@ parse_ptp_kernel(const char *param)
 	return 1;
 }
 
+static void
+servo_init(struct pi_servo *servo)
+{
+	memset(servo, 0x00, sizeof(*servo));
+
+	servo->drift = 100000000;
+	servo->last_freq = 100000000;
+	servo->count = 0;
+	servo->max_frequency = 100000000;
+	servo->step_threshold = 0.1 * NSEC_PER_SEC;
+}
+
 /* Parse the commandline arguments. */
 static int
 ptp_parse_args(int argc, char **argv)
@@ -696,7 +907,10 @@ ptp_parse_args(int argc, char **argv)
 	char **argvopt;
 	int option_index;
 	char *prgname = argv[0];
-	static struct option lgopts[] = { {NULL, 0, 0, 0} };
+	static struct option lgopts[] = {
+		{"controller", 1, 0, 0},
+		{NULL, 0, 0, 0}
+	};
 
 	argvopt = argv;
 
@@ -724,6 +938,11 @@ ptp_parse_args(int argc, char **argv)
 
 			ptp_data.kernel_time_set = ret;
 			break;
+		case 0:
+			if (!strcmp(lgopts[option_index].name, "controller"))
+				if (!strcmp(optarg, "pi"))
+					mode = MODE_PI;
+			break;
 
 		default:
 			print_usage(prgname);
@@ -778,6 +997,14 @@ main(int argc, char *argv[])
 		rte_exit(EXIT_FAILURE, "Error with PTP initialization\n");
 	/* >8 End of parsing specific arguments. */
 
+	if (mode == MODE_PI) {
+		ptp_data.servo = malloc(sizeof(*(ptp_data.servo)));
+		if (!ptp_data.servo)
+			rte_exit(EXIT_FAILURE, "no memory for servo\n");
+
+		servo_init(ptp_data.servo);
+	}
+
 	/* Check that there is an even number of ports to send/receive on. */
 	nb_ports = rte_eth_dev_count_avail();
 
@@ -831,6 +1058,9 @@ main(int argc, char *argv[])
 		rte_eth_dev_close(portid);
 	}
 
+	if (mode == MODE_PI)
+		free(ptp_data.servo);
+
 	/* clean up the EAL */
 	rte_eal_cleanup();
 
-- 
2.25.1

[PATCH 3/3] examples/ptpclient: add frequency adjustment support

[Mingjin Ye]

This patch adds PI servo algorithm to support frequency
adjustment API for IEEE1588 PTP.

For example, the command for starting ptpclient with PI algorithm is:
./build/examples/dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p 0x1
--controller=pi

Signed-off-by: Simei Su <simei.su@intel.com>
Signed-off-by: Wenjun Wu <wenjun1.wu@intel.com>
Signed-off-by: Mingjin Ye <mingjinx.ye@intel.com>
---
 examples/ptpclient/ptpclient.c | 300 +++++++++++++++++++++++++++++----
 1 file changed, 265 insertions(+), 35 deletions(-)

diff --git a/examples/ptpclient/ptpclient.c b/examples/ptpclient/ptpclient.c
index afb61bba51..b97b858310 100644
--- a/examples/ptpclient/ptpclient.c
+++ b/examples/ptpclient/ptpclient.c
@@ -46,6 +46,35 @@ static volatile bool force_quit;
 #define KERNEL_TIME_ADJUST_LIMIT  20000
 #define PTP_PROTOCOL             0x88F7
 
+#define KP 0.7
+#define KI 0.3
+#define FREQ_EST_MARGIN 0.001
+
+enum servo_state {
+	SERVO_UNLOCKED,
+	SERVO_JUMP,
+	SERVO_LOCKED,
+};
+
+struct pi_servo {
+	double offset[2];
+	double local[2];
+	double drift;
+	double last_freq;
+	int count;
+
+	double max_frequency;
+	double step_threshold;
+	double first_step_threshold;
+	int first_update;
+};
+
+enum controller_mode {
+	MODE_NONE,
+	MODE_PI,
+	MAX_ALL
+} mode = MODE_PI;
+
 struct rte_mempool *mbuf_pool;
 uint32_t ptp_enabled_port_mask;
 uint8_t ptp_enabled_port_nb;
@@ -135,6 +164,9 @@ struct ptpv2_data_slave_ordinary {
 	uint8_t ptpset;
 	uint8_t kernel_time_set;
 	uint16_t current_ptp_port;
+	int64_t master_offset;
+	int64_t path_delay;
+	struct pi_servo *servo;
 };
 
 static struct ptpv2_data_slave_ordinary ptp_data;
@@ -293,36 +325,44 @@ print_clock_info(struct ptpv2_data_slave_ordinary *ptp_data)
 			ptp_data->tstamp3.tv_sec,
 			(ptp_data->tstamp3.tv_nsec));
 
-	printf("\nT4 - Master Clock.  %lds %ldns ",
+	printf("\nT4 - Master Clock.  %lds %ldns\n",
 			ptp_data->tstamp4.tv_sec,
 			(ptp_data->tstamp4.tv_nsec));
 
-	printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
+	if (mode == MODE_NONE) {
+		printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
 			ptp_data->delta);
 
-	clock_gettime(CLOCK_REALTIME, &sys_time);
-	rte_eth_timesync_read_time(ptp_data->current_ptp_port, &net_time);
+		clock_gettime(CLOCK_REALTIME, &sys_time);
+		rte_eth_timesync_read_time(ptp_data->current_ptp_port,
+					   &net_time);
 
-	time_t ts = net_time.tv_sec;
+		time_t ts = net_time.tv_sec;
 
-	printf("\n\nComparison between Linux kernel Time and PTP:");
+		printf("\n\nComparison between Linux kernel Time and PTP:");
 
-	printf("\nCurrent PTP Time: %.24s %.9ld ns",
+		printf("\nCurrent PTP Time: %.24s %.9ld ns",
 			ctime(&ts), net_time.tv_nsec);
 
-	nsec = (int64_t)timespec64_to_ns(&net_time) -
+		nsec = (int64_t)timespec64_to_ns(&net_time) -
 			(int64_t)timespec64_to_ns(&sys_time);
-	ptp_data->new_adj = ns_to_timeval(nsec);
+		ptp_data->new_adj = ns_to_timeval(nsec);
 
-	gettimeofday(&ptp_data->new_adj, NULL);
+		gettimeofday(&ptp_data->new_adj, NULL);
 
-	time_t tp = ptp_data->new_adj.tv_sec;
+		time_t tp = ptp_data->new_adj.tv_sec;
 
-	printf("\nCurrent SYS Time: %.24s %.6ld ns",
-				ctime(&tp), ptp_data->new_adj.tv_usec);
+		printf("\nCurrent SYS Time: %.24s %.6ld ns",
+			ctime(&tp), ptp_data->new_adj.tv_usec);
 
-	printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
-				nsec);
+		printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
+			nsec);
+	}
+
+	if (mode == MODE_PI) {
+		printf("path delay: %"PRId64"ns\n", ptp_data->path_delay);
+		printf("master offset: %"PRId64"ns\n", ptp_data->master_offset);
+	}
 
 	printf("[Ctrl+C to quit]\n");
 
@@ -385,21 +425,11 @@ parse_sync(struct ptpv2_data_slave_ordinary *ptp_data, uint16_t rx_tstamp_idx)
 static void
 parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 {
-	struct rte_ether_hdr *eth_hdr;
-	struct rte_ether_addr eth_addr;
 	struct ptp_header *ptp_hdr;
-	struct clock_id *client_clkid;
 	struct ptp_message *ptp_msg;
-	struct delay_req_msg *req_msg;
-	struct rte_mbuf *created_pkt;
 	struct tstamp *origin_tstamp;
-	struct rte_ether_addr eth_multicast = ether_multicast;
-	size_t pkt_size;
-	int wait_us;
 	struct rte_mbuf *m = ptp_data->m;
-	int ret;
 
-	eth_hdr = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
 	ptp_hdr = rte_pktmbuf_mtod_offset(m, struct ptp_header *,
 					  sizeof(struct rte_ether_hdr));
 	if (memcmp(&ptp_data->master_clock_id,
@@ -416,6 +446,150 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 	ptp_data->tstamp1.tv_sec =
 		((uint64_t)ntohl(origin_tstamp->sec_lsb)) |
 		(((uint64_t)ntohs(origin_tstamp->sec_msb)) << 32);
+}
+
+static double
+pi_sample(struct pi_servo *s, int64_t offset, double local_ts,
+	  enum servo_state *state)
+{
+	double ki_term, ppb = s->last_freq;
+	double freq_est_interval, localdiff;
+
+	switch (s->count) {
+	case 0:
+		s->offset[0] = offset;
+		s->local[0] = local_ts;
+		*state = SERVO_UNLOCKED;
+		s->count = 1;
+		break;
+	case 1:
+		s->offset[1] = offset;
+		s->local[1] = local_ts;
+
+		/* Make sure the first sample is older than the second. */
+		if (s->local[0] >= s->local[1]) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
+		/* Wait long enough before estimating the frequency offset. */
+		localdiff = (s->local[1] - s->local[0]) / 1e9;
+		localdiff += localdiff * FREQ_EST_MARGIN;
+		freq_est_interval = 0.016 / KI;
+		if (freq_est_interval > 1000.0)
+			freq_est_interval = 1000.0;
+
+		if (localdiff < freq_est_interval) {
+			*state = SERVO_UNLOCKED;
+			break;
+		}
+
+		/* Adjust drift by the measured frequency offset. */
+		s->drift += (1e9 - s->drift) * (s->offset[1] - s->offset[0]) /
+						(s->local[1] - s->local[0]);
+
+		if (s->drift < -s->max_frequency)
+			s->drift = -s->max_frequency;
+		else if (s->drift > s->max_frequency)
+			s->drift = s->max_frequency;
+
+		if ((s->first_update &&
+		     s->first_step_threshold &&
+		     s->first_step_threshold < llabs(offset)) ||
+		    (s->step_threshold &&
+		     s->step_threshold < llabs(offset)))
+			*state = SERVO_JUMP;
+		else
+			*state = SERVO_LOCKED;
+
+		ppb = s->drift;
+		s->count = 2;
+		break;
+	case 2:
+		/*
+		 * reset the clock servo when offset is greater than the max
+		 * offset value. Note that the clock jump will be performed in
+		 * step 1, so it is not necessary to have clock jump
+		 * immediately. This allows re-calculating drift as in initial
+		 * clock startup.
+		 */
+		if (s->step_threshold &&
+		    s->step_threshold < llabs(offset)) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
+		ki_term = KI * offset;
+		ppb = KP * offset + s->drift + ki_term;
+		if (ppb < -s->max_frequency)
+			ppb = -s->max_frequency;
+		else if (ppb > s->max_frequency)
+			ppb = s->max_frequency;
+		else
+			s->drift += ki_term;
+
+		*state = SERVO_LOCKED;
+		break;
+	}
+
+	s->last_freq = ppb;
+	return ppb;
+}
+
+static void
+ptp_adjust_freq(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns;
+	double adj_freq;
+	enum servo_state state = SERVO_UNLOCKED;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	ptp_data->master_offset = t2_ns - t1_ns - ptp_data->path_delay;
+	if (!ptp_data->path_delay)
+		return;
+
+	adj_freq = pi_sample(ptp_data->servo, ptp_data->master_offset, t2_ns,
+		     &state);
+
+	switch (state) {
+	case SERVO_UNLOCKED:
+		break;
+	case SERVO_JUMP:
+		ptp_data->servo->first_update = 0;
+		rte_eth_timesync_adjust_freq(ptp_data->portid,
+						-(long)(adj_freq * 65.536));
+		rte_eth_timesync_adjust_time(ptp_data->portid,
+					     -ptp_data->master_offset);
+		break;
+	case SERVO_LOCKED:
+		ptp_data->servo->first_update = 0;
+		rte_eth_timesync_adjust_freq(ptp_data->portid,
+					     -(long)(adj_freq * 65.536));
+		break;
+	}
+}
+
+static void
+send_delay_request(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	struct rte_ether_hdr *eth_hdr;
+	struct rte_ether_addr eth_addr;
+	struct ptp_header *ptp_hdr;
+	struct clock_id *client_clkid;
+	struct delay_req_msg *req_msg;
+	struct rte_mbuf *created_pkt;
+	struct rte_ether_addr eth_multicast = ether_multicast;
+	size_t pkt_size;
+	int wait_us;
+	struct rte_mbuf *m = ptp_data->m;
+	int ret;
+
+	eth_hdr = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
+	ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
+			+ sizeof(struct rte_ether_hdr));
 
 	if (ptp_data->seqID_FOLLOWUP == ptp_data->seqID_SYNC) {
 		ret = rte_eth_macaddr_get(ptp_data->portid, &eth_addr);
@@ -488,11 +662,10 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 		ptp_data->tstamp3.tv_sec = 0;
 
 		/* Wait at least 1 us to read TX timestamp. */
-		while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
-				&ptp_data->tstamp3) < 0) && (wait_us < 1000)) {
+		do {
 			rte_delay_us(1);
-			wait_us++;
-		}
+		} while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
+				&ptp_data->tstamp3) < 0) && (wait_us++ < 10));
 	}
 }
 
@@ -529,6 +702,25 @@ update_kernel_time(void)
 
 }
 
+static void
+clock_path_delay(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns, t3_ns, t4_ns;
+	int64_t pd, diff;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	t3_ns = timespec64_to_ns(&ptp_data->tstamp3);
+	t4_ns = timespec64_to_ns(&ptp_data->tstamp4);
+
+	pd = (t2_ns - t3_ns) + (t4_ns - t1_ns);
+	diff = t3_ns - t2_ns;
+	if (diff <= INT32_MAX && diff >= INT32_MIN)
+		ptp_data->path_delay = pd / 2;
+	else
+		ptp_data->path_delay = 0;
+}
+
 /*
  * Parse the DELAY_RESP message.
  */
@@ -541,7 +733,7 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 	uint16_t seq_id;
 
 	ptp_msg = rte_pktmbuf_mtod_offset(m, struct ptp_message *,
-					  sizeof(struct rte_ether_hdr));
+					sizeof(struct rte_ether_hdr));
 	seq_id = rte_be_to_cpu_16(ptp_msg->delay_resp.hdr.seq_id);
 	if (memcmp(&ptp_data->client_clock_id,
 		   &ptp_msg->delay_resp.req_port_id.clock_id,
@@ -553,11 +745,15 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 				((uint64_t)ntohl(rx_tstamp->sec_lsb)) |
 				(((uint64_t)ntohs(rx_tstamp->sec_msb)) << 32);
 
-			/* Evaluate the delta for adjustment. */
-			ptp_data->delta = delta_eval(ptp_data);
+			if (mode == MODE_PI) {
+				clock_path_delay(ptp_data);
+			} else {
+				/* Evaluate the delta for adjustment. */
+				ptp_data->delta = delta_eval(ptp_data);
 
-			rte_eth_timesync_adjust_time(ptp_data->portid,
-						     ptp_data->delta);
+				rte_eth_timesync_adjust_time(ptp_data->portid,
+								ptp_data->delta);
+			}
 
 			ptp_data->current_ptp_port = ptp_data->portid;
 
@@ -597,6 +793,9 @@ parse_ptp_frames(uint16_t portid, struct rte_mbuf *m) {
 			break;
 		case FOLLOW_UP:
 			parse_fup(&ptp_data);
+			if (mode == MODE_PI)
+				ptp_adjust_freq(&ptp_data);
+			send_delay_request(&ptp_data);
 			break;
 		case DELAY_RESP:
 			parse_drsp(&ptp_data);
@@ -688,6 +887,18 @@ parse_ptp_kernel(const char *param)
 	return 1;
 }
 
+static void
+servo_init(struct pi_servo *servo)
+{
+	memset(servo, 0x00, sizeof(*servo));
+
+	servo->drift = 100000000;
+	servo->last_freq = 100000000;
+	servo->count = 0;
+	servo->max_frequency = 100000000;
+	servo->step_threshold = 0.1 * NSEC_PER_SEC;
+}
+
 /* Parse the commandline arguments. */
 static int
 ptp_parse_args(int argc, char **argv)
@@ -696,7 +907,10 @@ ptp_parse_args(int argc, char **argv)
 	char **argvopt;
 	int option_index;
 	char *prgname = argv[0];
-	static struct option lgopts[] = { {NULL, 0, 0, 0} };
+	static struct option lgopts[] = {
+		{"controller", 1, 0, 0},
+		{NULL, 0, 0, 0}
+	};
 
 	argvopt = argv;
 
@@ -724,6 +938,11 @@ ptp_parse_args(int argc, char **argv)
 
 			ptp_data.kernel_time_set = ret;
 			break;
+		case 0:
+			if (!strcmp(lgopts[option_index].name, "controller"))
+				if (!strcmp(optarg, "pi"))
+					mode = MODE_PI;
+			break;
 
 		default:
 			print_usage(prgname);
@@ -778,6 +997,14 @@ main(int argc, char *argv[])
 		rte_exit(EXIT_FAILURE, "Error with PTP initialization\n");
 	/* >8 End of parsing specific arguments. */
 
+	if (mode == MODE_PI) {
+		ptp_data.servo = malloc(sizeof(*(ptp_data.servo)));
+		if (!ptp_data.servo)
+			rte_exit(EXIT_FAILURE, "no memory for servo\n");
+
+		servo_init(ptp_data.servo);
+	}
+
 	/* Check that there is an even number of ports to send/receive on. */
 	nb_ports = rte_eth_dev_count_avail();
 
@@ -831,6 +1058,9 @@ main(int argc, char *argv[])
 		rte_eth_dev_close(portid);
 	}
 
+	if (mode == MODE_PI)
+		free(ptp_data.servo);
+
 	/* clean up the EAL */
 	rte_eal_cleanup();
 
-- 
2.25.1

RE: [PATCH 3/3] examples/ptpclient: add frequency adjustment support

[Su, Simei]

Hi Ferruh,

> -----Original Message-----
> From: Ferruh Yigit <ferruh.yigit@amd.com>
> Sent: Monday, September 18, 2023 10:55 PM
> To: Su, Simei <simei.su@intel.com>; thomas@monjalon.net;
> andrew.rybchenko@oktetlabs.ru; Rybalchenko, Kirill
> <kirill.rybalchenko@intel.com>; Zhang, Qi Z <qi.z.zhang@intel.com>
> Cc: dev@dpdk.org; Wu, Wenjun1 <wenjun1.wu@intel.com>
> Subject: Re: [PATCH 3/3] examples/ptpclient: add frequency adjustment
> support
> 
> On 8/9/2023 6:07 AM, Simei Su wrote:
> > This patch applys PI servo algorithm to leverage frequency adjustment
> > API to improve PTP timesync accuracy.
> >
> > The command for starting ptpclient with PI algorithm is:
> > ./build/examples/dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p
> > 0x1 --controller=pi
> >
> 
> Is there a sample application documentation to document new command line
> argument and document logic to select which API to use, perhaps document
> what is 'pi servo' algorithm is?
> 
> If there is no documentation, what do you think adding one?

We will try to give more explanations and related doc update in the next version.

Thanks,
Simei

Re: [PATCH 3/3] examples/ptpclient: add frequency adjustment support

[Ferruh Yigit]

On 8/9/2023 6:07 AM, Simei Su wrote:
> This patch applys PI servo algorithm to leverage frequency adjustment
> API to improve PTP timesync accuracy.
> 
> The command for starting ptpclient with PI algorithm is:
> ./build/examples/dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p 0x1
> --controller=pi
> 

Is there a sample application documentation to document new command line
argument and document logic to select which API to use, perhaps document
what is 'pi servo' algorithm is?

If there is no documentation, what do you think adding one?

[PATCH 3/3] examples/ptpclient: add frequency adjustment support

[Simei Su]

This patch applys PI servo algorithm to leverage frequency adjustment
API to improve PTP timesync accuracy.

The command for starting ptpclient with PI algorithm is:
./build/examples/dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p 0x1
--controller=pi

Signed-off-by: Simei Su <simei.su@intel.com>
Signed-off-by: Wenjun Wu <wenjun1.wu@intel.com>
---
 examples/ptpclient/ptpclient.c | 178 +++++++++++++++++++++++++++++++++++++----
 1 file changed, 161 insertions(+), 17 deletions(-)

diff --git a/examples/ptpclient/ptpclient.c b/examples/ptpclient/ptpclient.c
index 74a1bf5..55b63be 100644
--- a/examples/ptpclient/ptpclient.c
+++ b/examples/ptpclient/ptpclient.c
@@ -43,6 +43,28 @@
 #define KERNEL_TIME_ADJUST_LIMIT  20000
 #define PTP_PROTOCOL             0x88F7
 
+#define KP 0.7
+#define KI 0.3
+
+enum servo_state {
+	SERVO_UNLOCKED,
+	SERVO_JUMP,
+	SERVO_LOCKED,
+};
+
+struct pi_servo {
+	double offset[2];
+	double local[2];
+	double drift;
+	int count;
+};
+
+enum controller_mode {
+	MODE_NONE,
+	MODE_PI,
+	MAX_ALL
+} mode;
+
 struct rte_mempool *mbuf_pool;
 uint32_t ptp_enabled_port_mask;
 uint8_t ptp_enabled_port_nb;
@@ -132,6 +154,9 @@ struct ptpv2_data_slave_ordinary {
 	uint8_t ptpset;
 	uint8_t kernel_time_set;
 	uint16_t current_ptp_port;
+	int64_t master_offset;
+	int64_t path_delay;
+	struct pi_servo *servo;
 };
 
 static struct ptpv2_data_slave_ordinary ptp_data;
@@ -290,36 +315,44 @@ print_clock_info(struct ptpv2_data_slave_ordinary *ptp_data)
 			ptp_data->tstamp3.tv_sec,
 			(ptp_data->tstamp3.tv_nsec));
 
-	printf("\nT4 - Master Clock.  %lds %ldns ",
+	printf("\nT4 - Master Clock.  %lds %ldns\n",
 			ptp_data->tstamp4.tv_sec,
 			(ptp_data->tstamp4.tv_nsec));
 
-	printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
+	if (mode == MODE_NONE) {
+		printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
 			ptp_data->delta);
 
-	clock_gettime(CLOCK_REALTIME, &sys_time);
-	rte_eth_timesync_read_time(ptp_data->current_ptp_port, &net_time);
+		clock_gettime(CLOCK_REALTIME, &sys_time);
+		rte_eth_timesync_read_time(ptp_data->current_ptp_port,
+					   &net_time);
 
-	time_t ts = net_time.tv_sec;
+		time_t ts = net_time.tv_sec;
 
-	printf("\n\nComparison between Linux kernel Time and PTP:");
+		printf("\n\nComparison between Linux kernel Time and PTP:");
 
-	printf("\nCurrent PTP Time: %.24s %.9ld ns",
+		printf("\nCurrent PTP Time: %.24s %.9ld ns",
 			ctime(&ts), net_time.tv_nsec);
 
-	nsec = (int64_t)timespec64_to_ns(&net_time) -
+		nsec = (int64_t)timespec64_to_ns(&net_time) -
 			(int64_t)timespec64_to_ns(&sys_time);
-	ptp_data->new_adj = ns_to_timeval(nsec);
+		ptp_data->new_adj = ns_to_timeval(nsec);
+
+		gettimeofday(&ptp_data->new_adj, NULL);
 
-	gettimeofday(&ptp_data->new_adj, NULL);
+		time_t tp = ptp_data->new_adj.tv_sec;
 
-	time_t tp = ptp_data->new_adj.tv_sec;
+		printf("\nCurrent SYS Time: %.24s %.6ld ns",
+			ctime(&tp), ptp_data->new_adj.tv_usec);
 
-	printf("\nCurrent SYS Time: %.24s %.6ld ns",
-				ctime(&tp), ptp_data->new_adj.tv_usec);
+		printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
+			nsec);
+	}
 
-	printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
-				nsec);
+	if (mode == MODE_PI) {
+		printf("path delay: %"PRId64"ns\n", ptp_data->path_delay);
+		printf("master offset: %"PRId64"ns\n", ptp_data->master_offset);
+	}
 
 	printf("[Ctrl+C to quit]\n");
 
@@ -405,6 +438,76 @@ parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 		(((uint64_t)ntohs(origin_tstamp->sec_msb)) << 32);
 }
 
+static double
+pi_sample(struct pi_servo *s, double offset, double local_ts,
+	  enum servo_state *state)
+{
+	double ppb = 0.0;
+
+	switch (s->count) {
+	case 0:
+		s->offset[0] = offset;
+		s->local[0] = local_ts;
+		*state = SERVO_UNLOCKED;
+		s->count = 1;
+		break;
+	case 1:
+		s->offset[1] = offset;
+		s->local[1] = local_ts;
+		*state = SERVO_UNLOCKED;
+		s->count = 2;
+		break;
+	case 2:
+		s->drift += (s->offset[1] - s->offset[0]) /
+			(s->local[1] - s->local[0]);
+		*state = SERVO_UNLOCKED;
+		s->count = 3;
+		break;
+	case 3:
+		*state = SERVO_JUMP;
+		s->count = 4;
+		break;
+	case 4:
+		s->drift += KI * offset;
+		ppb = KP * offset + s->drift;
+		*state = SERVO_LOCKED;
+		break;
+	}
+
+	return ppb;
+}
+
+static void
+ptp_adjust_freq(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns;
+	double adj_freq;
+	enum servo_state state = SERVO_UNLOCKED;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	ptp_data->master_offset = t2_ns - t1_ns - ptp_data->path_delay;
+	if (!ptp_data->path_delay)
+		return;
+
+	adj_freq = pi_sample(ptp_data->servo, ptp_data->master_offset,
+			     t2_ns, &state);
+	switch (state) {
+	case SERVO_UNLOCKED:
+		break;
+	case SERVO_JUMP:
+		rte_eth_timesync_adjust_time(ptp_data->portid,
+					     -ptp_data->master_offset);
+		t1_ns = 0;
+		t2_ns = 0;
+		break;
+	case SERVO_LOCKED:
+		rte_eth_timesync_adjust_fine(ptp_data->portid,
+					     -(long)(adj_freq * 65.536));
+		break;
+	}
+}
+
 static void
 send_delay_request(struct ptpv2_data_slave_ordinary *ptp_data)
 {
@@ -536,6 +639,21 @@ update_kernel_time(void)
 
 }
 
+static void
+clock_path_delay(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	uint64_t t1_ns, t2_ns, t3_ns, t4_ns;
+	int64_t pd;
+
+	t1_ns = timespec64_to_ns(&ptp_data->tstamp1);
+	t2_ns = timespec64_to_ns(&ptp_data->tstamp2);
+	t3_ns = timespec64_to_ns(&ptp_data->tstamp3);
+	t4_ns = timespec64_to_ns(&ptp_data->tstamp4);
+
+	pd = (t2_ns - t3_ns) + (t4_ns - t1_ns);
+	ptp_data->path_delay = pd / 2;
+}
+
 /*
  * Parse the DELAY_RESP message.
  */
@@ -560,6 +678,9 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 				((uint64_t)ntohl(rx_tstamp->sec_lsb)) |
 				(((uint64_t)ntohs(rx_tstamp->sec_msb)) << 32);
 
+			if (mode == MODE_PI)
+				clock_path_delay(ptp_data);
+
 			ptp_data->current_ptp_port = ptp_data->portid;
 
 			/* Update kernel time if enabled in app parameters. */
@@ -608,11 +729,14 @@ parse_ptp_frames(uint16_t portid, struct rte_mbuf *m) {
 			break;
 		case FOLLOW_UP:
 			parse_fup(&ptp_data);
+			if (mode == MODE_PI)
+				ptp_adjust_freq(&ptp_data);
 			send_delay_request(&ptp_data);
 			break;
 		case DELAY_RESP:
 			parse_drsp(&ptp_data);
-			ptp_adjust_time(&ptp_data);
+			if (mode == MODE_NONE)
+				ptp_adjust_time(&ptp_data);
 			print_clock_info(&ptp_data);
 			break;
 		default:
@@ -709,7 +833,10 @@ ptp_parse_args(int argc, char **argv)
 	char **argvopt;
 	int option_index;
 	char *prgname = argv[0];
-	static struct option lgopts[] = { {NULL, 0, 0, 0} };
+	static struct option lgopts[] = {
+		{"controller", 1, 0, 0},
+		{NULL, 0, 0, 0}
+	};
 
 	argvopt = argv;
 
@@ -737,6 +864,11 @@ ptp_parse_args(int argc, char **argv)
 
 			ptp_data.kernel_time_set = ret;
 			break;
+		case 0:
+			if (!strcmp(lgopts[option_index].name, "controller"))
+				if (!strcmp(optarg, "pi"))
+					mode = MODE_PI;
+			break;
 
 		default:
 			print_usage(prgname);
@@ -780,6 +912,15 @@ main(int argc, char *argv[])
 		rte_exit(EXIT_FAILURE, "Error with PTP initialization\n");
 	/* >8 End of parsing specific arguments. */
 
+	if (mode == MODE_PI) {
+		ptp_data.servo = malloc(sizeof(*(ptp_data.servo)));
+		if (!ptp_data.servo)
+			rte_exit(EXIT_FAILURE, "no memory for servo\n");
+
+		ptp_data.servo->drift = 0;
+		ptp_data.servo->count = 0;
+	}
+
 	/* Check that there is an even number of ports to send/receive on. */
 	nb_ports = rte_eth_dev_count_avail();
 
@@ -819,6 +960,9 @@ main(int argc, char *argv[])
 	/* Call lcore_main on the main core only. */
 	lcore_main();
 
+	if (mode == MODE_PI)
+		free(ptp_data.servo);
+
 	/* clean up the EAL */
 	rte_eal_cleanup();
 
-- 
2.9.5