This patch adds PI servo controller to support frequency
adjustment API for IEEE1588 PTP.
For example, the command for starting ptpclient with PI controller is:
dpdk-ptpclient -a 0000:81:00.0 -c 1 -n 3 -- -T 0 -p 0x1 -c 1
Signed-off-by: Simei Su <simei...@intel.com>
Signed-off-by: Wenjun Wu <wenjun1...@intel.com>
Signed-off-by: Mingjin Ye <mingjinx...@intel.com>
---
doc/guides/sample_app_ug/ptpclient.rst | 12 +-
examples/ptpclient/ptpclient.c | 284 +++++++++++++++++++++++--
2 files changed, 274 insertions(+), 22 deletions(-)
diff --git a/doc/guides/sample_app_ug/ptpclient.rst
b/doc/guides/sample_app_ug/ptpclient.rst
index d47e942738..89fe575b5f 100644
--- a/doc/guides/sample_app_ug/ptpclient.rst
+++ b/doc/guides/sample_app_ug/ptpclient.rst
@@ -50,6 +50,10 @@ The adjustment for slave can be represented as:
If the command line parameter ``-T 1`` is used the application also
synchronizes the PTP PHC clock with the Linux kernel clock.
+If the command line parameter ``-c 1`` is used, the application will also
+use the servo of the local clock. Only one type of servo is currently
+implemented, the PI controller. Default 0 (not used).
+
Compiling the Application
-------------------------
@@ -65,7 +69,7 @@ To run the example in a ``linux`` environment:
.. code-block:: console
- ./<build_dir>/examples/dpdk-ptpclient -l 1 -n 4 -- -p 0x1 -T 0
+ ./<build_dir>/examples/dpdk-ptpclient -l 1 -n 4 -- -p 0x1 -T 0 -c 1
Refer to *DPDK Getting Started Guide* for general information on running
applications and the Environment Abstraction Layer (EAL) options.
@@ -73,7 +77,13 @@ applications and the Environment Abstraction Layer (EAL)
options.
* ``-p portmask``: Hexadecimal portmask.
* ``-T 0``: Update only the PTP slave clock.
* ``-T 1``: Update the PTP slave clock and synchronize the Linux Kernel to the
PTP clock.
+* ``-c 0``: Not used clock servo controller.
+* ``-c 1``: The clock servo PI controller is used and the log will print
information
+ about "master offset".
+Also, by adding ``-T 1`` and ``-c 1`` , the ``master offset`` value printed in
the
+log will slowly converge and eventually stabilise at the nanosecond level. The
+synchronisation accuracy is much higher compared to not using a servo
controller.
Code Explanation
----------------
diff --git a/examples/ptpclient/ptpclient.c b/examples/ptpclient/ptpclient.c
index afb61bba51..dea8d9d54a 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_NONE;
+
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");
@@ -529,6 +569,149 @@ 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;
+}
+
+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_servo(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;
+ }
+}
+
/*
* Parse the DELAY_RESP message.
*/
@@ -553,11 +736,16 @@ 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);
+ ptp_adjust_servo(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;
@@ -652,7 +840,9 @@ print_usage(const char *prgname)
printf("%s [EAL options] -- -p PORTMASK -T VALUE\n"
" -T VALUE: 0 - Disable, 1 - Enable Linux Clock"
" Synchronization (0 default)\n"
- " -p PORTMASK: hexadecimal bitmask of ports to configure\n",
+ " -p PORTMASK: hexadecimal bitmask of ports to configure\n"
+ " -c CONTROLLER: 0 - Not used, 1 - PI. The servo which is"
+ " used to synchronize the local clock. (0 default)\n",
prgname);
}
@@ -688,6 +878,36 @@ parse_ptp_kernel(const char *param)
return 1;
}
+static int
+parse_ptp_servo_mode(const char *param)
+{
+ char *end = NULL;
+ unsigned long pm;
+
+ /* Parse the hexadecimal string. */
+ pm = strtoul(param, &end, 10);
+
+ if ((param[0] == '\0') || (end == NULL) || (*end != '\0'))
+ return -1;
+
+ return pm;
+}
+
+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;
+ servo->first_step_threshold = 0.00002 * NSEC_PER_SEC;
+ servo->first_update = 1;
+}
+
/* Parse the commandline arguments. */
static int
ptp_parse_args(int argc, char **argv)
@@ -700,7 +920,7 @@ ptp_parse_args(int argc, char **argv)
argvopt = argv;
- while ((opt = getopt_long(argc, argvopt, "p:T:",
+ while ((opt = getopt_long(argc, argvopt, "p:T:c:",
lgopts, &option_index)) != EOF) {
switch (opt) {
@@ -724,6 +944,17 @@ ptp_parse_args(int argc, char **argv)
ptp_data.kernel_time_set = ret;
break;
+ case 'c':
+ ret = parse_ptp_servo_mode(optarg);
+ if (ret == 0) {
+ mode = MODE_NONE;
+ } else if (ret == 1) {
+ mode = MODE_PI;
+ } else {
+ print_usage(prgname);
+ return -1;
+ }
+ break;
default:
print_usage(prgname);
@@ -778,6 +1009,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 +1070,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
