net: dsa: mt7530: add support for port mirroring

Add support for configuring port mirroring through the cls_matchall
classifier. We do a full ingress and/or egress capture towards a
capture port.
MT7530 supports one monitor port and multiple mirrored ports.

Signed-off-by: DENG Qingfang <dqfext@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/dsa/mt7530.c b/drivers/net/dsa/mt7530.c
index 86818ab3bb07124cd2a964c5576fdeb8402e78c7..a6e365348459cf9f8ae8ba580ef798467b21b80d 100644 (file)
--- a/drivers/net/dsa/mt7530.c
+++ b/drivers/net/dsa/mt7530.c
@@ -1222,6 +1222,64 @@ mt7530_port_vlan_del(struct dsa_switch *ds, int port,
        return 0;
 }
 
+static int mt7530_port_mirror_add(struct dsa_switch *ds, int port,
+                                 struct dsa_mall_mirror_tc_entry *mirror,
+                                 bool ingress)
+{
+       struct mt7530_priv *priv = ds->priv;
+       u32 val;
+
+       /* Check for existent entry */
+       if ((ingress ? priv->mirror_rx : priv->mirror_tx) & BIT(port))
+               return -EEXIST;
+
+       val = mt7530_read(priv, MT7530_MFC);
+
+       /* MT7530 only supports one monitor port */
+       if (val & MIRROR_EN && MIRROR_PORT(val) != mirror->to_local_port)
+               return -EEXIST;
+
+       val |= MIRROR_EN;
+       val &= ~MIRROR_MASK;
+       val |= mirror->to_local_port;
+       mt7530_write(priv, MT7530_MFC, val);
+
+       val = mt7530_read(priv, MT7530_PCR_P(port));
+       if (ingress) {
+               val |= PORT_RX_MIR;
+               priv->mirror_rx |= BIT(port);
+       } else {
+               val |= PORT_TX_MIR;
+               priv->mirror_tx |= BIT(port);
+       }
+       mt7530_write(priv, MT7530_PCR_P(port), val);
+
+       return 0;
+}
+
+static void mt7530_port_mirror_del(struct dsa_switch *ds, int port,
+                                  struct dsa_mall_mirror_tc_entry *mirror)
+{
+       struct mt7530_priv *priv = ds->priv;
+       u32 val;
+
+       val = mt7530_read(priv, MT7530_PCR_P(port));
+       if (mirror->ingress) {
+               val &= ~PORT_RX_MIR;
+               priv->mirror_rx &= ~BIT(port);
+       } else {
+               val &= ~PORT_TX_MIR;
+               priv->mirror_tx &= ~BIT(port);
+       }
+       mt7530_write(priv, MT7530_PCR_P(port), val);
+
+       if (!priv->mirror_rx && !priv->mirror_tx) {
+               val = mt7530_read(priv, MT7530_MFC);
+               val &= ~MIRROR_EN;
+               mt7530_write(priv, MT7530_MFC, val);
+       }
+}
+
 static enum dsa_tag_protocol
 mtk_get_tag_protocol(struct dsa_switch *ds, int port,
                     enum dsa_tag_protocol mp)
@@ -1613,6 +1671,8 @@ static const struct dsa_switch_ops mt7530_switch_ops = {
        .port_vlan_prepare      = mt7530_port_vlan_prepare,
        .port_vlan_add          = mt7530_port_vlan_add,
        .port_vlan_del          = mt7530_port_vlan_del,
+       .port_mirror_add        = mt7530_port_mirror_add,
+       .port_mirror_del        = mt7530_port_mirror_del,
        .phylink_validate       = mt7530_phylink_validate,
        .phylink_mac_link_state = mt7530_phylink_mac_link_state,
        .phylink_mac_config     = mt7530_phylink_mac_config,

diff --git a/drivers/net/dsa/mt7530.h b/drivers/net/dsa/mt7530.h
index ccb9da8cad0d1c867fe29b58f5176819310f46ce..5e6c778feb232ce82b0c653de313f72ee2c62890 100644 (file)
--- a/drivers/net/dsa/mt7530.h
+++ b/drivers/net/dsa/mt7530.h
@@ -36,6 +36,9 @@ enum {
 #define  CPU_EN                                BIT(7)
 #define  CPU_PORT(x)                   ((x) << 4)
 #define  CPU_MASK                      (0xf << 4)
+#define  MIRROR_EN                     BIT(3)
+#define  MIRROR_PORT(x)                        ((x & 0x7))
+#define  MIRROR_MASK                   0x7
 
 /* Registers for address table access */
 #define MT7530_ATA1                    0x74
@@ -141,6 +144,8 @@ enum mt7530_stp_state {
 
 /* Register for port control */
 #define MT7530_PCR_P(x)                        (0x2004 + ((x) * 0x100))
+#define  PORT_TX_MIR                   BIT(9)
+#define  PORT_RX_MIR                   BIT(8)
 #define  PORT_VLAN(x)                  ((x) & 0x3)
 
 enum mt7530_port_mode {
@@ -460,6 +465,8 @@ struct mt7530_priv {
        phy_interface_t         p6_interface;
        phy_interface_t         p5_interface;
        unsigned int            p5_intf_sel;
+       u8                      mirror_rx;
+       u8                      mirror_tx;
 
        struct mt7530_port      ports[MT7530_NUM_PORTS];
        /* protect among processes for registers access*/