[PATCH v2 08/21] clk: imx: pllv3: add PLLV3_SYS support

[Giulio Benetti]

Add PLLV3_SYS support by adding set/get_rate() for PLLV3_SYS but keeping
generic enable()/disable(). Add a different driver because ops are
different respect to GENERIC/USB.

Signed-off-by: Giulio Benetti <giulio.benetti at benettiengineering.com>
Reviewed-by: Lukasz Majewski <lukma at denx.de>
---
 drivers/clk/imx/clk-pllv3.c | 53 +++++++++++++++++++++++++++++++++++++
 1 file changed, 53 insertions(+)

diff --git a/drivers/clk/imx/clk-pllv3.c b/drivers/clk/imx/clk-pllv3.c
index a721dbee94..d5087a104e 100644
--- a/drivers/clk/imx/clk-pllv3.c
+++ b/drivers/clk/imx/clk-pllv3.c
@@ -14,6 +14,7 @@
 #include "clk.h"
 
 #define UBOOT_DM_CLK_IMX_PLLV3_GENERIC	"imx_clk_pllv3_generic"
+#define UBOOT_DM_CLK_IMX_PLLV3_SYS	"imx_clk_pllv3_sys"
 #define UBOOT_DM_CLK_IMX_PLLV3_USB	"imx_clk_pllv3_usb"
 
 #define BM_PLL_POWER		(0x1 << 12)
@@ -102,6 +103,46 @@ static const struct clk_ops clk_pllv3_generic_ops = {
 	.set_rate	= clk_pllv3_generic_set_rate,
 };
 
+static ulong clk_pllv3_sys_get_rate(struct clk *clk)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(clk);
+	unsigned long parent_rate = clk_get_parent_rate(clk);
+	u32 div = readl(pll->base) & pll->div_mask;
+
+	return parent_rate * div / 2;
+}
+
+static ulong clk_pllv3_sys_set_rate(struct clk *clk, ulong rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(clk);
+	unsigned long parent_rate = clk_get_parent_rate(clk);
+	unsigned long min_rate = parent_rate * 54 / 2;
+	unsigned long max_rate = parent_rate * 108 / 2;
+	u32 val, div;
+
+	if (rate < min_rate || rate > max_rate)
+		return -EINVAL;
+
+	div = rate * 2 / parent_rate;
+	val = readl(pll->base);
+	val &= ~pll->div_mask;
+	val |= div;
+	writel(val, pll->base);
+
+	/* Wait for PLL to lock */
+	while (!(readl(pll->base) & BM_PLL_LOCK))
+		;
+
+	return 0;
+}
+
+static const struct clk_ops clk_pllv3_sys_ops = {
+	.enable 	= clk_pllv3_generic_enable,
+	.disable	= clk_pllv3_generic_disable,
+	.get_rate	= clk_pllv3_sys_get_rate,
+	.set_rate	= clk_pllv3_sys_set_rate,
+};
+
 struct clk *imx_clk_pllv3(enum imx_pllv3_type type, const char *name,
 			  const char *parent_name, void __iomem *base,
 			  u32 div_mask)
@@ -123,6 +164,11 @@ struct clk *imx_clk_pllv3(enum imx_pllv3_type type, const char *name,
 		pll->div_shift = 0;
 		pll->powerup_set = false;
 		break;
+	case IMX_PLLV3_SYS:
+		drv_name = UBOOT_DM_CLK_IMX_PLLV3_SYS;
+		pll->div_shift = 0;
+		pll->powerup_set = false;
+		break;
 	case IMX_PLLV3_USB:
 		drv_name = UBOOT_DM_CLK_IMX_PLLV3_USB;
 		pll->div_shift = 1;
@@ -153,6 +199,13 @@ U_BOOT_DRIVER(clk_pllv3_generic) = {
 	.flags = DM_FLAG_PRE_RELOC,
 };
 
+U_BOOT_DRIVER(clk_pllv3_sys) = {
+	.name	= UBOOT_DM_CLK_IMX_PLLV3_SYS,
+	.id	= UCLASS_CLK,
+	.ops	= &clk_pllv3_sys_ops,
+	.flags = DM_FLAG_PRE_RELOC,
+};
+
 U_BOOT_DRIVER(clk_pllv3_usb) = {
 	.name	= UBOOT_DM_CLK_IMX_PLLV3_USB,
 	.id	= UCLASS_CLK,
-- 
2.20.1