9 files changed, 689 insertions, 287 deletions

diff --git a/drivers/net/ethernet/marvell/Kconfig b/drivers/net/ethernet/marvell/Kconfig
index 41815b609569..32675de95ed2 100644
--- a/drivers/net/ethernet/marvell/Kconfig
+++ b/drivers/net/ethernet/marvell/Kconfig
@@ -59,6 +59,7 @@ config MVNETA_BM_ENABLE
 config MVNETA
 	tristate "Marvell Armada 370/38x/XP/37xx network interface support"
 	depends on ARCH_MVEBU || COMPILE_TEST
+	select MVGMAC
 	select MVMDIO
 	select PHYLINK
 	select PAGE_POOL
@@ -71,6 +72,9 @@ config MVNETA
 	  driver, which should be used for the older Marvell SoCs
 	  (Dove, Orion, Discovery, Kirkwood).
 
+config MVGMAC
+	tristate
+
 config MVNETA_BM
 	tristate
 	depends on !64BIT
diff --git a/drivers/net/ethernet/marvell/Makefile b/drivers/net/ethernet/marvell/Makefile
index 9f88fe822555..b1442ff46abf 100644
--- a/drivers/net/ethernet/marvell/Makefile
+++ b/drivers/net/ethernet/marvell/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_MVMDIO) += mvmdio.o
 obj-$(CONFIG_MV643XX_ETH) += mv643xx_eth.o
 obj-$(CONFIG_MVNETA_BM) += mvneta_bm.o
 obj-$(CONFIG_MVNETA) += mvneta.o
+obj-$(CONFIG_MVGMAC) += mvgmac.o
 obj-$(CONFIG_MVPP2) += mvpp2/
 obj-$(CONFIG_PXA168_ETH) += pxa168_eth.o
 obj-$(CONFIG_SKGE) += skge.o
diff --git a/drivers/net/ethernet/marvell/mvgmac.c b/drivers/net/ethernet/marvell/mvgmac.c
new file mode 100644
index 000000000000..e776d4e85d3f
--- /dev/null
+++ b/drivers/net/ethernet/marvell/mvgmac.c
@@ -0,0 +1,444 @@
+/*
+ * GMAC driver for Marvell network interfaces on Armada SoCs.
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Rami Rosen <rosenr@marvell.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * Split from mvneta and mvpp2 by Russell King.
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+#include <linux/export.h>
+#include <linux/io.h>
+#include <linux/phylink.h>
+
+#include "mvgmac.h"
+
+enum {
+	/* N = Neta, 21 = PPV2.1, 22 = PPV2.2 */
+	/* N: 0-14	21: 0,2-15	22: 0-14 */
+	GMAC_CTRL0_REG		= 0x00,
+	GMAC_CTRL0_PORT_ENABLE		= BIT(0),
+	GMAC_CTRL0_PORT_1000BASE_X	= BIT(1),
+	GMAC_CTRL0_MAX_RX_SIZE_SHIFT	= 2,
+	GMAC_CTRL0_MAX_RX_SIZE_MASK	= 0x1fff << GMAC_CTRL0_MAX_RX_SIZE_SHIFT,
+	GMAC_CTRL0_MIB_CNTR_ENABLE	= BIT(15),
+
+	/* N:		21: 1,5,6	22:	*/
+	GMAC_CTRL1_REG		= 0x04,
+	GMAC_CTRL1_PERIODIC_XON_ENABLE	= BIT(1),
+	GMAC_CTRL1_GMII_LB_ENABLE	= BIT(5),
+	GMAC_CTRL1_PCS_LB_ENABLE	= BIT(6),
+
+	/* ALL: 0,3,4,6 */
+	GMAC_CTRL2_REG		= 0x08,
+	GMAC_CTRL2_INBAND_AN_SGMII	= BIT(0),
+	GMAC_CTRL2_PCS_ENABLE		= BIT(3),
+	GMAC_CTRL2_PORT_RGMII		= BIT(4),
+	GMAC_CTRL2_PORT_RESET		= BIT(6),
+
+	/* N:0-9,11-13	21:0,1,5-7,9,12,13	22:0-7,9-15 */
+	/* 22 bit 2 - EN_PCS_AN */
+	GMAC_ANEG_REG		= 0x0c,
+	GMAC_ANEG_FORCE_LINK_DOWN	= BIT(0),
+	GMAC_ANEG_FORCE_LINK_PASS	= BIT(1),
+	GMAC_ANEG_INBAND_AN_ENABLE	= BIT(2),
+	GMAC_ANEG_AN_BYPASS_ENABLE	= BIT(3),
+	GMAC_ANEG_INBAND_RESTART_AN	= BIT(4),
+	GMAC_ANEG_MII_SPEED		= BIT(5),
+	GMAC_ANEG_GMII_SPEED		= BIT(6),
+	GMAC_ANEG_AN_SPEED_ENABLE	= BIT(7),
+	GMAC_ANEG_CONFIG_FLOW_CTRL	= BIT(8),
+	GMAC_ANEG_ADVERT_SYM_FLOW_CTRL	= BIT(9),
+	GMAC_ANEG_ADVERT_ASYM_FLOW_CTRL	= BIT(10),
+	GMAC_ANEG_AN_FLOW_CTRL_ENABLE	= BIT(11),
+	GMAC_ANEG_FULL_DUPLEX		= BIT(12),
+	GMAC_ANEG_AN_DUPLEX_ENABLE	= BIT(13),
+				/* pp22: bit 14 - phy mode */
+				/* pp22: bit 15 - choose sample tx config */
+
+	GMAC_STATUS_REG		= 0x10,
+	MVGMAC_LINK_UP			= BIT(0),
+	MVGMAC_SPEED_1000		= BIT(1),
+	MVGMAC_SPEED_100		= BIT(2),
+	MVGMAC_FULL_DUPLEX		= BIT(3),
+	MVGMAC_RX_FLOW_CTRL_ENABLE	= BIT(4),
+	MVGMAC_TX_FLOW_CTRL_ENABLE	= BIT(5),
+	MVGMAC_RX_FLOW_CTRL_ACTIVE	= BIT(6),
+	MVGMAC_TX_FLOW_CTRL_ACTIVE	= BIT(7),
+	MVGMAC_AN_COMPLETE		= BIT(11),
+	MVGMAC_SYNC_OK			= BIT(14),
+
+	/* N:		21:6-13		22: */
+	GMAC_FIFO_CFG1_REG	= 0x1c,
+	GMAC_FIFO_CFG1_TX_MIN_TH_SHIFT	= 6,
+	GMAC_FIFO_CFG1_TX_MIN_TH_MASK	= 0x7f <<
+						GMAC_FIFO_CFG1_TX_MIN_TH_SHIFT,
+
+	/* N:1		21:		22:0,3-7 */
+	GMAC_CTRL4_REG		= 0x90,
+	GMAC_CTRL4_EXT_PIN_GMII_SEL	= BIT(0),
+	GMAC_CTRL4_SHORT_PREAMBLE_ENABLE = BIT(1),
+	GMAC_CTRL4_FC_RX_ENABLE		= BIT(3),
+	GMAC_CTRL4_FC_TX_ENABLE		= BIT(4),
+	GMAC_CTRL4_DP_CLK_SEL		= BIT(5),
+	GMAC_CTRL4_SYNC_BYPASS		= BIT(6),
+	GMAC_CTRL4_QSGMII_BYPASS	= BIT(7),
+
+	GMAC_LPI_CTRL0_REG	= 0xc0,
+	GMAC_LPI_CTRL0_TS		= 0xff << 8,
+	GMAC_LPI_CTRL1_REG	= 0xc4,
+	GMAC_LPI_CTRL1_REQ_EN		= BIT(0),
+	GMAC_LPI_CTRL2_REG	= 0xc8,
+	GMAC_LPI_STATUS_REG	= 0xcc,
+	GMAC_LPI_CNTR_REG	= 0xd0,
+};
+
+#define insert(var, mask, val) ({					\
+	u32 __mask = mask;						\
+	((var) & ~(__mask)) | (((val) << __ffs(__mask)) & (__mask));	\
+})
+
+/* Change maximum receive size of the port. */
+void mvgmac_set_max_rx_size(struct mvgmac *gmac, size_t max_rx_size)
+{
+	int size = (max_rx_size - MARVELL_HEADER_SIZE) / 2;
+	u32 val;
+
+	val = readl_relaxed(gmac->base + GMAC_CTRL0_REG);
+	val = insert(val, GMAC_CTRL0_MAX_RX_SIZE_MASK, size);
+	writel_relaxed(val, gmac->base + GMAC_CTRL0_REG);
+}
+EXPORT_SYMBOL_GPL(mvgmac_set_max_rx_size);
+
+/* Enable the port by setting the port enable bit of the MAC control register */
+void mvgmac_enable(struct mvgmac *gmac)
+{
+	u32 val;
+
+	val = readl_relaxed(gmac->base + GMAC_CTRL0_REG);
+	val |= GMAC_CTRL0_PORT_ENABLE;
+	val |= GMAC_CTRL0_MIB_CNTR_ENABLE;
+	writel_relaxed(val, gmac->base + GMAC_CTRL0_REG);
+}
+EXPORT_SYMBOL_GPL(mvgmac_enable);
+
+/* Disable the port */
+void mvgmac_disable(struct mvgmac *gmac)
+{
+	u32 val;
+
+	val = readl_relaxed(gmac->base + GMAC_CTRL0_REG);
+	val &= ~GMAC_CTRL0_PORT_ENABLE;
+	writel_relaxed(val, gmac->base + GMAC_CTRL0_REG);
+}
+EXPORT_SYMBOL_GPL(mvgmac_disable);
+
+int mvgmac_configure(struct mvgmac *gmac, phy_interface_t phy_mode)
+{
+	bool ext_pin_gmii;
+	u32 val;
+
+	switch (phy_mode) {
+	case PHY_INTERFACE_MODE_QSGMII:
+	case PHY_INTERFACE_MODE_SGMII:
+		ext_pin_gmii = false;
+		break;
+
+	case PHY_INTERFACE_MODE_RGMII:
+	case PHY_INTERFACE_MODE_RGMII_ID:
+	case PHY_INTERFACE_MODE_RGMII_RXID:
+	case PHY_INTERFACE_MODE_RGMII_TXID:
+		ext_pin_gmii = true;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if (gmac->version == MVGMAC_PP21) {
+		/* Min. TX threshold must be less than minimum packet length */
+		val = readl_relaxed(gmac->base + GMAC_FIFO_CFG1_REG);
+		val = insert(val, GMAC_FIFO_CFG1_TX_MIN_TH_MASK, 64 - 4 - 2);
+		writel_relaxed(val, gmac->base + GMAC_FIFO_CFG1_REG);
+	} else if (gmac->version == MVGMAC_PP22) {
+		val = readl_relaxed(gmac->base + GMAC_CTRL4_REG);
+		val &= ~GMAC_CTRL4_DP_CLK_SEL;
+		val |= GMAC_CTRL4_SYNC_BYPASS;
+		val = insert(val, GMAC_CTRL4_QSGMII_BYPASS,
+			     phy_mode != PHY_INTERFACE_MODE_QSGMII);
+		val = insert(val, GMAC_CTRL4_EXT_PIN_GMII_SEL, ext_pin_gmii);
+		writel_relaxed(val, gmac->base + GMAC_CTRL4_REG);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mvgmac_configure);
+
+void mvgmac_link_unforce(struct mvgmac *gmac)
+{
+	u32 val;
+
+	val = readl_relaxed(gmac->base + GMAC_ANEG_REG);
+	val &= ~(GMAC_ANEG_FORCE_LINK_PASS | GMAC_ANEG_FORCE_LINK_DOWN);
+	writel_relaxed(val, gmac->base + GMAC_ANEG_REG);
+}
+EXPORT_SYMBOL_GPL(mvgmac_link_unforce);
+
+void mvgmac_link_force_down(struct mvgmac *gmac)
+{
+	u32 val;
+
+	val = readl_relaxed(gmac->base + GMAC_ANEG_REG);
+	val &= ~GMAC_ANEG_FORCE_LINK_PASS;
+	val |= GMAC_ANEG_FORCE_LINK_DOWN;
+	writel_relaxed(val, gmac->base + GMAC_ANEG_REG);
+}
+EXPORT_SYMBOL_GPL(mvgmac_link_force_down);
+
+void mvgmac_link_down(struct mvgmac *gmac, int mode)
+{
+	if (!phylink_autoneg_inband(mode))
+		mvgmac_link_force_down(gmac);
+}
+EXPORT_SYMBOL_GPL(mvgmac_link_down);
+
+void mvgmac_link_up(struct mvgmac *gmac, int mode, int speed, int duplex,
+		    bool tx_pause, bool rx_pause)
+{
+	u32 val, ctrl4;
+
+	val = readl_relaxed(gmac->base + GMAC_ANEG_REG);
+	val &= ~GMAC_ANEG_CONFIG_FLOW_CTRL;
+
+	if (!phylink_autoneg_inband(mode)) {
+		val &= ~(GMAC_ANEG_FORCE_LINK_DOWN |
+			 GMAC_ANEG_MII_SPEED |
+			 GMAC_ANEG_GMII_SPEED |
+			 GMAC_ANEG_FULL_DUPLEX);
+		val |= GMAC_ANEG_FORCE_LINK_PASS;
+
+		if (speed == SPEED_1000 || speed == SPEED_2500)
+			val |= GMAC_ANEG_GMII_SPEED;
+		else if (speed == SPEED_100)
+			val |= GMAC_ANEG_MII_SPEED;
+
+		if (duplex == DUPLEX_FULL)
+			val |= GMAC_ANEG_FULL_DUPLEX;
+	}
+
+	switch (gmac->version) {
+	case MVGMAC_NETA:
+		val = insert(val, GMAC_ANEG_CONFIG_FLOW_CTRL,
+			     tx_pause || rx_pause);
+		break;
+
+	case MVGMAC_PP22:
+		ctrl4 = readl_relaxed(gmac->base + GMAC_CTRL4_REG);
+		ctrl4 = insert(ctrl4, GMAC_CTRL4_FC_TX_ENABLE, tx_pause);
+		ctrl4 = insert(ctrl4, GMAC_CTRL4_FC_RX_ENABLE, rx_pause);
+		writel_relaxed(ctrl4, gmac->base + GMAC_CTRL4_REG);
+		break;
+	}
+
+	writel_relaxed(val, gmac->base + GMAC_ANEG_REG);
+}
+EXPORT_SYMBOL_GPL(mvgmac_link_up);
+
+bool mvgmac_link_is_up(struct mvgmac *gmac)
+{
+	u32 gmac_stat = readl_relaxed(gmac->base + GMAC_STATUS_REG);
+
+	return !!(gmac_stat & MVGMAC_LINK_UP);
+}
+EXPORT_SYMBOL_GPL(mvgmac_link_is_up);
+
+void mvgmac_pcs_get_state(struct mvgmac *gmac, struct phylink_link_state *state)
+{
+	u32 gmac_stat = readl_relaxed(gmac->base + GMAC_STATUS_REG);
+
+	if (gmac_stat & MVGMAC_SPEED_1000)
+		state->speed =
+			state->interface == PHY_INTERFACE_MODE_2500BASEX ?
+			SPEED_2500 : SPEED_1000;
+	else if (gmac_stat & MVGMAC_SPEED_100)
+		state->speed = SPEED_100;
+	else
+		state->speed = SPEED_10;
+
+	state->an_complete = !!(gmac_stat & MVGMAC_AN_COMPLETE);
+	state->link = !!(gmac_stat & MVGMAC_LINK_UP);
+	state->duplex = !!(gmac_stat & MVGMAC_FULL_DUPLEX);
+
+	if (gmac_stat & MVGMAC_RX_FLOW_CTRL_ENABLE)
+		state->pause |= MLO_PAUSE_RX;
+	if (gmac_stat & MVGMAC_TX_FLOW_CTRL_ENABLE)
+		state->pause |= MLO_PAUSE_TX;
+}
+EXPORT_SYMBOL_GPL(mvgmac_pcs_get_state);
+
+void mvgmac_pcs_an_restart(struct mvgmac *gmac)
+{
+	u32 gmac_an = readl_relaxed(gmac->base + GMAC_ANEG_REG);
+
+	writel_relaxed(gmac_an | GMAC_ANEG_INBAND_RESTART_AN,
+		       gmac->base + GMAC_ANEG_REG);
+	writel_relaxed(gmac_an & ~GMAC_ANEG_INBAND_RESTART_AN,
+		       gmac->base + GMAC_ANEG_REG);
+}
+EXPORT_SYMBOL_GPL(mvgmac_pcs_an_restart);
+
+int mvgmac_pcs_config(struct mvgmac *gmac, unsigned int mode,
+		      phy_interface_t interface,
+		      const unsigned long *advertising,
+		      bool permit_pause_to_mac)
+{
+	u32 mask, val, an, old_an, changed;
+
+	mask = GMAC_ANEG_INBAND_AN_ENABLE |
+	       GMAC_ANEG_INBAND_RESTART_AN |
+	       GMAC_ANEG_AN_SPEED_ENABLE |
+	       GMAC_ANEG_AN_FLOW_CTRL_ENABLE |
+	       GMAC_ANEG_AN_DUPLEX_ENABLE;
+
+	if (phylink_autoneg_inband(mode)) {
+		mask |= GMAC_ANEG_MII_SPEED |
+			GMAC_ANEG_GMII_SPEED |
+			GMAC_ANEG_FULL_DUPLEX;
+		val = GMAC_ANEG_INBAND_AN_ENABLE;
+
+		if (interface == PHY_INTERFACE_MODE_SGMII) {
+			/* SGMII mode receives the speed and duplex from PHY */
+			val |= GMAC_ANEG_AN_SPEED_ENABLE |
+			       GMAC_ANEG_AN_DUPLEX_ENABLE;
+		} else {
+			/* 802.3z mode has fixed speed and duplex */
+			val |= GMAC_ANEG_GMII_SPEED |
+			       GMAC_ANEG_FULL_DUPLEX;
+
+			/* The FLOW_CTRL_ENABLE bit selects either the hardware
+			 * automatically or the GMAC_ANEG_FLOW_CTRL manually
+			 * controls the GMAC pause mode.
+			 */
+			if (permit_pause_to_mac)
+				val |= GMAC_ANEG_AN_FLOW_CTRL_ENABLE;
+
+			/* Update the advertisement bits */
+			mask |= GMAC_ANEG_ADVERT_SYM_FLOW_CTRL;
+			if (phylink_test(advertising, Pause))
+				val |= GMAC_ANEG_ADVERT_SYM_FLOW_CTRL;
+			if (gmac->version == MVGMAC_PP22) {
+				mask |= GMAC_ANEG_ADVERT_ASYM_FLOW_CTRL;
+				if (phylink_test(advertising, Asym_Pause))
+					val |= GMAC_ANEG_ADVERT_ASYM_FLOW_CTRL;
+			}
+		}
+	} else {
+		/* Phy or fixed speed - disable in-band AN modes */
+		val = 0;
+	}
+
+	old_an = an = readl_relaxed(gmac->base + GMAC_ANEG_REG);
+	an = (an & ~mask) | val;
+	changed = old_an ^ an;
+	if (changed)
+		writel_relaxed(an, gmac->base + GMAC_ANEG_REG);
+
+	/* We are only interested in the advertisement bits changing */
+	return !!(changed & (GMAC_ANEG_ADVERT_SYM_FLOW_CTRL |
+			     GMAC_ANEG_ADVERT_ASYM_FLOW_CTRL));
+}
+EXPORT_SYMBOL_GPL(mvgmac_pcs_config);
+
+void mvgmac_config_mac(struct mvgmac *gmac, unsigned int mode,
+		       const struct phylink_link_state *state)
+{
+	u32 new_ctrl0, gmac_ctrl0 = readl_relaxed(gmac->base + GMAC_CTRL0_REG);
+	u32 new_ctrl2, gmac_ctrl2 = readl_relaxed(gmac->base + GMAC_CTRL2_REG);
+	u32 new_ctrl4, gmac_ctrl4 = readl_relaxed(gmac->base + GMAC_CTRL4_REG);
+
+	new_ctrl0 = gmac_ctrl0 & ~GMAC_CTRL0_PORT_1000BASE_X;
+	new_ctrl2 = gmac_ctrl2 & ~(GMAC_CTRL2_INBAND_AN_SGMII |
+				   GMAC_CTRL2_PORT_RESET);
+	new_ctrl4 = gmac_ctrl4;
+
+	/* Even though it might look weird, when we're configured in
+	 * SGMII or QSGMII mode, the RGMII bit needs to be set.
+	 */
+	new_ctrl2 |= GMAC_CTRL2_PORT_RGMII;
+
+	if (state->interface == PHY_INTERFACE_MODE_QSGMII ||
+	    state->interface == PHY_INTERFACE_MODE_SGMII ||
+	    phy_interface_mode_is_8023z(state->interface))
+		new_ctrl2 |= GMAC_CTRL2_PCS_ENABLE;
+
+	if (!phylink_autoneg_inband(mode)) {
+		/* Phy or fixed speed - nothing to do, leave the
+		 * configured speed, duplex and flow control as-is.
+		 */
+	} else if (state->interface == PHY_INTERFACE_MODE_SGMII) {
+		/* SGMII mode receives the state from the PHY */
+		new_ctrl2 |= GMAC_CTRL2_INBAND_AN_SGMII;
+	} else {
+		/* 802.3z negotiation - 1000BaseX */
+		new_ctrl0 |= GMAC_CTRL0_PORT_1000BASE_X;
+	}
+
+	if (gmac->version == MVGMAC_NETA) {
+		/* When at 2.5G, the link partner can send frames with
+		 * shortened preambles.
+		 */
+		new_ctrl4 &= ~GMAC_CTRL4_SHORT_PREAMBLE_ENABLE;
+		if (state->interface == PHY_INTERFACE_MODE_2500BASEX)
+			new_ctrl4 |= GMAC_CTRL4_SHORT_PREAMBLE_ENABLE;
+	}
+
+	if (new_ctrl0 != gmac_ctrl0)
+		writel_relaxed(new_ctrl0, gmac->base + GMAC_CTRL0_REG);
+	if (new_ctrl2 != gmac_ctrl2)
+		writel_relaxed(new_ctrl2, gmac->base + GMAC_CTRL2_REG);
+	if (new_ctrl4 != gmac_ctrl4)
+		writel_relaxed(new_ctrl4, gmac->base + GMAC_CTRL4_REG);
+
+	if (gmac_ctrl2 & GMAC_CTRL2_PORT_RESET) {
+		while ((readl_relaxed(gmac->base + GMAC_CTRL2_REG) &
+			GMAC_CTRL2_PORT_RESET) != 0)
+			continue;
+	}
+}
+EXPORT_SYMBOL_GPL(mvgmac_config_mac);
+
+int mvgmac_set_lpi_ts(struct mvgmac *gmac, unsigned int ts)
+{
+	u32 val;
+
+	if (!(readl_relaxed(gmac->base + GMAC_STATUS_REG) & MVGMAC_SPEED_1000))
+		ts = DIV_ROUND_UP(ts, 10);
+
+	if (ts > 255)
+		ts = 255;
+
+	val = readl_relaxed(gmac->base + GMAC_LPI_CTRL0_REG);
+	val = insert(val, GMAC_LPI_CTRL0_TS, ts);
+	writel_relaxed(val, gmac->base + GMAC_LPI_CTRL0_REG);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mvgmac_set_lpi_ts);
+
+void mvgmac_set_eee(struct mvgmac *gmac, bool enable)
+{
+	u32 val;
+
+	val = readl_relaxed(gmac->base + GMAC_LPI_CTRL1_REG);
+	val = insert(val, GMAC_LPI_CTRL1_REQ_EN, enable);
+	writel_relaxed(val, gmac->base + GMAC_LPI_CTRL1_REG);
+}
+EXPORT_SYMBOL_GPL(mvgmac_set_eee);
+
+MODULE_DESCRIPTION("Marvell GMAC driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/marvell/mvgmac.h b/drivers/net/ethernet/marvell/mvgmac.h
new file mode 100644
index 000000000000..f4111fba7258
--- /dev/null
+++ b/drivers/net/ethernet/marvell/mvgmac.h
@@ -0,0 +1,52 @@
+#ifndef MVGMAC_H
+#define MVGMAC_H
+
+#include <linux/phy.h>
+
+struct phylink_link_state;
+
+/* The two bytes Marvell header. Either contains a special value used by
+ * Marvell switches when a specific hardware mode is enabled (not supported
+ * by this driver) or is filled automatically by zeroes on the RX side.
+ * Those two bytes being at the front of the Ethernet header, they allow
+ * to have the IP header aligned on a 4 bytes boundary automatically: the
+ * hardware skips those two bytes on its own.
+ */
+#define MARVELL_HEADER_SIZE 2
+
+enum {
+	/* GMAC version */
+	MVGMAC_NETA,
+	MVGMAC_PP21,
+	MVGMAC_PP22,
+};
+
+struct mvgmac {
+	void __iomem *base;
+	unsigned int version;
+};
+
+void mvgmac_set_max_rx_size(struct mvgmac *gmac, size_t max_rx_size);
+void mvgmac_enable(struct mvgmac *gmac);
+void mvgmac_disable(struct mvgmac *gmac);
+int mvgmac_configure(struct mvgmac *gmac, phy_interface_t phy_mode);
+void mvgmac_link_unforce(struct mvgmac *gmac);
+void mvgmac_link_force_down(struct mvgmac *gmac);
+void mvgmac_link_down(struct mvgmac *gmac, int mode);
+void mvgmac_link_up(struct mvgmac *gmac, int mode, int speed, int duplex,
+		    bool tx_pause, bool rx_pause);
+bool mvgmac_link_is_up(struct mvgmac *gmac);
+void mvgmac_pcs_get_state(struct mvgmac *gmac,
+			  struct phylink_link_state *state);
+void mvgmac_pcs_an_restart(struct mvgmac *gmac);
+int mvgmac_pcs_config(struct mvgmac *gmac, unsigned int mode,
+		      phy_interface_t interface,
+		      const unsigned long *advertising,
+		      bool permit_pause_to_mac);
+void mvgmac_config_mac(struct mvgmac *gmac, unsigned int mode,
+		       const struct phylink_link_state *state);
+
+int mvgmac_set_lpi_ts(struct mvgmac *gmac, unsigned int ts);
+void mvgmac_set_eee(struct mvgmac *gmac, bool enable);
+
+#endif
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index bc4d8d144401..5bfbdaff1430 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -34,6 +34,7 @@
 #include <linux/skbuff.h>
 #include <net/hwbm.h>
 #include "mvneta_bm.h"
+#include "mvgmac.h"
 #include <net/ip.h>
 #include <net/ipv6.h>
 #include <net/tso.h>
@@ -191,43 +192,7 @@
 #define      MVNETA_RXQ_ENABLE_MASK              0x000000ff
 #define MVETH_TXQ_TOKEN_COUNT_REG(q)             (0x2700 + ((q) << 4))
 #define MVETH_TXQ_TOKEN_CFG_REG(q)               (0x2704 + ((q) << 4))
-#define MVNETA_GMAC_CTRL_0                       0x2c00
-#define      MVNETA_GMAC_MAX_RX_SIZE_SHIFT       2
-#define      MVNETA_GMAC_MAX_RX_SIZE_MASK        0x7ffc
-#define      MVNETA_GMAC0_PORT_1000BASE_X        BIT(1)
-#define      MVNETA_GMAC0_PORT_ENABLE            BIT(0)
-#define MVNETA_GMAC_CTRL_2                       0x2c08
-#define      MVNETA_GMAC2_INBAND_AN_ENABLE       BIT(0)
-#define      MVNETA_GMAC2_PCS_ENABLE             BIT(3)
-#define      MVNETA_GMAC2_PORT_RGMII             BIT(4)
-#define      MVNETA_GMAC2_PORT_RESET             BIT(6)
-#define MVNETA_GMAC_STATUS                       0x2c10
-#define      MVNETA_GMAC_LINK_UP                 BIT(0)
-#define      MVNETA_GMAC_SPEED_1000              BIT(1)
-#define      MVNETA_GMAC_SPEED_100               BIT(2)
-#define      MVNETA_GMAC_FULL_DUPLEX             BIT(3)
-#define      MVNETA_GMAC_RX_FLOW_CTRL_ENABLE     BIT(4)
-#define      MVNETA_GMAC_TX_FLOW_CTRL_ENABLE     BIT(5)
-#define      MVNETA_GMAC_RX_FLOW_CTRL_ACTIVE     BIT(6)
-#define      MVNETA_GMAC_TX_FLOW_CTRL_ACTIVE     BIT(7)
-#define      MVNETA_GMAC_AN_COMPLETE             BIT(11)
-#define      MVNETA_GMAC_SYNC_OK                 BIT(14)
-#define MVNETA_GMAC_AUTONEG_CONFIG               0x2c0c
-#define      MVNETA_GMAC_FORCE_LINK_DOWN         BIT(0)
-#define      MVNETA_GMAC_FORCE_LINK_PASS         BIT(1)
-#define      MVNETA_GMAC_INBAND_AN_ENABLE        BIT(2)
-#define      MVNETA_GMAC_AN_BYPASS_ENABLE        BIT(3)
-#define      MVNETA_GMAC_INBAND_RESTART_AN       BIT(4)
-#define      MVNETA_GMAC_CONFIG_MII_SPEED        BIT(5)
-#define      MVNETA_GMAC_CONFIG_GMII_SPEED       BIT(6)
-#define      MVNETA_GMAC_AN_SPEED_EN             BIT(7)
-#define      MVNETA_GMAC_CONFIG_FLOW_CTRL        BIT(8)
-#define      MVNETA_GMAC_ADVERT_SYM_FLOW_CTRL    BIT(9)
-#define      MVNETA_GMAC_AN_FLOW_CTRL_EN         BIT(11)
-#define      MVNETA_GMAC_CONFIG_FULL_DUPLEX      BIT(12)
-#define      MVNETA_GMAC_AN_DUPLEX_EN            BIT(13)
-#define MVNETA_GMAC_CTRL_4                       0x2c90
-#define      MVNETA_GMAC4_SHORT_PREAMBLE_ENABLE  BIT(1)
+#define MVNETA_GMAC_BASE                         0x2c00
 #define MVNETA_MIB_COUNTERS_BASE                 0x3000
 #define      MVNETA_MIB_LATE_COLLISION           0x7c
 #define MVNETA_DA_FILT_SPEC_MCAST                0x3400
@@ -251,12 +216,6 @@
 #define MVNETA_TXQ_TOKEN_SIZE_REG(q)             (0x3e40 + ((q) << 2))
 #define      MVNETA_TXQ_TOKEN_SIZE_MAX           0x7fffffff
 
-#define MVNETA_LPI_CTRL_0                        0x2cc0
-#define MVNETA_LPI_CTRL_1                        0x2cc4
-#define      MVNETA_LPI_REQUEST_ENABLE           BIT(0)
-#define MVNETA_LPI_CTRL_2                        0x2cc8
-#define MVNETA_LPI_STATUS                        0x2ccc
-
 #define MVNETA_CAUSE_TXQ_SENT_DESC_ALL_MASK	 0xff
 
 /* Descriptor ring Macros */
@@ -278,7 +237,7 @@
  * boundary automatically: the hardware skips those two bytes on its
  * own.
  */
-#define MVNETA_MH_SIZE			2
+#define MVNETA_MH_SIZE			MARVELL_HEADER_SIZE
 
 #define MVNETA_VLAN_TAG_LEN             4
 
@@ -496,8 +455,10 @@ struct mvneta_port {
 	unsigned int tx_csum_limit;
 	struct phylink *phylink;
 	struct phylink_config phylink_config;
+	struct phylink_pcs phylink_pcs;
 	struct phy *comphy;
 
+	struct mvgmac gmac;
 	struct mvneta_bm *bm_priv;
 	struct mvneta_bm_pool *pool_long;
 	struct mvneta_bm_pool *pool_short;
@@ -506,6 +467,7 @@ struct mvneta_port {
 	bool eee_enabled;
 	bool eee_active;
 	bool tx_lpi_enabled;
+	u32 tx_lpi_timer;
 
 	u64 ethtool_stats[ARRAY_SIZE(mvneta_statistics)];
 
@@ -888,19 +850,6 @@ mvneta_rxq_next_desc_get(struct mvneta_rx_queue *rxq)
 	return rxq->descs + rx_desc;
 }
 
-/* Change maximum receive size of the port. */
-static void mvneta_max_rx_size_set(struct mvneta_port *pp, int max_rx_size)
-{
-	u32 val;
-
-	val =  mvreg_read(pp, MVNETA_GMAC_CTRL_0);
-	val &= ~MVNETA_GMAC_MAX_RX_SIZE_MASK;
-	val |= ((max_rx_size - MVNETA_MH_SIZE) / 2) <<
-		MVNETA_GMAC_MAX_RX_SIZE_SHIFT;
-	mvreg_write(pp, MVNETA_GMAC_CTRL_0, val);
-}
-
-
 /* Set rx queue offset */
 static void mvneta_rxq_offset_set(struct mvneta_port *pp,
 				  struct mvneta_rx_queue *rxq,
@@ -1308,23 +1257,13 @@ static void mvneta_port_down(struct mvneta_port *pp)
 /* Enable the port by setting the port enable bit of the MAC control register */
 static void mvneta_port_enable(struct mvneta_port *pp)
 {
-	u32 val;
-
-	/* Enable port */
-	val = mvreg_read(pp, MVNETA_GMAC_CTRL_0);
-	val |= MVNETA_GMAC0_PORT_ENABLE;
-	mvreg_write(pp, MVNETA_GMAC_CTRL_0, val);
+	mvgmac_enable(&pp->gmac);
 }
 
 /* Disable the port and wait for about 200 usec before retuning */
 static void mvneta_port_disable(struct mvneta_port *pp)
 {
-	u32 val;
-
-	/* Reset the Enable bit in the Serial Control Register */
-	val = mvreg_read(pp, MVNETA_GMAC_CTRL_0);
-	val &= ~MVNETA_GMAC0_PORT_ENABLE;
-	mvreg_write(pp, MVNETA_GMAC_CTRL_0, val);
+	mvgmac_disable(&pp->gmac);
 
 	udelay(200);
 }
@@ -3134,9 +3073,9 @@ static irqreturn_t mvneta_percpu_isr(int irq, void *dev_id)
 
 static void mvneta_link_change(struct mvneta_port *pp)
 {
-	u32 gmac_stat = mvreg_read(pp, MVNETA_GMAC_STATUS);
+	bool link_is_up = mvgmac_link_is_up(&pp->gmac);
 
-	phylink_mac_change(pp->phylink, !!(gmac_stat & MVNETA_GMAC_LINK_UP));
+	phylink_mac_change(pp->phylink, link_is_up);
 }
 
 /* NAPI handler
@@ -3626,7 +3565,7 @@ static void mvneta_start_dev(struct mvneta_port *pp)
 
 	WARN_ON(mvneta_config_interface(pp, pp->phy_interface));
 
-	mvneta_max_rx_size_set(pp, pp->pkt_size);
+	mvgmac_set_max_rx_size(&pp->gmac, pp->pkt_size);
 	mvneta_txq_max_tx_size_set(pp, pp->pkt_size);
 
 	/* start the Rx/Tx activity */
@@ -3828,6 +3767,46 @@ static int mvneta_set_mac_addr(struct net_device *dev, void *addr)
 	return 0;
 }
 
+static struct mvneta_port *mvneta_pcs_to_port(struct phylink_pcs *pcs)
+{
+	return container_of(pcs, struct mvneta_port, phylink_pcs);
+}
+
+static void mvneta_pcs_get_state(struct phylink_pcs *pcs,
+				 struct phylink_link_state *state)
+{
+	struct mvneta_port *pp = mvneta_pcs_to_port(pcs);
+
+	mvgmac_pcs_get_state(&pp->gmac, state);
+}
+
+static int mvneta_pcs_config(struct phylink_pcs *pcs,
+			     unsigned int mode, phy_interface_t interface,
+			     const unsigned long *advertising,
+			     bool permit_pause_to_mac)
+{
+	struct mvneta_port *pp = mvneta_pcs_to_port(pcs);
+
+	/* We should never see Asym_Pause set */
+	WARN_ON(phylink_test(advertising, Asym_Pause));
+
+	return mvgmac_pcs_config(&pp->gmac, mode, interface, advertising,
+				 permit_pause_to_mac);
+}
+
+static void mvneta_pcs_an_restart(struct phylink_pcs *pcs)
+{
+	struct mvneta_port *pp = mvneta_pcs_to_port(pcs);
+
+	mvgmac_pcs_an_restart(&pp->gmac);
+}
+
+static const struct phylink_pcs_ops mvneta_phylink_pcs_ops = {
+	.pcs_get_state = mvneta_pcs_get_state,
+	.pcs_config = mvneta_pcs_config,
+	.pcs_an_restart = mvneta_pcs_an_restart,
+};
+
 static void mvneta_validate(struct phylink_config *config,
 			    unsigned long *supported,
 			    struct phylink_link_state *state)
@@ -3882,167 +3861,70 @@ static void mvneta_validate(struct phylink_config *config,
 	phylink_helper_basex_speed(state);
 }
 
-static void mvneta_mac_pcs_get_state(struct phylink_config *config,
-				     struct phylink_link_state *state)
+static int mvneta_mac_prepare(struct phylink_config *config, unsigned int mode,
+			      phy_interface_t interface)
 {
 	struct net_device *ndev = to_net_dev(config->dev);
 	struct mvneta_port *pp = netdev_priv(ndev);
-	u32 gmac_stat;
 
-	gmac_stat = mvreg_read(pp, MVNETA_GMAC_STATUS);
+	if (pp->phy_interface != interface ||
+	    phylink_autoneg_inband(mode)) {
+		/* Force the link down when changing the interface or if in
+		 * in-band mode. According to Armada 370 documentation, we
+		 * can only change the port mode and in-band enable when the
+		 * link is down.
+		 */
+		mvgmac_link_force_down(&pp->gmac);
+	}
+
+	if (pp->phy_interface != interface)
+		WARN_ON(phy_power_off(pp->comphy));
 
-	if (gmac_stat & MVNETA_GMAC_SPEED_1000)
-		state->speed =
-			state->interface == PHY_INTERFACE_MODE_2500BASEX ?
-			SPEED_2500 : SPEED_1000;
-	else if (gmac_stat & MVNETA_GMAC_SPEED_100)
-		state->speed = SPEED_100;
-	else
-		state->speed = SPEED_10;
+	/* Enable the 1ms clock */
+	if (phylink_autoneg_inband(mode)) {
+		unsigned long rate = clk_get_rate(pp->clk);
 
-	state->an_complete = !!(gmac_stat & MVNETA_GMAC_AN_COMPLETE);
-	state->link = !!(gmac_stat & MVNETA_GMAC_LINK_UP);
-	state->duplex = !!(gmac_stat & MVNETA_GMAC_FULL_DUPLEX);
+		mvreg_write(pp, MVNETA_GMAC_CLOCK_DIVIDER,
+			    MVNETA_GMAC_1MS_CLOCK_ENABLE | (rate / 1000));
+	}
 
-	state->pause = 0;
-	if (gmac_stat & MVNETA_GMAC_RX_FLOW_CTRL_ENABLE)
-		state->pause |= MLO_PAUSE_RX;
-	if (gmac_stat & MVNETA_GMAC_TX_FLOW_CTRL_ENABLE)
-		state->pause |= MLO_PAUSE_TX;
+	return 0;
 }
 
-static void mvneta_mac_an_restart(struct phylink_config *config)
+static void mvneta_mac_config(struct phylink_config *config, unsigned int mode,
+			      const struct phylink_link_state *state)
 {
 	struct net_device *ndev = to_net_dev(config->dev);
 	struct mvneta_port *pp = netdev_priv(ndev);
-	u32 gmac_an = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
 
-	mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG,
-		    gmac_an | MVNETA_GMAC_INBAND_RESTART_AN);
-	mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG,
-		    gmac_an & ~MVNETA_GMAC_INBAND_RESTART_AN);
+	mvgmac_config_mac(&pp->gmac, mode, state);
 }
 
-static void mvneta_mac_config(struct phylink_config *config, unsigned int mode,
-			      const struct phylink_link_state *state)
+static int mvneta_mac_finish(struct phylink_config *config, unsigned int mode,
+			     phy_interface_t interface)
 {
 	struct net_device *ndev = to_net_dev(config->dev);
 	struct mvneta_port *pp = netdev_priv(ndev);
-	u32 new_ctrl0, gmac_ctrl0 = mvreg_read(pp, MVNETA_GMAC_CTRL_0);
-	u32 new_ctrl2, gmac_ctrl2 = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
-	u32 new_ctrl4, gmac_ctrl4 = mvreg_read(pp, MVNETA_GMAC_CTRL_4);
-	u32 new_clk, gmac_clk = mvreg_read(pp, MVNETA_GMAC_CLOCK_DIVIDER);
-	u32 new_an, gmac_an = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
-
-	new_ctrl0 = gmac_ctrl0 & ~MVNETA_GMAC0_PORT_1000BASE_X;
-	new_ctrl2 = gmac_ctrl2 & ~(MVNETA_GMAC2_INBAND_AN_ENABLE |
-				   MVNETA_GMAC2_PORT_RESET);
-	new_ctrl4 = gmac_ctrl4 & ~(MVNETA_GMAC4_SHORT_PREAMBLE_ENABLE);
-	new_clk = gmac_clk & ~MVNETA_GMAC_1MS_CLOCK_ENABLE;
-	new_an = gmac_an & ~(MVNETA_GMAC_INBAND_AN_ENABLE |
-			     MVNETA_GMAC_INBAND_RESTART_AN |
-			     MVNETA_GMAC_AN_SPEED_EN |
-			     MVNETA_GMAC_ADVERT_SYM_FLOW_CTRL |
-			     MVNETA_GMAC_AN_FLOW_CTRL_EN |
-			     MVNETA_GMAC_AN_DUPLEX_EN);
-
-	/* Even though it might look weird, when we're configured in
-	 * SGMII or QSGMII mode, the RGMII bit needs to be set.
-	 */
-	new_ctrl2 |= MVNETA_GMAC2_PORT_RGMII;
-
-	if (state->interface == PHY_INTERFACE_MODE_QSGMII ||
-	    state->interface == PHY_INTERFACE_MODE_SGMII ||
-	    phy_interface_mode_is_8023z(state->interface))
-		new_ctrl2 |= MVNETA_GMAC2_PCS_ENABLE;
-
-	if (phylink_test(state->advertising, Pause))
-		new_an |= MVNETA_GMAC_ADVERT_SYM_FLOW_CTRL;
+	u32 clk;
 
+	/* Disable 1ms clock if not in in-band mode */
 	if (!phylink_autoneg_inband(mode)) {
-		/* Phy or fixed speed - nothing to do, leave the
-		 * configured speed, duplex and flow control as-is.
-		 */
-	} else if (state->interface == PHY_INTERFACE_MODE_SGMII) {
-		/* SGMII mode receives the state from the PHY */
-		new_ctrl2 |= MVNETA_GMAC2_INBAND_AN_ENABLE;
-		new_clk |= MVNETA_GMAC_1MS_CLOCK_ENABLE;
-		new_an = (new_an & ~(MVNETA_GMAC_FORCE_LINK_DOWN |
-				     MVNETA_GMAC_FORCE_LINK_PASS |
-				     MVNETA_GMAC_CONFIG_MII_SPEED |
-				     MVNETA_GMAC_CONFIG_GMII_SPEED |
-				     MVNETA_GMAC_CONFIG_FULL_DUPLEX)) |
-			 MVNETA_GMAC_INBAND_AN_ENABLE |
-			 MVNETA_GMAC_AN_SPEED_EN |
-			 MVNETA_GMAC_AN_DUPLEX_EN;
-	} else {
-		/* 802.3z negotiation - only 1000base-X */
-		new_ctrl0 |= MVNETA_GMAC0_PORT_1000BASE_X;
-		new_clk |= MVNETA_GMAC_1MS_CLOCK_ENABLE;
-		new_an = (new_an & ~(MVNETA_GMAC_FORCE_LINK_DOWN |
-				     MVNETA_GMAC_FORCE_LINK_PASS |
-				     MVNETA_GMAC_CONFIG_MII_SPEED)) |
-			 MVNETA_GMAC_INBAND_AN_ENABLE |
-			 MVNETA_GMAC_CONFIG_GMII_SPEED |
-			 /* The MAC only supports FD mode */
-			 MVNETA_GMAC_CONFIG_FULL_DUPLEX;
-
-		if (state->pause & MLO_PAUSE_AN && state->an_enabled)
-			new_an |= MVNETA_GMAC_AN_FLOW_CTRL_EN;
-	}
-
-	/* Armada 370 documentation says we can only change the port mode
-	 * and in-band enable when the link is down, so force it down
-	 * while making these changes. We also do this for GMAC_CTRL2 */
-	if ((new_ctrl0 ^ gmac_ctrl0) & MVNETA_GMAC0_PORT_1000BASE_X ||
-	    (new_ctrl2 ^ gmac_ctrl2) & MVNETA_GMAC2_INBAND_AN_ENABLE ||
-	    (new_an  ^ gmac_an) & MVNETA_GMAC_INBAND_AN_ENABLE) {
-		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG,
-			    (gmac_an & ~MVNETA_GMAC_FORCE_LINK_PASS) |
-			    MVNETA_GMAC_FORCE_LINK_DOWN);
-	}
-
-
-	/* When at 2.5G, the link partner can send frames with shortened
-	 * preambles.
-	 */
-	if (state->interface == PHY_INTERFACE_MODE_2500BASEX)
-		new_ctrl4 |= MVNETA_GMAC4_SHORT_PREAMBLE_ENABLE;
-
-	if (pp->phy_interface != state->interface) {
-		if (pp->comphy)
-			WARN_ON(phy_power_off(pp->comphy));
-		WARN_ON(mvneta_config_interface(pp, state->interface));
-	}
-
-	if (new_ctrl0 != gmac_ctrl0)
-		mvreg_write(pp, MVNETA_GMAC_CTRL_0, new_ctrl0);
-	if (new_ctrl2 != gmac_ctrl2)
-		mvreg_write(pp, MVNETA_GMAC_CTRL_2, new_ctrl2);
-	if (new_ctrl4 != gmac_ctrl4)
-		mvreg_write(pp, MVNETA_GMAC_CTRL_4, new_ctrl4);
-	if (new_clk != gmac_clk)
-		mvreg_write(pp, MVNETA_GMAC_CLOCK_DIVIDER, new_clk);
-	if (new_an != gmac_an)
-		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, new_an);
-
-	if (gmac_ctrl2 & MVNETA_GMAC2_PORT_RESET) {
-		while ((mvreg_read(pp, MVNETA_GMAC_CTRL_2) &
-			MVNETA_GMAC2_PORT_RESET) != 0)
-			continue;
+		clk = mvreg_read(pp, MVNETA_GMAC_CLOCK_DIVIDER);
+		clk &= ~MVNETA_GMAC_1MS_CLOCK_ENABLE;
+		mvreg_write(pp, MVNETA_GMAC_CLOCK_DIVIDER, clk);
 	}
-}
 
-static void mvneta_set_eee(struct mvneta_port *pp, bool enable)
-{
-	u32 lpi_ctl1;
+	if (pp->phy_interface != interface)
+		/* Enable the Serdes PHY */
+		WARN_ON(mvneta_config_interface(pp, interface));
 
-	lpi_ctl1 = mvreg_read(pp, MVNETA_LPI_CTRL_1);
-	if (enable)
-		lpi_ctl1 |= MVNETA_LPI_REQUEST_ENABLE;
-	else
-		lpi_ctl1 &= ~MVNETA_LPI_REQUEST_ENABLE;
-	mvreg_write(pp, MVNETA_LPI_CTRL_1, lpi_ctl1);
+	/* Allow the link to come up if in in-band mode, otherwise the
+	 * link is forced via mac_link_down()/mac_link_up()
+	 */
+	if (phylink_autoneg_inband(mode))
+		mvgmac_link_unforce(&pp->gmac);
+
+	return 0;
 }
 
 static void mvneta_mac_link_down(struct phylink_config *config,
@@ -4050,19 +3932,12 @@ static void mvneta_mac_link_down(struct phylink_config *config,
 {
 	struct net_device *ndev = to_net_dev(config->dev);
 	struct mvneta_port *pp = netdev_priv(ndev);
-	u32 val;
 
 	mvneta_port_down(pp);
-
-	if (!phylink_autoneg_inband(mode)) {
-		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
-		val &= ~MVNETA_GMAC_FORCE_LINK_PASS;
-		val |= MVNETA_GMAC_FORCE_LINK_DOWN;
-		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
-	}
+	mvgmac_link_down(&pp->gmac, mode);
 
 	pp->eee_active = false;
-	mvneta_set_eee(pp, false);
+	mvgmac_set_eee(&pp->gmac, false);
 }
 
 static void mvneta_mac_link_up(struct phylink_config *config,
@@ -4073,56 +3948,22 @@ static void mvneta_mac_link_up(struct phylink_config *config,
 {
 	struct net_device *ndev = to_net_dev(config->dev);
 	struct mvneta_port *pp = netdev_priv(ndev);
-	u32 val;
-
-	if (!phylink_autoneg_inband(mode)) {
-		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
-		val &= ~(MVNETA_GMAC_FORCE_LINK_DOWN |
-			 MVNETA_GMAC_CONFIG_MII_SPEED |
-			 MVNETA_GMAC_CONFIG_GMII_SPEED |
-			 MVNETA_GMAC_CONFIG_FLOW_CTRL |
-			 MVNETA_GMAC_CONFIG_FULL_DUPLEX);
-		val |= MVNETA_GMAC_FORCE_LINK_PASS;
-
-		if (speed == SPEED_1000 || speed == SPEED_2500)
-			val |= MVNETA_GMAC_CONFIG_GMII_SPEED;
-		else if (speed == SPEED_100)
-			val |= MVNETA_GMAC_CONFIG_MII_SPEED;
-
-		if (duplex == DUPLEX_FULL)
-			val |= MVNETA_GMAC_CONFIG_FULL_DUPLEX;
-
-		if (tx_pause || rx_pause)
-			val |= MVNETA_GMAC_CONFIG_FLOW_CTRL;
-
-		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
-	} else {
-		/* When inband doesn't cover flow control or flow control is
-		 * disabled, we need to manually configure it. This bit will
-		 * only have effect if MVNETA_GMAC_AN_FLOW_CTRL_EN is unset.
-		 */
-		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
-		val &= ~MVNETA_GMAC_CONFIG_FLOW_CTRL;
-
-		if (tx_pause || rx_pause)
-			val |= MVNETA_GMAC_CONFIG_FLOW_CTRL;
-
-		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
-	}
 
+	mvgmac_link_up(&pp->gmac, mode, speed, duplex, tx_pause, rx_pause);
 	mvneta_port_up(pp);
 
 	if (phy && pp->eee_enabled) {
 		pp->eee_active = phy_init_eee(phy, 0) >= 0;
-		mvneta_set_eee(pp, pp->eee_active && pp->tx_lpi_enabled);
+		mvgmac_set_lpi_ts(&pp->gmac, pp->tx_lpi_timer);
+		mvgmac_set_eee(&pp->gmac, pp->eee_active && pp->tx_lpi_enabled);
 	}
 }
 
 static const struct phylink_mac_ops mvneta_phylink_ops = {
 	.validate = mvneta_validate,
-	.mac_pcs_get_state = mvneta_mac_pcs_get_state,
-	.mac_an_restart = mvneta_mac_an_restart,
+	.mac_prepare = mvneta_mac_prepare,
 	.mac_config = mvneta_mac_config,
+	.mac_finish = mvneta_mac_finish,
 	.mac_link_down = mvneta_mac_link_down,
 	.mac_link_up = mvneta_mac_link_up,
 };
@@ -4883,14 +4724,11 @@ static int mvneta_ethtool_get_eee(struct net_device *dev,
 				  struct ethtool_eee *eee)
 {
 	struct mvneta_port *pp = netdev_priv(dev);
-	u32 lpi_ctl0;
-
-	lpi_ctl0 = mvreg_read(pp, MVNETA_LPI_CTRL_0);
 
 	eee->eee_enabled = pp->eee_enabled;
 	eee->eee_active = pp->eee_active;
 	eee->tx_lpi_enabled = pp->tx_lpi_enabled;
-	eee->tx_lpi_timer = (lpi_ctl0) >> 8; // * scale;
+	eee->tx_lpi_timer = pp->tx_lpi_timer;
 
 	return phylink_ethtool_get_eee(pp->phylink, eee);
 }
@@ -4899,22 +4737,14 @@ static int mvneta_ethtool_set_eee(struct net_device *dev,
 				  struct ethtool_eee *eee)
 {
 	struct mvneta_port *pp = netdev_priv(dev);
-	u32 lpi_ctl0;
-
-	/* The Armada 37x documents do not give limits for this other than
-	 * it being an 8-bit register. */
-	if (eee->tx_lpi_enabled && eee->tx_lpi_timer > 255)
-		return -EINVAL;
-
-	lpi_ctl0 = mvreg_read(pp, MVNETA_LPI_CTRL_0);
-	lpi_ctl0 &= ~(0xff << 8);
-	lpi_ctl0 |= eee->tx_lpi_timer << 8;
-	mvreg_write(pp, MVNETA_LPI_CTRL_0, lpi_ctl0);
 
 	pp->eee_enabled = eee->eee_enabled;
 	pp->tx_lpi_enabled = eee->tx_lpi_enabled;
+	pp->tx_lpi_timer = eee->tx_lpi_timer;
 
-	mvneta_set_eee(pp, eee->tx_lpi_enabled && eee->eee_enabled);
+	mvgmac_set_eee(&pp->gmac, false);
+	mvgmac_set_lpi_ts(&pp->gmac, eee->tx_lpi_timer);
+	mvgmac_set_eee(&pp->gmac, pp->eee_active && pp->tx_lpi_enabled);
 
 	return phylink_ethtool_set_eee(pp->phylink, eee);
 }
@@ -5114,6 +4944,22 @@ static int mvneta_probe(struct platform_device *pdev)
 
 	pp->phylink_config.dev = &dev->dev;
 	pp->phylink_config.type = PHYLINK_NETDEV;
+	__set_bit(PHY_INTERFACE_MODE_SGMII,
+		  pp->phylink_config.supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_RGMII,
+		  pp->phylink_config.supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_RGMII_ID,
+		  pp->phylink_config.supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_RGMII_RXID,
+		  pp->phylink_config.supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_RGMII_TXID,
+		  pp->phylink_config.supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_QSGMII,
+		  pp->phylink_config.supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_1000BASEX,
+		  pp->phylink_config.supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_2500BASEX,
+		  pp->phylink_config.supported_interfaces);
 
 	phylink = phylink_create(&pp->phylink_config, pdev->dev.fwnode,
 				 phy_mode, &mvneta_phylink_ops);
@@ -5122,6 +4968,9 @@ static int mvneta_probe(struct platform_device *pdev)
 		goto err_free_irq;
 	}
 
+	pp->phylink_pcs.ops = &mvneta_phylink_pcs_ops;
+	phylink_set_pcs(phylink, &pp->phylink_pcs);
+
 	dev->tx_queue_len = MVNETA_MAX_TXD;
 	dev->watchdog_timeo = 5 * HZ;
 	dev->netdev_ops = &mvneta_netdev_ops;
@@ -5160,6 +5009,10 @@ static int mvneta_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
+	pp->gmac.base = pp->base + MVNETA_GMAC_BASE;
+	pp->gmac.version = MVGMAC_NETA;
+	pp->tx_lpi_timer = 16;
+
 	/* Alloc per-cpu port structure */
 	pp->ports = alloc_percpu(struct mvneta_pcpu_port);
 	if (!pp->ports) {
diff --git a/drivers/net/ethernet/marvell/mvpp2/mvpp2_main.c b/drivers/net/ethernet/marvell/mvpp2/mvpp2_main.c
index 358119d98358..2171c30a0e90 100644
--- a/drivers/net/ethernet/marvell/mvpp2/mvpp2_main.c
+++ b/drivers/net/ethernet/marvell/mvpp2/mvpp2_main.c
@@ -6501,6 +6501,29 @@ static int mvpp2_port_probe(struct platform_device *pdev,
 		port->phylink_config.dev = &dev->dev;
 		port->phylink_config.type = PHYLINK_NETDEV;
 
+		if (mvpp2_port_supports_xlg(port)) {
+			__set_bit(PHY_INTERFACE_MODE_10GBASER,
+				  port->phylink_config.supported_interfaces);
+			__set_bit(PHY_INTERFACE_MODE_XAUI,
+				  port->phylink_config.supported_interfaces);
+		}
+		if (mvpp2_port_supports_rgmii(port)) {
+			__set_bit(PHY_INTERFACE_MODE_RGMII,
+				  port->phylink_config.supported_interfaces);
+			__set_bit(PHY_INTERFACE_MODE_RGMII_ID,
+				  port->phylink_config.supported_interfaces);
+			__set_bit(PHY_INTERFACE_MODE_RGMII_TXID,
+				  port->phylink_config.supported_interfaces);
+			__set_bit(PHY_INTERFACE_MODE_RGMII_RXID,
+				  port->phylink_config.supported_interfaces);
+		}
+		__set_bit(PHY_INTERFACE_MODE_SGMII,
+			  port->phylink_config.supported_interfaces);
+		__set_bit(PHY_INTERFACE_MODE_1000BASEX,
+			  port->phylink_config.supported_interfaces);
+		__set_bit(PHY_INTERFACE_MODE_2500BASEX,
+			  port->phylink_config.supported_interfaces);
+
 		phylink = phylink_create(&port->phylink_config, port_fwnode,
 					 phy_mode, &mvpp2_phylink_ops);
 		if (IS_ERR(phylink)) {
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
index 6d2d60675ffd..ab180f79a82c 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -335,7 +335,7 @@ static void mtk_mac_config(struct phylink_config *config, unsigned int mode,
 		/* Setup SGMIISYS with the determined property */
 		if (state->interface != PHY_INTERFACE_MODE_SGMII)
 			err = mtk_sgmii_setup_mode_force(eth->sgmii, sid,
-							 state);
+							 state->interface);
 		else if (phylink_autoneg_inband(mode))
 			err = mtk_sgmii_setup_mode_an(eth->sgmii, sid);
 
@@ -432,6 +432,15 @@ static void mtk_mac_link_up(struct phylink_config *config,
 					   phylink_config);
 	u32 mcr = mtk_r32(mac->hw, MTK_MAC_MCR(mac->id));
 
+	if (phy_interface_mode_is_8023z(interface)) {
+		struct mtk_eth *eth = mac->hw;
+
+		/* Decide how GMAC and SGMIISYS be mapped */
+		int sid = (MTK_HAS_CAPS(eth->soc->caps, MTK_SHARED_SGMII)) ?
+			   0 : mac->id;
+		mtk_sgmii_link_up(eth->sgmii, sid, speed, duplex);
+	}
+
 	mcr &= ~(MAC_MCR_SPEED_100 | MAC_MCR_SPEED_1000 |
 		 MAC_MCR_FORCE_DPX | MAC_MCR_FORCE_TX_FC |
 		 MAC_MCR_FORCE_RX_FC);
diff --git a/drivers/net/ethernet/mediatek/mtk_eth_soc.h b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
index 454cfcd465fd..6f4b99bb7bfb 100644
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -932,7 +932,8 @@ int mtk_sgmii_init(struct mtk_sgmii *ss, struct device_node *np,
 		   u32 ana_rgc3);
 int mtk_sgmii_setup_mode_an(struct mtk_sgmii *ss, int id);
 int mtk_sgmii_setup_mode_force(struct mtk_sgmii *ss, int id,
-			       const struct phylink_link_state *state);
+			       phy_interface_t interface);
+void mtk_sgmii_link_up(struct mtk_sgmii *ss, int id, int speed, int duplex);
 void mtk_sgmii_restart_an(struct mtk_eth *eth, int mac_id);
 
 int mtk_gmac_sgmii_path_setup(struct mtk_eth *eth, int mac_id);
diff --git a/drivers/net/ethernet/mediatek/mtk_sgmii.c b/drivers/net/ethernet/mediatek/mtk_sgmii.c
index 32d83421226a..372c85c830b5 100644
--- a/drivers/net/ethernet/mediatek/mtk_sgmii.c
+++ b/drivers/net/ethernet/mediatek/mtk_sgmii.c
@@ -60,7 +60,7 @@ int mtk_sgmii_setup_mode_an(struct mtk_sgmii *ss, int id)
 }
 
 int mtk_sgmii_setup_mode_force(struct mtk_sgmii *ss, int id,
-			       const struct phylink_link_state *state)
+			       phy_interface_t interface)
 {
 	unsigned int val;
 
@@ -69,7 +69,7 @@ int mtk_sgmii_setup_mode_force(struct mtk_sgmii *ss, int id,
 
 	regmap_read(ss->regmap[id], ss->ana_rgc3, &val);
 	val &= ~RG_PHY_SPEED_MASK;
-	if (state->interface == PHY_INTERFACE_MODE_2500BASEX)
+	if (interface == PHY_INTERFACE_MODE_2500BASEX)
 		val |= RG_PHY_SPEED_3_125G;
 	regmap_write(ss->regmap[id], ss->ana_rgc3, val);
 
@@ -78,11 +78,33 @@ int mtk_sgmii_setup_mode_force(struct mtk_sgmii *ss, int id,
 	val &= ~SGMII_AN_ENABLE;
 	regmap_write(ss->regmap[id], SGMSYS_PCS_CONTROL_1, val);
 
+	if (interface == PHY_INTERFACE_MODE_1000BASEX ||
+	    interface == PHY_INTERFACE_MODE_2500BASEX) {
+		/* SGMII force mode setting */
+		regmap_read(ss->regmap[id], SGMSYS_SGMII_MODE, &val);
+		val &= ~SGMII_IF_MODE_MASK;
+		val |= SGMII_SPEED_1000;
+		val |= SGMII_DUPLEX_FULL;
+		regmap_write(ss->regmap[id], SGMSYS_SGMII_MODE, val);
+	}
+
+	/* Release PHYA power down state */
+	regmap_read(ss->regmap[id], SGMSYS_QPHY_PWR_STATE_CTRL, &val);
+	val &= ~SGMII_PHYA_PWD;
+	regmap_write(ss->regmap[id], SGMSYS_QPHY_PWR_STATE_CTRL, val);
+
+	return 0;
+}
+
+void mtk_sgmii_link_up(struct mtk_sgmii *ss, int id, int speed, int duplex)
+{
+	unsigned int val;
+
 	/* SGMII force mode setting */
 	regmap_read(ss->regmap[id], SGMSYS_SGMII_MODE, &val);
 	val &= ~SGMII_IF_MODE_MASK;
 
-	switch (state->speed) {
+	switch (speed) {
 	case SPEED_10:
 		val |= SGMII_SPEED_10;
 		break;
@@ -95,17 +117,10 @@ int mtk_sgmii_setup_mode_force(struct mtk_sgmii *ss, int id,
 		break;
 	}
 
-	if (state->duplex == DUPLEX_FULL)
+	if (duplex == DUPLEX_FULL)
 		val |= SGMII_DUPLEX_FULL;
 
 	regmap_write(ss->regmap[id], SGMSYS_SGMII_MODE, val);
-
-	/* Release PHYA power down state */
-	regmap_read(ss->regmap[id], SGMSYS_QPHY_PWR_STATE_CTRL, &val);
-	val &= ~SGMII_PHYA_PWD;
-	regmap_write(ss->regmap[id], SGMSYS_QPHY_PWR_STATE_CTRL, val);
-
-	return 0;
 }
 
 void mtk_sgmii_restart_an(struct mtk_eth *eth, int mac_id)