clk: sparx5: Add Sparx5 SoC DPLL clock driver

This adds a device driver for the Sparx5 SoC DPLL clock

Signed-off-by: Lars Povlsen <lars.povlsen@microchip.com>
Link: https://lore.kernel.org/r/20200727084211.6632-9-lars.povlsen@microchip.com
Signed-off-by: Stephen Boyd <sboyd@kernel.org>

diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index ca9af11d3391301fb938638ab9365a6e4aee81cf..da8fcf147eb132ce2a15f5f748912775d893ac5a 100644 (file)
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -28,6 +28,7 @@ obj-$(CONFIG_COMMON_CLK_CDCE925)      += clk-cdce925.o
 obj-$(CONFIG_ARCH_CLPS711X)            += clk-clps711x.o
 obj-$(CONFIG_COMMON_CLK_CS2000_CP)     += clk-cs2000-cp.o
 obj-$(CONFIG_ARCH_EFM32)               += clk-efm32gg.o
+obj-$(CONFIG_ARCH_SPARX5)              += clk-sparx5.o
 obj-$(CONFIG_COMMON_CLK_FIXED_MMIO)    += clk-fixed-mmio.o
 obj-$(CONFIG_COMMON_CLK_FSL_SAI)       += clk-fsl-sai.o
 obj-$(CONFIG_COMMON_CLK_GEMINI)                += clk-gemini.o

diff --git a/drivers/clk/clk-sparx5.c b/drivers/clk/clk-sparx5.c
new file mode 100644 (file)
index 0000000..0fad0c1
--- /dev/null
+++ b/drivers/clk/clk-sparx5.c
@@ -0,0 +1,295 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Microchip Sparx5 SoC Clock driver.
+ *
+ * Copyright (c) 2019 Microchip Inc.
+ *
+ * Author: Lars Povlsen <lars.povlsen@microchip.com>
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/clk-provider.h>
+#include <linux/bitfield.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <dt-bindings/clock/microchip,sparx5.h>
+
+#define PLL_DIV                GENMASK(7, 0)
+#define PLL_PRE_DIV    GENMASK(10, 8)
+#define PLL_ROT_DIR    BIT(11)
+#define PLL_ROT_SEL    GENMASK(13, 12)
+#define PLL_ROT_ENA    BIT(14)
+#define PLL_CLK_ENA    BIT(15)
+
+#define MAX_SEL 4
+#define MAX_PRE BIT(3)
+
+static const u8 sel_rates[MAX_SEL] = { 0, 2*8, 2*4, 2*2 };
+
+static const char *clk_names[N_CLOCKS] = {
+       "core", "ddr", "cpu2", "arm2",
+       "aux1", "aux2", "aux3", "aux4",
+       "synce",
+};
+
+struct s5_hw_clk {
+       struct clk_hw hw;
+       void __iomem *reg;
+};
+
+struct s5_clk_data {
+       void __iomem *base;
+       struct s5_hw_clk s5_hw[N_CLOCKS];
+};
+
+struct s5_pll_conf {
+       unsigned long freq;
+       u8 div;
+       bool rot_ena;
+       u8 rot_sel;
+       u8 rot_dir;
+       u8 pre_div;
+};
+
+#define to_s5_pll(hw) container_of(hw, struct s5_hw_clk, hw)
+
+static unsigned long s5_calc_freq(unsigned long parent_rate,
+                                 const struct s5_pll_conf *conf)
+{
+       unsigned long rate = parent_rate / conf->div;
+
+       if (conf->rot_ena) {
+               int sign = conf->rot_dir ? -1 : 1;
+               int divt = sel_rates[conf->rot_sel] * (1 + conf->pre_div);
+               int divb = divt + sign;
+
+               rate = mult_frac(rate, divt, divb);
+               rate = roundup(rate, 1000);
+       }
+
+       return rate;
+}
+
+static void s5_search_fractional(unsigned long rate,
+                                unsigned long parent_rate,
+                                int div,
+                                struct s5_pll_conf *conf)
+{
+       struct s5_pll_conf best;
+       ulong cur_offset, best_offset = rate;
+       int d, i, j;
+
+       memset(conf, 0, sizeof(*conf));
+       conf->div = div;
+       conf->rot_ena = 1;      /* Fractional rate */
+
+       for (d = 0; best_offset > 0 && d <= 1 ; d++) {
+               conf->rot_dir = !!d;
+               for (i = 0; best_offset > 0 && i < MAX_PRE; i++) {
+                       conf->pre_div = i;
+                       for (j = 1; best_offset > 0 && j < MAX_SEL; j++) {
+                               conf->rot_sel = j;
+                               conf->freq = s5_calc_freq(parent_rate, conf);
+                               cur_offset = abs(rate - conf->freq);
+                               if (cur_offset < best_offset) {
+                                       best_offset = cur_offset;
+                                       best = *conf;
+                               }
+                       }
+               }
+       }
+
+       /* Best match */
+       *conf = best;
+}
+
+static unsigned long s5_calc_params(unsigned long rate,
+                                   unsigned long parent_rate,
+                                   struct s5_pll_conf *conf)
+{
+       if (parent_rate % rate) {
+               struct s5_pll_conf alt1, alt2;
+               int div;
+
+               div = DIV_ROUND_CLOSEST_ULL(parent_rate, rate);
+               s5_search_fractional(rate, parent_rate, div, &alt1);
+
+               /* Straight match? */
+               if (alt1.freq == rate) {
+                       *conf = alt1;
+               } else {
+                       /* Try without rounding divider */
+                       div = parent_rate / rate;
+                       if (div != alt1.div) {
+                               s5_search_fractional(rate, parent_rate, div,
+                                                    &alt2);
+                               /* Select the better match */
+                               if (abs(rate - alt1.freq) <
+                                   abs(rate - alt2.freq))
+                                       *conf = alt1;
+                               else
+                                       *conf = alt2;
+                       }
+               }
+       } else {
+               /* Straight fit */
+               memset(conf, 0, sizeof(*conf));
+               conf->div = parent_rate / rate;
+       }
+
+       return conf->freq;
+}
+
+static int s5_pll_enable(struct clk_hw *hw)
+{
+       struct s5_hw_clk *pll = to_s5_pll(hw);
+       u32 val = readl(pll->reg);
+
+       val |= PLL_CLK_ENA;
+       writel(val, pll->reg);
+
+       return 0;
+}
+
+static void s5_pll_disable(struct clk_hw *hw)
+{
+       struct s5_hw_clk *pll = to_s5_pll(hw);
+       u32 val = readl(pll->reg);
+
+       val &= ~PLL_CLK_ENA;
+       writel(val, pll->reg);
+}
+
+static int s5_pll_set_rate(struct clk_hw *hw,
+                          unsigned long rate,
+                          unsigned long parent_rate)
+{
+       struct s5_hw_clk *pll = to_s5_pll(hw);
+       struct s5_pll_conf conf;
+       unsigned long eff_rate;
+       u32 val;
+
+       eff_rate = s5_calc_params(rate, parent_rate, &conf);
+       if (eff_rate != rate)
+               return -EOPNOTSUPP;
+
+       val = readl(pll->reg) & PLL_CLK_ENA;
+       val |= FIELD_PREP(PLL_DIV, conf.div);
+       if (conf.rot_ena) {
+               val |= PLL_ROT_ENA;
+               val |= FIELD_PREP(PLL_ROT_SEL, conf.rot_sel);
+               val |= FIELD_PREP(PLL_PRE_DIV, conf.pre_div);
+               if (conf.rot_dir)
+                       val |= PLL_ROT_DIR;
+       }
+       writel(val, pll->reg);
+
+       return 0;
+}
+
+static unsigned long s5_pll_recalc_rate(struct clk_hw *hw,
+                                       unsigned long parent_rate)
+{
+       struct s5_hw_clk *pll = to_s5_pll(hw);
+       struct s5_pll_conf conf;
+       u32 val;
+
+       val = readl(pll->reg);
+
+       if (val & PLL_CLK_ENA) {
+               conf.div     = FIELD_GET(PLL_DIV, val);
+               conf.pre_div = FIELD_GET(PLL_PRE_DIV, val);
+               conf.rot_ena = FIELD_GET(PLL_ROT_ENA, val);
+               conf.rot_dir = FIELD_GET(PLL_ROT_DIR, val);
+               conf.rot_sel = FIELD_GET(PLL_ROT_SEL, val);
+
+               conf.freq = s5_calc_freq(parent_rate, &conf);
+       } else {
+               conf.freq = 0;
+       }
+
+       return conf.freq;
+}
+
+static long s5_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+                             unsigned long *parent_rate)
+{
+       struct s5_pll_conf conf;
+
+       return s5_calc_params(rate, *parent_rate, &conf);
+}
+
+static const struct clk_ops s5_pll_ops = {
+       .enable         = s5_pll_enable,
+       .disable        = s5_pll_disable,
+       .set_rate       = s5_pll_set_rate,
+       .round_rate     = s5_pll_round_rate,
+       .recalc_rate    = s5_pll_recalc_rate,
+};
+
+static struct clk_hw *s5_clk_hw_get(struct of_phandle_args *clkspec, void *data)
+{
+       struct s5_clk_data *s5_clk = data;
+       unsigned int idx = clkspec->args[0];
+
+       if (idx >= N_CLOCKS) {
+               pr_err("%s: invalid index %u\n", __func__, idx);
+               return ERR_PTR(-EINVAL);
+       }
+
+       return &s5_clk->s5_hw[idx].hw;
+}
+
+static int s5_clk_probe(struct platform_device *pdev)
+{
+       struct device *dev = &pdev->dev;
+       int i, ret;
+       struct s5_clk_data *s5_clk;
+       struct clk_parent_data pdata = { .index = 0 };
+       struct clk_init_data init = {
+               .ops = &s5_pll_ops,
+               .num_parents = 1,
+               .parent_data = &pdata,
+       };
+
+       s5_clk = devm_kzalloc(dev, sizeof(*s5_clk), GFP_KERNEL);
+       if (!s5_clk)
+               return -ENOMEM;
+
+       s5_clk->base = devm_platform_ioremap_resource(pdev, 0);
+       if (IS_ERR(s5_clk->base))
+               return PTR_ERR(s5_clk->base);
+
+       for (i = 0; i < N_CLOCKS; i++) {
+               struct s5_hw_clk *s5_hw = &s5_clk->s5_hw[i];
+
+               init.name = clk_names[i];
+               s5_hw->reg = s5_clk->base + (i * 4);
+               s5_hw->hw.init = &init;
+               ret = devm_clk_hw_register(dev, &s5_hw->hw);
+               if (ret) {
+                       dev_err(dev, "failed to register %s clock\n",
+                               init.name);
+                       return ret;
+               }
+       }
+
+       return devm_of_clk_add_hw_provider(dev, s5_clk_hw_get, s5_clk);
+}
+
+static const struct of_device_id s5_clk_dt_ids[] = {
+       { .compatible = "microchip,sparx5-dpll", },
+       { }
+};
+MODULE_DEVICE_TABLE(of, s5_clk_dt_ids);
+
+static struct platform_driver s5_clk_driver = {
+       .probe  = s5_clk_probe,
+       .driver = {
+               .name = "sparx5-clk",
+               .of_match_table = s5_clk_dt_ids,
+       },
+};
+builtin_platform_driver(s5_clk_driver);