[PATCH net-next v1 4/4] net: stmmac: use delays reported by the PHY driver to correct MAC propagation delay

[Oleksij Rempel]

Now after PHY drivers are able to report data path delays, we can use
them in the MAC drivers for the delay correction.

Signed-off-by: Oleksij Rempel <o.rempel@xxxxxxxxxxxxxx>
---
 drivers/net/ethernet/stmicro/stmmac/stmmac.h    |  2 ++
 .../ethernet/stmicro/stmmac/stmmac_hwtstamp.c   |  4 ++++
 .../net/ethernet/stmicro/stmmac/stmmac_main.c   | 17 ++++++++++++++++-
 3 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index dddcaa9220cc3..6db54ce33ea72 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -288,6 +288,8 @@ struct stmmac_priv {
 	u32 sub_second_inc;
 	u32 systime_flags;
 	u32 adv_ts;
+	u64 phy_tx_delay_ns;
+	u64 phy_rx_delay_ns;
 	int use_riwt;
 	int irq_wake;
 	rwlock_t ptp_lock;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
index f05bd757dfe52..bbf284cb7cc2a 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
@@ -71,6 +71,8 @@ static void hwtstamp_correct_latency(struct stmmac_priv *priv)
 	/* MAC-internal ingress latency */
 	scaled_ns = readl(ioaddr + PTP_TS_INGR_LAT);
 
+	scaled_ns += priv->phy_rx_delay_ns << 16;
+
 	/* See section 11.7.2.5.3.1 "Ingress Correction" on page 4001 of
 	 * i.MX8MP Applications Processor Reference Manual Rev. 1, 06/2021
 	 */
@@ -95,6 +97,8 @@ static void hwtstamp_correct_latency(struct stmmac_priv *priv)
 	/* MAC-internal egress latency */
 	scaled_ns = readl(ioaddr + PTP_TS_EGR_LAT);
 
+	scaled_ns += priv->phy_tx_delay_ns << 16;
+
 	reg_tsec = scaled_ns >> 16;
 	reg_tsecsns = scaled_ns & 0xff00;
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index fe3498e86de9d..30c7c278b7062 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -1097,8 +1097,23 @@ static void stmmac_mac_link_up(struct phylink_config *config,
 	if (priv->dma_cap.fpesel)
 		stmmac_fpe_link_state_handle(priv, true);
 
-	if (priv->plat->flags & STMMAC_FLAG_HWTSTAMP_CORRECT_LATENCY)
+	if (priv->plat->flags & STMMAC_FLAG_HWTSTAMP_CORRECT_LATENCY) {
+		int ret = 0;
+
+		if (phy)
+			ret = phy_get_timesync_data_path_delays(phy,
+								&priv->phy_tx_delay_ns,
+								&priv->phy_rx_delay_ns);
+		if (!phy || ret) {
+			if (ret != -EOPNOTSUPP)
+				netdev_err(priv->dev, "Failed to get PHY delay: %pe\n",
+					   ERR_PTR(ret));
+			priv->phy_tx_delay_ns = 0;
+			priv->phy_rx_delay_ns = 0;
+		}
+
 		stmmac_hwtstamp_correct_latency(priv, priv);
+	}
 }
 
 static const struct phylink_mac_ops stmmac_phylink_mac_ops = {
-- 
2.39.2