Implement switching to different parent clocks in order to get as close

to the desired frequency as possible when setting the frequency of a clock.

1 files changed, 68 insertions, 10 deletions

diff --git a/sys/dev/fdt/rkclock.c b/sys/dev/fdt/rkclock.c
index 5ea53a86c27..cd5df9b1d6a 100644
--- a/sys/dev/fdt/rkclock.c
+++ b/sys/dev/fdt/rkclock.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: rkclock.c,v 1.37 2019/01/01 12:19:57 kettenis Exp $	*/
+/*	$OpenBSD: rkclock.c,v 1.38 2019/01/01 12:35:41 kettenis Exp $	*/
 /*
  * Copyright (c) 2017, 2018 Mark Kettenis <kettenis@openbsd.org>
  *
@@ -137,11 +137,15 @@ struct rkclock {
 	uint16_t sel_mask;
 	uint16_t div_mask;
 	uint16_t parents[8];
+	uint32_t flags;
 };
 
 #define SEL(l, f)	(((1 << (l - f + 1)) - 1) << f)
 #define DIV(l, f)	SEL(l, f)
 
+#define FIXED_PARENT	(1 << 0)
+#define SET_PARENT	(1 << 1)
+
 #define HREAD4(sc, reg)							\
 	(bus_space_read_4((sc)->sc_iot, (sc)->sc_ioh, (reg)))
 #define HWRITE4(sc, reg, val)						\
@@ -331,6 +335,18 @@ rkclock_div_con(struct rkclock_softc *sc, struct rkclock *clk,
 }
 
 uint32_t
+rkclock_freq(struct rkclock_softc *sc, struct rkclock *clk,
+    uint32_t mux, uint32_t freq)
+{
+	uint32_t parent_freq, div_con;
+	uint32_t idx = clk->parents[mux];
+
+	parent_freq = sc->sc_cd.cd_get_frequency(sc, &idx);
+	div_con = rkclock_div_con(sc, clk, mux, freq);
+	return parent_freq / (div_con + 1);
+}
+
+uint32_t
 rkclock_get_frequency(struct rkclock_softc *sc, uint32_t idx)
 {
 	struct rkclock *clk;
@@ -368,7 +384,8 @@ rkclock_set_frequency(struct rkclock_softc *sc, uint32_t idx, uint32_t freq)
 {
 	struct rkclock *clk;
 	uint32_t reg, mux, div_con;
-	int shift;
+	uint32_t best_freq, best_mux, f;
+	int sel_shift, div_shift, i;
 
 	clk = rkclock_lookup(sc, idx);
 	if (clk == NULL || clk->div_mask == 0) {
@@ -377,20 +394,51 @@ rkclock_set_frequency(struct rkclock_softc *sc, uint32_t idx, uint32_t freq)
 	}
 
 	reg = HREAD4(sc, clk->reg);
-	shift = ffs(clk->sel_mask) - 1;
-	if (shift == -1)
-		mux = 0;
+	sel_shift = ffs(clk->sel_mask) - 1;
+	if (sel_shift == -1)
+		mux = sel_shift = 0;
 	else
-		mux = (reg & clk->sel_mask) >> shift;
+		mux = (reg & clk->sel_mask) >> sel_shift;
 
 	if (clk->parents[mux] == 0) {
 		printf("%s: parent 0x%08x\n", __func__, idx);
 		return 0;
 	}
 
-	div_con = rkclock_div_con(sc, clk, mux, freq);
-	shift = ffs(clk->div_mask) - 1;
-	HWRITE4(sc, clk->reg, clk->div_mask << 16 | div_con << shift);
+	if (clk->flags & SET_PARENT) {
+		idx = clk->parents[mux];
+		sc->sc_cd.cd_set_frequency(sc, &idx, freq);
+	}
+
+	/*
+	 * Start out with the current parent.  This prevents
+	 * unecessary switching to a different parent.
+	 */
+	best_freq = rkclock_freq(sc, clk, mux, freq);
+	best_mux = mux;
+
+	/*
+	 * Find the parent that allows configuration of a frequency
+	 * closest to the target frequency.
+	 */
+	if ((clk->flags & FIXED_PARENT) == 0) {
+		for (i = 0; i < nitems(clk->parents); i++) {
+			if (clk->parents[i] == 0)
+				continue;
+			f = rkclock_freq(sc, clk, i, freq);
+			if ((f > best_freq && f <= freq) ||
+			    (f < best_freq && f >= freq)) {
+				best_freq = f;
+				best_mux = i;
+			}
+		}
+	}
+
+	div_con = rkclock_div_con(sc, clk, best_mux, freq);
+	div_shift = ffs(clk->div_mask) - 1;
+	HWRITE4(sc, clk->reg,
+	    clk->sel_mask << 16 | best_mux << sel_shift |
+	    clk->div_mask << 16 | div_con << div_shift);
 	return 0;
 }
 
@@ -797,7 +845,8 @@ struct rkclock rk3328_clocks[] = {
 	{
 		RK3328_CLK_PDM, RK3328_CRU_CLKSEL_CON(20),
 		SEL(15, 14), DIV(12, 8),
-		{ RK3328_PLL_CPLL, RK3328_PLL_GPLL, RK3328_PLL_APLL }
+		{ RK3328_PLL_CPLL, RK3328_PLL_GPLL, RK3328_PLL_APLL },
+		FIXED_PARENT | SET_PARENT
 	},
 	{
 		RK3328_CLK_VDEC_CABAC, RK3328_CRU_CLKSEL_CON(48),
@@ -1265,6 +1314,15 @@ rk3328_get_frequency(void *cookie, uint32_t *cells)
 		return rk3328_get_armclk(sc);
 	case RK3328_XIN24M:
 		return 24000000;
+	/*
+	 * XXX The HDMIPHY and USB480M clocks are external.  Returning
+	 * zero here will cause them to be ignored for reparenting
+	 * purposes.
+	 */
+	case RK3328_HDMIPHY:
+		return 0;
+	case RK3328_USB480M:
+		return 0;
 	default:
 		break;
 	}