arm64: rockchip: clk: Add support to reparent to clk_composite

All clk_composite type have the possibility to reparent (choosing another
parent to find a better frequency), add the support for that.

1 files changed, 33 insertions, 2 deletions

diff --git a/sys/arm64/rockchip/clk/rk_clk_composite.c b/sys/arm64/rockchip/clk/rk_clk_composite.c
index 80277b48f30a..ee8c66c03b70 100644
--- a/sys/arm64/rockchip/clk/rk_clk_composite.c
+++ b/sys/arm64/rockchip/clk/rk_clk_composite.c
@@ -155,22 +155,50 @@ rk_clk_composite_recalc(struct clknode *clk, uint64_t *freq)
 	return (0);
 }
 
+static uint32_t
+rk_clk_composite_find_best(struct rk_clk_composite_sc *sc, uint64_t fparent,
+    uint64_t freq)
+{
+	uint64_t best, cur;
+	uint32_t best_div, div;
+
+	for (best = 0, best_div = 0, div = 0;
+	     div <= ((sc->div_mask >> sc->div_shift) + 1); div++) {
+		cur = fparent / div;
+		if ((freq - cur) < (freq - best)) {
+			best = cur;
+			best_div = div;
+			break;
+		}
+	}
+
+	return (best_div);
+}
+
 static int
 rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
     int flags, int *stop)
 {
 	struct rk_clk_composite_sc *sc;
+	struct clknode *p_clk;
+	const char **p_names;
 	uint64_t best, cur;
 	uint32_t div, best_div, val;
+	int p_idx, best_parent;
 
 	sc = clknode_get_softc(clk);
 
-	for (best = 0, best_div = 0, div = 0; div <= sc->div_mask; div++) {
+	p_names = clknode_get_parent_names(clk);
+	for (best_div = 0, best = 0, p_idx = 0;
+	     p_idx != clknode_get_parents_num(clk); p_idx++) {
+		p_clk = clknode_find_by_name(p_names[p_idx]);
+		clknode_get_freq(p_clk, &fparent);
+		div = rk_clk_composite_find_best(sc, fparent, *fout);
 		cur = fparent / div;
 		if ((*fout - cur) < (*fout - best)) {
 			best = cur;
 			best_div = div;
-			break;
+			best_parent = p_idx;
 		}
 	}
 
@@ -194,6 +222,9 @@ rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
 		return (0);
 	}
 
+	if (p_idx != best_parent)
+		clknode_set_parent_by_idx(clk, best_parent);
+
 	DEVICE_LOCK(clk);
 	READ4(clk, sc->muxdiv_offset, &val);
 	val &= ~sc->div_mask;