[PATCH 5.15] Revert "drivers: clk: zynqmp: update divider round rate logic"

[jguittet.opensource@xxxxxxxxxxx]

From: Joel Guittet <jguittet@xxxxxxxxxxx>

This reverts commit 9117fc44fd3a9538261e530ba5a022dfc9519620 which is
commit 1fe15be1fb613534ecbac5f8c3f8744f757d237d upstream.

It is reported to cause regressions in the 5.15.y tree, so revert it for
now.

Link: https://www.spinics.net/lists/kernel/msg5397998.html
Signed-off-by: Joel Guittet <jguittet.opensource@xxxxxxxxxxx>
---
 drivers/clk/zynqmp/divider.c | 66 +++++++++++++++++++++++++++++++++---
 1 file changed, 61 insertions(+), 5 deletions(-)

diff --git a/drivers/clk/zynqmp/divider.c b/drivers/clk/zynqmp/divider.c
index e25c76ff2739..47a199346ddf 100644
--- a/drivers/clk/zynqmp/divider.c
+++ b/drivers/clk/zynqmp/divider.c
@@ -110,6 +110,52 @@ static unsigned long zynqmp_clk_divider_recalc_rate(struct clk_hw *hw,
 	return DIV_ROUND_UP_ULL(parent_rate, value);
 }
 
+static void zynqmp_get_divider2_val(struct clk_hw *hw,
+				    unsigned long rate,
+				    struct zynqmp_clk_divider *divider,
+				    u32 *bestdiv)
+{
+	int div1;
+	int div2;
+	long error = LONG_MAX;
+	unsigned long div1_prate;
+	struct clk_hw *div1_parent_hw;
+	struct zynqmp_clk_divider *pdivider;
+	struct clk_hw *div2_parent_hw = clk_hw_get_parent(hw);
+
+	if (!div2_parent_hw)
+		return;
+
+	pdivider = to_zynqmp_clk_divider(div2_parent_hw);
+	if (!pdivider)
+		return;
+
+	div1_parent_hw = clk_hw_get_parent(div2_parent_hw);
+	if (!div1_parent_hw)
+		return;
+
+	div1_prate = clk_hw_get_rate(div1_parent_hw);
+	*bestdiv = 1;
+	for (div1 = 1; div1 <= pdivider->max_div;) {
+		for (div2 = 1; div2 <= divider->max_div;) {
+			long new_error = ((div1_prate / div1) / div2) - rate;
+
+			if (abs(new_error) < abs(error)) {
+				*bestdiv = div2;
+				error = new_error;
+			}
+			if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+				div2 = div2 << 1;
+			else
+				div2++;
+		}
+		if (pdivider->flags & CLK_DIVIDER_POWER_OF_TWO)
+			div1 = div1 << 1;
+		else
+			div1++;
+	}
+}
+
 /**
  * zynqmp_clk_divider_round_rate() - Round rate of divider clock
  * @hw:			handle between common and hardware-specific interfaces
@@ -128,7 +174,6 @@ static long zynqmp_clk_divider_round_rate(struct clk_hw *hw,
 	u32 div_type = divider->div_type;
 	u32 bestdiv;
 	int ret;
-	u8 width;
 
 	/* if read only, just return current value */
 	if (divider->flags & CLK_DIVIDER_READ_ONLY) {
@@ -148,12 +193,23 @@ static long zynqmp_clk_divider_round_rate(struct clk_hw *hw,
 		return DIV_ROUND_UP_ULL((u64)*prate, bestdiv);
 	}
 
-	width = fls(divider->max_div);
+	bestdiv = zynqmp_divider_get_val(*prate, rate, divider->flags);
+
+	/*
+	 * In case of two divisors, compute best divider values and return
+	 * divider2 value based on compute value. div1 will  be automatically
+	 * set to optimum based on required total divider value.
+	 */
+	if (div_type == TYPE_DIV2 &&
+	    (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
+		zynqmp_get_divider2_val(hw, rate, divider, &bestdiv);
+	}
 
-	rate = divider_round_rate(hw, rate, prate, NULL, width, divider->flags);
+	if ((clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) && divider->is_frac)
+		bestdiv = rate % *prate ? 1 : bestdiv;
 
-	if (divider->is_frac && (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) && (rate % *prate))
-		*prate = rate;
+	bestdiv = min_t(u32, bestdiv, divider->max_div);
+	*prate = rate * bestdiv;
 
 	return rate;
 }
-- 
2.25.1