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; + 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_REFDIV_OFFSET, &refdiv); - refdiv = (refdiv & RK_CLK_PLL_REFDIV_MASK) >> RK_CLK_PLL_REFDIV_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_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; - - READ4(clk, sc->base_offset + RK_CLK_PLL_POSTDIV2_OFFSET, &postdiv2); - postdiv2 = (postdiv2 & 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 &= ~(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 &= ~(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 &= ~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, ®); + 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 +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