From 6bfb37e70209d2797c0a37ca257a64ee3f127319 Mon Sep 17 00:00:00 2001
From: Elaine Zhang <zhangqing@rock-chips.com>
Date: Thu, 12 Oct 2023 18:18:28 +0800
Subject: [PATCH] clk: rockchip: rk3588: fix up the frac pll calculation

rk3588 frac pll:
FFVCO = ((m + k / 65536) * FFIN) / p
FFOUT = ((m + k / 65536) * FFIN) / (p * 2s)
k is the original code, but the K[15:0] is complement code
(6'b1000_0000_0000_0000 <= K[15:0] <= 16'b0111_1111_1111_1111),
need to be converted.

Signed-off-by: Elaine Zhang <zhangqing@rock-chips.com>
Reviewed-by: Kever Yang <kever.yang@rock-chips.com>
---
 drivers/clk/rockchip/clk_pll.c | 102 +++++++++++++++++++++++++--------
 1 file changed, 78 insertions(+), 24 deletions(-)

diff --git a/drivers/clk/rockchip/clk_pll.c b/drivers/clk/rockchip/clk_pll.c
index d657ef38f3..1bb31b3313 100644
--- a/drivers/clk/rockchip/clk_pll.c
+++ b/drivers/clk/rockchip/clk_pll.c
@@ -168,13 +168,71 @@ rockchip_pll_clk_set_by_auto(ulong fin_hz,
 	return rate_table;
 }
 
+static u32
+rockchip_rk3588_pll_k_get(u32 m, u32 p, u32 s, u64 fin_hz, u64 fvco)
+{
+	u64 fref, fout, ffrac;
+	u32 k = 0;
+
+	fref = fin_hz / p;
+	ffrac = fvco - (m * fref);
+	fout = ffrac * 65536;
+	k = fout / fref;
+	if (k > 32767) {
+		fref = fin_hz / p;
+		ffrac = ((m + 1) * fref) - fvco;
+		fout = ffrac * 65536;
+		k = ((fout * 10 / fref) + 7) / 10;
+		if (k > 32767)
+			k = 0;
+		else
+			k = ~k + 1;
+	}
+	return k;
+}
+
+static struct rockchip_pll_rate_table *
+rockchip_rk3588_pll_frac_by_auto(unsigned long fin_hz, unsigned long fout_hz)
+{
+	struct rockchip_pll_rate_table *rate_table = &rockchip_auto_table;
+	u32 p, m, s, k;
+	u64 fvco;
+
+	for (s = 0; s <= 6; s++) {
+		fvco = (u64)fout_hz << s;
+		if (fvco < RK3588_VCO_MIN_HZ || fvco > RK3588_VCO_MAX_HZ)
+			continue;
+		for (p = 1; p <= 4; p++) {
+			for (m = 64; m <= 1023; m++) {
+				if ((fvco >= m * fin_hz / p) &&
+				    (fvco < (m + 1) * fin_hz / p)) {
+					k = rockchip_rk3588_pll_k_get(m, p, s,
+								      fin_hz,
+								      fvco);
+					if (!k)
+						continue;
+					rate_table->p = p;
+					rate_table->s = s;
+					rate_table->k = k;
+					if (k > 32767)
+						rate_table->m = m + 1;
+					else
+						rate_table->m = m;
+					return rate_table;
+				}
+			}
+		}
+	}
+	return NULL;
+}
+
 static struct rockchip_pll_rate_table *
 rk3588_pll_clk_set_by_auto(unsigned long fin_hz,
 			   unsigned long fout_hz)
 {
 	struct rockchip_pll_rate_table *rate_table = &rockchip_auto_table;
 	u32 p, m, s;
-	ulong fvco, fref, fout, ffrac;
+	ulong fvco;
 
 	if (fin_hz == 0 || fout_hz == 0 || fout_hz == fin_hz)
 		return NULL;
@@ -202,27 +260,12 @@ rk3588_pll_clk_set_by_auto(unsigned long fin_hz,
 		}
 		pr_err("CANNOT FIND Fout by auto,fout = %lu\n", fout_hz);
 	} else {
-		for (s = 0; s <= 6; s++) {
-			fvco = fout_hz << s;
-			if (fvco < RK3588_VCO_MIN_HZ ||
-			    fvco > RK3588_VCO_MAX_HZ)
-				continue;
-			for (p = 1; p <= 4; p++) {
-				for (m = 64; m <= 1023; m++) {
-					if ((fvco >= m * fin_hz / p) && (fvco < (m + 1) * fin_hz / p)) {
-						rate_table->p = p;
-						rate_table->m = m;
-						rate_table->s = s;
-						fref = fin_hz / p;
-						ffrac = fvco - (m * fref);
-						fout = ffrac * 65536;
-						rate_table->k = fout / fref;
-						return rate_table;
-					}
-				}
-			}
-		}
-		pr_err("CANNOT FIND Fout by auto,fout = %lu\n", fout_hz);
+		rate_table = rockchip_rk3588_pll_frac_by_auto(fin_hz, fout_hz);
+		if (!rate_table)
+			pr_err("CANNOT FIND Fout by auto,fout = %lu\n",
+			       fout_hz);
+		else
+			return rate_table;
 	}
 	return NULL;
 }
@@ -533,11 +576,22 @@ static ulong rk3588_pll_get_rate(struct rockchip_pll_clock *pll,
 
 		rate = OSC_HZ / p;
 		rate *= m;
-		if (k) {
+		if (k & BIT(15)) {
+			/* fractional mode */
+			u64 frac_rate64;
+
+			k = (~(k - 1)) & RK3588_PLLCON2_K_MASK;
+			frac_rate64 = OSC_HZ * k;
+			postdiv = p;
+			postdiv *= 65536;
+			do_div(frac_rate64, postdiv);
+			rate -= frac_rate64;
+		} else {
 			/* fractional mode */
 			u64 frac_rate64 = OSC_HZ * k;
 
-			postdiv = p * 65536;
+			postdiv = p;
+			postdiv *= 65536;
 			do_div(frac_rate64, postdiv);
 			rate += frac_rate64;
 		}
-- 
2.39.5