[PATCH 5/8] clk: qcom: support for duplicate freq in RCG2 freq table

[Ziyang Huang <hzyitc@outlook.com>]

From: Praveenkumar I <ipkumar@codeaurora.org>

Currently RCG code looks up the frequency table during set
rate and return the first available frequency greater than
requested rate. If CLK_SET_RATE_PARENT flag is set then the
set_rate request will go to its parent otherwise the clock
framework will configure pre-div, m and n according to the
returned frequency table entry. In this case, it is assuming
that parent clock will run in the same frequency with which
pre-div, m and n has been derived. But it may be possible
that the parent clock supports multiple frequency and the
same frequency can be derived with different pre-div, m and
n values depending upon current frequency.  Also, the same
frequency can be derived from different parent sources and
currently there is no option for having duplicate
frequencies in frequency table and choosing the best one
according to current rate.

Now this patch adds the support for having duplicate
frequencies in frequency table. During set rate, it will
compare the actual rate for each entry with requested rate
and will select the best entry in which the difference will
be less.

The existing functionality won’t be affected with this code
change since this code change will hit only if frequency
table has duplicate values.

Change-Id: I97d9e1b55d8f3ee095f6f01729af527ba90e50e5
Signed-off-by: Abhishek Sahu <absahu@codeaurora.org>
(cherry picked from commit 775e7d3b69ffc97afb5bd5a6c9c423f2f4d8a0b2)
Signed-off-by: Praveenkumar I <ipkumar@codeaurora.org>

Change-Id: If10193fc79a3c1375ab73597813745ff1f4df0ad

Pick from https://git.codelinaro.org/clo/qsdk/oss/kernel/linux-ipq-5.4/-/commit/6dfb368bae130bee58e00ddf8330b55066e1c8c5

Signed-off-by: Ziyang Huang <hzyitc@outlook.com>
---
 drivers/clk/qcom/clk-rcg2.c | 86 ++++++++++++++++++++++++++++---------
 1 file changed, 66 insertions(+), 20 deletions(-)

diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index e22baf3a7112..6141e4991fbc 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -209,26 +209,82 @@ clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 	return __clk_rcg2_recalc_rate(hw, parent_rate, cfg);
 }
 
-static int _freq_tbl_determine_rate(struct clk_hw *hw, const struct freq_tbl *f,
-				    struct clk_rate_request *req,
-				    enum freq_policy policy)
+static const struct freq_tbl *
+clk_rcg2_find_best_freq(struct clk_hw *hw, const struct freq_tbl *f,
+			unsigned long rate, enum freq_policy policy)
 {
-	unsigned long clk_flags, rate = req->rate;
-	struct clk_hw *p;
+	unsigned long req_rate = rate, best = 0, freq;
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	int index;
+	u64 tmp;
+	const struct freq_tbl *best_ftable = NULL;
 
 	switch (policy) {
 	case FLOOR:
-		f = qcom_find_freq_floor(f, rate);
+		f = qcom_find_freq_floor(rcg->freq_tbl, rate);
 		break;
 	case CEIL:
-		f = qcom_find_freq(f, rate);
+		f = qcom_find_freq(rcg->freq_tbl, rate);
 		break;
 	default:
-		return -EINVAL;
+		return best_ftable;
 	}
 
+	/*
+	 * Check for duplicate frequencies in frequency table if
+	 * CLK_SET_RATE_PARENT flag is not set
+	 */
+	if (!f || (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) ||
+	    ((f->freq && (f + 1)->freq != f->freq)))
+		return f;
+
+	/*
+	 * Check for all the duplicate entries in frequency table and
+	 * calculate the actual rate from current parent rate with each
+	 * entries pre_div, m and n values. The entry, which gives the
+	 * minimum difference in requested rate and actual rate, will be
+	 * selected as the best one.
+	 */
+	for (freq = f->freq; freq == f->freq; f++) {
+		index = qcom_find_src_index(hw, rcg->parent_map, f->src);
+		if (index < 0)
+			continue;
+
+		rate =  clk_hw_get_rate(clk_hw_get_parent_by_index(hw, index));
+		if (rcg->hid_width && f->pre_div) {
+			rate *= 2;
+			rate /= f->pre_div + 1;
+		}
+
+		if (rcg->mnd_width && f->n) {
+			tmp = rate;
+			tmp = tmp * f->n;
+			do_div(tmp, f->m);
+			rate = tmp;
+		}
+
+		if (abs(req_rate - rate) < abs(best - rate)) {
+			best_ftable = f;
+			best = rate;
+
+			if (req_rate == rate)
+				break;
+		}
+	}
+
+	return best_ftable;
+}
+
+static int _freq_tbl_determine_rate(struct clk_hw *hw, const struct freq_tbl *f,
+				    struct clk_rate_request *req,
+				    enum freq_policy policy)
+{
+	unsigned long clk_flags, rate = req->rate;
+	struct clk_hw *p;
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	int index;
+
+	f = clk_rcg2_find_best_freq(hw, f, rate, policy);
 	if (!f)
 		return -EINVAL;
 
@@ -360,17 +416,7 @@ static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	const struct freq_tbl *f;
 
-	switch (policy) {
-	case FLOOR:
-		f = qcom_find_freq_floor(rcg->freq_tbl, rate);
-		break;
-	case CEIL:
-		f = qcom_find_freq(rcg->freq_tbl, rate);
-		break;
-	default:
-		return -EINVAL;
-	}
-
+	f = clk_rcg2_find_best_freq(hw, rcg->freq_tbl, rate, policy);
 	if (!f)
 		return -EINVAL;
 
@@ -1032,7 +1078,7 @@ static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate,
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	const struct freq_tbl *f;
 
-	f = qcom_find_freq(rcg->freq_tbl, rate);
+	f = clk_rcg2_find_best_freq(hw, rcg->freq_tbl, rate, CEIL);
 	if (!f)
 		return -EINVAL;
 
-- 
2.40.1