net: gptp: adjust only frequency for continuous synchronization

Current clock synchronization was always stepping clock. This was
causing large offset, and discontiguous ptp hardware clock time.
For TSN hardware, discontiguous ptp hardware clock time was not
able to be used for other TSN protocols.

This patch is to convert to frequency adjustment with a basic
PI control algorithm.

Signed-off-by: Yangbo Lu <yangbo.lu@nxp.com>

Author: yangbolu1991

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
  *