[PATCH net-next 26/30] net: dsa: mt7530: properly set MT7530_CPU_PORT

[arinc9.unal@gmail.com]

From: Arınç ÜNAL <arinc.unal@arinc9.com>

The MT7530_CPU_PORT bits represent the CPU port to trap frames to for the
MT7530 switch. There are two issues with the current way of setting these
bits. ID_MT7530 which is for the standalone MT7530 switch is not included.
When multiple CPU ports are being used, the trapped frames won't be
received when the DSA conduit interface, which the frames are supposed to
be trapped to, is down because it's not affine to any user port. This
requires the DSA conduit interface to be manually set up for the trapped
frames to be received.

Address these issues by implementing ds->ops->master_state_change() on this
subdriver and setting the MT7530_CPU_PORT bits there. Introduce the
active_cpu_ports field to store the information of active CPU ports.
Correct the macros, MT7530_CPU_PORT is bits 4 through 6 of the register.

Any frames set for trapping to CPU port will be trapped to the numerically
smallest CPU port which is affine to the DSA conduit interface that is set
up. To make the understatement obvious, the frames won't necessarily be
trapped to the CPU port the user port, which these frames are received
from, is affine to. This operation is only there to make sure the trapped
frames always reach the CPU.

Tested-by: Arınç ÜNAL <arinc.unal@arinc9.com>
Co-developed-by: Vladimir Oltean <olteanv@gmail.com>
Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
Signed-off-by: Arınç ÜNAL <arinc.unal@arinc9.com>
---
 drivers/net/dsa/mt7530.c | 33 ++++++++++++++++++++++++++++-----
 drivers/net/dsa/mt7530.h |  6 ++++--
 2 files changed, 32 insertions(+), 7 deletions(-)

diff --git a/drivers/net/dsa/mt7530.c b/drivers/net/dsa/mt7530.c
index 0b513e3628fe..cd16911fcb01 100644
--- a/drivers/net/dsa/mt7530.c
+++ b/drivers/net/dsa/mt7530.c
@@ -958,11 +958,6 @@ mt753x_cpu_port_enable(struct dsa_switch *ds, int port)
 	mt7530_set(priv, MT753X_MFC, MT753X_BC_FFP(BIT(port)) |
 		   MT753X_UNM_FFP(BIT(port)) | MT753X_UNU_FFP(BIT(port)));
 
-	/* Set CPU port number */
-	if (priv->id == ID_MT7621)
-		mt7530_rmw(priv, MT753X_MFC, MT7530_CPU_MASK, MT7530_CPU_EN |
-			   MT7530_CPU_PORT(port));
-
 	/* Add the CPU port to the CPU port bitmap for MT7531 and the switch on
 	 * the MT7988 SoC. Any frames set for trapping to CPU port will be
 	 * trapped to the CPU port the user port is affine to.
@@ -2947,6 +2942,33 @@ static int mt753x_set_mac_eee(struct dsa_switch *ds, int port,
 	return 0;
 }
 
+static void
+mt753x_master_state_change(struct dsa_switch *ds,
+			   const struct net_device *master,
+			   bool operational)
+{
+	struct mt7530_priv *priv = ds->priv;
+	struct dsa_port *cpu_dp = master->dsa_ptr;
+
+	/* Set the CPU port to trap frames to for MT7530. There can be only one
+	 * CPU port due to MT7530_CPU_PORT having only 3 bits. Any frames set
+	 * for trapping to CPU port will be trapped to the numerically smallest
+	 * CPU port which is affine to the DSA conduit interface that is set up.
+	 */
+	if (priv->id != ID_MT7530 && priv->id != ID_MT7621)
+		return;
+
+	if (operational)
+		priv->active_cpu_ports |= BIT(cpu_dp->index);
+	else
+		priv->active_cpu_ports &= ~BIT(cpu_dp->index);
+
+	if (priv->active_cpu_ports)
+		mt7530_rmw(priv, MT753X_MFC, MT7530_CPU_EN |
+			   MT7530_CPU_PORT_MASK, MT7530_CPU_EN |
+			   MT7530_CPU_PORT(__ffs(priv->active_cpu_ports)));
+}
+
 static int mt7988_setup(struct dsa_switch *ds)
 {
 	struct mt7530_priv *priv = ds->priv;
@@ -2996,6 +3018,7 @@ const struct dsa_switch_ops mt7530_switch_ops = {
 	.phylink_mac_link_up	= mt753x_phylink_mac_link_up,
 	.get_mac_eee		= mt753x_get_mac_eee,
 	.set_mac_eee		= mt753x_set_mac_eee,
+	.master_state_change	= mt753x_master_state_change,
 };
 EXPORT_SYMBOL_GPL(mt7530_switch_ops);
 
diff --git a/drivers/net/dsa/mt7530.h b/drivers/net/dsa/mt7530.h
index fd2a2f726b8a..52e5d71a04d3 100644
--- a/drivers/net/dsa/mt7530.h
+++ b/drivers/net/dsa/mt7530.h
@@ -41,8 +41,8 @@ enum mt753x_id {
 #define  MT753X_UNU_FFP(x)		(((x) & 0xff) << 8)
 #define  MT753X_UNU_FFP_MASK		MT753X_UNU_FFP(~0)
 #define  MT7530_CPU_EN			BIT(7)
-#define  MT7530_CPU_PORT(x)		((x) << 4)
-#define  MT7530_CPU_MASK		(0xf << 4)
+#define  MT7530_CPU_PORT(x)		(((x) & 0x7) << 4)
+#define  MT7530_CPU_PORT_MASK		MT7530_CPU_PORT(~0)
 #define  MT7530_MIRROR_EN		BIT(3)
 #define  MT7530_MIRROR_PORT(x)		((x) & 0x7)
 #define  MT7530_MIRROR_MASK		0x7
@@ -753,6 +753,7 @@ struct mt753x_info {
  * @irq_domain:		IRQ domain of the switch irq_chip
  * @irq_enable:		IRQ enable bits, synced to SYS_INT_EN
  * @create_sgmii:	Pointer to function creating SGMII PCS instance(s)
+ * @active_cpu_ports:	Holding the active CPU ports
  */
 struct mt7530_priv {
 	struct device		*dev;
@@ -780,6 +781,7 @@ struct mt7530_priv {
 	struct irq_domain *irq_domain;
 	u32 irq_enable;
 	int (*create_sgmii)(struct mt7530_priv *priv);
+	unsigned long active_cpu_ports;
 };
 
 struct mt7530_hw_vlan_entry {
-- 
2.39.2