[PATCH] clk: versal: Fix the function versal_clock_ref

[Venkatesh Yadav Abbarapu]

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 at amd.com>
---
 drivers/clk/clk_versal.c | 98 ++++++++++++++++++++++++++--------------
 1 file changed, 65 insertions(+), 33 deletions(-)

diff --git a/drivers/clk/clk_versal.c b/drivers/clk/clk_versal.c
index b3b3333123..2e004beca2 100644
--- 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;
-- 
2.17.1