net: dsa: sja1105: Use correct dsa_8021q VIDs for FDB commands
[linux-2.6-microblaze.git] / drivers / net / dsa / sja1105 / sja1105_main.c
index 8963b21..cadee76 100644 (file)
@@ -70,8 +70,7 @@ static int sja1105_init_mac_settings(struct sja1105_private *priv)
                /* Keep standard IFG of 12 bytes on egress. */
                .ifg = 0,
                /* Always put the MAC speed in automatic mode, where it can be
-                * retrieved from the PHY object through phylib and
-                * sja1105_adjust_port_config.
+                * adjusted at runtime by PHYLINK.
                 */
                .speed = SJA1105_SPEED_AUTO,
                /* No static correction for 1-step 1588 events */
@@ -81,7 +80,7 @@ static int sja1105_init_mac_settings(struct sja1105_private *priv)
                .maxage = 0xFF,
                /* Internal VLAN (pvid) to apply to untagged ingress */
                .vlanprio = 0,
-               .vlanid = 0,
+               .vlanid = 1,
                .ing_mirr = false,
                .egr_mirr = false,
                /* Don't drop traffic with other EtherType than ETH_P_IP */
@@ -116,7 +115,6 @@ static int sja1105_init_mac_settings(struct sja1105_private *priv)
        if (!table->entries)
                return -ENOMEM;
 
-       /* Override table based on phylib DT bindings */
        table->entry_count = SJA1105_NUM_PORTS;
 
        mac = table->entries;
@@ -157,7 +155,7 @@ static int sja1105_init_mii_settings(struct sja1105_private *priv,
        if (!table->entries)
                return -ENOMEM;
 
-       /* Override table based on phylib DT bindings */
+       /* Override table based on PHYLINK DT bindings */
        table->entry_count = SJA1105_MAX_XMII_PARAMS_COUNT;
 
        mii = table->entries;
@@ -205,6 +203,7 @@ static int sja1105_init_static_fdb(struct sja1105_private *priv)
 static int sja1105_init_l2_lookup_params(struct sja1105_private *priv)
 {
        struct sja1105_table *table;
+       u64 max_fdb_entries = SJA1105_MAX_L2_LOOKUP_COUNT / SJA1105_NUM_PORTS;
        struct sja1105_l2_lookup_params_entry default_l2_lookup_params = {
                /* Learned FDB entries are forgotten after 300 seconds */
                .maxage = SJA1105_AGEING_TIME_MS(300000),
@@ -212,6 +211,8 @@ static int sja1105_init_l2_lookup_params(struct sja1105_private *priv)
                .dyn_tbsz = SJA1105ET_FDB_BIN_SIZE,
                /* And the P/Q/R/S equivalent setting: */
                .start_dynspc = 0,
+               .maxaddrp = {max_fdb_entries, max_fdb_entries, max_fdb_entries,
+                            max_fdb_entries, max_fdb_entries, },
                /* 2^8 + 2^5 + 2^3 + 2^2 + 2^1 + 1 in Koopman notation */
                .poly = 0x97,
                /* This selects between Independent VLAN Learning (IVL) and
@@ -266,20 +267,15 @@ static int sja1105_init_static_vlan(struct sja1105_private *priv)
                .vmemb_port = 0,
                .vlan_bc = 0,
                .tag_port = 0,
-               .vlanid = 0,
+               .vlanid = 1,
        };
        int i;
 
        table = &priv->static_config.tables[BLK_IDX_VLAN_LOOKUP];
 
-       /* The static VLAN table will only contain the initial pvid of 0.
+       /* The static VLAN table will only contain the initial pvid of 1.
         * All other VLANs are to be configured through dynamic entries,
         * and kept in the static configuration table as backing memory.
-        * The pvid of 0 is sufficient to pass traffic while the ports are
-        * standalone and when vlan_filtering is disabled. When filtering
-        * gets enabled, the switchdev core sets up the VLAN ID 1 and sets
-        * it as the new pvid. Actually 'pvid 1' still comes up in 'bridge
-        * vlan' even when vlan_filtering is off, but it has no effect.
         */
        if (table->entry_count) {
                kfree(table->entries);
@@ -293,7 +289,7 @@ static int sja1105_init_static_vlan(struct sja1105_private *priv)
 
        table->entry_count = 1;
 
-       /* VLAN ID 0: all DT-defined ports are members; no restrictions on
+       /* VLAN 1: all DT-defined ports are members; no restrictions on
         * forwarding; always transmit priority-tagged frames as untagged.
         */
        for (i = 0; i < SJA1105_NUM_PORTS; i++) {
@@ -689,26 +685,18 @@ static int sja1105_parse_dt(struct sja1105_private *priv,
        return rc;
 }
 
-/* Convert back and forth MAC speed from Mbps to SJA1105 encoding */
+/* Convert link speed from SJA1105 to ethtool encoding */
 static int sja1105_speed[] = {
-       [SJA1105_SPEED_AUTO]     = 0,
-       [SJA1105_SPEED_10MBPS]   10,
-       [SJA1105_SPEED_100MBPS]  100,
-       [SJA1105_SPEED_1000MBPS] 1000,
+       [SJA1105_SPEED_AUTO]            = SPEED_UNKNOWN,
+       [SJA1105_SPEED_10MBPS]          = SPEED_10,
+       [SJA1105_SPEED_100MBPS]         = SPEED_100,
+       [SJA1105_SPEED_1000MBPS]        = SPEED_1000,
 };
 
-/* Set link speed and enable/disable traffic I/O in the MAC configuration
- * for a specific port.
- *
- * @speed_mbps: If 0, leave the speed unchanged, else adapt MAC to PHY speed.
- * @enabled: Manage Rx and Tx settings for this port. If false, overrides the
- *          settings from the STP state, but not persistently (does not
- *          overwrite the static MAC info for this port).
- */
+/* Set link speed in the MAC configuration for a specific port. */
 static int sja1105_adjust_port_config(struct sja1105_private *priv, int port,
-                                     int speed_mbps, bool enabled)
+                                     int speed_mbps)
 {
-       struct sja1105_mac_config_entry dyn_mac;
        struct sja1105_xmii_params_entry *mii;
        struct sja1105_mac_config_entry *mac;
        struct device *dev = priv->ds->dev;
@@ -716,21 +704,27 @@ static int sja1105_adjust_port_config(struct sja1105_private *priv, int port,
        sja1105_speed_t speed;
        int rc;
 
-       mii = priv->static_config.tables[BLK_IDX_XMII_PARAMS].entries;
+       /* On P/Q/R/S, one can read from the device via the MAC reconfiguration
+        * tables. On E/T, MAC reconfig tables are not readable, only writable.
+        * We have to *know* what the MAC looks like.  For the sake of keeping
+        * the code common, we'll use the static configuration tables as a
+        * reasonable approximation for both E/T and P/Q/R/S.
+        */
        mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
+       mii = priv->static_config.tables[BLK_IDX_XMII_PARAMS].entries;
 
        switch (speed_mbps) {
-       case 0:
+       case SPEED_UNKNOWN:
                /* No speed update requested */
                speed = SJA1105_SPEED_AUTO;
                break;
-       case 10:
+       case SPEED_10:
                speed = SJA1105_SPEED_10MBPS;
                break;
-       case 100:
+       case SPEED_100:
                speed = SJA1105_SPEED_100MBPS;
                break;
-       case 1000:
+       case SPEED_1000:
                speed = SJA1105_SPEED_1000MBPS;
                break;
        default:
@@ -738,26 +732,16 @@ static int sja1105_adjust_port_config(struct sja1105_private *priv, int port,
                return -EINVAL;
        }
 
-       /* If requested, overwrite SJA1105_SPEED_AUTO from the static MAC
-        * configuration table, since this will be used for the clocking setup,
-        * and we no longer need to store it in the static config (already told
-        * hardware we want auto during upload phase).
+       /* Overwrite SJA1105_SPEED_AUTO from the static MAC configuration
+        * table, since this will be used for the clocking setup, and we no
+        * longer need to store it in the static config (already told hardware
+        * we want auto during upload phase).
         */
        mac[port].speed = speed;
 
-       /* On P/Q/R/S, one can read from the device via the MAC reconfiguration
-        * tables. On E/T, MAC reconfig tables are not readable, only writable.
-        * We have to *know* what the MAC looks like.  For the sake of keeping
-        * the code common, we'll use the static configuration tables as a
-        * reasonable approximation for both E/T and P/Q/R/S.
-        */
-       dyn_mac = mac[port];
-       dyn_mac.ingress = enabled && mac[port].ingress;
-       dyn_mac.egress  = enabled && mac[port].egress;
-
        /* Write to the dynamic reconfiguration tables */
-       rc = sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG,
-                                         port, &dyn_mac, true);
+       rc = sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, port,
+                                         &mac[port], true);
        if (rc < 0) {
                dev_err(dev, "Failed to write MAC config: %d\n", rc);
                return rc;
@@ -769,9 +753,6 @@ static int sja1105_adjust_port_config(struct sja1105_private *priv, int port,
         * the clock setup does interrupt the clock signal for a certain time
         * which causes trouble for all PHYs relying on this signal.
         */
-       if (!enabled)
-               return 0;
-
        phy_mode = mii->xmii_mode[port];
        if (phy_mode != XMII_MODE_RGMII)
                return 0;
@@ -786,9 +767,24 @@ static void sja1105_mac_config(struct dsa_switch *ds, int port,
        struct sja1105_private *priv = ds->priv;
 
        if (!state->link)
-               sja1105_adjust_port_config(priv, port, 0, false);
-       else
-               sja1105_adjust_port_config(priv, port, state->speed, true);
+               return;
+
+       sja1105_adjust_port_config(priv, port, state->speed);
+}
+
+static void sja1105_mac_link_down(struct dsa_switch *ds, int port,
+                                 unsigned int mode,
+                                 phy_interface_t interface)
+{
+       sja1105_inhibit_tx(ds->priv, BIT(port), true);
+}
+
+static void sja1105_mac_link_up(struct dsa_switch *ds, int port,
+                               unsigned int mode,
+                               phy_interface_t interface,
+                               struct phy_device *phydev)
+{
+       sja1105_inhibit_tx(ds->priv, BIT(port), false);
 }
 
 static void sja1105_phylink_validate(struct dsa_switch *ds, int port,
@@ -820,6 +816,77 @@ static void sja1105_phylink_validate(struct dsa_switch *ds, int port,
                   __ETHTOOL_LINK_MODE_MASK_NBITS);
 }
 
+static int
+sja1105_find_static_fdb_entry(struct sja1105_private *priv, int port,
+                             const struct sja1105_l2_lookup_entry *requested)
+{
+       struct sja1105_l2_lookup_entry *l2_lookup;
+       struct sja1105_table *table;
+       int i;
+
+       table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];
+       l2_lookup = table->entries;
+
+       for (i = 0; i < table->entry_count; i++)
+               if (l2_lookup[i].macaddr == requested->macaddr &&
+                   l2_lookup[i].vlanid == requested->vlanid &&
+                   l2_lookup[i].destports & BIT(port))
+                       return i;
+
+       return -1;
+}
+
+/* We want FDB entries added statically through the bridge command to persist
+ * across switch resets, which are a common thing during normal SJA1105
+ * operation. So we have to back them up in the static configuration tables
+ * and hence apply them on next static config upload... yay!
+ */
+static int
+sja1105_static_fdb_change(struct sja1105_private *priv, int port,
+                         const struct sja1105_l2_lookup_entry *requested,
+                         bool keep)
+{
+       struct sja1105_l2_lookup_entry *l2_lookup;
+       struct sja1105_table *table;
+       int rc, match;
+
+       table = &priv->static_config.tables[BLK_IDX_L2_LOOKUP];
+
+       match = sja1105_find_static_fdb_entry(priv, port, requested);
+       if (match < 0) {
+               /* Can't delete a missing entry. */
+               if (!keep)
+                       return 0;
+
+               /* No match => new entry */
+               rc = sja1105_table_resize(table, table->entry_count + 1);
+               if (rc)
+                       return rc;
+
+               match = table->entry_count - 1;
+       }
+
+       /* Assign pointer after the resize (it may be new memory) */
+       l2_lookup = table->entries;
+
+       /* We have a match.
+        * If the job was to add this FDB entry, it's already done (mostly
+        * anyway, since the port forwarding mask may have changed, case in
+        * which we update it).
+        * Otherwise we have to delete it.
+        */
+       if (keep) {
+               l2_lookup[match] = *requested;
+               return 0;
+       }
+
+       /* To remove, the strategy is to overwrite the element with
+        * the last one, and then reduce the array size by 1
+        */
+       l2_lookup[match] = l2_lookup[table->entry_count - 1];
+       return sja1105_table_resize(table, table->entry_count - 1);
+}
+
 /* First-generation switches have a 4-way set associative TCAM that
  * holds the FDB entries. An FDB index spans from 0 to 1023 and is comprised of
  * a "bin" (grouping of 4 entries) and a "way" (an entry within a bin).
@@ -870,7 +937,7 @@ int sja1105et_fdb_add(struct dsa_switch *ds, int port,
        struct sja1105_private *priv = ds->priv;
        struct device *dev = ds->dev;
        int last_unused = -1;
-       int bin, way;
+       int bin, way, rc;
 
        bin = sja1105et_fdb_hash(priv, addr, vid);
 
@@ -914,9 +981,13 @@ int sja1105et_fdb_add(struct dsa_switch *ds, int port,
        }
        l2_lookup.index = sja1105et_fdb_index(bin, way);
 
-       return sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
-                                           l2_lookup.index, &l2_lookup,
-                                           true);
+       rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
+                                         l2_lookup.index, &l2_lookup,
+                                         true);
+       if (rc < 0)
+               return rc;
+
+       return sja1105_static_fdb_change(priv, port, &l2_lookup, true);
 }
 
 int sja1105et_fdb_del(struct dsa_switch *ds, int port,
@@ -924,7 +995,7 @@ int sja1105et_fdb_del(struct dsa_switch *ds, int port,
 {
        struct sja1105_l2_lookup_entry l2_lookup = {0};
        struct sja1105_private *priv = ds->priv;
-       int index, bin, way;
+       int index, bin, way, rc;
        bool keep;
 
        bin = sja1105et_fdb_hash(priv, addr, vid);
@@ -946,8 +1017,12 @@ int sja1105et_fdb_del(struct dsa_switch *ds, int port,
        else
                keep = false;
 
-       return sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
-                                           index, &l2_lookup, keep);
+       rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
+                                         index, &l2_lookup, keep);
+       if (rc < 0)
+               return rc;
+
+       return sja1105_static_fdb_change(priv, port, &l2_lookup, keep);
 }
 
 int sja1105pqrs_fdb_add(struct dsa_switch *ds, int port,
@@ -995,12 +1070,17 @@ int sja1105pqrs_fdb_add(struct dsa_switch *ds, int port,
                dev_err(ds->dev, "FDB is full, cannot add entry.\n");
                return -EINVAL;
        }
+       l2_lookup.lockeds = true;
        l2_lookup.index = i;
 
 skip_finding_an_index:
-       return sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
-                                           l2_lookup.index, &l2_lookup,
-                                           true);
+       rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
+                                         l2_lookup.index, &l2_lookup,
+                                         true);
+       if (rc < 0)
+               return rc;
+
+       return sja1105_static_fdb_change(priv, port, &l2_lookup, true);
 }
 
 int sja1105pqrs_fdb_del(struct dsa_switch *ds, int port,
@@ -1034,52 +1114,72 @@ int sja1105pqrs_fdb_del(struct dsa_switch *ds, int port,
        else
                keep = false;
 
-       return sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
-                                           l2_lookup.index, &l2_lookup, keep);
+       rc = sja1105_dynamic_config_write(priv, BLK_IDX_L2_LOOKUP,
+                                         l2_lookup.index, &l2_lookup, keep);
+       if (rc < 0)
+               return rc;
+
+       return sja1105_static_fdb_change(priv, port, &l2_lookup, keep);
 }
 
 static int sja1105_fdb_add(struct dsa_switch *ds, int port,
                           const unsigned char *addr, u16 vid)
 {
        struct sja1105_private *priv = ds->priv;
-       int rc;
+       u16 rx_vid, tx_vid;
+       int rc, i;
+
+       if (dsa_port_is_vlan_filtering(&ds->ports[port]))
+               return priv->info->fdb_add_cmd(ds, port, addr, vid);
 
        /* Since we make use of VLANs even when the bridge core doesn't tell us
         * to, translate these FDB entries into the correct dsa_8021q ones.
+        * The basic idea (also repeats for removal below) is:
+        * - Each of the other front-panel ports needs to be able to forward a
+        *   pvid-tagged (aka tagged with their rx_vid) frame that matches this
+        *   DMAC.
+        * - The CPU port (aka the tx_vid of this port) needs to be able to
+        *   send a frame matching this DMAC to the specified port.
+        * For a better picture see net/dsa/tag_8021q.c.
         */
-       if (!dsa_port_is_vlan_filtering(&ds->ports[port])) {
-               unsigned int upstream = dsa_upstream_port(priv->ds, port);
-               u16 tx_vid = dsa_8021q_tx_vid(ds, port);
-               u16 rx_vid = dsa_8021q_rx_vid(ds, port);
+       for (i = 0; i < SJA1105_NUM_PORTS; i++) {
+               if (i == port)
+                       continue;
+               if (i == dsa_upstream_port(priv->ds, port))
+                       continue;
 
-               rc = priv->info->fdb_add_cmd(ds, port, addr, tx_vid);
+               rx_vid = dsa_8021q_rx_vid(ds, i);
+               rc = priv->info->fdb_add_cmd(ds, port, addr, rx_vid);
                if (rc < 0)
                        return rc;
-               return priv->info->fdb_add_cmd(ds, upstream, addr, rx_vid);
        }
-       return priv->info->fdb_add_cmd(ds, port, addr, vid);
+       tx_vid = dsa_8021q_tx_vid(ds, port);
+       return priv->info->fdb_add_cmd(ds, port, addr, tx_vid);
 }
 
 static int sja1105_fdb_del(struct dsa_switch *ds, int port,
                           const unsigned char *addr, u16 vid)
 {
        struct sja1105_private *priv = ds->priv;
-       int rc;
+       u16 rx_vid, tx_vid;
+       int rc, i;
 
-       /* Since we make use of VLANs even when the bridge core doesn't tell us
-        * to, translate these FDB entries into the correct dsa_8021q ones.
-        */
-       if (!dsa_port_is_vlan_filtering(&ds->ports[port])) {
-               unsigned int upstream = dsa_upstream_port(priv->ds, port);
-               u16 tx_vid = dsa_8021q_tx_vid(ds, port);
-               u16 rx_vid = dsa_8021q_rx_vid(ds, port);
+       if (dsa_port_is_vlan_filtering(&ds->ports[port]))
+               return priv->info->fdb_del_cmd(ds, port, addr, vid);
+
+       for (i = 0; i < SJA1105_NUM_PORTS; i++) {
+               if (i == port)
+                       continue;
+               if (i == dsa_upstream_port(priv->ds, port))
+                       continue;
 
-               rc = priv->info->fdb_del_cmd(ds, port, addr, tx_vid);
+               rx_vid = dsa_8021q_rx_vid(ds, i);
+               rc = priv->info->fdb_del_cmd(ds, port, addr, rx_vid);
                if (rc < 0)
                        return rc;
-               return priv->info->fdb_del_cmd(ds, upstream, addr, rx_vid);
        }
-       return priv->info->fdb_del_cmd(ds, port, addr, vid);
+       tx_vid = dsa_8021q_tx_vid(ds, port);
+       return priv->info->fdb_del_cmd(ds, port, addr, tx_vid);
 }
 
 static int sja1105_fdb_dump(struct dsa_switch *ds, int port,
@@ -1087,8 +1187,12 @@ static int sja1105_fdb_dump(struct dsa_switch *ds, int port,
 {
        struct sja1105_private *priv = ds->priv;
        struct device *dev = ds->dev;
+       u16 rx_vid, tx_vid;
        int i;
 
+       rx_vid = dsa_8021q_rx_vid(ds, port);
+       tx_vid = dsa_8021q_tx_vid(ds, port);
+
        for (i = 0; i < SJA1105_MAX_L2_LOOKUP_COUNT; i++) {
                struct sja1105_l2_lookup_entry l2_lookup = {0};
                u8 macaddr[ETH_ALEN];
@@ -1114,15 +1218,25 @@ static int sja1105_fdb_dump(struct dsa_switch *ds, int port,
                        continue;
                u64_to_ether_addr(l2_lookup.macaddr, macaddr);
 
-               /* We need to hide the dsa_8021q VLAN from the user.
-                * Convert the TX VID into the pvid that is active in
-                * standalone and non-vlan_filtering modes, aka 1.
-                * The RX VID is applied on the CPU port, which is not seen by
-                * the bridge core anyway, so there's nothing to hide.
+               /* We need to hide the dsa_8021q VLANs from the user. This
+                * basically means hiding the duplicates and only showing
+                * the pvid that is supposed to be active in standalone and
+                * non-vlan_filtering modes (aka 1).
+                * - For statically added FDB entries (bridge fdb add), we
+                *   can convert the TX VID (coming from the CPU port) into the
+                *   pvid and ignore the RX VIDs of the other ports.
+                * - For dynamically learned FDB entries, a single entry with
+                *   no duplicates is learned - that which has the real port's
+                *   pvid, aka RX VID.
                 */
-               if (!dsa_port_is_vlan_filtering(&ds->ports[port]))
-                       l2_lookup.vlanid = 1;
-               cb(macaddr, l2_lookup.vlanid, false, data);
+               if (!dsa_port_is_vlan_filtering(&ds->ports[port])) {
+                       if (l2_lookup.vlanid == tx_vid ||
+                           l2_lookup.vlanid == rx_vid)
+                               l2_lookup.vlanid = 1;
+                       else
+                               continue;
+               }
+               cb(macaddr, l2_lookup.vlanid, l2_lookup.lockeds, data);
        }
        return 0;
 }
@@ -1243,27 +1357,6 @@ static void sja1105_bridge_leave(struct dsa_switch *ds, int port,
        sja1105_bridge_member(ds, port, br, false);
 }
 
-static u8 sja1105_stp_state_get(struct sja1105_private *priv, int port)
-{
-       struct sja1105_mac_config_entry *mac;
-
-       mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
-
-       if (!mac[port].ingress && !mac[port].egress && !mac[port].dyn_learn)
-               return BR_STATE_BLOCKING;
-       if (mac[port].ingress && !mac[port].egress && !mac[port].dyn_learn)
-               return BR_STATE_LISTENING;
-       if (mac[port].ingress && !mac[port].egress && mac[port].dyn_learn)
-               return BR_STATE_LEARNING;
-       if (mac[port].ingress && mac[port].egress && mac[port].dyn_learn)
-               return BR_STATE_FORWARDING;
-       /* This is really an error condition if the MAC was in none of the STP
-        * states above. But treating the port as disabled does nothing, which
-        * is adequate, and it also resets the MAC to a known state later on.
-        */
-       return BR_STATE_DISABLED;
-}
-
 /* For situations where we need to change a setting at runtime that is only
  * available through the static configuration, resetting the switch in order
  * to upload the new static config is unavoidable. Back up the settings we
@@ -1274,27 +1367,18 @@ static int sja1105_static_config_reload(struct sja1105_private *priv)
 {
        struct sja1105_mac_config_entry *mac;
        int speed_mbps[SJA1105_NUM_PORTS];
-       u8 stp_state[SJA1105_NUM_PORTS];
        int rc, i;
 
        mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
 
-       /* Back up settings changed by sja1105_adjust_port_config and
-        * sja1105_bridge_stp_state_set and restore their defaults.
+       /* Back up the dynamic link speed changed by sja1105_adjust_port_config
+        * in order to temporarily restore it to SJA1105_SPEED_AUTO - which the
+        * switch wants to see in the static config in order to allow us to
+        * change it through the dynamic interface later.
         */
        for (i = 0; i < SJA1105_NUM_PORTS; i++) {
                speed_mbps[i] = sja1105_speed[mac[i].speed];
                mac[i].speed = SJA1105_SPEED_AUTO;
-               if (i == dsa_upstream_port(priv->ds, i)) {
-                       mac[i].ingress = true;
-                       mac[i].egress = true;
-                       mac[i].dyn_learn = true;
-               } else {
-                       stp_state[i] = sja1105_stp_state_get(priv, i);
-                       mac[i].ingress = false;
-                       mac[i].egress = false;
-                       mac[i].dyn_learn = false;
-               }
        }
 
        /* Reset switch and send updated static configuration */
@@ -1311,13 +1395,7 @@ static int sja1105_static_config_reload(struct sja1105_private *priv)
                goto out;
 
        for (i = 0; i < SJA1105_NUM_PORTS; i++) {
-               bool enabled = (speed_mbps[i] != 0);
-
-               if (i != dsa_upstream_port(priv->ds, i))
-                       sja1105_bridge_stp_state_set(priv->ds, i, stp_state[i]);
-
-               rc = sja1105_adjust_port_config(priv, i, speed_mbps[i],
-                                               enabled);
+               rc = sja1105_adjust_port_config(priv, i, speed_mbps[i]);
                if (rc < 0)
                        goto out;
        }
@@ -1755,6 +1833,100 @@ static int sja1105_set_ageing_time(struct dsa_switch *ds,
        return sja1105_static_config_reload(priv);
 }
 
+/* Caller must hold priv->tagger_data.meta_lock */
+static int sja1105_change_rxtstamping(struct sja1105_private *priv,
+                                     bool on)
+{
+       struct sja1105_general_params_entry *general_params;
+       struct sja1105_table *table;
+       int rc;
+
+       table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
+       general_params = table->entries;
+       general_params->send_meta1 = on;
+       general_params->send_meta0 = on;
+
+       rc = sja1105_init_avb_params(priv, on);
+       if (rc < 0)
+               return rc;
+
+       /* Initialize the meta state machine to a known state */
+       if (priv->tagger_data.stampable_skb) {
+               kfree_skb(priv->tagger_data.stampable_skb);
+               priv->tagger_data.stampable_skb = NULL;
+       }
+
+       return sja1105_static_config_reload(priv);
+}
+
+static int sja1105_hwtstamp_set(struct dsa_switch *ds, int port,
+                               struct ifreq *ifr)
+{
+       struct sja1105_private *priv = ds->priv;
+       struct hwtstamp_config config;
+       bool rx_on;
+       int rc;
+
+       if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+               return -EFAULT;
+
+       switch (config.tx_type) {
+       case HWTSTAMP_TX_OFF:
+               priv->ports[port].hwts_tx_en = false;
+               break;
+       case HWTSTAMP_TX_ON:
+               priv->ports[port].hwts_tx_en = true;
+               break;
+       default:
+               return -ERANGE;
+       }
+
+       switch (config.rx_filter) {
+       case HWTSTAMP_FILTER_NONE:
+               rx_on = false;
+               break;
+       default:
+               rx_on = true;
+               break;
+       }
+
+       if (rx_on != priv->tagger_data.hwts_rx_en) {
+               spin_lock(&priv->tagger_data.meta_lock);
+               rc = sja1105_change_rxtstamping(priv, rx_on);
+               spin_unlock(&priv->tagger_data.meta_lock);
+               if (rc < 0) {
+                       dev_err(ds->dev,
+                               "Failed to change RX timestamping: %d\n", rc);
+                       return -EFAULT;
+               }
+               priv->tagger_data.hwts_rx_en = rx_on;
+       }
+
+       if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
+               return -EFAULT;
+       return 0;
+}
+
+static int sja1105_hwtstamp_get(struct dsa_switch *ds, int port,
+                               struct ifreq *ifr)
+{
+       struct sja1105_private *priv = ds->priv;
+       struct hwtstamp_config config;
+
+       config.flags = 0;
+       if (priv->ports[port].hwts_tx_en)
+               config.tx_type = HWTSTAMP_TX_ON;
+       else
+               config.tx_type = HWTSTAMP_TX_OFF;
+       if (priv->tagger_data.hwts_rx_en)
+               config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
+       else
+               config.rx_filter = HWTSTAMP_FILTER_NONE;
+
+       return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
+               -EFAULT : 0;
+}
+
 #define to_tagger(d) \
        container_of((d), struct sja1105_tagger_data, rxtstamp_work)
 #define to_sja1105(d) \
@@ -1789,8 +1961,8 @@ static void sja1105_rxtstamp_work(struct work_struct *work)
 }
 
 /* Called from dsa_skb_defer_rx_timestamp */
-bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
-                          struct sk_buff *skb, unsigned int type)
+static bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
+                                 struct sk_buff *skb, unsigned int type)
 {
        struct sja1105_private *priv = ds->priv;
        struct sja1105_tagger_data *data = &priv->tagger_data;
@@ -1810,8 +1982,8 @@ bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
  * the skb and have it available in DSA_SKB_CB in the .port_deferred_xmit
  * callback, where we will timestamp it synchronously.
  */
-bool sja1105_port_txtstamp(struct dsa_switch *ds, int port,
-                          struct sk_buff *skb, unsigned int type)
+static bool sja1105_port_txtstamp(struct dsa_switch *ds, int port,
+                                 struct sk_buff *skb, unsigned int type)
 {
        struct sja1105_private *priv = ds->priv;
        struct sja1105_port *sp = &priv->ports[port];
@@ -1829,6 +2001,8 @@ static const struct dsa_switch_ops sja1105_switch_ops = {
        .set_ageing_time        = sja1105_set_ageing_time,
        .phylink_validate       = sja1105_phylink_validate,
        .phylink_mac_config     = sja1105_mac_config,
+       .phylink_mac_link_up    = sja1105_mac_link_up,
+       .phylink_mac_link_down  = sja1105_mac_link_down,
        .get_strings            = sja1105_get_strings,
        .get_ethtool_stats      = sja1105_get_ethtool_stats,
        .get_sset_count         = sja1105_get_sset_count,
@@ -1847,6 +2021,8 @@ static const struct dsa_switch_ops sja1105_switch_ops = {
        .port_mdb_add           = sja1105_mdb_add,
        .port_mdb_del           = sja1105_mdb_del,
        .port_deferred_xmit     = sja1105_port_deferred_xmit,
+       .port_hwtstamp_get      = sja1105_hwtstamp_get,
+       .port_hwtstamp_set      = sja1105_hwtstamp_set,
        .port_rxtstamp          = sja1105_port_rxtstamp,
        .port_txtstamp          = sja1105_port_txtstamp,
 };