[PATCH 4/4] net: dsa: b53: mmap: Implement phy_enable for BCM63268 gphy

[Kyle Hendry <kylehendrydev@gmail.com>]

Add register defines for gphy control register. When gphy is
enabled, disable low power mode.

Signed-off-by: Kyle Hendry <kylehendrydev@gmail.com>
---
 drivers/net/dsa/b53/b53_mmap.c | 34 ++++++++++++++++++++++++++++++++++
 drivers/net/dsa/b53/b53_regs.h |  7 +++++++
 2 files changed, 41 insertions(+)

diff --git a/drivers/net/dsa/b53/b53_mmap.c b/drivers/net/dsa/b53/b53_mmap.c
index 8157f9871133..ac0b8fc7eb4e 100644
--- a/drivers/net/dsa/b53/b53_mmap.c
+++ b/drivers/net/dsa/b53/b53_mmap.c
@@ -230,6 +230,38 @@ static int b53_mmap_phy_write16(struct b53_device *dev, int addr, int reg,
 	return -EIO;
 }
 
+static void bcm63268_gphy_set(struct b53_device *dev, bool enable)
+{
+	struct b53_mmap_priv *priv = dev->priv;
+	void __iomem *gphy_ctrl = priv->gphy_ctrl;
+	u32 val;
+
+	val = ioread32be(gphy_ctrl);
+
+	if (enable)
+		val &= ~(GPHY_CTRL_IDDQ_BIAS | GPHY_CTRL_LOW_PWR);
+	else
+		val |= GPHY_CTRL_IDDQ_BIAS | GPHY_CTRL_LOW_PWR;
+
+	iowrite32be(val, gphy_ctrl);
+}
+
+static void b53_mmap_phy_enable(struct b53_device *dev, int port)
+{
+	struct b53_mmap_priv *priv = dev->priv;
+
+	if ((dev->internal_gphy_mask & BIT(port)) && priv->gphy_ctrl)
+		bcm63268_gphy_set(dev, true);
+}
+
+static void b53_mmap_phy_disable(struct b53_device *dev, int port)
+{
+	struct b53_mmap_priv *priv = dev->priv;
+
+	if ((dev->internal_gphy_mask & BIT(port)) && priv->gphy_ctrl)
+		bcm63268_gphy_set(dev, false);
+}
+
 static const struct b53_io_ops b53_mmap_ops = {
 	.read8 = b53_mmap_read8,
 	.read16 = b53_mmap_read16,
@@ -243,6 +275,8 @@ static const struct b53_io_ops b53_mmap_ops = {
 	.write64 = b53_mmap_write64,
 	.phy_read16 = b53_mmap_phy_read16,
 	.phy_write16 = b53_mmap_phy_write16,
+	.phy_enable = b53_mmap_phy_enable,
+	.phy_disable = b53_mmap_phy_disable,
 };
 
 static int b53_mmap_probe_of(struct platform_device *pdev,
diff --git a/drivers/net/dsa/b53/b53_regs.h b/drivers/net/dsa/b53/b53_regs.h
index bfbcb66bef66..9607b28bbb86 100644
--- a/drivers/net/dsa/b53/b53_regs.h
+++ b/drivers/net/dsa/b53/b53_regs.h
@@ -525,4 +525,11 @@
 /* CFP Control Register with ports map (8 bit) */
 #define B53_CFP_CTRL			0x00
 
+/*************************************************************************
+ * Gigabit PHY Control Register
+ *************************************************************************/
+
+#define GPHY_CTRL_IDDQ_BIAS		BIT(0)
+#define GPHY_CTRL_LOW_PWR		BIT(3)
+
 #endif /* !__B53_REGS_H */
-- 
2.43.0