[PATCH v3 05/11] clk: k210: Re-add support for setting rate

[Sean Anderson <seanga2@gmail.com>]

This adds support for setting clock rates, which was left out of the
initial CCF expunging. There are several tricky bits here, mostly related
to the PLLS:

* The PLL's bypass is broken. If the PLL is reconfigured, any child clocks
  will be stopped.
* PLL0 is the parent of ACLK which is the CPU and SRAM's clock. To prevent
  stopping the CPU while we configure PLL0's rate, ACLK is reparented
  to IN0 while PLL0 is disabled.
* PLL1 is the parent of the AISRAM clock. This clock cannot be reparented,
  so we instead just disallow changing PLL1's rate after relocation (when
  we are using the AISRAM).

Signed-off-by: Sean Anderson <seanga2@gmail.com>
---

Changes in v3:
- Fix inverted condition for setting defaults
- Fix val not being set for K210_DIV_POWER

Changes in v2:
- Only force probe clocks pre-reloc

 drivers/clk/kendryte/clk.c | 89 +++++++++++++++++++++++++++++++++++---
 1 file changed, 84 insertions(+), 5 deletions(-)

diff --git a/drivers/clk/kendryte/clk.c b/drivers/clk/kendryte/clk.c
index 203d5f741c..a2901f9b96 100644
--- a/drivers/clk/kendryte/clk.c
+++ b/drivers/clk/kendryte/clk.c
@@ -17,6 +17,8 @@
 #include <kendryte/pll.h>
 #include <linux/bitfield.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 /**
  * struct k210_clk_priv - K210 clock driver private data
  * @base: The base address of the sysctl device
@@ -1059,11 +1061,6 @@ static ulong k210_clk_get_rate(struct clk *clk)
 	return do_k210_clk_get_rate(dev_get_priv(clk->dev), clk->id);
 }
 
-static ulong k210_clk_set_rate(struct clk *clk, unsigned long rate)
-{
-	return -ENOSYS;
-}
-
 static int do_k210_clk_set_parent(struct k210_clk_priv *priv, int id, int new)
 {
 	int i;
@@ -1089,6 +1086,81 @@ static int k210_clk_set_parent(struct clk *clk, struct clk *parent)
 				      parent->id);
 }
 
+static ulong k210_clk_set_rate(struct clk *clk, unsigned long rate)
+{
+	int parent, ret, err;
+	ulong rate_in, val;
+	const struct k210_div_params *div;
+	struct k210_clk_priv *priv = dev_get_priv(clk->dev);
+
+	if (clk->id == K210_CLK_IN0)
+		return clk_set_rate(&priv->in0, rate);
+
+	parent = k210_clk_get_parent(priv, clk->id);
+	rate_in = do_k210_clk_get_rate(priv, parent);
+
+	log_debug("id=%ld rate=%lu rate_in=%lu\n", clk->id, rate, rate_in);
+
+	if (clk->id == K210_CLK_PLL0) {
+		/* Bypass ACLK so the CPU keeps going */
+		ret = do_k210_clk_set_parent(priv, K210_CLK_ACLK, K210_CLK_IN0);
+		if (ret)
+			return ret;
+	} else if (clk->id == K210_CLK_PLL1 && gd->flags & GD_FLG_RELOC) {
+		/*
+		 * We can't bypass the AI clock like we can ACLK, and after
+		 * relocation we are using the AI ram.
+		 */
+		return -EPERM;
+	}
+
+	if (k210_clks[clk->id].flags & K210_CLKF_PLL) {
+		ret = k210_pll_set_rate(priv, k210_clks[clk->id].pll, rate,
+					rate_in);
+		if (!IS_ERR_VALUE(ret) && clk->id == K210_CLK_PLL0) {
+			/*
+			 * This may have the side effect of reparenting ACLK,
+			 * but I don't really want to keep track of what the old
+			 * parent was.
+			 */
+			err = do_k210_clk_set_parent(priv, K210_CLK_ACLK,
+						     K210_CLK_PLL0);
+			if (err)
+				return err;
+		}
+		return ret;
+	}
+
+	if (k210_clks[clk->id].div == K210_CLK_DIV_NONE)
+		return -ENOSYS;
+	div = &k210_divs[k210_clks[clk->id].div];
+
+	switch (div->type) {
+	case K210_DIV_ONE:
+		val = DIV_ROUND_CLOSEST_ULL((u64)rate_in, rate);
+		val = val ? val - 1 : 0;
+		break;
+	case K210_DIV_EVEN:
+		val = DIV_ROUND_CLOSEST_ULL((u64)rate_in, 2 * rate);
+		break;
+	case K210_DIV_POWER:
+		/* This is ACLK, which has no divider on IN0 */
+		if (parent == K210_CLK_IN0)
+			return -ENOSYS;
+
+		val = DIV_ROUND_CLOSEST_ULL((u64)rate_in, rate);
+		val = __ffs(val);
+		break;
+	default:
+		assert(false);
+		return -EINVAL;
+	};
+
+	val = val ? val - 1 : 0;
+	k210_clk_writel(priv, div->off, div->shift, div->width, val);
+	return do_k210_clk_get_rate(priv, clk->id);
+}
+
 static int k210_clk_endisable(struct k210_clk_priv *priv, int id, bool enable)
 {
 	int parent = k210_clk_get_parent(priv, id);
@@ -1163,6 +1235,13 @@ static int k210_clk_probe(struct udevice *dev)
 	if (ret)
 		return ret;
 
+	/*
+	 * Force setting defaults, even before relocation. This is so we can
+	 * set the clock rate for PLL1 before we relocate into aisram.
+	 */
+	if (!(gd->flags & GD_FLG_RELOC))
+		clk_set_defaults(dev, CLK_DEFAULTS_POST_FORCE);
+
 	return 0;
 }
 
-- 
2.31.0