From e764664761d48d1f3102c4e2625056c62402a311 Mon Sep 17 00:00:00 2001
From: Vignesh Raghavendra <vigneshr@ti.com>
Date: Thu, 14 Oct 2021 14:14:28 +0530
Subject: [PATCH] net: ethernet: ti: icssg_prueth: Add support for ICSSG switch
 firmware on AM654 PG2.0

Add support for ICSSG switch firmware using existing Dual EMAC driver
with switchdev and devlink framework.

Limitations:
VLAN offloading is limited to 0-256 IDs.
MDB/FDB static entries are limited to 511 entries and different FDBs can
hash to same bucket and thus may not completely offloaded

Switch mode requires loading of new firmware into ICSSG cores. This
means interfaces have to taken down and then reconfigured to switch mode
using devlink.

Example assuming ETH1 and ETH2 as ICSSG2 interfaces:

Switch to ICSSG Switch mode:
 ip link set dev eth1 down
 ip link set dev eth2 down
 devlink dev param set platform/icssg2-eth name switch_mode value 1 cmode runtime
 ip link add name br0 type bridge
 ip link set dev eth1 master br0
 ip link set dev eth2 master br0
 ip link set dev br0 up
 ip link set dev eth1 up
 ip link set dev eth2 up
 bridge vlan add dev br0 vid 1 pvid untagged self

Going back to Dual EMAC mode:

 ip link set dev br0 down
 ip link set dev eth1 nomaster
 ip link set dev eth2 nomaster
 ip link set dev eth1 down
 ip link set dev eth2 down
 devlink dev param set platform/icssg2-eth name switch_mode value 0 cmode runtime
 ip link del name br0 type bridge
 ip link set dev eth1 up
 ip link set dev eth2 up

By default, Dual EMAC firmware is loaded, and can be changed to switch
mode by above steps

Signed-off-by: Vignesh Raghavendra <vigneshr@ti.com>
---
 drivers/net/ethernet/ti/Kconfig            |   1 +
 drivers/net/ethernet/ti/Makefile           |   2 +-
 drivers/net/ethernet/ti/icssg_classifier.c |   8 +
 drivers/net/ethernet/ti/icssg_config.c     | 143 +++++++-
 drivers/net/ethernet/ti/icssg_config.h     |   6 +
 drivers/net/ethernet/ti/icssg_prueth.c     | 398 ++++++++++++++++++++-
 drivers/net/ethernet/ti/icssg_prueth.h     |   1 +
 7 files changed, 545 insertions(+), 14 deletions(-)

diff --git a/drivers/net/ethernet/ti/Kconfig b/drivers/net/ethernet/ti/Kconfig
index f17df3b25dd0..90ede5a5f108 100644
--- a/drivers/net/ethernet/ti/Kconfig
+++ b/drivers/net/ethernet/ti/Kconfig
@@ -186,6 +186,7 @@ config TI_ICSSG_PRUETH
 	select TI_ICSS_IEP
 	imply PTP_1588_CLOCK
 	depends on PRU_REMOTEPROC
+	depends on NET_SWITCHDEV
 	depends on ARCH_K3 && OF && TI_K3_UDMA_GLUE_LAYER
 	help
 	  Support dual Gigabit Ethernet ports over the ICSSG PRU Subsystem
diff --git a/drivers/net/ethernet/ti/Makefile b/drivers/net/ethernet/ti/Makefile
index b40292eff9bf..5b4cc0a3e3f8 100644
--- a/drivers/net/ethernet/ti/Makefile
+++ b/drivers/net/ethernet/ti/Makefile
@@ -31,4 +31,4 @@ obj-$(CONFIG_TI_K3_AM65_CPTS) += am65-cpts.o
 obj-$(CONFIG_TI_ICSS_IEP) += icss_iep.o
 
 obj-$(CONFIG_TI_ICSSG_PRUETH) += icssg-prueth.o
-icssg-prueth-y := icssg_prueth.o icssg_classifier.o icssg_ethtool.o icssg_queues.o icssg_config.o k3-cppi-desc-pool.o icssg_mii_cfg.o
+icssg-prueth-y := icssg_prueth.o icssg_classifier.o icssg_ethtool.o icssg_queues.o icssg_config.o k3-cppi-desc-pool.o icssg_mii_cfg.o icssg_switchdev.o
diff --git a/drivers/net/ethernet/ti/icssg_classifier.c b/drivers/net/ethernet/ti/icssg_classifier.c
index ea9a1c7bb0fe..47b13672d723 100644
--- a/drivers/net/ethernet/ti/icssg_classifier.c
+++ b/drivers/net/ethernet/ti/icssg_classifier.c
@@ -114,6 +114,8 @@ enum rx_class_sel_type {
 #define RX_CLASS_SEL_MASK(n)	(0x3 << RX_CLASS_SEL_SHIFT((n)))
 
 #define ICSSG_CFG_OFFSET	0
+#define MAC_INTERFACE_0		0x18
+#define MAC_INTERFACE_1		0x1c
 
 #define ICSSG_CFG_RX_L2_G_EN	BIT(2)
 
@@ -287,6 +289,12 @@ static u32 rx_class_get_or(struct regmap *miig_rt, int slice, int n)
 	return val;
 }
 
+void icssg_class_set_host_mac_addr(struct regmap *miig_rt, u8 *mac)
+{
+	regmap_write(miig_rt, MAC_INTERFACE_0, addr_to_da0(mac));
+	regmap_write(miig_rt, MAC_INTERFACE_1, addr_to_da1(mac));
+}
+
 void icssg_class_set_mac_addr(struct regmap *miig_rt, int slice, u8 *mac)
 {
 	regmap_write(miig_rt, offs[slice].mac0, addr_to_da0(mac));
diff --git a/drivers/net/ethernet/ti/icssg_config.c b/drivers/net/ethernet/ti/icssg_config.c
index ec996b540ce8..8d813da0baf6 100644
--- a/drivers/net/ethernet/ti/icssg_config.c
+++ b/drivers/net/ethernet/ti/icssg_config.c
@@ -98,6 +98,42 @@ struct map hwq_map[2][ICSSG_NUM_OTHER_QUEUES] = {
 	},
 };
 
+static void icssg_config_mii_init_switch(struct prueth_emac *emac)
+{
+	struct prueth *prueth = emac->prueth;
+	struct regmap *mii_rt = prueth->mii_rt;
+	int mii = prueth_emac_slice(emac);
+	u32 rxcfg_reg, txcfg_reg, pcnt_reg;
+	u32 rxcfg, txcfg;
+
+	rxcfg_reg = (mii == ICSS_MII0) ? PRUSS_MII_RT_RXCFG0 :
+				       PRUSS_MII_RT_RXCFG1;
+	txcfg_reg = (mii == ICSS_MII0) ? PRUSS_MII_RT_TXCFG0 :
+				       PRUSS_MII_RT_TXCFG1;
+	pcnt_reg = (mii == ICSS_MII0) ? PRUSS_MII_RT_RX_PCNT0 :
+				       PRUSS_MII_RT_RX_PCNT1;
+
+	rxcfg =	PRUSS_MII_RT_RXCFG_RX_ENABLE |
+		PRUSS_MII_RT_RXCFG_RX_L2_EN |
+		PRUSS_MII_RT_RXCFG_RX_L2_EOF_SCLR_DIS;
+
+	txcfg = PRUSS_MII_RT_TXCFG_TX_ENABLE |
+		PRUSS_MII_RT_TXCFG_TX_AUTO_PREAMBLE |
+		PRUSS_MII_RT_TXCFG_TX_IPG_WIRE_CLK_EN;
+
+	if (mii == ICSS_MII1)
+		rxcfg |= PRUSS_MII_RT_RXCFG_RX_MUX_SEL;
+
+	if (emac->phy_if == PHY_INTERFACE_MODE_MII && mii == ICSS_MII1)
+		txcfg |= PRUSS_MII_RT_TXCFG_TX_MUX_SEL;
+	else if (emac->phy_if != PHY_INTERFACE_MODE_MII && mii == ICSS_MII0)
+		txcfg |= PRUSS_MII_RT_TXCFG_TX_MUX_SEL;
+
+	regmap_write(mii_rt, rxcfg_reg, rxcfg);
+	regmap_write(mii_rt, txcfg_reg, txcfg);
+	regmap_write(mii_rt, pcnt_reg, 0x1);
+}
+
 static void icssg_config_mii_init(struct prueth_emac *emac)
 {
 	struct prueth *prueth = emac->prueth;
@@ -295,6 +331,62 @@ static int emac_r30_is_done(struct prueth_emac *emac)
 	return 1;
 }
 
+static int prueth_switch_buffer_setup(struct prueth_emac *emac)
+{
+	struct icssg_buffer_pool_cfg *bpool_cfg;
+	struct prueth *prueth = emac->prueth;
+	int slice = prueth_emac_slice(emac);
+	struct icssg_rxq_ctx *rxq_ctx;
+	u32 addr;
+	int i;
+
+	addr = lower_32_bits(prueth->msmcram.pa);
+	if (slice)
+		addr += PRUETH_NUM_BUF_POOLS_SR2 * PRUETH_EMAC_BUF_POOL_SIZE_SR2;
+
+	if (addr % SZ_64K) {
+		dev_warn(prueth->dev, "buffer pool needs to be 64KB aligned\n");
+		return -EINVAL;
+	}
+
+	bpool_cfg = emac->dram.va + BUFFER_POOL_0_ADDR_OFFSET;
+	/* workaround for f/w bug. bpool 0 needs to be initilalized */
+	for (i = 0;
+	     i <  PRUETH_NUM_BUF_POOLS_SR2;
+	     i++) {
+		bpool_cfg[i].addr = cpu_to_le32(addr);
+		bpool_cfg[i].len = cpu_to_le32(PRUETH_EMAC_BUF_POOL_SIZE_SR2);
+		addr += PRUETH_EMAC_BUF_POOL_SIZE_SR2;
+	}
+
+	if (!slice)
+		addr += PRUETH_NUM_BUF_POOLS_SR2 * PRUETH_EMAC_BUF_POOL_SIZE_SR2;
+	else
+		addr += PRUETH_SW_NUM_BUF_POOLS_HOST_SR2 * PRUETH_SW_BUF_POOL_SIZE_HOST_SR2;
+
+	for (i = PRUETH_NUM_BUF_POOLS_SR2;
+	     i <  PRUETH_SW_NUM_BUF_POOLS_HOST_SR2 + PRUETH_NUM_BUF_POOLS_SR2;
+	     i++) {
+		bpool_cfg[i].addr = cpu_to_le32(addr);
+		bpool_cfg[i].len = cpu_to_le32(PRUETH_SW_BUF_POOL_SIZE_HOST_SR2);
+		addr += PRUETH_SW_BUF_POOL_SIZE_HOST_SR2;
+	}
+
+	if (!slice)
+		addr += PRUETH_SW_NUM_BUF_POOLS_HOST_SR2 * PRUETH_SW_BUF_POOL_SIZE_HOST_SR2;
+	else
+		addr += PRUETH_EMAC_RX_CTX_BUF_SIZE;
+
+	rxq_ctx = emac->dram.va + HOST_RX_Q_PRE_CONTEXT_OFFSET;
+	for (i = 0; i < 3; i++)
+		rxq_ctx->start[i] = cpu_to_le32(addr);
+
+	addr += PRUETH_EMAC_RX_CTX_BUF_SIZE;
+	rxq_ctx->end = cpu_to_le32(addr) - SZ_2K;
+
+	return 0;
+}
+
 static int prueth_emac_buffer_setup(struct prueth_emac *emac)
 {
 	struct icssg_buffer_pool_cfg *bpool_cfg;
@@ -344,6 +436,42 @@ static int prueth_emac_buffer_setup(struct prueth_emac *emac)
 	return 0;
 }
 
+static void icssg_init_emac_mode(struct prueth *prueth)
+{
+	u8 mac[ETH_ALEN] = { 0 };
+
+	if (prueth->emacs_initialized)
+		return;
+
+	regmap_update_bits(prueth->miig_rt, FDB_GEN_CFG1, SMEM_VLAN_OFFSET_MASK, 0);
+	regmap_write(prueth->miig_rt, FDB_GEN_CFG2, 0);
+	/* Clear host MAC address */
+	icssg_class_set_host_mac_addr(prueth->miig_rt, mac);
+}
+
+static void icssg_init_switch_mode(struct prueth *prueth)
+{
+	int i;
+	u32 addr = prueth->shram.pa + EMAC_ICSSG_SWITCH_DEFAULT_VLAN_TABLE_OFFSET;
+
+	if (prueth->emacs_initialized)
+		return;
+
+	/* Set VLAN TABLE address base */
+	regmap_update_bits(prueth->miig_rt, FDB_GEN_CFG1, SMEM_VLAN_OFFSET_MASK,
+			   addr <<  SMEM_VLAN_OFFSET);
+	/* Set enable VLAN aware mode, and FDBs for all PRUs */
+	regmap_write(prueth->miig_rt, FDB_GEN_CFG2, FDB_EN_ALL);
+	prueth->vlan_tbl = prueth->shram.va + EMAC_ICSSG_SWITCH_DEFAULT_VLAN_TABLE_OFFSET;
+	for (i = 0; i < SZ_4K - 1; i++) {
+		prueth->vlan_tbl[i].fid = i;
+		prueth->vlan_tbl[i].fid_c1 = 0;
+	}
+
+	icssg_class_set_host_mac_addr(prueth->miig_rt, prueth->hw_bridge_dev->dev_addr);
+	icssg_set_pvid(prueth, prueth->default_vlan, PRUETH_PORT_HOST);
+}
+
 int icssg_config_sr2(struct prueth *prueth, struct prueth_emac *emac, int slice)
 {
 	void *config = emac->dram.va + ICSSG_CONFIG_OFFSET;
@@ -352,6 +480,11 @@ int icssg_config_sr2(struct prueth *prueth, struct prueth_emac *emac, int slice)
 	u32 mask;
 	int ret;
 
+	if (prueth->is_switch_mode)
+		icssg_init_switch_mode(prueth);
+	else
+		icssg_init_emac_mode(prueth);
+
 	memset_io(config, 0, TAS_GATE_MASK_LIST0);
 	icssg_miig_queues_init(prueth, slice);
 
@@ -363,7 +496,10 @@ int icssg_config_sr2(struct prueth *prueth, struct prueth_emac *emac, int slice)
 	}
 	regmap_update_bits(prueth->miig_rt, ICSSG_CFG_OFFSET, ICSSG_CFG_DEFAULT, ICSSG_CFG_DEFAULT);
 	icssg_miig_set_interface_mode(prueth->miig_rt, slice, emac->phy_if);
-	icssg_config_mii_init(emac);
+	if (prueth->is_switch_mode)
+		icssg_config_mii_init_switch(emac);
+	else
+		icssg_config_mii_init(emac);
 	icssg_config_ipg(emac);
 	icssg_update_rgmii_cfg(prueth->miig_rt, emac);
 
@@ -386,7 +522,10 @@ int icssg_config_sr2(struct prueth *prueth, struct prueth_emac *emac, int slice)
 	*(cfg_byte_ptr + SPL_PKT_DEFAULT_PRIORITY) = 0;
 	*(cfg_byte_ptr + QUEUE_NUM_UNTAGGED) = 0x0;
 
-	ret = prueth_emac_buffer_setup(emac);
+	if (prueth->is_switch_mode)
+		ret = prueth_switch_buffer_setup(emac);
+	else
+		ret = prueth_emac_buffer_setup(emac);
 	if (ret)
 		return ret;
 
diff --git a/drivers/net/ethernet/ti/icssg_config.h b/drivers/net/ethernet/ti/icssg_config.h
index 887e85789f05..bcac3d6e1dd7 100644
--- a/drivers/net/ethernet/ti/icssg_config.h
+++ b/drivers/net/ethernet/ti/icssg_config.h
@@ -105,6 +105,12 @@ struct icssg_config_sr1 {
 	(2 * (PRUETH_EMAC_BUF_POOL_SIZE_SR2 * PRUETH_NUM_BUF_POOLS_SR2 + \
 	 PRUETH_EMAC_RX_CTX_BUF_SIZE))
 
+#define PRUETH_SW_BUF_POOL_SIZE_HOST_SR2 SZ_2K
+#define PRUETH_SW_NUM_BUF_POOLS_HOST_SR2 16
+#define MSMC_RAM_SIZE_SR2_SWITCH_MODE \
+	(MSMC_RAM_SIZE_SR2 + \
+	(2 * PRUETH_SW_BUF_POOL_SIZE_HOST_SR2 * PRUETH_SW_NUM_BUF_POOLS_HOST_SR2))
+
 #define PRUETH_SWITCH_FDB_MASK ((SIZE_OF_FDB / NUMBER_OF_FDB_BUCKET_ENTRIES) - 1)
 
 struct icssg_rxq_ctx {
diff --git a/drivers/net/ethernet/ti/icssg_prueth.c b/drivers/net/ethernet/ti/icssg_prueth.c
index d58588b5294c..95e383a01bc8 100644
--- a/drivers/net/ethernet/ti/icssg_prueth.c
+++ b/drivers/net/ethernet/ti/icssg_prueth.c
@@ -581,6 +581,8 @@ static int emac_rx_packet(struct prueth_emac *emac, u32 flow_id)
 		new_skb = skb;
 	} else {
 		/* send the filled skb up the n/w stack */
+		if (emac->prueth->is_switch_mode)
+			skb->offload_fwd_mark = emac->offload_fwd_mark;
 		skb_put(skb, pkt_len);
 		skb->protocol = eth_type_trans(skb, ndev);
 		netif_receive_skb(skb);
@@ -876,9 +878,10 @@ static int emac_ndo_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	}
 
 	/* set dst tag to indicate internal qid at the firmware which is at
-	 * bit8..bit15
+	 * bit8..bit15. bit0..bit7 indicates port num for directed
+	 * packets in case of switch mode operation
 	 */
-	cppi5_desc_set_tags_ids(&first_desc->hdr, 0, (q_idx << 8));
+	cppi5_desc_set_tags_ids(&first_desc->hdr, 0, (emac->port_id | (q_idx << 8)));
 	cppi5_hdesc_attach_buf(first_desc, buf_dma, pkt_len, buf_dma, pkt_len);
 	swdata = cppi5_hdesc_get_swdata(first_desc);
 	*swdata = skb;
@@ -1155,11 +1158,49 @@ static irqreturn_t prueth_rx_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+struct icssg_firmwares {
+	char *pru;
+	char *rtu;
+	char *txpru;
+};
+
+static struct icssg_firmwares icssg_switch_firmwares[] = {
+	{
+		.pru = "ti-pruss/am65x-sr2-pru0-prusw-fw.elf",
+		.rtu = "ti-pruss/am65x-sr2-rtu0-prusw-fw.elf",
+		.txpru = "ti-pruss/am65x-sr2-txpru0-prusw-fw.elf",
+	},
+	{
+		.pru = "ti-pruss/am65x-sr2-pru1-prusw-fw.elf",
+		.rtu = "ti-pruss/am65x-sr2-rtu1-prusw-fw.elf",
+		.txpru = "ti-pruss/am65x-sr2-txpru1-prusw-fw.elf",
+	}
+};
+
+static struct icssg_firmwares icssg_emac_firmwares[] = {
+	{
+		.pru = "ti-pruss/am65x-sr2-pru0-prueth-fw.elf",
+		.rtu = "ti-pruss/am65x-sr2-rtu0-prueth-fw.elf",
+		.txpru = "ti-pruss/am65x-sr2-txpru0-prueth-fw.elf",
+	},
+	{
+		.pru = "ti-pruss/am65x-sr2-pru1-prueth-fw.elf",
+		.rtu = "ti-pruss/am65x-sr2-rtu1-prueth-fw.elf",
+		.txpru = "ti-pruss/am65x-sr2-txpru1-prueth-fw.elf",
+	}
+};
+
 static int prueth_emac_start(struct prueth *prueth, struct prueth_emac *emac)
 {
+	struct icssg_firmwares *firmwares;
 	struct device *dev = prueth->dev;
 	int slice, ret;
 
+	if (prueth->is_switch_mode)
+		firmwares = icssg_switch_firmwares;
+	else
+		firmwares = icssg_emac_firmwares;
+
 	slice = prueth_emac_slice(emac);
 	if (slice < 0) {
 		netdev_err(emac->ndev, "invalid port\n");
@@ -1175,12 +1216,14 @@ static int prueth_emac_start(struct prueth *prueth, struct prueth_emac *emac)
 			return ret;
 	}
 
+	ret = rproc_set_firmware(prueth->pru[slice], firmwares[slice].pru);
 	ret = rproc_boot(prueth->pru[slice]);
 	if (ret) {
 		dev_err(dev, "failed to boot PRU%d: %d\n", slice, ret);
 		return -EINVAL;
 	}
 
+	ret = rproc_set_firmware(prueth->rtu[slice], firmwares[slice].rtu);
 	ret = rproc_boot(prueth->rtu[slice]);
 	if (ret) {
 		dev_err(dev, "failed to boot RTU%d: %d\n", slice, ret);
@@ -1190,6 +1233,7 @@ static int prueth_emac_start(struct prueth *prueth, struct prueth_emac *emac)
 	if (emac->is_sr1)
 		goto done;
 
+	ret = rproc_set_firmware(prueth->txpru[slice], firmwares[slice].txpru);
 	ret = rproc_boot(prueth->txpru[slice]);
 	if (ret) {
 		dev_err(dev, "failed to boot TX_PRU%d: %d\n", slice, ret);
@@ -1700,6 +1744,18 @@ static int emac_ndo_open(struct net_device *ndev)
 	if (netif_msg_drv(emac))
 		dev_notice(&ndev->dev, "started\n");
 
+	if (prueth->is_switch_mode) {
+		icssg_fdb_add_del(emac, eth_stp_addr, prueth->default_vlan,
+				  ICSSG_FDB_ENTRY_P0_MEMBERSHIP |
+				  ICSSG_FDB_ENTRY_P1_MEMBERSHIP |
+				  ICSSG_FDB_ENTRY_P2_MEMBERSHIP |
+				  ICSSG_FDB_ENTRY_BLOCK,
+				  true);
+		icssg_vtbl_modify(emac, emac->port_vlan, BIT(emac->port_id),
+				  BIT(emac->port_id), true);
+		icssg_set_pvid(emac->prueth, emac->port_vlan, emac->port_id);
+		emac_set_port_state(emac, ICSSG_EMAC_PORT_VLAN_AWARE_ENABLE);
+	}
 	return 0;
 
 reset_tx_chan:
@@ -2000,17 +2056,11 @@ static int emac_ndo_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd)
 	return phy_mii_ioctl(emac->phydev, ifr, cmd);
 }
 
-static int emac_ndo_get_phys_port_name(struct net_device *ndev, char *name, size_t len)
+static struct devlink_port *emac_ndo_get_devlink_port(struct net_device *ndev)
 {
 	struct prueth_emac *emac = netdev_priv(ndev);
-	int err;
-
-	err = snprintf(name, len, "p%d", emac->port_id);
-
-	if (err >= len)
-		return -EINVAL;
 
-	return 0;
+	return &emac->devlink_port;
 }
 
 static const struct net_device_ops emac_netdev_ops = {
@@ -2022,7 +2072,7 @@ static const struct net_device_ops emac_netdev_ops = {
 	.ndo_tx_timeout = emac_ndo_tx_timeout,
 	.ndo_set_rx_mode = emac_ndo_set_rx_mode,
 	.ndo_do_ioctl = emac_ndo_ioctl,
-	.ndo_get_phys_port_name = emac_ndo_get_phys_port_name,
+	.ndo_get_devlink_port = emac_ndo_get_devlink_port,
 };
 
 /* get emac_port corresponding to eth_node name */
@@ -2344,6 +2394,310 @@ static void prueth_put_cores(struct prueth *prueth, int slice)
 		pru_rproc_put(prueth->pru[slice]);
 }
 
+static void prueth_offload_fwd_mark_update(struct prueth *prueth)
+{
+	int set_val = 0;
+	int i;
+
+	if (prueth->br_members == (PRUETH_PORT_MII0 | PRUETH_PORT_MII1))
+		set_val = 1;
+
+	dev_dbg(prueth->dev, "set offload_fwd_mark %d\n", set_val);
+
+	for (i = PRUETH_MAC0; i < PRUETH_NUM_MACS; i++) {
+		struct prueth_emac *emac = prueth->emac[i];
+
+		if (!emac || !emac->ndev)
+			continue;
+
+		emac->offload_fwd_mark = set_val;
+	}
+}
+
+bool prueth_dev_check(const struct net_device *ndev)
+{
+	if (ndev->netdev_ops == &emac_netdev_ops && netif_running(ndev)) {
+		struct prueth_emac *emac = netdev_priv(ndev);
+
+		return emac->prueth->is_switch_mode;
+	}
+
+	return false;
+}
+
+static int prueth_netdevice_port_link(struct net_device *ndev, struct net_device *br_ndev)
+{
+	struct prueth_emac *emac = netdev_priv(ndev);
+	struct prueth *prueth = emac->prueth;
+
+	if (!prueth->is_switch_mode)
+		return NOTIFY_DONE;
+
+	if (!prueth->br_members) {
+		prueth->hw_bridge_dev = br_ndev;
+	} else {
+		/* This is adding the port to a second bridge, this is
+		 * unsupported
+		 */
+		if (prueth->hw_bridge_dev != br_ndev)
+			return -EOPNOTSUPP;
+	}
+
+	prueth->br_members |= BIT(emac->port_id);
+
+	prueth_offload_fwd_mark_update(prueth);
+
+	return NOTIFY_DONE;
+}
+
+static void prueth_netdevice_port_unlink(struct net_device *ndev)
+{
+	struct prueth_emac *emac = netdev_priv(ndev);
+	struct prueth *prueth = emac->prueth;
+
+	prueth->br_members &= ~BIT(emac->port_id);
+
+	prueth_offload_fwd_mark_update(prueth);
+
+	if (!prueth->br_members)
+		prueth->hw_bridge_dev = NULL;
+}
+
+/* netdev notifier */
+static int prueth_netdevice_event(struct notifier_block *unused,
+				  unsigned long event, void *ptr)
+{
+	struct net_device *ndev = netdev_notifier_info_to_dev(ptr);
+	struct netdev_notifier_changeupper_info *info;
+	int ret = NOTIFY_DONE;
+
+	if (ndev->netdev_ops != &emac_netdev_ops)
+		return NOTIFY_DONE;
+
+	switch (event) {
+	case NETDEV_CHANGEUPPER:
+		info = ptr;
+
+		if (netif_is_bridge_master(info->upper_dev)) {
+			if (info->linking)
+				ret = prueth_netdevice_port_link(ndev, info->upper_dev);
+			else
+				prueth_netdevice_port_unlink(ndev);
+		}
+		break;
+	default:
+		return NOTIFY_DONE;
+	}
+
+	return notifier_from_errno(ret);
+}
+
+static int prueth_register_notifiers(struct prueth *prueth)
+{
+	int ret = 0;
+
+	prueth->prueth_netdevice_nb.notifier_call = &prueth_netdevice_event;
+	ret = register_netdevice_notifier(&prueth->prueth_netdevice_nb);
+	if (ret) {
+		dev_err(prueth->dev, "can't register netdevice notifier\n");
+		return ret;
+	}
+
+	ret = prueth_switchdev_register_notifiers(prueth);
+	if (ret)
+		unregister_netdevice_notifier(&prueth->prueth_netdevice_nb);
+
+	return ret;
+}
+
+static void prueth_unregister_notifiers(struct prueth *prueth)
+{
+	prueth_switchdev_unregister_notifiers(prueth);
+	unregister_netdevice_notifier(&prueth->prueth_netdevice_nb);
+}
+
+static const struct devlink_ops prueth_devlink_ops = {};
+
+static int prueth_dl_switch_mode_get(struct devlink *dl, u32 id,
+				     struct devlink_param_gset_ctx *ctx)
+{
+	struct prueth_devlink *dl_priv = devlink_priv(dl);
+	struct prueth *prueth = dl_priv->prueth;
+
+	dev_dbg(prueth->dev, "%s id:%u\n", __func__, id);
+
+	if (id != PRUETH_DL_PARAM_SWITCH_MODE)
+		return -EOPNOTSUPP;
+
+	ctx->val.vbool = prueth->is_switch_mode;
+
+	return 0;
+}
+
+static int prueth_dl_switch_mode_set(struct devlink *dl, u32 id,
+				     struct devlink_param_gset_ctx *ctx)
+{
+	struct prueth_devlink *dl_priv = devlink_priv(dl);
+	struct prueth *prueth = dl_priv->prueth;
+	bool switch_en = ctx->val.vbool;
+	int i;
+
+	dev_dbg(prueth->dev, "%s id:%u\n", __func__, id);
+
+	if (id != PRUETH_DL_PARAM_SWITCH_MODE)
+		return -EOPNOTSUPP;
+
+	if (switch_en == prueth->is_switch_mode)
+		return 0;
+
+	if (!switch_en && prueth->br_members) {
+		dev_err(prueth->dev, "Remove ports from bridge before disabling switch mode\n");
+		return -EINVAL;
+	}
+
+	rtnl_lock();
+
+	prueth->default_vlan = 1;
+	prueth->is_switch_mode = switch_en;
+
+	for (i = PRUETH_MAC0; i < PRUETH_NUM_MACS; i++) {
+		struct net_device *sl_ndev = prueth->emac[i]->ndev;
+
+		if (!sl_ndev || !netif_running(sl_ndev))
+			continue;
+
+		dev_err(prueth->dev, "Cannot switch modes when i/f are up\n");
+		goto exit;
+	}
+
+	for (i = PRUETH_MAC0; i < PRUETH_NUM_MACS; i++) {
+		struct net_device *sl_ndev = prueth->emac[i]->ndev;
+		struct prueth_emac *emac;
+
+		if (!sl_ndev)
+			continue;
+
+		emac = netdev_priv(sl_ndev);
+		if (switch_en)
+			emac->port_vlan = prueth->default_vlan;
+		else
+			emac->port_vlan = 0;
+	}
+
+	dev_info(prueth->dev, "Enabling %s mode\n",
+		 switch_en ? "switch" : "Dual EMAC");
+
+exit:
+	rtnl_unlock();
+
+	return 0;
+}
+
+static const struct devlink_param prueth_devlink_params[] = {
+	DEVLINK_PARAM_DRIVER(PRUETH_DL_PARAM_SWITCH_MODE, "switch_mode",
+			     DEVLINK_PARAM_TYPE_BOOL,
+			     BIT(DEVLINK_PARAM_CMODE_RUNTIME),
+			     prueth_dl_switch_mode_get,
+			     prueth_dl_switch_mode_set, NULL),
+};
+
+static void prueth_unregister_devlink_ports(struct prueth *prueth)
+{
+	struct devlink_port *dl_port;
+	struct prueth_emac *emac;
+	int i;
+
+	for (i = PRUETH_MAC0; i < PRUETH_NUM_MACS; i++) {
+		emac = prueth->emac[i];
+		dl_port = &emac->devlink_port;
+
+		if (dl_port->registered)
+			devlink_port_unregister(dl_port);
+	}
+}
+
+static int prueth_register_devlink(struct prueth *prueth)
+{
+	struct devlink_port_attrs attrs = {};
+	struct prueth_devlink *dl_priv;
+	struct device *dev = prueth->dev;
+	struct devlink_port *dl_port;
+	struct prueth_emac *emac;
+	int ret = 0;
+	int i;
+
+	prueth->devlink =
+		devlink_alloc(&prueth_devlink_ops, sizeof(*dl_priv));
+	if (!prueth->devlink)
+		return -ENOMEM;
+
+	dl_priv = devlink_priv(prueth->devlink);
+	dl_priv->prueth = prueth;
+
+	ret = devlink_register(prueth->devlink, dev);
+	if (ret) {
+		dev_err(dev, "devlink reg fail ret:%d\n", ret);
+		goto dl_free;
+	}
+
+	/* Provide devlink hook to switch mode when multiple external ports
+	 * are present NUSS switchdev driver is enabled.
+	 */
+	if (prueth->is_switchmode_supported) {
+		ret = devlink_params_register(prueth->devlink,
+					      prueth_devlink_params,
+					      ARRAY_SIZE(prueth_devlink_params));
+		if (ret) {
+			dev_err(dev, "devlink params reg fail ret:%d\n", ret);
+			goto dl_unreg;
+		}
+		devlink_params_publish(prueth->devlink);
+	}
+
+	for (i = PRUETH_MAC0; i < PRUETH_NUM_MACS; i++) {
+		emac = prueth->emac[i];
+		dl_port = &emac->devlink_port;
+
+		attrs.flavour = DEVLINK_PORT_FLAVOUR_PHYSICAL;
+		attrs.phys.port_number = emac->port_id;
+		attrs.switch_id.id_len = sizeof(resource_size_t);
+		memcpy(attrs.switch_id.id, prueth->switch_id, attrs.switch_id.id_len);
+		devlink_port_attrs_set(dl_port, &attrs);
+
+		ret = devlink_port_register(prueth->devlink, dl_port, emac->port_id);
+		if (ret) {
+			dev_err(dev, "devlink_port reg fail for port %d, ret:%d\n",
+				emac->port_id, ret);
+			goto dl_port_unreg;
+		}
+		devlink_port_type_eth_set(dl_port, emac->ndev);
+	}
+
+	return ret;
+
+dl_port_unreg:
+	prueth_unregister_devlink_ports(prueth);
+dl_unreg:
+	devlink_unregister(prueth->devlink);
+dl_free:
+	devlink_free(prueth->devlink);
+
+	return ret;
+}
+
+static void prueth_unregister_devlink(struct prueth *prueth)
+{
+	if (prueth->is_switchmode_supported) {
+		devlink_params_unpublish(prueth->devlink);
+		devlink_params_unregister(prueth->devlink, prueth_devlink_params,
+					  ARRAY_SIZE(prueth_devlink_params));
+	}
+
+	prueth_unregister_devlink_ports(prueth);
+	devlink_unregister(prueth->devlink);
+	devlink_free(prueth->devlink);
+}
+
 static const struct of_device_id prueth_dt_match[];
 
 static int prueth_probe(struct platform_device *pdev)
@@ -2447,6 +2801,10 @@ static int prueth_probe(struct platform_device *pdev)
 	}
 
 	msmc_ram_size = prueth->is_sr1 ? MSMC_RAM_SIZE_SR1 : MSMC_RAM_SIZE_SR2;
+	prueth->is_switchmode_supported = prueth->pdata.switch_mode;
+	if (prueth->is_switchmode_supported)
+		msmc_ram_size = MSMC_RAM_SIZE_SR2_SWITCH_MODE;
+
 	if (prueth->is_sr1) {
 		prueth->msmcram.va =
 			(void __iomem *)gen_pool_alloc(prueth->sram_pool,
@@ -2558,6 +2916,18 @@ static int prueth_probe(struct platform_device *pdev)
 		prueth->registered_netdevs[PRUETH_MAC1] = prueth->emac[PRUETH_MAC1]->ndev;
 	}
 
+	if (prueth->is_switchmode_supported) {
+		ret = prueth_register_notifiers(prueth);
+		if (ret)
+			goto netdev_unregister;
+
+		ret = prueth_register_devlink(prueth);
+		if (ret)
+			goto clean_unregister_notifiers;
+
+		sprintf(prueth->switch_id, "%s", dev_name(dev));
+	}
+
 	dev_info(dev, "TI PRU ethernet driver initialized: %s EMAC mode\n",
 		 (!eth0_node || !eth1_node) ? "single" : "dual");
 
@@ -2568,6 +2938,8 @@ static int prueth_probe(struct platform_device *pdev)
 
 	return 0;
 
+clean_unregister_notifiers:
+	prueth_unregister_notifiers(prueth);
 netdev_unregister:
 	for (i = 0; i < PRUETH_NUM_MACS; i++) {
 		if (!prueth->registered_netdevs[i])
@@ -2625,6 +2997,9 @@ static int prueth_remove(struct platform_device *pdev)
 	struct prueth *prueth = platform_get_drvdata(pdev);
 	int i;
 
+	prueth_unregister_notifiers(prueth);
+	prueth_unregister_devlink(prueth);
+
 	for (i = 0; i < PRUETH_NUM_MACS; i++) {
 		if (!prueth->registered_netdevs[i])
 			continue;
@@ -2729,6 +3104,7 @@ static const struct prueth_pdata am654_icssg_pdata_sr1 = {
 static const struct prueth_pdata am654_icssg_pdata = {
 	.fdqring_mode = K3_RINGACC_RING_MODE_MESSAGE,
 	.quirk_10m_link_issue = 1,
+	.switch_mode = 1,
 };
 
 static const struct prueth_pdata am64x_icssg_pdata = {
diff --git a/drivers/net/ethernet/ti/icssg_prueth.h b/drivers/net/ethernet/ti/icssg_prueth.h
index e5068223bdf8..f1b986493218 100644
--- a/drivers/net/ethernet/ti/icssg_prueth.h
+++ b/drivers/net/ethernet/ti/icssg_prueth.h
@@ -298,6 +298,7 @@ struct emac_tx_ts_response {
 
 /* Classifier helpers */
 void icssg_class_set_mac_addr(struct regmap *miig_rt, int slice, u8 *mac);
+void icssg_class_set_host_mac_addr(struct regmap *miig_rt, u8 *mac);
 void icssg_class_disable(struct regmap *miig_rt, int slice);
 void icssg_class_default(struct regmap *miig_rt, int slice, bool allmulti,
 			 bool is_sr1);
