phy: qcom: qmp-combo: extract common function to setup clocks

Extact qmp_combo_configure_dp_clocks(), a common function to setup PHY
clocks depending on the selected link rate.

Signed-off-by: Dmitry Baryshkov <dmitry.baryshkov@linaro.org>
Link: https://lore.kernel.org/r/20230621153317.1025914-8-dmitry.baryshkov@linaro.org
Signed-off-by: Vinod Koul <vkoul@kernel.org>

diff --git a/drivers/phy/qualcomm/phy-qcom-qmp-combo.c b/drivers/phy/qualcomm/phy-qcom-qmp-combo.c
index 61a1f7c..36eb516 100644 (file)
--- a/drivers/phy/qualcomm/phy-qcom-qmp-combo.c
+++ b/drivers/phy/qualcomm/phy-qcom-qmp-combo.c
@@ -2074,18 +2074,12 @@ static bool qmp_combo_configure_dp_mode(struct qmp_combo *qmp)
        return reverse;
 }
 
-static int qmp_v3_configure_dp_phy(struct qmp_combo *qmp)
+static int qmp_combo_configure_dp_clocks(struct qmp_combo *qmp)
 {
        const struct phy_configure_opts_dp *dp_opts = &qmp->dp_opts;
-       const struct qmp_phy_cfg *cfg = qmp->cfg;
-       u32 phy_vco_div, status;
+       u32 phy_vco_div;
        unsigned long pixel_freq;
 
-       qmp_combo_configure_dp_mode(qmp);
-
-       writel(0x05, qmp->dp_dp_phy + QSERDES_V3_DP_PHY_TX0_TX1_LANE_CTL);
-       writel(0x05, qmp->dp_dp_phy + QSERDES_V3_DP_PHY_TX2_TX3_LANE_CTL);
-
        switch (dp_opts->link_rate) {
        case 1620:
                phy_vco_div = 0x1;
@@ -2107,11 +2101,29 @@ static int qmp_v3_configure_dp_phy(struct qmp_combo *qmp)
                /* Other link rates aren't supported */
                return -EINVAL;
        }
-       writel(phy_vco_div, qmp->dp_dp_phy + QSERDES_V3_DP_PHY_VCO_DIV);
+       writel(phy_vco_div, qmp->dp_dp_phy + QSERDES_V4_DP_PHY_VCO_DIV);
 
        clk_set_rate(qmp->dp_link_hw.clk, dp_opts->link_rate * 100000);
        clk_set_rate(qmp->dp_pixel_hw.clk, pixel_freq);
 
+       return 0;
+}
+
+static int qmp_v3_configure_dp_phy(struct qmp_combo *qmp)
+{
+       const struct qmp_phy_cfg *cfg = qmp->cfg;
+       u32 status;
+       int ret;
+
+       qmp_combo_configure_dp_mode(qmp);
+
+       writel(0x05, qmp->dp_dp_phy + QSERDES_V3_DP_PHY_TX0_TX1_LANE_CTL);
+       writel(0x05, qmp->dp_dp_phy + QSERDES_V3_DP_PHY_TX2_TX3_LANE_CTL);
+
+       ret = qmp_combo_configure_dp_clocks(qmp);
+       if (ret)
+               return ret;
+
        writel(0x04, qmp->dp_dp_phy + QSERDES_DP_PHY_AUX_CFG2);
        writel(0x01, qmp->dp_dp_phy + QSERDES_DP_PHY_CFG);
        writel(0x05, qmp->dp_dp_phy + QSERDES_DP_PHY_CFG);
@@ -2210,10 +2222,9 @@ static void qmp_v4_configure_dp_tx(struct qmp_combo *qmp)
 
 static int qmp_v456_configure_dp_phy(struct qmp_combo *qmp)
 {
-       const struct phy_configure_opts_dp *dp_opts = &qmp->dp_opts;
        const struct qmp_phy_cfg *cfg = qmp->cfg;
-       u32 phy_vco_div, status;
-       unsigned long pixel_freq;
+       u32 status;
+       int ret;
 
        writel(0x0f, qmp->dp_dp_phy + QSERDES_V4_DP_PHY_CFG_1);
 
@@ -2225,31 +2236,9 @@ static int qmp_v456_configure_dp_phy(struct qmp_combo *qmp)
        writel(0x05, qmp->dp_dp_phy + QSERDES_V4_DP_PHY_TX0_TX1_LANE_CTL);
        writel(0x05, qmp->dp_dp_phy + QSERDES_V4_DP_PHY_TX2_TX3_LANE_CTL);
 
-       switch (dp_opts->link_rate) {
-       case 1620:
-               phy_vco_div = 0x1;
-               pixel_freq = 1620000000UL / 2;
-               break;
-       case 2700:
-               phy_vco_div = 0x1;
-               pixel_freq = 2700000000UL / 2;
-               break;
-       case 5400:
-               phy_vco_div = 0x2;
-               pixel_freq = 5400000000UL / 4;
-               break;
-       case 8100:
-               phy_vco_div = 0x0;
-               pixel_freq = 8100000000UL / 6;
-               break;
-       default:
-               /* Other link rates aren't supported */
-               return -EINVAL;
-       }
-       writel(phy_vco_div, qmp->dp_dp_phy + QSERDES_V4_DP_PHY_VCO_DIV);
-
-       clk_set_rate(qmp->dp_link_hw.clk, dp_opts->link_rate * 100000);
-       clk_set_rate(qmp->dp_pixel_hw.clk, pixel_freq);
+       ret = qmp_combo_configure_dp_clocks(qmp);
+       if (ret)
+               return ret;
 
        writel(0x01, qmp->dp_dp_phy + QSERDES_DP_PHY_CFG);
        writel(0x05, qmp->dp_dp_phy + QSERDES_DP_PHY_CFG);