Merge branch 'New-DSA-driver-for-VSC9953-Seville-switch'

Vladimir Oltean says:

====================
New DSA driver for VSC9953 Seville switch

Looking at the Felix and Ocelot drivers, Maxim asked if it would be
possible to use them as a base for a new driver for the Seville switch
inside NXP T1040. Turns out, it is! The result is that the mscc_felix
driver was extended to probe on Seville.

The biggest challenge seems to be getting register read/write API
generic enough to cover such wild bitfield variations between hardware
generations.

Currently, both felix and seville are built under the same kernel config
option (NET_DSA_MSCC_FELIX). This has both some advantages (no need to
duplicate the Lynx PCS code from felix_vsc9959.c) and some disadvantages
(Seville needs to depend on PCI and on ENETC_MDIO). This will be further
refined as time progresses.

The driver has been completely reviewed. Previous submission was here,
it wasn't accepted due to a conflict with Mark Brown's tree, very late
in the release cycle:

https://patchwork.ozlabs.org/project/netdev/cover/20200531122640.1375715-1-olteanv@gmail.com/

So this is more of a repost, with the only changes being related to
rebasing on top of the cleanup I had to do in Ocelot.
====================

Signed-off-by: David S. Miller <davem@davemloft.net>

17 files changed, 1767 insertions, 410 deletions

diff --git a/Documentation/devicetree/bindings/net/dsa/ocelot.txt b/Documentation/devicetree/bindings/net/dsa/ocelot.txt
index 66a129fea705..7a271d070b72 100644
--- a/Documentation/devicetree/bindings/net/dsa/ocelot.txt
+++ b/Documentation/devicetree/bindings/net/dsa/ocelot.txt
@@ -4,10 +4,15 @@ Microchip Ocelot switch driver family
 Felix
 -----
 
-The VSC9959 core is currently the only switch supported by the driver, and is
-found in the NXP LS1028A. It is a PCI device, part of the larger ENETC root
-complex. As a result, the ethernet-switch node is a sub-node of the PCIe root
-complex node and its "reg" property conforms to the parent node bindings:
+Currently the switches supported by the felix driver are:
+
+- VSC9959 (Felix)
+- VSC9953 (Seville)
+
+The VSC9959 switch is found in the NXP LS1028A. It is a PCI device, part of the
+larger ENETC root complex. As a result, the ethernet-switch node is a sub-node
+of the PCIe root complex node and its "reg" property conforms to the parent
+node bindings:
 
 * reg: Specifies PCIe Device Number and Function Number of the endpoint device,
   in this case for the Ethernet L2Switch it is PF5 (of device 0, bus 0).
@@ -114,3 +119,95 @@ Example:
 		};
 	};
 };
+
+The VSC9953 switch is found inside NXP T1040. It is a platform device with the
+following required properties:
+
+- compatible:
+	Must be "mscc,vsc9953-switch".
+
+Supported PHY interface types (appropriate SerDes protocol setting changes are
+needed in the RCW binary):
+
+* phy_mode = "internal": on ports 8 and 9
+* phy_mode = "sgmii": on ports 0, 1, 2, 3, 4, 5, 6, 7
+* phy_mode = "qsgmii": on ports 0, 1, 2, 3, 4, 5, 6, 7
+
+Example:
+
+&soc {
+	ethernet-switch@800000 {
+		#address-cells = <0x1>;
+		#size-cells = <0x0>;
+		compatible = "mscc,vsc9953-switch";
+		little-endian;
+		reg = <0x800000 0x290000>;
+
+		ports {
+			#address-cells = <0x1>;
+			#size-cells = <0x0>;
+
+			port@0 {
+				reg = <0x0>;
+				label = "swp0";
+			};
+
+			port@1 {
+				reg = <0x1>;
+				label = "swp1";
+			};
+
+			port@2 {
+				reg = <0x2>;
+				label = "swp2";
+			};
+
+			port@3 {
+				reg = <0x3>;
+				label = "swp3";
+			};
+
+			port@4 {
+				reg = <0x4>;
+				label = "swp4";
+			};
+
+			port@5 {
+				reg = <0x5>;
+				label = "swp5";
+			};
+
+			port@6 {
+				reg = <0x6>;
+				label = "swp6";
+			};
+
+			port@7 {
+				reg = <0x7>;
+				label = "swp7";
+			};
+
+			port@8 {
+				reg = <0x8>;
+				phy-mode = "internal";
+				ethernet = <&enet0>;
+
+				fixed-link {
+					speed = <2500>;
+					full-duplex;
+				};
+			};
+
+			port@9 {
+				reg = <0x9>;
+				phy-mode = "internal";
+				status = "disabled";
+
+				fixed-link {
+					speed = <2500>;
+					full-duplex;
+				};
+			};
+		};
+	};
+};
diff --git a/drivers/net/dsa/ocelot/Kconfig b/drivers/net/dsa/ocelot/Kconfig
index 3d3c2a6fb0c0..5b23bf6ba848 100644
--- a/drivers/net/dsa/ocelot/Kconfig
+++ b/drivers/net/dsa/ocelot/Kconfig
@@ -10,7 +10,11 @@ config NET_DSA_MSCC_FELIX
 	select NET_DSA_TAG_OCELOT
 	select FSL_ENETC_MDIO
 	help
-	  This driver supports the VSC9959 network switch, which is a member of
-	  the Vitesse / Microsemi / Microchip Ocelot family of switching cores.
-	  It is embedded as a PCIe function of the NXP LS1028A ENETC integrated
-	  endpoint.
+	  This driver supports network switches from the the Vitesse /
+	  Microsemi / Microchip Ocelot family of switching cores that are
+	  connected to their host CPU via Ethernet.
+	  The following switches are supported:
+	  - VSC9959 (Felix): embedded as a PCIe function of the NXP LS1028A
+	    ENETC integrated endpoint.
+	  - VSC9953 (Seville): embedded as a platform device on the
+	    NXP T1040 SoC.
diff --git a/drivers/net/dsa/ocelot/Makefile b/drivers/net/dsa/ocelot/Makefile
index 37ad403e0b2a..ec57a5a12330 100644
--- a/drivers/net/dsa/ocelot/Makefile
+++ b/drivers/net/dsa/ocelot/Makefile
@@ -3,4 +3,5 @@ obj-$(CONFIG_NET_DSA_MSCC_FELIX) += mscc_felix.o
 
 mscc_felix-objs := \
 	felix.o \
-	felix_vsc9959.o
+	felix_vsc9959.o \
+	seville_vsc9953.o
diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
index 75652ed99b24..c69d9592a2b7 100644
--- a/drivers/net/dsa/ocelot/felix.c
+++ b/drivers/net/dsa/ocelot/felix.c
@@ -1,5 +1,9 @@
 // SPDX-License-Identifier: GPL-2.0
 /* Copyright 2019 NXP Semiconductors
+ *
+ * This is an umbrella module for all network switches that are
+ * register-compatible with Ocelot and that perform I/O to their host CPU
+ * through an NPI (Node Processor Interface) Ethernet port.
  */
 #include <uapi/linux/if_bridge.h>
 #include <soc/mscc/ocelot_vcap.h>
@@ -9,6 +13,7 @@
 #include <soc/mscc/ocelot_ana.h>
 #include <soc/mscc/ocelot_ptp.h>
 #include <soc/mscc/ocelot.h>
+#include <linux/platform_device.h>
 #include <linux/packing.h>
 #include <linux/module.h>
 #include <linux/of_net.h>
@@ -185,37 +190,10 @@ static void felix_phylink_validate(struct dsa_switch *ds, int port,
 				   struct phylink_link_state *state)
 {
 	struct ocelot *ocelot = ds->priv;
-	struct ocelot_port *ocelot_port = ocelot->ports[port];
-	__ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
-
-	if (state->interface != PHY_INTERFACE_MODE_NA &&
-	    state->interface != ocelot_port->phy_mode) {
-		bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS);
-		return;
-	}
-
-	phylink_set_port_modes(mask);
-	phylink_set(mask, Autoneg);
-	phylink_set(mask, Pause);
-	phylink_set(mask, Asym_Pause);
-	phylink_set(mask, 10baseT_Half);
-	phylink_set(mask, 10baseT_Full);
-	phylink_set(mask, 100baseT_Half);
-	phylink_set(mask, 100baseT_Full);
-	phylink_set(mask, 1000baseT_Half);
-	phylink_set(mask, 1000baseT_Full);
-
-	if (state->interface == PHY_INTERFACE_MODE_INTERNAL ||
-	    state->interface == PHY_INTERFACE_MODE_2500BASEX ||
-	    state->interface == PHY_INTERFACE_MODE_USXGMII) {
-		phylink_set(mask, 2500baseT_Full);
-		phylink_set(mask, 2500baseX_Full);
-	}
+	struct felix *felix = ocelot_to_felix(ocelot);
 
-	bitmap_and(supported, supported, mask,
-		   __ETHTOOL_LINK_MODE_MASK_NBITS);
-	bitmap_and(state->advertising, state->advertising, mask,
-		   __ETHTOOL_LINK_MODE_MASK_NBITS);
+	if (felix->info->phylink_validate)
+		felix->info->phylink_validate(ocelot, port, supported, state);
 }
 
 static int felix_phylink_mac_pcs_get_state(struct dsa_switch *ds, int port,
@@ -249,8 +227,7 @@ static void felix_phylink_mac_link_down(struct dsa_switch *ds, int port,
 	struct ocelot_port *ocelot_port = ocelot->ports[port];
 
 	ocelot_port_writel(ocelot_port, 0, DEV_MAC_ENA_CFG);
-	ocelot_rmw_rix(ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
-		       QSYS_SWITCH_PORT_MODE, port);
+	ocelot_fields_write(ocelot, port, QSYS_SWITCH_PORT_MODE_PORT_ENA, 0);
 }
 
 static void felix_phylink_mac_link_up(struct dsa_switch *ds, int port,
@@ -326,10 +303,8 @@ static void felix_phylink_mac_link_up(struct dsa_switch *ds, int port,
 			 ANA_PORT_PORT_CFG, port);
 
 	/* Core: Enable port for frame transfer */
-	ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
-			 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
-			 QSYS_SWITCH_PORT_MODE_PORT_ENA,
-			 QSYS_SWITCH_PORT_MODE, port);
+	ocelot_fields_write(ocelot, port,
+			    QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
 
 	if (felix->info->pcs_link_up)
 		felix->info->pcs_link_up(ocelot, port, link_an_mode, interface,
@@ -459,7 +434,6 @@ static int felix_init_structs(struct felix *felix, int num_phys_ports)
 {
 	struct ocelot *ocelot = &felix->ocelot;
 	phy_interface_t *port_phy_modes;
-	resource_size_t switch_base;
 	struct resource res;
 	int port, i, err;
 
@@ -490,9 +464,6 @@ static int felix_init_structs(struct felix *felix, int num_phys_ports)
 		return err;
 	}
 
-	switch_base = pci_resource_start(felix->pdev,
-					 felix->info->switch_pci_bar);
-
 	for (i = 0; i < TARGET_MAX; i++) {
 		struct regmap *target;
 
@@ -501,8 +472,8 @@ static int felix_init_structs(struct felix *felix, int num_phys_ports)
 
 		memcpy(&res, &felix->info->target_io_res[i], sizeof(res));
 		res.flags = IORESOURCE_MEM;
-		res.start += switch_base;
-		res.end += switch_base;
+		res.start += felix->switch_base;
+		res.end += felix->switch_base;
 
 		target = ocelot_regmap_init(ocelot, &res);
 		if (IS_ERR(target)) {
@@ -524,7 +495,8 @@ static int felix_init_structs(struct felix *felix, int num_phys_ports)
 
 	for (port = 0; port < num_phys_ports; port++) {
 		struct ocelot_port *ocelot_port;
-		void __iomem *port_regs;
+		struct regmap *target;
+		u8 *template;
 
 		ocelot_port = devm_kzalloc(ocelot->dev,
 					   sizeof(struct ocelot_port),
@@ -538,21 +510,34 @@ static int felix_init_structs(struct felix *felix, int num_phys_ports)
 
 		memcpy(&res, &felix->info->port_io_res[port], sizeof(res));
 		res.flags = IORESOURCE_MEM;
-		res.start += switch_base;
-		res.end += switch_base;
+		res.start += felix->switch_base;
+		res.end += felix->switch_base;
 
-		port_regs = devm_ioremap_resource(ocelot->dev, &res);
-		if (IS_ERR(port_regs)) {
+		target = ocelot_regmap_init(ocelot, &res);
+		if (IS_ERR(target)) {
 			dev_err(ocelot->dev,
-				"failed to map registers for port %d\n", port);
+				"Failed to map memory space for port %d\n",
+				port);
 			kfree(port_phy_modes);
-			return PTR_ERR(port_regs);
+			return PTR_ERR(target);
+		}
+
+		template = devm_kzalloc(ocelot->dev, OCELOT_TAG_LEN,
+					GFP_KERNEL);
+		if (!template) {
+			dev_err(ocelot->dev,
+				"Failed to allocate memory for DSA tag\n");
+			kfree(port_phy_modes);
+			return -ENOMEM;
 		}
 
 		ocelot_port->phy_mode = port_phy_modes[port];
 		ocelot_port->ocelot = ocelot;
-		ocelot_port->regs = port_regs;
+		ocelot_port->target = target;
+		ocelot_port->xmit_template = template;
 		ocelot->ports[port] = ocelot_port;
+
+		felix->info->xmit_template_populate(ocelot, port);
 	}
 
 	kfree(port_phy_modes);
@@ -791,7 +776,7 @@ static int felix_port_setup_tc(struct dsa_switch *ds, int port,
 		return -EOPNOTSUPP;
 }
 
-static const struct dsa_switch_ops felix_switch_ops = {
+const struct dsa_switch_ops felix_switch_ops = {
 	.get_tag_protocol	= felix_get_tag_protocol,
 	.setup			= felix_setup,
 	.teardown		= felix_teardown,
@@ -834,149 +819,28 @@ static const struct dsa_switch_ops felix_switch_ops = {
 	.port_setup_tc          = felix_port_setup_tc,
 };
 
-static struct felix_info *felix_instance_tbl[] = {
-	[FELIX_INSTANCE_VSC9959] = &felix_info_vsc9959,
-};
-
-static irqreturn_t felix_irq_handler(int irq, void *data)
-{
-	struct ocelot *ocelot = (struct ocelot *)data;
-
-	/* The INTB interrupt is used for both PTP TX timestamp interrupt
-	 * and preemption status change interrupt on each port.
-	 *
-	 * - Get txtstamp if have
-	 * - TODO: handle preemption. Without handling it, driver may get
-	 *   interrupt storm.
-	 */
-
-	ocelot_get_txtstamp(ocelot);
-
-	return IRQ_HANDLED;
-}
-
-static int felix_pci_probe(struct pci_dev *pdev,
-			   const struct pci_device_id *id)
+static int __init felix_init(void)
 {
-	enum felix_instance instance = id->driver_data;
-	struct dsa_switch *ds;
-	struct ocelot *ocelot;
-	struct felix *felix;
 	int err;
 
-	if (pdev->dev.of_node && !of_device_is_available(pdev->dev.of_node)) {
-		dev_info(&pdev->dev, "device is disabled, skipping\n");
-		return -ENODEV;
-	}
-
-	err = pci_enable_device(pdev);
-	if (err) {
-		dev_err(&pdev->dev, "device enable failed\n");
-		goto err_pci_enable;
-	}
-
-	/* set up for high or low dma */
-	err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64));
-	if (err) {
-		err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
-		if (err) {
-			dev_err(&pdev->dev,
-				"DMA configuration failed: 0x%x\n", err);
-			goto err_dma;
-		}
-	}
-
-	felix = kzalloc(sizeof(struct felix), GFP_KERNEL);
-	if (!felix) {
-		err = -ENOMEM;
-		dev_err(&pdev->dev, "Failed to allocate driver memory\n");
-		goto err_alloc_felix;
-	}
-
-	pci_set_drvdata(pdev, felix);
-	ocelot = &felix->ocelot;
-	ocelot->dev = &pdev->dev;
-	felix->pdev = pdev;
-	felix->info = felix_instance_tbl[instance];
-
-	pci_set_master(pdev);
-
-	err = devm_request_threaded_irq(&pdev->dev, pdev->irq, NULL,
-					&felix_irq_handler, IRQF_ONESHOT,
-					"felix-intb", ocelot);
-	if (err) {
-		dev_err(&pdev->dev, "Failed to request irq\n");
-		goto err_alloc_irq;
-	}
-
-	ocelot->ptp = 1;
-
-	ds = kzalloc(sizeof(struct dsa_switch), GFP_KERNEL);
-	if (!ds) {
-		err = -ENOMEM;
-		dev_err(&pdev->dev, "Failed to allocate DSA switch\n");
-		goto err_alloc_ds;
-	}
-
-	ds->dev = &pdev->dev;
-	ds->num_ports = felix->info->num_ports;
-	ds->num_tx_queues = felix->info->num_tx_queues;
-	ds->ops = &felix_switch_ops;
-	ds->priv = ocelot;
-	felix->ds = ds;
+	err = pci_register_driver(&felix_vsc9959_pci_driver);
+	if (err)
+		return err;
 
-	err = dsa_register_switch(ds);
-	if (err) {
-		dev_err(&pdev->dev, "Failed to register DSA switch: %d\n", err);
-		goto err_register_ds;
-	}
+	err = platform_driver_register(&seville_vsc9953_driver);
+	if (err)
+		return err;
 
 	return 0;
-
-err_register_ds:
-	kfree(ds);
-err_alloc_ds:
-err_alloc_irq:
-err_alloc_felix:
-	kfree(felix);
-err_dma:
-	pci_disable_device(pdev);
-err_pci_enable:
-	return err;
 }
+module_init(felix_init);
 
-static void felix_pci_remove(struct pci_dev *pdev)
+static void __exit felix_exit(void)
 {
-	struct felix *felix;
-
-	felix = pci_get_drvdata(pdev);
-
-	dsa_unregister_switch(felix->ds);
-
-	kfree(felix->ds);
-	kfree(felix);
-
-	pci_disable_device(pdev);
+	pci_unregister_driver(&felix_vsc9959_pci_driver);
+	platform_driver_unregister(&seville_vsc9953_driver);
 }
-
-static struct pci_device_id felix_ids[] = {
-	{
-		/* NXP LS1028A */
-		PCI_DEVICE(PCI_VENDOR_ID_FREESCALE, 0xEEF0),
-		.driver_data = FELIX_INSTANCE_VSC9959,
-	},
-	{ 0, }
-};
-MODULE_DEVICE_TABLE(pci, felix_ids);
-
-static struct pci_driver felix_pci_driver = {
-	.name		= KBUILD_MODNAME,
-	.id_table	= felix_ids,
-	.probe		= felix_pci_probe,
-	.remove		= felix_pci_remove,
-};
-
-module_pci_driver(felix_pci_driver);
+module_exit(felix_exit);
 
 MODULE_DESCRIPTION("Felix Switch driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/dsa/ocelot/felix.h b/drivers/net/dsa/ocelot/felix.h
index 00137b64132b..98f14621ac23 100644
--- a/drivers/net/dsa/ocelot/felix.h
+++ b/drivers/net/dsa/ocelot/felix.h
@@ -37,28 +37,42 @@ struct felix_info {
 			       int speed, int duplex);
 	void	(*pcs_link_state)(struct ocelot *ocelot, int port,
 				  struct phylink_link_state *state);
+	void	(*phylink_validate)(struct ocelot *ocelot, int port,
+				    unsigned long *supported,
+				    struct phylink_link_state *state);
 	int	(*prevalidate_phy_mode)(struct ocelot *ocelot, int port,
 					phy_interface_t phy_mode);
 	int	(*port_setup_tc)(struct dsa_switch *ds, int port,
 				 enum tc_setup_type type, void *type_data);
 	void	(*port_sched_speed_set)(struct ocelot *ocelot, int port,
 					u32 speed);
+	void	(*xmit_template_populate)(struct ocelot *ocelot, int port);
 };
 
-extern struct felix_info		felix_info_vsc9959;
-
-enum felix_instance {
-	FELIX_INSTANCE_VSC9959		= 0,
-};
+extern const struct dsa_switch_ops felix_switch_ops;
+extern struct pci_driver felix_vsc9959_pci_driver;
+extern struct platform_driver seville_vsc9953_driver;
 
 /* DSA glue / front-end for struct ocelot */
 struct felix {
 	struct dsa_switch		*ds;
-	struct pci_dev			*pdev;
-	struct felix_info		*info;
+	const struct felix_info		*info;
 	struct ocelot			ocelot;
 	struct mii_bus			*imdio;
 	struct phy_device		**pcs;
+	resource_size_t			switch_base;
+	resource_size_t			imdio_base;
 };
 
+void vsc9959_pcs_link_state(struct ocelot *ocelot, int port,
+			    struct phylink_link_state *state);
+void vsc9959_pcs_config(struct ocelot *ocelot, int port,
+			unsigned int link_an_mode,
+			const struct phylink_link_state *state);
+void vsc9959_pcs_link_up(struct ocelot *ocelot, int port,
+			 unsigned int link_an_mode,
+			 phy_interface_t interface,
+			 int speed, int duplex);
+void vsc9959_mdio_bus_free(struct ocelot *ocelot);
+
 #endif
diff --git a/drivers/net/dsa/ocelot/felix_vsc9959.c b/drivers/net/dsa/ocelot/felix_vsc9959.c
index 19614537b1ba..1bdf74fd8be4 100644
--- a/drivers/net/dsa/ocelot/felix_vsc9959.c
+++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
@@ -8,6 +8,7 @@
 #include <soc/mscc/ocelot_ptp.h>
 #include <soc/mscc/ocelot_sys.h>
 #include <soc/mscc/ocelot.h>
+#include <linux/packing.h>
 #include <net/pkt_sched.h>
 #include <linux/iopoll.h>
 #include <linux/pci.h>
@@ -329,7 +330,49 @@ static const u32 vsc9959_gcb_regmap[] = {
 	REG(GCB_SOFT_RST,			0x000004),
 };
 
-static const u32 *vsc9959_regmap[] = {
+static const u32 vsc9959_dev_gmii_regmap[] = {
+	REG(DEV_CLOCK_CFG,			0x0),
+	REG(DEV_PORT_MISC,			0x4),
+	REG(DEV_EVENTS,				0x8),
+	REG(DEV_EEE_CFG,			0xc),
+	REG(DEV_RX_PATH_DELAY,			0x10),
+	REG(DEV_TX_PATH_DELAY,			0x14),
+	REG(DEV_PTP_PREDICT_CFG,		0x18),
+	REG(DEV_MAC_ENA_CFG,			0x1c),
+	REG(DEV_MAC_MODE_CFG,			0x20),
+	REG(DEV_MAC_MAXLEN_CFG,			0x24),
+	REG(DEV_MAC_TAGS_CFG,			0x28),
+	REG(DEV_MAC_ADV_CHK_CFG,		0x2c),
+	REG(DEV_MAC_IFG_CFG,			0x30),
+	REG(DEV_MAC_HDX_CFG,			0x34),
+	REG(DEV_MAC_DBG_CFG,			0x38),
+	REG(DEV_MAC_FC_MAC_LOW_CFG,		0x3c),
+	REG(DEV_MAC_FC_MAC_HIGH_CFG,		0x40),
+	REG(DEV_MAC_STICKY,			0x44),
+	REG_RESERVED(PCS1G_CFG),
+	REG_RESERVED(PCS1G_MODE_CFG),
+	REG_RESERVED(PCS1G_SD_CFG),
+	REG_RESERVED(PCS1G_ANEG_CFG),
+	REG_RESERVED(PCS1G_ANEG_NP_CFG),
+	REG_RESERVED(PCS1G_LB_CFG),
+	REG_RESERVED(PCS1G_DBG_CFG),
+	REG_RESERVED(PCS1G_CDET_CFG),
+	REG_RESERVED(PCS1G_ANEG_STATUS),
+	REG_RESERVED(PCS1G_ANEG_NP_STATUS),
+	REG_RESERVED(PCS1G_LINK_STATUS),
+	REG_RESERVED(PCS1G_LINK_DOWN_CNT),
+	REG_RESERVED(PCS1G_STICKY),
+	REG_RESERVED(PCS1G_DEBUG_STATUS),
+	REG_RESERVED(PCS1G_LPI_CFG),
+	REG_RESERVED(PCS1G_LPI_WAKE_ERROR_CNT),
+	REG_RESERVED(PCS1G_LPI_STATUS),
+	REG_RESERVED(PCS1G_TSTPAT_MODE_CFG),
+	REG_RESERVED(PCS1G_TSTPAT_STATUS),
+	REG_RESERVED(DEV_PCS_FX100_CFG),
+	REG_RESERVED(DEV_PCS_FX100_STATUS),
+};
+
+static const u32 *vsc9959_regmap[TARGET_MAX] = {
 	[ANA]	= vsc9959_ana_regmap,
 	[QS]	= vsc9959_qs_regmap,
 	[QSYS]	= vsc9959_qsys_regmap,
@@ -338,10 +381,11 @@ static const u32 *vsc9959_regmap[] = {
 	[S2]	= vsc9959_s2_regmap,
 	[PTP]	= vsc9959_ptp_regmap,
 	[GCB]	= vsc9959_gcb_regmap,
+	[DEV_GMII] = vsc9959_dev_gmii_regmap,
 };
 
 /* Addresses are relative to the PCI device's base address */
-static const struct resource vsc9959_target_io_res[] = {
+static const struct resource vsc9959_target_io_res[TARGET_MAX] = {
 	[ANA] = {
 		.start	= 0x0280000,
 		.end	= 0x028ffff,
@@ -426,7 +470,7 @@ static const struct resource vsc9959_imdio_res = {
 	.name		= "imdio",
 };
 
-static const struct reg_field vsc9959_regfields[] = {
+static const struct reg_field vsc9959_regfields[REGFIELD_MAX] = {
 	[ANA_ADVLEARN_VLAN_CHK] = REG_FIELD(ANA_ADVLEARN, 6, 6),
 	[ANA_ADVLEARN_LEARN_MIRROR] = REG_FIELD(ANA_ADVLEARN, 0, 5),
 	[ANA_ANEVENTS_FLOOD_DISCARD] = REG_FIELD(ANA_ANEVENTS, 30, 30),
@@ -460,6 +504,20 @@ static const struct reg_field vsc9959_regfields[] = {
 	[ANA_TABLES_MACTINDX_M_INDEX] = REG_FIELD(ANA_TABLES_MACTINDX, 0, 10),
 	[SYS_RESET_CFG_CORE_ENA] = REG_FIELD(SYS_RESET_CFG, 0, 0),
 	[GCB_SOFT_RST_SWC_RST] = REG_FIELD(GCB_SOFT_RST, 0, 0),
+	/* Replicated per number of ports (7), register size 4 per port */
+	[QSYS_SWITCH_PORT_MODE_PORT_ENA] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 14, 14, 7, 4),
+	[QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 11, 13, 7, 4),
+	[QSYS_SWITCH_PORT_MODE_YEL_RSRVD] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 10, 10, 7, 4),
+	[QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 9, 9, 7, 4),
+	[QSYS_SWITCH_PORT_MODE_TX_PFC_ENA] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 1, 8, 7, 4),
+	[QSYS_SWITCH_PORT_MODE_TX_PFC_MODE] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 0, 0, 7, 4),
+	[SYS_PORT_MODE_DATA_WO_TS] = REG_FIELD_ID(SYS_PORT_MODE, 5, 6, 7, 4),
+	[SYS_PORT_MODE_INCL_INJ_HDR] = REG_FIELD_ID(SYS_PORT_MODE, 3, 4, 7, 4),
+	[SYS_PORT_MODE_INCL_XTR_HDR] = REG_FIELD_ID(SYS_PORT_MODE, 1, 2, 7, 4),
+	[SYS_PORT_MODE_INCL_HDR_ERR] = REG_FIELD_ID(SYS_PORT_MODE, 0, 0, 7, 4),
+	[SYS_PAUSE_CFG_PAUSE_START] = REG_FIELD_ID(SYS_PAUSE_CFG, 10, 18, 7, 4),
+	[SYS_PAUSE_CFG_PAUSE_STOP] = REG_FIELD_ID(SYS_PAUSE_CFG, 1, 9, 7, 4),
+	[SYS_PAUSE_CFG_PAUSE_ENA] = REG_FIELD_ID(SYS_PAUSE_CFG, 0, 1, 7, 4),
 };
 
 static const struct ocelot_stat_layout vsc9959_stats_layout[] = {
@@ -794,9 +852,9 @@ static void vsc9959_pcs_config_usxgmii(struct phy_device *pcs,
 		      USXGMII_ADVERTISE_FDX);
 }
 
-static void vsc9959_pcs_config(struct ocelot *ocelot, int port,
-			       unsigned int link_an_mode,
-			       const struct phylink_link_state *state)
+void vsc9959_pcs_config(struct ocelot *ocelot, int port,
+			unsigned int link_an_mode,
+			const struct phylink_link_state *state)
 {
 	struct felix *felix = ocelot_to_felix(ocelot);
 	struct phy_device *pcs = felix->pcs[port];
@@ -907,10 +965,10 @@ static void vsc9959_pcs_link_up_2500basex(struct phy_device *pcs,
 	phy_clear_bits(pcs, MII_BMCR, BMCR_ANENABLE);
 }
 
-static void vsc9959_pcs_link_up(struct ocelot *ocelot, int port,
-				unsigned int link_an_mode,
-				phy_interface_t interface,
-				int speed, int duplex)
+void vsc9959_pcs_link_up(struct ocelot *ocelot, int port,
+			 unsigned int link_an_mode,
+			 phy_interface_t interface,
+			 int speed, int duplex)
 {
 	struct felix *felix = ocelot_to_felix(ocelot);
 	struct phy_device *pcs = felix->pcs[port];
@@ -1038,8 +1096,8 @@ static void vsc9959_pcs_link_state_usxgmii(struct phy_device *pcs,
 		pcs->duplex = DUPLEX_HALF;
 }
 
-static void vsc9959_pcs_link_state(struct ocelot *ocelot, int port,
-				   struct phylink_link_state *state)
+void vsc9959_pcs_link_state(struct ocelot *ocelot, int port,
+			    struct phylink_link_state *state)
 {
 	struct felix *felix = ocelot_to_felix(ocelot);
 	struct phy_device *pcs = felix->pcs[port];
@@ -1070,6 +1128,43 @@ static void vsc9959_pcs_link_state(struct ocelot *ocelot, int port,
 	vsc9959_pcs_link_state_resolve(pcs, state);
 }
 
+static void vsc9959_phylink_validate(struct ocelot *ocelot, int port,
+				     unsigned long *supported,
+				     struct phylink_link_state *state)
+{
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	__ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
+
+	if (state->interface != PHY_INTERFACE_MODE_NA &&
+	    state->interface != ocelot_port->phy_mode) {
+		bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS);
+		return;
+	}
+
+	phylink_set_port_modes(mask);
+	phylink_set(mask, Autoneg);
+	phylink_set(mask, Pause);
+	phylink_set(mask, Asym_Pause);
+	phylink_set(mask, 10baseT_Half);
+	phylink_set(mask, 10baseT_Full);
+	phylink_set(mask, 100baseT_Half);
+	phylink_set(mask, 100baseT_Full);
+	phylink_set(mask, 1000baseT_Half);
+	phylink_set(mask, 1000baseT_Full);
+
+	if (state->interface == PHY_INTERFACE_MODE_INTERNAL ||
+	    state->interface == PHY_INTERFACE_MODE_2500BASEX ||
+	    state->interface == PHY_INTERFACE_MODE_USXGMII) {
+		phylink_set(mask, 2500baseT_Full);
+		phylink_set(mask, 2500baseX_Full);
+	}
+
+	bitmap_and(supported, supported, mask,
+		   __ETHTOOL_LINK_MODE_MASK_NBITS);
+	bitmap_and(state->advertising, state->advertising, mask,
+		   __ETHTOOL_LINK_MODE_MASK_NBITS);
+}
+
 static int vsc9959_prevalidate_phy_mode(struct ocelot *ocelot, int port,
 					phy_interface_t phy_mode)
 {
@@ -1091,8 +1186,21 @@ static int vsc9959_prevalidate_phy_mode(struct ocelot *ocelot, int port,
 	}
 }
 
+/* Watermark encode
+ * Bit 8:   Unit; 0:1, 1:16
+ * Bit 7-0: Value to be multiplied with unit
+ */
+static u16 vsc9959_wm_enc(u16 value)
+{
+	if (value >= BIT(8))
+		return BIT(8) | (value / 16);
+
+	return value;
+}
+
 static const struct ocelot_ops vsc9959_ops = {
 	.reset			= vsc9959_reset,
+	.wm_enc			= vsc9959_wm_enc,
 };
 
 static int vsc9959_mdio_bus_alloc(struct ocelot *ocelot)
@@ -1100,7 +1208,6 @@ static int vsc9959_mdio_bus_alloc(struct ocelot *ocelot)
 	struct felix *felix = ocelot_to_felix(ocelot);
 	struct enetc_mdio_priv *mdio_priv;
 	struct device *dev = ocelot->dev;
-	resource_size_t imdio_base;
 	void __iomem *imdio_regs;
 	struct resource res;
 	struct enetc_hw *hw;
@@ -1116,13 +1223,10 @@ static int vsc9959_mdio_bus_alloc(struct ocelot *ocelot)
 		return -ENOMEM;
 	}
 
-	imdio_base = pci_resource_start(felix->pdev,
-					felix->info->imdio_pci_bar);
-
 	memcpy(&res, felix->info->imdio_res, sizeof(res));
 	res.flags = IORESOURCE_MEM;
-	res.start += imdio_base;
-	res.end += imdio_base;
+	res.start += felix->imdio_base;
+	res.end += felix->imdio_base;
 
 	imdio_regs = devm_ioremap_resource(dev, &res);
 	if (IS_ERR(imdio_regs)) {
@@ -1182,7 +1286,7 @@ static int vsc9959_mdio_bus_alloc(struct ocelot *ocelot)
 	return 0;
 }
 
-static void vsc9959_mdio_bus_free(struct ocelot *ocelot)
+void vsc9959_mdio_bus_free(struct ocelot *ocelot)
 {
 	struct felix *felix = ocelot_to_felix(ocelot);
 	int port;
@@ -1378,7 +1482,25 @@ static int vsc9959_port_setup_tc(struct dsa_switch *ds, int port,
 	}
 }
 
-struct felix_info felix_info_vsc9959 = {
+static void vsc9959_xmit_template_populate(struct ocelot *ocelot, int port)
+{
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	u8 *template = ocelot_port->xmit_template;
+	u64 bypass, dest, src;
+
+	/* Set the source port as the CPU port module and not the
+	 * NPI port
+	 */
+	src = ocelot->num_phys_ports;
+	dest = BIT(port);
+	bypass = true;
+
+	packing(template, &bypass, 127, 127, OCELOT_TAG_LEN, PACK, 0);
+	packing(template, &dest,    68,  56, OCELOT_TAG_LEN, PACK, 0);
+	packing(template, &src,     46,  43, OCELOT_TAG_LEN, PACK, 0);
+}
+
+static const struct felix_info felix_info_vsc9959 = {
 	.target_io_res		= vsc9959_target_io_res,
 	.port_io_res		= vsc9959_port_io_res,
 	.imdio_res		= &vsc9959_imdio_res,
@@ -1401,7 +1523,148 @@ struct felix_info felix_info_vsc9959 = {
 	.pcs_config		= vsc9959_pcs_config,
 	.pcs_link_up		= vsc9959_pcs_link_up,
 	.pcs_link_state		= vsc9959_pcs_link_state,
+	.phylink_validate	= vsc9959_phylink_validate,
 	.prevalidate_phy_mode	= vsc9959_prevalidate_phy_mode,
 	.port_setup_tc          = vsc9959_port_setup_tc,
 	.port_sched_speed_set   = vsc9959_sched_speed_set,
+	.xmit_template_populate	= vsc9959_xmit_template_populate,
+};
+
+static irqreturn_t felix_irq_handler(int irq, void *data)
+{
+	struct ocelot *ocelot = (struct ocelot *)data;
+
+	/* The INTB interrupt is used for both PTP TX timestamp interrupt
+	 * and preemption status change interrupt on each port.
+	 *
+	 * - Get txtstamp if have
+	 * - TODO: handle preemption. Without handling it, driver may get
+	 *   interrupt storm.
+	 */
+
+	ocelot_get_txtstamp(ocelot);
+
+	return IRQ_HANDLED;
+}
+
+static int felix_pci_probe(struct pci_dev *pdev,
+			   const struct pci_device_id *id)
+{
+	struct dsa_switch *ds;
+	struct ocelot *ocelot;
+	struct felix *felix;
+	int err;
+
+	if (pdev->dev.of_node && !of_device_is_available(pdev->dev.of_node)) {
+		dev_info(&pdev->dev, "device is disabled, skipping\n");
+		return -ENODEV;
+	}
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "device enable failed\n");
+		goto err_pci_enable;
+	}
+
+	/* set up for high or low dma */
+	err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64));
+	if (err) {
+		err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+		if (err) {
+			dev_err(&pdev->dev,
+				"DMA configuration failed: 0x%x\n", err);
+			goto err_dma;
+		}
+	}
+
+	felix = kzalloc(sizeof(struct felix), GFP_KERNEL);
+	if (!felix) {
+		err = -ENOMEM;
+		dev_err(&pdev->dev, "Failed to allocate driver memory\n");
+		goto err_alloc_felix;
+	}
+
+	pci_set_drvdata(pdev, felix);
+	ocelot = &felix->ocelot;
+	ocelot->dev = &pdev->dev;
+	felix->info = &felix_info_vsc9959;
+	felix->switch_base = pci_resource_start(pdev,
+						felix->info->switch_pci_bar);
+	felix->imdio_base = pci_resource_start(pdev,
+					       felix->info->imdio_pci_bar);
+
+	pci_set_master(pdev);
+
+	err = devm_request_threaded_irq(&pdev->dev, pdev->irq, NULL,
+					&felix_irq_handler, IRQF_ONESHOT,
+					"felix-intb", ocelot);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to request irq\n");
+		goto err_alloc_irq;
+	}
+
+	ocelot->ptp = 1;
+
+	ds = kzalloc(sizeof(struct dsa_switch), GFP_KERNEL);
+	if (!ds) {
+		err = -ENOMEM;
+		dev_err(&pdev->dev, "Failed to allocate DSA switch\n");
+		goto err_alloc_ds;
+	}
+
+	ds->dev = &pdev->dev;
+	ds->num_ports = felix->info->num_ports;
+	ds->num_tx_queues = felix->info->num_tx_queues;
+	ds->ops = &felix_switch_ops;
+	ds->priv = ocelot;
+	felix->ds = ds;
+
+	err = dsa_register_switch(ds);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to register DSA switch: %d\n", err);
+		goto err_register_ds;
+	}
+
+	return 0;
+
+err_register_ds:
+	kfree(ds);
+err_alloc_ds:
+err_alloc_irq:
+err_alloc_felix:
+	kfree(felix);
+err_dma:
+	pci_disable_device(pdev);
+err_pci_enable:
+	return err;
+}
+
+static void felix_pci_remove(struct pci_dev *pdev)
+{
+	struct felix *felix;
+
+	felix = pci_get_drvdata(pdev);
+
+	dsa_unregister_switch(felix->ds);
+
+	kfree(felix->ds);
+	kfree(felix);
+
+	pci_disable_device(pdev);
+}
+
+static struct pci_device_id felix_ids[] = {
+	{
+		/* NXP LS1028A */
+		PCI_DEVICE(PCI_VENDOR_ID_FREESCALE, 0xEEF0),
+	},
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, felix_ids);
+
+struct pci_driver felix_vsc9959_pci_driver = {
+	.name		= "mscc_felix",
+	.id_table	= felix_ids,
+	.probe		= felix_pci_probe,
+	.remove		= felix_pci_remove,
 };
diff --git a/drivers/net/dsa/ocelot/seville_vsc9953.c b/drivers/net/dsa/ocelot/seville_vsc9953.c
new file mode 100644
index 000000000000..625b1891d955
--- /dev/null
+++ b/drivers/net/dsa/ocelot/seville_vsc9953.c
@@ -0,0 +1,1104 @@
+// SPDX-License-Identifier: (GPL-2.0 OR MIT)
+/* Distributed Switch Architecture VSC9953 driver
+ * Copyright (C) 2020, Maxim Kochetkov <fido_max@inbox.ru>
+ */
+#include <linux/types.h>
+#include <soc/mscc/ocelot_vcap.h>
+#include <soc/mscc/ocelot_sys.h>
+#include <soc/mscc/ocelot.h>
+#include <linux/of_platform.h>
+#include <linux/packing.h>
+#include <linux/iopoll.h>
+#include "felix.h"
+
+#define VSC9953_VCAP_IS2_CNT			1024
+#define VSC9953_VCAP_IS2_ENTRY_WIDTH		376
+#define VSC9953_VCAP_PORT_CNT			10
+
+#define MSCC_MIIM_REG_STATUS			0x0
+#define		MSCC_MIIM_STATUS_STAT_BUSY	BIT(3)
+#define MSCC_MIIM_REG_CMD			0x8
+#define		MSCC_MIIM_CMD_OPR_WRITE		BIT(1)
+#define		MSCC_MIIM_CMD_OPR_READ		BIT(2)
+#define		MSCC_MIIM_CMD_WRDATA_SHIFT	4
+#define		MSCC_MIIM_CMD_REGAD_SHIFT	20
+#define		MSCC_MIIM_CMD_PHYAD_SHIFT	25
+#define		MSCC_MIIM_CMD_VLD		BIT(31)
+#define MSCC_MIIM_REG_DATA			0xC
+#define		MSCC_MIIM_DATA_ERROR		(BIT(16) | BIT(17))
+
+#define MSCC_PHY_REG_PHY_CFG		0x0
+#define		PHY_CFG_PHY_ENA		(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define		PHY_CFG_PHY_COMMON_RESET BIT(4)
+#define		PHY_CFG_PHY_RESET	(BIT(5) | BIT(6) | BIT(7) | BIT(8))
+#define MSCC_PHY_REG_PHY_STATUS		0x4
+
+static const u32 vsc9953_ana_regmap[] = {
+	REG(ANA_ADVLEARN,			0x00b500),
+	REG(ANA_VLANMASK,			0x00b504),
+	REG_RESERVED(ANA_PORT_B_DOMAIN),
+	REG(ANA_ANAGEFIL,			0x00b50c),
+	REG(ANA_ANEVENTS,			0x00b510),
+	REG(ANA_STORMLIMIT_BURST,		0x00b514),
+	REG(ANA_STORMLIMIT_CFG,			0x00b518),
+	REG(ANA_ISOLATED_PORTS,			0x00b528),
+	REG(ANA_COMMUNITY_PORTS,		0x00b52c),
+	REG(ANA_AUTOAGE,			0x00b530),
+	REG(ANA_MACTOPTIONS,			0x00b534),
+	REG(ANA_LEARNDISC,			0x00b538),
+	REG(ANA_AGENCTRL,			0x00b53c),
+	REG(ANA_MIRRORPORTS,			0x00b540),
+	REG(ANA_EMIRRORPORTS,			0x00b544),
+	REG(ANA_FLOODING,			0x00b548),
+	REG(ANA_FLOODING_IPMC,			0x00b54c),
+	REG(ANA_SFLOW_CFG,			0x00b550),
+	REG(ANA_PORT_MODE,			0x00b57c),
+	REG_RESERVED(ANA_CUT_THRU_CFG),
+	REG(ANA_PGID_PGID,			0x00b600),
+	REG(ANA_TABLES_ANMOVED,			0x00b4ac),
+	REG(ANA_TABLES_MACHDATA,		0x00b4b0),
+	REG(ANA_TABLES_MACLDATA,		0x00b4b4),
+	REG_RESERVED(ANA_TABLES_STREAMDATA),
+	REG(ANA_TABLES_MACACCESS,		0x00b4b8),
+	REG(ANA_TABLES_MACTINDX,		0x00b4bc),
+	REG(ANA_TABLES_VLANACCESS,		0x00b4c0),
+	REG(ANA_TABLES_VLANTIDX,		0x00b4c4),
+	REG_RESERVED(ANA_TABLES_ISDXACCESS),
+	REG_RESERVED(ANA_TABLES_ISDXTIDX),
+	REG(ANA_TABLES_ENTRYLIM,		0x00b480),
+	REG_RESERVED(ANA_TABLES_PTP_ID_HIGH),
+	REG_RESERVED(ANA_TABLES_PTP_ID_LOW),
+	REG_RESERVED(ANA_TABLES_STREAMACCESS),
+	REG_RESERVED(ANA_TABLES_STREAMTIDX),
+	REG_RESERVED(ANA_TABLES_SEQ_HISTORY),
+	REG_RESERVED(ANA_TABLES_SEQ_MASK),
+	REG_RESERVED(ANA_TABLES_SFID_MASK),
+	REG_RESERVED(ANA_TABLES_SFIDACCESS),
+	REG_RESERVED(ANA_TABLES_SFIDTIDX),
+	REG_RESERVED(ANA_MSTI_STATE),
+	REG_RESERVED(ANA_OAM_UPM_LM_CNT),
+	REG_RESERVED(ANA_SG_ACCESS_CTRL),
+	REG_RESERVED(ANA_SG_CONFIG_REG_1),
+	REG_RESERVED(ANA_SG_CONFIG_REG_2),
+	REG_RESERVED(ANA_SG_CONFIG_REG_3),
+	REG_RESERVED(ANA_SG_CONFIG_REG_4),
+	REG_RESERVED(ANA_SG_CONFIG_REG_5),
+	REG_RESERVED(ANA_SG_GCL_GS_CONFIG),
+	REG_RESERVED(ANA_SG_GCL_TI_CONFIG),
+	REG_RESERVED(ANA_SG_STATUS_REG_1),
+	REG_RESERVED(ANA_SG_STATUS_REG_2),
+	REG_RESERVED(ANA_SG_STATUS_REG_3),
+	REG(ANA_PORT_VLAN_CFG,			0x000000),
+	REG(ANA_PORT_DROP_CFG,			0x000004),
+	REG(ANA_PORT_QOS_CFG,			0x000008),
+	REG(ANA_PORT_VCAP_CFG,			0x00000c),
+	REG(ANA_PORT_VCAP_S1_KEY_CFG,		0x000010),
+	REG(ANA_PORT_VCAP_S2_CFG,		0x00001c),
+	REG(ANA_PORT_PCP_DEI_MAP,		0x000020),
+	REG(ANA_PORT_CPU_FWD_CFG,		0x000060),
+	REG(ANA_PORT_CPU_FWD_BPDU_CFG,		0x000064),
+	REG(ANA_PORT_CPU_FWD_GARP_CFG,		0x000068),
+	REG(ANA_PORT_CPU_FWD_CCM_CFG,		0x00006c),
+	REG(ANA_PORT_PORT_CFG,			0x000070),
+	REG(ANA_PORT_POL_CFG,			0x000074),
+	REG_RESERVED(ANA_PORT_PTP_CFG),
+	REG_RESERVED(ANA_PORT_PTP_DLY1_CFG),
+	REG_RESERVED(ANA_PORT_PTP_DLY2_CFG),
+	REG_RESERVED(ANA_PORT_SFID_CFG),
+	REG(ANA_PFC_PFC_CFG,			0x00c000),
+	REG_RESERVED(ANA_PFC_PFC_TIMER),
+	REG_RESERVED(ANA_IPT_OAM_MEP_CFG),
+	REG_RESERVED(ANA_IPT_IPT),
+	REG_RESERVED(ANA_PPT_PPT),
+	REG_RESERVED(ANA_FID_MAP_FID_MAP),
+	REG(ANA_AGGR_CFG,			0x00c600),
+	REG(ANA_CPUQ_CFG,			0x00c604),
+	REG_RESERVED(ANA_CPUQ_CFG2),
+	REG(ANA_CPUQ_8021_CFG,			0x00c60c),
+	REG(ANA_DSCP_CFG,			0x00c64c),
+	REG(ANA_DSCP_REWR_CFG,			0x00c74c),
+	REG(ANA_VCAP_RNG_TYPE_CFG,		0x00c78c),
+	REG(ANA_VCAP_RNG_VAL_CFG,		0x00c7ac),
+	REG_RESERVED(ANA_VRAP_CFG),
+	REG_RESERVED(ANA_VRAP_HDR_DATA),
+	REG_RESERVED(ANA_VRAP_HDR_MASK),
+	REG(ANA_DISCARD_CFG,			0x00c7d8),
+	REG(ANA_FID_CFG,			0x00c7dc),
+	REG(ANA_POL_PIR_CFG,			0x00a000),
+	REG(ANA_POL_CIR_CFG,			0x00a004),
+	REG(ANA_POL_MODE_CFG,			0x00a008),
+	REG(ANA_POL_PIR_STATE,			0x00a00c),
+	REG(ANA_POL_CIR_STATE,			0x00a010),
+	REG_RESERVED(ANA_POL_STATE),
+	REG(ANA_POL_FLOWC,			0x00c280),
+	REG(ANA_POL_HYST,			0x00c2ec),
+	REG_RESERVED(ANA_POL_MISC_CFG),
+};
+
+static const u32 vsc9953_qs_regmap[] = {
+	REG(QS_XTR_GRP_CFG,			0x000000),
+	REG(QS_XTR_RD,				0x000008),
+	REG(QS_XTR_FRM_PRUNING,			0x000010),
+	REG(QS_XTR_FLUSH,			0x000018),
+	REG(QS_XTR_DATA_PRESENT,		0x00001c),
+	REG(QS_XTR_CFG,				0x000020),
+	REG(QS_INJ_GRP_CFG,			0x000024),
+	REG(QS_INJ_WR,				0x00002c),
+	REG(QS_INJ_CTRL,			0x000034),
+	REG(QS_INJ_STATUS,			0x00003c),
+	REG(QS_INJ_ERR,				0x000040),
+	REG_RESERVED(QS_INH_DBG),
+};
+
+static const u32 vsc9953_s2_regmap[] = {
+	REG(S2_CORE_UPDATE_CTRL,		0x000000),
+	REG(S2_CORE_MV_CFG,			0x000004),
+	REG(S2_CACHE_ENTRY_DAT,			0x000008),
+	REG(S2_CACHE_MASK_DAT,			0x000108),
+	REG(S2_CACHE_ACTION_DAT,		0x000208),
+	REG(S2_CACHE_CNT_DAT,			0x000308),
+	REG(S2_CACHE_TG_DAT,			0x000388),
+};
+
+static const u32 vsc9953_qsys_regmap[] = {
+	REG(QSYS_PORT_MODE,			0x003600),
+	REG(QSYS_SWITCH_PORT_MODE,		0x003630),
+	REG(QSYS_STAT_CNT_CFG,			0x00365c),
+	REG(QSYS_EEE_CFG,			0x003660),
+	REG(QSYS_EEE_THRES,			0x003688),
+	REG(QSYS_IGR_NO_SHARING,		0x00368c),
+	REG(QSYS_EGR_NO_SHARING,		0x003690),
+	REG(QSYS_SW_STATUS,			0x003694),
+	REG(QSYS_EXT_CPU_CFG,			0x0036c0),
+	REG_RESERVED(QSYS_PAD_CFG),
+	REG(QSYS_CPU_GROUP_MAP,			0x0036c8),
+	REG_RESERVED(QSYS_QMAP),
+	REG_RESERVED(QSYS_ISDX_SGRP),
+	REG_RESERVED(QSYS_TIMED_FRAME_ENTRY),
+	REG_RESERVED(QSYS_TFRM_MISC),
+	REG_RESERVED(QSYS_TFRM_PORT_DLY),
+	REG_RESERVED(QSYS_TFRM_TIMER_CFG_1),
+	REG_RESERVED(QSYS_TFRM_TIMER_CFG_2),
+	REG_RESERVED(QSYS_TFRM_TIMER_CFG_3),
+	REG_RESERVED(QSYS_TFRM_TIMER_CFG_4),
+	REG_RESERVED(QSYS_TFRM_TIMER_CFG_5),
+	REG_RESERVED(QSYS_TFRM_TIMER_CFG_6),
+	REG_RESERVED(QSYS_TFRM_TIMER_CFG_7),
+	REG_RESERVED(QSYS_TFRM_TIMER_CFG_8),
+	REG(QSYS_RED_PROFILE,			0x003724),
+	REG(QSYS_RES_QOS_MODE,			0x003764),
+	REG(QSYS_RES_CFG,			0x004000),
+	REG(QSYS_RES_STAT,			0x004004),
+	REG(QSYS_EGR_DROP_MODE,			0x003768),
+	REG(QSYS_EQ_CTRL,			0x00376c),
+	REG_RESERVED(QSYS_EVENTS_CORE),
+	REG_RESERVED(QSYS_QMAXSDU_CFG_0),
+	REG_RESERVED(QSYS_QMAXSDU_CFG_1),
+	REG_RESERVED(QSYS_QMAXSDU_CFG_2),
+	REG_RESERVED(QSYS_QMAXSDU_CFG_3),
+	REG_RESERVED(QSYS_QMAXSDU_CFG_4),
+	REG_RESERVED(QSYS_QMAXSDU_CFG_5),
+	REG_RESERVED(QSYS_QMAXSDU_CFG_6),
+	REG_RESERVED(QSYS_QMAXSDU_CFG_7),
+	REG_RESERVED(QSYS_PREEMPTION_CFG),
+	REG(QSYS_CIR_CFG,			0x000000),
+	REG_RESERVED(QSYS_EIR_CFG),
+	REG(QSYS_SE_CFG,			0x000008),
+	REG(QSYS_SE_DWRR_CFG,			0x00000c),
+	REG_RESERVED(QSYS_SE_CONNECT),
+	REG_RESERVED(QSYS_SE_DLB_SENSE),
+	REG(QSYS_CIR_STATE,			0x000044),
+	REG_RESERVED(QSYS_EIR_STATE),
+	REG_RESERVED(QSYS_SE_STATE),
+	REG(QSYS_HSCH_MISC_CFG,			0x003774),
+	REG_RESERVED(QSYS_TAG_CONFIG),
+	REG_RESERVED(QSYS_TAS_PARAM_CFG_CTRL),
+	REG_RESERVED(QSYS_PORT_MAX_SDU),
+	REG_RESERVED(QSYS_PARAM_CFG_REG_1),
+	REG_RESERVED(QSYS_PARAM_CFG_REG_2),
+	REG_RESERVED(QSYS_PARAM_CFG_REG_3),
+	REG_RESERVED(QSYS_PARAM_CFG_REG_4),
+	REG_RESERVED(QSYS_PARAM_CFG_REG_5),
+	REG_RESERVED(QSYS_GCL_CFG_REG_1),
+	REG_RESERVED(QSYS_GCL_CFG_REG_2),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_1),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_2),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_3),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_4),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_5),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_6),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_7),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_8),
+	REG_RESERVED(QSYS_PARAM_STATUS_REG_9),
+	REG_RESERVED(QSYS_GCL_STATUS_REG_1),
+	REG_RESERVED(QSYS_GCL_STATUS_REG_2),
+};
+
+static const u32 vsc9953_rew_regmap[] = {
+	REG(REW_PORT_VLAN_CFG,			0x000000),
+	REG(REW_TAG_CFG,			0x000004),
+	REG(REW_PORT_CFG,			0x000008),
+	REG(REW_DSCP_CFG,			0x00000c),
+	REG(REW_PCP_DEI_QOS_MAP_CFG,		0x000010),
+	REG_RESERVED(REW_PTP_CFG),
+	REG_RESERVED(REW_PTP_DLY1_CFG),
+	REG_RESERVED(REW_RED_TAG_CFG),
+	REG(REW_DSCP_REMAP_DP1_CFG,		0x000610),
+	REG(REW_DSCP_REMAP_CFG,			0x000710),
+	REG_RESERVED(REW_STAT_CFG),
+	REG_RESERVED(REW_REW_STICKY),
+	REG_RESERVED(REW_PPT),
+};
+
+static const u32 vsc9953_sys_regmap[] = {
+	REG(SYS_COUNT_RX_OCTETS,		0x000000),
+	REG(SYS_COUNT_RX_MULTICAST,		0x000008),
+	REG(SYS_COUNT_RX_SHORTS,		0x000010),
+	REG(SYS_COUNT_RX_FRAGMENTS,		0x000014),
+	REG(SYS_COUNT_RX_JABBERS,		0x000018),
+	REG(SYS_COUNT_RX_64,			0x000024),
+	REG(SYS_COUNT_RX_65_127,		0x000028),
+	REG(SYS_COUNT_RX_128_255,		0x00002c),
+	REG(SYS_COUNT_RX_256_1023,		0x000030),
+	REG(SYS_COUNT_RX_1024_1526,		0x000034),
+	REG(SYS_COUNT_RX_1527_MAX,		0x000038),
+	REG(SYS_COUNT_RX_LONGS,			0x000048),
+	REG(SYS_COUNT_TX_OCTETS,		0x000100),
+	REG(SYS_COUNT_TX_COLLISION,		0x000110),
+	REG(SYS_COUNT_TX_DROPS,			0x000114),
+	REG(SYS_COUNT_TX_64,			0x00011c),
+	REG(SYS_COUNT_TX_65_127,		0x000120),
+	REG(SYS_COUNT_TX_128_511,		0x000124),
+	REG(SYS_COUNT_TX_512_1023,		0x000128),
+	REG(SYS_COUNT_TX_1024_1526,		0x00012c),
+	REG(SYS_COUNT_TX_1527_MAX,		0x000130),
+	REG(SYS_COUNT_TX_AGING,			0x000178),
+	REG(SYS_RESET_CFG,			0x000318),
+	REG_RESERVED(SYS_SR_ETYPE_CFG),
+	REG(SYS_VLAN_ETYPE_CFG,			0x000320),
+	REG(SYS_PORT_MODE,			0x000324),
+	REG(SYS_FRONT_PORT_MODE,		0x000354),
+	REG(SYS_FRM_AGING,			0x00037c),
+	REG(SYS_STAT_CFG,			0x000380),
+	REG_RESERVED(SYS_SW_STATUS),
+	REG_RESERVED(SYS_MISC_CFG),
+	REG_RESERVED(SYS_REW_MAC_HIGH_CFG),
+	REG_RESERVED(SYS_REW_MAC_LOW_CFG),
+	REG_RESERVED(SYS_TIMESTAMP_OFFSET),
+	REG(SYS_PAUSE_CFG,			0x00044c),
+	REG(SYS_PAUSE_TOT_CFG,			0x000478),
+	REG(SYS_ATOP,				0x00047c),
+	REG(SYS_ATOP_TOT_CFG,			0x0004a8),
+	REG(SYS_MAC_FC_CFG,			0x0004ac),
+	REG(SYS_MMGT,				0x0004d4),
+	REG_RESERVED(SYS_MMGT_FAST),
+	REG_RESERVED(SYS_EVENTS_DIF),
+	REG_RESERVED(SYS_EVENTS_CORE),
+	REG_RESERVED(SYS_CNT),
+	REG_RESERVED(SYS_PTP_STATUS),
+	REG_RESERVED(SYS_PTP_TXSTAMP),
+	REG_RESERVED(SYS_PTP_NXT),
+	REG_RESERVED(SYS_PTP_CFG),
+	REG_RESERVED(SYS_RAM_INIT),
+	REG_RESERVED(SYS_CM_ADDR),
+	REG_RESERVED(SYS_CM_DATA_WR),
+	REG_RESERVED(SYS_CM_DATA_RD),
+	REG_RESERVED(SYS_CM_OP),
+	REG_RESERVED(SYS_CM_DATA),
+};
+
+static const u32 vsc9953_gcb_regmap[] = {
+	REG(GCB_SOFT_RST,			0x000008),
+	REG(GCB_MIIM_MII_STATUS,		0x0000ac),
+	REG(GCB_MIIM_MII_CMD,			0x0000b4),
+	REG(GCB_MIIM_MII_DATA,			0x0000b8),
+};
+
+static const u32 vsc9953_dev_gmii_regmap[] = {
+	REG(DEV_CLOCK_CFG,			0x0),
+	REG(DEV_PORT_MISC,			0x4),
+	REG_RESERVED(DEV_EVENTS),
+	REG(DEV_EEE_CFG,			0xc),
+	REG_RESERVED(DEV_RX_PATH_DELAY),
+	REG_RESERVED(DEV_TX_PATH_DELAY),
+	REG_RESERVED(DEV_PTP_PREDICT_CFG),
+	REG(DEV_MAC_ENA_CFG,			0x10),
+	REG(DEV_MAC_MODE_CFG,			0x14),
+	REG(DEV_MAC_MAXLEN_CFG,			0x18),
+	REG(DEV_MAC_TAGS_CFG,			0x1c),
+	REG(DEV_MAC_ADV_CHK_CFG,		0x20),
+	REG(DEV_MAC_IFG_CFG,			0x24),
+	REG(DEV_MAC_HDX_CFG,			0x28),
+	REG_RESERVED(DEV_MAC_DBG_CFG),
+	REG(DEV_MAC_FC_MAC_LOW_CFG,		0x30),
+	REG(DEV_MAC_FC_MAC_HIGH_CFG,		0x34),
+	REG(DEV_MAC_STICKY,			0x38),
+	REG_RESERVED(PCS1G_CFG),
+	REG_RESERVED(PCS1G_MODE_CFG),
+	REG_RESERVED(PCS1G_SD_CFG),
+	REG_RESERVED(PCS1G_ANEG_CFG),
+	REG_RESERVED(PCS1G_ANEG_NP_CFG),
+	REG_RESERVED(PCS1G_LB_CFG),
+	REG_RESERVED(PCS1G_DBG_CFG),
+	REG_RESERVED(PCS1G_CDET_CFG),
+	REG_RESERVED(PCS1G_ANEG_STATUS),
+	REG_RESERVED(PCS1G_ANEG_NP_STATUS),
+	REG_RESERVED(PCS1G_LINK_STATUS),
+	REG_RESERVED(PCS1G_LINK_DOWN_CNT),
+	REG_RESERVED(PCS1G_STICKY),
+	REG_RESERVED(PCS1G_DEBUG_STATUS),
+	REG_RESERVED(PCS1G_LPI_CFG),
+	REG_RESERVED(PCS1G_LPI_WAKE_ERROR_CNT),
+	REG_RESERVED(PCS1G_LPI_STATUS),
+	REG_RESERVED(PCS1G_TSTPAT_MODE_CFG),
+	REG_RESERVED(PCS1G_TSTPAT_STATUS),
+	REG_RESERVED(DEV_PCS_FX100_CFG),
+	REG_RESERVED(DEV_PCS_FX100_STATUS),
+};
+
+static const u32 *vsc9953_regmap[TARGET_MAX] = {
+	[ANA]		= vsc9953_ana_regmap,
+	[QS]		= vsc9953_qs_regmap,
+	[QSYS]		= vsc9953_qsys_regmap,
+	[REW]		= vsc9953_rew_regmap,
+	[SYS]		= vsc9953_sys_regmap,
+	[S2]		= vsc9953_s2_regmap,
+	[GCB]		= vsc9953_gcb_regmap,
+	[DEV_GMII]	= vsc9953_dev_gmii_regmap,
+};
+
+/* Addresses are relative to the device's base address */
+static const struct resource vsc9953_target_io_res[TARGET_MAX] = {
+	[ANA] = {
+		.start	= 0x0280000,
+		.end	= 0x028ffff,
+		.name	= "ana",
+	},
+	[QS] = {
+		.start	= 0x0080000,
+		.end	= 0x00800ff,
+		.name	= "qs",
+	},
+	[QSYS] = {
+		.start	= 0x0200000,
+		.end	= 0x021ffff,
+		.name	= "qsys",
+	},
+	[REW] = {
+		.start	= 0x0030000,
+		.end	= 0x003ffff,
+		.name	= "rew",
+	},
+	[SYS] = {
+		.start	= 0x0010000,
+		.end	= 0x001ffff,
+		.name	= "sys",
+	},
+	[S2] = {
+		.start	= 0x0060000,
+		.end	= 0x00603ff,
+		.name	= "s2",
+	},
+	[PTP] = {
+		.start	= 0x0090000,
+		.end	= 0x00900cb,
+		.name	= "ptp",
+	},
+	[GCB] = {
+		.start	= 0x0070000,
+		.end	= 0x00701ff,
+		.name	= "devcpu_gcb",
+	},
+};
+
+static const struct resource vsc9953_port_io_res[] = {
+	{
+		.start	= 0x0100000,
+		.end	= 0x010ffff,
+		.name	= "port0",
+	},
+	{
+		.start	= 0x0110000,
+		.end	= 0x011ffff,
+		.name	= "port1",
+	},
+	{
+		.start	= 0x0120000,
+		.end	= 0x012ffff,
+		.name	= "port2",
+	},
+	{
+		.start	= 0x0130000,
+		.end	= 0x013ffff,
+		.name	= "port3",
+	},
+	{
+		.start	= 0x0140000,
+		.end	= 0x014ffff,
+		.name	= "port4",
+	},
+	{
+		.start	= 0x0150000,
+		.end	= 0x015ffff,
+		.name	= "port5",
+	},
+	{
+		.start	= 0x0160000,
+		.end	= 0x016ffff,
+		.name	= "port6",
+	},
+	{
+		.start	= 0x0170000,
+		.end	= 0x017ffff,
+		.name	= "port7",
+	},
+	{
+		.start	= 0x0180000,
+		.end	= 0x018ffff,
+		.name	= "port8",
+	},
+	{
+		.start	= 0x0190000,
+		.end	= 0x019ffff,
+		.name	= "port9",
+	},
+};
+
+static const struct reg_field vsc9953_regfields[REGFIELD_MAX] = {
+	[ANA_ADVLEARN_VLAN_CHK] = REG_FIELD(ANA_ADVLEARN, 10, 10),
+	[ANA_ADVLEARN_LEARN_MIRROR] = REG_FIELD(ANA_ADVLEARN, 0, 9),
+	[ANA_ANEVENTS_AUTOAGE] = REG_FIELD(ANA_ANEVENTS, 24, 24),
+	[ANA_ANEVENTS_STORM_DROP] = REG_FIELD(ANA_ANEVENTS, 22, 22),
+	[ANA_ANEVENTS_LEARN_DROP] = REG_FIELD(ANA_ANEVENTS, 21, 21),
+	[ANA_ANEVENTS_AGED_ENTRY] = REG_FIELD(ANA_ANEVENTS, 20, 20),
+	[ANA_ANEVENTS_CPU_LEARN_FAILED] = REG_FIELD(ANA_ANEVENTS, 19, 19),
+	[ANA_ANEVENTS_AUTO_LEARN_FAILED] = REG_FIELD(ANA_ANEVENTS, 18, 18),
+	[ANA_ANEVENTS_LEARN_REMOVE] = REG_FIELD(ANA_ANEVENTS, 17, 17),
+	[ANA_ANEVENTS_AUTO_LEARNED] = REG_FIELD(ANA_ANEVENTS, 16, 16),
+	[ANA_ANEVENTS_AUTO_MOVED] = REG_FIELD(ANA_ANEVENTS, 15, 15),
+	[ANA_ANEVENTS_CLASSIFIED_DROP] = REG_FIELD(ANA_ANEVENTS, 13, 13),
+	[ANA_ANEVENTS_CLASSIFIED_COPY] = REG_FIELD(ANA_ANEVENTS, 12, 12),
+	[ANA_ANEVENTS_VLAN_DISCARD] = REG_FIELD(ANA_ANEVENTS, 11, 11),
+	[ANA_ANEVENTS_FWD_DISCARD] = REG_FIELD(ANA_ANEVENTS, 10, 10),
+	[ANA_ANEVENTS_MULTICAST_FLOOD] = REG_FIELD(ANA_ANEVENTS, 9, 9),
+	[ANA_ANEVENTS_UNICAST_FLOOD] = REG_FIELD(ANA_ANEVENTS, 8, 8),
+	[ANA_ANEVENTS_DEST_KNOWN] = REG_FIELD(ANA_ANEVENTS, 7, 7),
+	[ANA_ANEVENTS_BUCKET3_MATCH] = REG_FIELD(ANA_ANEVENTS, 6, 6),
+	[ANA_ANEVENTS_BUCKET2_MATCH] = REG_FIELD(ANA_ANEVENTS, 5, 5),
+	[ANA_ANEVENTS_BUCKET1_MATCH] = REG_FIELD(ANA_ANEVENTS, 4, 4),
+	[ANA_ANEVENTS_BUCKET0_MATCH] = REG_FIELD(ANA_ANEVENTS, 3, 3),
+	[ANA_ANEVENTS_CPU_OPERATION] = REG_FIELD(ANA_ANEVENTS, 2, 2),
+	[ANA_ANEVENTS_DMAC_LOOKUP] = REG_FIELD(ANA_ANEVENTS, 1, 1),
+	[ANA_ANEVENTS_SMAC_LOOKUP] = REG_FIELD(ANA_ANEVENTS, 0, 0),
+	[ANA_TABLES_MACACCESS_B_DOM] = REG_FIELD(ANA_TABLES_MACACCESS, 16, 16),
+	[ANA_TABLES_MACTINDX_BUCKET] = REG_FIELD(ANA_TABLES_MACTINDX, 11, 12),
+	[ANA_TABLES_MACTINDX_M_INDEX] = REG_FIELD(ANA_TABLES_MACTINDX, 0, 10),
+	[SYS_RESET_CFG_CORE_ENA] = REG_FIELD(SYS_RESET_CFG, 7, 7),
+	[SYS_RESET_CFG_MEM_ENA] = REG_FIELD(SYS_RESET_CFG, 6, 6),
+	[SYS_RESET_CFG_MEM_INIT] = REG_FIELD(SYS_RESET_CFG, 5, 5),
+	[GCB_SOFT_RST_SWC_RST] = REG_FIELD(GCB_SOFT_RST, 0, 0),
+	[GCB_MIIM_MII_STATUS_PENDING] = REG_FIELD(GCB_MIIM_MII_STATUS, 2, 2),
+	[GCB_MIIM_MII_STATUS_BUSY] = REG_FIELD(GCB_MIIM_MII_STATUS, 3, 3),
+	/* Replicated per number of ports (11), register size 4 per port */
+	[QSYS_SWITCH_PORT_MODE_PORT_ENA] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 13, 13, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_YEL_RSRVD] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 10, 10, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 9, 9, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_TX_PFC_ENA] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 1, 8, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_TX_PFC_MODE] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 0, 0, 11, 4),
+	[SYS_PORT_MODE_INCL_INJ_HDR] = REG_FIELD_ID(SYS_PORT_MODE, 4, 5, 11, 4),
+	[SYS_PORT_MODE_INCL_XTR_HDR] = REG_FIELD_ID(SYS_PORT_MODE, 2, 3, 11, 4),
+	[SYS_PORT_MODE_INCL_HDR_ERR] = REG_FIELD_ID(SYS_PORT_MODE, 0, 0, 11, 4),
+	[SYS_PAUSE_CFG_PAUSE_START] = REG_FIELD_ID(SYS_PAUSE_CFG, 11, 20, 11, 4),
+	[SYS_PAUSE_CFG_PAUSE_STOP] = REG_FIELD_ID(SYS_PAUSE_CFG, 1, 10, 11, 4),
+	[SYS_PAUSE_CFG_PAUSE_ENA] = REG_FIELD_ID(SYS_PAUSE_CFG, 0, 1, 11, 4),
+};
+
+static const struct ocelot_stat_layout vsc9953_stats_layout[] = {
+	{ .offset = 0x00,	.name = "rx_octets", },
+	{ .offset = 0x01,	.name = "rx_unicast", },
+	{ .offset = 0x02,	.name = "rx_multicast", },
+	{ .offset = 0x03,	.name = "rx_broadcast", },
+	{ .offset = 0x04,	.name = "rx_shorts", },
+	{ .offset = 0x05,	.name = "rx_fragments", },
+	{ .offset = 0x06,	.name = "rx_jabbers", },
+	{ .offset = 0x07,	.name = "rx_crc_align_errs", },
+	{ .offset = 0x08,	.name = "rx_sym_errs", },
+	{ .offset = 0x09,	.name = "rx_frames_below_65_octets", },
+	{ .offset = 0x0A,	.name = "rx_frames_65_to_127_octets", },
+	{ .offset = 0x0B,	.name = "rx_frames_128_to_255_octets", },
+	{ .offset = 0x0C,	.name = "rx_frames_256_to_511_octets", },
+	{ .offset = 0x0D,	.name = "rx_frames_512_to_1023_octets", },
+	{ .offset = 0x0E,	.name = "rx_frames_1024_to_1526_octets", },
+	{ .offset = 0x0F,	.name = "rx_frames_over_1526_octets", },
+	{ .offset = 0x10,	.name = "rx_pause", },
+	{ .offset = 0x11,	.name = "rx_control", },
+	{ .offset = 0x12,	.name = "rx_longs", },
+	{ .offset = 0x13,	.name = "rx_classified_drops", },
+	{ .offset = 0x14,	.name = "rx_red_prio_0", },
+	{ .offset = 0x15,	.name = "rx_red_prio_1", },
+	{ .offset = 0x16,	.name = "rx_red_prio_2", },
+	{ .offset = 0x17,	.name = "rx_red_prio_3", },
+	{ .offset = 0x18,	.name = "rx_red_prio_4", },
+	{ .offset = 0x19,	.name = "rx_red_prio_5", },
+	{ .offset = 0x1A,	.name = "rx_red_prio_6", },
+	{ .offset = 0x1B,	.name = "rx_red_prio_7", },
+	{ .offset = 0x1C,	.name = "rx_yellow_prio_0", },
+	{ .offset = 0x1D,	.name = "rx_yellow_prio_1", },
+	{ .offset = 0x1E,	.name = "rx_yellow_prio_2", },
+	{ .offset = 0x1F,	.name = "rx_yellow_prio_3", },
+	{ .offset = 0x20,	.name = "rx_yellow_prio_4", },
+	{ .offset = 0x21,	.name = "rx_yellow_prio_5", },
+	{ .offset = 0x22,	.name = "rx_yellow_prio_6", },
+	{ .offset = 0x23,	.name = "rx_yellow_prio_7", },
+	{ .offset = 0x24,	.name = "rx_green_prio_0", },
+	{ .offset = 0x25,	.name = "rx_green_prio_1", },
+	{ .offset = 0x26,	.name = "rx_green_prio_2", },
+	{ .offset = 0x27,	.name = "rx_green_prio_3", },
+	{ .offset = 0x28,	.name = "rx_green_prio_4", },
+	{ .offset = 0x29,	.name = "rx_green_prio_5", },
+	{ .offset = 0x2A,	.name = "rx_green_prio_6", },
+	{ .offset = 0x2B,	.name = "rx_green_prio_7", },
+	{ .offset = 0x40,	.name = "tx_octets", },
+	{ .offset = 0x41,	.name = "tx_unicast", },
+	{ .offset = 0x42,	.name = "tx_multicast", },
+	{ .offset = 0x43,	.name = "tx_broadcast", },
+	{ .offset = 0x44,	.name = "tx_collision", },
+	{ .offset = 0x45,	.name = "tx_drops", },
+	{ .offset = 0x46,	.name = "tx_pause", },
+	{ .offset = 0x47,	.name = "tx_frames_below_65_octets", },
+	{ .offset = 0x48,	.name = "tx_frames_65_to_127_octets", },
+	{ .offset = 0x49,	.name = "tx_frames_128_255_octets", },
+	{ .offset = 0x4A,	.name = "tx_frames_256_511_octets", },
+	{ .offset = 0x4B,	.name = "tx_frames_512_1023_octets", },
+	{ .offset = 0x4C,	.name = "tx_frames_1024_1526_octets", },
+	{ .offset = 0x4D,	.name = "tx_frames_over_1526_octets", },
+	{ .offset = 0x4E,	.name = "tx_yellow_prio_0", },
+	{ .offset = 0x4F,	.name = "tx_yellow_prio_1", },
+	{ .offset = 0x50,	.name = "tx_yellow_prio_2", },
+	{ .offset = 0x51,	.name = "tx_yellow_prio_3", },
+	{ .offset = 0x52,	.name = "tx_yellow_prio_4", },
+	{ .offset = 0x53,	.name = "tx_yellow_prio_5", },
+	{ .offset = 0x54,	.name = "tx_yellow_prio_6", },
+	{ .offset = 0x55,	.name = "tx_yellow_prio_7", },
+	{ .offset = 0x56,	.name = "tx_green_prio_0", },
+	{ .offset = 0x57,	.name = "tx_green_prio_1", },
+	{ .offset = 0x58,	.name = "tx_green_prio_2", },
+	{ .offset = 0x59,	.name = "tx_green_prio_3", },
+	{ .offset = 0x5A,	.name = "tx_green_prio_4", },
+	{ .offset = 0x5B,	.name = "tx_green_prio_5", },
+	{ .offset = 0x5C,	.name = "tx_green_prio_6", },
+	{ .offset = 0x5D,	.name = "tx_green_prio_7", },
+	{ .offset = 0x5E,	.name = "tx_aged", },
+	{ .offset = 0x80,	.name = "drop_local", },
+	{ .offset = 0x81,	.name = "drop_tail", },
+	{ .offset = 0x82,	.name = "drop_yellow_prio_0", },
+	{ .offset = 0x83,	.name = "drop_yellow_prio_1", },
+	{ .offset = 0x84,	.name = "drop_yellow_prio_2", },
+	{ .offset = 0x85,	.name = "drop_yellow_prio_3", },
+	{ .offset = 0x86,	.name = "drop_yellow_prio_4", },
+	{ .offset = 0x87,	.name = "drop_yellow_prio_5", },
+	{ .offset = 0x88,	.name = "drop_yellow_prio_6", },
+	{ .offset = 0x89,	.name = "drop_yellow_prio_7", },
+	{ .offset = 0x8A,	.name = "drop_green_prio_0", },
+	{ .offset = 0x8B,	.name = "drop_green_prio_1", },
+	{ .offset = 0x8C,	.name = "drop_green_prio_2", },
+	{ .offset = 0x8D,	.name = "drop_green_prio_3", },
+	{ .offset = 0x8E,	.name = "drop_green_prio_4", },
+	{ .offset = 0x8F,	.name = "drop_green_prio_5", },
+	{ .offset = 0x90,	.name = "drop_green_prio_6", },
+	{ .offset = 0x91,	.name = "drop_green_prio_7", },
+};
+
+static struct vcap_field vsc9953_vcap_is2_keys[] = {
+	/* Common: 41 bits */
+	[VCAP_IS2_TYPE]				= {  0,   4},
+	[VCAP_IS2_HK_FIRST]			= {  4,   1},
+	[VCAP_IS2_HK_PAG]			= {  5,   8},
+	[VCAP_IS2_HK_IGR_PORT_MASK]		= { 13,  11},
+	[VCAP_IS2_HK_RSV2]			= { 24,   1},
+	[VCAP_IS2_HK_HOST_MATCH]		= { 25,   1},
+	[VCAP_IS2_HK_L2_MC]			= { 26,   1},
+	[VCAP_IS2_HK_L2_BC]			= { 27,   1},
+	[VCAP_IS2_HK_VLAN_TAGGED]		= { 28,   1},
+	[VCAP_IS2_HK_VID]			= { 29,  12},
+	[VCAP_IS2_HK_DEI]			= { 41,   1},
+	[VCAP_IS2_HK_PCP]			= { 42,   3},
+	/* MAC_ETYPE / MAC_LLC / MAC_SNAP / OAM common */
+	[VCAP_IS2_HK_L2_DMAC]			= { 45,  48},
+	[VCAP_IS2_HK_L2_SMAC]			= { 93,  48},
+	/* MAC_ETYPE (TYPE=000) */
+	[VCAP_IS2_HK_MAC_ETYPE_ETYPE]		= {141,  16},
+	[VCAP_IS2_HK_MAC_ETYPE_L2_PAYLOAD0]	= {157,  16},
+	[VCAP_IS2_HK_MAC_ETYPE_L2_PAYLOAD1]	= {173,   8},
+	[VCAP_IS2_HK_MAC_ETYPE_L2_PAYLOAD2]	= {181,   3},
+	/* MAC_LLC (TYPE=001) */
+	[VCAP_IS2_HK_MAC_LLC_L2_LLC]		= {141,  40},
+	/* MAC_SNAP (TYPE=010) */
+	[VCAP_IS2_HK_MAC_SNAP_L2_SNAP]		= {141,  40},
+	/* MAC_ARP (TYPE=011) */
+	[VCAP_IS2_HK_MAC_ARP_SMAC]		= { 45,  48},
+	[VCAP_IS2_HK_MAC_ARP_ADDR_SPACE_OK]	= { 93,   1},
+	[VCAP_IS2_HK_MAC_ARP_PROTO_SPACE_OK]	= { 94,   1},
+	[VCAP_IS2_HK_MAC_ARP_LEN_OK]		= { 95,   1},
+	[VCAP_IS2_HK_MAC_ARP_TARGET_MATCH]	= { 96,   1},
+	[VCAP_IS2_HK_MAC_ARP_SENDER_MATCH]	= { 97,   1},
+	[VCAP_IS2_HK_MAC_ARP_OPCODE_UNKNOWN]	= { 98,   1},
+	[VCAP_IS2_HK_MAC_ARP_OPCODE]		= { 99,   2},
+	[VCAP_IS2_HK_MAC_ARP_L3_IP4_DIP]	= {101,  32},
+	[VCAP_IS2_HK_MAC_ARP_L3_IP4_SIP]	= {133,  32},
+	[VCAP_IS2_HK_MAC_ARP_DIP_EQ_SIP]	= {165,   1},
+	/* IP4_TCP_UDP / IP4_OTHER common */
+	[VCAP_IS2_HK_IP4]			= { 45,   1},
+	[VCAP_IS2_HK_L3_FRAGMENT]		= { 46,   1},
+	[VCAP_IS2_HK_L3_FRAG_OFS_GT0]		= { 47,   1},
+	[VCAP_IS2_HK_L3_OPTIONS]		= { 48,   1},
+	[VCAP_IS2_HK_IP4_L3_TTL_GT0]		= { 49,   1},
+	[VCAP_IS2_HK_L3_TOS]			= { 50,   8},
+	[VCAP_IS2_HK_L3_IP4_DIP]		= { 58,  32},
+	[VCAP_IS2_HK_L3_IP4_SIP]		= { 90,  32},
+	[VCAP_IS2_HK_DIP_EQ_SIP]		= {122,   1},
+	/* IP4_TCP_UDP (TYPE=100) */
+	[VCAP_IS2_HK_TCP]			= {123,   1},
+	[VCAP_IS2_HK_L4_SPORT]			= {124,  16},
+	[VCAP_IS2_HK_L4_DPORT]			= {140,  16},
+	[VCAP_IS2_HK_L4_RNG]			= {156,   8},
+	[VCAP_IS2_HK_L4_SPORT_EQ_DPORT]		= {164,   1},
+	[VCAP_IS2_HK_L4_SEQUENCE_EQ0]		= {165,   1},
+	[VCAP_IS2_HK_L4_URG]			= {166,   1},
+	[VCAP_IS2_HK_L4_ACK]			= {167,   1},
+	[VCAP_IS2_HK_L4_PSH]			= {168,   1},
+	[VCAP_IS2_HK_L4_RST]			= {169,   1},
+	[VCAP_IS2_HK_L4_SYN]			= {170,   1},
+	[VCAP_IS2_HK_L4_FIN]			= {171,   1},
+	/* IP4_OTHER (TYPE=101) */
+	[VCAP_IS2_HK_IP4_L3_PROTO]		= {123,   8},
+	[VCAP_IS2_HK_L3_PAYLOAD]		= {131,  56},
+	/* IP6_STD (TYPE=110) */
+	[VCAP_IS2_HK_IP6_L3_TTL_GT0]		= { 45,   1},
+	[VCAP_IS2_HK_L3_IP6_SIP]		= { 46, 128},
+	[VCAP_IS2_HK_IP6_L3_PROTO]		= {174,   8},
+};
+
+static struct vcap_field vsc9953_vcap_is2_actions[] = {
+	[VCAP_IS2_ACT_HIT_ME_ONCE]		= {  0,  1},
+	[VCAP_IS2_ACT_CPU_COPY_ENA]		= {  1,  1},
+	[VCAP_IS2_ACT_CPU_QU_NUM]		= {  2,  3},
+	[VCAP_IS2_ACT_MASK_MODE]		= {  5,  2},
+	[VCAP_IS2_ACT_MIRROR_ENA]		= {  7,  1},
+	[VCAP_IS2_ACT_LRN_DIS]			= {  8,  1},
+	[VCAP_IS2_ACT_POLICE_ENA]		= {  9,  1},
+	[VCAP_IS2_ACT_POLICE_IDX]		= { 10,  8},
+	[VCAP_IS2_ACT_POLICE_VCAP_ONLY]		= { 21,  1},
+	[VCAP_IS2_ACT_PORT_MASK]		= { 22, 10},
+	[VCAP_IS2_ACT_ACL_ID]			= { 44,  6},
+	[VCAP_IS2_ACT_HIT_CNT]			= { 50, 32},
+};
+
+static const struct vcap_props vsc9953_vcap_props[] = {
+	[VCAP_IS2] = {
+		.tg_width = 2,
+		.sw_count = 4,
+		.entry_count = VSC9953_VCAP_IS2_CNT,
+		.entry_width = VSC9953_VCAP_IS2_ENTRY_WIDTH,
+		.action_count = VSC9953_VCAP_IS2_CNT +
+				VSC9953_VCAP_PORT_CNT + 2,
+		.action_width = 101,
+		.action_type_width = 1,
+		.action_table = {
+			[IS2_ACTION_TYPE_NORMAL] = {
+				.width = 44,
+				.count = 2
+			},
+			[IS2_ACTION_TYPE_SMAC_SIP] = {
+				.width = 6,
+				.count = 4
+			},
+		},
+		.counter_words = 4,
+		.counter_width = 32,
+	},
+};
+
+#define VSC9953_INIT_TIMEOUT			50000
+#define VSC9953_GCB_RST_SLEEP			100
+#define VSC9953_SYS_RAMINIT_SLEEP		80
+#define VCS9953_MII_TIMEOUT			10000
+
+static int vsc9953_gcb_soft_rst_status(struct ocelot *ocelot)
+{
+	int val;
+
+	ocelot_field_read(ocelot, GCB_SOFT_RST_SWC_RST, &val);
+
+	return val;
+}
+
+static int vsc9953_sys_ram_init_status(struct ocelot *ocelot)
+{
+	int val;
+
+	ocelot_field_read(ocelot, SYS_RESET_CFG_MEM_INIT, &val);
+
+	return val;
+}
+
+static int vsc9953_gcb_miim_pending_status(struct ocelot *ocelot)
+{
+	int val;
+
+	ocelot_field_read(ocelot, GCB_MIIM_MII_STATUS_PENDING, &val);
+
+	return val;
+}
+
+static int vsc9953_gcb_miim_busy_status(struct ocelot *ocelot)
+{
+	int val;
+
+	ocelot_field_read(ocelot, GCB_MIIM_MII_STATUS_BUSY, &val);
+
+	return val;
+}
+
+static int vsc9953_mdio_write(struct mii_bus *bus, int phy_id, int regnum,
+			      u16 value)
+{
+	struct ocelot *ocelot = bus->priv;
+	int err, cmd, val;
+
+	/* Wait while MIIM controller becomes idle */
+	err = readx_poll_timeout(vsc9953_gcb_miim_pending_status, ocelot,
+				 val, !val, 10, VCS9953_MII_TIMEOUT);
+	if (err) {
+		dev_err(ocelot->dev, "MDIO write: pending timeout\n");
+		goto out;
+	}
+
+	cmd = MSCC_MIIM_CMD_VLD | (phy_id << MSCC_MIIM_CMD_PHYAD_SHIFT) |
+	      (regnum << MSCC_MIIM_CMD_REGAD_SHIFT) |
+	      (value << MSCC_MIIM_CMD_WRDATA_SHIFT) |
+	      MSCC_MIIM_CMD_OPR_WRITE;
+
+	ocelot_write(ocelot, cmd, GCB_MIIM_MII_CMD);
+
+out:
+	return err;
+}
+
+static int vsc9953_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
+{
+	struct ocelot *ocelot = bus->priv;
+	int err, cmd, val;
+
+	/* Wait until MIIM controller becomes idle */
+	err = readx_poll_timeout(vsc9953_gcb_miim_pending_status, ocelot,
+				 val, !val, 10, VCS9953_MII_TIMEOUT);
+	if (err) {
+		dev_err(ocelot->dev, "MDIO read: pending timeout\n");
+		goto out;
+	}
+
+	/* Write the MIIM COMMAND register */
+	cmd = MSCC_MIIM_CMD_VLD | (phy_id << MSCC_MIIM_CMD_PHYAD_SHIFT) |
+	      (regnum << MSCC_MIIM_CMD_REGAD_SHIFT) | MSCC_MIIM_CMD_OPR_READ;
+
+	ocelot_write(ocelot, cmd, GCB_MIIM_MII_CMD);
+
+	/* Wait while read operation via the MIIM controller is in progress */
+	err = readx_poll_timeout(vsc9953_gcb_miim_busy_status, ocelot,
+				 val, !val, 10, VCS9953_MII_TIMEOUT);
+	if (err) {
+		dev_err(ocelot->dev, "MDIO read: busy timeout\n");
+		goto out;
+	}
+
+	val = ocelot_read(ocelot, GCB_MIIM_MII_DATA);
+
+	err = val & 0xFFFF;
+out:
+	return err;
+}
+
+static int vsc9953_reset(struct ocelot *ocelot)
+{
+	int val, err;
+
+	/* soft-reset the switch core */
+	ocelot_field_write(ocelot, GCB_SOFT_RST_SWC_RST, 1);
+
+	err = readx_poll_timeout(vsc9953_gcb_soft_rst_status, ocelot, val, !val,
+				 VSC9953_GCB_RST_SLEEP, VSC9953_INIT_TIMEOUT);
+	if (err) {
+		dev_err(ocelot->dev, "timeout: switch core reset\n");
+		return err;
+	}
+
+	/* initialize switch mem ~40us */
+	ocelot_field_write(ocelot, SYS_RESET_CFG_MEM_INIT, 1);
+	ocelot_field_write(ocelot, SYS_RESET_CFG_MEM_ENA, 1);
+
+	err = readx_poll_timeout(vsc9953_sys_ram_init_status, ocelot, val, !val,
+				 VSC9953_SYS_RAMINIT_SLEEP,
+				 VSC9953_INIT_TIMEOUT);
+	if (err) {
+		dev_err(ocelot->dev, "timeout: switch sram init\n");
+		return err;
+	}
+
+	/* enable switch core */
+	ocelot_field_write(ocelot, SYS_RESET_CFG_MEM_ENA, 1);
+	ocelot_field_write(ocelot, SYS_RESET_CFG_CORE_ENA, 1);
+
+	return 0;
+}
+
+static void vsc9953_phylink_validate(struct ocelot *ocelot, int port,
+				     unsigned long *supported,
+				     struct phylink_link_state *state)
+{
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	__ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
+
+	if (state->interface != PHY_INTERFACE_MODE_NA &&
+	    state->interface != ocelot_port->phy_mode) {
+		bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS);
+		return;
+	}
+
+	phylink_set_port_modes(mask);
+	phylink_set(mask, Autoneg);
+	phylink_set(mask, Pause);
+	phylink_set(mask, Asym_Pause);
+	phylink_set(mask, 10baseT_Full);
+	phylink_set(mask, 10baseT_Half);
+	phylink_set(mask, 100baseT_Full);
+	phylink_set(mask, 100baseT_Half);
+	phylink_set(mask, 1000baseT_Full);
+
+	if (state->interface == PHY_INTERFACE_MODE_INTERNAL) {
+		phylink_set(mask, 2500baseT_Full);
+		phylink_set(mask, 2500baseX_Full);
+	}
+
+	bitmap_and(supported, supported, mask,
+		   __ETHTOOL_LINK_MODE_MASK_NBITS);
+	bitmap_and(state->advertising, state->advertising, mask,
+		   __ETHTOOL_LINK_MODE_MASK_NBITS);
+}
+
+static int vsc9953_prevalidate_phy_mode(struct ocelot *ocelot, int port,
+					phy_interface_t phy_mode)
+{
+	switch (phy_mode) {
+	case PHY_INTERFACE_MODE_INTERNAL:
+		if (port != 8 && port != 9)
+			return -ENOTSUPP;
+		return 0;
+	case PHY_INTERFACE_MODE_SGMII:
+	case PHY_INTERFACE_MODE_QSGMII:
+		/* Not supported on internal to-CPU ports */
+		if (port == 8 || port == 9)
+			return -ENOTSUPP;
+		return 0;
+	default:
+		return -ENOTSUPP;
+	}
+}
+
+/* Watermark encode
+ * Bit 9:   Unit; 0:1, 1:16
+ * Bit 8-0: Value to be multiplied with unit
+ */
+static u16 vsc9953_wm_enc(u16 value)
+{
+	if (value >= BIT(9))
+		return BIT(9) | (value / 16);
+
+	return value;
+}
+
+static const struct ocelot_ops vsc9953_ops = {
+	.reset			= vsc9953_reset,
+	.wm_enc			= vsc9953_wm_enc,
+};
+
+static int vsc9953_mdio_bus_alloc(struct ocelot *ocelot)
+{
+	struct felix *felix = ocelot_to_felix(ocelot);
+	struct device *dev = ocelot->dev;
+	struct mii_bus *bus;
+	int port;
+	int rc;
+
+	felix->pcs = devm_kcalloc(dev, felix->info->num_ports,
+				  sizeof(struct phy_device *),
+				  GFP_KERNEL);
+	if (!felix->pcs) {
+		dev_err(dev, "failed to allocate array for PCS PHYs\n");
+		return -ENOMEM;
+	}
+
+	bus = devm_mdiobus_alloc(dev);
+	if (!bus)
+		return -ENOMEM;
+
+	bus->name = "VSC9953 internal MDIO bus";
+	bus->read = vsc9953_mdio_read;
+	bus->write = vsc9953_mdio_write;
+	bus->parent = dev;
+	bus->priv = ocelot;
+	snprintf(bus->id, MII_BUS_ID_SIZE, "%s-imdio", dev_name(dev));
+
+	/* Needed in order to initialize the bus mutex lock */
+	rc = mdiobus_register(bus);
+	if (rc < 0) {
+		dev_err(dev, "failed to register MDIO bus\n");
+		return rc;
+	}
+
+	felix->imdio = bus;
+
+	for (port = 0; port < felix->info->num_ports; port++) {
+		struct ocelot_port *ocelot_port = ocelot->ports[port];
+		struct phy_device *pcs;
+		int addr = port + 4;
+
+		if (ocelot_port->phy_mode == PHY_INTERFACE_MODE_INTERNAL)
+			continue;
+
+		pcs = get_phy_device(felix->imdio, addr, false);
+		if (IS_ERR(pcs))
+			continue;
+
+		pcs->interface = ocelot_port->phy_mode;
+		felix->pcs[port] = pcs;
+
+		dev_info(dev, "Found PCS at internal MDIO address %d\n", addr);
+	}
+
+	return 0;
+}
+
+static void vsc9953_xmit_template_populate(struct ocelot *ocelot, int port)
+{
+	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	u8 *template = ocelot_port->xmit_template;
+	u64 bypass, dest, src;
+
+	/* Set the source port as the CPU port module and not the
+	 * NPI port
+	 */
+	src = ocelot->num_phys_ports;
+	dest = BIT(port);
+	bypass = true;
+
+	packing(template, &bypass, 127, 127, OCELOT_TAG_LEN, PACK, 0);
+	packing(template, &dest,    67,  57, OCELOT_TAG_LEN, PACK, 0);
+	packing(template, &src,     46,  43, OCELOT_TAG_LEN, PACK, 0);
+}
+
+static const struct felix_info seville_info_vsc9953 = {
+	.target_io_res		= vsc9953_target_io_res,
+	.port_io_res		= vsc9953_port_io_res,
+	.regfields		= vsc9953_regfields,
+	.map			= vsc9953_regmap,
+	.ops			= &vsc9953_ops,
+	.stats_layout		= vsc9953_stats_layout,
+	.num_stats		= ARRAY_SIZE(vsc9953_stats_layout),
+	.vcap_is2_keys		= vsc9953_vcap_is2_keys,
+	.vcap_is2_actions	= vsc9953_vcap_is2_actions,
+	.vcap			= vsc9953_vcap_props,
+	.shared_queue_sz	= 128 * 1024,
+	.num_mact_rows		= 2048,
+	.num_ports		= 10,
+	.mdio_bus_alloc		= vsc9953_mdio_bus_alloc,
+	.mdio_bus_free		= vsc9959_mdio_bus_free,
+	.pcs_config		= vsc9959_pcs_config,
+	.pcs_link_up		= vsc9959_pcs_link_up,
+	.pcs_link_state		= vsc9959_pcs_link_state,
+	.phylink_validate	= vsc9953_phylink_validate,
+	.prevalidate_phy_mode	= vsc9953_prevalidate_phy_mode,
+	.xmit_template_populate	= vsc9953_xmit_template_populate,
+};
+
+static int seville_probe(struct platform_device *pdev)
+{
+	struct dsa_switch *ds;
+	struct ocelot *ocelot;
+	struct resource *res;
+	struct felix *felix;
+	int err;
+
+	felix = kzalloc(sizeof(struct felix), GFP_KERNEL);
+	if (!felix) {
+		err = -ENOMEM;
+		dev_err(&pdev->dev, "Failed to allocate driver memory\n");
+		goto err_alloc_felix;
+	}
+
+	platform_set_drvdata(pdev, felix);
+
+	ocelot = &felix->ocelot;
+	ocelot->dev = &pdev->dev;
+	felix->info = &seville_info_vsc9953;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	felix->switch_base = res->start;
+
+	ds = kzalloc(sizeof(struct dsa_switch), GFP_KERNEL);
+	if (!ds) {
+		err = -ENOMEM;
+		dev_err(&pdev->dev, "Failed to allocate DSA switch\n");
+		goto err_alloc_ds;
+	}
+
+	ds->dev = &pdev->dev;
+	ds->num_ports = felix->info->num_ports;
+	ds->ops = &felix_switch_ops;
+	ds->priv = ocelot;
+	felix->ds = ds;
+
+	err = dsa_register_switch(ds);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to register DSA switch: %d\n", err);
+		goto err_register_ds;
+	}
+
+	return 0;
+
+err_register_ds:
+	kfree(ds);
+err_alloc_ds:
+err_alloc_felix:
+	kfree(felix);
+	return err;
+}
+
+static int seville_remove(struct platform_device *pdev)
+{
+	struct felix *felix;
+
+	felix = platform_get_drvdata(pdev);
+
+	dsa_unregister_switch(felix->ds);
+
+	kfree(felix->ds);
+	kfree(felix);
+
+	return 0;
+}
+
+static const struct of_device_id seville_of_match[] = {
+	{ .compatible = "mscc,vsc9953-switch" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, seville_of_match);
+
+struct platform_driver seville_vsc9953_driver = {
+	.probe		= seville_probe,
+	.remove		= seville_remove,
+	.driver = {
+		.name		= "mscc_seville",
+		.of_match_table	= of_match_ptr(seville_of_match),
+	},
+};
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index e815aad8d85e..f2d94b026d88 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -309,18 +309,6 @@ static void ocelot_vlan_init(struct ocelot *ocelot)
 	}
 }
 
-/* Watermark encode
- * Bit 8:   Unit; 0:1, 1:16
- * Bit 7-0: Value to be multiplied with unit
- */
-static u16 ocelot_wm_enc(u16 value)
-{
-	if (value >= BIT(8))
-		return BIT(8) | (value / 16);
-
-	return value;
-}
-
 void ocelot_adjust_link(struct ocelot *ocelot, int port,
 			struct phy_device *phydev)
 {
@@ -389,10 +377,8 @@ void ocelot_adjust_link(struct ocelot *ocelot, int port,
 			 ANA_PFC_PFC_CFG, port);
 
 	/* Core: Enable port for frame transfer */
-	ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
-			 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
-			 QSYS_SWITCH_PORT_MODE_PORT_ENA,
-			 QSYS_SWITCH_PORT_MODE, port);
+	ocelot_fields_write(ocelot, port,
+			    QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
 
 	/* Flow control */
 	ocelot_write_rix(ocelot, SYS_MAC_FC_CFG_PAUSE_VAL_CFG(0xffff) |
@@ -423,8 +409,7 @@ void ocelot_port_disable(struct ocelot *ocelot, int port)
 	struct ocelot_port *ocelot_port = ocelot->ports[port];
 
 	ocelot_port_writel(ocelot_port, 0, DEV_MAC_ENA_CFG);
-	ocelot_rmw_rix(ocelot, 0, QSYS_SWITCH_PORT_MODE_PORT_ENA,
-		       QSYS_SWITCH_PORT_MODE, port);
+	ocelot_fields_write(ocelot, port, QSYS_SWITCH_PORT_MODE_PORT_ENA, 0);
 }
 EXPORT_SYMBOL(ocelot_port_disable);
 
@@ -1262,6 +1247,7 @@ void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu)
 {
 	struct ocelot_port *ocelot_port = ocelot->ports[port];
 	int maxlen = sdu + ETH_HLEN + ETH_FCS_LEN;
+	int pause_start, pause_stop;
 	int atop_wm;
 
 	if (port == ocelot->npi) {
@@ -1275,20 +1261,20 @@ void ocelot_port_set_maxlen(struct ocelot *ocelot, int port, size_t sdu)
 
 	ocelot_port_writel(ocelot_port, maxlen, DEV_MAC_MAXLEN_CFG);
 
-	/* Set Pause WM hysteresis
-	 * 152 = 6 * maxlen / OCELOT_BUFFER_CELL_SZ
-	 * 101 = 4 * maxlen / OCELOT_BUFFER_CELL_SZ
-	 */
-	ocelot_write_rix(ocelot, SYS_PAUSE_CFG_PAUSE_ENA |
-			 SYS_PAUSE_CFG_PAUSE_STOP(101) |
-			 SYS_PAUSE_CFG_PAUSE_START(152), SYS_PAUSE_CFG, port);
+	/* Set Pause watermark hysteresis */
+	pause_start = 6 * maxlen / OCELOT_BUFFER_CELL_SZ;
+	pause_stop = 4 * maxlen / OCELOT_BUFFER_CELL_SZ;
+	ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_START,
+			    pause_start);
+	ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_STOP,
+			    pause_stop);
 
 	/* Tail dropping watermark */
 	atop_wm = (ocelot->shared_queue_sz - 9 * maxlen) /
 		   OCELOT_BUFFER_CELL_SZ;
-	ocelot_write_rix(ocelot, ocelot_wm_enc(9 * maxlen),
+	ocelot_write_rix(ocelot, ocelot->ops->wm_enc(9 * maxlen),
 			 SYS_ATOP, port);
-	ocelot_write(ocelot, ocelot_wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
+	ocelot_write(ocelot, ocelot->ops->wm_enc(atop_wm), SYS_ATOP_TOT_CFG);
 }
 EXPORT_SYMBOL(ocelot_port_set_maxlen);
 
@@ -1344,6 +1330,9 @@ void ocelot_init_port(struct ocelot *ocelot, int port)
 	ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_HIGH_CFG);
 	ocelot_port_writel(ocelot_port, 0, DEV_MAC_FC_MAC_LOW_CFG);
 
+	/* Enable transmission of pause frames */
+	ocelot_fields_write(ocelot, port, SYS_PAUSE_CFG_PAUSE_ENA, 1);
+
 	/* Drop frames with multicast source address */
 	ocelot_rmw_gix(ocelot, ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
 		       ANA_PORT_DROP_CFG_DROP_MC_SMAC_ENA,
@@ -1392,27 +1381,25 @@ void ocelot_configure_cpu(struct ocelot *ocelot, int npi,
 			     QSYS_EXT_CPU_CFG);
 
 		/* Enable NPI port */
-		ocelot_write_rix(ocelot,
-				 QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
-				 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
-				 QSYS_SWITCH_PORT_MODE_PORT_ENA,
-				 QSYS_SWITCH_PORT_MODE, npi);
+		ocelot_fields_write(ocelot, npi,
+				    QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
 		/* NPI port Injection/Extraction configuration */
-		ocelot_write_rix(ocelot,
-				 SYS_PORT_MODE_INCL_XTR_HDR(extraction) |
-				 SYS_PORT_MODE_INCL_INJ_HDR(injection),
-				 SYS_PORT_MODE, npi);
+		ocelot_fields_write(ocelot, npi, SYS_PORT_MODE_INCL_XTR_HDR,
+				    extraction);
+		ocelot_fields_write(ocelot, npi, SYS_PORT_MODE_INCL_INJ_HDR,
+				    injection);
+
+		/* Disable transmission of pause frames */
+		ocelot_fields_write(ocelot, npi, SYS_PAUSE_CFG_PAUSE_ENA, 0);
 	}
 
 	/* Enable CPU port module */
-	ocelot_write_rix(ocelot, QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE |
-			 QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(1) |
-			 QSYS_SWITCH_PORT_MODE_PORT_ENA,
-			 QSYS_SWITCH_PORT_MODE, cpu);
+	ocelot_fields_write(ocelot, cpu, QSYS_SWITCH_PORT_MODE_PORT_ENA, 1);
 	/* CPU port Injection/Extraction configuration */
-	ocelot_write_rix(ocelot, SYS_PORT_MODE_INCL_XTR_HDR(extraction) |
-			 SYS_PORT_MODE_INCL_INJ_HDR(injection),
-			 SYS_PORT_MODE, cpu);
+	ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_XTR_HDR,
+			    extraction);
+	ocelot_fields_write(ocelot, cpu, SYS_PORT_MODE_INCL_INJ_HDR,
+			    injection);
 
 	/* Configure the CPU port to be VLAN aware */
 	ocelot_write_gix(ocelot, ANA_PORT_VLAN_CFG_VLAN_VID(0) |
diff --git a/drivers/net/ethernet/mscc/ocelot.h b/drivers/net/ethernet/mscc/ocelot.h
index 394362e23c47..dc29e05103a1 100644
--- a/drivers/net/ethernet/mscc/ocelot.h
+++ b/drivers/net/ethernet/mscc/ocelot.h
@@ -102,11 +102,7 @@ void ocelot_port_lag_leave(struct ocelot *ocelot, int port,
 u32 ocelot_port_readl(struct ocelot_port *port, u32 reg);
 void ocelot_port_writel(struct ocelot_port *port, u32 val, u32 reg);
 
-#define ocelot_field_write(ocelot, reg, val) regmap_field_write((ocelot)->regfields[(reg)], (val))
-#define ocelot_field_read(ocelot, reg, val) regmap_field_read((ocelot)->regfields[(reg)], (val))
-
-int ocelot_probe_port(struct ocelot *ocelot, u8 port,
-		      void __iomem *regs,
+int ocelot_probe_port(struct ocelot *ocelot, int port, struct regmap *target,
 		      struct phy_device *phy);
 
 void ocelot_set_cpu_port(struct ocelot *ocelot, int cpu,
@@ -117,7 +113,4 @@ extern struct notifier_block ocelot_netdevice_nb;
 extern struct notifier_block ocelot_switchdev_nb;
 extern struct notifier_block ocelot_switchdev_blocking_nb;
 
-#define ocelot_field_write(ocelot, reg, val) regmap_field_write((ocelot)->regfields[(reg)], (val))
-#define ocelot_field_read(ocelot, reg, val) regmap_field_read((ocelot)->regfields[(reg)], (val))
-
 #endif
diff --git a/drivers/net/ethernet/mscc/ocelot_io.c b/drivers/net/ethernet/mscc/ocelot_io.c
index b229b1cb68ef..d22711282183 100644
--- a/drivers/net/ethernet/mscc/ocelot_io.c
+++ b/drivers/net/ethernet/mscc/ocelot_io.c
@@ -49,13 +49,25 @@ EXPORT_SYMBOL(__ocelot_rmw_ix);
 
 u32 ocelot_port_readl(struct ocelot_port *port, u32 reg)
 {
-	return readl(port->regs + reg);
+	struct ocelot *ocelot = port->ocelot;
+	u16 target = reg >> TARGET_OFFSET;
+	u32 val;
+
+	WARN_ON(!target);
+
+	regmap_read(port->target, ocelot->map[target][reg & REG_MASK], &val);
+	return val;
 }
 EXPORT_SYMBOL(ocelot_port_readl);
 
 void ocelot_port_writel(struct ocelot_port *port, u32 val, u32 reg)
 {
-	writel(val, port->regs + reg);
+	struct ocelot *ocelot = port->ocelot;
+	u16 target = reg >> TARGET_OFFSET;
+
+	WARN_ON(!target);
+
+	regmap_write(port->target, ocelot->map[target][reg & REG_MASK], val);
 }
 EXPORT_SYMBOL(ocelot_port_writel);
 
@@ -77,6 +89,8 @@ int ocelot_regfields_init(struct ocelot *ocelot,
 		regfield.reg = ocelot->map[target][reg & REG_MASK];
 		regfield.lsb = regfields[i].lsb;
 		regfield.msb = regfields[i].msb;
+		regfield.id_size = regfields[i].id_size;
+		regfield.id_offset = regfields[i].id_offset;
 
 		ocelot->regfields[i] =
 		devm_regmap_field_alloc(ocelot->dev,
diff --git a/drivers/net/ethernet/mscc/ocelot_net.c b/drivers/net/ethernet/mscc/ocelot_net.c
index 41a1b5f6df95..0668d23cdbfa 100644
--- a/drivers/net/ethernet/mscc/ocelot_net.c
+++ b/drivers/net/ethernet/mscc/ocelot_net.c
@@ -1005,8 +1005,7 @@ struct notifier_block ocelot_switchdev_blocking_nb __read_mostly = {
 	.notifier_call = ocelot_switchdev_blocking_event,
 };
 
-int ocelot_probe_port(struct ocelot *ocelot, u8 port,
-		      void __iomem *regs,
+int ocelot_probe_port(struct ocelot *ocelot, int port, struct regmap *target,
 		      struct phy_device *phy)
 {
 	struct ocelot_port_private *priv;
@@ -1024,7 +1023,7 @@ int ocelot_probe_port(struct ocelot *ocelot, u8 port,
 	priv->chip_port = port;
 	ocelot_port = &priv->port;
 	ocelot_port->ocelot = ocelot;
-	ocelot_port->regs = regs;
+	ocelot_port->target = target;
 	ocelot->ports[port] = ocelot_port;
 
 	dev->netdev_ops = &ocelot_port_netdev_ops;
diff --git a/drivers/net/ethernet/mscc/ocelot_vsc7514.c b/drivers/net/ethernet/mscc/ocelot_vsc7514.c
index 43716e8dc0ac..0ead1ef11c6c 100644
--- a/drivers/net/ethernet/mscc/ocelot_vsc7514.c
+++ b/drivers/net/ethernet/mscc/ocelot_vsc7514.c
@@ -263,7 +263,49 @@ static const u32 ocelot_ptp_regmap[] = {
 	REG(PTP_CLK_CFG_ADJ_FREQ,			0x0000a8),
 };
 
-static const u32 *ocelot_regmap[] = {
+static const u32 ocelot_dev_gmii_regmap[] = {
+	REG(DEV_CLOCK_CFG,				0x0),
+	REG(DEV_PORT_MISC,				0x4),
+	REG(DEV_EVENTS,					0x8),
+	REG(DEV_EEE_CFG,				0xc),
+	REG(DEV_RX_PATH_DELAY,				0x10),
+	REG(DEV_TX_PATH_DELAY,				0x14),
+	REG(DEV_PTP_PREDICT_CFG,			0x18),
+	REG(DEV_MAC_ENA_CFG,				0x1c),
+	REG(DEV_MAC_MODE_CFG,				0x20),
+	REG(DEV_MAC_MAXLEN_CFG,				0x24),
+	REG(DEV_MAC_TAGS_CFG,				0x28),
+	REG(DEV_MAC_ADV_CHK_CFG,			0x2c),
+	REG(DEV_MAC_IFG_CFG,				0x30),
+	REG(DEV_MAC_HDX_CFG,				0x34),
+	REG(DEV_MAC_DBG_CFG,				0x38),
+	REG(DEV_MAC_FC_MAC_LOW_CFG,			0x3c),
+	REG(DEV_MAC_FC_MAC_HIGH_CFG,			0x40),
+	REG(DEV_MAC_STICKY,				0x44),
+	REG(PCS1G_CFG,					0x48),
+	REG(PCS1G_MODE_CFG,				0x4c),
+	REG(PCS1G_SD_CFG,				0x50),
+	REG(PCS1G_ANEG_CFG,				0x54),
+	REG(PCS1G_ANEG_NP_CFG,				0x58),
+	REG(PCS1G_LB_CFG,				0x5c),
+	REG(PCS1G_DBG_CFG,				0x60),
+	REG(PCS1G_CDET_CFG,				0x64),
+	REG(PCS1G_ANEG_STATUS,				0x68),
+	REG(PCS1G_ANEG_NP_STATUS,			0x6c),
+	REG(PCS1G_LINK_STATUS,				0x70),
+	REG(PCS1G_LINK_DOWN_CNT,			0x74),
+	REG(PCS1G_STICKY,				0x78),
+	REG(PCS1G_DEBUG_STATUS,				0x7c),
+	REG(PCS1G_LPI_CFG,				0x80),
+	REG(PCS1G_LPI_WAKE_ERROR_CNT,			0x84),
+	REG(PCS1G_LPI_STATUS,				0x88),
+	REG(PCS1G_TSTPAT_MODE_CFG,			0x8c),
+	REG(PCS1G_TSTPAT_STATUS,			0x90),
+	REG(DEV_PCS_FX100_CFG,				0x94),
+	REG(DEV_PCS_FX100_STATUS,			0x98),
+};
+
+static const u32 *ocelot_regmap[TARGET_MAX] = {
 	[ANA] = ocelot_ana_regmap,
 	[QS] = ocelot_qs_regmap,
 	[QSYS] = ocelot_qsys_regmap,
@@ -271,9 +313,10 @@ static const u32 *ocelot_regmap[] = {
 	[SYS] = ocelot_sys_regmap,
 	[S2] = ocelot_s2_regmap,
 	[PTP] = ocelot_ptp_regmap,
+	[DEV_GMII] = ocelot_dev_gmii_regmap,
 };
 
-static const struct reg_field ocelot_regfields[] = {
+static const struct reg_field ocelot_regfields[REGFIELD_MAX] = {
 	[ANA_ADVLEARN_VLAN_CHK] = REG_FIELD(ANA_ADVLEARN, 11, 11),
 	[ANA_ADVLEARN_LEARN_MIRROR] = REG_FIELD(ANA_ADVLEARN, 0, 10),
 	[ANA_ANEVENTS_MSTI_DROP] = REG_FIELD(ANA_ANEVENTS, 27, 27),
@@ -315,6 +358,20 @@ static const struct reg_field ocelot_regfields[] = {
 	[SYS_RESET_CFG_CORE_ENA] = REG_FIELD(SYS_RESET_CFG, 2, 2),
 	[SYS_RESET_CFG_MEM_ENA] = REG_FIELD(SYS_RESET_CFG, 1, 1),
 	[SYS_RESET_CFG_MEM_INIT] = REG_FIELD(SYS_RESET_CFG, 0, 0),
+	/* Replicated per number of ports (11), register size 4 per port */
+	[QSYS_SWITCH_PORT_MODE_PORT_ENA] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 14, 14, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 11, 13, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_YEL_RSRVD] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 10, 10, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 9, 9, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_TX_PFC_ENA] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 1, 8, 11, 4),
+	[QSYS_SWITCH_PORT_MODE_TX_PFC_MODE] = REG_FIELD_ID(QSYS_SWITCH_PORT_MODE, 0, 0, 11, 4),
+	[SYS_PORT_MODE_DATA_WO_TS] = REG_FIELD_ID(SYS_PORT_MODE, 5, 6, 11, 4),
+	[SYS_PORT_MODE_INCL_INJ_HDR] = REG_FIELD_ID(SYS_PORT_MODE, 3, 4, 11, 4),
+	[SYS_PORT_MODE_INCL_XTR_HDR] = REG_FIELD_ID(SYS_PORT_MODE, 1, 2, 11, 4),
+	[SYS_PORT_MODE_INCL_HDR_ERR] = REG_FIELD_ID(SYS_PORT_MODE, 0, 0, 11, 4),
+	[SYS_PAUSE_CFG_PAUSE_START] = REG_FIELD_ID(SYS_PAUSE_CFG, 10, 18, 11, 4),
+	[SYS_PAUSE_CFG_PAUSE_STOP] = REG_FIELD_ID(SYS_PAUSE_CFG, 1, 9, 11, 4),
+	[SYS_PAUSE_CFG_PAUSE_ENA] = REG_FIELD_ID(SYS_PAUSE_CFG, 0, 1, 11, 4),
 };
 
 static const struct ocelot_stat_layout ocelot_stats_layout[] = {
@@ -682,8 +739,21 @@ static int ocelot_reset(struct ocelot *ocelot)
 	return 0;
 }
 
+/* Watermark encode
+ * Bit 8:   Unit; 0:1, 1:16
+ * Bit 7-0: Value to be multiplied with unit
+ */
+static u16 ocelot_wm_enc(u16 value)
+{
+	if (value >= BIT(8))
+		return BIT(8) | (value / 16);
+
+	return value;
+}
+
 static const struct ocelot_ops ocelot_ops = {
 	.reset			= ocelot_reset,
+	.wm_enc			= ocelot_wm_enc,
 };
 
 static const struct vcap_field vsc7514_vcap_is2_keys[] = {
@@ -948,9 +1018,9 @@ static int mscc_ocelot_probe(struct platform_device *pdev)
 		struct device_node *phy_node;
 		phy_interface_t phy_mode;
 		struct phy_device *phy;
+		struct regmap *target;
 		struct resource *res;
 		struct phy *serdes;
-		void __iomem *regs;
 		char res_name[8];
 		u32 port;
 
@@ -961,8 +1031,8 @@ static int mscc_ocelot_probe(struct platform_device *pdev)
 
 		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 						   res_name);
-		regs = devm_ioremap_resource(&pdev->dev, res);
-		if (IS_ERR(regs))
+		target = ocelot_regmap_init(ocelot, res);
+		if (IS_ERR(target))
 			continue;
 
 		phy_node = of_parse_phandle(portnp, "phy-handle", 0);
@@ -974,7 +1044,7 @@ static int mscc_ocelot_probe(struct platform_device *pdev)
 		if (!phy)
 			continue;
 
-		err = ocelot_probe_port(ocelot, port, regs, phy);
+		err = ocelot_probe_port(ocelot, port, target, phy);
 		if (err) {
 			of_node_put(portnp);
 			goto out_put_ports;
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index e050f8121ba2..da369b12005f 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -126,6 +126,7 @@ enum ocelot_target {
 	HSIO,
 	PTP,
 	GCB,
+	DEV_GMII,
 	TARGET_MAX,
 };
 
@@ -408,6 +409,48 @@ enum ocelot_reg {
 	PTP_CLK_CFG_ADJ_CFG,
 	PTP_CLK_CFG_ADJ_FREQ,
 	GCB_SOFT_RST = GCB << TARGET_OFFSET,
+	GCB_MIIM_MII_STATUS,
+	GCB_MIIM_MII_CMD,
+	GCB_MIIM_MII_DATA,
+	DEV_CLOCK_CFG = DEV_GMII << TARGET_OFFSET,
+	DEV_PORT_MISC,
+	DEV_EVENTS,
+	DEV_EEE_CFG,
+	DEV_RX_PATH_DELAY,
+	DEV_TX_PATH_DELAY,
+	DEV_PTP_PREDICT_CFG,
+	DEV_MAC_ENA_CFG,
+	DEV_MAC_MODE_CFG,
+	DEV_MAC_MAXLEN_CFG,
+	DEV_MAC_TAGS_CFG,
+	DEV_MAC_ADV_CHK_CFG,
+	DEV_MAC_IFG_CFG,
+	DEV_MAC_HDX_CFG,
+	DEV_MAC_DBG_CFG,
+	DEV_MAC_FC_MAC_LOW_CFG,
+	DEV_MAC_FC_MAC_HIGH_CFG,
+	DEV_MAC_STICKY,
+	PCS1G_CFG,
+	PCS1G_MODE_CFG,
+	PCS1G_SD_CFG,
+	PCS1G_ANEG_CFG,
+	PCS1G_ANEG_NP_CFG,
+	PCS1G_LB_CFG,
+	PCS1G_DBG_CFG,
+	PCS1G_CDET_CFG,
+	PCS1G_ANEG_STATUS,
+	PCS1G_ANEG_NP_STATUS,
+	PCS1G_LINK_STATUS,
+	PCS1G_LINK_DOWN_CNT,
+	PCS1G_STICKY,
+	PCS1G_DEBUG_STATUS,
+	PCS1G_LPI_CFG,
+	PCS1G_LPI_WAKE_ERROR_CNT,
+	PCS1G_LPI_STATUS,
+	PCS1G_TSTPAT_MODE_CFG,
+	PCS1G_TSTPAT_STATUS,
+	DEV_PCS_FX100_CFG,
+	DEV_PCS_FX100_STATUS,
 };
 
 enum ocelot_regfield {
@@ -447,15 +490,30 @@ enum ocelot_regfield {
 	ANA_TABLES_MACACCESS_B_DOM,
 	ANA_TABLES_MACTINDX_BUCKET,
 	ANA_TABLES_MACTINDX_M_INDEX,
+	QSYS_SWITCH_PORT_MODE_PORT_ENA,
+	QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG,
+	QSYS_SWITCH_PORT_MODE_YEL_RSRVD,
+	QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE,
+	QSYS_SWITCH_PORT_MODE_TX_PFC_ENA,
+	QSYS_SWITCH_PORT_MODE_TX_PFC_MODE,
 	QSYS_TIMED_FRAME_ENTRY_TFRM_VLD,
 	QSYS_TIMED_FRAME_ENTRY_TFRM_FP,
 	QSYS_TIMED_FRAME_ENTRY_TFRM_PORTNO,
 	QSYS_TIMED_FRAME_ENTRY_TFRM_TM_SEL,
 	QSYS_TIMED_FRAME_ENTRY_TFRM_TM_T,
+	SYS_PORT_MODE_DATA_WO_TS,
+	SYS_PORT_MODE_INCL_INJ_HDR,
+	SYS_PORT_MODE_INCL_XTR_HDR,
+	SYS_PORT_MODE_INCL_HDR_ERR,
 	SYS_RESET_CFG_CORE_ENA,
 	SYS_RESET_CFG_MEM_ENA,
 	SYS_RESET_CFG_MEM_INIT,
 	GCB_SOFT_RST_SWC_RST,
+	GCB_MIIM_MII_STATUS_PENDING,
+	GCB_MIIM_MII_STATUS_BUSY,
+	SYS_PAUSE_CFG_PAUSE_START,
+	SYS_PAUSE_CFG_PAUSE_STOP,
+	SYS_PAUSE_CFG_PAUSE_ENA,
 	REGFIELD_MAX
 };
 
@@ -483,6 +541,7 @@ struct ocelot;
 
 struct ocelot_ops {
 	int (*reset)(struct ocelot *ocelot);
+	u16 (*wm_enc)(u16 value);
 };
 
 struct ocelot_vcap_block {
@@ -494,7 +553,7 @@ struct ocelot_vcap_block {
 struct ocelot_port {
 	struct ocelot			*ocelot;
 
-	void __iomem			*regs;
+	struct regmap			*target;
 
 	bool				vlan_aware;
 
@@ -509,6 +568,8 @@ struct ocelot_port {
 	u8				ts_id;
 
 	phy_interface_t			phy_mode;
+
+	u8				*xmit_template;
 };
 
 struct ocelot {
@@ -593,6 +654,11 @@ struct ocelot_policer {
 #define ocelot_rmw_rix(ocelot, val, m, reg, ri) __ocelot_rmw_ix(ocelot, val, m, reg, reg##_RSZ * (ri))
 #define ocelot_rmw(ocelot, val, m, reg) __ocelot_rmw_ix(ocelot, val, m, reg, 0)
 
+#define ocelot_field_write(ocelot, reg, val) regmap_field_write((ocelot)->regfields[(reg)], (val))
+#define ocelot_field_read(ocelot, reg, val) regmap_field_read((ocelot)->regfields[(reg)], (val))
+#define ocelot_fields_write(ocelot, id, reg, val) regmap_fields_write((ocelot)->regfields[(reg)], (id), (val))
+#define ocelot_fields_read(ocelot, id, reg, val) regmap_fields_read((ocelot)->regfields[(reg)], (id), (val))
+
 /* I/O */
 u32 ocelot_port_readl(struct ocelot_port *port, u32 reg);
 void ocelot_port_writel(struct ocelot_port *port, u32 val, u32 reg);
diff --git a/include/soc/mscc/ocelot_dev.h b/include/soc/mscc/ocelot_dev.h
index 7c08437061fc..0c6021f02fee 100644
--- a/include/soc/mscc/ocelot_dev.h
+++ b/include/soc/mscc/ocelot_dev.h
@@ -8,8 +8,6 @@
 #ifndef _MSCC_OCELOT_DEV_H_
 #define _MSCC_OCELOT_DEV_H_
 
-#define DEV_CLOCK_CFG                                     0x0
-
 #define DEV_CLOCK_CFG_MAC_TX_RST                          BIT(7)
 #define DEV_CLOCK_CFG_MAC_RX_RST                          BIT(6)
 #define DEV_CLOCK_CFG_PCS_TX_RST                          BIT(5)
@@ -19,18 +17,12 @@
 #define DEV_CLOCK_CFG_LINK_SPEED(x)                       ((x) & GENMASK(1, 0))
 #define DEV_CLOCK_CFG_LINK_SPEED_M                        GENMASK(1, 0)
 
-#define DEV_PORT_MISC                                     0x4
-
 #define DEV_PORT_MISC_FWD_ERROR_ENA                       BIT(4)
 #define DEV_PORT_MISC_FWD_PAUSE_ENA                       BIT(3)
 #define DEV_PORT_MISC_FWD_CTRL_ENA                        BIT(2)
 #define DEV_PORT_MISC_DEV_LOOP_ENA                        BIT(1)
 #define DEV_PORT_MISC_HDX_FAST_DIS                        BIT(0)
 
-#define DEV_EVENTS                                        0x8
-
-#define DEV_EEE_CFG                                       0xc
-
 #define DEV_EEE_CFG_EEE_ENA                               BIT(22)
 #define DEV_EEE_CFG_EEE_TIMER_AGE(x)                      (((x) << 15) & GENMASK(21, 15))
 #define DEV_EEE_CFG_EEE_TIMER_AGE_M                       GENMASK(21, 15)
@@ -43,33 +35,19 @@
 #define DEV_EEE_CFG_EEE_TIMER_HOLDOFF_X(x)                (((x) & GENMASK(7, 1)) >> 1)
 #define DEV_EEE_CFG_PORT_LPI                              BIT(0)
 
-#define DEV_RX_PATH_DELAY                                 0x10
-
-#define DEV_TX_PATH_DELAY                                 0x14
-
-#define DEV_PTP_PREDICT_CFG                               0x18
-
 #define DEV_PTP_PREDICT_CFG_PTP_PHY_PREDICT_CFG(x)        (((x) << 4) & GENMASK(11, 4))
 #define DEV_PTP_PREDICT_CFG_PTP_PHY_PREDICT_CFG_M         GENMASK(11, 4)
 #define DEV_PTP_PREDICT_CFG_PTP_PHY_PREDICT_CFG_X(x)      (((x) & GENMASK(11, 4)) >> 4)
 #define DEV_PTP_PREDICT_CFG_PTP_PHASE_PREDICT_CFG(x)      ((x) & GENMASK(3, 0))
 #define DEV_PTP_PREDICT_CFG_PTP_PHASE_PREDICT_CFG_M       GENMASK(3, 0)
 
-#define DEV_MAC_ENA_CFG                                   0x1c
-
 #define DEV_MAC_ENA_CFG_RX_ENA                            BIT(4)
 #define DEV_MAC_ENA_CFG_TX_ENA                            BIT(0)
 
-#define DEV_MAC_MODE_CFG                                  0x20
-
 #define DEV_MAC_MODE_CFG_FC_WORD_SYNC_ENA                 BIT(8)
 #define DEV_MAC_MODE_CFG_GIGA_MODE_ENA                    BIT(4)
 #define DEV_MAC_MODE_CFG_FDX_ENA                          BIT(0)
 
-#define DEV_MAC_MAXLEN_CFG                                0x24
-
-#define DEV_MAC_TAGS_CFG                                  0x28
-
 #define DEV_MAC_TAGS_CFG_TAG_ID(x)                        (((x) << 16) & GENMASK(31, 16))
 #define DEV_MAC_TAGS_CFG_TAG_ID_M                         GENMASK(31, 16)
 #define DEV_MAC_TAGS_CFG_TAG_ID_X(x)                      (((x) & GENMASK(31, 16)) >> 16)
@@ -77,12 +55,8 @@
 #define DEV_MAC_TAGS_CFG_VLAN_DBL_AWR_ENA                 BIT(1)
 #define DEV_MAC_TAGS_CFG_VLAN_AWR_ENA                     BIT(0)
 
-#define DEV_MAC_ADV_CHK_CFG                               0x2c
-
 #define DEV_MAC_ADV_CHK_CFG_LEN_DROP_ENA                  BIT(0)
 
-#define DEV_MAC_IFG_CFG                                   0x30
-
 #define DEV_MAC_IFG_CFG_RESTORE_OLD_IPG_CHECK             BIT(17)
 #define DEV_MAC_IFG_CFG_REDUCED_TX_IFG                    BIT(16)
 #define DEV_MAC_IFG_CFG_TX_IFG(x)                         (((x) << 8) & GENMASK(12, 8))
@@ -94,8 +68,6 @@
 #define DEV_MAC_IFG_CFG_RX_IFG1(x)                        ((x) & GENMASK(3, 0))
 #define DEV_MAC_IFG_CFG_RX_IFG1_M                         GENMASK(3, 0)
 
-#define DEV_MAC_HDX_CFG                                   0x34
-
 #define DEV_MAC_HDX_CFG_BYPASS_COL_SYNC                   BIT(26)
 #define DEV_MAC_HDX_CFG_OB_ENA                            BIT(25)
 #define DEV_MAC_HDX_CFG_WEXC_DIS                          BIT(24)
@@ -107,17 +79,9 @@
 #define DEV_MAC_HDX_CFG_LATE_COL_POS(x)                   ((x) & GENMASK(6, 0))
 #define DEV_MAC_HDX_CFG_LATE_COL_POS_M                    GENMASK(6, 0)
 
-#define DEV_MAC_DBG_CFG                                   0x38
-
 #define DEV_MAC_DBG_CFG_TBI_MODE                          BIT(4)
 #define DEV_MAC_DBG_CFG_IFG_CRS_EXT_CHK_ENA               BIT(0)
 
-#define DEV_MAC_FC_MAC_LOW_CFG                            0x3c
-
-#define DEV_MAC_FC_MAC_HIGH_CFG                           0x40
-
-#define DEV_MAC_STICKY                                    0x44
-
 #define DEV_MAC_STICKY_RX_IPG_SHRINK_STICKY               BIT(9)
 #define DEV_MAC_STICKY_RX_PREAM_SHRINK_STICKY             BIT(8)
 #define DEV_MAC_STICKY_RX_CARRIER_EXT_STICKY              BIT(7)
@@ -129,25 +93,17 @@
 #define DEV_MAC_STICKY_TX_FRM_LEN_OVR_STICKY              BIT(1)
 #define DEV_MAC_STICKY_TX_ABORT_STICKY                    BIT(0)
 
-#define PCS1G_CFG                                         0x48
-
 #define PCS1G_CFG_LINK_STATUS_TYPE                        BIT(4)
 #define PCS1G_CFG_AN_LINK_CTRL_ENA                        BIT(1)
 #define PCS1G_CFG_PCS_ENA                                 BIT(0)
 
-#define PCS1G_MODE_CFG                                    0x4c
-
 #define PCS1G_MODE_CFG_UNIDIR_MODE_ENA                    BIT(4)
 #define PCS1G_MODE_CFG_SGMII_MODE_ENA                     BIT(0)
 
-#define PCS1G_SD_CFG                                      0x50
-
 #define PCS1G_SD_CFG_SD_SEL                               BIT(8)
 #define PCS1G_SD_CFG_SD_POL                               BIT(4)
 #define PCS1G_SD_CFG_SD_ENA                               BIT(0)
 
-#define PCS1G_ANEG_CFG                                    0x54
-
 #define PCS1G_ANEG_CFG_ADV_ABILITY(x)                     (((x) << 16) & GENMASK(31, 16))
 #define PCS1G_ANEG_CFG_ADV_ABILITY_M                      GENMASK(31, 16)
 #define PCS1G_ANEG_CFG_ADV_ABILITY_X(x)                   (((x) & GENMASK(31, 16)) >> 16)
@@ -155,29 +111,19 @@
 #define PCS1G_ANEG_CFG_ANEG_RESTART_ONE_SHOT              BIT(1)
 #define PCS1G_ANEG_CFG_ANEG_ENA                           BIT(0)
 
-#define PCS1G_ANEG_NP_CFG                                 0x58
-
 #define PCS1G_ANEG_NP_CFG_NP_TX(x)                        (((x) << 16) & GENMASK(31, 16))
 #define PCS1G_ANEG_NP_CFG_NP_TX_M                         GENMASK(31, 16)
 #define PCS1G_ANEG_NP_CFG_NP_TX_X(x)                      (((x) & GENMASK(31, 16)) >> 16)
 #define PCS1G_ANEG_NP_CFG_NP_LOADED_ONE_SHOT              BIT(0)
 
-#define PCS1G_LB_CFG                                      0x5c
-
 #define PCS1G_LB_CFG_RA_ENA                               BIT(4)
 #define PCS1G_LB_CFG_GMII_PHY_LB_ENA                      BIT(1)
 #define PCS1G_LB_CFG_TBI_HOST_LB_ENA                      BIT(0)
 
-#define PCS1G_DBG_CFG                                     0x60
-
 #define PCS1G_DBG_CFG_UDLT                                BIT(0)
 
-#define PCS1G_CDET_CFG                                    0x64
-
 #define PCS1G_CDET_CFG_CDET_ENA                           BIT(0)
 
-#define PCS1G_ANEG_STATUS                                 0x68
-
 #define PCS1G_ANEG_STATUS_LP_ADV_ABILITY(x)               (((x) << 16) & GENMASK(31, 16))
 #define PCS1G_ANEG_STATUS_LP_ADV_ABILITY_M                GENMASK(31, 16)
 #define PCS1G_ANEG_STATUS_LP_ADV_ABILITY_X(x)             (((x) & GENMASK(31, 16)) >> 16)
@@ -185,10 +131,6 @@
 #define PCS1G_ANEG_STATUS_PAGE_RX_STICKY                  BIT(3)
 #define PCS1G_ANEG_STATUS_ANEG_COMPLETE                   BIT(0)
 
-#define PCS1G_ANEG_NP_STATUS                              0x6c
-
-#define PCS1G_LINK_STATUS                                 0x70
-
 #define PCS1G_LINK_STATUS_DELAY_VAR(x)                    (((x) << 12) & GENMASK(15, 12))
 #define PCS1G_LINK_STATUS_DELAY_VAR_M                     GENMASK(15, 12)
 #define PCS1G_LINK_STATUS_DELAY_VAR_X(x)                  (((x) & GENMASK(15, 12)) >> 12)
@@ -196,17 +138,9 @@
 #define PCS1G_LINK_STATUS_LINK_STATUS                     BIT(4)
 #define PCS1G_LINK_STATUS_SYNC_STATUS                     BIT(0)
 
-#define PCS1G_LINK_DOWN_CNT                               0x74
-
-#define PCS1G_STICKY                                      0x78
-
 #define PCS1G_STICKY_LINK_DOWN_STICKY                     BIT(4)
 #define PCS1G_STICKY_OUT_OF_SYNC_STICKY                   BIT(0)
 
-#define PCS1G_DEBUG_STATUS                                0x7c
-
-#define PCS1G_LPI_CFG                                     0x80
-
 #define PCS1G_LPI_CFG_QSGMII_MS_SEL                       BIT(20)
 #define PCS1G_LPI_CFG_RX_LPI_OUT_DIS                      BIT(17)
 #define PCS1G_LPI_CFG_LPI_TESTMODE                        BIT(16)
@@ -215,10 +149,6 @@
 #define PCS1G_LPI_CFG_LPI_RX_WTIM_X(x)                    (((x) & GENMASK(5, 4)) >> 4)
 #define PCS1G_LPI_CFG_TX_ASSERT_LPIDLE                    BIT(0)
 
-#define PCS1G_LPI_WAKE_ERROR_CNT                          0x84
-
-#define PCS1G_LPI_STATUS                                  0x88
-
 #define PCS1G_LPI_STATUS_RX_LPI_FAIL                      BIT(16)
 #define PCS1G_LPI_STATUS_RX_LPI_EVENT_STICKY              BIT(12)
 #define PCS1G_LPI_STATUS_RX_QUIET                         BIT(9)
@@ -227,18 +157,12 @@
 #define PCS1G_LPI_STATUS_TX_QUIET                         BIT(1)
 #define PCS1G_LPI_STATUS_TX_LPI_MODE                      BIT(0)
 
-#define PCS1G_TSTPAT_MODE_CFG                             0x8c
-
-#define PCS1G_TSTPAT_STATUS                               0x90
-
 #define PCS1G_TSTPAT_STATUS_JTP_ERR_CNT(x)                (((x) << 8) & GENMASK(15, 8))
 #define PCS1G_TSTPAT_STATUS_JTP_ERR_CNT_M                 GENMASK(15, 8)
 #define PCS1G_TSTPAT_STATUS_JTP_ERR_CNT_X(x)              (((x) & GENMASK(15, 8)) >> 8)
 #define PCS1G_TSTPAT_STATUS_JTP_ERR                       BIT(4)
 #define PCS1G_TSTPAT_STATUS_JTP_LOCK                      BIT(0)
 
-#define DEV_PCS_FX100_CFG                                 0x94
-
 #define DEV_PCS_FX100_CFG_SD_SEL                          BIT(26)
 #define DEV_PCS_FX100_CFG_SD_POL                          BIT(25)
 #define DEV_PCS_FX100_CFG_SD_ENA                          BIT(24)
@@ -259,8 +183,6 @@
 #define DEV_PCS_FX100_CFG_FEFGEN_ENA                      BIT(1)
 #define DEV_PCS_FX100_CFG_PCS_ENA                         BIT(0)
 
-#define DEV_PCS_FX100_STATUS                              0x98
-
 #define DEV_PCS_FX100_STATUS_EDGE_POS_PTP(x)              (((x) << 8) & GENMASK(11, 8))
 #define DEV_PCS_FX100_STATUS_EDGE_POS_PTP_M               GENMASK(11, 8)
 #define DEV_PCS_FX100_STATUS_EDGE_POS_PTP_X(x)            (((x) & GENMASK(11, 8)) >> 8)
diff --git a/include/soc/mscc/ocelot_qsys.h b/include/soc/mscc/ocelot_qsys.h
index d8c63aa761be..a814bc2017d8 100644
--- a/include/soc/mscc/ocelot_qsys.h
+++ b/include/soc/mscc/ocelot_qsys.h
@@ -13,19 +13,6 @@
 #define QSYS_PORT_MODE_DEQUEUE_DIS                        BIT(1)
 #define QSYS_PORT_MODE_DEQUEUE_LATE                       BIT(0)
 
-#define QSYS_SWITCH_PORT_MODE_RSZ                         0x4
-
-#define QSYS_SWITCH_PORT_MODE_PORT_ENA                    BIT(14)
-#define QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG(x)             (((x) << 11) & GENMASK(13, 11))
-#define QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG_M              GENMASK(13, 11)
-#define QSYS_SWITCH_PORT_MODE_SCH_NEXT_CFG_X(x)           (((x) & GENMASK(13, 11)) >> 11)
-#define QSYS_SWITCH_PORT_MODE_YEL_RSRVD                   BIT(10)
-#define QSYS_SWITCH_PORT_MODE_INGRESS_DROP_MODE           BIT(9)
-#define QSYS_SWITCH_PORT_MODE_TX_PFC_ENA(x)               (((x) << 1) & GENMASK(8, 1))
-#define QSYS_SWITCH_PORT_MODE_TX_PFC_ENA_M                GENMASK(8, 1)
-#define QSYS_SWITCH_PORT_MODE_TX_PFC_ENA_X(x)             (((x) & GENMASK(8, 1)) >> 1)
-#define QSYS_SWITCH_PORT_MODE_TX_PFC_MODE                 BIT(0)
-
 #define QSYS_STAT_CNT_CFG_TX_GREEN_CNT_MODE               BIT(5)
 #define QSYS_STAT_CNT_CFG_TX_YELLOW_CNT_MODE              BIT(4)
 #define QSYS_STAT_CNT_CFG_DROP_GREEN_CNT_MODE             BIT(3)
diff --git a/include/soc/mscc/ocelot_sys.h b/include/soc/mscc/ocelot_sys.h
index 16f91e172bcb..79cf40ccdbe6 100644
--- a/include/soc/mscc/ocelot_sys.h
+++ b/include/soc/mscc/ocelot_sys.h
@@ -12,19 +12,6 @@
 
 #define SYS_COUNT_TX_OCTETS_RSZ                           0x4
 
-#define SYS_PORT_MODE_RSZ                                 0x4
-
-#define SYS_PORT_MODE_DATA_WO_TS(x)                       (((x) << 5) & GENMASK(6, 5))
-#define SYS_PORT_MODE_DATA_WO_TS_M                        GENMASK(6, 5)
-#define SYS_PORT_MODE_DATA_WO_TS_X(x)                     (((x) & GENMASK(6, 5)) >> 5)
-#define SYS_PORT_MODE_INCL_INJ_HDR(x)                     (((x) << 3) & GENMASK(4, 3))
-#define SYS_PORT_MODE_INCL_INJ_HDR_M                      GENMASK(4, 3)
-#define SYS_PORT_MODE_INCL_INJ_HDR_X(x)                   (((x) & GENMASK(4, 3)) >> 3)
-#define SYS_PORT_MODE_INCL_XTR_HDR(x)                     (((x) << 1) & GENMASK(2, 1))
-#define SYS_PORT_MODE_INCL_XTR_HDR_M                      GENMASK(2, 1)
-#define SYS_PORT_MODE_INCL_XTR_HDR_X(x)                   (((x) & GENMASK(2, 1)) >> 1)
-#define SYS_PORT_MODE_INJ_HDR_ERR                         BIT(0)
-
 #define SYS_FRONT_PORT_MODE_RSZ                           0x4
 
 #define SYS_FRONT_PORT_MODE_HDX_MODE                      BIT(0)
@@ -56,16 +43,6 @@
 #define SYS_TIMESTAMP_OFFSET_TIMESTAMP_OFFSET(x)          ((x) & GENMASK(5, 0))
 #define SYS_TIMESTAMP_OFFSET_TIMESTAMP_OFFSET_M           GENMASK(5, 0)
 
-#define SYS_PAUSE_CFG_RSZ                                 0x4
-
-#define SYS_PAUSE_CFG_PAUSE_START(x)                      (((x) << 10) & GENMASK(18, 10))
-#define SYS_PAUSE_CFG_PAUSE_START_M                       GENMASK(18, 10)
-#define SYS_PAUSE_CFG_PAUSE_START_X(x)                    (((x) & GENMASK(18, 10)) >> 10)
-#define SYS_PAUSE_CFG_PAUSE_STOP(x)                       (((x) << 1) & GENMASK(9, 1))
-#define SYS_PAUSE_CFG_PAUSE_STOP_M                        GENMASK(9, 1)
-#define SYS_PAUSE_CFG_PAUSE_STOP_X(x)                     (((x) & GENMASK(9, 1)) >> 1)
-#define SYS_PAUSE_CFG_PAUSE_ENA                           BIT(0)
-
 #define SYS_PAUSE_TOT_CFG_PAUSE_TOT_START(x)              (((x) << 9) & GENMASK(17, 9))
 #define SYS_PAUSE_TOT_CFG_PAUSE_TOT_START_M               GENMASK(17, 9)
 #define SYS_PAUSE_TOT_CFG_PAUSE_TOT_START_X(x)            (((x) & GENMASK(17, 9)) >> 9)
diff --git a/net/dsa/tag_ocelot.c b/net/dsa/tag_ocelot.c
index b0c98ee4e13b..42f327c06dca 100644
--- a/net/dsa/tag_ocelot.c
+++ b/net/dsa/tag_ocelot.c
@@ -137,11 +137,10 @@ static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
 				   struct net_device *netdev)
 {
 	struct dsa_port *dp = dsa_slave_to_port(netdev);
-	u64 bypass, dest, src, qos_class, rew_op;
 	struct dsa_switch *ds = dp->ds;
-	int port = dp->index;
 	struct ocelot *ocelot = ds->priv;
-	struct ocelot_port *ocelot_port = ocelot->ports[port];
+	struct ocelot_port *ocelot_port;
+	u64 qos_class, rew_op;
 	u8 *injection;
 
 	if (unlikely(skb_cow_head(skb, OCELOT_TAG_LEN) < 0)) {
@@ -149,19 +148,15 @@ static struct sk_buff *ocelot_xmit(struct sk_buff *skb,
 		return NULL;
 	}
 
-	injection = skb_push(skb, OCELOT_TAG_LEN);
+	ocelot_port = ocelot->ports[dp->index];
 
-	memset(injection, 0, OCELOT_TAG_LEN);
+	injection = skb_push(skb, OCELOT_TAG_LEN);
 
-	/* Set the source port as the CPU port module and not the NPI port */
-	src = ocelot->num_phys_ports;
-	dest = BIT(port);
-	bypass = true;
+	memcpy(injection, ocelot_port->xmit_template, OCELOT_TAG_LEN);
+	/* Fix up the fields which are not statically determined
+	 * in the template
+	 */
 	qos_class = skb->priority;
-
-	packing(injection, &bypass,   127, 127, OCELOT_TAG_LEN, PACK, 0);
-	packing(injection, &dest,      68,  56, OCELOT_TAG_LEN, PACK, 0);
-	packing(injection, &src,       46,  43, OCELOT_TAG_LEN, PACK, 0);
 	packing(injection, &qos_class, 19,  17, OCELOT_TAG_LEN, PACK, 0);
 
 	if (ocelot->ptp && (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {