1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
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 4 |
27 | #define MAX_PRE BIT(3) |
28 | |
29 | static const u8 sel_rates[MAX_SEL] = { 0, 2*8, 2*4, 2*2 }; |
30 | |
31 | static const char *clk_names[N_CLOCKS] = { |
32 | "core" , "ddr" , "cpu2" , "arm2" , |
33 | "aux1" , "aux2" , "aux3" , "aux4" , |
34 | "synce" , |
35 | }; |
36 | |
37 | struct s5_hw_clk { |
38 | struct clk_hw hw; |
39 | void __iomem *reg; |
40 | }; |
41 | |
42 | struct s5_clk_data { |
43 | void __iomem *base; |
44 | struct s5_hw_clk s5_hw[N_CLOCKS]; |
45 | }; |
46 | |
47 | struct 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 | |
58 | static 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 | |
75 | static 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 | |
108 | static 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, conf: &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 | conf: &alt2); |
128 | /* Select the better match */ |
129 | if (abs(rate - alt1.freq) < |
130 | abs(rate - alt2.freq)) |
131 | *conf = alt1; |
132 | else |
133 | *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 | |
145 | static int s5_pll_enable(struct clk_hw *hw) |
146 | { |
147 | struct s5_hw_clk *pll = to_s5_pll(hw); |
148 | u32 val = readl(addr: pll->reg); |
149 | |
150 | val |= PLL_CLK_ENA; |
151 | writel(val, addr: pll->reg); |
152 | |
153 | return 0; |
154 | } |
155 | |
156 | static void s5_pll_disable(struct clk_hw *hw) |
157 | { |
158 | struct s5_hw_clk *pll = to_s5_pll(hw); |
159 | u32 val = readl(addr: pll->reg); |
160 | |
161 | val &= ~PLL_CLK_ENA; |
162 | writel(val, addr: pll->reg); |
163 | } |
164 | |
165 | static 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: &conf); |
175 | if (eff_rate != rate) |
176 | return -EOPNOTSUPP; |
177 | |
178 | val = readl(addr: 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, addr: pll->reg); |
188 | |
189 | return 0; |
190 | } |
191 | |
192 | static 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(addr: 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: &conf); |
209 | } else { |
210 | conf.freq = 0; |
211 | } |
212 | |
213 | return conf.freq; |
214 | } |
215 | |
216 | static 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: *parent_rate, conf: &conf); |
222 | } |
223 | |
224 | static 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 | |
232 | static 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(error: -EINVAL); |
240 | } |
241 | |
242 | return &s5_clk->s5_hw[idx].hw; |
243 | } |
244 | |
245 | static 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, size: sizeof(*s5_clk), GFP_KERNEL); |
258 | if (!s5_clk) |
259 | return -ENOMEM; |
260 | |
261 | s5_clk->base = devm_platform_ioremap_resource(pdev, index: 0); |
262 | if (IS_ERR(ptr: s5_clk->base)) |
263 | return PTR_ERR(ptr: 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, hw: &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, get: s5_clk_hw_get, data: s5_clk); |
280 | } |
281 | |
282 | static const struct of_device_id s5_clk_dt_ids[] = { |
283 | { .compatible = "microchip,sparx5-dpll" , }, |
284 | { } |
285 | }; |
286 | MODULE_DEVICE_TABLE(of, s5_clk_dt_ids); |
287 | |
288 | static 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 | }; |
295 | builtin_platform_driver(s5_clk_driver); |
296 | |