1 --- a/drivers/net/dsa/mt7530.c
2 +++ b/drivers/net/dsa/mt7530.c
3 @@ -627,7 +627,7 @@ mt7530_setup(struct dsa_switch *ds)
5 /* Enable Port 6 only; P5 as GMAC5 which currently is not supported */
6 val = mt7530_read(priv, MT7530_MHWTRAP);
7 - val &= ~MHWTRAP_P6_DIS & ~MHWTRAP_PHY_ACCESS;
8 + val &= ~MHWTRAP_P5_DIS & ~MHWTRAP_P6_DIS & ~MHWTRAP_PHY_ACCESS;
10 if (!dsa_is_cpu_port(ds, 5)) {
11 val |= MHWTRAP_P5_DIS;
12 @@ -735,6 +735,9 @@ static int
13 mt7530_cpu_port_enable(struct mt7530_priv *priv,
19 /* Enable Mediatek header mode on the cpu port */
20 mt7530_write(priv, MT7530_PVC_P(port),
22 @@ -751,8 +754,12 @@ mt7530_cpu_port_enable(struct mt7530_pri
23 /* CPU port gets connected to all user ports of
26 + for (i = 0; i < MT7530_NUM_PORTS; i++)
27 + if ((priv->ds->enabled_port_mask & BIT(i)) &&
28 + (dsa_port_upstream_port(priv->ds, i) == port))
29 + port_mask |= BIT(i);
30 mt7530_write(priv, MT7530_PCR_P(port),
31 - PCR_MATRIX(priv->ds->enabled_port_mask));
32 + PCR_MATRIX(port_mask));
36 @@ -762,6 +769,7 @@ mt7530_port_enable(struct dsa_switch *ds
37 struct phy_device *phy)
39 struct mt7530_priv *priv = ds->priv;
40 + u8 upstream = dsa_port_upstream_port(ds, port);
42 mutex_lock(&priv->reg_mutex);
44 @@ -772,7 +780,7 @@ mt7530_port_enable(struct dsa_switch *ds
45 * restore the port matrix if the port is the member of a certain
48 - priv->ports[port].pm |= PCR_MATRIX(BIT(MT7530_CPU_PORT));
49 + priv->ports[port].pm |= PCR_MATRIX(BIT(upstream));
50 priv->ports[port].enable = true;
51 mt7530_rmw(priv, MT7530_PCR_P(port), PCR_MATRIX_MASK,
52 priv->ports[port].pm);
53 @@ -835,7 +843,8 @@ mt7530_port_bridge_join(struct dsa_switc
54 struct net_device *bridge)
56 struct mt7530_priv *priv = ds->priv;
57 - u32 port_bitmap = BIT(MT7530_CPU_PORT);
58 + u8 upstream = dsa_port_upstream_port(ds, port);
59 + u32 port_bitmap = BIT(upstream);
62 mutex_lock(&priv->reg_mutex);
63 @@ -873,6 +882,7 @@ static void
64 mt7530_port_bridge_leave(struct dsa_switch *ds, int port)
66 struct mt7530_priv *priv = ds->priv;
67 + u8 upstream = dsa_port_upstream_port(ds, port);
70 mutex_lock(&priv->reg_mutex);
71 @@ -898,8 +908,8 @@ mt7530_port_bridge_leave(struct dsa_swit
72 priv->bridge_dev[port] = NULL;
73 if (priv->ports[port].enable)
74 mt7530_rmw(priv, MT7530_PCR_P(port), PCR_MATRIX_MASK,
75 - PCR_MATRIX(BIT(MT7530_CPU_PORT)));
76 - priv->ports[port].pm = PCR_MATRIX(BIT(MT7530_CPU_PORT));
77 + PCR_MATRIX(BIT(upstream)));
78 + priv->ports[port].pm = PCR_MATRIX(BIT(upstream));
80 mutex_unlock(&priv->reg_mutex);