clk: imx: pllv3: add set_rate() support

Add generic set_rate() support.

Signed-off-by: Giulio Benetti <giulio.benetti@benettiengineering.com>
Reviewed-by: Lukasz Majewski <lukma@denx.de>

diff --git a/drivers/clk/imx/clk-pllv3.c b/drivers/clk/imx/clk-pllv3.c
index 9b37cd9cd9d5489f562306257f5f085a39933a98..a721dbee943be99287dbf86865bc05da867383fb 100644 (file)
--- a/drivers/clk/imx/clk-pllv3.c
+++ b/drivers/clk/imx/clk-pllv3.c
@@ -17,6 +17,7 @@
 #define UBOOT_DM_CLK_IMX_PLLV3_USB     "imx_clk_pllv3_usb"
 
 #define BM_PLL_POWER           (0x1 << 12)
+#define BM_PLL_LOCK            (0x1 << 31)
 
 struct clk_pllv3 {
        struct clk      clk;
@@ -39,6 +40,31 @@ static ulong clk_pllv3_generic_get_rate(struct clk *clk)
        return (div == 1) ? parent_rate * 22 : parent_rate * 20;
 }
 
+static ulong clk_pllv3_generic_set_rate(struct clk *clk, ulong rate)
+{
+       struct clk_pllv3 *pll = to_clk_pllv3(clk);
+       unsigned long parent_rate = clk_get_parent_rate(clk);
+       u32 val, div;
+
+       if (rate == parent_rate * 22)
+               div = 1;
+       else if (rate == parent_rate * 20)
+               div = 0;
+       else
+               return -EINVAL;
+
+       val = readl(pll->base);
+       val &= ~(pll->div_mask << pll->div_shift);
+       val |= (div << pll->div_shift);
+       writel(val, pll->base);
+
+       /* Wait for PLL to lock */
+       while (!(readl(pll->base) & BM_PLL_LOCK))
+               ;
+
+       return 0;
+}
+
 static int clk_pllv3_generic_enable(struct clk *clk)
 {
        struct clk_pllv3 *pll = to_clk_pllv3(clk);
@@ -73,6 +99,7 @@ static const struct clk_ops clk_pllv3_generic_ops = {
        .get_rate       = clk_pllv3_generic_get_rate,
        .enable         = clk_pllv3_generic_enable,
        .disable        = clk_pllv3_generic_disable,
+       .set_rate       = clk_pllv3_generic_set_rate,
 };
 
 struct clk *imx_clk_pllv3(enum imx_pllv3_type type, const char *name,