@@ -14,6 +14,103 @@
 #include "ksz_common.h"
 #include "lan937x_dev.h"
 
+static int lan937x_wait_vlan_ctrl_ready(struct ksz_device *dev)
+{
+	unsigned int val;
+
+	return regmap_read_poll_timeout(dev->regmap[0], REG_SW_VLAN_CTRL,
+					val, !(val & VLAN_START), 10, 1000);
+}
+
+static int lan937x_get_vlan_table(struct ksz_device *dev, u16 vid,
+				  u32 *vlan_table)
+{
+	int rc;
+
+	mutex_lock(&dev->vlan_mutex);
+
+	rc = ksz_write16(dev, REG_SW_VLAN_ENTRY_INDEX__2, vid & VLAN_INDEX_M);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_write8(dev, REG_SW_VLAN_CTRL, VLAN_READ | VLAN_START);
+	if (rc < 0)
+		goto exit;
+
+	/* wait to be cleared */
+	rc = lan937x_wait_vlan_ctrl_ready(dev);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_read32(dev, REG_SW_VLAN_ENTRY__4, &vlan_table[0]);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_read32(dev, REG_SW_VLAN_ENTRY_UNTAG__4, &vlan_table[1]);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_read32(dev, REG_SW_VLAN_ENTRY_PORTS__4, &vlan_table[2]);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_write8(dev, REG_SW_VLAN_CTRL, 0);
+	if (rc < 0)
+		goto exit;
+
+exit:
+	mutex_unlock(&dev->vlan_mutex);
+
+	return rc;
+}
+
+static int lan937x_set_vlan_table(struct ksz_device *dev, u16 vid,
+				  u32 *vlan_table)
+{
+	int rc;
+
+	mutex_lock(&dev->vlan_mutex);
+
+	rc = ksz_write32(dev, REG_SW_VLAN_ENTRY__4, vlan_table[0]);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_write32(dev, REG_SW_VLAN_ENTRY_UNTAG__4, vlan_table[1]);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_write32(dev, REG_SW_VLAN_ENTRY_PORTS__4, vlan_table[2]);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_write16(dev, REG_SW_VLAN_ENTRY_INDEX__2, vid & VLAN_INDEX_M);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_write8(dev, REG_SW_VLAN_CTRL, VLAN_START | VLAN_WRITE);
+	if (rc < 0)
+		goto exit;
+
+	/* wait to be cleared */
+	rc = lan937x_wait_vlan_ctrl_ready(dev);
+	if (rc < 0)
+		goto exit;
+
+	rc = ksz_write8(dev, REG_SW_VLAN_CTRL, 0);
+	if (rc < 0)
+		goto exit;
+
+	/* update vlan cache table */
+	dev->vlan_cache[vid].table[0] = vlan_table[0];
+	dev->vlan_cache[vid].table[1] = vlan_table[1];
+	dev->vlan_cache[vid].table[2] = vlan_table[2];
+
+exit:
+	mutex_unlock(&dev->vlan_mutex);
+
+	return rc;
+}
+
 static int lan937x_read_table(struct ksz_device *dev, u32 *table)
 {
 	int rc;

@@ -190,6 +287,120 @@ static void lan937x_port_stp_state_set(struct dsa_switch *ds, int port,
 	mutex_unlock(&dev->dev_mutex);
 }
 
+static int lan937x_port_vlan_filtering(struct dsa_switch *ds, int port,
+				       bool flag,
+				       struct netlink_ext_ack *extack)
+{
+	struct ksz_device *dev = ds->priv;
+	int rc;
+
+	if (flag) {
+		rc = lan937x_port_cfg(dev, port, REG_PORT_LUE_CTRL,
+				      PORT_VLAN_LOOKUP_VID_0, true);
+		if (rc < 0)
+			return rc;
+
+		rc = lan937x_cfg(dev, REG_SW_LUE_CTRL_0, SW_VLAN_ENABLE, true);
+	} else {
+		rc = lan937x_cfg(dev, REG_SW_LUE_CTRL_0, SW_VLAN_ENABLE, false);
+		if (rc < 0)
+			return rc;
+
+		rc = lan937x_port_cfg(dev, port, REG_PORT_LUE_CTRL,
+				      PORT_VLAN_LOOKUP_VID_0, false);
+	}
+
+	return rc;
+}
+
+static int lan937x_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;
+	struct ksz_device *dev = ds->priv;
+	u32 vlan_table[3];
+	int rc;
+
+	rc = lan937x_get_vlan_table(dev, vlan->vid, vlan_table);
+	if (rc < 0) {
+		dev_err(dev->dev, "Failed to get vlan table\n");
+		return rc;
+	}
+
+	vlan_table[0] = VLAN_VALID | (vlan->vid & VLAN_FID_M);
+
+	/* set/clear switch port when updating vlan table registers */
+	if (untagged)
+		vlan_table[1] |= BIT(port);
+	else
+		vlan_table[1] &= ~BIT(port);
+	vlan_table[1] &= ~(BIT(dev->cpu_port));
+
+	vlan_table[2] |= BIT(port) | BIT(dev->cpu_port);
+
+	rc = lan937x_set_vlan_table(dev, vlan->vid, vlan_table);
+	if (rc < 0) {
+		dev_err(dev->dev, "Failed to set vlan table\n");
+		return rc;
+	}
+
+	/* change PVID */
+	if (vlan->flags & BRIDGE_VLAN_INFO_PVID) {
+		rc = lan937x_pwrite16(dev, port, REG_PORT_DEFAULT_VID, vlan->vid);
+
+		if (rc < 0) {
+			dev_err(dev->dev, "Failed to set pvid\n");
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+static int lan937x_port_vlan_del(struct dsa_switch *ds, int port,
+				 const struct switchdev_obj_port_vlan *vlan)
+{
+	bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
+	struct ksz_device *dev = ds->priv;
+	u32 vlan_table[3];
+	u16 pvid;
+	int rc;
+
+	lan937x_pread16(dev, port, REG_PORT_DEFAULT_VID, &pvid);
+	pvid &= 0xFFF;
+
+	rc = lan937x_get_vlan_table(dev, vlan->vid, vlan_table);
+
+	if (rc < 0) {
+		dev_err(dev->dev, "Failed to get vlan table\n");
+		return rc;
+	}
+	/* clear switch port number */
+	vlan_table[2] &= ~BIT(port);
+
+	if (pvid == vlan->vid)
+		pvid = 1;
+
+	if (untagged)
+		vlan_table[1] &= ~BIT(port);
+
+	rc = lan937x_set_vlan_table(dev, vlan->vid, vlan_table);
+	if (rc < 0) {
+		dev_err(dev->dev, "Failed to set vlan table\n");
+		return rc;
+	}
+
+	rc = lan937x_pwrite16(dev, port, REG_PORT_DEFAULT_VID, pvid);
+
+	if (rc < 0) {
+		dev_err(dev->dev, "Failed to set pvid\n");
+		return rc;
+	}
+
+	return 0;
+}
+
 static u8 lan937x_get_fid(u16 vid)
 {
 	if (vid > ALU_FID_SIZE)

@@ -955,6 +1166,9 @@ const struct dsa_switch_ops lan937x_switch_ops = {
 	.port_bridge_flags	= lan937x_port_bridge_flags,
 	.port_stp_state_set	= lan937x_port_stp_state_set,
 	.port_fast_age		= ksz_port_fast_age,
+	.port_vlan_filtering	= lan937x_port_vlan_filtering,
+	.port_vlan_add		= lan937x_port_vlan_add,
+	.port_vlan_del		= lan937x_port_vlan_del,
 	.port_fdb_dump		= lan937x_port_fdb_dump,
 	.port_fdb_add		= lan937x_port_fdb_add,
 	.port_fdb_del		= lan937x_port_fdb_del,