296 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Microchip Sparx5 SoC Clock driver.4 *5 * Copyright (c) 2019 Microchip Inc.6 *7 * Author: Lars Povlsen <lars.povlsen@microchip.com>8 */9 10#include <linux/io.h>11#include <linux/module.h>12#include <linux/clk-provider.h>13#include <linux/bitfield.h>14#include <linux/of.h>15#include <linux/slab.h>16#include <linux/platform_device.h>17#include <dt-bindings/clock/microchip,sparx5.h>18 19#define PLL_DIV GENMASK(7, 0)20#define PLL_PRE_DIV GENMASK(10, 8)21#define PLL_ROT_DIR BIT(11)22#define PLL_ROT_SEL GENMASK(13, 12)23#define PLL_ROT_ENA BIT(14)24#define PLL_CLK_ENA BIT(15)25 26#define MAX_SEL 427#define MAX_PRE BIT(3)28 29static const u8 sel_rates[MAX_SEL] = { 0, 2*8, 2*4, 2*2 };30 31static const char *clk_names[N_CLOCKS] = {32 "core", "ddr", "cpu2", "arm2",33 "aux1", "aux2", "aux3", "aux4",34 "synce",35};36 37struct s5_hw_clk {38 struct clk_hw hw;39 void __iomem *reg;40};41 42struct s5_clk_data {43 void __iomem *base;44 struct s5_hw_clk s5_hw[N_CLOCKS];45};46 47struct s5_pll_conf {48 unsigned long freq;49 u8 div;50 bool rot_ena;51 u8 rot_sel;52 u8 rot_dir;53 u8 pre_div;54};55 56#define to_s5_pll(hw) container_of(hw, struct s5_hw_clk, hw)57 58static unsigned long s5_calc_freq(unsigned long parent_rate,59 const struct s5_pll_conf *conf)60{61 unsigned long rate = parent_rate / conf->div;62 63 if (conf->rot_ena) {64 int sign = conf->rot_dir ? -1 : 1;65 int divt = sel_rates[conf->rot_sel] * (1 + conf->pre_div);66 int divb = divt + sign;67 68 rate = mult_frac(rate, divt, divb);69 rate = roundup(rate, 1000);70 }71 72 return rate;73}74 75static void s5_search_fractional(unsigned long rate,76 unsigned long parent_rate,77 int div,78 struct s5_pll_conf *conf)79{80 struct s5_pll_conf best;81 ulong cur_offset, best_offset = rate;82 int d, i, j;83 84 memset(conf, 0, sizeof(*conf));85 conf->div = div;86 conf->rot_ena = 1; /* Fractional rate */87 88 for (d = 0; best_offset > 0 && d <= 1 ; d++) {89 conf->rot_dir = !!d;90 for (i = 0; best_offset > 0 && i < MAX_PRE; i++) {91 conf->pre_div = i;92 for (j = 1; best_offset > 0 && j < MAX_SEL; j++) {93 conf->rot_sel = j;94 conf->freq = s5_calc_freq(parent_rate, conf);95 cur_offset = abs(rate - conf->freq);96 if (cur_offset < best_offset) {97 best_offset = cur_offset;98 best = *conf;99 }100 }101 }102 }103 104 /* Best match */105 *conf = best;106}107 108static unsigned long s5_calc_params(unsigned long rate,109 unsigned long parent_rate,110 struct s5_pll_conf *conf)111{112 if (parent_rate % rate) {113 struct s5_pll_conf alt1, alt2;114 int div;115 116 div = DIV_ROUND_CLOSEST_ULL(parent_rate, rate);117 s5_search_fractional(rate, parent_rate, div, &alt1);118 119 /* Straight match? */120 if (alt1.freq == rate) {121 *conf = alt1;122 } else {123 /* Try without rounding divider */124 div = parent_rate / rate;125 if (div != alt1.div) {126 s5_search_fractional(rate, parent_rate, div,127 &alt2);128 /* Select the better match */129 if (abs(rate - alt1.freq) <130 abs(rate - alt2.freq))131 *conf = alt1;132 else133 *conf = alt2;134 }135 }136 } else {137 /* Straight fit */138 memset(conf, 0, sizeof(*conf));139 conf->div = parent_rate / rate;140 }141 142 return conf->freq;143}144 145static int s5_pll_enable(struct clk_hw *hw)146{147 struct s5_hw_clk *pll = to_s5_pll(hw);148 u32 val = readl(pll->reg);149 150 val |= PLL_CLK_ENA;151 writel(val, pll->reg);152 153 return 0;154}155 156static void s5_pll_disable(struct clk_hw *hw)157{158 struct s5_hw_clk *pll = to_s5_pll(hw);159 u32 val = readl(pll->reg);160 161 val &= ~PLL_CLK_ENA;162 writel(val, pll->reg);163}164 165static int s5_pll_set_rate(struct clk_hw *hw,166 unsigned long rate,167 unsigned long parent_rate)168{169 struct s5_hw_clk *pll = to_s5_pll(hw);170 struct s5_pll_conf conf;171 unsigned long eff_rate;172 u32 val;173 174 eff_rate = s5_calc_params(rate, parent_rate, &conf);175 if (eff_rate != rate)176 return -EOPNOTSUPP;177 178 val = readl(pll->reg) & PLL_CLK_ENA;179 val |= FIELD_PREP(PLL_DIV, conf.div);180 if (conf.rot_ena) {181 val |= PLL_ROT_ENA;182 val |= FIELD_PREP(PLL_ROT_SEL, conf.rot_sel);183 val |= FIELD_PREP(PLL_PRE_DIV, conf.pre_div);184 if (conf.rot_dir)185 val |= PLL_ROT_DIR;186 }187 writel(val, pll->reg);188 189 return 0;190}191 192static unsigned long s5_pll_recalc_rate(struct clk_hw *hw,193 unsigned long parent_rate)194{195 struct s5_hw_clk *pll = to_s5_pll(hw);196 struct s5_pll_conf conf;197 u32 val;198 199 val = readl(pll->reg);200 201 if (val & PLL_CLK_ENA) {202 conf.div = FIELD_GET(PLL_DIV, val);203 conf.pre_div = FIELD_GET(PLL_PRE_DIV, val);204 conf.rot_ena = FIELD_GET(PLL_ROT_ENA, val);205 conf.rot_dir = FIELD_GET(PLL_ROT_DIR, val);206 conf.rot_sel = FIELD_GET(PLL_ROT_SEL, val);207 208 conf.freq = s5_calc_freq(parent_rate, &conf);209 } else {210 conf.freq = 0;211 }212 213 return conf.freq;214}215 216static long s5_pll_round_rate(struct clk_hw *hw, unsigned long rate,217 unsigned long *parent_rate)218{219 struct s5_pll_conf conf;220 221 return s5_calc_params(rate, *parent_rate, &conf);222}223 224static const struct clk_ops s5_pll_ops = {225 .enable = s5_pll_enable,226 .disable = s5_pll_disable,227 .set_rate = s5_pll_set_rate,228 .round_rate = s5_pll_round_rate,229 .recalc_rate = s5_pll_recalc_rate,230};231 232static struct clk_hw *s5_clk_hw_get(struct of_phandle_args *clkspec, void *data)233{234 struct s5_clk_data *s5_clk = data;235 unsigned int idx = clkspec->args[0];236 237 if (idx >= N_CLOCKS) {238 pr_err("%s: invalid index %u\n", __func__, idx);239 return ERR_PTR(-EINVAL);240 }241 242 return &s5_clk->s5_hw[idx].hw;243}244 245static int s5_clk_probe(struct platform_device *pdev)246{247 struct device *dev = &pdev->dev;248 int i, ret;249 struct s5_clk_data *s5_clk;250 struct clk_parent_data pdata = { .index = 0 };251 struct clk_init_data init = {252 .ops = &s5_pll_ops,253 .num_parents = 1,254 .parent_data = &pdata,255 };256 257 s5_clk = devm_kzalloc(dev, sizeof(*s5_clk), GFP_KERNEL);258 if (!s5_clk)259 return -ENOMEM;260 261 s5_clk->base = devm_platform_ioremap_resource(pdev, 0);262 if (IS_ERR(s5_clk->base))263 return PTR_ERR(s5_clk->base);264 265 for (i = 0; i < N_CLOCKS; i++) {266 struct s5_hw_clk *s5_hw = &s5_clk->s5_hw[i];267 268 init.name = clk_names[i];269 s5_hw->reg = s5_clk->base + (i * 4);270 s5_hw->hw.init = &init;271 ret = devm_clk_hw_register(dev, &s5_hw->hw);272 if (ret) {273 dev_err(dev, "failed to register %s clock\n",274 init.name);275 return ret;276 }277 }278 279 return devm_of_clk_add_hw_provider(dev, s5_clk_hw_get, s5_clk);280}281 282static const struct of_device_id s5_clk_dt_ids[] = {283 { .compatible = "microchip,sparx5-dpll", },284 { }285};286MODULE_DEVICE_TABLE(of, s5_clk_dt_ids);287 288static struct platform_driver s5_clk_driver = {289 .probe = s5_clk_probe,290 .driver = {291 .name = "sparx5-clk",292 .of_match_table = s5_clk_dt_ids,293 },294};295builtin_platform_driver(s5_clk_driver);296