From mboxrd@z Thu Jan 1 00:00:00 1970 Return-Path: Received: from mails.dpdk.org (mails.dpdk.org [217.70.189.124]) by inbox.dpdk.org (Postfix) with ESMTP id 742CE4287D; Fri, 31 Mar 2023 04:25:03 +0200 (CEST) Received: from mails.dpdk.org (localhost [127.0.0.1]) by mails.dpdk.org (Postfix) with ESMTP id 44B2842D17; Fri, 31 Mar 2023 04:24:50 +0200 (CEST) Received: from mga07.intel.com (mga07.intel.com [134.134.136.100]) by mails.dpdk.org (Postfix) with ESMTP id 06D1E42D16 for ; Fri, 31 Mar 2023 04:24:47 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=intel.com; i=@intel.com; q=dns/txt; s=Intel; t=1680229488; x=1711765488; h=from:to:cc:subject:date:message-id:in-reply-to: references; bh=xoK8X50X8DeJj9Chic+PK2Ujql1PKOcnA8eqeXPndcg=; b=i3DgbJYYLRJaz1TkwmVMx1qLn8dqP/wLZYK+iJFcpdwtSzSLZaCEJGSU DeJ5f2HLel5feGImN6wvZM6F+XAALGIXaLBFCYMPizBFbeoVn7e3j42h0 9S6W8rnAYivf8dU5rMWIdL5nwA4wBXInmFD7Kh70O0tPR26rae8cCT1W1 lX/z8RehapsIuRqGIaiDHWvXomOxwyfLmWs0MJ/RikiGtMir4mPy1iLPZ Zu6XwCpqycUBWbsq9M5CMOnrPc2IrLXxGh1SzlNZxCxA2Ayu7hq9AyYFq HIsZiONt/bHbYXOjs36osSB1Cf7q9kK/OX/2K3V+APnAJCGiVT8XBd3Df w==; X-IronPort-AV: E=McAfee;i="6600,9927,10665"; a="406339295" X-IronPort-AV: E=Sophos;i="5.98,306,1673942400"; d="scan'208";a="406339295" Received: from orsmga007.jf.intel.com ([10.7.209.58]) by orsmga105.jf.intel.com with ESMTP/TLS/ECDHE-RSA-AES256-GCM-SHA384; 30 Mar 2023 19:24:47 -0700 X-ExtLoop1: 1 X-IronPort-AV: E=McAfee;i="6600,9927,10665"; a="678419767" X-IronPort-AV: E=Sophos;i="5.98,306,1673942400"; d="scan'208";a="678419767" Received: from unknown (HELO npg-dpdk-simeisu-cvl-119d218.sh.intel.com) ([10.67.119.208]) by orsmga007.jf.intel.com with ESMTP; 30 Mar 2023 19:24:44 -0700 From: Simei Su To: thomas@monjalon.net, ferruh.yigit@amd.com, andrew.rybchenko@oktetlabs.ru, kirill.rybalchenko@intel.com, qi.z.zhang@intel.com Cc: dev@dpdk.org, wenjun1.wu@intel.com, Simei Su Subject: [RFC 4/4] examples/ptpclient: add frequency adjustment support Date: Fri, 31 Mar 2023 10:22:58 +0800 Message-Id: <20230331022258.382085-5-simei.su@intel.com> X-Mailer: git-send-email 2.9.5 In-Reply-To: <20230331022258.382085-1-simei.su@intel.com> References: <20230331022258.382085-1-simei.su@intel.com> X-BeenThere: dev@dpdk.org X-Mailman-Version: 2.1.29 Precedence: list List-Id: DPDK patches and discussions List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , Errors-To: dev-bounces@dpdk.org 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 Signed-off-by: Wenjun Wu --- 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..3d074af 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_freq(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