[PATCH v6] clk: starfive: jh7110-sys: Fix lower rate of CPUfreq by setting PLL0 rate to 1.5GHz

[Xingyu Wu]

CPUfreq supports 4 cpu frequency loads on 375/500/750/1500MHz. But now
PLL0 rate is 1GHz and the cpu frequency loads become 250/333/500/1000MHz
in fact. The PLL0 rate should be default set to 1.5GHz and set the
divider of cpu_core clock to 2 in safe.

To keeo the cpu frequency stable when setting PLL0, the parent clock of
the cpu_root clock needs to be switched from PLL0 to another parent
clock and add notifier function to do this for PLL0 clock. In the
function, the cpu_root clock should be operated by saving its current
parent and setting a new safe parent (osc clock) before setting the PLL0
clock rate. After setting PLL0 rate, it should be switched back to the
original parent clock.

To keep the DTS same in Linux and U-Boot and the PLL0 rate is 1GHz in
U-Boot, the PLL0 rate should be set to 1.5GHz in the driver instead of
DTS.

Fixes: e2c510d6d630 ("riscv: dts: starfive: Add cpu scaling for JH7110 SoC")
Signed-off-by: Xingyu Wu <xingyu.wu@xxxxxxxxxxxxxxxx>
---

Hi Stephen and Emil,

This patch is to fix the lower rate of CPUfreq by adding the notifier
for PLL0 clock and changing the PLL0 rate to 1.5GHz.

To keep the DTS same in Linux and U-Boot as Conor wants[1] and the PLL0
rate is 1GHz in U-Boot, the PLL0 rate should be set to 1.5GHz in the 
driver instead of DTS.

[1]: https://lore.kernel.org/all/20240515-reorder-even-8b9eebd91b45@spud/

Thanks,
Xingyu Wu

---
 .../clk/starfive/clk-starfive-jh7110-sys.c    | 54 ++++++++++++++++++-
 drivers/clk/starfive/clk-starfive-jh71x0.h    |  2 +
 2 files changed, 54 insertions(+), 2 deletions(-)

diff --git a/drivers/clk/starfive/clk-starfive-jh7110-sys.c b/drivers/clk/starfive/clk-starfive-jh7110-sys.c
index 8f5e5abfa178..7469981fb405 100644
--- a/drivers/clk/starfive/clk-starfive-jh7110-sys.c
+++ b/drivers/clk/starfive/clk-starfive-jh7110-sys.c
@@ -385,6 +385,32 @@ int jh7110_reset_controller_register(struct jh71x0_clk_priv *priv,
 }
 EXPORT_SYMBOL_GPL(jh7110_reset_controller_register);
 
+/*
+ * This clock notifier is called when the rate of PLL0 clock is to be changed.
+ * The cpu_root clock should save the curent parent clock and swicth its parent
+ * clock to osc before PLL0 rate will be changed. Then swicth its parent clock
+ * back after the PLL0 rate is completed.
+ */
+static int jh7110_pll0_clk_notifier_cb(struct notifier_block *nb,
+				       unsigned long action, void *data)
+{
+	struct jh71x0_clk_priv *priv = container_of(nb, struct jh71x0_clk_priv, pll_clk_nb);
+	struct clk *cpu_root = priv->reg[JH7110_SYSCLK_CPU_ROOT].hw.clk;
+	int ret = 0;
+
+	if (action == PRE_RATE_CHANGE) {
+		struct clk *osc = clk_get(priv->dev, "osc");
+
+		priv->original_clk = clk_get_parent(cpu_root);
+		ret = clk_set_parent(cpu_root, osc);
+		clk_put(osc);
+	} else if (action == POST_RATE_CHANGE) {
+		ret = clk_set_parent(cpu_root, priv->original_clk);
+	}
+
+	return notifier_from_errno(ret);
+}
+
 static int __init jh7110_syscrg_probe(struct platform_device *pdev)
 {
 	struct jh71x0_clk_priv *priv;
@@ -413,7 +439,11 @@ static int __init jh7110_syscrg_probe(struct platform_device *pdev)
 		if (IS_ERR(priv->pll[0]))
 			return PTR_ERR(priv->pll[0]);
 	} else {
-		clk_put(pllclk);
+		priv->pll_clk_nb.notifier_call = jh7110_pll0_clk_notifier_cb;
+		ret = clk_notifier_register(pllclk, &priv->pll_clk_nb);
+		if (ret)
+			return ret;
+
 		priv->pll[0] = NULL;
 	}
 
@@ -501,7 +531,27 @@ static int __init jh7110_syscrg_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	return jh7110_reset_controller_register(priv, "rst-sys", 0);
+	ret = jh7110_reset_controller_register(priv, "rst-sys", 0);
+	if (ret)
+		return ret;
+
+	/* Set the divider cpu_core to 2 and set the PLL0 rate to 1.5G. */
+	pllclk = clk_get(priv->dev, "pll0_out");
+	if (!IS_ERR(pllclk)) {
+		struct clk *cpu_core = priv->reg[JH7110_SYSCLK_CPU_CORE].hw.clk;
+
+		ret = clk_set_rate(cpu_core, clk_get_rate(cpu_core) / 2);
+		if (ret)
+			return ret;
+
+		ret = clk_set_rate(pllclk, 1500000000);
+		if (ret)
+			return ret;
+
+		clk_put(pllclk);
+	}
+
+	return 0;
 }
 
 static const struct of_device_id jh7110_syscrg_match[] = {
diff --git a/drivers/clk/starfive/clk-starfive-jh71x0.h b/drivers/clk/starfive/clk-starfive-jh71x0.h
index 23e052fc1549..e3f441393e48 100644
--- a/drivers/clk/starfive/clk-starfive-jh71x0.h
+++ b/drivers/clk/starfive/clk-starfive-jh71x0.h
@@ -114,6 +114,8 @@ struct jh71x0_clk_priv {
 	spinlock_t rmw_lock;
 	struct device *dev;
 	void __iomem *base;
+	struct clk *original_clk;
+	struct notifier_block pll_clk_nb;
 	struct clk_hw *pll[3];
 	struct jh71x0_clk reg[];
 };
-- 
2.34.1