clk: rockchip: Add support for clk compensation

Change-Id: I099261a5906dd72dca15cbbf6acea16179c471ad
Signed-off-by: Elaine Zhang <zhangqing@rock-chips.com>
Signed-off-by: Sugar Zhang <sugar.zhang@rock-chips.com>

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

drivers/clk/rockchip/clk-pll.c

@@ -16,6 +16,7 @@
 #include <linux/regmap.h>
 #include <linux/clk.h>
 #include <linux/gcd.h>
+#include <linux/clk/rockchip.h>
 #include <linux/mfd/syscon.h>
 #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
  */