[PATCH net-next] net: dsa: mv88e6xxx: Add support for port mirroring

[Iwan R Timmer]

Add support for configuring port mirroring through the cls_matchall
classifier. We do a full ingress and/or egress capture towards the
capture port, configured with set_egress_port.

Signed-off-by: Iwan R Timmer <irtim...@gmail.com>
---
 drivers/net/dsa/mv88e6xxx/chip.c | 36 ++++++++++++++++++++++++++++++++
 drivers/net/dsa/mv88e6xxx/port.c | 21 +++++++++++++++++++
 drivers/net/dsa/mv88e6xxx/port.h |  2 ++
 3 files changed, 59 insertions(+)

diff --git a/drivers/net/dsa/mv88e6xxx/chip.c b/drivers/net/dsa/mv88e6xxx/chip.c
index d0a97eb73a37..3a48f9d07cbf 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.c
+++ b/drivers/net/dsa/mv88e6xxx/chip.c
@@ -4595,6 +4595,40 @@ static int mv88e6xxx_port_mdb_del(struct dsa_switch *ds, 
int port,
        return err;
 }
 
+static int mv88e6xxx_port_mirror_add(struct dsa_switch *ds, int port,
+                                    struct dsa_mall_mirror_tc_entry *mirror,
+                                    bool ingress)
+{
+       struct mv88e6xxx_chip *chip = ds->priv;
+       int err = -EOPNOTSUPP;
+
+       mutex_lock(&chip->reg_lock);
+       if (chip->info->ops->set_egress_port)
+               err = chip->info->ops->set_egress_port(chip,
+                                                      mirror->to_local_port);
+
+       if (err)
+               goto out;
+
+       err = mv88e6xxx_port_set_mirror(chip, port, true, ingress);
+out:
+       mutex_unlock(&chip->reg_lock);
+
+       return err;
+}
+
+static void mv88e6xxx_port_mirror_del(struct dsa_switch *ds, int port,
+                                     struct dsa_mall_mirror_tc_entry *mirror)
+{
+       struct mv88e6xxx_chip *chip = ds->priv;
+
+       mutex_lock(&chip->reg_lock);
+       if (mv88e6xxx_port_set_mirror(chip, port, false, false))
+               dev_err(ds->dev, "p%d: failed to disable mirroring\n", port);
+
+       mutex_unlock(&chip->reg_lock);
+}
+
 static int mv88e6xxx_port_egress_floods(struct dsa_switch *ds, int port,
                                         bool unicast, bool multicast)
 {
@@ -4647,6 +4681,8 @@ static const struct dsa_switch_ops mv88e6xxx_switch_ops = 
{
        .port_mdb_prepare       = mv88e6xxx_port_mdb_prepare,
        .port_mdb_add           = mv88e6xxx_port_mdb_add,
        .port_mdb_del           = mv88e6xxx_port_mdb_del,
+       .port_mirror_add        = mv88e6xxx_port_mirror_add,
+       .port_mirror_del        = mv88e6xxx_port_mirror_del,
        .crosschip_bridge_join  = mv88e6xxx_crosschip_bridge_join,
        .crosschip_bridge_leave = mv88e6xxx_crosschip_bridge_leave,
        .port_hwtstamp_set      = mv88e6xxx_port_hwtstamp_set,
diff --git a/drivers/net/dsa/mv88e6xxx/port.c b/drivers/net/dsa/mv88e6xxx/port.c
index 04309ef0a1cc..301bf704c877 100644
--- a/drivers/net/dsa/mv88e6xxx/port.c
+++ b/drivers/net/dsa/mv88e6xxx/port.c
@@ -1086,6 +1086,27 @@ int mv88e6095_port_set_upstream_port(struct 
mv88e6xxx_chip *chip, int port,
        return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
 }
 
+int mv88e6xxx_port_set_mirror(struct mv88e6xxx_chip *chip, int port,
+                             bool mirror, bool ingress)
+{
+       u16 reg;
+       u16 bit;
+       int err;
+
+       err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
+       if (err)
+               return err;
+
+       bit = ingress ? MV88E6XXX_PORT_CTL2_INGRESS_MONITOR :
+                       MV88E6XXX_PORT_CTL2_EGRESS_MONITOR;
+       reg &= ~bit;
+
+       if (mirror)
+               reg |= bit;
+
+       return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
+}
+
 int mv88e6xxx_port_set_8021q_mode(struct mv88e6xxx_chip *chip, int port,
                                  u16 mode)
 {
diff --git a/drivers/net/dsa/mv88e6xxx/port.h b/drivers/net/dsa/mv88e6xxx/port.h
index 8d5a6cd6fb19..40ed60a2099b 100644
--- a/drivers/net/dsa/mv88e6xxx/port.h
+++ b/drivers/net/dsa/mv88e6xxx/port.h
@@ -347,6 +347,8 @@ int mv88e6352_port_link_state(struct mv88e6xxx_chip *chip, 
int port,
 int mv88e6xxx_port_set_map_da(struct mv88e6xxx_chip *chip, int port);
 int mv88e6095_port_set_upstream_port(struct mv88e6xxx_chip *chip, int port,
                                     int upstream_port);
+int mv88e6xxx_port_set_mirror(struct mv88e6xxx_chip *chip, int port,
+                             bool mirror, bool ingress);
 
 int mv88e6xxx_port_disable_learn_limit(struct mv88e6xxx_chip *chip, int port);
 int mv88e6xxx_port_disable_pri_override(struct mv88e6xxx_chip *chip, int port);
-- 
2.23.0