clk: vt8500: Add support for WM8750/WM8850 PLL clocks

This patch adds support for the new PLL module found in WM8750 and
WM8850 SoCs.

Signed-off-by: Tony Prisk <linux@prisktech.co.nz>
Signed-off-by: Mike Turquette <mturquette@linaro.org>

1 files changed, 100 insertions, 2 deletions

diff --git a/drivers/clk/clk-vt8500.c b/drivers/clk/clk-vt8500.c
index db7d41f25046..2515d4f31758 100644
--- a/drivers/clk/clk-vt8500.c
+++ b/drivers/clk/clk-vt8500.c
@@ -41,6 +41,7 @@ struct clk_device {
 
 #define PLL_TYPE_VT8500         0
 #define PLL_TYPE_WM8650         1
+#define PLL_TYPE_WM8750         2
 
 struct clk_pll {
         struct clk_hw   hw;
@@ -316,6 +317,16 @@ static __init void vtwm_device_clk_init(struct device_node *node)
 #define WM8650_BITS_TO_VAL(m, d1, d2)                                   \
                                 ((d2 << 13) | (d1 << 10) | (m & 0x3FF))
 
+/* Helper macros for PLL_WM8750 */
+#define WM8750_PLL_MUL(x)       (((x >> 16) & 0xFF) + 1)
+#define WM8750_PLL_DIV(x)       ((((x >> 8) & 1) + 1) * (1 << (x & 7)))
+
+#define WM8750_BITS_TO_FREQ(r, m, d1, d2)                               \
+                                (r * (m+1) / ((d1+1) * (1 << d2)))
+
+#define WM8750_BITS_TO_VAL(f, m, d1, d2)                                \
+                ((f << 24) | ((m - 1) << 16) | ((d1 - 1) << 8) | d2)
+
 
 static void vt8500_find_pll_bits(unsigned long rate, unsigned long parent_rate,
                                 u32 *multiplier, u32 *prediv)
@@ -384,11 +395,82 @@ static void wm8650_find_pll_bits(unsigned long rate, unsigned long parent_rate,
         *divisor2 = best_div2;
 }
 
+static u32 wm8750_get_filter(u32 parent_rate, u32 divisor1)
+{
+        /* calculate frequency (MHz) after pre-divisor */
+        u32 freq = (parent_rate / 1000000) / (divisor1 + 1);
+
+        if ((freq < 10) || (freq > 200))
+                pr_warn("%s: PLL recommended input frequency 10..200Mhz (requested %d Mhz)\n",
+                                __func__, freq);
+
+        if (freq >= 166)
+                return 7;
+        else if (freq >= 104)
+                return 6;
+        else if (freq >= 65)
+                return 5;
+        else if (freq >= 42)
+                return 4;
+        else if (freq >= 26)
+                return 3;
+        else if (freq >= 16)
+                return 2;
+        else if (freq >= 10)
+                return 1;
+
+        return 0;
+}
+
+static void wm8750_find_pll_bits(unsigned long rate, unsigned long parent_rate,
+                                u32 *filter, u32 *multiplier, u32 *divisor1, u32 *divisor2)
+{
+        u32 mul, div1, div2;
+        u32 best_mul, best_div1, best_div2;
+        unsigned long tclk, rate_err, best_err;
+
+        best_err = (unsigned long)-1;
+
+        /* Find the closest match (lower or equal to requested) */
+        for (div1 = 1; div1 >= 0; div1--)
+                for (div2 = 7; div2 >= 0; div2--)
+                        for (mul = 0; mul <= 255; mul++) {
+                                tclk = parent_rate * (mul + 1) / ((div1 + 1) * (1 << div2));
+                                if (tclk > rate)
+                                        continue;
+                                /* error will always be +ve */
+                                rate_err = rate - tclk;
+                                if (rate_err == 0) {
+                                        *filter = wm8750_get_filter(parent_rate, div1);
+                                        *multiplier = mul;
+                                        *divisor1 = div1;
+                                        *divisor2 = div2;
+                                        return;
+                                }
+
+                                if (rate_err < best_err) {
+                                        best_err = rate_err;
+                                        best_mul = mul;
+                                        best_div1 = div1;
+                                        best_div2 = div2;
+                                }
+                        }
+
+        /* if we got here, it wasn't an exact match */
+        pr_warn("%s: requested rate %lu, found rate %lu\n", __func__, rate,
+                                                        rate - best_err);
+
+        *filter = wm8750_get_filter(parent_rate, best_div1);
+        *multiplier = best_mul;
+        *divisor1 = best_div1;
+        *divisor2 = best_div2;
+}
+
 static int vtwm_pll_set_rate(struct clk_hw *hw, unsigned long rate,
                                 unsigned long parent_rate)
 {
         struct clk_pll *pll = to_clk_pll(hw);
-        u32 mul, div1, div2;
+        u32 filter, mul, div1, div2;
         u32 pll_val;
         unsigned long flags = 0;
 
@@ -403,6 +485,9 @@ static int vtwm_pll_set_rate(struct clk_hw *hw, unsigned long rate,
                 wm8650_find_pll_bits(rate, parent_rate, &mul, &div1, &div2);
                 pll_val = WM8650_BITS_TO_VAL(mul, div1, div2);
                 break;
+        case PLL_TYPE_WM8750:
+                wm8750_find_pll_bits(rate, parent_rate, &filter, &mul, &div1, &div2);
+                pll_val = WM8750_BITS_TO_VAL(filter, mul, div1, div2);
         default:
                 pr_err("%s: invalid pll type\n", __func__);
                 return 0;
@@ -423,7 +508,7 @@ static long vtwm_pll_round_rate(struct clk_hw *hw, unsigned long rate,
                                 unsigned long *prate)
 {
         struct clk_pll *pll = to_clk_pll(hw);
-        u32 mul, div1, div2;
+        u32 filter, mul, div1, div2;
         long round_rate;
 
         switch (pll->type) {
@@ -435,6 +520,9 @@ static long vtwm_pll_round_rate(struct clk_hw *hw, unsigned long rate,
                 wm8650_find_pll_bits(rate, *prate, &mul, &div1, &div2);
                 round_rate = WM8650_BITS_TO_FREQ(*prate, mul, div1, div2);
                 break;
+        case PLL_TYPE_WM8750:
+                wm8750_find_pll_bits(rate, *prate, &filter, &mul, &div1, &div2);
+                round_rate = WM8750_BITS_TO_FREQ(*prate, mul, div1, div2);
         default:
                 round_rate = 0;
         }
@@ -458,6 +546,10 @@ static unsigned long vtwm_pll_recalc_rate(struct clk_hw *hw,
                 pll_freq = parent_rate * WM8650_PLL_MUL(pll_val);
                 pll_freq /= WM8650_PLL_DIV(pll_val);
                 break;
+        case PLL_TYPE_WM8750:
+                pll_freq = parent_rate * WM8750_PLL_MUL(pll_val);
+                pll_freq /= WM8750_PLL_DIV(pll_val);
+                break;
         default:
                 pll_freq = 0;
         }
@@ -526,10 +618,16 @@ static void __init wm8650_pll_init(struct device_node *node)
         vtwm_pll_clk_init(node, PLL_TYPE_WM8650);
 }
 
+static void __init wm8750_pll_init(struct device_node *node)
+{
+        vtwm_pll_clk_init(node, PLL_TYPE_WM8750);
+}
+
 static const __initconst struct of_device_id clk_match[] = {
         { .compatible = "fixed-clock", .data = of_fixed_clk_setup, },
         { .compatible = "via,vt8500-pll-clock", .data = vt8500_pll_init, },
         { .compatible = "wm,wm8650-pll-clock", .data = wm8650_pll_init, },
+        { .compatible = "wm,wm8750-pll-clock", .data = wm8750_pll_init, },
         { .compatible = "via,vt8500-device-clock",
                                         .data = vtwm_device_clk_init, },
         { /* sentinel */ }