From 3b3254cfec0137bb90a44f4d4bb1259241dc3a7b Mon Sep 17 00:00:00 2001
From: Wyon Bi <bivvy.bi@rock-chips.com>
Date: Mon, 28 Mar 2022 09:19:23 +0000
Subject: [PATCH] phy/rockchip: samsung-hdptx: Support DT specified training
 table

Signed-off-by: Wyon Bi <bivvy.bi@rock-chips.com>
Change-Id: I70680470ecc7aecb79cc5d35820cf8c983b542c3
---
 .../phy/rockchip/phy-rockchip-samsung-hdptx.c | 56 ++++++++++++++++---
 1 file changed, 47 insertions(+), 9 deletions(-)

diff --git a/drivers/phy/rockchip/phy-rockchip-samsung-hdptx.c b/drivers/phy/rockchip/phy-rockchip-samsung-hdptx.c
index 2aee0c9b6684..a3c9619b7cc6 100644
--- a/drivers/phy/rockchip/phy-rockchip-samsung-hdptx.c
+++ b/drivers/phy/rockchip/phy-rockchip-samsung-hdptx.c
@@ -348,9 +348,9 @@ struct tx_drv_ctrl {
 	u8 ana_tx_drv_idrv_iup_ctrl;
 	u8 ana_tx_drv_accdrv_en;
 	u8 ana_tx_drv_accdrv_ctrl;
-};
+} __packed;
 
-static const struct tx_drv_ctrl tx_drv_ctrl_rbr[4][4] = {
+static struct tx_drv_ctrl tx_drv_ctrl_rbr[4][4] = {
 	/* voltage swing 0, pre-emphasis 0->3 */
 	{
 		{ 0x1, 0x0, 0x4, 0x6, 0x0, 0x4 },
@@ -378,7 +378,7 @@ static const struct tx_drv_ctrl tx_drv_ctrl_rbr[4][4] = {
 	}
 };
 
-static const struct tx_drv_ctrl tx_drv_ctrl_hbr[4][4] = {
+static struct tx_drv_ctrl tx_drv_ctrl_hbr[4][4] = {
 	/* voltage swing 0, pre-emphasis 0->3 */
 	{
 		{ 0x2, 0x1, 0x4, 0x6, 0x0, 0x4 },
@@ -406,7 +406,7 @@ static const struct tx_drv_ctrl tx_drv_ctrl_hbr[4][4] = {
 	}
 };
 
-static const struct tx_drv_ctrl tx_drv_ctrl_hbr2[4][4] = {
+static struct tx_drv_ctrl tx_drv_ctrl_hbr2[4][4] = {
 	/* voltage swing 0, pre-emphasis 0->3 */
 	{
 		{ 0x2, 0x1, 0x4, 0x6, 0x0, 0x4 },
@@ -434,6 +434,43 @@ static const struct tx_drv_ctrl tx_drv_ctrl_hbr2[4][4] = {
 	}
 };
 
+static int rockchip_hdptx_phy_parse_training_table(struct device *dev)
+{
+	size_t size = sizeof(struct tx_drv_ctrl) * 10;
+	u8 *buf, *training_table;
+	int i, j;
+
+	buf = kzalloc(size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	if (device_property_read_u8_array(dev, "training-table", buf, size)) {
+		kfree(buf);
+		return 0;
+	}
+
+	training_table = buf;
+
+	for (i = 0; i < 4; i++) {
+		for (j = 0; j < 4; j++) {
+			struct tx_drv_ctrl *ctrl;
+
+			if (i + j > 3)
+				continue;
+
+			ctrl = (struct tx_drv_ctrl *)training_table;
+			tx_drv_ctrl_rbr[i][j] = *ctrl;
+			tx_drv_ctrl_hbr[i][j] = *ctrl;
+			tx_drv_ctrl_hbr2[i][j] = *ctrl;
+			training_table += sizeof(*ctrl);
+		}
+	}
+
+	kfree(buf);
+
+	return 0;
+}
+
 static int rockchip_grf_write(struct regmap *grf, unsigned int reg,
 			      unsigned int mask, unsigned int val)
 {
@@ -1160,12 +1197,13 @@ static int rockchip_hdptx_phy_probe(struct platform_device *pdev)
 	device_property_read_u32_array(dev, "lane-polarity-invert",
 				       hdptx->lane_polarity_invert, 4);
 
+	ret = rockchip_hdptx_phy_parse_training_table(dev);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to parse training table\n");
+
 	phy = devm_phy_create(dev, NULL, &rockchip_hdptx_phy_ops);
-	if (IS_ERR(phy)) {
-		ret = PTR_ERR(phy);
-		dev_err(dev, "failed to create PHY: %d\n", ret);
-		return ret;
-	}
+	if (IS_ERR(phy))
+		return dev_err_probe(dev, PTR_ERR(phy), "failed to create PHY\n");
 
 	phy_set_drvdata(phy, hdptx);