1
2
3
4
5
6
7
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 4
27#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;
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
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
120 if (alt1.freq == rate) {
121 *conf = alt1;
122 } else {
123
124 div = parent_rate / rate;
125 if (div != alt1.div) {
126 s5_search_fractional(rate, parent_rate, div,
127 &alt2);
128
129 if (abs(rate - alt1.freq) <
130 abs(rate - alt2.freq))
131 *conf = alt1;
132 else
133 *conf = alt2;
134 }
135 }
136 } else {
137
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
