From patchwork Mon May 22 13:23:30 2023 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Simei Su X-Patchwork-Id: 127167 X-Patchwork-Delegate: ferruh.yigit@amd.com Return-Path: X-Original-To: patchwork@inbox.dpdk.org Delivered-To: patchwork@inbox.dpdk.org Received: from mails.dpdk.org (mails.dpdk.org [217.70.189.124]) by inbox.dpdk.org (Postfix) with ESMTP id 207C942B71; Mon, 22 May 2023 15:15:26 +0200 (CEST) Received: from mails.dpdk.org (localhost [127.0.0.1]) by mails.dpdk.org (Postfix) with ESMTP id 90DC042670; Mon, 22 May 2023 15:15:24 +0200 (CEST) Received: from mga02.intel.com (mga02.intel.com [134.134.136.20]) by mails.dpdk.org (Postfix) with ESMTP id A8E56410F2 for ; Mon, 22 May 2023 15:15:23 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=intel.com; i=@intel.com; q=dns/txt; s=Intel; t=1684761323; x=1716297323; h=from:to:cc:subject:date:message-id:in-reply-to: references; bh=XPbQq40HporXpWKxkHfhQ6M5HAx3UtNEPXYJqjUgXlM=; b=Dfjt6nZVX1JjyFjTg+ixs/FkVgMHbpoI4HoQBFdS5nIxvjww9GtBCUz4 WtlmW1if06iZ5L1nVwZ8rewSxSuAvTqfHHLT2Xjgk9VgGLVqKgk2XUiyu yI+EvaoddebrmcEXXWFIpw79MDpyQDDL23Mzj4NafTQST8+URWZvaDka4 SP6kovUnCZLYmM2KOi8p+qad72fW/tYe5wnrpzUpxvWjCvfsW2E8k5PiZ uR45FyoNZwuwJZOM87uJ2A9EWj3me7TrmqNln7dHk/awATyZ8azXOe32k B6UwRyvlILVGsMyVRzEu4YyC6CyjfyF/hMIqmfjPcXuz99AwXHWP1aiB5 w==; X-IronPort-AV: E=McAfee;i="6600,9927,10717"; a="342369339" X-IronPort-AV: E=Sophos;i="6.00,184,1681196400"; d="scan'208";a="342369339" Received: from fmsmga006.fm.intel.com ([10.253.24.20]) by orsmga101.jf.intel.com with ESMTP/TLS/ECDHE-RSA-AES256-GCM-SHA384; 22 May 2023 06:15:23 -0700 X-ExtLoop1: 1 X-IronPort-AV: E=McAfee;i="6600,9927,10717"; a="950085689" X-IronPort-AV: E=Sophos;i="6.00,184,1681196400"; d="scan'208";a="950085689" Received: from unknown (HELO npg-dpdk-simeisu-cvl-119d218.sh.intel.com) ([10.67.119.231]) by fmsmga006.fm.intel.com with ESMTP; 22 May 2023 06:15:21 -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 v3 1/3] ethdev: add frequency adjustment API Date: Mon, 22 May 2023 21:23:30 +0800 Message-Id: <20230522132332.102030-2-simei.su@intel.com> X-Mailer: git-send-email 2.9.5 In-Reply-To: <20230522132332.102030-1-simei.su@intel.com> References: <20230403092248.81551-1-simei.su@intel.com> <20230522132332.102030-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 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 Signed-off-by: Wenjun Wu --- 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 2c9d615..b6fce64 100644 --- a/lib/ethdev/ethdev_driver.h +++ b/lib/ethdev/ethdev_driver.h @@ -633,6 +633,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); @@ -1344,6 +1347,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 3dc7d02..1b10aab 100644 --- a/lib/ethdev/ethdev_trace.h +++ b/lib/ethdev/ethdev_trace.h @@ -2196,6 +2196,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 61010ca..b7ac391 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 4d03255..17be7c8 100644 --- a/lib/ethdev/rte_ethdev.c +++ b/lib/ethdev/rte_ethdev.c @@ -6017,6 +6017,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 99fe9e2..a743d83 100644 --- a/lib/ethdev/rte_ethdev.h +++ b/lib/ethdev/rte_ethdev.h @@ -5102,6 +5102,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 From patchwork Mon May 22 13:23:31 2023 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Simei Su X-Patchwork-Id: 127168 X-Patchwork-Delegate: andrew.rybchenko@oktetlabs.ru Return-Path: X-Original-To: patchwork@inbox.dpdk.org Delivered-To: patchwork@inbox.dpdk.org Received: from mails.dpdk.org (mails.dpdk.org [217.70.189.124]) by inbox.dpdk.org (Postfix) with ESMTP id 29FA042B71; Mon, 22 May 2023 15:15:38 +0200 (CEST) Received: from mails.dpdk.org (localhost [127.0.0.1]) by mails.dpdk.org (Postfix) with ESMTP id E7F3D42D3B; Mon, 22 May 2023 15:15:28 +0200 (CEST) Received: from mga02.intel.com (mga02.intel.com [134.134.136.20]) by mails.dpdk.org (Postfix) with ESMTP id 62CC342D33 for ; Mon, 22 May 2023 15:15:26 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=intel.com; i=@intel.com; q=dns/txt; s=Intel; t=1684761326; x=1716297326; h=from:to:cc:subject:date:message-id:in-reply-to: references; bh=HDq555mK4EuzjhEPRoDom8uNW9fLrYn0Z9z8SIiAZ6k=; b=N7+SHduyncjf00EWysMOhsks94JtqS8fbTTTFEUbGu19AlyhV+VZGZvn BoBy6Jv0fSkEB2o8kfanD6oIzmpMCjGaad+VrRx1N/W8UE7AtMneuHzv0 BFOHGkjUHRw2Szi/pc2OjFMKqDVXFZ3G06h816WslAY/ICez+LjWjGxt2 r8mHWwZ9K8dE7KnOAgan85TVJ8sGssrF3ZLmqsOLoVL5t/loIRqjsGj+Y bHL8FzPw6kLZpHfnItxXEF8XmRmrPMIudjAj/kleAVYc/6788qXfsg8Yl DtXLS21qqtdks9nXpSVIeJVgAzoZM6S6tX8KR8dWR6LOqgaUJ4v7frHKw w==; X-IronPort-AV: E=McAfee;i="6600,9927,10717"; a="342369358" X-IronPort-AV: E=Sophos;i="6.00,184,1681196400"; d="scan'208";a="342369358" Received: from fmsmga006.fm.intel.com ([10.253.24.20]) by orsmga101.jf.intel.com with ESMTP/TLS/ECDHE-RSA-AES256-GCM-SHA384; 22 May 2023 06:15:26 -0700 X-ExtLoop1: 1 X-IronPort-AV: E=McAfee;i="6600,9927,10717"; a="950085717" X-IronPort-AV: E=Sophos;i="6.00,184,1681196400"; d="scan'208";a="950085717" Received: from unknown (HELO npg-dpdk-simeisu-cvl-119d218.sh.intel.com) ([10.67.119.231]) by fmsmga006.fm.intel.com with ESMTP; 22 May 2023 06:15:23 -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 v3 2/3] examples/ptpclient: refine application Date: Mon, 22 May 2023 21:23:31 +0800 Message-Id: <20230522132332.102030-3-simei.su@intel.com> X-Mailer: git-send-email 2.9.5 In-Reply-To: <20230522132332.102030-1-simei.su@intel.com> References: <20230403092248.81551-1-simei.su@intel.com> <20230522132332.102030-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 reworks code to split delay request message parsing from follow up message parsing which doesn't break original logic. Signed-off-by: Simei Su Signed-off-by: Wenjun Wu --- 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, ð_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: From patchwork Mon May 22 13:23:32 2023 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Simei Su X-Patchwork-Id: 127169 X-Patchwork-Delegate: andrew.rybchenko@oktetlabs.ru Return-Path: X-Original-To: patchwork@inbox.dpdk.org Delivered-To: patchwork@inbox.dpdk.org Received: from mails.dpdk.org (mails.dpdk.org [217.70.189.124]) by inbox.dpdk.org (Postfix) with ESMTP id 5A31842B71; Mon, 22 May 2023 15:15:44 +0200 (CEST) Received: from mails.dpdk.org (localhost [127.0.0.1]) by mails.dpdk.org (Postfix) with ESMTP id 0EB4242D41; Mon, 22 May 2023 15:15:32 +0200 (CEST) Received: from mga02.intel.com (mga02.intel.com [134.134.136.20]) by mails.dpdk.org (Postfix) with ESMTP id 984E442D3E for ; Mon, 22 May 2023 15:15:29 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=intel.com; i=@intel.com; q=dns/txt; s=Intel; t=1684761329; x=1716297329; h=from:to:cc:subject:date:message-id:in-reply-to: references; bh=KRlg7pr+RoaXhsnmNvs/0BZvHyNQERlkHZx41naT2/A=; b=TI/An6KTaIYp1WTs5vlj6oKAWrsmncCV1ocPHxWxOTLvkQVArSW0B4OZ 0mo11rC4lGIhbXPvzGG/rEk023u/O5PcWIi3RaxdfAf7MbzN2FNK+C0Bq RQczKCrUwnDph60UAF2BkDwyLZeBXwyp5Fc/KGEGjhcfl+JrjnDVYH2qo MjsUPa2PVaUWOrNCQylUts2e2RTEzv37yg+YRnsrn5Hh4pU4mH0Loz2Sb RCd1VNWBY+jgy3upflg5zWuEx9dhxXt0tOpk9La7CMreP0dAzVGgvrf85 rqPHKDVS4QHVeGm8HzvFx+h7RKoL3kJiSCgRYMBwsUwAmkTpH2ZMGhkNg A==; X-IronPort-AV: E=McAfee;i="6600,9927,10717"; a="342369369" X-IronPort-AV: E=Sophos;i="6.00,184,1681196400"; d="scan'208";a="342369369" Received: from fmsmga006.fm.intel.com ([10.253.24.20]) by orsmga101.jf.intel.com with ESMTP/TLS/ECDHE-RSA-AES256-GCM-SHA384; 22 May 2023 06:15:29 -0700 X-ExtLoop1: 1 X-IronPort-AV: E=McAfee;i="6600,9927,10717"; a="950085748" X-IronPort-AV: E=Sophos;i="6.00,184,1681196400"; d="scan'208";a="950085748" Received: from unknown (HELO npg-dpdk-simeisu-cvl-119d218.sh.intel.com) ([10.67.119.231]) by fmsmga006.fm.intel.com with ESMTP; 22 May 2023 06:15:26 -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 v3 3/3] examples/ptpclient: add frequency adjustment support Date: Mon, 22 May 2023 21:23:32 +0800 Message-Id: <20230522132332.102030-4-simei.su@intel.com> X-Mailer: git-send-email 2.9.5 In-Reply-To: <20230522132332.102030-1-simei.su@intel.com> References: <20230403092248.81551-1-simei.su@intel.com> <20230522132332.102030-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 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 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..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();