[PATCH 1/2] net: stmmac: retain PTP-clock at hwtstamp_set

[Holger Assmann]

As it is, valid SIOCSHWTSTAMP ioctl calls - i.e. enable/disable time
stamping or changing filter settings - lead to synchronization of the
NIC's hardware clock with CLOCK_REALTIME. This might be necessary
during system initialization, but at runtime, when the PTP clock has
already been synchronized to a grand master, a reset of the timestamp
settings might result in a clock jump.

This further differs from how drivers like IGB and FEC behave: Those
initialize the PTP system time less frequently - on interface up and
at probe time, respectively.

We consequently introduce the new function stmmac_init_hwtstamp(), which
gets called during ndo_open(). It contains the code snippet moved
from stmmac_hwtstamp_set() that manages the time synchronization. Besides,
the sub second increment configuration is also moved here since the
related values are hardware dependent and do not change during runtime.

Furthermore, the hardware clock must be kept running even when no time
stamping mode is selected in order to retain the once synced time basis.
That way, time stamping can be enabled again at any time only with the
need for compensation of the clock's natural drifting.

As a side effect, this patch fixes a potential race between SIOCSHWTSTAMP
and ptp_clock_info::enable regarding priv->systime_flags. Subsequently,
since this variable becomes deprecated by this commit, it should be
removed completely in a follow-up patch.

Fixes: 92ba6888510c ("stmmac: add the support for PTP hw clock driver")
Fixes: cc4c9001ce31 ("net: stmmac: Switch stmmac_hwtimestamp to generic
HW Interface Helpers")

Reported-by: Michael Olbrich <m.olbrich@xxxxxxxxxxxxxx>
Signed-off-by: Ahmad Fatoum <a.fatoum@xxxxxxxxxxxxxx>
Signed-off-by: Holger Assmann <h.assmann@xxxxxxxxxxxxxx>
---
 .../net/ethernet/stmicro/stmmac/stmmac_main.c | 121 ++++++++++++------
 1 file changed, 80 insertions(+), 41 deletions(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 5b1c12ff98c0..55f5e6cd1cad 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -46,6 +46,13 @@
 #include "dwxgmac2.h"
 #include "hwif.h"
 
+
+/* As long the interface is active, we keep the timestamping HW enabled with
+ * fine resolution and binary rollover. This avoid non-monotonic behavior
+ * when changing timestamp settings at runtime
+ * */
+#define STMMAC_HWTS_ACTIVE (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | PTP_TCR_TSCTRLSSR)
+
 #define	STMMAC_ALIGN(x)		ALIGN(ALIGN(x, SMP_CACHE_BYTES), 16)
 #define	TSO_MAX_BUFF_SIZE	(SZ_16K - 1)
 
@@ -509,8 +516,6 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
 	struct hwtstamp_config config;
-	struct timespec64 now;
-	u64 temp = 0;
 	u32 ptp_v2 = 0;
 	u32 tstamp_all = 0;
 	u32 ptp_over_ipv4_udp = 0;
@@ -519,7 +524,6 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 	u32 snap_type_sel = 0;
 	u32 ts_master_en = 0;
 	u32 ts_event_en = 0;
-	u32 sec_inc = 0;
 	u32 value = 0;
 	bool xmac;
 
@@ -686,39 +690,16 @@ static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 	priv->hwts_tx_en = config.tx_type == HWTSTAMP_TX_ON;
 
 	if (!priv->hwts_tx_en && !priv->hwts_rx_en)
-		stmmac_config_hw_tstamping(priv, priv->ptpaddr, 0);
+		stmmac_config_hw_tstamping(priv, priv->ptpaddr, STMMAC_HWTS_ACTIVE);
 	else {
-		value = (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | PTP_TCR_TSCTRLSSR |
+		value = (STMMAC_HWTS_ACTIVE |
 			 tstamp_all | ptp_v2 | ptp_over_ethernet |
 			 ptp_over_ipv6_udp | ptp_over_ipv4_udp | ts_event_en |
 			 ts_master_en | snap_type_sel);
 		stmmac_config_hw_tstamping(priv, priv->ptpaddr, value);
-
-		/* program Sub Second Increment reg */
-		stmmac_config_sub_second_increment(priv,
-				priv->ptpaddr, priv->plat->clk_ptp_rate,
-				xmac, &sec_inc);
-		temp = div_u64(1000000000ULL, sec_inc);
-
-		/* Store sub second increment and flags for later use */
-		priv->sub_second_inc = sec_inc;
+		
+		/* Store flags for later use */
 		priv->systime_flags = value;
-
-		/* calculate default added value:
-		 * formula is :
-		 * addend = (2^32)/freq_div_ratio;
-		 * where, freq_div_ratio = 1e9ns/sec_inc
-		 */
-		temp = (u64)(temp << 32);
-		priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate);
-		stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend);
-
-		/* initialize system time */
-		ktime_get_real_ts64(&now);
-
-		/* lower 32 bits of tv_sec are safe until y2106 */
-		stmmac_init_systime(priv, priv->ptpaddr,
-				(u32)now.tv_sec, now.tv_nsec);
 	}
 
 	memcpy(&priv->tstamp_config, &config, sizeof(config));
@@ -791,6 +772,63 @@ static void stmmac_release_ptp(struct stmmac_priv *priv)
 	stmmac_ptp_unregister(priv);
 }
 
+/**
+ * stmmac_init_hwtstamp - init Timestamping Hardware
+ * @priv: driver private structure
+ * Description: Initialize hardware for Timestamping use
+ * This is valid as long as the interface is open and not suspended.
+ * Will be rerun after resume from suspension.
+ */
+static int stmmac_init_hwtstamp(struct stmmac_priv *priv)
+{
+	bool xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
+	struct timespec64 now;
+	u32 sec_inc = 0;
+	u64 temp = 0;
+	u32 value;
+	int ret;
+
+	ret = clk_prepare_enable(priv->plat->clk_ptp_ref);
+	if (ret < 0) {
+		netdev_warn(priv->dev, "failed to enable PTP reference clock: %d\n", ret);
+		return ret;
+	}
+
+	if (!(priv->dma_cap.time_stamp || priv->dma_cap.atime_stamp))
+		return -EOPNOTSUPP;
+
+	value = STMMAC_HWTS_ACTIVE;
+	stmmac_config_hw_tstamping(priv, priv->ptpaddr, value);
+
+	/* program Sub Second Increment reg */
+	stmmac_config_sub_second_increment(priv,
+			priv->ptpaddr, priv->plat->clk_ptp_rate,
+			xmac, &sec_inc);
+	temp = div_u64(1000000000ULL, sec_inc);
+
+	/* Store sub second increment and flags for later use */
+	priv->sub_second_inc = sec_inc;
+	priv->systime_flags = value;
+
+	/* calculate default added value:
+	 * formula is :
+	 * addend = (2^32)/freq_div_ratio;
+	 * where, freq_div_ratio = 1e9ns/sec_inc
+	 */
+	temp = (u64)(temp << 32);
+	priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate);
+	stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend);
+
+	/* initialize system time */
+	ktime_get_real_ts64(&now);
+
+	/* lower 32 bits of tv_sec are safe until y2106 */
+	stmmac_init_systime(priv, priv->ptpaddr,
+			(u32)now.tv_sec, now.tv_nsec);
+
+	return 0;
+}
+
 /**
  *  stmmac_mac_flow_ctrl - Configure flow control in all queues
  *  @priv: driver private structure
@@ -2713,15 +2751,17 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
 	stmmac_mmc_setup(priv);
 
 	if (init_ptp) {
-		ret = clk_prepare_enable(priv->plat->clk_ptp_ref);
-		if (ret < 0)
-			netdev_warn(priv->dev, "failed to enable PTP reference clock: %d\n", ret);
-
-		ret = stmmac_init_ptp(priv);
-		if (ret == -EOPNOTSUPP)
-			netdev_warn(priv->dev, "PTP not supported by HW\n");
-		else if (ret)
-			netdev_warn(priv->dev, "PTP init failed\n");
+		ret = stmmac_init_hwtstamp(priv);
+		if (ret) {
+			netdev_warn(priv->dev, "HW Timestamping init failed: %pe\n",
+					ERR_PTR(ret));
+		} else {
+			ret = stmmac_init_ptp(priv);
+			if (ret == -EOPNOTSUPP)
+				netdev_warn(priv->dev, "PTP not supported by HW\n");
+			else if (ret)
+				netdev_warn(priv->dev, "PTP init failed\n");
+		}
 	}
 
 	priv->eee_tw_timer = STMMAC_DEFAULT_TWT_LS;
@@ -5290,8 +5330,7 @@ int stmmac_resume(struct device *dev)
 		/* enable the clk previously disabled */
 		clk_prepare_enable(priv->plat->stmmac_clk);
 		clk_prepare_enable(priv->plat->pclk);
-		if (priv->plat->clk_ptp_ref)
-			clk_prepare_enable(priv->plat->clk_ptp_ref);
+		stmmac_init_hwtstamp(priv);
 		/* reset the phy so that it's ready */
 		if (priv->mii)
 			stmmac_mdio_reset(priv->mii);

base-commit: 3db1a3fa98808aa90f95ec3e0fa2fc7abf28f5c9
-- 
2.20.1