drivers/net/mscc_eswitch/servalt_switch.c - u-boot-mtk - Git at Google

 // SPDX-License-Identifier: (GPL-2.0+ OR MIT)
 /*
  * Copyright (c) 2019 Microsemi Corporation
  */

 #include <common.h>
 #include <config.h>
 #include <dm.h>
 #include <dm/of_access.h>
 #include <dm/of_addr.h>
 #include <fdt_support.h>
 #include <linux/io.h>
 #include <linux/ioport.h>
 #include <miiphy.h>
 #include <net.h>
 #include <wait_bit.h>

 #include "mscc_xfer.h"
 #include "mscc_miim.h"

 #define PHY_CFG				0x0
 #define PHY_CFG_ENA				0x3
 #define PHY_CFG_COMMON_RST			BIT(2)
 #define PHY_CFG_RST				(0x3 << 3)
 #define PHY_STAT			0x4
 #define PHY_STAT_SUPERVISOR_COMPLETE		BIT(0)

 #define ANA_AC_RAM_CTRL_RAM_INIT		0x14fdc
 #define ANA_AC_STAT_GLOBAL_CFG_PORT_RESET	0x15474

 #define ANA_CL_PORT_VLAN_CFG(x)			(0xa018 + 0xc8 * (x))
 #define		ANA_CL_PORT_VLAN_CFG_AWARE_ENA			BIT(19)
 #define		ANA_CL_PORT_VLAN_CFG_POP_CNT(x)			((x) << 17)

 #define ANA_L2_COMMON_FWD_CFG			0x18498
 #define		ANA_L2_COMMON_FWD_CFG_CPU_DMAC_COPY_ENA	BIT(6)

 #define ASM_CFG_STAT_CFG			0xb08
 #define ASM_CFG_PORT(x)				(0xb74 + 0x4 * (x))
 #define		ASM_CFG_PORT_NO_PREAMBLE_ENA		BIT(8)
 #define		ASM_CFG_PORT_INJ_FORMAT_CFG(x)		((x) << 1)
 #define ASM_RAM_CTRL_RAM_INIT			0xbfc

 #define DEV_DEV_CFG_DEV_RST_CTRL	0x0
 #define		DEV_DEV_CFG_DEV_RST_CTRL_SPEED_SEL(x)	((x) << 20)
 #define DEV_MAC_CFG_MAC_ENA		0x24
 #define		DEV_MAC_CFG_MAC_ENA_RX_ENA		BIT(4)
 #define		DEV_MAC_CFG_MAC_ENA_TX_ENA		BIT(0)
 #define DEV_MAC_CFG_MAC_IFG		0x3c
 #define		DEV_MAC_CFG_MAC_IFG_TX_IFG(x)		((x) << 8)
 #define		DEV_MAC_CFG_MAC_IFG_RX_IFG2(x)		((x) << 4)
 #define		DEV_MAC_CFG_MAC_IFG_RX_IFG1(x)		(x)
 #define DEV_PCS1G_CFG_PCS1G_CFG		0x48
 #define		DEV_PCS1G_CFG_PCS1G_CFG_PCS_ENA		BIT(0)
 #define DEV_PCS1G_CFG_PCS1G_MODE	0x4c
 #define DEV_PCS1G_CFG_PCS1G_SD		0x50
 #define DEV_PCS1G_CFG_PCS1G_ANEG	0x54
 #define		DEV_PCS1G_CFG_PCS1G_ANEG_ADV_ABILITY(x)	((x) << 16)

 #define LRN_COMMON_ACCESS_CTRL			0x0
 #define		LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT	BIT(0)
 #define LRN_COMMON_MAC_ACCESS_CFG0		0x4
 #define LRN_COMMON_MAC_ACCESS_CFG1		0x8
 #define LRN_COMMON_MAC_ACCESS_CFG2		0xc
 #define		LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_ADDR(x)	(x)
 #define		LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_TYPE(x)	((x) << 12)
 #define		LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_VLD	BIT(15)
 #define		LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_LOCKED	BIT(16)
 #define		LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_COPY	BIT(23)
 #define		LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_QU(x)	((x) << 24)

 #define QFWD_SYSTEM_SWITCH_PORT_MODE(x)		(0x4400 + 0x4 * (x))
 #define		QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA		BIT(17)

 #define QS_XTR_GRP_CFG(x)		(4 * (x))
 #define QS_INJ_GRP_CFG(x)		(0x24 + (x) * 4)

 #define QSYS_SYSTEM_RESET_CFG			0x1048
 #define QSYS_CALCFG_CAL_AUTO			0x1134
 #define QSYS_CALCFG_CAL_CTRL			0x113c
 #define		QSYS_CALCFG_CAL_CTRL_CAL_MODE(x)		((x) << 11)
 #define QSYS_RAM_CTRL_RAM_INIT			0x1140

 #define REW_RAM_CTRL_RAM_INIT			0xFFF4

 #define MAC_VID			0
 #define CPU_PORT		11
 #define IFH_LEN			7
 #define ETH_ALEN		6
 #define PGID_BROADCAST		50
 #define PGID_UNICAST		51

 static const char * const regs_names[] = {
 	"port0", "port1",
 	"ana_ac", "ana_cl", "ana_l2", "asm", "lrn", "qfwd", "qs", "qsys", "rew",
 };

 #define REGS_NAMES_COUNT ARRAY_SIZE(regs_names) + 1
 #define MAX_PORT 2

 enum servalt_ctrl_regs {
 	ANA_AC = MAX_PORT,
 	ANA_CL,
 	ANA_L2,
 	ASM,
 	LRN,
 	QFWD,
 	QS,
 	QSYS,
 	REW,
 };

 #define SERVALT_MIIM_BUS_COUNT 2

 struct servalt_phy_port_t {
 	size_t phy_addr;
 	struct mii_dev *bus;
 };

 struct servalt_private {
 	void __iomem *regs[REGS_NAMES_COUNT];
 	struct mii_dev *bus[SERVALT_MIIM_BUS_COUNT];
 	struct servalt_phy_port_t ports[MAX_PORT];
 };

 static const unsigned long servalt_regs_qs[] = {
 	[MSCC_QS_XTR_RD] = 0x8,
 	[MSCC_QS_XTR_FLUSH] = 0x18,
 	[MSCC_QS_XTR_DATA_PRESENT] = 0x1c,
 	[MSCC_QS_INJ_WR] = 0x2c,
 	[MSCC_QS_INJ_CTRL] = 0x34,
 };

 static struct mscc_miim_dev miim[SERVALT_MIIM_BUS_COUNT];
 static int miim_count = -1;

 static void mscc_phy_reset(void)
 {
 	writel(0, BASE_DEVCPU_GCB + GCB_PHY_CFG + PHY_CFG);
 	writel(PHY_CFG_RST | PHY_CFG_COMMON_RST
 	       | PHY_CFG_ENA, BASE_DEVCPU_GCB + GCB_PHY_CFG + PHY_CFG);
 	if (wait_for_bit_le32((const void *)(BASE_DEVCPU_GCB + GCB_PHY_CFG) +
 			      PHY_STAT, PHY_STAT_SUPERVISOR_COMPLETE,
 			      true, 2000, false)) {
 		pr_err("Timeout in phy reset\n");
 	}
 }

 static void servalt_cpu_capture_setup(struct servalt_private *priv)
 {
 	/* ASM: No preamble and IFH prefix on CPU injected frames */
 	writel(ASM_CFG_PORT_NO_PREAMBLE_ENA |
 	       ASM_CFG_PORT_INJ_FORMAT_CFG(1),
 	       priv->regs[ASM] + ASM_CFG_PORT(CPU_PORT));

 	/* Set Manual injection via DEVCPU_QS registers for CPU queue 0 */
 	writel(0x5, priv->regs[QS] + QS_INJ_GRP_CFG(0));

 	/* Set Manual extraction via DEVCPU_QS registers for CPU queue 0 */
 	writel(0x7, priv->regs[QS] + QS_XTR_GRP_CFG(0));

 	/* Enable CPU port for any frame transfer */
 	setbits_le32(priv->regs[QFWD] + QFWD_SYSTEM_SWITCH_PORT_MODE(CPU_PORT),
 		     QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA);

 	/* Send a copy to CPU when found as forwarding entry */
 	setbits_le32(priv->regs[ANA_L2] + ANA_L2_COMMON_FWD_CFG,
 		     ANA_L2_COMMON_FWD_CFG_CPU_DMAC_COPY_ENA);
 }

 static void servalt_port_init(struct servalt_private *priv, int port)
 {
 	void __iomem *regs = priv->regs[port];

 	/* Enable PCS */
 	writel(DEV_PCS1G_CFG_PCS1G_CFG_PCS_ENA,
 	       regs + DEV_PCS1G_CFG_PCS1G_CFG);

 	/* Disable Signal Detect */
 	writel(0, regs + DEV_PCS1G_CFG_PCS1G_SD);

 	/* Enable MAC RX and TX */
 	writel(DEV_MAC_CFG_MAC_ENA_RX_ENA |
 	       DEV_MAC_CFG_MAC_ENA_TX_ENA,
 	       regs + DEV_MAC_CFG_MAC_ENA);

 	/* Clear sgmii_mode_ena */
 	writel(0, regs + DEV_PCS1G_CFG_PCS1G_MODE);

 	/*
 	 * Clear sw_resolve_ena(bit 0) and set adv_ability to
 	 * something meaningful just in case
 	 */
 	writel(DEV_PCS1G_CFG_PCS1G_ANEG_ADV_ABILITY(0x20),
 	       regs + DEV_PCS1G_CFG_PCS1G_ANEG);

 	/* Set MAC IFG Gaps */
 	writel(DEV_MAC_CFG_MAC_IFG_TX_IFG(4) |
 	       DEV_MAC_CFG_MAC_IFG_RX_IFG1(5) |
 	       DEV_MAC_CFG_MAC_IFG_RX_IFG2(1),
 	       regs + DEV_MAC_CFG_MAC_IFG);

 	/* Set link speed and release all resets */
 	writel(DEV_DEV_CFG_DEV_RST_CTRL_SPEED_SEL(2),
 	       regs + DEV_DEV_CFG_DEV_RST_CTRL);

 	/* Make VLAN aware for CPU traffic */
 	writel(ANA_CL_PORT_VLAN_CFG_AWARE_ENA |
 	       ANA_CL_PORT_VLAN_CFG_POP_CNT(1) |
 	       MAC_VID,
 	       priv->regs[ANA_CL] + ANA_CL_PORT_VLAN_CFG(port));

 	/* Enable CPU port for any frame transfer */
 	setbits_le32(priv->regs[QFWD] + QFWD_SYSTEM_SWITCH_PORT_MODE(port),
 		     QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA);
 }

 static int ram_init(u32 val, void __iomem *addr)
 {
 	writel(val, addr);

 	if (wait_for_bit_le32(addr, BIT(1), false, 2000, false)) {
 		printf("Timeout in memory reset, reg = 0x%08x\n", val);
 		return 1;
 	}

 	return 0;
 }

 static int servalt_switch_init(struct servalt_private *priv)
 {
 	/* Initialize memories */
 	ram_init(0x3, priv->regs[QSYS] + QSYS_RAM_CTRL_RAM_INIT);
 	ram_init(0x3, priv->regs[ASM] + ASM_RAM_CTRL_RAM_INIT);
 	ram_init(0x3, priv->regs[ANA_AC] + ANA_AC_RAM_CTRL_RAM_INIT);
 	ram_init(0x3, priv->regs[REW] + REW_RAM_CTRL_RAM_INIT);

 	/* Reset counters */
 	writel(0x1, priv->regs[ANA_AC] + ANA_AC_STAT_GLOBAL_CFG_PORT_RESET);
 	writel(0x1, priv->regs[ASM] + ASM_CFG_STAT_CFG);

 	/* Enable switch-core and queue system */
 	writel(0x1, priv->regs[QSYS] + QSYS_SYSTEM_RESET_CFG);

 	return 0;
 }

 static void servalt_switch_config(struct servalt_private *priv)
 {
 	writel(0x55555555, priv->regs[QSYS] + QSYS_CALCFG_CAL_AUTO);

 	writel(readl(priv->regs[QSYS] + QSYS_CALCFG_CAL_CTRL) |
 	       QSYS_CALCFG_CAL_CTRL_CAL_MODE(8),
 	       priv->regs[QSYS] + QSYS_CALCFG_CAL_CTRL);
 }

 static int servalt_initialize(struct servalt_private *priv)
 {
 	int ret, i;

 	/* Initialize switch memories, enable core */
 	ret = servalt_switch_init(priv);
 	if (ret)
 		return ret;

 	servalt_switch_config(priv);

 	for (i = 0; i < MAX_PORT; i++)
 		servalt_port_init(priv, i);

 	servalt_cpu_capture_setup(priv);

 	return 0;
 }

 static inline
 int servalt_vlant_wait_for_completion(struct servalt_private *priv)
 {
 	if (wait_for_bit_le32(priv->regs[LRN] + LRN_COMMON_ACCESS_CTRL,
 			      LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT,
 			      false, 2000, false))
 		return -ETIMEDOUT;

 	return 0;
 }

 static int servalt_mac_table_add(struct servalt_private *priv,
 				 const unsigned char mac[ETH_ALEN], int pgid)
 {
 	u32 macl = 0, mach = 0;

 	/*
 	 * Set the MAC address to handle and the vlan associated in a format
 	 * understood by the hardware.
 	 */
 	mach |= MAC_VID << 16;
 	mach |= ((u32)mac[0]) << 8;
 	mach |= ((u32)mac[1]) << 0;
 	macl |= ((u32)mac[2]) << 24;
 	macl |= ((u32)mac[3]) << 16;
 	macl |= ((u32)mac[4]) << 8;
 	macl |= ((u32)mac[5]) << 0;

 	writel(mach, priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG0);
 	writel(macl, priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG1);

 	writel(LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_ADDR(pgid) |
 	       LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_TYPE(0x3) |
 	       LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_COPY |
 	       LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_QU(0) |
 	       LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_VLD |
 	       LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_LOCKED,
 	       priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG2);

 	writel(LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT,
 	       priv->regs[LRN] + LRN_COMMON_ACCESS_CTRL);

 	return servalt_vlant_wait_for_completion(priv);
 }

 static int servalt_write_hwaddr(struct udevice *dev)
 {
 	struct servalt_private *priv = dev_get_priv(dev);
 	struct eth_pdata *pdata = dev_get_platdata(dev);

 	return servalt_mac_table_add(priv, pdata->enetaddr, PGID_UNICAST);
 }

 static int servalt_start(struct udevice *dev)
 {
 	struct servalt_private *priv = dev_get_priv(dev);
 	struct eth_pdata *pdata = dev_get_platdata(dev);
 	const unsigned char mac[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff,
 		0xff };
 	int ret;

 	ret = servalt_initialize(priv);
 	if (ret)
 		return ret;

 	/* Set MAC address tables entries for CPU redirection */
 	ret = servalt_mac_table_add(priv, mac, PGID_BROADCAST);
 	if (ret)
 		return ret;

 	ret = servalt_mac_table_add(priv, pdata->enetaddr, PGID_UNICAST);
 	if (ret)
 		return ret;

 	return 0;
 }

 static void servalt_stop(struct udevice *dev)
 {
 }

 static int servalt_send(struct udevice *dev, void *packet, int length)
 {
 	struct servalt_private *priv = dev_get_priv(dev);
 	u32 ifh[IFH_LEN];
 	u32 *buf = packet;

 	memset(ifh, '\0', IFH_LEN * 4);

 	/* Set DST PORT_MASK */
 	ifh[0] = htonl(0);
 	ifh[1] = htonl(0x1FFFFF);
 	ifh[2] = htonl(~0);
 	/* Set DST_MODE to INJECT and UPDATE_FCS */
 	ifh[5] = htonl(0x4c0);

 	return mscc_send(priv->regs[QS], servalt_regs_qs,
 			 ifh, IFH_LEN, buf, length);
 }

 static int servalt_recv(struct udevice *dev, int flags, uchar **packetp)
 {
 	struct servalt_private *priv = dev_get_priv(dev);
 	u32 *rxbuf = (u32 *)net_rx_packets[0];
 	int byte_cnt = 0;

 	byte_cnt = mscc_recv(priv->regs[QS], servalt_regs_qs, rxbuf, IFH_LEN,
 			     false);

 	*packetp = net_rx_packets[0];

 	return byte_cnt;
 }

 static struct mii_dev *get_mdiobus(phys_addr_t base, unsigned long size)
 {
 	int i = 0;

 	for (i = 0; i < SERVALT_MIIM_BUS_COUNT; ++i)
 		if (miim[i].miim_base == base && miim[i].miim_size == size)
 			return miim[i].bus;

 	return NULL;
 }

 static void add_port_entry(struct servalt_private *priv, size_t index,
 			   size_t phy_addr, struct mii_dev *bus)
 {
 	priv->ports[index].phy_addr = phy_addr;
 	priv->ports[index].bus = bus;
 }

 static int servalt_probe(struct udevice *dev)
 {
 	struct servalt_private *priv = dev_get_priv(dev);
 	int i;
 	struct resource res;
 	fdt32_t faddr;
 	phys_addr_t addr_base;
 	unsigned long addr_size;
 	ofnode eth_node, node, mdio_node;
 	size_t phy_addr;
 	struct mii_dev *bus;
 	struct ofnode_phandle_args phandle;

 	if (!priv)
 		return -EINVAL;

 	/* Get registers and map them to the private structure */
 	for (i = 0; i < ARRAY_SIZE(regs_names); i++) {
 		priv->regs[i] = dev_remap_addr_name(dev, regs_names[i]);
 		if (!priv->regs[i]) {
 			debug
 			    ("Error can't get regs base addresses for %s\n",
 			     regs_names[i]);
 			return -ENOMEM;
 		}
 	}

 	/* Initialize miim buses */
 	memset(&miim, 0x0, sizeof(struct mscc_miim_dev) *
 	       SERVALT_MIIM_BUS_COUNT);

 	/* iterate all the ports and find out on which bus they are */
 	i = 0;
 	eth_node = dev_read_first_subnode(dev);
 	for (node = ofnode_first_subnode(eth_node);
 	     ofnode_valid(node);
 	     node = ofnode_next_subnode(node)) {
 		if (ofnode_read_resource(node, 0, &res))
 			return -ENOMEM;
 		i = res.start;

 		ofnode_parse_phandle_with_args(node, "phy-handle", NULL, 0, 0,
 					       &phandle);

 		/* Get phy address on mdio bus */
 		if (ofnode_read_resource(phandle.node, 0, &res))
 			return -ENOMEM;
 		phy_addr = res.start;

 		/* Get mdio node */
 		mdio_node = ofnode_get_parent(phandle.node);

 		if (ofnode_read_resource(mdio_node, 0, &res))
 			return -ENOMEM;
 		faddr = cpu_to_fdt32(res.start);

 		addr_base = ofnode_translate_address(mdio_node, &faddr);
 		addr_size = res.end - res.start;

 		/* If the bus is new then create a new bus */
 		if (!get_mdiobus(addr_base, addr_size))
 			priv->bus[miim_count] =
 				mscc_mdiobus_init(miim, &miim_count, addr_base,
 						  addr_size);

 		/* Connect mdio bus with the port */
 		bus = get_mdiobus(addr_base, addr_size);
 		add_port_entry(priv, i, phy_addr, bus);
 	}

 	mscc_phy_reset();

 	for (i = 0; i < MAX_PORT; i++) {
 		if (!priv->ports[i].bus)
 			continue;

 		phy_connect(priv->ports[i].bus, priv->ports[i].phy_addr, dev,
 			    PHY_INTERFACE_MODE_NONE);
 	}

 	return 0;
 }

 static int servalt_remove(struct udevice *dev)
 {
 	struct servalt_private *priv = dev_get_priv(dev);
 	int i;

 	for (i = 0; i < SERVALT_MIIM_BUS_COUNT; i++) {
 		mdio_unregister(priv->bus[i]);
 		mdio_free(priv->bus[i]);
 	}

 	return 0;
 }

 static const struct eth_ops servalt_ops = {
 	.start        = servalt_start,
 	.stop         = servalt_stop,
 	.send         = servalt_send,
 	.recv         = servalt_recv,
 	.write_hwaddr = servalt_write_hwaddr,
 };

 static const struct udevice_id mscc_servalt_ids[] = {
 	{.compatible = "mscc,vsc7437-switch" },
 	{ /* Sentinel */ }
 };

 U_BOOT_DRIVER(servalt) = {
 	.name				= "servalt-switch",
 	.id				= UCLASS_ETH,
 	.of_match			= mscc_servalt_ids,
 	.probe				= servalt_probe,
 	.remove				= servalt_remove,
 	.ops				= &servalt_ops,
 	.priv_auto_alloc_size		= sizeof(struct servalt_private),
 	.platdata_auto_alloc_size	= sizeof(struct eth_pdata),
 };
	// SPDX-License-Identifier: (GPL-2.0+ OR MIT)
	/*
	* Copyright (c) 2019 Microsemi Corporation
	*/

	#include <common.h>
	#include <config.h>
	#include <dm.h>
	#include <dm/of_access.h>
	#include <dm/of_addr.h>
	#include <fdt_support.h>
	#include <linux/io.h>
	#include <linux/ioport.h>
	#include <miiphy.h>
	#include <net.h>
	#include <wait_bit.h>

	#include "mscc_xfer.h"
	#include "mscc_miim.h"

	#define PHY_CFG 0x0
	#define PHY_CFG_ENA 0x3
	#define PHY_CFG_COMMON_RST BIT(2)
	#define PHY_CFG_RST (0x3 << 3)
	#define PHY_STAT 0x4
	#define PHY_STAT_SUPERVISOR_COMPLETE BIT(0)

	#define ANA_AC_RAM_CTRL_RAM_INIT 0x14fdc
	#define ANA_AC_STAT_GLOBAL_CFG_PORT_RESET 0x15474

	#define ANA_CL_PORT_VLAN_CFG(x) (0xa018 + 0xc8 * (x))
	#define ANA_CL_PORT_VLAN_CFG_AWARE_ENA BIT(19)
	#define ANA_CL_PORT_VLAN_CFG_POP_CNT(x) ((x) << 17)

	#define ANA_L2_COMMON_FWD_CFG 0x18498
	#define ANA_L2_COMMON_FWD_CFG_CPU_DMAC_COPY_ENA BIT(6)

	#define ASM_CFG_STAT_CFG 0xb08
	#define ASM_CFG_PORT(x) (0xb74 + 0x4 * (x))
	#define ASM_CFG_PORT_NO_PREAMBLE_ENA BIT(8)
	#define ASM_CFG_PORT_INJ_FORMAT_CFG(x) ((x) << 1)
	#define ASM_RAM_CTRL_RAM_INIT 0xbfc

	#define DEV_DEV_CFG_DEV_RST_CTRL 0x0
	#define DEV_DEV_CFG_DEV_RST_CTRL_SPEED_SEL(x) ((x) << 20)
	#define DEV_MAC_CFG_MAC_ENA 0x24
	#define DEV_MAC_CFG_MAC_ENA_RX_ENA BIT(4)
	#define DEV_MAC_CFG_MAC_ENA_TX_ENA BIT(0)
	#define DEV_MAC_CFG_MAC_IFG 0x3c
	#define DEV_MAC_CFG_MAC_IFG_TX_IFG(x) ((x) << 8)
	#define DEV_MAC_CFG_MAC_IFG_RX_IFG2(x) ((x) << 4)
	#define DEV_MAC_CFG_MAC_IFG_RX_IFG1(x) (x)
	#define DEV_PCS1G_CFG_PCS1G_CFG 0x48
	#define DEV_PCS1G_CFG_PCS1G_CFG_PCS_ENA BIT(0)
	#define DEV_PCS1G_CFG_PCS1G_MODE 0x4c
	#define DEV_PCS1G_CFG_PCS1G_SD 0x50
	#define DEV_PCS1G_CFG_PCS1G_ANEG 0x54
	#define DEV_PCS1G_CFG_PCS1G_ANEG_ADV_ABILITY(x) ((x) << 16)

	#define LRN_COMMON_ACCESS_CTRL 0x0
	#define LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT BIT(0)
	#define LRN_COMMON_MAC_ACCESS_CFG0 0x4
	#define LRN_COMMON_MAC_ACCESS_CFG1 0x8
	#define LRN_COMMON_MAC_ACCESS_CFG2 0xc
	#define LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_ADDR(x) (x)
	#define LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_TYPE(x) ((x) << 12)
	#define LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_VLD BIT(15)
	#define LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_LOCKED BIT(16)
	#define LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_COPY BIT(23)
	#define LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_QU(x) ((x) << 24)

	#define QFWD_SYSTEM_SWITCH_PORT_MODE(x) (0x4400 + 0x4 * (x))
	#define QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA BIT(17)

	#define QS_XTR_GRP_CFG(x) (4 * (x))
	#define QS_INJ_GRP_CFG(x) (0x24 + (x) * 4)

	#define QSYS_SYSTEM_RESET_CFG 0x1048
	#define QSYS_CALCFG_CAL_AUTO 0x1134
	#define QSYS_CALCFG_CAL_CTRL 0x113c
	#define QSYS_CALCFG_CAL_CTRL_CAL_MODE(x) ((x) << 11)
	#define QSYS_RAM_CTRL_RAM_INIT 0x1140

	#define REW_RAM_CTRL_RAM_INIT 0xFFF4

	#define MAC_VID 0
	#define CPU_PORT 11
	#define IFH_LEN 7
	#define ETH_ALEN 6
	#define PGID_BROADCAST 50
	#define PGID_UNICAST 51

	static const char * const regs_names[] = {
	"port0", "port1",
	"ana_ac", "ana_cl", "ana_l2", "asm", "lrn", "qfwd", "qs", "qsys", "rew",
	};

	#define REGS_NAMES_COUNT ARRAY_SIZE(regs_names) + 1
	#define MAX_PORT 2

	enum servalt_ctrl_regs {
	ANA_AC = MAX_PORT,
	ANA_CL,
	ANA_L2,
	ASM,
	LRN,
	QFWD,
	QS,
	QSYS,
	REW,
	};

	#define SERVALT_MIIM_BUS_COUNT 2

	struct servalt_phy_port_t {
	size_t phy_addr;
	struct mii_dev *bus;
	};

	struct servalt_private {
	void __iomem *regs[REGS_NAMES_COUNT];
	struct mii_dev *bus[SERVALT_MIIM_BUS_COUNT];
	struct servalt_phy_port_t ports[MAX_PORT];
	};

	static const unsigned long servalt_regs_qs[] = {
	[MSCC_QS_XTR_RD] = 0x8,
	[MSCC_QS_XTR_FLUSH] = 0x18,
	[MSCC_QS_XTR_DATA_PRESENT] = 0x1c,
	[MSCC_QS_INJ_WR] = 0x2c,
	[MSCC_QS_INJ_CTRL] = 0x34,
	};

	static struct mscc_miim_dev miim[SERVALT_MIIM_BUS_COUNT];
	static int miim_count = -1;

	static void mscc_phy_reset(void)
	{
	writel(0, BASE_DEVCPU_GCB + GCB_PHY_CFG + PHY_CFG);
	writel(PHY_CFG_RST \| PHY_CFG_COMMON_RST
	\| PHY_CFG_ENA, BASE_DEVCPU_GCB + GCB_PHY_CFG + PHY_CFG);
	if (wait_for_bit_le32((const void *)(BASE_DEVCPU_GCB + GCB_PHY_CFG) +
	PHY_STAT, PHY_STAT_SUPERVISOR_COMPLETE,
	true, 2000, false)) {
	pr_err("Timeout in phy reset\n");
	}
	}

	static void servalt_cpu_capture_setup(struct servalt_private *priv)
	{
	/* ASM: No preamble and IFH prefix on CPU injected frames */
	writel(ASM_CFG_PORT_NO_PREAMBLE_ENA \|
	ASM_CFG_PORT_INJ_FORMAT_CFG(1),
	priv->regs[ASM] + ASM_CFG_PORT(CPU_PORT));

	/* Set Manual injection via DEVCPU_QS registers for CPU queue 0 */
	writel(0x5, priv->regs[QS] + QS_INJ_GRP_CFG(0));

	/* Set Manual extraction via DEVCPU_QS registers for CPU queue 0 */
	writel(0x7, priv->regs[QS] + QS_XTR_GRP_CFG(0));

	/* Enable CPU port for any frame transfer */
	setbits_le32(priv->regs[QFWD] + QFWD_SYSTEM_SWITCH_PORT_MODE(CPU_PORT),
	QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA);

	/* Send a copy to CPU when found as forwarding entry */
	setbits_le32(priv->regs[ANA_L2] + ANA_L2_COMMON_FWD_CFG,
	ANA_L2_COMMON_FWD_CFG_CPU_DMAC_COPY_ENA);
	}

	static void servalt_port_init(struct servalt_private *priv, int port)
	{
	void __iomem *regs = priv->regs[port];

	/* Enable PCS */
	writel(DEV_PCS1G_CFG_PCS1G_CFG_PCS_ENA,
	regs + DEV_PCS1G_CFG_PCS1G_CFG);

	/* Disable Signal Detect */
	writel(0, regs + DEV_PCS1G_CFG_PCS1G_SD);

	/* Enable MAC RX and TX */
	writel(DEV_MAC_CFG_MAC_ENA_RX_ENA \|
	DEV_MAC_CFG_MAC_ENA_TX_ENA,
	regs + DEV_MAC_CFG_MAC_ENA);

	/* Clear sgmii_mode_ena */
	writel(0, regs + DEV_PCS1G_CFG_PCS1G_MODE);

	/*
	* Clear sw_resolve_ena(bit 0) and set adv_ability to
	* something meaningful just in case
	*/
	writel(DEV_PCS1G_CFG_PCS1G_ANEG_ADV_ABILITY(0x20),
	regs + DEV_PCS1G_CFG_PCS1G_ANEG);

	/* Set MAC IFG Gaps */
	writel(DEV_MAC_CFG_MAC_IFG_TX_IFG(4) \|
	DEV_MAC_CFG_MAC_IFG_RX_IFG1(5) \|
	DEV_MAC_CFG_MAC_IFG_RX_IFG2(1),
	regs + DEV_MAC_CFG_MAC_IFG);

	/* Set link speed and release all resets */
	writel(DEV_DEV_CFG_DEV_RST_CTRL_SPEED_SEL(2),
	regs + DEV_DEV_CFG_DEV_RST_CTRL);

	/* Make VLAN aware for CPU traffic */
	writel(ANA_CL_PORT_VLAN_CFG_AWARE_ENA \|
	ANA_CL_PORT_VLAN_CFG_POP_CNT(1) \|
	MAC_VID,
	priv->regs[ANA_CL] + ANA_CL_PORT_VLAN_CFG(port));

	/* Enable CPU port for any frame transfer */
	setbits_le32(priv->regs[QFWD] + QFWD_SYSTEM_SWITCH_PORT_MODE(port),
	QFWD_SYSTEM_SWITCH_PORT_MODE_PORT_ENA);
	}

	static int ram_init(u32 val, void __iomem *addr)
	{
	writel(val, addr);

	if (wait_for_bit_le32(addr, BIT(1), false, 2000, false)) {
	printf("Timeout in memory reset, reg = 0x%08x\n", val);
	return 1;
	}

	return 0;
	}

	static int servalt_switch_init(struct servalt_private *priv)
	{
	/* Initialize memories */
	ram_init(0x3, priv->regs[QSYS] + QSYS_RAM_CTRL_RAM_INIT);
	ram_init(0x3, priv->regs[ASM] + ASM_RAM_CTRL_RAM_INIT);
	ram_init(0x3, priv->regs[ANA_AC] + ANA_AC_RAM_CTRL_RAM_INIT);
	ram_init(0x3, priv->regs[REW] + REW_RAM_CTRL_RAM_INIT);

	/* Reset counters */
	writel(0x1, priv->regs[ANA_AC] + ANA_AC_STAT_GLOBAL_CFG_PORT_RESET);
	writel(0x1, priv->regs[ASM] + ASM_CFG_STAT_CFG);

	/* Enable switch-core and queue system */
	writel(0x1, priv->regs[QSYS] + QSYS_SYSTEM_RESET_CFG);

	return 0;
	}

	static void servalt_switch_config(struct servalt_private *priv)
	{
	writel(0x55555555, priv->regs[QSYS] + QSYS_CALCFG_CAL_AUTO);

	writel(readl(priv->regs[QSYS] + QSYS_CALCFG_CAL_CTRL) \|
	QSYS_CALCFG_CAL_CTRL_CAL_MODE(8),
	priv->regs[QSYS] + QSYS_CALCFG_CAL_CTRL);
	}

	static int servalt_initialize(struct servalt_private *priv)
	{
	int ret, i;

	/* Initialize switch memories, enable core */
	ret = servalt_switch_init(priv);
	if (ret)
	return ret;

	servalt_switch_config(priv);

	for (i = 0; i < MAX_PORT; i++)
	servalt_port_init(priv, i);

	servalt_cpu_capture_setup(priv);

	return 0;
	}

	static inline
	int servalt_vlant_wait_for_completion(struct servalt_private *priv)
	{
	if (wait_for_bit_le32(priv->regs[LRN] + LRN_COMMON_ACCESS_CTRL,
	LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT,
	false, 2000, false))
	return -ETIMEDOUT;

	return 0;
	}

	static int servalt_mac_table_add(struct servalt_private *priv,
	const unsigned char mac[ETH_ALEN], int pgid)
	{
	u32 macl = 0, mach = 0;

	/*
	* Set the MAC address to handle and the vlan associated in a format
	* understood by the hardware.
	*/
	mach \|= MAC_VID << 16;
	mach \|= ((u32)mac[0]) << 8;
	mach \|= ((u32)mac[1]) << 0;
	macl \|= ((u32)mac[2]) << 24;
	macl \|= ((u32)mac[3]) << 16;
	macl \|= ((u32)mac[4]) << 8;
	macl \|= ((u32)mac[5]) << 0;

	writel(mach, priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG0);
	writel(macl, priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG1);

	writel(LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_ADDR(pgid) \|
	LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_TYPE(0x3) \|
	LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_COPY \|
	LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_CPU_QU(0) \|
	LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_VLD \|
	LRN_COMMON_MAC_ACCESS_CFG2_MAC_ENTRY_LOCKED,
	priv->regs[LRN] + LRN_COMMON_MAC_ACCESS_CFG2);

	writel(LRN_COMMON_ACCESS_CTRL_MAC_TABLE_ACCESS_SHOT,
	priv->regs[LRN] + LRN_COMMON_ACCESS_CTRL);

	return servalt_vlant_wait_for_completion(priv);
	}

	static int servalt_write_hwaddr(struct udevice *dev)
	{
	struct servalt_private *priv = dev_get_priv(dev);
	struct eth_pdata *pdata = dev_get_platdata(dev);

	return servalt_mac_table_add(priv, pdata->enetaddr, PGID_UNICAST);
	}

	static int servalt_start(struct udevice *dev)
	{
	struct servalt_private *priv = dev_get_priv(dev);
	struct eth_pdata *pdata = dev_get_platdata(dev);
	const unsigned char mac[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff,
	0xff };
	int ret;

	ret = servalt_initialize(priv);
	if (ret)
	return ret;

	/* Set MAC address tables entries for CPU redirection */
	ret = servalt_mac_table_add(priv, mac, PGID_BROADCAST);
	if (ret)
	return ret;

	ret = servalt_mac_table_add(priv, pdata->enetaddr, PGID_UNICAST);
	if (ret)
	return ret;

	return 0;
	}

	static void servalt_stop(struct udevice *dev)
	{
	}

	static int servalt_send(struct udevice dev, void packet, int length)
	{
	struct servalt_private *priv = dev_get_priv(dev);
	u32 ifh[IFH_LEN];
	u32 *buf = packet;

	memset(ifh, '\0', IFH_LEN * 4);

	/* Set DST PORT_MASK */
	ifh[0] = htonl(0);
	ifh[1] = htonl(0x1FFFFF);
	ifh[2] = htonl(~0);
	/* Set DST_MODE to INJECT and UPDATE_FCS */
	ifh[5] = htonl(0x4c0);

	return mscc_send(priv->regs[QS], servalt_regs_qs,
	ifh, IFH_LEN, buf, length);
	}

	static int servalt_recv(struct udevice dev, int flags, uchar *packetp)
	{
	struct servalt_private *priv = dev_get_priv(dev);
	u32 rxbuf = (u32 )net_rx_packets[0];
	int byte_cnt = 0;

	byte_cnt = mscc_recv(priv->regs[QS], servalt_regs_qs, rxbuf, IFH_LEN,
	false);

	*packetp = net_rx_packets[0];

	return byte_cnt;
	}

	static struct mii_dev *get_mdiobus(phys_addr_t base, unsigned long size)
	{
	int i = 0;

	for (i = 0; i < SERVALT_MIIM_BUS_COUNT; ++i)
	if (miim[i].miim_base == base && miim[i].miim_size == size)
	return miim[i].bus;

	return NULL;
	}

	static void add_port_entry(struct servalt_private *priv, size_t index,
	size_t phy_addr, struct mii_dev *bus)
	{
	priv->ports[index].phy_addr = phy_addr;
	priv->ports[index].bus = bus;
	}

	static int servalt_probe(struct udevice *dev)
	{
	struct servalt_private *priv = dev_get_priv(dev);
	int i;
	struct resource res;
	fdt32_t faddr;
	phys_addr_t addr_base;
	unsigned long addr_size;
	ofnode eth_node, node, mdio_node;
	size_t phy_addr;
	struct mii_dev *bus;
	struct ofnode_phandle_args phandle;

	if (!priv)
	return -EINVAL;

	/* Get registers and map them to the private structure */
	for (i = 0; i < ARRAY_SIZE(regs_names); i++) {
	priv->regs[i] = dev_remap_addr_name(dev, regs_names[i]);
	if (!priv->regs[i]) {
	debug
	("Error can't get regs base addresses for %s\n",
	regs_names[i]);
	return -ENOMEM;
	}
	}

	/* Initialize miim buses */
	memset(&miim, 0x0, sizeof(struct mscc_miim_dev) *
	SERVALT_MIIM_BUS_COUNT);

	/* iterate all the ports and find out on which bus they are */
	i = 0;
	eth_node = dev_read_first_subnode(dev);
	for (node = ofnode_first_subnode(eth_node);
	ofnode_valid(node);
	node = ofnode_next_subnode(node)) {
	if (ofnode_read_resource(node, 0, &res))
	return -ENOMEM;
	i = res.start;

	ofnode_parse_phandle_with_args(node, "phy-handle", NULL, 0, 0,
	&phandle);

	/* Get phy address on mdio bus */
	if (ofnode_read_resource(phandle.node, 0, &res))
	return -ENOMEM;
	phy_addr = res.start;

	/* Get mdio node */
	mdio_node = ofnode_get_parent(phandle.node);

	if (ofnode_read_resource(mdio_node, 0, &res))
	return -ENOMEM;
	faddr = cpu_to_fdt32(res.start);

	addr_base = ofnode_translate_address(mdio_node, &faddr);
	addr_size = res.end - res.start;

	/* If the bus is new then create a new bus */
	if (!get_mdiobus(addr_base, addr_size))
	priv->bus[miim_count] =
	mscc_mdiobus_init(miim, &miim_count, addr_base,
	addr_size);

	/* Connect mdio bus with the port */
	bus = get_mdiobus(addr_base, addr_size);
	add_port_entry(priv, i, phy_addr, bus);
	}

	mscc_phy_reset();

	for (i = 0; i < MAX_PORT; i++) {
	if (!priv->ports[i].bus)
	continue;

	phy_connect(priv->ports[i].bus, priv->ports[i].phy_addr, dev,
	PHY_INTERFACE_MODE_NONE);
	}

	return 0;
	}

	static int servalt_remove(struct udevice *dev)
	{
	struct servalt_private *priv = dev_get_priv(dev);
	int i;

	for (i = 0; i < SERVALT_MIIM_BUS_COUNT; i++) {
	mdio_unregister(priv->bus[i]);
	mdio_free(priv->bus[i]);
	}

	return 0;
	}

	static const struct eth_ops servalt_ops = {
	.start = servalt_start,
	.stop = servalt_stop,
	.send = servalt_send,
	.recv = servalt_recv,
	.write_hwaddr = servalt_write_hwaddr,
	};

	static const struct udevice_id mscc_servalt_ids[] = {
	{.compatible = "mscc,vsc7437-switch" },
	{ /* Sentinel */ }
	};

	U_BOOT_DRIVER(servalt) = {
	.name = "servalt-switch",
	.id = UCLASS_ETH,
	.of_match = mscc_servalt_ids,
	.probe = servalt_probe,
	.remove = servalt_remove,
	.ops = &servalt_ops,
	.priv_auto_alloc_size = sizeof(struct servalt_private),
	.platdata_auto_alloc_size = sizeof(struct eth_pdata),
	};