stm32mp1: clk: add common function pll_get_fvco

the function compute the VCO PLL freq, used in
- stm32mp1_read_pll_freq()
- pll_set_rate()

Signed-off-by: Patrick Delaunay <patrick.delaunay@st.com>
# Conflicts:
#	drivers/clk/clk_stm32mp1.c

diff --git a/drivers/clk/clk_stm32mp1.c b/drivers/clk/clk_stm32mp1.c
index 10e3836257a3bc3886484fa85eb733ad4b2cb320..317526af9191c1a74f90ad5a70f3aac9779bff42 100644 (file)
--- a/drivers/clk/clk_stm32mp1.c
+++ b/drivers/clk/clk_stm32mp1.c
@@ -808,56 +808,87 @@ static int stm32mp1_clk_get_parent(struct stm32mp1_clk_priv *priv,
        return -EINVAL;
 }
 
-static ulong stm32mp1_read_pll_freq(struct stm32mp1_clk_priv *priv,
-                                   int pll_id, int div_id)
+static ulong  pll_get_fref_ck(struct stm32mp1_clk_priv *priv,
+                             int pll_id)
 {
        const struct stm32mp1_clk_pll *pll = priv->data->pll;
-       int divm, divn, divy, src;
-       ulong refclk, dfout;
-       u32 selr, cfgr1, cfgr2, fracr;
-
-       debug("%s(%d, %d)\n", __func__, pll_id, div_id);
-       if (div_id > _DIV_NB)
-               return 0;
+       u32 selr;
+       int src;
+       ulong refclk;
 
+       /* Get current refclk */
        selr = readl(priv->base + pll[pll_id].rckxselr);
+       src = selr & RCC_SELR_SRC_MASK;
+
+       refclk = stm32mp1_clk_get_fixed(priv, pll[pll_id].refclk[src]);
+       debug("PLL%d : selr=%x refclk = %d kHz\n",
+             pll_id, selr, (u32)(refclk / 1000));
+
+       return refclk;
+}
+
+/*
+ * pll_get_fvco() : return the VCO or (VCO / 2) frequency for the requested PLL
+ * - PLL1 & PLL2 => return VCO / 2 with Fpll_y_ck = FVCO / 2 * (DIVy + 1)
+ * - PLL3 & PLL4 => return VCO     with Fpll_y_ck = FVCO / (DIVy + 1)
+ * => in all the case Fpll_y_ck = pll_get_fvco() / (DIVy + 1)
+ */
+static ulong pll_get_fvco(struct stm32mp1_clk_priv *priv,
+                         int pll_id)
+{
+       const struct stm32mp1_clk_pll *pll = priv->data->pll;
+       int divm, divn;
+       ulong refclk, fvco;
+       u32 cfgr1, fracr;
+
        cfgr1 = readl(priv->base + pll[pll_id].pllxcfgr1);
-       cfgr2 = readl(priv->base + pll[pll_id].pllxcfgr2);
        fracr = readl(priv->base + pll[pll_id].pllxfracr);
 
-       debug("PLL%d : selr=%x cfgr1=%x cfgr2=%x fracr=%x\n",
-             pll_id, selr, cfgr1, cfgr2, fracr);
-
        divm = (cfgr1 & (RCC_PLLNCFGR1_DIVM_MASK)) >> RCC_PLLNCFGR1_DIVM_SHIFT;
        divn = cfgr1 & RCC_PLLNCFGR1_DIVN_MASK;
-       divy = (cfgr2 >> RCC_PLLNCFGR2_SHIFT(div_id)) & RCC_PLLNCFGR2_DIVX_MASK;
 
-       debug("        DIVN=%d DIVM=%d DIVY=%d\n", divn, divm, divy);
+       debug("PLL%d : cfgr1=%x fracr=%x DIVN=%d DIVM=%d\n",
+             pll_id, cfgr1, fracr, divn, divm);
 
-       src = selr & RCC_SELR_SRC_MASK;
-       refclk = stm32mp1_clk_get_fixed(priv, pll[pll_id].refclk[src]);
+       refclk = pll_get_fref_ck(priv, pll_id);
 
-       debug("        refclk = %d kHz\n", (u32)(refclk / 1000));
-
-       /*
-        * For: PLL1 & PLL2 => VCO is * 2 but ck_pll_y is also / 2
-        * So same final result than PLL2 et 4
-        * with FRACV :
-        *   Fck_pll_y = Fck_ref * ((DIVN + 1) + FRACV / 2^13)
-        *               / (DIVM + 1) * (DIVy + 1)
+       /* with FRACV :
+        *   Fvco = Fck_ref * ((DIVN + 1) + FRACV / 2^13) / (DIVM + 1)
         * without FRACV
-        *   Fck_pll_y = Fck_ref * ((DIVN + 1) / (DIVM + 1) *(DIVy + 1)
+        *   Fvco = Fck_ref * ((DIVN + 1) / (DIVM + 1)
         */
        if (fracr & RCC_PLLNFRACR_FRACLE) {
                u32 fracv = (fracr & RCC_PLLNFRACR_FRACV_MASK)
                            >> RCC_PLLNFRACR_FRACV_SHIFT;
-               dfout = (ulong)lldiv((unsigned long long)refclk *
+               fvco = (ulong)lldiv((unsigned long long)refclk *
                                     (((divn + 1) << 13) + fracv),
-                                    ((unsigned long long)(divm + 1) *
-                                     (divy + 1)) << 13);
+                                    ((unsigned long long)(divm + 1)) << 13);
        } else {
-               dfout = (ulong)(refclk * (divn + 1) / (divm + 1) * (divy + 1));
+               fvco = (ulong)(refclk * (divn + 1) / (divm + 1));
        }
+       debug("PLL%d : %s = %ld\n", pll_id, __func__, fvco);
+
+       return fvco;
+}
+
+static ulong stm32mp1_read_pll_freq(struct stm32mp1_clk_priv *priv,
+                                   int pll_id, int div_id)
+{
+       const struct stm32mp1_clk_pll *pll = priv->data->pll;
+       int divy;
+       ulong dfout;
+       u32 cfgr2;
+
+       debug("%s(%d, %d)\n", __func__, pll_id, div_id);
+       if (div_id >= _DIV_NB)
+               return 0;
+
+       cfgr2 = readl(priv->base + pll[pll_id].pllxcfgr2);
+       divy = (cfgr2 >> RCC_PLLNCFGR2_SHIFT(div_id)) & RCC_PLLNCFGR2_DIVX_MASK;
+
+       debug("PLL%d : cfgr2=%x DIVY=%d\n", pll_id, cfgr2, divy);
+
+       dfout = pll_get_fvco(priv, pll_id) / (divy + 1);
        debug("        => dfout = %d kHz\n", (u32)(dfout / 1000));
 
        return dfout;