1 files changed, 269 insertions, 83 deletions

diff --git a/drivers/net/cpsw.c b/drivers/net/cpsw.c
index b9a6575009..3aafebaad4 100644
--- a/drivers/net/cpsw.c
+++ b/drivers/net/cpsw.c
@@ -9,7 +9,6 @@
 
 #include <command.h>
 #include <dma.h>
-#include <net.h>
 #include <malloc.h>
 #include <net.h>
 #include <linux/phy.h>
@@ -23,7 +22,7 @@
 #include <asm/system.h>
 #include <linux/err.h>
 
-#include <mach/cpsw.h>
+#include <mach/omap/cpsw.h>
 
 #define CPSW_VERSION_1		0x19010a
 #define CPSW_VERSION_2		0x19010c
@@ -52,6 +51,12 @@
 #define CPDMA_DESC_EOP		BIT(30)
 #define CPDMA_DESC_OWNER	BIT(29)
 #define CPDMA_DESC_EOQ		BIT(28)
+#define CPDMA_DESC_PASS_CRC	BIT(26)
+#define CPDMA_DESC_TO_PORT_EN	BIT(20)
+#define CPDMA_FROM_TO_PORT_SHIFT	16
+#define CPDMA_RX_SOURCE_PORT(__status__)	\
+	(((__status__) >> CPDMA_FROM_TO_PORT_SHIFT) & 0x7)
+#define CPDMA_DESC_CRC_LEN	4
 
 #define SLIVER_SIZE		0x40
 
@@ -161,16 +166,18 @@ enum cpsw_ale_port_state {
 /* ALE unicast entry flags - passed into cpsw_ale_add_ucast() */
 #define ALE_SECURE	1
 #define ALE_BLOCKED	2
+#define ALE_VLAN	4
 
 struct cpsw_slave {
 	struct cpsw_slave_regs		*regs;
 	struct cpsw_sliver_regs		*sliver;
+	int				port_vlan;
 	int				slave_num;
 	int				phy_id;
 	phy_interface_t			phy_if;
 	struct eth_device		edev;
 	struct cpsw_priv		*cpsw;
-	struct device_d			dev;
+	struct device			dev;
 };
 
 struct cpdma_desc {
@@ -190,7 +197,7 @@ struct cpdma_chan {
 };
 
 struct cpsw_priv {
-	struct device_d			*dev;
+	struct device			*dev;
 
 	u32				version;
 	struct cpsw_platform_data	data;
@@ -210,6 +217,8 @@ struct cpsw_priv {
 	unsigned int			slave_size;
 	unsigned int			sliver_ofs;
 
+	void				*rx_buffer[PKTBUFSRX - 2];
+
 	struct cpdma_desc		*descs;
 	struct cpdma_desc		*desc_free;
 	struct cpdma_chan		rx_chan, tx_chan;
@@ -218,7 +227,7 @@ struct cpsw_priv {
 };
 
 struct cpsw_mdio_priv {
-	struct device_d			*dev;
+	struct device			*dev;
 	struct mii_bus			miibus;
 	struct cpsw_mdio_regs		*mdio_regs;
 };
@@ -258,6 +267,7 @@ static inline void cpsw_ale_set_##name(u32 *ale_entry, u32 value)	\
 }
 
 DEFINE_ALE_FIELD(entry_type,		60,	2)
+DEFINE_ALE_FIELD(vlan_id,		48,	12)
 DEFINE_ALE_FIELD(mcast_state,		62,	2)
 DEFINE_ALE_FIELD(port_mask,		66,	3)
 DEFINE_ALE_FIELD(ucast_type,		62,	2)
@@ -265,6 +275,10 @@ DEFINE_ALE_FIELD(port_num,		66,	2)
 DEFINE_ALE_FIELD(blocked,		65,	1)
 DEFINE_ALE_FIELD(secure,		64,	1)
 DEFINE_ALE_FIELD(mcast,			40,	1)
+DEFINE_ALE_FIELD(vlan_untag,		24,	3)
+DEFINE_ALE_FIELD(vlan_reg_mcast,	16,	3)
+DEFINE_ALE_FIELD(vlan_unreg_mcast,	8,	3)
+DEFINE_ALE_FIELD(vlan_member_list,	0,	3)
 
 static char ethbdaddr[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 
@@ -309,6 +323,23 @@ static int cpsw_ale_write(struct cpsw_priv *priv, int idx, u32 *ale_entry)
 	return idx;
 }
 
+static int cpsw_ale_match_vlan(struct cpsw_priv *priv, u16 vid)
+{
+	u32 ale_entry[ALE_ENTRY_WORDS];
+	int type, idx;
+
+	for (idx = 0; idx < priv->ale_entries; idx++) {
+		cpsw_ale_read(priv, idx, ale_entry);
+		type = cpsw_ale_get_entry_type(ale_entry);
+		if (type != ALE_TYPE_VLAN)
+			continue;
+		if (cpsw_ale_get_vlan_id(ale_entry) == vid)
+			return idx;
+	}
+
+	return -ENOENT;
+}
+
 static int cpsw_ale_match_addr(struct cpsw_priv *priv, u8* addr)
 {
 	u32 ale_entry[ALE_ENTRY_WORDS];
@@ -373,13 +404,47 @@ static int cpsw_ale_find_ageable(struct cpsw_priv *priv)
 	return -ENOENT;
 }
 
+static int cpsw_ale_add_vlan(struct cpsw_priv *priv, u16 vid, int port_mask,
+			     int untag, int reg_mcast, int unreg_mcast)
+{
+	u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0};
+	int idx;
+
+	idx = cpsw_ale_match_vlan(priv, vid);
+	if (idx >= 0)
+		cpsw_ale_read(priv, idx, ale_entry);
+
+	cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_VLAN);
+	cpsw_ale_set_vlan_id(ale_entry, vid);
+	cpsw_ale_set_vlan_untag(ale_entry, untag);
+	cpsw_ale_set_vlan_reg_mcast(ale_entry, reg_mcast);
+	cpsw_ale_set_vlan_unreg_mcast(ale_entry, unreg_mcast);
+	cpsw_ale_set_vlan_member_list(ale_entry, port_mask);
+
+	if (idx < 0)
+		idx = cpsw_ale_match_free(priv);
+	if (idx < 0)
+		idx = cpsw_ale_find_ageable(priv);
+	if (idx < 0)
+		return -ENOMEM;
+
+	cpsw_ale_write(priv, idx, ale_entry);
+	return 0;
+}
+
 static int cpsw_ale_add_ucast(struct cpsw_priv *priv, u8 *addr,
-			      int port, int flags)
+			      int port, int flags, u16 vid)
 {
 	u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0};
 	int idx;
 
-	cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_ADDR);
+	if (flags & ALE_VLAN) {
+		cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_VLAN_ADDR);
+		cpsw_ale_set_vlan_id(ale_entry, vid);
+	} else {
+		cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_ADDR);
+	}
+
 	cpsw_ale_set_addr(ale_entry, addr);
 	cpsw_ale_set_ucast_type(ale_entry, ALE_UCAST_PERSISTANT);
 	cpsw_ale_set_secure(ale_entry, (flags & ALE_SECURE) ? 1 : 0);
@@ -398,7 +463,8 @@ static int cpsw_ale_add_ucast(struct cpsw_priv *priv, u8 *addr,
 	return 0;
 }
 
-static int cpsw_ale_add_mcast(struct cpsw_priv *priv, u8 *addr, int port_mask)
+static int cpsw_ale_add_mcast(struct cpsw_priv *priv, u8 *addr, int port_mask,
+			      int flags, u16 vid, int mcast_state)
 {
 	u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0};
 	int idx, mask;
@@ -407,9 +473,14 @@ static int cpsw_ale_add_mcast(struct cpsw_priv *priv, u8 *addr, int port_mask)
 	if (idx >= 0)
 		cpsw_ale_read(priv, idx, ale_entry);
 
-	cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_ADDR);
+	if (flags & ALE_VLAN) {
+		cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_VLAN_ADDR);
+		cpsw_ale_set_vlan_id(ale_entry, vid);
+	} else {
+		cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_ADDR);
+	}
 	cpsw_ale_set_addr(ale_entry, addr);
-	cpsw_ale_set_mcast_state(ale_entry, ALE_MCAST_FWD_2);
+	cpsw_ale_set_mcast_state(ale_entry, mcast_state);
 
 	mask = cpsw_ale_get_port_mask(ale_entry);
 	port_mask |= mask;
@@ -514,7 +585,7 @@ static int cpsw_mdio_write(struct mii_bus *bus, int phy_id, int phy_reg, u16 val
 	return 0;
 }
 
-static int cpsw_mdio_probe(struct device_d *dev)
+static int cpsw_mdio_probe(struct device *dev)
 {
 	struct resource *iores;
 	struct cpsw_mdio_priv *priv;
@@ -524,7 +595,12 @@ static int cpsw_mdio_probe(struct device_d *dev)
 
 	priv = xzalloc(sizeof(*priv));
 
+	/* If we can't request I/O memory region, we'll assume parent did
+	 * it for us
+	 */
 	iores = dev_request_mem_resource(dev, 0);
+	if (IS_ERR(iores) && PTR_ERR(iores) == -EBUSY)
+		iores = dev_get_resource(dev, IORESOURCE_MEM, 0);
 	if (IS_ERR(iores))
 		return PTR_ERR(iores);
 	priv->mdio_regs = IOMEM(iores->start);
@@ -548,7 +624,7 @@ static int cpsw_mdio_probe(struct device_d *dev)
 	 * silicon.  Since the effect of (b) was found to be largely
 	 * negligible, we keep things simple here.
 	 */
-	udelay(1000);
+	udelay(2000);
 
 	start = get_time_ns();
 	while (1) {
@@ -581,8 +657,9 @@ static __maybe_unused struct of_device_id cpsw_mdio_dt_ids[] = {
 		/* sentinel */
 	}
 };
+MODULE_DEVICE_TABLE(of, cpsw_mdio_dt_ids);
 
-static struct driver_d cpsw_mdio_driver = {
+static struct driver cpsw_mdio_driver = {
 	.name   = "cpsw-mdio",
 	.probe  = cpsw_mdio_probe,
 	.of_compatible = DRV_OF_COMPAT(cpsw_mdio_dt_ids),
@@ -673,6 +750,7 @@ static inline u32 cpsw_get_slave_port(struct cpsw_priv *priv, u32 slave_num)
 static void cpsw_slave_init(struct cpsw_slave *slave, struct cpsw_priv *priv)
 {
 	u32	slave_port;
+	u32 port_mask;
 
 	dev_dbg(&slave->dev, "* %s\n", __func__);
 
@@ -689,8 +767,22 @@ static void cpsw_slave_init(struct cpsw_slave *slave, struct cpsw_priv *priv)
 	slave_port = cpsw_get_slave_port(priv, slave->slave_num);
 	cpsw_ale_port_state(priv, slave_port, ALE_PORT_STATE_FORWARD);
 
+	port_mask = BIT(slave_port) | BIT(priv->host_port);
+
+	/* set port_vlan to host_vlan */
+	writel(BIT(slave->slave_num), &slave->regs->port_vlan);
+	slave->port_vlan = readl(&slave->regs->port_vlan);
+	slave->port_vlan &= 0xfff;
+
+	/* add dual emac default entries */
+	cpsw_ale_add_vlan(priv, slave->port_vlan, port_mask,
+			  port_mask, port_mask, 0);
 	/* add broadcast address */
-	cpsw_ale_add_mcast(priv, ethbdaddr, 1 << slave_port);
+	cpsw_ale_add_mcast(priv, ethbdaddr, BIT(priv->host_port), ALE_VLAN,
+			   slave->port_vlan, 0);
+	cpsw_ale_add_ucast(priv, priv->mac_addr, priv->host_port,
+			   ALE_SECURE | ALE_VLAN,
+			   slave->port_vlan);
 }
 
 static struct cpdma_desc *cpdma_desc_alloc(struct cpsw_priv *priv)
@@ -714,7 +806,8 @@ static void cpdma_desc_free(struct cpsw_priv *priv, struct cpdma_desc *desc)
 }
 
 static int cpdma_submit(struct cpsw_priv *priv, struct cpdma_chan *chan,
-			void *buffer, int len)
+			void *sw_buffer, dma_addr_t hw_buffer,
+			int len, int port)
 {
 	struct cpdma_desc *desc, *prev;
 	u32 mode;
@@ -728,11 +821,15 @@ static int cpdma_submit(struct cpsw_priv *priv, struct cpdma_chan *chan,
 
 	mode = CPDMA_DESC_OWNER | CPDMA_DESC_SOP | CPDMA_DESC_EOP;
 
+	if (port)
+		mode |= CPDMA_DESC_TO_PORT_EN |
+			(port << CPDMA_FROM_TO_PORT_SHIFT);
+
 	writel(0, &desc->hw_next);
-	writel((u32)buffer, &desc->hw_buffer);
+	writel(hw_buffer, &desc->hw_buffer);
 	writel(len, &desc->hw_len);
 	writel(mode | len, &desc->hw_mode);
-	writel((u32)buffer, &desc->sw_buffer);
+	writel((u32)sw_buffer, &desc->sw_buffer);
 	writel((u32)len, &desc->sw_len);
 
 	if (!chan->head) {
@@ -758,10 +855,11 @@ done:
 	return 0;
 }
 
-static int cpdma_process(struct cpsw_priv *priv, struct cpdma_chan *chan,
-			 void **buffer, int *len)
+static int cpdma_process(struct cpsw_slave *slave, struct cpdma_chan *chan,
+			 void **buffer, dma_addr_t *dma, int *len)
 {
 	struct cpdma_desc *desc = chan->head;
+	struct cpsw_priv *priv = slave->cpsw;
 	u32 status;
 
 	if (!desc)
@@ -769,9 +867,14 @@ static int cpdma_process(struct cpsw_priv *priv, struct cpdma_chan *chan,
 
 	status = readl(&desc->hw_mode);
 
-	if (len)
+	if (len) {
 		*len = status & 0x7ff;
+		if (status & CPDMA_DESC_PASS_CRC)
+			*len -= CPDMA_DESC_CRC_LEN;
+	}
 
+	if (dma)
+		*dma = readl(&desc->hw_buffer);
 	if (buffer)
 		*buffer = (void *)readl(&desc->sw_buffer);
 
@@ -783,6 +886,14 @@ static int cpdma_process(struct cpsw_priv *priv, struct cpdma_chan *chan,
 		return -EBUSY;
 	}
 
+	/* cpsw_send is cleaning finished descriptors on next send
+	 * so we only have to check for rx channel here
+	 */
+	if (CPDMA_RX_SOURCE_PORT(status) != BIT(slave->slave_num) &&
+	    chan == &priv->rx_chan) {
+		return -ENOMSG;
+	}
+
 	chan->head = (void *)readl(&desc->hw_next);
 
 	writel((u32)desc, chan->cp);
@@ -801,15 +912,25 @@ static int cpsw_open(struct eth_device *edev)
 {
 	struct cpsw_slave *slave = edev->priv;
 	struct cpsw_priv *priv = slave->cpsw;
-	int i, ret;
+	int ret;
 
 	dev_dbg(&slave->dev, "* %s\n", __func__);
 
+	cpsw_slave_init(slave, priv);
+
 	ret = phy_device_connect(edev, NULL, slave->phy_id,
 				 cpsw_adjust_link, 0, slave->phy_if);
 	if (ret)
 		return ret;
 
+	return 0;
+}
+
+static int cpsw_setup(struct device *dev)
+{
+	struct cpsw_priv *priv = dev->priv;
+	int i, ret;
+
 	/* soft reset the controller and initialize priv */
 	soft_reset(priv, &priv->regs->soft_reset);
 
@@ -817,7 +938,10 @@ static int cpsw_open(struct eth_device *edev)
 	cpsw_ale_enable(priv, 1);
 	cpsw_ale_clear(priv, 1);
 	cpsw_ale_bypass(priv, 0);
-	cpsw_ale_vlan_aware(priv, 0); /* vlan unaware mode */
+	cpsw_ale_vlan_aware(priv, 1); /* vlan aware mode */
+
+	/* dual mac mode in fifo */
+	writel(BIT(16), &priv->host_port_regs->flow_thresh);
 
 	/* setup host port priority mapping */
 	writel(0x76543210, &priv->host_port_regs->cpdma_tx_pri_map);
@@ -831,12 +955,6 @@ static int cpsw_open(struct eth_device *edev)
 
 	cpsw_ale_port_state(priv, priv->host_port, ALE_PORT_STATE_FORWARD);
 
-	cpsw_ale_add_ucast(priv, priv->mac_addr, priv->host_port,
-			   ALE_SECURE);
-	cpsw_ale_add_mcast(priv, ethbdaddr, 1 << priv->host_port);
-
-	cpsw_slave_init(slave, priv);
-
 	/* init descriptor pool */
 	for (i = 0; i < NUM_DESCS; i++) {
 		u32 val = (i == (NUM_DESCS - 1)) ? 0 : (u32)&priv->descs[i + 1];
@@ -872,10 +990,17 @@ static int cpsw_open(struct eth_device *edev)
 
 	/* submit rx descs */
 	for (i = 0; i < PKTBUFSRX - 2; i++) {
-		ret = cpdma_submit(priv, &priv->rx_chan, NetRxPackets[i],
-				   PKTSIZE);
+		void *buffer = priv->rx_buffer[i];
+		unsigned len = PKTSIZE;
+		dma_addr_t dma;
+
+		dma = dma_map_single(priv->dev, buffer, len, DMA_FROM_DEVICE);
+		if (dma_mapping_error(priv->dev, dma))
+			return -EFAULT;
+
+		ret = cpdma_submit(priv, &priv->rx_chan, buffer, dma, len, 0);
 		if (ret < 0) {
-			dev_err(&slave->dev, "error %d submitting rx desc\n", ret);
+			dev_err(dev, "error %d submitting rx desc\n", ret);
 			break;
 		}
 	}
@@ -904,19 +1029,21 @@ static int cpsw_send(struct eth_device *edev, void *packet, int length)
 {
 	struct cpsw_slave *slave = edev->priv;
 	struct cpsw_priv *priv = slave->cpsw;
-	void *buffer;
-	int ret, len;
+	dma_addr_t dma;
+	int ret;
 
 	dev_dbg(&slave->dev, "* %s slave %d\n", __func__, slave->slave_num);
 
 	/* first reap completed packets */
-	while (cpdma_process(priv, &priv->tx_chan, &buffer, &len) >= 0);
+	while (cpdma_process(slave, &priv->tx_chan, NULL, NULL, NULL) >= 0)
+		;
 
 	dev_dbg(&slave->dev, "%s: %i bytes @ 0x%p\n", __func__, length, packet);
 
-	dma_sync_single_for_device((unsigned long)packet, length, DMA_TO_DEVICE);
-	ret = cpdma_submit(priv, &priv->tx_chan, packet, length);
-	dma_sync_single_for_cpu((unsigned long)packet, length, DMA_TO_DEVICE);
+	dma = dma_map_single(priv->dev, packet, length, DMA_TO_DEVICE);
+	ret = cpdma_submit(priv, &priv->tx_chan, packet, dma,
+			   length, BIT(slave->slave_num));
+	dma_unmap_single(priv->dev, dma, length, DMA_TO_DEVICE);
 
 	return ret;
 }
@@ -925,16 +1052,15 @@ static int cpsw_recv(struct eth_device *edev)
 {
 	struct cpsw_slave *slave = edev->priv;
 	struct cpsw_priv *priv = slave->cpsw;
+	dma_addr_t dma;
 	void *buffer;
 	int len;
 
-	while (cpdma_process(priv, &priv->rx_chan, &buffer, &len) >= 0) {
-		dma_sync_single_for_cpu((unsigned long)buffer, len,
-				DMA_FROM_DEVICE);
+	while (cpdma_process(slave, &priv->rx_chan, &buffer, &dma, &len) >= 0) {
+		dma_sync_single_for_cpu(priv->dev, dma, len, DMA_FROM_DEVICE);
 		net_receive(edev, buffer, len);
-		dma_sync_single_for_device((unsigned long)buffer, len,
-				DMA_FROM_DEVICE);
-		cpdma_submit(priv, &priv->rx_chan, buffer, PKTSIZE);
+		dma_sync_single_for_device(priv->dev, dma, len, DMA_FROM_DEVICE);
+		cpdma_submit(priv, &priv->rx_chan, buffer, dma, PKTSIZE, 0);
 	}
 
 	return 0;
@@ -954,7 +1080,7 @@ static int cpsw_slave_setup(struct cpsw_slave *slave, int slave_num,
 {
 	void			*regs = priv->regs;
 	struct eth_device	*edev = &slave->edev;
-	struct device_d		*dev = &slave->dev;
+	struct device		*dev = &slave->dev;
 	int ret;
 
 	edev->parent = dev;
@@ -1015,7 +1141,7 @@ static struct cpsw_data cpsw1_data = {
 	.cpdma_reg_ofs		= 0x100,
 	.state_ram_ofs		= 0x200,
 	.ale_reg_ofs		= 0x600,
-	.slave_ofs		= 0x050,
+	.slave_ofs		= 0x058,
 	.slave_size		= 0x040,
 	.sliver_ofs		= 0x700,
 	/* FIXME: mdio_reg_ofs and cppi_ram_ofs missing */
@@ -1026,7 +1152,7 @@ static struct cpsw_data cpsw2_data = {
 	.cpdma_reg_ofs		= 0x800,
 	.state_ram_ofs		= 0xa00,
 	.ale_reg_ofs		= 0xd00,
-	.slave_ofs		= 0x200,
+	.slave_ofs		= 0x208,
 	.slave_size		= 0x100,
 	.sliver_ofs		= 0xd80,
 	.mdio_reg_ofs		= 0x1000,
@@ -1110,11 +1236,27 @@ static void cpsw_gmii_sel_am335x(struct cpsw_slave *slave)
 	writel(reg, phy_sel_addr);
 }
 
-static int cpsw_probe_dt(struct cpsw_priv *priv)
+static void cpsw_add_slave(struct cpsw_slave *slave, struct device_node *child, int i)
 {
-	struct device_d *dev = priv->dev;
-	struct device_node *np = dev->device_node, *child;
-	struct device_node *physel;
+	uint32_t phy_id[2] = {-1, -1};
+	int ret;
+
+	if (!of_find_node_by_name_address(child, "fixed-link")) {
+		ret = of_property_read_u32_array(child, "phy_id", phy_id, 2);
+		if (!ret)
+			dev_warn(slave->cpsw->dev, "phy_id is deprecated, use phy-handle\n");
+	}
+
+	slave->dev.of_node = child;
+	slave->phy_id = phy_id[1];
+	slave->phy_if = of_get_phy_mode(child);
+	slave->slave_num = i;
+}
+
+static int cpsw_legacy_probe_dt(struct cpsw_priv *priv)
+{
+	struct device *dev = priv->dev;
+	struct device_node *np = dev->of_node, *child;
 	int ret, i = 0;
 
 	ret = of_property_read_u32(np, "slaves", &priv->num_slaves);
@@ -1123,15 +1265,6 @@ static int cpsw_probe_dt(struct cpsw_priv *priv)
 
 	priv->slaves = xzalloc(sizeof(struct cpsw_slave) * priv->num_slaves);
 
-	physel = of_find_compatible_node(NULL, NULL, "ti,am3352-phy-gmii-sel");
-	if (!physel) {
-		dev_err(dev, "Cannot find ti,am3352-phy-gmii-sel node\n");
-		return -EINVAL;
-	}
-	ret = cpsw_phy_sel_init(priv, physel);
-	if (ret)
-		return ret;
-
 	for_each_child_of_node(np, child) {
 		if (of_device_is_compatible(child, "ti,davinci_mdio")) {
 			ret = of_pinctrl_select_state_default(child);
@@ -1140,34 +1273,78 @@ static int cpsw_probe_dt(struct cpsw_priv *priv)
 		}
 
 		if (i < priv->num_slaves && !strncmp(child->name, "slave", 5)) {
-			struct cpsw_slave *slave = &priv->slaves[i];
-			uint32_t phy_id[2] = {-1, -1};
+			cpsw_add_slave(&priv->slaves[i], child, i);
+			i++;
+		}
+	}
 
-			if (!of_find_node_by_name(child, "fixed-link")) {
-				ret = of_property_read_u32_array(child, "phy_id", phy_id, 2);
-				if (!ret)
-					dev_warn(dev, "phy_id is deprecated, use phy-handle\n");
-			}
+	return 0;
+}
 
-			slave->dev.device_node = child;
-			slave->phy_id = phy_id[1];
-			slave->phy_if = of_get_phy_mode(child);
-			slave->slave_num = i;
+static int cpsw_switch_probe_dt(struct cpsw_priv *priv)
+{
+	struct device *dev = priv->dev;
+	struct device_node *np = dev->of_node, *child;
+	struct device_node *ports = NULL;
+	int ret, i = 0;
 
-			i++;
+	for_each_child_of_node(np, child) {
+		if (of_device_is_compatible(child, "ti,davinci_mdio")) {
+			ret = of_pinctrl_select_state_default(child);
+			if (ret)
+				return ret;
+		}
+
+		if (!strcmp(child->name, "ethernet-ports")) {
+			ports = child;
+			priv->num_slaves = of_get_available_child_count(ports);
 		}
 	}
 
-	for (i = 0; i < priv->num_slaves; i++) {
-		struct cpsw_slave *slave = &priv->slaves[i];
+	if (!ports)
+		return -EINVAL;
 
-		cpsw_gmii_sel_am335x(slave);
+	priv->slaves = xzalloc(sizeof(struct cpsw_slave) * priv->num_slaves);
+
+	for_each_available_child_of_node(ports, child) {
+		cpsw_add_slave(&priv->slaves[i], child, i);
+		i++;
 	}
 
 	return 0;
 }
 
-static int cpsw_probe(struct device_d *dev)
+static int cpsw_probe_dt(struct cpsw_priv *priv)
+{
+	struct device *dev = priv->dev;
+	struct device_node *physel;
+	int (*probe_slaves_dt)(struct cpsw_priv *priv);
+	int ret, i = 0;
+
+	physel = of_find_compatible_node(NULL, NULL, "ti,am3352-phy-gmii-sel");
+	if (!physel) {
+		dev_err(dev, "Cannot find ti,am3352-phy-gmii-sel node\n");
+		return -EINVAL;
+	}
+	ret = cpsw_phy_sel_init(priv, physel);
+	if (ret)
+		return ret;
+
+	probe_slaves_dt = device_get_match_data(dev);
+	if (!probe_slaves_dt)
+		return -EINVAL;
+
+	ret = probe_slaves_dt(priv);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < priv->num_slaves; i++)
+		cpsw_gmii_sel_am335x(&priv->slaves[i]);
+
+	return 0;
+}
+
+static int cpsw_probe(struct device *dev)
 {
 	struct resource *iores;
 	struct cpsw_platform_data *data = (struct cpsw_platform_data *)dev->platform_data;
@@ -1178,19 +1355,23 @@ static int cpsw_probe(struct device_d *dev)
 
 	dev_dbg(dev, "* %s\n", __func__);
 
-	ret = of_platform_populate(dev->device_node, NULL, dev);
-	if (ret)
-		return ret;
-
 	iores = dev_request_mem_resource(dev, 0);
 	if (IS_ERR(iores))
 		return PTR_ERR(iores);
 	regs = IOMEM(iores->start);
 
+	ret = of_platform_populate(dev->of_node, NULL, dev);
+	if (ret)
+		return ret;
+
 	priv = xzalloc(sizeof(*priv));
 	priv->dev = dev;
 
-	if (dev->device_node) {
+	ret = net_alloc_packets(priv->rx_buffer, ARRAY_SIZE(priv->rx_buffer));
+	if (ret)
+		goto out;
+
+	if (dev->of_node) {
 		ret = cpsw_probe_dt(priv);
 		if (ret)
 			goto out;
@@ -1241,6 +1422,8 @@ static int cpsw_probe(struct device_d *dev)
 
 	dev->priv = priv;
 
+	cpsw_setup(dev);
+
 	return 0;
 out:
 	free(priv->slaves);
@@ -1249,7 +1432,7 @@ out:
 	return ret;
 }
 
-static void cpsw_remove(struct device_d *dev)
+static void cpsw_remove(struct device *dev)
 {
 	struct cpsw_priv	*priv = dev->priv;
 	int i;
@@ -1265,13 +1448,16 @@ static void cpsw_remove(struct device_d *dev)
 
 static __maybe_unused struct of_device_id cpsw_dt_ids[] = {
 	{
-		.compatible = "ti,cpsw",
+		.compatible = "ti,cpsw", .data = cpsw_legacy_probe_dt
+	}, {
+		.compatible = "ti,cpsw-switch", .data = cpsw_switch_probe_dt
 	}, {
 		/* sentinel */
 	}
 };
+MODULE_DEVICE_TABLE(of, cpsw_dt_ids);
 
-static struct driver_d cpsw_driver = {
+static struct driver cpsw_driver = {
 	.name   = "cpsw",
 	.probe  = cpsw_probe,
 	.remove = cpsw_remove,