arm64: rk: Add support for setting pll rate

Add support for setting pll rate. On RockChip SoC two kind of plls are
supported, integer mode and fractional mode.
The two modes are intended to support more frequencies for the core plls.
While here change the recalc method as it appears that the datasheet is
wrong on the calculation method.

2 files changed, 113 insertions, 30 deletions

diff --git a/sys/arm64/rockchip/clk/rk_clk_pll.c b/sys/arm64/rockchip/clk/rk_clk_pll.c
index 6539f5424ef0..dbfb8f4f1537 100644
--- a/sys/arm64/rockchip/clk/rk_clk_pll.c
+++ b/sys/arm64/rockchip/clk/rk_clk_pll.c
@@ -48,6 +48,9 @@ struct rk_clk_pll_sc {
 	uint32_t	gate_shift;
 
 	uint32_t	flags;
+
+	struct rk_clk_pll_rate	*rates;
+	struct rk_clk_pll_rate	*frac_rates;
 };
 
 #define	WRITE4(_clk, off, val)					\
@@ -59,14 +62,6 @@ struct rk_clk_pll_sc {
 #define	DEVICE_UNLOCK(_clk)					\
 	CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
 
-#define	RK_CLK_PLL_DSMPD_OFFSET		4
-#define	RK_CLK_PLL_DSMPD_SHIFT		12
-#define	RK_CLK_PLL_DSMPD_MASK		0x1000
-
-#define	RK_CLK_PLL_REFDIV_OFFSET	4
-#define	RK_CLK_PLL_REFDIV_SHIFT		0
-#define	RK_CLK_PLL_REFDIV_MASK		0x3F
-
 #define	RK_CLK_PLL_FBDIV_OFFSET		0
 #define	RK_CLK_PLL_FBDIV_SHIFT		0
 #define	RK_CLK_PLL_FBDIV_MASK		0xFFF
@@ -75,6 +70,14 @@ struct rk_clk_pll_sc {
 #define	RK_CLK_PLL_POSTDIV1_SHIFT	12
 #define	RK_CLK_PLL_POSTDIV1_MASK	0x7000
 
+#define	RK_CLK_PLL_DSMPD_OFFSET		4
+#define	RK_CLK_PLL_DSMPD_SHIFT		12
+#define	RK_CLK_PLL_DSMPD_MASK		0x1000
+
+#define	RK_CLK_PLL_REFDIV_OFFSET	4
+#define	RK_CLK_PLL_REFDIV_SHIFT		0
+#define	RK_CLK_PLL_REFDIV_MASK		0x3F
+
 #define	RK_CLK_PLL_POSTDIV2_OFFSET	4
 #define	RK_CLK_PLL_POSTDIV2_SHIFT	6
 #define	RK_CLK_PLL_POSTDIV2_MASK	0x1C0
@@ -83,6 +86,10 @@ struct rk_clk_pll_sc {
 #define	RK_CLK_PLL_FRAC_SHIFT		0
 #define	RK_CLK_PLL_FRAC_MASK		0xFFFFFF
 
+#define	RK_CLK_PLL_LOCK_MASK		0x400
+
+#define	RK_CLK_PLL_WRITE_MASK		0xFFFF0000
+
 static int
 rk_clk_pll_init(struct clknode *clk, device_t dev)
 {
@@ -122,10 +129,10 @@ static int
 rk_clk_pll_recalc(struct clknode *clk, uint64_t *freq)
 {
 	struct rk_clk_pll_sc *sc;
-	uint64_t foutvco;
+	uint64_t rate;
 	uint32_t dsmpd, refdiv, fbdiv;
 	uint32_t postdiv1, postdiv2, frac;
-	uint32_t raw1, raw2;
+	uint32_t raw1, raw2, raw3;
 
 	sc = clknode_get_softc(clk);
 
@@ -133,38 +140,98 @@ rk_clk_pll_recalc(struct clknode *clk, uint64_t *freq)
 
 	READ4(clk, sc->base_offset, &raw1);
 	READ4(clk, sc->base_offset + 4, &raw2);
+	READ4(clk, sc->base_offset + 8, &raw3);
 
-	READ4(clk, sc->base_offset + RK_CLK_PLL_DSMPD_OFFSET, &dsmpd);
-	dsmpd = (dsmpd & RK_CLK_PLL_DSMPD_MASK) >> RK_CLK_PLL_DSMPD_SHIFT;
-
-	READ4(clk, sc->base_offset + RK_CLK_PLL_REFDIV_OFFSET, &refdiv);
-	refdiv = (refdiv & RK_CLK_PLL_REFDIV_MASK) >> RK_CLK_PLL_REFDIV_SHIFT;
-
-	READ4(clk, sc->base_offset + RK_CLK_PLL_FBDIV_OFFSET, &fbdiv);
-	fbdiv = (fbdiv & RK_CLK_PLL_FBDIV_MASK) >> RK_CLK_PLL_FBDIV_SHIFT;
-
-	READ4(clk, sc->base_offset + RK_CLK_PLL_POSTDIV1_OFFSET, &postdiv1);
-	postdiv1 = (postdiv1 & RK_CLK_PLL_POSTDIV1_MASK) >> RK_CLK_PLL_POSTDIV1_SHIFT;
+	fbdiv = (raw1 & RK_CLK_PLL_FBDIV_MASK) >> RK_CLK_PLL_FBDIV_SHIFT;
+	postdiv1 = (raw1 & RK_CLK_PLL_POSTDIV1_MASK) >> RK_CLK_PLL_POSTDIV1_SHIFT;
 
-	READ4(clk, sc->base_offset + RK_CLK_PLL_POSTDIV2_OFFSET, &postdiv2);
-	postdiv2 = (postdiv2 & RK_CLK_PLL_POSTDIV2_MASK) >> RK_CLK_PLL_POSTDIV2_SHIFT;
+	dsmpd = (raw2 & RK_CLK_PLL_DSMPD_MASK) >> RK_CLK_PLL_DSMPD_SHIFT;
+	refdiv = (raw2 & RK_CLK_PLL_REFDIV_MASK) >> RK_CLK_PLL_REFDIV_SHIFT;
+	postdiv2 = (raw2 & RK_CLK_PLL_POSTDIV2_MASK) >> RK_CLK_PLL_POSTDIV2_SHIFT;
 
-	READ4(clk, sc->base_offset + RK_CLK_PLL_FRAC_OFFSET, &frac);
-	frac = (frac & RK_CLK_PLL_FRAC_MASK) >> RK_CLK_PLL_FRAC_SHIFT;
+	frac = (raw3 & RK_CLK_PLL_FRAC_MASK) >> RK_CLK_PLL_FRAC_SHIFT;
 
 	DEVICE_UNLOCK(clk);
 
+	rate = *freq * fbdiv / refdiv;
 	if (dsmpd == 0) {
 		/* Fractional mode */
-		foutvco = *freq / refdiv * (fbdiv + frac / 224);
-	} else {
-		/* Integer mode */
+		uint64_t frac_rate;
 
-		foutvco = *freq / refdiv * fbdiv;
+		frac_rate = *freq * frac / refdiv;
+		rate += frac_rate >> 24;
 	}
 
-	*freq = foutvco / postdiv1 / postdiv2;
+	*freq = rate / postdiv1 / postdiv2;
+
+	if (*freq % 2)
+		*freq = *freq + 1;
+
+	return (0);
+}
+
+static int
+rk_clk_pll_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
+    int flags, int *stop)
+{
+	struct rk_clk_pll_rate *rates;
+	struct rk_clk_pll_sc *sc;
+	uint32_t reg;
+	int timeout;
+
+	sc = clknode_get_softc(clk);
+
+	if (sc->rates)
+		rates = sc->rates;
+	else if (sc->frac_rates)
+		rates = sc->frac_rates;
+	else
+		return (EINVAL);
+
+	for (; rates->freq; rates++) {
+		if (rates->freq == *fout)
+			break;
+	}
+	if (rates->freq == 0) {
+		*stop = 1;
+		return (EINVAL);
+	}
+
+	DEVICE_LOCK(clk);
+
+	/* Setting postdiv1 and fbdiv */
+	READ4(clk, sc->base_offset, &reg);
+	reg &= ~(RK_CLK_PLL_POSTDIV1_MASK | RK_CLK_PLL_FBDIV_MASK);
+	reg |= rates->postdiv1 << RK_CLK_PLL_POSTDIV1_SHIFT;
+	reg |= rates->fbdiv << RK_CLK_PLL_FBDIV_SHIFT;
+	WRITE4(clk, sc->base_offset, reg | RK_CLK_PLL_WRITE_MASK);
+
+	/* Setting dsmpd, postdiv2 and refdiv */
+	READ4(clk, sc->base_offset + 0x4, &reg);
+	reg &= ~(RK_CLK_PLL_DSMPD_MASK | RK_CLK_PLL_POSTDIV2_MASK |
+	    RK_CLK_PLL_REFDIV_MASK);
+	reg |= rates->dsmpd << RK_CLK_PLL_DSMPD_SHIFT;
+	reg |= rates->postdiv2 << RK_CLK_PLL_POSTDIV2_SHIFT;
+	reg |= rates->refdiv << RK_CLK_PLL_REFDIV_SHIFT;
+	WRITE4(clk, sc->base_offset + 0x4, reg | RK_CLK_PLL_WRITE_MASK);
+
+	/* Setting frac */
+	READ4(clk, sc->base_offset + 0x8, &reg);
+	reg &= ~RK_CLK_PLL_FRAC_MASK;
+	reg |= rates->frac << RK_CLK_PLL_FRAC_SHIFT;
+	WRITE4(clk, sc->base_offset + 0x8, reg);
+
+	/* Reading lock */
+	for (timeout = 1000; timeout; timeout--) {
+		READ4(clk, sc->base_offset + 0x4, &reg);
+		if ((reg & RK_CLK_PLL_LOCK_MASK) == 0)
+			break;
+		DELAY(1);
+	}
+
+	DEVICE_UNLOCK(clk);
 
+	*stop = 1;
 	return (0);
 }
 
@@ -173,6 +240,7 @@ static clknode_method_t rk_clk_pll_clknode_methods[] = {
 	CLKNODEMETHOD(clknode_init,		rk_clk_pll_init),
 	CLKNODEMETHOD(clknode_set_gate,		rk_clk_pll_set_gate),
 	CLKNODEMETHOD(clknode_recalc_freq,	rk_clk_pll_recalc),
+	CLKNODEMETHOD(clknode_set_freq,		rk_clk_pll_set_freq),
 	CLKNODEMETHOD_END
 };
 
@@ -196,6 +264,8 @@ rk_clk_pll_register(struct clkdom *clkdom, struct rk_clk_pll_def *clkdef)
 	sc->gate_offset = clkdef->gate_offset;
 	sc->gate_shift = clkdef->gate_shift;
 	sc->flags = clkdef->flags;
+	sc->rates = clkdef->rates;
+	sc->frac_rates = clkdef->frac_rates;
 
 	clknode_register(clkdom, clk);
 
diff --git a/sys/arm64/rockchip/clk/rk_clk_pll.h b/sys/arm64/rockchip/clk/rk_clk_pll.h
index ee6364dc8539..f5e63d5bfcdf 100644
--- a/sys/arm64/rockchip/clk/rk_clk_pll.h
+++ b/sys/arm64/rockchip/clk/rk_clk_pll.h
@@ -33,6 +33,16 @@
 
 #include <dev/extres/clk/clk.h>
 
+struct rk_clk_pll_rate {
+	uint32_t	freq;
+	uint32_t	refdiv;
+	uint32_t	fbdiv;
+	uint32_t	postdiv1;
+	uint32_t	postdiv2;
+	uint32_t	dsmpd;
+	uint32_t	frac;
+};
+
 struct rk_clk_pll_def {
 	struct clknode_init_def	clkdef;
 	uint32_t		base_offset;
@@ -41,6 +51,9 @@ struct rk_clk_pll_def {
 	uint32_t		gate_shift;
 
 	uint32_t		flags;
+
+	struct rk_clk_pll_rate	*rates;
+	struct rk_clk_pll_rate	*frac_rates;
 };
 
 #define	RK_CLK_PLL_HAVE_GATE	0x1