From 149c45abfec9a2a26650da5824568e99220301e4 Mon Sep 17 00:00:00 2001
From: Roger Quadros <rogerq@kernel.org>
Date: Tue, 3 May 2022 15:43:21 +0300
Subject: [PATCH] net: ti: icssg_prueth: fix missed back to back TX timestamp
 responses

There may be more than one TX timestamp responses pending
so check and process those before returning from interrupt
worker thread.

Fixes: 81e29e8cb07c ("net: ti: icssg_prueth: Support multiple TX timestamp requests")
Signed-off-by: Roger Quadros <rogerq@kernel.org>
Signed-off-by: Vignesh Raghavendra <vigneshr@ti.com>
---
 drivers/net/ethernet/ti/icssg_prueth.c | 60 +++++++++++++-------------
 drivers/net/ethernet/ti/icssg_prueth.h |  1 +
 2 files changed, 30 insertions(+), 31 deletions(-)

diff --git a/drivers/net/ethernet/ti/icssg_prueth.c b/drivers/net/ethernet/ti/icssg_prueth.c
index 0feb046ff253..dab44c8172c6 100644
--- a/drivers/net/ethernet/ti/icssg_prueth.c
+++ b/drivers/net/ethernet/ti/icssg_prueth.c
@@ -766,48 +766,44 @@ static void tx_ts_work(struct prueth_emac *emac)
 	u64 ns;
 	struct skb_shared_hwtstamps ssh;
 	struct sk_buff *skb;
-	int timeout = 10;
 	int ret = 0;
 	struct emac_tx_ts_response tsr;
 	u32 hi_sw;
 
-	while (timeout-- > 0) {
-		/* wait for response or timeout */
+	/* There may be more than one pending requests */
+	while (1) {
 		ret = emac_get_tx_ts(emac, &tsr);
-		if (!ret)
+		if (ret)	/* nothing more */
 			break;
-		usleep_range(10, 20);
-	}
 
-	if (ret) {
-		netdev_err(emac->ndev, "TX timestamp timeout\n");
-		return;
-	}
+		if (tsr.cookie >= PRUETH_MAX_TX_TS_REQUESTS ||
+		    !emac->tx_ts_skb[tsr.cookie]) {
+			netdev_err(emac->ndev, "Invalid TX TS cookie 0x%x\n",
+				   tsr.cookie);
+			break;
+		}
 
-	if (tsr.cookie >= PRUETH_MAX_TX_TS_REQUESTS ||
-	    !emac->tx_ts_skb[tsr.cookie]) {
-		netdev_err(emac->ndev, "Invalid TX TS cookie 0x%x\n",
-			   tsr.cookie);
-		return;
-	}
+		skb = emac->tx_ts_skb[tsr.cookie];
+		emac->tx_ts_skb[tsr.cookie] = NULL;	/* free slot */
+		if (!skb) {
+			netdev_err(emac->ndev, "Driver Bug! got NULL skb\n");
+			break;
+		}
 
-	skb = emac->tx_ts_skb[tsr.cookie];
-	emac->tx_ts_skb[tsr.cookie] = NULL;	/* free slot */
-	if (!skb) {
-		netdev_err(emac->ndev, "Driver Bug! got NULL skb\n");
-		return;
-	}
+		hi_sw = readl(emac->prueth->shram.va +
+			      TIMESYNC_FW_WC_COUNT_HI_SW_OFFSET_OFFSET);
+		ns = icssg_ts_to_ns(hi_sw, tsr.hi_ts, tsr.lo_ts,
+				    IEP_DEFAULT_CYCLE_TIME_NS);
 
-	hi_sw = readl(emac->prueth->shram.va +
-		      TIMESYNC_FW_WC_COUNT_HI_SW_OFFSET_OFFSET);
-	ns = icssg_ts_to_ns(hi_sw, tsr.hi_ts, tsr.lo_ts,
-			    IEP_DEFAULT_CYCLE_TIME_NS);
+		memset(&ssh, 0, sizeof(ssh));
+		ssh.hwtstamp = ns_to_ktime(ns);
 
-	memset(&ssh, 0, sizeof(ssh));
-	ssh.hwtstamp = ns_to_ktime(ns);
+		skb_tstamp_tx(skb, &ssh);
+		dev_consume_skb_any(skb);
 
-	skb_tstamp_tx(skb, &ssh);
-	dev_consume_skb_any(skb);
+		if (atomic_dec_and_test(&emac->tx_ts_pending))	/* no more? */
+			break;
+	}
 
 	return;
 }
@@ -883,7 +879,6 @@ static int emac_ndo_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	epib[1] = 0;
 	if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
 	    emac->tx_ts_enabled) {
-		/* We currently support only one TX HW timestamp at a time */
 		tx_ts_cookie = prueth_tx_ts_cookie_get(emac);
 		if (tx_ts_cookie >= 0) {
 			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
@@ -961,6 +956,9 @@ static int emac_ndo_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 		goto drop_free_descs;
 	}
 
+	if (in_tx_ts)
+		atomic_inc(&emac->tx_ts_pending);
+
 	if (k3_cppi_desc_pool_avail(tx_chn->desc_pool) < MAX_SKB_FRAGS) {
 		netif_tx_stop_queue(netif_txq);
 		/* Barrier, so that stop_queue visible to other cpus */
diff --git a/drivers/net/ethernet/ti/icssg_prueth.h b/drivers/net/ethernet/ti/icssg_prueth.h
index b176ac96c2d7..9c0d948ed700 100644
--- a/drivers/net/ethernet/ti/icssg_prueth.h
+++ b/drivers/net/ethernet/ti/icssg_prueth.h
@@ -170,6 +170,7 @@ struct prueth_emac {
 	/* TX HW Timestamping */
 	/* TX TS cookie will be index to the tx_ts_skb array */
 	struct sk_buff *tx_ts_skb[PRUETH_MAX_TX_TS_REQUESTS];
+	atomic_t tx_ts_pending;
 	int tx_ts_irq;
 
 	u8 cmd_seq;
