2 files changed, 169 insertions, 6 deletions

diff --git a/target/linux/generic/files/drivers/net/phy/mvsw61xx.c b/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
index 9a689e6c81..253ebff835 100644
--- a/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
+++ b/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
@@ -454,6 +454,86 @@ static int mvsw61xx_set_enable_vlan(struct switch_dev *dev,
 	return 0;
 }
 
+static int mvsw61xx_get_mirror_rx_enable(struct switch_dev *dev,
+		const struct switch_attr *attr, struct switch_val *val)
+{
+	struct mvsw61xx_state *state = get_state(dev);
+
+	val->value.i = state->mirror_rx;
+
+	return 0;
+}
+
+static int mvsw61xx_set_mirror_rx_enable(struct switch_dev *dev,
+		const struct switch_attr *attr, struct switch_val *val)
+{
+	struct mvsw61xx_state *state = get_state(dev);
+
+	state->mirror_rx = val->value.i;
+
+	return 0;
+}
+
+static int mvsw61xx_get_mirror_tx_enable(struct switch_dev *dev,
+		const struct switch_attr *attr, struct switch_val *val)
+{
+	struct mvsw61xx_state *state = get_state(dev);
+
+	val->value.i = state->mirror_tx;
+
+	return 0;
+}
+
+static int mvsw61xx_set_mirror_tx_enable(struct switch_dev *dev,
+		const struct switch_attr *attr, struct switch_val *val)
+{
+	struct mvsw61xx_state *state = get_state(dev);
+
+	state->mirror_tx = val->value.i;
+
+	return 0;
+}
+
+static int mvsw61xx_get_mirror_monitor_port(struct switch_dev *dev,
+		const struct switch_attr *attr, struct switch_val *val)
+{
+	struct mvsw61xx_state *state = get_state(dev);
+
+	val->value.i = state->monitor_port;
+
+	return 0;
+}
+
+static int mvsw61xx_set_mirror_monitor_port(struct switch_dev *dev,
+		const struct switch_attr *attr, struct switch_val *val)
+{
+	struct mvsw61xx_state *state = get_state(dev);
+
+	state->monitor_port = val->value.i;
+
+	return 0;
+}
+
+static int mvsw61xx_get_mirror_source_port(struct switch_dev *dev,
+		const struct switch_attr *attr, struct switch_val *val)
+{
+	struct mvsw61xx_state *state = get_state(dev);
+
+	val->value.i = state->source_port;
+
+	return 0;
+}
+
+static int mvsw61xx_set_mirror_source_port(struct switch_dev *dev,
+		const struct switch_attr *attr, struct switch_val *val)
+{
+	struct mvsw61xx_state *state = get_state(dev);
+
+	state->source_port = val->value.i;
+
+	return 0;
+}
+
 static int mvsw61xx_vtu_program(struct switch_dev *dev)
 {
 	struct mvsw61xx_state *state = get_state(dev);
@@ -604,6 +684,40 @@ static int mvsw61xx_update_state(struct switch_dev *dev)
 
 	mvsw61xx_vtu_program(dev);
 
+	/* port mirroring */
+	/* reset all mirror registers */
+	for (i = 0; i < dev->ports; i++) {
+		reg = sr16(dev, MV_PORTREG(CONTROL2, i));
+		reg &= ~(MV_MIRROR_RX_SRC_MASK | MV_MIRROR_TX_SRC_MASK);
+		sw16(dev, MV_PORTREG(CONTROL2, i), reg);
+	}
+	reg = sr16(dev, MV_GLOBALREG(MONITOR_CTRL));
+	reg |= MV_MIRROR_RX_DEST_MASK | MV_MIRROR_TX_DEST_MASK;
+	sw16(dev, MV_GLOBALREG(MONITOR_CTRL), reg);
+
+	/* now enable mirroring if necessary */
+	if (state->mirror_rx) {
+		/* set ingress monitor source */
+		reg = sr16(dev, MV_PORTREG(CONTROL2, state->source_port)) & ~MV_MIRROR_RX_SRC_MASK;
+		reg |= state->mirror_rx << MV_MIRROR_RX_SRC_SHIFT;
+		sw16(dev, MV_PORTREG(CONTROL2, state->source_port), reg);
+		/* set ingress monitor destination */
+		reg = sr16(dev, MV_GLOBALREG(MONITOR_CTRL)) & ~MV_MIRROR_RX_DEST_MASK;
+		reg |= state->monitor_port << MV_MIRROR_RX_DEST_SHIFT;
+		sw16(dev, MV_GLOBALREG(MONITOR_CTRL), reg);
+	}
+
+	if (state->mirror_tx) {
+		/* set egress monitor source */
+		reg = sr16(dev, MV_PORTREG(CONTROL2, state->source_port)) & ~MV_MIRROR_TX_SRC_MASK;
+		reg |= state->mirror_tx << MV_MIRROR_TX_SRC_SHIFT;
+		sw16(dev, MV_PORTREG(CONTROL2, state->source_port), reg);
+		/* set egress monitor destination */
+		reg = sr16(dev, MV_GLOBALREG(MONITOR_CTRL)) & ~MV_MIRROR_TX_DEST_MASK;
+		reg |= state->monitor_port << MV_MIRROR_TX_DEST_SHIFT;
+		sw16(dev, MV_GLOBALREG(MONITOR_CTRL), reg);
+	}
+
 	return 0;
 }
 
@@ -693,6 +807,11 @@ static int _mvsw61xx_reset(struct switch_dev *dev, bool full)
 
 	state->vlan_enabled = 0;
 
+	state->mirror_rx = false;
+	state->mirror_tx = false;
+	state->source_port = 0;
+	state->monitor_port = 0;
+
 	mvsw61xx_update_state(dev);
 
 	/* Re-enable ports */
@@ -711,10 +830,6 @@ static int mvsw61xx_reset(struct switch_dev *dev)
 }
 
 enum {
-	MVSW61XX_ENABLE_VLAN,
-};
-
-enum {
 	MVSW61XX_VLAN_PORT_BASED,
 	MVSW61XX_VLAN_ID,
 };
@@ -725,14 +840,45 @@ enum {
 };
 
 static const struct switch_attr mvsw61xx_global[] = {
-	[MVSW61XX_ENABLE_VLAN] = {
-		.id = MVSW61XX_ENABLE_VLAN,
+	{
 		.type = SWITCH_TYPE_INT,
 		.name = "enable_vlan",
 		.description = "Enable 802.1q VLAN support",
 		.get = mvsw61xx_get_enable_vlan,
 		.set = mvsw61xx_set_enable_vlan,
 	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "enable_mirror_rx",
+		.description = "Enable mirroring of RX packets",
+		.set = mvsw61xx_set_mirror_rx_enable,
+		.get = mvsw61xx_get_mirror_rx_enable,
+		.max = 1
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "enable_mirror_tx",
+		.description = "Enable mirroring of TX packets",
+		.set = mvsw61xx_set_mirror_tx_enable,
+		.get = mvsw61xx_get_mirror_tx_enable,
+		.max = 1
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "mirror_monitor_port",
+		.description = "Mirror monitor port",
+		.set = mvsw61xx_set_mirror_monitor_port,
+		.get = mvsw61xx_get_mirror_monitor_port,
+		.max = MV_PORTS - 1
+	},
+	{
+		.type = SWITCH_TYPE_INT,
+		.name = "mirror_source_port",
+		.description = "Mirror source port",
+		.set = mvsw61xx_set_mirror_source_port,
+		.get = mvsw61xx_get_mirror_source_port,
+		.max = MV_PORTS - 1
+	},
 };
 
 static const struct switch_attr mvsw61xx_vlan[] = {
diff --git a/target/linux/generic/files/drivers/net/phy/mvsw61xx.h b/target/linux/generic/files/drivers/net/phy/mvsw61xx.h
index a07b09cd14..545e2dd660 100644
--- a/target/linux/generic/files/drivers/net/phy/mvsw61xx.h
+++ b/target/linux/generic/files/drivers/net/phy/mvsw61xx.h
@@ -165,6 +165,7 @@ enum {
 	MV_GLOBAL_VTU_DATA1		= 0x07,
 	MV_GLOBAL_VTU_DATA2		= 0x08,
 	MV_GLOBAL_VTU_DATA3		= 0x09,
+	MV_GLOBAL_MONITOR_CTRL		= 0x1a,
 	MV_GLOBAL_CONTROL2		= 0x1c,
 };
 #define MV_GLOBALREG(_type) MV_SWITCH_GLOBAL, MV_GLOBAL_##_type
@@ -242,6 +243,16 @@ enum {
 #define MV_FDB_HI_SHIFT			4
 #define MV_FDB_LO_SHIFT			12
 
+#define MV_MIRROR_RX_DEST_MASK		0xf000
+#define MV_MIRROR_TX_DEST_MASK		0x0f00
+#define MV_MIRROR_RX_DEST_SHIFT		12
+#define MV_MIRROR_TX_DEST_SHIFT		8
+
+#define MV_MIRROR_RX_SRC_SHIFT		4
+#define MV_MIRROR_RX_SRC_MASK		(1 << MV_MIRROR_RX_SRC_SHIFT)
+#define MV_MIRROR_TX_SRC_SHIFT		5
+#define MV_MIRROR_TX_SRC_MASK		(1 << MV_MIRROR_TX_SRC_SHIFT)
+
 /* Marvell Specific PHY register */
 #define MII_MV_SPEC_CTRL		16
 enum {
@@ -284,6 +295,12 @@ struct mvsw61xx_state {
 		u32 port_sstate;
 	} vlans[MV_VLANS];
 
+	/* mirroring */
+	bool mirror_rx;
+	bool mirror_tx;
+	int source_port;
+	int monitor_port;
+
 	char buf[128];
 };