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

[PATCH v2 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 ++
 doc/guides/rel_notes/release_24_11.rst |  30 +--
 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                |  20 ++
 lib/ethdev/version.map                 |   3 +
 13 files changed, 482 insertions(+), 108 deletions(-)

-- 
2.25.1

[PATCH v2 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>
---
v2: rte_eth_timesync_adjust_freq marked as experimental
---
 doc/guides/nics/features.rst           |  4 +++-
 doc/guides/rel_notes/release_24_11.rst | 30 +++-----------------------
 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                | 20 +++++++++++++++++
 lib/ethdev/version.map                 |  3 +++
 8 files changed, 64 insertions(+), 28 deletions(-)

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/doc/guides/rel_notes/release_24_11.rst b/doc/guides/rel_notes/release_24_11.rst
index 0ff70d9057..10ac35e5c0 100644
--- a/doc/guides/rel_notes/release_24_11.rst
+++ b/doc/guides/rel_notes/release_24_11.rst
@@ -25,35 +25,11 @@ New Features
 ------------
 
 .. This section should contain new features added in this release.
-   Sample format:
 
-   * **Add a title in the past tense with a full stop.**
+* **Added Ethernet device clock incremental rate adjustment.**
 
-     Add a short 1-2 sentence description in the past tense.
-     The description should be enough to allow someone scanning
-     the release notes to understand the new feature.
-
-     If the feature adds a lot of sub-features you can use a bullet list
-     like this:
-
-     * Added feature foo to do something.
-     * Enhanced feature bar to do something else.
-
-     Refer to the previous release notes for examples.
-
-     Suggested order in release notes items:
-     * Core libs (EAL, mempool, ring, mbuf, buses)
-     * Device abstraction libs and PMDs (ordered alphabetically by vendor name)
-       - ethdev (lib, PMDs)
-       - cryptodev (lib, PMDs)
-       - eventdev (lib, PMDs)
-       - etc
-     * Other libs
-     * Apps, Examples, Tools (if significant)
-
-     This section is a comment. Do not overwrite or remove it.
-     Also, make sure to start the actual text at the margin.
-     =======================================================
+  Added new function ``rte_eth_timesync_adjust_freq`` to
+  adjust the clock increment rate for Ethernet devices.
 
 
 Removed Items
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..c3587089b9 100644
--- a/lib/ethdev/rte_ethdev.h
+++ b/lib/ethdev/rte_ethdev.h
@@ -5297,6 +5297,26 @@ 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.
+ */
+__rte_experimental
+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 v2 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 v2 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 v2 3/3] examples/ptpclient: add frequency adjustment support

[Stephen Hemminger]

On Fri,  6 Sep 2024 05:19:39 +0000
Mingjin Ye <mingjinx.ye@intel.com> wrote:

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

Looks like missing include, build is failing

*Build Failed #1:
OS: OpenAnolis8.9-64
Target: x86_64-native-linuxapp-gcc
FAILED: examples/dpdk-ptpclient.p/ptpclient_ptpclient.c.o 
gcc -Iexamples/dpdk-ptpclient.p -Iexamples -I../examples -Iexamples/ptpclient -I../examples/ptpclient -I../examples/common -I. -I.. -Iconfig -I../config -Ilib/eal/include -I../lib/eal/include -Ilib/eal/linux/include -I../lib/eal/linux/include -Ilib/eal/x86/include -I../lib/eal/x86/include -Ilib/eal/common -I../lib/eal/common -Ilib/eal -I../lib/eal -Ilib/kvargs -I../lib/kvargs -Ilib/log -I../lib/log -Ilib/metrics -I../lib/metrics -Ilib/telemetry -I../lib/telemetry -Ilib/mempool -I../lib/mempool -Ilib/ring -I../lib/ring -Ilib/net -I../lib/net -Ilib/mbuf -I../lib/mbuf -Ilib/ethdev -I../lib/ethdev -Ilib/meter -I../lib/meter -Ilib/cmdline -I../lib/cmdline -fdiagnostics-color=always -D_FILE_OFFSET_BITS=64 -Wall -Winvalid-pch -Wextra -Werror -std=c11 -O3 -include rte_config.h -Wcast-qual -Wdeprecated -Wformat -Wformat-nonliteral -Wformat-security -Wmissing-declarations -Wmissing-prototypes -Wnested-externs -Wold-style-definition -Wpointer-arith -Wsign-compare -Wstrict-prototypes -Wundef -Wwrite-strings -Wno-packed-not-aligned -Wno-missing-field-initializers -D_GNU_SOURCE -march=native -mrtm -Wno-format-truncation -DALLOW_EXPERIMENTAL_API -MD -MQ examples/dpdk-ptpclient.p/ptpclient_ptpclient.c.o -MF examples/dpdk-ptpclient.p/ptpclient_ptpclient.c.o.d -o examples/dpdk-ptpclient.p/ptpclient_ptpclient.c.o -c ../examples/ptpclient/ptpclient.c
../examples/ptpclient/ptpclient.c: In function ‘port_init’:
../examples/ptpclient/ptpclient.c:302:12: error: implicit declaration of function ‘rte_eth_timesync_adjust_freq’; did you mean ‘rte_eth_timesync_adjust_time’? [-Werror=implicit-function-declaration]
   retval = rte_eth_timesync_adjust_freq(port, 0);
            ^~~~~~~~~~~~~~~~~~~~~~~~~~~~
            rte_eth_timesync_adjust_time
../examples/ptpclient/ptpclient.c:302:12: error: nested extern declaration of ‘rte_eth_timesync_adjust_freq’ [-Werror=nested-externs]
cc1: all warnings being treated as errors
[2975/3043] Generating examples/multi_process/simple_mp/commands_hdr with a custom command (wrapped by meson to capture output)
[2976/3043] Linking target examples/dpdk-l2fwd-keepalive
[2977/3043] Linking target examples/dpdk-l3fwd-graph
[2978/3043] Linking target examples/dpdk-l3fwd
[2979/3043] Compiling C object examples/dpdk-l3fwd-power.p/l3fwd-power_main.c.o
[2980/3043] Compiling C object examples/dpdk-mp_server.p/multi_process_client_server_mp_mp_server_main.c.o
[2981/3043] Linking target examples/dpdk-l2fwd-macsec
[2982/3043] Linking target examples/dpdk-hotplug_mp
[2983/3043] Linking target examples/dpdk-link_status_interrupt
ninja: build stopped

[PATCH v2 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 | 298 +++++++++++++++++++++++++++++----
 1 file changed, 264 insertions(+), 34 deletions(-)

diff --git a/examples/ptpclient/ptpclient.c b/examples/ptpclient/ptpclient.c
index afb61bba51..e92f7c5c8d 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.
  */
@@ -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 v2 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