[PATCH net-next 5/6] net: dsa: vsc73xx: Add vlan filtering

[Pawel Dembicki <paweldembicki@gmail.com>]

This patch implement vlan filtering for vsc73xx driver.

After vlan filtering start, switch is reconfigured from QinQ to simple
vlan aware mode. It's required, because VSC73XX chips haven't support
for inner vlan tag filter.

Signed-off-by: Pawel Dembicki <paweldembicki@gmail.com>
---
 drivers/net/dsa/vitesse-vsc73xx-core.c | 101 +++++++++++++++++++++++++
 1 file changed, 101 insertions(+)

diff --git a/drivers/net/dsa/vitesse-vsc73xx-core.c b/drivers/net/dsa/vitesse-vsc73xx-core.c
index 427b6f964811..fcce47cf6da4 100644
--- a/drivers/net/dsa/vitesse-vsc73xx-core.c
+++ b/drivers/net/dsa/vitesse-vsc73xx-core.c
@@ -1239,6 +1239,30 @@ static int vsc73xx_port_set_double_vlan_aware(struct dsa_switch *ds, int port)
 	return ret;
 }
 
+static int
+vsc73xx_port_vlan_filtering(struct dsa_switch *ds, int port,
+			    bool vlan_filtering, struct netlink_ext_ack *extack)
+{
+	int ret, i;
+
+	if (vlan_filtering) {
+		vsc73xx_port_set_vlan_conf(ds, port, VSC73XX_VLAN_AWARE);
+	} else {
+		if (port == CPU_PORT)
+			vsc73xx_port_set_vlan_conf(ds, port, VSC73XX_DOUBLE_VLAN_CPU_AWARE);
+		else
+			vsc73xx_port_set_vlan_conf(ds, port, VSC73XX_DOUBLE_VLAN_AWARE);
+	}
+
+	for (i = 0; i <= 3072; i++) {
+		ret = vsc73xx_port_update_vlan_table(ds, port, i, 0);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
 static int vsc73xx_vlan_set_untagged(struct dsa_switch *ds, int port, u16 vid,
 				     bool port_vlan)
 {
@@ -1317,6 +1341,80 @@ static int vsc73xx_vlan_set_pvid(struct dsa_switch *ds, int port, u16 vid,
 	return 0;
 }
 
+static int vsc73xx_port_vlan_add(struct dsa_switch *ds, int port,
+				 const struct switchdev_obj_port_vlan *vlan,
+				 struct netlink_ext_ack *extack)
+{
+	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
+	bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
+	int ret;
+
+	/* Be sure to deny alterations to the configuration done by tag_8021q.
+	 */
+	if (vid_is_dsa_8021q(vlan->vid)) {
+		NL_SET_ERR_MSG_MOD(extack,
+				   "Range 3072-4095 reserved for dsa_8021q operation");
+		return -EBUSY;
+	}
+
+	if (untagged && port != CPU_PORT) {
+		ret = vsc73xx_vlan_set_untagged(ds, port, vlan->vid, true);
+		if (ret)
+			return ret;
+	}
+	if (pvid && port != CPU_PORT) {
+		ret = vsc73xx_vlan_set_pvid(ds, port, vlan->vid, true);
+		if (ret)
+			return ret;
+	}
+
+	ret = vsc73xx_port_update_vlan_table(ds, port, vlan->vid, 1);
+
+	return ret;
+}
+
+static int vsc73xx_port_vlan_del(struct dsa_switch *ds, int port,
+				 const struct switchdev_obj_port_vlan *vlan)
+{
+	struct vsc73xx *vsc = ds->priv;
+	u16 vlan_no;
+	int ret;
+	u32 val;
+
+	ret =
+	    vsc73xx_port_update_vlan_table(ds, port, vlan->vid, 0);
+	if (ret)
+		return ret;
+
+	vsc73xx_read(vsc, VSC73XX_BLOCK_MAC, port, VSC73XX_TXUPDCFG, &val);
+
+	if (val & VSC73XX_TXUPDCFG_TX_UNTAGGED_VID_ENA) {
+		vsc73xx_read(vsc, VSC73XX_BLOCK_MAC, port,
+			     VSC73XX_TXUPDCFG, &val);
+		vlan_no = (val & VSC73XX_TXUPDCFG_TX_UNTAGGED_VID) >>
+			  VSC73XX_TXUPDCFG_TX_UNTAGGED_VID_SHIFT;
+		if (vlan_no == vlan->vid) {
+			vsc73xx_update_bits(vsc, VSC73XX_BLOCK_MAC, port,
+					    VSC73XX_TXUPDCFG,
+					    VSC73XX_TXUPDCFG_TX_UNTAGGED_VID_ENA,
+					    0);
+			vsc73xx_update_bits(vsc, VSC73XX_BLOCK_MAC, port,
+					    VSC73XX_TXUPDCFG,
+					    VSC73XX_TXUPDCFG_TX_UNTAGGED_VID, 0);
+		}
+	}
+
+	vsc73xx_read(vsc, VSC73XX_BLOCK_MAC, port, VSC73XX_CAT_PORT_VLAN, &val);
+	vlan_no = val & VSC73XX_CAT_PORT_VLAN_VLAN_VID;
+	if (vlan_no && vlan_no == vlan->vid) {
+		vsc73xx_update_bits(vsc, VSC73XX_BLOCK_MAC, port,
+				    VSC73XX_CAT_PORT_VLAN,
+				    VSC73XX_CAT_PORT_VLAN_VLAN_VID, 0);
+	}
+
+	return 0;
+}
+
 static void vsc73xx_update_forwarding_map(struct vsc73xx *vsc)
 {
 	int i;
@@ -1537,6 +1635,9 @@ static const struct dsa_switch_ops vsc73xx_ds_ops = {
 	.port_change_mtu = vsc73xx_change_mtu,
 	.port_max_mtu = vsc73xx_get_max_mtu,
 	.port_stp_state_set = vsc73xx_port_stp_state_set,
+	.port_vlan_filtering = vsc73xx_port_vlan_filtering,
+	.port_vlan_add = vsc73xx_port_vlan_add,
+	.port_vlan_del = vsc73xx_port_vlan_del,
 	.tag_8021q_vlan_add = vsc73xx_tag_8021q_vlan_add,
 	.tag_8021q_vlan_del = vsc73xx_tag_8021q_vlan_del,
 };
-- 
2.34.1