From patchwork Thu Sep 5 09:05:45 2024 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: "Ye, MingjinX" X-Patchwork-Id: 143640 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 4153C4590C; Thu, 5 Sep 2024 11:29:35 +0200 (CEST) Received: from mails.dpdk.org (localhost [127.0.0.1]) by mails.dpdk.org (Postfix) with ESMTP id 314FE42E61; Thu, 5 Sep 2024 11:29:22 +0200 (CEST) Received: from mgamail.intel.com (mgamail.intel.com [198.175.65.14]) by mails.dpdk.org (Postfix) with ESMTP id 2707A42E45 for ; Thu, 5 Sep 2024 11:29:20 +0200 (CEST) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=intel.com; i=@intel.com; q=dns/txt; s=Intel; t=1725528560; x=1757064560; h=from:to:cc:subject:date:message-id:in-reply-to: references:mime-version:content-transfer-encoding; bh=aWHMWwJX/4fxfYXlDWKBQSFvsmrNl16PjHpvefPTP6w=; b=ZfXkEzAiDU7O9avSP3JYAf8HPcrmcdvnxFOpaykASgdH+dfv1xb99ZNU jTczEEFv0rgfvO2ynsGi+6pGRN5WZF/GBVMco+znoNpA2LD4RJc7eRpxE XOjBHsLJrWwK/3Oa4jhMootNgUy8GdFvsm3yWBX1agpads8jrBssaCivd E5kxySi0HVh+0+v9FtxdRkougOQ1lQedeEhhsd8STsXlaaz2Igvb6WbR8 80zdkJBMv9uSLMxsl7czK4ZjQPxLZZOjcOcReqUqrJrt+mPCJO8exix5y SmeXBFCUiKTZ0xAUQpXgkjwDsmrlD6x1NfbvccNepF60iTnlxmimwqE2I A==; X-CSE-ConnectionGUID: TYAzhWWgTRabYbdB7k6/AQ== X-CSE-MsgGUID: kVQpdTSGTdWrr9Bo4ve0yQ== X-IronPort-AV: E=McAfee;i="6700,10204,11185"; a="28023279" X-IronPort-AV: E=Sophos;i="6.10,204,1719903600"; d="scan'208";a="28023279" Received: from fmviesa010.fm.intel.com ([10.60.135.150]) by orvoesa106.jf.intel.com with ESMTP/TLS/ECDHE-RSA-AES256-GCM-SHA384; 05 Sep 2024 02:29:20 -0700 X-CSE-ConnectionGUID: Ocz7K//+Ss6ZnSKVWkV7xA== X-CSE-MsgGUID: dAjqqPnyT5iljVra056D/w== X-ExtLoop1: 1 X-IronPort-AV: E=Sophos;i="6.10,204,1719903600"; d="scan'208";a="65807947" Received: from unknown (HELO localhost.localdomain) ([10.239.252.253]) by fmviesa010-auth.fm.intel.com with ESMTP/TLS/ECDHE-RSA-AES256-GCM-SHA384; 05 Sep 2024 02:29:18 -0700 From: Mingjin Ye To: dev@dpdk.org Cc: Mingjin Ye , Simei Su , Wenjun Wu , Kirill Rybalchenko Subject: [PATCH 3/3] examples/ptpclient: add frequency adjustment support Date: Thu, 5 Sep 2024 09:05:45 +0000 Message-Id: <20240905090545.853730-4-mingjinx.ye@intel.com> X-Mailer: git-send-email 2.25.1 In-Reply-To: <20240905090545.853730-1-mingjinx.ye@intel.com> References: <20240905090545.853730-1-mingjinx.ye@intel.com> MIME-Version: 1.0 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 Signed-off-by: Mingjin Ye --- 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, ð_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();