clk: versal: Fix the function versal_clock_ref

For reference clocks, PM_CLK_GET_PARENT call is not allowed.
PM_CLK_GET_PARENT only allowed for MUX clocks. Rename the
versal_clock_ref() with versal_clock_get_ref_rate() for better
readability. Fix the versal_clock_get_ref_rate function by
passing the parent_id, and check whether the parent_id
belongs to ref_clk or pl_alt_ref_clk.
Also adding the function versal_clock_get_fixed_factor_rate()
if the clk_id belongs to the fixed factor clock.

Signed-off-by: Venkatesh Yadav Abbarapu <venkatesh.abbarapu@amd.com>
Link: https://lore.kernel.org/r/20230912033055.2549-1-venkatesh.abbarapu@amd.com
Signed-off-by: Michal Simek <michal.simek@amd.com>

diff --git a/drivers/clk/clk_versal.c b/drivers/clk/clk_versal.c
index b3b333312358fe7e0114990ed9e9ca206fc42fcb..2e004beca2f952ab32d27becd0533844af15a4e5 100644 (file)
--- a/drivers/clk/clk_versal.c
+++ b/drivers/clk/clk_versal.c
@@ -68,6 +68,13 @@
 #define CLOCK_NODE_TYPE_DIV    4
 #define CLOCK_NODE_TYPE_GATE   6
 
+#define PM_CLK_REF_CLK         (0x830c06aU)
+#define PM_CLK_PL_ALT_REF_CLK  (0x830c06bU)
+#define PM_CLK_MUXED_IRO       (0x830c06cU)
+#define PM_CLK_EMIO            (0x830c071U)
+
+#define TOPOLOGY_TYPE_FIXEDFACTOR      0x3
+
 enum clk_type {
        CLK_TYPE_OUTPUT,
        CLK_TYPE_EXTERNAL,
@@ -365,48 +372,37 @@ static u32 versal_clock_set_div(u32 clk_id, u32 div)
        return div;
 }
 
-static u64 versal_clock_ref(u32 clk_id)
+static u64 versal_clock_get_ref_rate(u32 clk_id)
 {
-       u32 ret_payload[PAYLOAD_ARG_CNT];
-       int ref;
-
-       xilinx_pm_request(PM_CLOCK_GETPARENT, clk_id, 0, 0, 0, ret_payload);
-       ref = ret_payload[0];
-       if (!(ref & 1))
+       if (clk_id == PM_CLK_REF_CLK || clk_id == PM_CLK_MUXED_IRO || clk_id == PM_CLK_EMIO)
                return ref_clk;
-       if (ref & 2)
+       else if (clk_id == PM_CLK_PL_ALT_REF_CLK)
                return pl_alt_ref_clk;
-       return 0;
+       else
+               return 0;
 }
 
-static u64 versal_clock_get_pll_rate(u32 clk_id)
+static int versal_clock_get_fixed_factor_rate(u32 clock_id, u32 parent_id)
 {
+       struct versal_pm_query_data qdata = {0};
        u32 ret_payload[PAYLOAD_ARG_CNT];
-       u32 fbdiv;
-       u32 res;
-       u32 frac;
-       u64 freq;
-       u32 parent_rate, parent_id;
-       u32 id = clk_id & 0xFFF;
+       u32 mult, div;
+       u32 parent_rate;
+       int ret;
 
-       xilinx_pm_request(PM_CLOCK_GETSTATE, clk_id, 0, 0, 0, ret_payload);
-       res = ret_payload[1];
-       if (!res) {
-               printf("0%x PLL not enabled\n", clk_id);
-               return 0;
-       }
+       qdata.qid = PM_QID_CLOCK_GET_FIXEDFACTOR_PARAMS;
+       qdata.arg1 = clock_id;
 
-       parent_id = clock[clock[id].parent[0].id].clk_id;
-       parent_rate = versal_clock_ref(parent_id);
+       ret = versal_pm_query(qdata, ret_payload);
+       if (ret)
+               return ret;
 
-       xilinx_pm_request(PM_CLOCK_GETDIVIDER, clk_id, 0, 0, 0, ret_payload);
-       fbdiv = ret_payload[1];
-       xilinx_pm_request(PM_CLOCK_PLL_GETPARAM, clk_id, 2, 0, 0, ret_payload);
-       frac = ret_payload[1];
+       mult = ret_payload[1];
+       div = ret_payload[2];
 
-       freq = (fbdiv * parent_rate) >> (1 << frac);
+       parent_rate = versal_clock_get_ref_rate(parent_id);
+       return parent_rate * mult / div;
 
-       return freq;
 }
 
 static u32 versal_clock_mux(u32 clk_id)
@@ -437,6 +433,37 @@ static u32 versal_clock_get_parentid(u32 clk_id)
        return clock[clock[id].parent[parent_id].id].clk_id;
 }
 
+static u64 versal_clock_get_pll_rate(u32 clk_id)
+{
+       u32 ret_payload[PAYLOAD_ARG_CNT];
+       u32 fbdiv;
+       u32 res;
+       u32 frac;
+       u64 freq;
+       u32 parent_rate, parent_id, parent_ref_clk_id;
+       u32 id = clk_id & 0xFFF;
+
+       xilinx_pm_request(PM_CLOCK_GETSTATE, clk_id, 0, 0, 0, ret_payload);
+       res = ret_payload[1];
+       if (!res) {
+               printf("0%x PLL not enabled\n", clk_id);
+               return 0;
+       }
+
+       parent_id = clock[clock[id].parent[0].id].clk_id;
+       parent_ref_clk_id = versal_clock_get_parentid(parent_id);
+       parent_rate = versal_clock_get_ref_rate(parent_ref_clk_id);
+
+       xilinx_pm_request(PM_CLOCK_GETDIVIDER, clk_id, 0, 0, 0, ret_payload);
+       fbdiv = ret_payload[1];
+       xilinx_pm_request(PM_CLOCK_PLL_GETPARAM, clk_id, 2, 0, 0, ret_payload);
+       frac = ret_payload[1];
+
+       freq = (fbdiv * parent_rate) >> (1 << frac);
+
+       return freq;
+}
+
 static u32 versal_clock_gate(u32 clk_id)
 {
        u32 id = clk_id & 0xFFF;
@@ -479,14 +506,19 @@ static u64 versal_clock_calc(u32 clk_id)
        u32 parent_id;
        u64 clk_rate;
        u32 div;
+       struct clock_topology topology;
 
        if (versal_clock_pll(clk_id, &clk_rate))
                return clk_rate;
 
        parent_id = versal_clock_get_parentid(clk_id);
        if (((parent_id >> NODE_SUBCLASS_SHIFT) &
-            NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_REF)
-               return versal_clock_ref(clk_id);
+            NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_REF) {
+               topology = clock[clk_id & 0x3FF].node[0];
+               if (topology.type == TOPOLOGY_TYPE_FIXEDFACTOR)
+                       return versal_clock_get_fixed_factor_rate(clk_id, parent_id);
+               return versal_clock_get_ref_rate(parent_id);
+       }
 
        if (!parent_id)
                return 0;
@@ -505,7 +537,7 @@ static int versal_clock_get_rate(u32 clk_id, u64 *clk_rate)
 {
        if (((clk_id >>  NODE_SUBCLASS_SHIFT) &
             NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_REF)
-               *clk_rate = versal_clock_ref(clk_id);
+               *clk_rate = versal_clock_get_ref_rate(clk_id);
 
        if (versal_clock_pll(clk_id, clk_rate))
                return 0;