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

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

[Simei Su]

This patchset cover below parts:
(1)Introduce a new timesync API called "rte_eth_timesync_adjust_freq" that
   enables frequency adjustment during PTP timesync. This new API aligns with
   the kernel PTP which already supports frequency adjustment. It brings DPDK
   closer in alignment with the kernel's best practice.

(2)Refine the ptpclient application by applying a PI algorithm that leverages
   the new API to further improve timesync accuracy. These changes doesn't break
   original solution and creates a more accurate solution for DPDK-based high
   accuracy PTP. We have provided significant improvements for timesync accuracy
   on e810 and we believe these improvements will also benefit other devices.

The original command for starting ptpclient is:
./build/examples/dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p 0x1

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

[1/3] ethdev: add frequency adjustment API.
[2/3] examples/ptpclient: refine application.
[3/3] examples/ptpclient: add frequency adjustment support.

Simei Su (3):
  ethdev: add frequency adjustment API
  examples/ptpclient: refine application
  examples/ptpclient: add frequency adjustment support

 examples/ptpclient/ptpclient.c   | 222 +++++++++++++++++++++++++++++++++------
 lib/ethdev/ethdev_driver.h       |   5 +
 lib/ethdev/ethdev_trace.h        |   9 ++
 lib/ethdev/ethdev_trace_points.c |   3 +
 lib/ethdev/rte_ethdev.c          |  19 ++++
 lib/ethdev/rte_ethdev.h          |  38 +++++++
 6 files changed, 265 insertions(+), 31 deletions(-)

-- 
2.9.5

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

[Simei Su]

This patch introduces a new timesync API "rte_eth_timesync_adjust_fine"
which enables finer adjustment of the PHC clock. During PTP timesync,
"rte_eth_timesync_adjust_time" focuses on phase adjustment while
"rte_eth_timesync_adjust_fine" focuses on frequency adjustment.

This new function gets the scaled_ppm (desired frequency offset from
nominal frequency in parts per million, but with a 16 bit binary
fractional field).

Signed-off-by: Simei Su <simei.su@intel.com>
Signed-off-by: Wenjun Wu <wenjun1.wu@intel.com>
---
 lib/ethdev/ethdev_driver.h       |  5 +++++
 lib/ethdev/ethdev_trace.h        |  9 +++++++++
 lib/ethdev/ethdev_trace_points.c |  3 +++
 lib/ethdev/rte_ethdev.c          | 19 +++++++++++++++++++
 lib/ethdev/rte_ethdev.h          | 38 ++++++++++++++++++++++++++++++++++++++
 5 files changed, 74 insertions(+)

diff --git a/lib/ethdev/ethdev_driver.h b/lib/ethdev/ethdev_driver.h
index 980f837..4c39c3d 100644
--- a/lib/ethdev/ethdev_driver.h
+++ b/lib/ethdev/ethdev_driver.h
@@ -636,6 +636,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 increment rate. */
+typedef int (*eth_timesync_adjust_fine)(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);
@@ -1347,6 +1350,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 increment rate */
+	eth_timesync_adjust_fine   timesync_adjust_fine;
 	/** 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 423e712..d613eb2 100644
--- a/lib/ethdev/ethdev_trace.h
+++ b/lib/ethdev/ethdev_trace.h
@@ -2175,6 +2175,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_fine,
+	RTE_TRACE_POINT_ARGS(uint16_t port_id, int64_t scaled_ppm, int ret),
+	rte_trace_point_emit_u16(port_id);
+	rte_trace_point_emit_i64(scaled_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 91f71d8..eb539bb 100644
--- a/lib/ethdev/ethdev_trace_points.c
+++ b/lib/ethdev/ethdev_trace_points.c
@@ -406,6 +406,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_fine,
+	lib.ethdev.timesync_adjust_fine)
+
 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 0840d2b..7b75214 100644
--- a/lib/ethdev/rte_ethdev.c
+++ b/lib/ethdev/rte_ethdev.c
@@ -6109,6 +6109,25 @@ rte_eth_timesync_adjust_time(uint16_t port_id, int64_t delta)
 }
 
 int
+rte_eth_timesync_adjust_fine(uint16_t port_id, int64_t scaled_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_fine == NULL)
+		return -ENOTSUP;
+	ret = eth_err(port_id, (*dev->dev_ops->timesync_adjust_fine)(dev,
+								scaled_ppm));
+
+	rte_eth_trace_timesync_adjust_fine(port_id, scaled_ppm, ret);
+
+	return ret;
+}
+
+int
 rte_eth_timesync_read_time(uint16_t port_id, struct timespec *timestamp)
 {
 	struct rte_eth_dev *dev;
diff --git a/lib/ethdev/rte_ethdev.h b/lib/ethdev/rte_ethdev.h
index 3d44979..78fc07c 100644
--- a/lib/ethdev/rte_ethdev.h
+++ b/lib/ethdev/rte_ethdev.h
@@ -5145,6 +5145,44 @@ 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 frequency of the PHC cycle counter by the indicated amount
+ * from the base frequency.
+ *
+ * This function is used to do hardware timestamp adjustment in fine
+ * granularity. It can be used in conjunction with rte_eth_timesync_adjust_time
+ * to do more precise time control.
+ *
+ * E.g, below is a simple usage:
+ * if master offset > master offset threshold
+ *	do rte_eth_timesync_adjust_time;
+ * else
+ *	do rte_eth_timesync_adjust_fine;
+ *
+ * The user can apply a control algorithm to leverage these two APIs, one
+ * example is in dpdk-ptpclient.
+ *
+ * This API is implemented with the below basic logic:
+ *   - Determine a base frequency value
+ *   - Multiply this by the abs() of the requested adjustment, then divide by
+ *     the appropriate divisor (65536 billion).
+ *   - Add or subtract this difference from the base frequency to calculate a
+ *     new adjustment.
+ *
+ * @param port_id
+ *  The port identifier of the Ethernet device.
+ * @param scaled_ppm
+ *  Desired frequency change in scaled parts per million. Scaled parts per
+ *  million is ppm with a 16-bit binary 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_fine(uint16_t port_id, int64_t scaled_ppm);
+
+/**
  * Read the time from the timesync clock on an Ethernet device.
  *
  * This is usually used in conjunction with other Ethdev timesync functions to
-- 
2.9.5

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

[Ferruh Yigit]

On 8/9/2023 6:06 AM, Simei Su wrote:
> This patch introduces a new timesync API "rte_eth_timesync_adjust_fine"
> which enables finer adjustment of the PHC clock. During PTP timesync,
> "rte_eth_timesync_adjust_time" focuses on phase adjustment while
> "rte_eth_timesync_adjust_fine" focuses on frequency adjustment.
> 

Hi Simei,

The patch adds new API and new dev_ops, mechanics of it mostly looks
good, there is minor comment below.

But there is not much comment on it because concept may be not well
known as it is not much known to me. Can you please try to add more
details and documentations to help users and driver developers?


I have some understanding based on your comments, can you please check
if it is correct:

In PTP protocol master and client synchronize clocks periodically, and
adjust the delta with 'rte_eth_timesync_adjust_time()' API, which simply
use this delta as offset in its time function.

But new API added by this patch, 'rte_eth_timesync_adjust_fine()', adds
a new synchronization method by saying device to change its HW timer
frequency.

The name of the API says this is a finer adjustment method, I wonder
why? I don't know much about timer HW but my understanding is they are
pretty accurate, why changing frequency gives a finer adjustment? Is it
common that difference frequency of HW timers cause drift by time?

Btw, what is PHC clock? Is it "Pulse Hollow Cathode" (as chatGPT
suggests :), is there any significance of this type of clock? Is this
API valid only for this type of clock?


And for users, assuming the HW supports both methods, how should they
decide which one to use? Can we provide some guidance to them? I can see
API suggest to use 'rte_eth_timesync_adjust_fine()' when time diff
offset is less than a threshold but what is the threshold and what
happens if user only uses 'rte_eth_timesync_adjust_time()'?
Is there a more real life numbers to provide as samples, maybe can
provide them in the commit log as sample if we can't provide them in API
documentation?


> This new function gets the scaled_ppm (desired frequency offset from
> nominal frequency in parts per million, but with a 16 bit binary
> fractional field).
> 

What is "scaled parts per million"? I think I understand what parameter
does, but not sure about what is the unit here?

Also PTP client sample mentions about algorithms to use for frequency
adjustment, like PI servo algorithm next patch uses, what are these
algorithms?
Should we mention from algorithms or specifically from "PI servo
algorithm" in the API documentation?

> Signed-off-by: Simei Su <simei.su@intel.com>
> Signed-off-by: Wenjun Wu <wenjun1.wu@intel.com>
> ---
>  lib/ethdev/ethdev_driver.h       |  5 +++++
>  lib/ethdev/ethdev_trace.h        |  9 +++++++++
>  lib/ethdev/ethdev_trace_points.c |  3 +++
>  lib/ethdev/rte_ethdev.c          | 19 +++++++++++++++++++
>  lib/ethdev/rte_ethdev.h          | 38 ++++++++++++++++++++++++++++++++++++++

'version.map' also should be updated to export the API, so that DPDK
applications linked with shared DPDK library can use it.

CI already complaining about this:
https://mails.dpdk.org/archives/test-report/2023-August/435936.html


>  5 files changed, 74 insertions(+)
> 
> diff --git a/lib/ethdev/ethdev_driver.h b/lib/ethdev/ethdev_driver.h
> index 980f837..4c39c3d 100644
> --- a/lib/ethdev/ethdev_driver.h
> +++ b/lib/ethdev/ethdev_driver.h
> @@ -636,6 +636,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 increment rate. */
> +typedef int (*eth_timesync_adjust_fine)(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);
> @@ -1347,6 +1350,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 increment rate */
> +	eth_timesync_adjust_fine   timesync_adjust_fine;
>  	/** 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 423e712..d613eb2 100644
> --- a/lib/ethdev/ethdev_trace.h
> +++ b/lib/ethdev/ethdev_trace.h
> @@ -2175,6 +2175,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_fine,
> +	RTE_TRACE_POINT_ARGS(uint16_t port_id, int64_t scaled_ppm, int ret),
> +	rte_trace_point_emit_u16(port_id);
> +	rte_trace_point_emit_i64(scaled_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 91f71d8..eb539bb 100644
> --- a/lib/ethdev/ethdev_trace_points.c
> +++ b/lib/ethdev/ethdev_trace_points.c
> @@ -406,6 +406,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_fine,
> +	lib.ethdev.timesync_adjust_fine)
> +
>  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 0840d2b..7b75214 100644
> --- a/lib/ethdev/rte_ethdev.c
> +++ b/lib/ethdev/rte_ethdev.c
> @@ -6109,6 +6109,25 @@ rte_eth_timesync_adjust_time(uint16_t port_id, int64_t delta)
>  }
>  
>  int
> +rte_eth_timesync_adjust_fine(uint16_t port_id, int64_t scaled_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_fine == NULL)
> +		return -ENOTSUP;
> +	ret = eth_err(port_id, (*dev->dev_ops->timesync_adjust_fine)(dev,
> +								scaled_ppm));
> +
> +	rte_eth_trace_timesync_adjust_fine(port_id, scaled_ppm, ret);
> +
> +	return ret;
> +}
> +
> +int
>  rte_eth_timesync_read_time(uint16_t port_id, struct timespec *timestamp)
>  {
>  	struct rte_eth_dev *dev;
> diff --git a/lib/ethdev/rte_ethdev.h b/lib/ethdev/rte_ethdev.h
> index 3d44979..78fc07c 100644
> --- a/lib/ethdev/rte_ethdev.h
> +++ b/lib/ethdev/rte_ethdev.h
> @@ -5145,6 +5145,44 @@ 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 frequency of the PHC cycle counter by the indicated amount
> + * from the base frequency.
> + *
> + * This function is used to do hardware timestamp adjustment in fine
> + * granularity. It can be used in conjunction with rte_eth_timesync_adjust_time
> + * to do more precise time control.
> + *
> + * E.g, below is a simple usage:
> + * if master offset > master offset threshold
> + *	do rte_eth_timesync_adjust_time;
> + * else
> + *	do rte_eth_timesync_adjust_fine;
> + *
> + * The user can apply a control algorithm to leverage these two APIs, one
> + * example is in dpdk-ptpclient.
> + *
> + * This API is implemented with the below basic logic:
> + *   - Determine a base frequency value
> + *   - Multiply this by the abs() of the requested adjustment, then divide by
> + *     the appropriate divisor (65536 billion).
> + *   - Add or subtract this difference from the base frequency to calculate a
> + *     new adjustment.
> + *
> + * @param port_id
> + *  The port identifier of the Ethernet device.
> + * @param scaled_ppm
> + *  Desired frequency change in scaled parts per million. Scaled parts per
> + *  million is ppm with a 16-bit binary 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_fine(uint16_t port_id, int64_t scaled_ppm);
> +
> +/**
>   * Read the time from the timesync clock on an Ethernet device.
>   *
>   * This is usually used in conjunction with other Ethdev timesync functions to

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

[Su, Simei]

Hi Ferruh,

> -----Original Message-----
> From: Ferruh Yigit <ferruh.yigit@amd.com>
> Sent: Monday, September 18, 2023 10:51 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 1/3] ethdev: add frequency adjustment API
> 
> On 8/9/2023 6:06 AM, Simei Su wrote:
> > This patch introduces a new timesync API "rte_eth_timesync_adjust_fine"
> > which enables finer adjustment of the PHC clock. During PTP timesync,
> > "rte_eth_timesync_adjust_time" focuses on phase adjustment while
> > "rte_eth_timesync_adjust_fine" focuses on frequency adjustment.
> >
> 
> Hi Simei,
> 
> The patch adds new API and new dev_ops, mechanics of it mostly looks good,
> there is minor comment below.
> 
> But there is not much comment on it because concept may be not well known
> as it is not much known to me. Can you please try to add more details and
> documentations to help users and driver developers?

OK. I will rework this patchset and add more details in the next version.

> 
> 
> I have some understanding based on your comments, can you please check if it
> is correct:
> 
> In PTP protocol master and client synchronize clocks periodically, and adjust
> the delta with 'rte_eth_timesync_adjust_time()' API, which simply use this
> delta as offset in its time function.
> 
> But new API added by this patch, 'rte_eth_timesync_adjust_fine()', adds a new
> synchronization method by saying device to change its HW timer frequency.

Yes. It's correct.

> 
> The name of the API says this is a finer adjustment method, I wonder why? I
> don't know much about timer HW but my understanding is they are pretty
> accurate, why changing frequency gives a finer adjustment? Is it common that
> difference frequency of HW timers cause drift by time?

The "fine" in 'rte_eth_timesync_adjust_fine()' means that it's adjusted in fine-granularity while
'rte_eth_timesync_adjust_time()' is in coarse-granularity adjustment.
 
We refer to API in Linux kernel.
The internal PTP Hardware Clock (PHC) interface limits the resolution for frequency adjustments to one part per billion. However, some hardware devices allow finer adjustment and making use of the increased resolution improves synchronization measurably on such devices.

This method allows finer frequency tuning by passing the scaled ppm value to PHC drivers. This value comes from user space.

> 
> Btw, what is PHC clock? Is it "Pulse Hollow Cathode" (as chatGPT suggests :), is
> there any significance of this type of clock? Is this API valid only for this type of
> clock?
> 

PHC means "PTP Hardware Clock". It's hardware clock on NIC.

> 
> And for users, assuming the HW supports both methods, how should they
> decide which one to use? Can we provide some guidance to them? I can see
> API suggest to use 'rte_eth_timesync_adjust_fine()' when time diff offset is less
> than a threshold but what is the threshold and what happens if user only uses
> 'rte_eth_timesync_adjust_time()'?
> Is there a more real life numbers to provide as samples, maybe can provide
> them in the commit log as sample if we can't provide them in API
> documentation?
> 

Theoretically, both methods should be used at different moments.
At the beginning, the offset between master and slave is large, time adjustment API should be called at one time to make the offset smaller. When the offset is less than a value(called as threshold), fine-granularity adjustment API should be always called.

In this patch, I let user choose whether using fine-granularity adjustment with "-- controller=pi" because I don't want to break current design in "ptpclient" application and compatible with current logic. For example, some PMDs don't have the new API support which can't use fine-granularity adjustment method.

> 
> > This new function gets the scaled_ppm (desired frequency offset from
> > nominal frequency in parts per million, but with a 16 bit binary
> > fractional field).
> >
> 
> What is "scaled parts per million"? I think I understand what parameter does,
> but not sure about what is the unit here?
> 
> Also PTP client sample mentions about algorithms to use for frequency
> adjustment, like PI servo algorithm next patch uses, what are these
> algorithms?
> Should we mention from algorithms or specifically from "PI servo algorithm" in
> the API documentation?

This is a classic control algorithm (Proportional-Integral algorithm).
For this algorithm, we need to continue to optimize it and will give more explanations in the next version.

Because of the further optimization for the algorithm, we may plan to upstream it in the early DPDK 24.03, not in DPDK 23.11.

> 
> > Signed-off-by: Simei Su <simei.su@intel.com>
> > Signed-off-by: Wenjun Wu <wenjun1.wu@intel.com>
> > ---
> >  lib/ethdev/ethdev_driver.h       |  5 +++++
> >  lib/ethdev/ethdev_trace.h        |  9 +++++++++
> >  lib/ethdev/ethdev_trace_points.c |  3 +++
> >  lib/ethdev/rte_ethdev.c          | 19 +++++++++++++++++++
> >  lib/ethdev/rte_ethdev.h          | 38
> ++++++++++++++++++++++++++++++++++++++
> 
> 'version.map' also should be updated to export the API, so that DPDK
> applications linked with shared DPDK library can use it.
> 
> CI already complaining about this:
> https://mails.dpdk.org/archives/test-report/2023-August/435936.html

OK. I will update 'version.map' in the next version.

Thanks,
Simei

> 
> 
> >  5 files changed, 74 insertions(+)
> >
> > diff --git a/lib/ethdev/ethdev_driver.h b/lib/ethdev/ethdev_driver.h
> > index 980f837..4c39c3d 100644
> > --- a/lib/ethdev/ethdev_driver.h
> > +++ b/lib/ethdev/ethdev_driver.h
> > @@ -636,6 +636,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 increment rate. */
> > +typedef int (*eth_timesync_adjust_fine)(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); @@ -1347,6 +1350,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 increment rate */
> > +	eth_timesync_adjust_fine   timesync_adjust_fine;
> >  	/** 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 423e712..d613eb2 100644
> > --- a/lib/ethdev/ethdev_trace.h
> > +++ b/lib/ethdev/ethdev_trace.h
> > @@ -2175,6 +2175,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_fine,
> > +	RTE_TRACE_POINT_ARGS(uint16_t port_id, int64_t scaled_ppm, int ret),
> > +	rte_trace_point_emit_u16(port_id);
> > +	rte_trace_point_emit_i64(scaled_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 91f71d8..eb539bb 100644
> > --- a/lib/ethdev/ethdev_trace_points.c
> > +++ b/lib/ethdev/ethdev_trace_points.c
> > @@ -406,6 +406,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_fine,
> > +	lib.ethdev.timesync_adjust_fine)
> > +
> >  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
> > 0840d2b..7b75214 100644
> > --- a/lib/ethdev/rte_ethdev.c
> > +++ b/lib/ethdev/rte_ethdev.c
> > @@ -6109,6 +6109,25 @@ rte_eth_timesync_adjust_time(uint16_t port_id,
> > int64_t delta)  }
> >
> >  int
> > +rte_eth_timesync_adjust_fine(uint16_t port_id, int64_t scaled_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_fine == NULL)
> > +		return -ENOTSUP;
> > +	ret = eth_err(port_id, (*dev->dev_ops->timesync_adjust_fine)(dev,
> > +								scaled_ppm));
> > +
> > +	rte_eth_trace_timesync_adjust_fine(port_id, scaled_ppm, ret);
> > +
> > +	return ret;
> > +}
> > +
> > +int
> >  rte_eth_timesync_read_time(uint16_t port_id, struct timespec
> > *timestamp)  {
> >  	struct rte_eth_dev *dev;
> > diff --git a/lib/ethdev/rte_ethdev.h b/lib/ethdev/rte_ethdev.h index
> > 3d44979..78fc07c 100644
> > --- a/lib/ethdev/rte_ethdev.h
> > +++ b/lib/ethdev/rte_ethdev.h
> > @@ -5145,6 +5145,44 @@ 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 frequency of the PHC cycle counter by the indicated
> > +amount
> > + * from the base frequency.
> > + *
> > + * This function is used to do hardware timestamp adjustment in fine
> > + * granularity. It can be used in conjunction with
> > +rte_eth_timesync_adjust_time
> > + * to do more precise time control.
> > + *
> > + * E.g, below is a simple usage:
> > + * if master offset > master offset threshold
> > + *	do rte_eth_timesync_adjust_time;
> > + * else
> > + *	do rte_eth_timesync_adjust_fine;
> > + *
> > + * The user can apply a control algorithm to leverage these two APIs,
> > +one
> > + * example is in dpdk-ptpclient.
> > + *
> > + * This API is implemented with the below basic logic:
> > + *   - Determine a base frequency value
> > + *   - Multiply this by the abs() of the requested adjustment, then divide by
> > + *     the appropriate divisor (65536 billion).
> > + *   - Add or subtract this difference from the base frequency to calculate a
> > + *     new adjustment.
> > + *
> > + * @param port_id
> > + *  The port identifier of the Ethernet device.
> > + * @param scaled_ppm
> > + *  Desired frequency change in scaled parts per million. Scaled
> > +parts per
> > + *  million is ppm with a 16-bit binary 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_fine(uint16_t port_id, int64_t
> > +scaled_ppm);
> > +
> > +/**
> >   * Read the time from the timesync clock on an Ethernet device.
> >   *
> >   * This is usually used in conjunction with other Ethdev timesync
> > functions to

[PATCH 2/3] examples/ptpclient: refine application

[Simei Su]

This patch reworks code to split delay request message parsing
from follow up message parsing which doesn't break original logic.

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

diff --git a/examples/ptpclient/ptpclient.c b/examples/ptpclient/ptpclient.c
index cdf2da6..74a1bf5 100644
--- a/examples/ptpclient/ptpclient.c
+++ b/examples/ptpclient/ptpclient.c
@@ -382,21 +382,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 = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
 			+ sizeof(struct rte_ether_hdr));
 	if (memcmp(&ptp_data->master_clock_id,
@@ -413,6 +403,26 @@ 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 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);
@@ -550,12 +560,6 @@ 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);
-
-			rte_eth_timesync_adjust_time(ptp_data->portid,
-						     ptp_data->delta);
-
 			ptp_data->current_ptp_port = ptp_data->portid;
 
 			/* Update kernel time if enabled in app parameters. */
@@ -568,6 +572,16 @@ parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 	}
 }
 
+static void
+ptp_adjust_time(struct ptpv2_data_slave_ordinary *ptp_data)
+{
+	/* Evaluate the delta for adjustment. */
+	ptp_data->delta = delta_eval(ptp_data);
+
+	rte_eth_timesync_adjust_time(ptp_data->portid,
+				     ptp_data->delta);
+}
+
 /* This function processes PTP packets, implementing slave PTP IEEE1588 L2
  * functionality.
  */
@@ -594,9 +608,11 @@ parse_ptp_frames(uint16_t portid, struct rte_mbuf *m) {
 			break;
 		case FOLLOW_UP:
 			parse_fup(&ptp_data);
+			send_delay_request(&ptp_data);
 			break;
 		case DELAY_RESP:
 			parse_drsp(&ptp_data);
+			ptp_adjust_time(&ptp_data);
 			print_clock_info(&ptp_data);
 			break;
 		default:
-- 
2.9.5

[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

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?

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

[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     |   3 +-
 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 ++
 11 files changed, 474 insertions(+), 81 deletions(-)

-- 
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 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           |   1 +
 12 files changed, 476 insertions(+), 81 deletions(-)

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