clock: Support for [A|M]PLL's (35xx) set_rate and round_rate functions

Support for [A|M]PLL's [set|round]_rate functions has been added.
Now simple call to clk_set_rate is responsible for setting proper
frequency.

This commit allows for further refactoring of exynos4x12-cpufreq.c code.

Signed-off-by: Lukasz Majewski <l.majewski@samsung.com>

clk: Fix bug on setting xpll's pms value in clk-pll.c

Signed-off-by: Jonghwa Lee <jonghwa3.lee@samsung.com>

3 files changed, 146 insertions, 12 deletions

diff --git a/drivers/clk/samsung/clk-exynos4.c b/drivers/clk/samsung/clk-exynos4.c
index b4b283b4730..10ea9e19bff 100644
--- a/drivers/clk/samsung/clk-exynos4.c
+++ b/drivers/clk/samsung/clk-exynos4.c
@@ -97,12 +97,14 @@
 #define GATE_IP_PERIL		0xc950
 #define E4210_GATE_IP_PERIR	0xc960
 #define GATE_BLOCK		0xc970
+#define E4X12_MPLL_LOCK		0x10008
 #define E4X12_MPLL_CON0		0x10108
 #define SRC_DMC			0x10200
 #define SRC_MASK_DMC		0x10300
 #define DIV_DMC0		0x10500
 #define DIV_DMC1		0x10504
 #define GATE_IP_DMC		0x10900
+#define APLL_LOCK		0x14000
 #define APLL_CON0		0x14100
 #define E4210_MPLL_CON0		0x14108
 #define SRC_CPU			0x14200
@@ -983,6 +985,25 @@ static __initdata struct of_device_id ext_clk_match[] = {
 	{},
 };
 
+/* PLLs PMS values */
+struct pll_pms pll35xx_exynos4412_pms[] = {
+	{.p = 4, .m = 250, .s = 0},
+	{.p = 3, .m = 175, .s = 0},
+	{.p = 6, .m = 325, .s = 0},
+	{.p = 4, .m = 200, .s = 0},
+	{.p = 6, .m = 275, .s = 0},
+	{.p = 3, .m = 125, .s = 0},
+	{.p = 4, .m = 150, .s = 0},
+	{.p = 3, .m = 100, .s = 0},
+	{.p = 3, .m = 175, .s = 1},
+	{.p = 4, .m = 200, .s = 1},
+	{.p = 3, .m = 125, .s = 1},
+	{.p = 3, .m = 100, .s = 1},
+	{.p = 4, .m = 200, .s = 2},
+	{.p = 3, .m = 100, .s = 2},
+	{.f_out = F_OUT_INVAL},
+};
+
 /* register exynos4 clocks */
 void __init exynos4_clk_init(struct device_node *np, enum exynos4_soc exynos4_soc, void __iomem *reg_base, unsigned long xom)
 {
@@ -1021,9 +1042,9 @@ void __init exynos4_clk_init(struct device_node *np, enum exynos4_soc exynos4_so
 					reg_base + VPLL_CON0, pll_4650c);
 	} else {
 		apll = samsung_clk_register_pll35xx("fout_apll", "fin_pll",
-					reg_base + APLL_CON0);
+			reg_base + APLL_LOCK, pll35xx_exynos4412_pms);
 		mpll = samsung_clk_register_pll35xx("fout_mpll", "fin_pll",
-					reg_base + E4X12_MPLL_CON0);
+			reg_base + E4X12_MPLL_LOCK, pll35xx_exynos4412_pms);
 		epll = samsung_clk_register_pll36xx("fout_epll", "fin_pll",
 					reg_base + EPLL_CON0);
 		vpll = samsung_clk_register_pll36xx("fout_vpll", "fin_pll",
diff --git a/drivers/clk/samsung/clk-pll.c b/drivers/clk/samsung/clk-pll.c
index 362f12dcd94..15b488229c1 100644
--- a/drivers/clk/samsung/clk-pll.c
+++ b/drivers/clk/samsung/clk-pll.c
@@ -24,11 +24,38 @@
 #define PLL35XX_PDIV_SHIFT      (8)
 #define PLL35XX_SDIV_SHIFT      (0)
 
+#define PLL35XX_PLL_LOCK        0x0
+#define PLL35XX_PLL_LOCK_CONST  270
+#define PLL35XX_PLL_CON0        0x100
+#define PLL35XX_LOCKED_SHIFT    29
+#define PLL35XX_LOCKED          (1 << PLL35XX_LOCKED_SHIFT)
+
 struct samsung_clk_pll35xx {
 	struct clk_hw		hw;
-	const void __iomem	*con_reg;
+	const void __iomem	*base_reg;
+	struct pll_pms          *pms;
 };
 
+static int get_index(unsigned long rate, struct pll_pms *pms)
+{
+	int i;
+
+	for (i = 0; pms[i].f_out != F_OUT_INVAL; i++)
+		if (pms[i].f_out == rate)
+			return i;
+
+	return -EINVAL;
+}
+
+static inline unsigned long samsung_pll35xx_calc_f_out(u64 f_in,
+							    int p, int m, int s)
+{
+	f_in *= m;
+	do_div(f_in, (p << s));
+
+	return (unsigned long) f_in;
+}
+
 #define to_clk_pll35xx(_hw) container_of(_hw, struct samsung_clk_pll35xx, hw)
 
 static unsigned long samsung_pll35xx_recalc_rate(struct clk_hw *hw,
@@ -36,29 +63,95 @@ static unsigned long samsung_pll35xx_recalc_rate(struct clk_hw *hw,
 {
 	struct samsung_clk_pll35xx *pll = to_clk_pll35xx(hw);
 	u32 mdiv, pdiv, sdiv, pll_con;
-	u64 fvco = parent_rate;
 
-	pll_con = __raw_readl(pll->con_reg);
+	pll_con = __raw_readl(pll->base_reg + PLL35XX_PLL_CON0);
 	mdiv = (pll_con >> PLL35XX_MDIV_SHIFT) & PLL35XX_MDIV_MASK;
 	pdiv = (pll_con >> PLL35XX_PDIV_SHIFT) & PLL35XX_PDIV_MASK;
 	sdiv = (pll_con >> PLL35XX_SDIV_SHIFT) & PLL35XX_SDIV_MASK;
 
-	fvco *= mdiv;
-	do_div(fvco, (pdiv << sdiv));
+	return samsung_pll35xx_calc_f_out(parent_rate, pdiv, mdiv, sdiv);
+}
 
-	return (unsigned long)fvco;
+static long samsung_pll35xx_round_rate(struct clk_hw *hw,
+				unsigned long drate, unsigned long *prate)
+{
+	struct samsung_clk_pll35xx *pll = to_clk_pll35xx(hw);
+	struct pll_pms *pms = pll->pms;
+	int i;
+
+	if (!pms) {
+		pr_err("%s: no pms table passed", __func__);
+		return -ENOTSUPP;
+	}
+
+	for (i = 0; pms[i].f_out != F_OUT_INVAL; i++)
+		if (drate >= pms[i].f_out)
+			return pms[i].f_out;
+
+	return -EINVAL;
+}
+
+static int samsung_pll35xx_set_rate(struct clk_hw *hw, unsigned long drate,
+				unsigned long prate)
+{
+	struct samsung_clk_pll35xx *pll = to_clk_pll35xx(hw);
+	u32 p = 0, m = 0, s = 0, tmp = 0;
+	struct pll_pms *pms = pll->pms;
+	int index;
+
+	if (!pms) {
+		pr_err("%s: no pms table passed", __func__);
+		return -ENOTSUPP;
+	}
+
+	index = get_index(drate, pll->pms);
+
+	/* Define PLL lock time */
+	p = pms[index].p;
+	__raw_writel((p * PLL35XX_PLL_LOCK_CONST),
+		     (u32*) (pll->base_reg + PLL35XX_PLL_LOCK));
+
+	/* Change PLL PMS */
+	m = pms[index].m;
+	s = pms[index].s;
+
+	tmp = __raw_readl(pll->base_reg + PLL35XX_PLL_CON0);
+	tmp &= ~((PLL35XX_PDIV_MASK << PLL35XX_PDIV_SHIFT) |
+		(PLL35XX_MDIV_MASK << PLL35XX_MDIV_SHIFT) |
+		(PLL35XX_SDIV_MASK << PLL35XX_SDIV_SHIFT));
+	tmp |= (p << PLL35XX_PDIV_SHIFT) | (m << PLL35XX_MDIV_SHIFT) |
+		(s << PLL35XX_SDIV_SHIFT);
+	__raw_writel(tmp, (u32*) (pll->base_reg  + PLL35XX_PLL_CON0));
+
+	/* Wait for locking */
+	do {
+		cpu_relax();
+		tmp = __raw_readl(pll->base_reg + PLL35XX_PLL_CON0);
+	} while (!(tmp & PLL35XX_LOCKED));
+
+	return 0;
 }
 
 static const struct clk_ops samsung_pll35xx_clk_ops = {
 	.recalc_rate = samsung_pll35xx_recalc_rate,
+	.round_rate = samsung_pll35xx_round_rate,
+	.set_rate = samsung_pll35xx_set_rate,
 };
 
-struct clk * __init samsung_clk_register_pll35xx(const char *name,
-			const char *pname, const void __iomem *con_reg)
+struct clk * __init
+samsung_clk_register_pll35xx(const char *name,
+			     const char *pname, const void __iomem *base_reg,
+			     struct pll_pms *pms)
 {
 	struct samsung_clk_pll35xx *pll;
 	struct clk *clk;
 	struct clk_init_data init;
+	int i;
+
+	if (!pms) {
+		pr_err("%s:  %s\n", __func__, name);
+		return NULL;
+	}
 
 	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
 	if (!pll) {
@@ -73,7 +166,8 @@ struct clk * __init samsung_clk_register_pll35xx(const char *name,
 	init.num_parents = 1;
 
 	pll->hw.init = &init;
-	pll->con_reg = con_reg;
+	pll->base_reg = base_reg;
+	pll->pms = pms;
 
 	clk = clk_register(NULL, &pll->hw);
 	if (IS_ERR(clk)) {
@@ -85,6 +179,14 @@ struct clk * __init samsung_clk_register_pll35xx(const char *name,
 	if (clk_register_clkdev(clk, name, NULL))
 		pr_err("%s: failed to register lookup for %s", __func__, name);
 
+	/* Fill in received frequency table */
+	for (i = 0; pms[i].f_out != F_OUT_INVAL; i++)
+		pms[i].f_out =
+			samsung_pll35xx_calc_f_out(clk_get_rate(clk_get_parent(clk)),
+							pms[i].p,
+							pms[i].m,
+							pms[i].s);
+
 	return clk;
 }
 
diff --git a/drivers/clk/samsung/clk-pll.h b/drivers/clk/samsung/clk-pll.h
index f33786e9a78..3ce3657b24f 100644
--- a/drivers/clk/samsung/clk-pll.h
+++ b/drivers/clk/samsung/clk-pll.h
@@ -24,8 +24,19 @@ enum pll46xx_type {
 	pll_4650c,
 };
 
+struct pll_pms {
+	unsigned int f_out;
+	int p;
+	int m;
+	int s;
+	int k;
+};
+
+#define F_OUT_INVAL ~0
+
 extern struct clk * __init samsung_clk_register_pll35xx(const char *name,
-			const char *pname, const void __iomem *con_reg);
+			const char *pname, const void __iomem *base_reg,
+			struct pll_pms *pms);
 extern struct clk * __init samsung_clk_register_pll36xx(const char *name,
 			const char *pname, const void __iomem *con_reg);
 extern struct clk * __init samsung_clk_register_pll45xx(const char *name,