diff --git a/drivers/clk/rockchip/Kconfig b/drivers/clk/rockchip/Kconfig index 47cd6c5de837..0426d0f20c75 100644 --- a/drivers/clk/rockchip/Kconfig +++ b/drivers/clk/rockchip/Kconfig @@ -75,4 +75,9 @@ config CLK_RK3399 default y help Build the driver for RK3399 Clock Driver. + +config ROCKCHIP_CLK_COMPENSATION + bool "Rockchip Clk Compensation" + help + Say y here to enable clk compensation(+/- 1000 ppm). endif diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c index dba0503eee14..d4c84f77da49 100644 --- a/drivers/clk/rockchip/clk-pll.c +++ b/drivers/clk/rockchip/clk-pll.c @@ -16,6 +16,7 @@ #include #include #include +#include #include #include "clk.h" @@ -1180,6 +1181,80 @@ static const struct clk_ops rockchip_rk3399_pll_clk_ops = { .init = rockchip_rk3399_pll_init, }; +#ifdef CONFIG_ROCKCHIP_CLK_COMPENSATION +int rockchip_pll_clk_compensation(struct clk *clk, int ppm) +{ + struct clk *parent = clk_get_parent(clk); + struct rockchip_clk_pll *pll; + static u32 frac, fbdiv; + bool negative; + u32 pllcon, pllcon0, pllcon2, fbdiv_mask, frac_mask, frac_shift; + u64 fracdiv, m, n; + + if ((ppm > 1000) || (ppm < -1000)) + return -EINVAL; + + if (IS_ERR_OR_NULL(parent)) + return -EINVAL; + + pll = to_rockchip_clk_pll(__clk_get_hw(parent)); + if (!pll) + return -EINVAL; + + switch (pll->type) { + case pll_rk3036: + case pll_rk3328: + pllcon0 = RK3036_PLLCON(0); + pllcon2 = RK3036_PLLCON(2); + fbdiv_mask = RK3036_PLLCON0_FBDIV_MASK; + frac_mask = RK3036_PLLCON2_FRAC_MASK; + frac_shift = RK3036_PLLCON2_FRAC_SHIFT; + break; + case pll_rk3066: + return -EINVAL; + case pll_rk3399: + pllcon0 = RK3399_PLLCON(0); + pllcon2 = RK3399_PLLCON(2); + fbdiv_mask = RK3399_PLLCON0_FBDIV_MASK; + frac_mask = RK3399_PLLCON2_FRAC_MASK; + frac_shift = RK3399_PLLCON2_FRAC_SHIFT; + break; + default: + return -EINVAL; + } + + negative = !!(ppm & BIT(31)); + ppm = negative ? ~ppm + 1 : ppm; + + if (!frac) { + frac = readl_relaxed(pll->reg_base + pllcon2) & frac_mask; + fbdiv = readl_relaxed(pll->reg_base + pllcon0) & fbdiv_mask; + } + + /* + * delta frac frac ppm + * -------------- = (fbdiv + ----------) * --------- + * 1 << 24 1 << 24 1000000 + * + */ + m = div64_u64((uint64_t)frac * ppm, 1000000); + n = div64_u64((uint64_t)ppm << 24, 1000000) * fbdiv; + + fracdiv = negative ? frac - (m + n) : frac + (m + n); + + if (!frac || fracdiv > frac_mask) + return -EINVAL; + + pllcon = readl_relaxed(pll->reg_base + pllcon2); + pllcon &= ~(frac_mask << frac_shift); + pllcon |= fracdiv << frac_shift; + writel_relaxed(pllcon, pll->reg_base + pllcon2); + + return 0; +} +EXPORT_SYMBOL(rockchip_pll_clk_compensation); +#endif + /* * Common registering of pll clocks */