[PATCH net v2 10/10] net: systemport: Update TDMA queue mapping dynamically on changeupper

[Florian Fainelli <florian.fainelli@broadcom.com>]

When DSA upper devices are dynamically attached or detached while the
master SYSTEMPORT interface is already up and running (netif_running()),
bcm_sysport_map_queues() and bcm_sysport_unmap_queues() updated the
internal software mappings but did not update the TDMA_DESC_RING_MAPPING
hardware registers, because programming was previously deferred until
bcm_sysport_init_tx_ring().

Update TDMA_DESC_RING_MAPPING registers immediately if netif_running()
is true during map_queues and unmap_queues.

Fixes: 1593cd40d785 ("net: systemport: use standard netdevice notifier to detect DSA presence")
Assisted-by: LLM
Signed-off-by: Florian Fainelli <florian.fainelli@broadcom.com>
---
 drivers/net/ethernet/broadcom/bcmsysport.c | 23 +++++++++++++++++++---
 1 file changed, 20 insertions(+), 3 deletions(-)

diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index dedd49ad6c4e..ca1b86953dba 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -2315,6 +2315,7 @@ static int bcm_sysport_map_queues(struct net_device *dev,
 	struct bcm_sysport_tx_ring *ring;
 	unsigned int num_tx_queues;
 	unsigned int q, qp, port;
+	u32 reg;
 
 	/* We can't be setting up queue inspection for non directly attached
 	 * switches
@@ -2350,14 +2351,21 @@ static int bcm_sysport_map_queues(struct net_device *dev,
 		if (ring->inspect)
 			continue;
 
-		/* Just remember the mapping actual programming done
-		 * during bcm_sysport_init_tx_ring
-		 */
 		ring->switch_queue = qp;
 		ring->switch_port = port;
 		ring->inspect = true;
 		if (qp + port * num_tx_queues < ARRAY_SIZE(priv->ring_map))
 			priv->ring_map[qp + port * num_tx_queues] = ring;
+
+		if (netif_running(dev)) {
+			reg = tdma_readl(priv, TDMA_DESC_RING_MAPPING(q));
+			reg &= ~(RING_QID_MASK |
+				 RING_PORT_ID_MASK << RING_PORT_ID_SHIFT |
+				 RING_IGNORE_STATUS);
+			reg |= (qp & RING_QID_MASK);
+			reg |= (port << RING_PORT_ID_SHIFT);
+			tdma_writel(priv, reg, TDMA_DESC_RING_MAPPING(q));
+		}
 		qp++;
 	}
 
@@ -2372,6 +2380,7 @@ static int bcm_sysport_unmap_queues(struct net_device *dev,
 	struct bcm_sysport_tx_ring *ring;
 	unsigned int num_tx_queues;
 	unsigned int q, qp, port;
+	u32 reg;
 
 	port = dp->index;
 
@@ -2390,6 +2399,14 @@ static int bcm_sysport_unmap_queues(struct net_device *dev,
 		qp = ring->switch_queue;
 		if (qp + port * num_tx_queues < ARRAY_SIZE(priv->ring_map))
 			priv->ring_map[qp + port * num_tx_queues] = NULL;
+
+		if (netif_running(dev)) {
+			reg = tdma_readl(priv, TDMA_DESC_RING_MAPPING(q));
+			reg &= ~(RING_QID_MASK |
+				 RING_PORT_ID_MASK << RING_PORT_ID_SHIFT);
+			reg |= RING_IGNORE_STATUS;
+			tdma_writel(priv, reg, TDMA_DESC_RING_MAPPING(q));
+		}
 	}
 
 	return 0;
-- 
2.34.1