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/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_domain_data gptp_domain_data;
 
 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_domain_data.pi_drift += ki * nanosecond_diff;
+	ppb = kp * nanosecond_diff + gptp_domain_data.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_domain_data.domain = &gptp_domain;
+	gptp_domain_data.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();
@@ -837,13 +835,17 @@ static void gptp_update_local_port_clock(void)
 	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_adjust(clk, nanosecond_diff);
+		ptp_clock_rate_adjust(clk, 1.0 + (ppb / 1000000000.0));
+		/* This prints continuous messages but can be enabled to see
+		 * the real time convergence of synchronization offset.
+		 * This is what users care about.
+		 */
+		if (0) {
+			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 domain all data.
+ */
+struct gptp_domain_data {
+	/** gptp_domain pointer */
+	struct gptp_domain *domain;
+	/** pi control drift value */
+	double pi_drift;
+};
+
+extern struct gptp_domain_data gptp_domain_data;
+
 /**
  * @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
  *