Support gPTP sync with PI control and fix i.MXRT1060 support

drivers/clock_control/clock_control_mcux_ccm.c

@@ -296,7 +296,11 @@ static int mcux_ccm_get_subsys_rate(const struct device *dev,
 
 #ifdef CONFIG_PTP_CLOCK_NXP_ENET
 	case IMX_CCM_ENET_PLL:
+#if defined(CONFIG_SOC_SERIES_IMXRT10XX)
+		*rate = CLOCK_GetPllFreq(kCLOCK_PllEnet25M);
+#else
 		*rate = CLOCK_GetPllFreq(kCLOCK_PllEnet);
+#endif
 		break;
 #endif
 

drivers/ethernet/eth_nxp_enet.c

@@ -106,8 +106,7 @@ struct nxp_enet_mac_data {
 	struct k_mutex tx_frame_buf_mutex;
 	struct k_mutex rx_frame_buf_mutex;
 #ifdef CONFIG_PTP_CLOCK_NXP_ENET
-	struct k_sem ptp_ts_sem;
-	struct k_mutex *ptp_mutex; /* created in PTP driver */
+	struct nxp_enet_ptp_data ptp;
 #endif
 	uint8_t *tx_frame_buf;
 	uint8_t *rx_frame_buf;
@@ -165,15 +164,15 @@ static inline void ts_register_tx_event(const struct device *dev,
 
 	if (pkt && atomic_get(&pkt->atomic_ref) > 0) {
 		if (eth_get_ptp_data(net_pkt_iface(pkt), pkt) && frameinfo->isTsAvail) {
-			k_mutex_lock(data->ptp_mutex, K_FOREVER);
+			k_mutex_lock(data->ptp.ptp_mutex, K_FOREVER);
 
 			pkt->timestamp.nanosecond = frameinfo->timeStamp.nanosecond;
 			pkt->timestamp.second = frameinfo->timeStamp.second;
 
 			net_if_add_tx_timestamp(pkt);
-			k_sem_give(&data->ptp_ts_sem);
+			k_sem_give(&data->ptp.ptp_ts_sem);
 
-			k_mutex_unlock(data->ptp_mutex);
+			k_mutex_unlock(data->ptp.ptp_mutex);
 		}
 		net_pkt_unref(pkt);
 	}
@@ -184,7 +183,7 @@ static inline void eth_wait_for_ptp_ts(const struct device *dev, struct net_pkt
 	struct nxp_enet_mac_data *data = dev->data;
 
 	net_pkt_ref(pkt);
-	k_sem_take(&data->ptp_ts_sem, K_FOREVER);
+	k_sem_take(&data->ptp.ptp_ts_sem, K_FOREVER);
 }
 #else
 #define eth_get_ptp_data(...) false
@@ -389,7 +388,7 @@ static int eth_nxp_enet_rx(const struct device *dev)
 	}
 
 #if defined(CONFIG_PTP_CLOCK_NXP_ENET)
-	k_mutex_lock(data->ptp_mutex, K_FOREVER);
+	k_mutex_lock(data->ptp.ptp_mutex, K_FOREVER);
 
 	/* Invalid value by default. */
 	pkt->timestamp.nanosecond = UINT32_MAX;
@@ -411,7 +410,7 @@ static int eth_nxp_enet_rx(const struct device *dev)
 		pkt->timestamp.nanosecond = ts;
 		pkt->timestamp.second = ptp_time.second;
 	}
-	k_mutex_unlock(data->ptp_mutex);
+	k_mutex_unlock(data->ptp.ptp_mutex);
 #endif /* CONFIG_PTP_CLOCK_NXP_ENET */
 
 	iface = get_iface(data);
@@ -709,7 +708,7 @@ static int eth_nxp_enet_init(const struct device *dev)
 	k_sem_init(&data->tx_buf_sem,
 		   CONFIG_ETH_NXP_ENET_TX_BUFFERS, CONFIG_ETH_NXP_ENET_TX_BUFFERS);
 #if defined(CONFIG_PTP_CLOCK_NXP_ENET)
-	k_sem_init(&data->ptp_ts_sem, 0, 1);
+	k_sem_init(&data->ptp.ptp_ts_sem, 0, 1);
 #endif
 	k_work_init(&data->rx_work, eth_nxp_enet_rx_thread);
 
@@ -781,8 +780,9 @@ static int eth_nxp_enet_init(const struct device *dev)
 	nxp_enet_driver_cb(config->mdio, NXP_ENET_MDIO, NXP_ENET_MODULE_RESET, NULL);
 
 #if defined(CONFIG_PTP_CLOCK_NXP_ENET)
+	data->ptp.enet = &data->enet_handle;
 	nxp_enet_driver_cb(config->ptp_clock, NXP_ENET_PTP_CLOCK,
-				NXP_ENET_MODULE_RESET, &data->ptp_mutex);
+				NXP_ENET_MODULE_RESET, &data->ptp);
 	ENET_SetTxReclaim(&data->enet_handle, true, 0);
 #endif
 

drivers/ptp_clock/ptp_clock_nxp_enet.c

@@ -29,8 +29,7 @@ struct ptp_clock_nxp_enet_config {
 
 struct ptp_clock_nxp_enet_data {
 	ENET_Type *base;
-	double clock_ratio;
-	enet_handle_t enet_handle;
+	enet_handle_t *enet_handle;
 	struct k_mutex ptp_mutex;
 };
 
@@ -43,7 +42,7 @@ static int ptp_clock_nxp_enet_set(const struct device *dev,
 	enet_time.second = tm->second;
 	enet_time.nanosecond = tm->nanosecond;
 
-	ENET_Ptp1588SetTimer(data->base, &data->enet_handle, &enet_time);
+	ENET_Ptp1588SetTimer(data->base, data->enet_handle, &enet_time);
 
 	return 0;
 }
@@ -54,7 +53,7 @@ static int ptp_clock_nxp_enet_get(const struct device *dev,
 	struct ptp_clock_nxp_enet_data *data = dev->data;
 	enet_ptp_time_t enet_time;
 
-	ENET_Ptp1588GetTimer(data->base, &data->enet_handle, &enet_time);
+	ENET_Ptp1588GetTimer(data->base, data->enet_handle, &enet_time);
 
 	tm->second = enet_time.second;
 	tm->nanosecond = enet_time.nanosecond;
@@ -110,17 +109,12 @@ static int ptp_clock_nxp_enet_rate_adjust(const struct device *dev,
 		return 0;
 	}
 
-	ratio *= data->clock_ratio;
-
 	/* Limit possible ratio. */
 	if ((ratio > 1.0 + 1.0/(2 * hw_inc)) ||
 			(ratio < 1.0 - 1.0/(2 * hw_inc))) {
 		return -EINVAL;
 	}
 
-	/* Save new ratio. */
-	data->clock_ratio = ratio;
-
 	if (ratio < 1.0) {
 		corr = hw_inc - 1;
 		val = 1.0 / (hw_inc * (1.0 - ratio));
@@ -156,6 +150,11 @@ void nxp_enet_ptp_clock_callback(const struct device *dev,
 {
 	const struct ptp_clock_nxp_enet_config *config = dev->config;
 	struct ptp_clock_nxp_enet_data *data = dev->data;
+	struct nxp_enet_ptp_data *ptp_data;
+
+	__ASSERT(cb_data == NULL, "ptp data is NULL");
+
+	ptp_data = (struct nxp_enet_ptp_data *)cb_data;
 
 	if (event == NXP_ENET_MODULE_RESET) {
 		enet_ptp_config_t ptp_config;
@@ -172,17 +171,15 @@ void nxp_enet_ptp_clock_callback(const struct device *dev,
 		/* only for ERRATA_2579 */
 		ptp_config.channel = kENET_PtpTimerChannel3;
 		ptp_config.ptp1588ClockSrc_Hz = enet_ref_pll_rate;
-		data->clock_ratio = 1.0;
+
+		/* Share the mutex with mac driver */
+		ptp_data->ptp_mutex = &data->ptp_mutex;
+		/* Get enet handle from mac driver */
+		data->enet_handle = ptp_data->enet;
 
 		ENET_Ptp1588SetChannelMode(data->base, kENET_PtpTimerChannel3,
 				kENET_PtpChannelPulseHighonCompare, true);
-		ENET_Ptp1588Configure(data->base, &data->enet_handle,
-				      &ptp_config);
-	}
-
-	if (cb_data != NULL) {
-		/* Share the mutex with mac driver */
-		*(uintptr_t *)cb_data = (uintptr_t)&data->ptp_mutex;
+		ENET_Ptp1588StartTimer(data->base, ptp_config.ptp1588ClockSrc_Hz);
 	}
 }
 
@@ -220,7 +217,7 @@ static void ptp_clock_nxp_enet_isr(const struct device *dev)
 		}
 	}
 
-	ENET_TimeStampIRQHandler(data->base, &data->enet_handle);
+	ENET_TimeStampIRQHandler(data->base, data->enet_handle);
 
 	irq_unlock(irq_lock_key);
 }

include/zephyr/drivers/ethernet/eth_nxp_enet.h

@@ -40,6 +40,12 @@ enum nxp_enet_driver {
 	NXP_ENET_PTP_CLOCK,
 };
 
+struct nxp_enet_ptp_data {
+	struct k_sem ptp_ts_sem;
+	struct k_mutex *ptp_mutex; /* created in PTP driver */
+	void *enet; /* enet_handle poiniter used by PTP driver */
+};
+
 extern void nxp_enet_mdio_callback(const struct device *mdio_dev,
 		enum nxp_enet_callback_reason event,
 		void *data);

subsys/net/l2/ethernet/gptp/Kconfig

@@ -211,4 +211,10 @@ config NET_GPTP_STATISTICS
 	  Enable this if you need to collect gPTP statistics. The statistics
 	  can be seen in net-shell if needed.
 
+config NET_GPTP_MONITOR_SYNC_STATUS
+	bool "Monitor real-time synchronization status"
+	help
+	  Monitor real-time synchronization status, like synchronization offset,
+	  frequency offset and so on. This will print continuous messages.
+
 endif # NET_GPTP

subsys/net/l2/ethernet/gptp/gptp.c

@@ -35,6 +35,7 @@ K_FIFO_DEFINE(gptp_rx_queue);
 static k_tid_t tid;
 static struct k_thread gptp_thread_data;
 struct gptp_domain gptp_domain;
+struct gptp_clock_data gptp_clock;
 
 int gptp_get_port_number(struct net_if *iface)
 {
@@ -911,6 +912,18 @@ int gptp_get_port_data(struct gptp_domain *domain,
 	return 0;
 }
 
+double gptp_servo_pi(int64_t nanosecond_diff)
+{
+	double kp = 0.7;
+	double ki = 0.3;
+	double ppb;
+
+	gptp_clock.pi_drift += ki * nanosecond_diff;
+	ppb = kp * nanosecond_diff + gptp_clock.pi_drift;
+
+	return ppb;
+}
+
 static void init_ports(void)
 {
 	net_if_foreach(gptp_add_port, &gptp_domain.default_ds.nb_ports);
@@ -929,5 +942,8 @@ void net_gptp_init(void)
 {
 	gptp_domain.default_ds.nb_ports = 0U;
 
+	gptp_clock.domain = &gptp_domain;
+	gptp_clock.pi_drift = 0.0;
+
 	init_ports();
 }

subsys/net/l2/ethernet/gptp/gptp_mi.c

@@ -786,14 +786,12 @@ static void gptp_update_local_port_clock(void)
 		nanosecond_diff = -(int64_t)NSEC_PER_SEC + nanosecond_diff;
 	}
 
-	ptp_clock_rate_adjust(clk, port_ds->neighbor_rate_ratio);
-
 	/* If time difference is too high, set the clock value.
 	 * Otherwise, adjust it.
 	 */
 	if (second_diff || (second_diff == 0 &&
-			    (nanosecond_diff < -5000 ||
-			     nanosecond_diff > 5000))) {
+			    (nanosecond_diff < -50000000 ||
+			     nanosecond_diff > 50000000))) {
 		bool underflow = false;
 
 		key = irq_lock();
@@ -822,28 +820,22 @@ static void gptp_update_local_port_clock(void)
 			tm.second++;
 			tm.nanosecond -= NSEC_PER_SEC;
 		}
-
-		/* This prints too much data normally but can be enabled to see
-		 * what time we are setting to the local clock.
-		 */
-		if (0) {
-			NET_INFO("Set local clock %lu.%lu",
-				 (unsigned long int)tm.second,
-				 (unsigned long int)tm.nanosecond);
+		if (IS_ENABLED(CONFIG_NET_GPTP_MONITOR_SYNC_STATUS)) {
+			NET_INFO("Set local clock %"PRIu64".%09u", tm.second, tm.nanosecond);
 		}
-
 		ptp_clock_set(clk, &tm);
 
 	skip_clock_set:
 		irq_unlock(key);
 	} else {
-		if (nanosecond_diff < -200) {
-			nanosecond_diff = -200;
-		} else if (nanosecond_diff > 200) {
-			nanosecond_diff = 200;
-		}
+		double ppb = gptp_servo_pi(nanosecond_diff);
+
+		ptp_clock_rate_adjust(clk, 1.0 + (ppb / 1000000000.0));
 
-		ptp_clock_adjust(clk, nanosecond_diff);
+		if (IS_ENABLED(CONFIG_NET_GPTP_MONITOR_SYNC_STATUS)) {
+			NET_INFO("sync offset %9"PRId64" ns, freq offset %f ppb",
+				 nanosecond_diff, ppb);
+		}
 	}
 }
 #endif /* CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE */

subsys/net/l2/ethernet/gptp/gptp_private.h

@@ -30,6 +30,18 @@ extern "C" {
 #define GPTP_STATS_INC(port, var)
 #endif
 
+/**
+ * @brief gPTP clock data.
+ */
+struct gptp_clock_data {
+	/** gptp_domain pointer */
+	struct gptp_domain *domain;
+	/** pi control drift value */
+	double pi_drift;
+};
+
+extern struct gptp_clock_data gptp_clock;
+
 /**
  * @brief Is a slave acting as a slave.
  *
@@ -117,6 +129,15 @@ static inline uint64_t gptp_timestamp_to_nsec(struct net_ptp_time *ts)
 	return (ts->second * NSEC_PER_SEC) + ts->nanosecond;
 }
 
+/**
+ * @brief gPTP PI servo.
+ *
+ * @param nanosecond_diff nanosecond offset.
+ *
+ * @return ppb value to adjust.
+ */
+double gptp_servo_pi(int64_t nanosecond_diff);
+
 /**
  * @brief Change the port state
  *