[net-next,v2,4/4] net: dp83869: Add RGMII internal delay configuration

diff --git a/drivers/net/phy/dp83869.c b/drivers/net/phy/dp83869.c
index cfb22a21a2e6..a9008d32e2b6 100644
--- a/drivers/net/phy/dp83869.c
+++ b/drivers/net/phy/dp83869.c
@@ -99,6 +99,14 @@ 
 #define DP83869_OP_MODE_MII			BIT(5)
 #define DP83869_SGMII_RGMII_BRIDGE		BIT(6)
 
+/* RGMIIDCTL bits */
+#define DP83869_RGMII_TX_CLK_DELAY_SHIFT	4
+#define DP83869_RGMII_CLK_DELAY_INV		0
+
+static int dp83869_internal_delay[] = {250, 500, 750, 1000, 1250, 1500, 1750,
+				       2000, 2250, 2500, 2750, 3000, 3250,
+				       3500, 3750, 4000};
+
 enum {
 	DP83869_PORT_MIRRORING_KEEP,
 	DP83869_PORT_MIRRORING_EN,
@@ -108,6 +116,8 @@  enum {
 struct dp83869_private {
 	int tx_fifo_depth;
 	int rx_fifo_depth;
+	u32 rx_id_delay;
+	u32 tx_id_delay;
 	int io_impedance;
 	int port_mirroring;
 	bool rxctrl_strap_quirk;
@@ -182,6 +192,7 @@  static int dp83869_of_init(struct phy_device *phydev)
 	struct dp83869_private *dp83869 = phydev->priv;
 	struct device *dev = &phydev->mdio.dev;
 	struct device_node *of_node = dev->of_node;
+	int delay_size = ARRAY_SIZE(dp83869_internal_delay);
 	int ret;
 
 	if (!of_node)
@@ -232,6 +243,26 @@  static int dp83869_of_init(struct phy_device *phydev)
 				 &dp83869->tx_fifo_depth))
 		dp83869->tx_fifo_depth = DP83869_PHYCR_FIFO_DEPTH_4_B_NIB;
 
+	dp83869->rx_id_delay = DP83869_RGMII_CLK_DELAY_INV;
+	ret = of_property_read_u32(of_node, "rx-internal-delay-ps",
+				   &dp83869->rx_id_delay);
+	if (!ret && dp83869->rx_id_delay > dp83869_internal_delay[delay_size]) {
+		phydev_err(phydev,
+			   "rx-internal-delay value of %u out of range\n",
+			   dp83869->rx_id_delay);
+		return -EINVAL;
+	}
+
+	dp83869->tx_id_delay = DP83869_RGMII_CLK_DELAY_INV;
+	ret = of_property_read_u32(of_node, "tx-internal-delay-ps",
+				   &dp83869->tx_id_delay);
+	if (!ret && dp83869->tx_id_delay > dp83869_internal_delay[delay_size]) {
+		phydev_err(phydev,
+			   "tx-internal-delay value of %u out of range\n",
+			   dp83869->tx_id_delay);
+		return -EINVAL;
+	}
+
 	return ret;
 }
 #else
@@ -270,6 +301,29 @@  static int dp83869_configure_rgmii(struct phy_device *phydev,
 	return ret;
 }
 
+static int dp83869_verify_rgmii_cfg(struct phy_device *phydev)
+{
+	struct dp83869_private *dp83869 = phydev->priv;
+
+	/* RX delay *must* be specified if internal delay of RX is used. */
+	if ((phydev->interface == PHY_INTERFACE_MODE_RGMII_ID ||
+	     phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID) &&
+	     dp83869->rx_id_delay == DP83869_RGMII_CLK_DELAY_INV) {
+		phydev_err(phydev, "ti,rx-internal-delay must be specified\n");
+		return -EINVAL;
+	}
+
+	/* TX delay *must* be specified if internal delay of TX is used. */
+	if ((phydev->interface == PHY_INTERFACE_MODE_RGMII_ID ||
+	     phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID) &&
+	     dp83869->tx_id_delay == DP83869_RGMII_CLK_DELAY_INV) {
+		phydev_err(phydev, "ti,tx-internal-delay must be specified\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int dp83869_configure_mode(struct phy_device *phydev,
 				  struct dp83869_private *dp83869)
 {
@@ -371,6 +425,12 @@  static int dp83869_config_init(struct phy_device *phydev)
 {
 	struct dp83869_private *dp83869 = phydev->priv;
 	int ret, val;
+	int delay_size = ARRAY_SIZE(dp83869_internal_delay);
+	int delay = 0;
+
+	ret = dp83869_verify_rgmii_cfg(phydev);
+	if (ret)
+		return ret;
 
 	ret = dp83869_configure_mode(phydev, dp83869);
 	if (ret)
@@ -394,6 +454,47 @@  static int dp83869_config_init(struct phy_device *phydev)
 				     dp83869->clk_output_sel <<
 				     DP83869_IO_MUX_CFG_CLK_O_SEL_SHIFT);
 
+	if (phy_interface_is_rgmii(phydev)) {
+		val = phy_read_mmd(phydev, DP83869_DEVADDR, DP83869_RGMIICTL);
+
+		val &= ~(DP83869_RGMII_TX_CLK_DELAY_EN | DP83869_RGMII_RX_CLK_DELAY_EN);
+		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID)
+			val |= (DP83869_RGMII_TX_CLK_DELAY_EN | DP83869_RGMII_RX_CLK_DELAY_EN);
+
+		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)
+			val |= DP83869_RGMII_TX_CLK_DELAY_EN;
+
+		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID)
+			val |= DP83869_RGMII_RX_CLK_DELAY_EN;
+
+		phy_write_mmd(phydev, DP83869_DEVADDR, DP83869_RGMIICTL, val);
+
+		if (dp83869->rx_id_delay) {
+			val = phy_get_delay_index(phydev,
+						  &dp83869_internal_delay[0],
+						  delay_size,
+						  dp83869->rx_id_delay);
+			if (val < 0)
+				return val;
+
+			delay |= val;
+		}
+
+		if (dp83869->tx_id_delay) {
+			val = phy_get_delay_index(phydev,
+						  &dp83869_internal_delay[0],
+						  delay_size,
+						  dp83869->tx_id_delay);
+			if (val < 0)
+				return val;
+
+			delay |= val << DP83869_RGMII_TX_CLK_DELAY_SHIFT;
+		}
+
+		phy_write_mmd(phydev, DP83869_DEVADDR, DP83869_RGMIIDCTL,
+			      delay);
+	}
+
 	return ret;
 }