[PATCH net-next v2 3/8] net: phy: meson-gxl: add read and write helpers for bank registers

[jbrunet@baylibre.com (Jerome Brunet)]

Add read and write helpers to manipulate banked registers on this PHY
This helps clarify the settings applied to these registers in the init
function and upcoming changes.

Signed-off-by: Jerome Brunet <jbrunet@baylibre.com>
---
 drivers/net/phy/meson-gxl.c | 103 ++++++++++++++++++++++++++++----------------
 1 file changed, 67 insertions(+), 36 deletions(-)

diff --git a/drivers/net/phy/meson-gxl.c b/drivers/net/phy/meson-gxl.c
index d82aa8cea401..05054770aefb 100644
--- a/drivers/net/phy/meson-gxl.c
+++ b/drivers/net/phy/meson-gxl.c
@@ -45,11 +45,13 @@
 #define FR_PLL_DIV0	0x1c
 #define FR_PLL_DIV1	0x1d
 
-static int meson_gxl_config_init(struct phy_device *phydev)
+static int meson_gxl_open_banks(struct phy_device *phydev)
 {
 	int ret;
 
-	/* Enable Analog and DSP register Bank access by */
+	/* Enable Analog and DSP register Bank access by
+	 * toggling TSTCNTL_TEST_MODE bit in the TSTCNTL register
+	 */
 	ret = phy_write(phydev, TSTCNTL, 0);
 	if (ret)
 		return ret;
@@ -59,55 +61,84 @@ static int meson_gxl_config_init(struct phy_device *phydev)
 	ret = phy_write(phydev, TSTCNTL, 0);
 	if (ret)
 		return ret;
-	ret = phy_write(phydev, TSTCNTL, TSTCNTL_TEST_MODE);
-	if (ret)
-		return ret;
+	return phy_write(phydev, TSTCNTL, TSTCNTL_TEST_MODE);
+}
 
-	/* Write CONFIG_A6*/
-	ret = phy_write(phydev, TSTWRITE, 0x8e0d)
+static void meson_gxl_close_banks(struct phy_device *phydev)
+{
+	phy_write(phydev, TSTCNTL, 0);
+}
+
+static int meson_gxl_read_reg(struct phy_device *phydev,
+			      unsigned int bank, unsigned int reg)
+{
+	int ret;
+
+	ret = meson_gxl_open_banks(phydev);
 	if (ret)
-		return ret;
-	ret = phy_write(phydev, TSTCNTL,
-			TSTCNTL_WRITE
-			| FIELD_PREP(TSTCNTL_REG_BANK_SEL, BANK_ANALOG_DSP)
-			| TSTCNTL_TEST_MODE
-			| FIELD_PREP(TSTCNTL_WRITE_ADDRESS, A6_CONFIG_REG));
+		goto out;
+
+	ret = phy_write(phydev, TSTCNTL, TSTCNTL_READ |
+			FIELD_PREP(TSTCNTL_REG_BANK_SEL, bank) |
+			TSTCNTL_TEST_MODE |
+			FIELD_PREP(TSTCNTL_READ_ADDRESS, reg));
 	if (ret)
-		return ret;
+		goto out;
 
-	/* Enable fractional PLL */
-	ret = phy_write(phydev, TSTWRITE, 0x0005);
+	ret = phy_read(phydev, TSTREAD1);
+out:
+	/* Close the bank access on our way out */
+	meson_gxl_close_banks(phydev);
+	return ret;
+}
+
+static int meson_gxl_write_reg(struct phy_device *phydev,
+			       unsigned int bank, unsigned int reg,
+			       uint16_t value)
+{
+	int ret;
+
+	ret = meson_gxl_open_banks(phydev);
 	if (ret)
-		return ret;
-	ret = phy_write(phydev, TSTCNTL,
-			TSTCNTL_WRITE
-			| FIELD_PREP(TSTCNTL_REG_BANK_SEL, BANK_BIST)
-			| TSTCNTL_TEST_MODE
-			| FIELD_PREP(TSTCNTL_WRITE_ADDRESS, FR_PLL_CONTROL));
+		goto out;
+
+	ret = phy_write(phydev, TSTWRITE, value);
 	if (ret)
-		return ret;
+		goto out;
 
-	/* Program fraction FR_PLL_DIV1 */
-	ret = phy_write(phydev, TSTWRITE, 0x029a);
+	ret = phy_write(phydev, TSTCNTL, TSTCNTL_WRITE |
+			FIELD_PREP(TSTCNTL_REG_BANK_SEL, bank) |
+			TSTCNTL_TEST_MODE |
+			FIELD_PREP(TSTCNTL_WRITE_ADDRESS, reg));
+
+out:
+	/* Close the bank access on our way out */
+	meson_gxl_close_banks(phydev);
+	return ret;
+}
+
+static int meson_gxl_config_init(struct phy_device *phydev)
+{
+	int ret;
+
+	/* Write CONFIG_A6*/
+	ret = meson_gxl_write_reg(phydev, BANK_ANALOG_DSP, A6_CONFIG_REG,
+				  0x8e0d);
 	if (ret)
 		return ret;
-	ret = phy_write(phydev, TSTCNTL,
-			TSTCNTL_WRITE
-			| FIELD_PREP(TSTCNTL_REG_BANK_SEL, BANK_BIST)
-			| TSTCNTL_TEST_MODE
-			| FIELD_PREP(TSTCNTL_WRITE_ADDRESS, FR_PLL_DIV1));
+
+	/* Enable fractional PLL */
+	ret = meson_gxl_write_reg(phydev, BANK_BIST, FR_PLL_CONTROL, 0x5);
 	if (ret)
 		return ret;
 
 	/* Program fraction FR_PLL_DIV1 */
-	ret = phy_write(phydev, TSTWRITE, 0xaaaa);
+	ret = meson_gxl_write_reg(phydev, BANK_BIST, FR_PLL_DIV1, 0x029a);
 	if (ret)
 		return ret;
-	ret = phy_write(phydev, TSTCNTL,
-			TSTCNTL_WRITE
-			| FIELD_PREP(TSTCNTL_REG_BANK_SEL, BANK_BIST)
-			| TSTCNTL_TEST_MODE
-			| FIELD_PREP(TSTCNTL_WRITE_ADDRESS, FR_PLL_DIV0));
+
+	/* Program fraction FR_PLL_DIV1 */
+	ret = meson_gxl_write_reg(phydev, BANK_BIST, FR_PLL_DIV0, 0xaaaa);
 	if (ret)
 		return ret;
 
-- 
2.14.3