From 554c9209e31368c7a103ba451c16b588b3cff74b Mon Sep 17 00:00:00 2001 From: Emmanuel Vadot Date: Tue, 26 Feb 2019 13:14:49 +0000 Subject: [PATCH] arm64: rockchip: clk: rk_clk_composite: Properly use the mask bits RockChip clocks register have a write mask in the upper 16 bits, if a 1 is present the corresponding bit in the lower 16 ones is set. Use this instead of always setting the mask to 0xFFFF0000. This avoids a read of the register. While here add some debug printf useful for debuging clock problems MFC after: 1 week --- sys/arm64/rockchip/clk/rk_clk_composite.c | 50 ++++++++++++++--------- sys/arm64/rockchip/clk/rk_clk_composite.h | 2 - 2 files changed, 31 insertions(+), 21 deletions(-) diff --git a/sys/arm64/rockchip/clk/rk_clk_composite.c b/sys/arm64/rockchip/clk/rk_clk_composite.c index 1f64dacdeec2..c26552b423f2 100644 --- a/sys/arm64/rockchip/clk/rk_clk_composite.c +++ b/sys/arm64/rockchip/clk/rk_clk_composite.c @@ -66,7 +66,10 @@ struct rk_clk_composite_sc { #define DEVICE_UNLOCK(_clk) \ CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk)) -#define RK_COMPOSITE_WRITE_MASK 0xFFFF0000 +#define RK_CLK_COMPOSITE_MASK_SHIFT 16 + +/* #define dprintf(format, arg...) printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg) */ +#define dprintf(format, arg...) static int rk_clk_composite_init(struct clknode *clk, device_t dev) @@ -94,20 +97,21 @@ static int rk_clk_composite_set_gate(struct clknode *clk, bool enable) { struct rk_clk_composite_sc *sc; - uint32_t val; + uint32_t val = 0; sc = clknode_get_softc(clk); if ((sc->flags & RK_CLK_COMPOSITE_HAVE_GATE) == 0) return (0); - DEVICE_LOCK(clk); - READ4(clk, sc->gate_offset, &val); - if (enable) - val &= ~(1 << sc->gate_shift); - else + dprintf("%sabling gate\n", enable ? "En" : "Dis"); + if (!enable) val |= 1 << sc->gate_shift; - WRITE4(clk, sc->gate_offset, val | RK_CLK_COMPOSITE_MASK); + dprintf("sc->gate_shift: %x\n", sc->gate_shift); + val |= (1 << sc->gate_shift) << RK_CLK_COMPOSITE_MASK_SHIFT; + dprintf("Write: gate_offset=%x, val=%x\n", sc->gate_offset, val); + DEVICE_LOCK(clk); + WRITE4(clk, sc->gate_offset, val); DEVICE_UNLOCK(clk); return (0); @@ -117,18 +121,19 @@ static int rk_clk_composite_set_mux(struct clknode *clk, int index) { struct rk_clk_composite_sc *sc; - uint32_t val; + uint32_t val = 0; sc = clknode_get_softc(clk); if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) == 0) return (0); + dprintf("Set mux to %d\n", index); DEVICE_LOCK(clk); - READ4(clk, sc->muxdiv_offset, &val); - val &= ~sc->mux_mask; - val |= index << sc->mux_shift; - WRITE4(clk, sc->muxdiv_offset, val | RK_CLK_COMPOSITE_MASK); + val |= (index << sc->mux_shift); + val |= sc->mux_mask << RK_CLK_COMPOSITE_MASK_SHIFT; + dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val); + WRITE4(clk, sc->muxdiv_offset, val); DEVICE_UNLOCK(clk); return (0); @@ -145,11 +150,12 @@ rk_clk_composite_recalc(struct clknode *clk, uint64_t *freq) DEVICE_LOCK(clk); READ4(clk, sc->muxdiv_offset, ®); + dprintf("Read: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, reg); DEVICE_UNLOCK(clk); div = ((reg & sc->div_mask) >> sc->div_shift) + 1; - + dprintf("parent_freq=%lu, div=%u\n", *freq, div); *freq = *freq / div; return (0); @@ -183,22 +189,25 @@ rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout, struct clknode *p_clk; const char **p_names; uint64_t best, cur; - uint32_t div, best_div, val; + uint32_t div, best_div, val = 0; int p_idx, best_parent; sc = clknode_get_softc(clk); + dprintf("Finding best parent/div for target freq of %lu\n", *fout); 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); + dprintf("Testing with parent %s (%d) at freq %lu\n", clknode_get_name(p_clk), p_idx, fparent); div = rk_clk_composite_find_best(sc, fparent, *fout); cur = fparent / div; if ((*fout - cur) < (*fout - best)) { best = cur; best_div = div; best_parent = p_idx; + dprintf("Best parent so far %s (%d) with best freq at %lu\n", clknode_get_name(p_clk), p_idx, best); } } @@ -223,14 +232,17 @@ rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout, } p_idx = clknode_get_parent_idx(clk); - if (p_idx != best_parent) + if (p_idx != best_parent) { + dprintf("Switching parent index from %d to %d\n", p_idx, best_parent); clknode_set_parent_by_idx(clk, best_parent); + } + dprintf("Setting divider to %d\n", best_div); DEVICE_LOCK(clk); - READ4(clk, sc->muxdiv_offset, &val); - val &= ~sc->div_mask; val |= (best_div - 1) << sc->div_shift; - WRITE4(clk, sc->muxdiv_offset, val | RK_CLK_COMPOSITE_MASK); + val |= (sc->div_mask << sc->div_shift) << RK_CLK_COMPOSITE_MASK_SHIFT; + dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val); + WRITE4(clk, sc->muxdiv_offset, val); DEVICE_UNLOCK(clk); *fout = best; diff --git a/sys/arm64/rockchip/clk/rk_clk_composite.h b/sys/arm64/rockchip/clk/rk_clk_composite.h index 12a14fd95205..13e415a42ccb 100644 --- a/sys/arm64/rockchip/clk/rk_clk_composite.h +++ b/sys/arm64/rockchip/clk/rk_clk_composite.h @@ -53,8 +53,6 @@ struct rk_clk_composite_def { #define RK_CLK_COMPOSITE_HAVE_MUX 0x0001 #define RK_CLK_COMPOSITE_HAVE_GATE 0x0002 -#define RK_CLK_COMPOSITE_MASK 0xFFFF0000 - int rk_clk_composite_register(struct clkdom *clkdom, struct rk_clk_composite_def *clkdef);