> -----Original Message-----
> From: Ye, MingjinX <mingjinx...@intel.com>
> Sent: Tuesday 15 October 2024 09:22
> To: dev@dpdk.org
> Cc: Ye, MingjinX <mingjinx...@intel.com>; Su, Simei <simei...@intel.com>;
> Wenjun Wu <wenjun1...@intel.com>; Rybalchenko, Kirill
> <kirill.rybalche...@intel.com>
> Subject: [PATCH v7] examples/ptpclient: add frequency adjustment
> 
> 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>
Acked-by: Kirill Rybalchenko <kirill.rybalche...@intel.com>

> ---
> v2: Add doc.
> ---
> v3: Enable The servo controller needs to support frequency adjustment.
> ---
> v2: Enable servo controller, PMD must
>                               support ‘rte_eth_timesync_adjust_freq()’
> API.
> ---
>  doc/guides/sample_app_ug/ptpclient.rst |  15 +-
>  examples/ptpclient/ptpclient.c         | 297 +++++++++++++++++++++++--
>  2 files changed, 289 insertions(+), 23 deletions(-)
> 
> diff --git a/doc/guides/sample_app_ug/ptpclient.rst
> b/doc/guides/sample_app_ug/ptpclient.rst
> index d47e942738..014d5f477c 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,14 @@ 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". Note that the PMD needs to support
> +            the ``rte_eth_timesync_adjust_freq()`` API to enable the servo
> controller.
> +
> +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..e8f6f95f18 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;
> @@ -262,6 +294,19 @@ port_init(uint16_t port, struct rte_mempool
> *mbuf_pool)
>               return retval;
>       }
> 
> +     /*
> +      * If the clock servo controller is enabled, the PMD must support
> +      * adjustment of the clock frequency.
> +      */
> +     if (mode != MODE_NONE) {
> +             retval = rte_eth_timesync_adjust_freq(port, 0);
> +             if (retval == -ENOTSUP) {
> +                     printf("The servo controller cannot work on devices
> that"
> +                                     " do not support frequency
> adjustment.\n");
> +                     return retval;
> +             }
> +     }
> +
>       return 0;
>  }
> 
> @@ -293,36 +338,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 +582,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 +749,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 +853,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 +891,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 +933,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 +957,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 +1022,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 +1083,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

Reply via email to