From 9cfb2d426c38272f245e9e6f62b3552d1ed5852b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ren=C3=A9=20van=20Dorst?= <opensource@vdorst.com>
Date: Tue, 21 Apr 2020 00:18:08 +0200
Subject: [PATCH] net: dsa: mt7530: Support EEE features
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: René van Dorst <opensource@vdorst.com>
--- a/drivers/net/dsa/mt7530.c
+++ b/drivers/net/dsa/mt7530.c
@@ -1408,9 +1408,13 @@ static void mt7530_phylink_mac_config(st
 	switch (state->speed) {
 	case SPEED_1000:
 		mcr_new |= PMCR_FORCE_SPEED_1000;
+		if (priv->eee_enable & BIT(port))
+			mcr_new |= PMCR_FORCE_EEE1G;
 		break;
 	case SPEED_100:
 		mcr_new |= PMCR_FORCE_SPEED_100;
+		if (priv->eee_enable & BIT(port))
+			mcr_new |= PMCR_FORCE_EEE100;
 		break;
 	}
 	if (state->duplex == DUPLEX_FULL) {
@@ -1546,6 +1550,54 @@ mt7530_phylink_mac_link_state(struct dsa
 	return 1;
 }
 
+static int mt7530_get_mac_eee(struct dsa_switch *ds, int port,
+			      struct ethtool_eee *e)
+{
+	struct mt7530_priv *priv = ds->priv;
+	u32 eeecr, pmsr;
+
+	e->eee_enabled = !!(priv->eee_enable & BIT(port));
+
+	if (e->eee_enabled) {
+		eeecr = mt7530_read(priv, MT7530_PMEEECR_P(port));
+		e->tx_lpi_enabled = !(eeecr & LPI_MODE_EN);
+		e->tx_lpi_timer   = (eeecr >> 4) & 0xFFF;
+		pmsr = mt7530_read(priv, MT7530_PMSR_P(port));
+		e->eee_active  = e->eee_enabled && !!(pmsr & PMSR_EEE1G);
+	} else {
+		e->tx_lpi_enabled = 0;
+		e->tx_lpi_timer = 0;
+		e->eee_active = 0;
+	}
+
+	return 0;
+}
+
+static int mt7530_set_mac_eee(struct dsa_switch *ds, int port,
+			      struct ethtool_eee *e)
+{
+	struct mt7530_priv *priv = ds->priv;
+	u32 eeecr;
+
+	if (e->tx_lpi_enabled && e->tx_lpi_timer > 0xFFF)
+		return -EINVAL;
+
+	if (e->eee_enabled) {
+		priv->eee_enable |= BIT(port);
+		//MT7530_PMEEECR_P
+		eeecr = mt7530_read(priv, MT7530_PMEEECR_P(port));
+		eeecr &= 0xFFFF0000;
+		if (!e->tx_lpi_enabled)
+			eeecr |= LPI_MODE_EN;
+		eeecr |= LPI_THRESH(e->tx_lpi_timer);
+		mt7530_write(priv, MT7530_PMEEECR_P(port), eeecr);
+	} else {
+		priv->eee_enable &= ~(BIT(port));
+	}
+
+	return 0;
+}
+
 static const struct dsa_switch_ops mt7530_switch_ops = {
 	.get_tag_protocol	= mtk_get_tag_protocol,
 	.setup			= mt7530_setup,
@@ -1573,6 +1625,8 @@ static const struct dsa_switch_ops mt753
 	.phylink_mac_config	= mt7530_phylink_mac_config,
 	.phylink_mac_link_down	= mt7530_phylink_mac_link_down,
 	.phylink_mac_link_up	= mt7530_phylink_mac_link_up,
+	.get_mac_eee		= mt7530_get_mac_eee,
+	.set_mac_eee		= mt7530_set_mac_eee,
 };
 
 static const struct of_device_id mt7530_of_match[] = {
--- a/drivers/net/dsa/mt7530.h
+++ b/drivers/net/dsa/mt7530.h
@@ -212,6 +212,8 @@ enum mt7530_vlan_port_attr {
 #define  PMCR_RX_EN			BIT(13)
 #define  PMCR_BACKOFF_EN		BIT(9)
 #define  PMCR_BACKPR_EN			BIT(8)
+#define  PMCR_FORCE_EEE1G		BIT(7)
+#define  PMCR_FORCE_EEE100		BIT(6)
 #define  PMCR_TX_FC_EN			BIT(5)
 #define  PMCR_RX_FC_EN			BIT(4)
 #define  PMCR_FORCE_SPEED_1000		BIT(3)
@@ -233,6 +235,12 @@ enum mt7530_vlan_port_attr {
 #define  PMSR_DPX			BIT(1)
 #define  PMSR_LINK			BIT(0)
 
+#define MT7530_PMEEECR_P(x)		(0x3004 + (x) * 0x100)
+#define  WAKEUP_TIME_1000(x)		((x & 0xFF) << 24)
+#define  WAKEUP_TIME_100(x)		((x & 0xFF) << 16)
+#define  LPI_THRESH(x)			((x & 0xFFF) << 4)
+#define  LPI_MODE_EN			BIT(0)
+
 /* Register for MIB */
 #define MT7530_PORT_MIB_COUNTER(x)	(0x4000 + (x) * 0x100)
 #define MT7530_MIB_CCR			0x4fe0
@@ -471,6 +479,7 @@ struct mt7530_priv {
 	unsigned int		p5_intf_sel;
 	u8			mirror_rx;
 	u8			mirror_tx;
+	u8			eee_enable;
 
 	struct mt7530_port	ports[MT7530_NUM_PORTS];
 	/* protect among processes for registers access*/