[PATCH net-next v1 4/6] net: dsa: mt7628: add VLAN filtering support

[Joris Vaisvila <joey@tinyisr.com>]

Add support for VLAN filtering to the MT7628's built-in switch.

The only way to control forwarding on this switch is through VLANs.
Bridges and VLANs are represented by the same hardware, programmed
differently.

VLAN membership for tag_8021q and user-requested VLANs is stored in
software, but membership is only programmed for one type of VLAN, based
on port VLAN filtering state. If VLAN filtering is enabled on a port, it
is programmed only into "VLAN_AWARE" VLANs and double-tag is disabled.
Otherwise it is programmed into "VLAN_UNAWARE" VLANs and double-tag is
enabled to simulate VLAN-unaware bridge behaviour.

The primary limitation is that a VID must not be common between separate
bridges, as that would allow forwarding between bridge ports of separate
bridges. This means we can't use tag_8021q VIDs or VLANs used on other
bridges. This is checked in mt7628_port_vlan_add.

Signed-off-by: Joris Vaisvila <joey@tinyisr.com>
---
 drivers/net/dsa/mt7628.c | 122 ++++++++++++++++++++++++++++++++++++---
 1 file changed, 114 insertions(+), 8 deletions(-)

diff --git a/drivers/net/dsa/mt7628.c b/drivers/net/dsa/mt7628.c
index 7eabfad3e114..629dbf968a17 100644
--- a/drivers/net/dsa/mt7628.c
+++ b/drivers/net/dsa/mt7628.c
@@ -174,9 +174,10 @@ struct mt7628_esw {
 	struct reset_control *rst_esw;
 	struct regmap *regmap;
 	struct dsa_switch *ds;
-	u16 tag_8021q_pvid[MT7628_ESW_NUM_PORTS];
+	u16 pvid[MT7628_VLAN_TYPE_NUM][MT7628_ESW_NUM_PORTS];
 	struct mt7628_vlan vlans[MT7628_NUM_VLANS];
 	struct device *dev;
+	u8 vlan_filtering;
 };
 
 static int mt7628_mii_read(struct mii_bus *bus, int port, int regnum)
@@ -442,10 +443,16 @@ static int mt7628_port_join_vlan_block(struct dsa_switch *ds, int port, u16 vid,
 		return -ENOSPC;
 
 	vlan->members |= BIT(port);
+
 	if (flags & BRIDGE_VLAN_INFO_PVID)
-		esw->tag_8021q_pvid[port] = vid;
+		esw->pvid[type][port] = vid;
+	else if (esw->pvid[type][port] == vid)
+		esw->pvid[type][port] = 0;
+
 	if (flags & BRIDGE_VLAN_INFO_UNTAGGED)
 		vlan->untag |= BIT(port);
+	else
+		vlan->untag &= ~BIT(port);
 	return 0;
 }
 
@@ -458,8 +465,8 @@ static int mt7628_port_leave_vlan_block(struct dsa_switch *ds, int port,
 	if (!vlan)
 		return -ENOENT;
 
-	if (esw->tag_8021q_pvid[port] == vid)
-		esw->tag_8021q_pvid[port] = 0;
+	if (esw->pvid[type][port] == vid)
+		esw->pvid[type][port] = 0;
 	vlan->members &= ~BIT(port);
 	vlan->untag &= ~BIT(port);
 	/*
@@ -473,18 +480,42 @@ static int mt7628_port_leave_vlan_block(struct dsa_switch *ds, int port,
 static void mt7628_vlan_sync(struct dsa_switch *ds)
 {
 	struct mt7628_esw *esw = ds->priv;
+	struct dsa_port *dp;
 	int i;
 
 	for (i = 0; i < MT7628_NUM_VLANS; i++) {
 		struct mt7628_vlan *vlan = &esw->vlans[i];
+		u8 member_mask;
+
+		if (vlan->type == MT7628_VLAN_TYPE_AWARE)
+			member_mask = esw->vlan_filtering;
+		else
+			member_mask = ~esw->vlan_filtering;
+		member_mask |= MT7628_ESW_PORTS_CPU;
+		/*
+		 * Put VLAN filtering ports only into VLAN aware VLANs and
+		 * non VLAN filtering ports into VLAN unaware VLANs.
+		 *
+		 * CPU may not be removed from any VLAN, as VLAN filtering
+		 * applies only to user ports.
+		 */
 
-		mt7628_esw_set_vmsc(esw, i, vlan->members);
 		mt7628_esw_set_vlan_id(esw, i, vlan->vid);
-		mt7628_esw_set_vub(esw, i, vlan->untag);
+		mt7628_esw_set_vmsc(esw, i, vlan->members & member_mask);
+		mt7628_esw_set_vub(esw, i, vlan->untag & member_mask);
+
 	}
 
-	for (i = 0; i < ds->num_ports; i++)
-		mt7628_esw_set_pvid(esw, i, esw->tag_8021q_pvid[i]);
+	dsa_switch_for_each_user_port(dp, ds) {
+		unsigned int type = BIT(dp->index) & esw->vlan_filtering ?
+		    MT7628_VLAN_TYPE_AWARE : MT7628_VLAN_TYPE_UNAWARE;
+		mt7628_esw_set_pvid(esw, dp->index, esw->pvid[type][dp->index]);
+	}
+	regmap_update_bits(esw->regmap, MT7628_ESW_REG_SGC2,
+			   MT7628_ESW_SGC2_DOUBLE_TAG_EN,
+			   FIELD_PREP(MT7628_ESW_SGC2_DOUBLE_TAG_EN,
+				      MT7628_ESW_PORTS_NOCPU &
+				      ~esw->vlan_filtering));
 }
 
 static int mt7628_setup(struct dsa_switch *ds)
@@ -598,6 +629,78 @@ static int mt7628_dsa_8021q_vlan_del(struct dsa_switch *ds, int port, u16 vid)
 	return 0;
 }
 
+static int mt7628_port_vlan_filtering(struct dsa_switch *ds, int port,
+				      bool vlan_filtering,
+				      struct netlink_ext_ack *extack)
+{
+	struct mt7628_esw *esw = ds->priv;
+
+	if (vlan_filtering)
+		esw->vlan_filtering |= BIT(port);
+	else
+		esw->vlan_filtering &= ~BIT(port);
+	mt7628_vlan_sync(ds);
+	return 0;
+}
+
+static int mt7628_port_vlan_add(struct dsa_switch *ds, int port,
+				const struct switchdev_obj_port_vlan *vlan,
+				struct netlink_ext_ack *extack)
+{
+	struct mt7628_vlan *vlan_block;
+	struct dsa_port *other_dp;
+	struct dsa_port *dp;
+	int ret;
+
+	if (vid_is_dsa_8021q(vlan->vid)) {
+		NL_SET_ERR_MSG_MOD(extack,
+				   "Range 3072-4095 reserved for dsa_8021q operation");
+		return -EBUSY;
+	}
+
+	vlan_block =
+	    mt7628_find_vlan_block(ds, vlan->vid, MT7628_VLAN_TYPE_AWARE);
+	dp = dsa_to_port(ds, port);
+	/*
+	 * CPU port can be added to any VLANs, but user ports need to ensure,
+	 * that if the VLAN already exists it's not used by a bridge we're not
+	 * a member of, because VLANs are the only form of forwarding control
+	 * we have on this switch.
+	 */
+	if (vlan_block && !dsa_port_is_cpu(dp)) {
+		dsa_switch_for_each_user_port(other_dp, ds) {
+			if (other_dp == dp)
+				continue;
+			if (other_dp->bridge == dp->bridge)
+				continue;
+			if (!(vlan_block->members & BIT(other_dp->index)))
+				continue;
+			NL_SET_ERR_MSG_MOD(extack,
+					   "VLAN ID used on another bridge");
+			return -EBUSY;
+		}
+	}
+	ret =
+	    mt7628_port_join_vlan_block(ds, port, vlan->vid,
+					MT7628_VLAN_TYPE_AWARE, vlan->flags);
+	if (ret)
+		return ret;
+
+	mt7628_vlan_sync(ds);
+	return 0;
+}
+
+static int mt7628_port_vlan_del(struct dsa_switch *ds, int port,
+				const struct switchdev_obj_port_vlan *vlan)
+{
+	int ret = mt7628_port_leave_vlan_block(ds, port, vlan->vid,
+					       MT7628_VLAN_TYPE_AWARE);
+	if (ret)
+		return ret;
+	mt7628_vlan_sync(ds);
+	return 0;
+}
+
 static void mt7628_teardown(struct dsa_switch *ds)
 {
 	rtnl_lock();
@@ -651,6 +754,9 @@ static const struct dsa_switch_ops mt7628_switch_ops = {
 	.port_bridge_join = dsa_tag_8021q_bridge_join,
 	.port_bridge_leave = dsa_tag_8021q_bridge_leave,
 	.port_stp_state_set = mt7628_stp_state_set,
+	.port_vlan_filtering = mt7628_port_vlan_filtering,
+	.port_vlan_add = mt7628_port_vlan_add,
+	.port_vlan_del = mt7628_port_vlan_del,
 };
 
 static int mt7628_probe(struct platform_device *pdev)
-- 
2.55.0