net: dsa: sja1105: Add support for port mirroring

Amazingly, of all features, this does not require a switch reset.

Tested with:

tc qdisc add dev swp2 clsact
tc filter add dev swp2 ingress matchall skip_sw \
	action mirred egress mirror dev swp3
tc filter show dev swp2 ingress
tc filter del dev swp2 ingress pref 49152

Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/dsa/sja1105/sja1105_main.c b/drivers/net/dsa/sja1105/sja1105_main.c
index e950397..06fcd45 100644 (file)
--- a/drivers/net/dsa/sja1105/sja1105_main.c
+++ b/drivers/net/dsa/sja1105/sja1105_main.c
@@ -382,8 +382,8 @@ static int sja1105_init_l2_forwarding_params(struct sja1105_private *priv)
 static int sja1105_init_general_params(struct sja1105_private *priv)
 {
        struct sja1105_general_params_entry default_general_params = {
-               /* Disallow dynamic changing of the mirror port */
-               .mirr_ptacu = 0,
+               /* Allow dynamic changing of the mirror port */
+               .mirr_ptacu = true,
                .switchid = priv->ds->index,
                /* Priority queue for link-local management frames
                 * (both ingress to and egress from CPU - PTP, STP etc)
@@ -403,8 +403,8 @@ static int sja1105_init_general_params(struct sja1105_private *priv)
                 * by installing a temporary 'management route'
                 */
                .host_port = dsa_upstream_port(priv->ds, 0),
-               /* Same as host port */
-               .mirr_port = dsa_upstream_port(priv->ds, 0),
+               /* Default to an invalid value */
+               .mirr_port = SJA1105_NUM_PORTS,
                /* Link-local traffic received on casc_port will be forwarded
                 * to host_port without embedding the source port and device ID
                 * info in the destination MAC address (presumably because it
@@ -2069,6 +2069,84 @@ static int sja1105_port_setup_tc(struct dsa_switch *ds, int port,
        }
 }
 
+/* We have a single mirror (@to) port, but can configure ingress and egress
+ * mirroring on all other (@from) ports.
+ * We need to allow mirroring rules only as long as the @to port is always the
+ * same, and we need to unset the @to port from mirr_port only when there is no
+ * mirroring rule that references it.
+ */
+static int sja1105_mirror_apply(struct sja1105_private *priv, int from, int to,
+                               bool ingress, bool enabled)
+{
+       struct sja1105_general_params_entry *general_params;
+       struct sja1105_mac_config_entry *mac;
+       struct sja1105_table *table;
+       bool already_enabled;
+       u64 new_mirr_port;
+       int rc;
+
+       table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
+       general_params = table->entries;
+
+       mac = priv->static_config.tables[BLK_IDX_MAC_CONFIG].entries;
+
+       already_enabled = (general_params->mirr_port != SJA1105_NUM_PORTS);
+       if (already_enabled && enabled && general_params->mirr_port != to) {
+               dev_err(priv->ds->dev,
+                       "Delete mirroring rules towards port %llu first\n",
+                       general_params->mirr_port);
+               return -EBUSY;
+       }
+
+       new_mirr_port = to;
+       if (!enabled) {
+               bool keep = false;
+               int port;
+
+               /* Anybody still referencing mirr_port? */
+               for (port = 0; port < SJA1105_NUM_PORTS; port++) {
+                       if (mac[port].ing_mirr || mac[port].egr_mirr) {
+                               keep = true;
+                               break;
+                       }
+               }
+               /* Unset already_enabled for next time */
+               if (!keep)
+                       new_mirr_port = SJA1105_NUM_PORTS;
+       }
+       if (new_mirr_port != general_params->mirr_port) {
+               general_params->mirr_port = new_mirr_port;
+
+               rc = sja1105_dynamic_config_write(priv, BLK_IDX_GENERAL_PARAMS,
+                                                 0, general_params, true);
+               if (rc < 0)
+                       return rc;
+       }
+
+       if (ingress)
+               mac[from].ing_mirr = enabled;
+       else
+               mac[from].egr_mirr = enabled;
+
+       return sja1105_dynamic_config_write(priv, BLK_IDX_MAC_CONFIG, from,
+                                           &mac[from], true);
+}
+
+static int sja1105_mirror_add(struct dsa_switch *ds, int port,
+                             struct dsa_mall_mirror_tc_entry *mirror,
+                             bool ingress)
+{
+       return sja1105_mirror_apply(ds->priv, port, mirror->to_local_port,
+                                   ingress, true);
+}
+
+static void sja1105_mirror_del(struct dsa_switch *ds, int port,
+                              struct dsa_mall_mirror_tc_entry *mirror)
+{
+       sja1105_mirror_apply(ds->priv, port, mirror->to_local_port,
+                            mirror->ingress, false);
+}
+
 static const struct dsa_switch_ops sja1105_switch_ops = {
        .get_tag_protocol       = sja1105_get_tag_protocol,
        .setup                  = sja1105_setup,
@@ -2102,6 +2180,8 @@ static const struct dsa_switch_ops sja1105_switch_ops = {
        .port_rxtstamp          = sja1105_port_rxtstamp,
        .port_txtstamp          = sja1105_port_txtstamp,
        .port_setup_tc          = sja1105_port_setup_tc,
+       .port_mirror_add        = sja1105_mirror_add,
+       .port_mirror_del        = sja1105_mirror_del,
 };
 
 static int sja1105_check_device_id(struct sja1105_private *priv)