1 files changed, 71 insertions, 19 deletions

diff --git a/drivers/net/dsa/b53/b53_common.c b/drivers/net/dsa/b53/b53_common.c
index 0e4bbdcc614f..c76892ac4e69 100644
--- a/drivers/net/dsa/b53/b53_common.c
+++ b/drivers/net/dsa/b53/b53_common.c
@@ -344,7 +344,8 @@ static void b53_set_forwarding(struct b53_device *dev, int enable)
         b53_write8(dev, B53_CTRL_PAGE, B53_SWITCH_CTRL, mgmt);
 }
 
-static void b53_enable_vlan(struct b53_device *dev, bool enable)
+static void b53_enable_vlan(struct b53_device *dev, bool enable,
+                            bool enable_filtering)
 {
         u8 mgmt, vc0, vc1, vc4 = 0, vc5;
 
@@ -369,8 +370,13 @@ static void b53_enable_vlan(struct b53_device *dev, bool enable)
                 vc0 |= VC0_VLAN_EN | VC0_VID_CHK_EN | VC0_VID_HASH_VID;
                 vc1 |= VC1_RX_MCST_UNTAG_EN | VC1_RX_MCST_FWD_EN;
                 vc4 &= ~VC4_ING_VID_CHECK_MASK;
-                vc4 |= VC4_ING_VID_VIO_DROP << VC4_ING_VID_CHECK_S;
-                vc5 |= VC5_DROP_VTABLE_MISS;
+                if (enable_filtering) {
+                        vc4 |= VC4_ING_VID_VIO_DROP << VC4_ING_VID_CHECK_S;
+                        vc5 |= VC5_DROP_VTABLE_MISS;
+                } else {
+                        vc4 |= VC4_ING_VID_VIO_FWD << VC4_ING_VID_CHECK_S;
+                        vc5 &= ~VC5_DROP_VTABLE_MISS;
+                }
 
                 if (is5325(dev))
                         vc0 &= ~VC0_RESERVED_1;
@@ -420,6 +426,9 @@ static void b53_enable_vlan(struct b53_device *dev, bool enable)
         }
 
         b53_write8(dev, B53_CTRL_PAGE, B53_SWITCH_MODE, mgmt);
+
+        dev->vlan_enabled = enable;
+        dev->vlan_filtering_enabled = enable_filtering;
 }
 
 static int b53_set_jumbo(struct b53_device *dev, bool enable, bool allow_10_100)
@@ -632,25 +641,35 @@ static void b53_enable_mib(struct b53_device *dev)
         b53_write8(dev, B53_MGMT_PAGE, B53_GLOBAL_CONFIG, gc);
 }
 
+static u16 b53_default_pvid(struct b53_device *dev)
+{
+        if (is5325(dev) || is5365(dev))
+                return 1;
+        else
+                return 0;
+}
+
 int b53_configure_vlan(struct dsa_switch *ds)
 {
         struct b53_device *dev = ds->priv;
         struct b53_vlan vl = { 0 };
-        int i;
+        int i, def_vid;
+
+        def_vid = b53_default_pvid(dev);
 
         /* clear all vlan entries */
         if (is5325(dev) || is5365(dev)) {
-                for (i = 1; i < dev->num_vlans; i++)
+                for (i = def_vid; i < dev->num_vlans; i++)
                         b53_set_vlan_entry(dev, i, &vl);
         } else {
                 b53_do_vlan_op(dev, VTA_CMD_CLEAR);
         }
 
-        b53_enable_vlan(dev, false);
+        b53_enable_vlan(dev, false, dev->vlan_filtering_enabled);
 
         b53_for_each_port(dev, i)
                 b53_write16(dev, B53_VLAN_PAGE,
-                            B53_VLAN_PORT_DEF_TAG(i), 1);
+                            B53_VLAN_PORT_DEF_TAG(i), def_vid);
 
         if (!is5325(dev) && !is5365(dev))
                 b53_set_jumbo(dev, dev->enable_jumbo, false);
@@ -1255,6 +1274,46 @@ EXPORT_SYMBOL(b53_phylink_mac_link_up);
 
 int b53_vlan_filtering(struct dsa_switch *ds, int port, bool vlan_filtering)
 {
+        struct b53_device *dev = ds->priv;
+        struct net_device *bridge_dev;
+        unsigned int i;
+        u16 pvid, new_pvid;
+
+        /* Handle the case were multiple bridges span the same switch device
+         * and one of them has a different setting than what is being requested
+         * which would be breaking filtering semantics for any of the other
+         * bridge devices.
+         */
+        b53_for_each_port(dev, i) {
+                bridge_dev = dsa_to_port(ds, i)->bridge_dev;
+                if (bridge_dev &&
+                    bridge_dev != dsa_to_port(ds, port)->bridge_dev &&
+                    br_vlan_enabled(bridge_dev) != vlan_filtering) {
+                        netdev_err(bridge_dev,
+                                   "VLAN filtering is global to the switch!\n");
+                        return -EINVAL;
+                }
+        }
+
+        b53_read16(dev, B53_VLAN_PAGE, B53_VLAN_PORT_DEF_TAG(port), &pvid);
+        new_pvid = pvid;
+        if (dev->vlan_filtering_enabled && !vlan_filtering) {
+                /* Filtering is currently enabled, use the default PVID since
+                 * the bridge does not expect tagging anymore
+                 */
+                dev->ports[port].pvid = pvid;
+                new_pvid = b53_default_pvid(dev);
+        } else if (!dev->vlan_filtering_enabled && vlan_filtering) {
+                /* Filtering is currently disabled, restore the previous PVID */
+                new_pvid = dev->ports[port].pvid;
+        }
+
+        if (pvid != new_pvid)
+                b53_write16(dev, B53_VLAN_PAGE, B53_VLAN_PORT_DEF_TAG(port),
+                            new_pvid);
+
+        b53_enable_vlan(dev, dev->vlan_enabled, vlan_filtering);
+
         return 0;
 }
 EXPORT_SYMBOL(b53_vlan_filtering);
@@ -1270,7 +1329,7 @@ int b53_vlan_prepare(struct dsa_switch *ds, int port,
         if (vlan->vid_end > dev->num_vlans)
                 return -ERANGE;
 
-        b53_enable_vlan(dev, true);
+        b53_enable_vlan(dev, true, dev->vlan_filtering_enabled);
 
         return 0;
 }
@@ -1300,7 +1359,7 @@ void b53_vlan_add(struct dsa_switch *ds, int port,
                 b53_fast_age_vlan(dev, vid);
         }
 
-        if (pvid) {
+        if (pvid && !dsa_is_cpu_port(ds, port)) {
                 b53_write16(dev, B53_VLAN_PAGE, B53_VLAN_PORT_DEF_TAG(port),
                             vlan->vid_end);
                 b53_fast_age_vlan(dev, vid);
@@ -1326,12 +1385,8 @@ int b53_vlan_del(struct dsa_switch *ds, int port,
 
                 vl->members &= ~BIT(port);
 
-                if (pvid == vid) {
-                        if (is5325(dev) || is5365(dev))
-                                pvid = 1;
-                        else
-                                pvid = 0;
-                }
+                if (pvid == vid)
+                        pvid = b53_default_pvid(dev);
 
                 if (untagged && !dsa_is_cpu_port(ds, port))
                         vl->untag &= ~(BIT(port));
@@ -1644,10 +1699,7 @@ void b53_br_leave(struct dsa_switch *ds, int port, struct net_device *br)
         b53_write16(dev, B53_PVLAN_PAGE, B53_PVLAN_PORT_MASK(port), pvlan);
         dev->ports[port].vlan_ctl_mask = pvlan;
 
-        if (is5325(dev) || is5365(dev))
-                pvid = 1;
-        else
-                pvid = 0;
+        pvid = b53_default_pvid(dev);
 
         /* Make this port join all VLANs without VLAN entries */
         if (is58xx(dev)) {