1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> |
4 | */ |
5 | |
6 | #include <linux/clk-provider.h> |
7 | #include <linux/clkdev.h> |
8 | #include <linux/clk/at91_pmc.h> |
9 | #include <linux/of.h> |
10 | #include <linux/mfd/syscon.h> |
11 | #include <linux/regmap.h> |
12 | |
13 | #include "pmc.h" |
14 | |
15 | #define SAM9X5_USB_DIV_SHIFT 8 |
16 | #define SAM9X5_USB_MAX_DIV 0xf |
17 | |
18 | #define RM9200_USB_DIV_SHIFT 28 |
19 | #define RM9200_USB_DIV_TAB_SIZE 4 |
20 | |
21 | #define SAM9X5_USBS_MASK GENMASK(0, 0) |
22 | #define SAM9X60_USBS_MASK GENMASK(1, 0) |
23 | |
24 | struct at91sam9x5_clk_usb { |
25 | struct clk_hw hw; |
26 | struct regmap *regmap; |
27 | struct at91_clk_pms pms; |
28 | u32 usbs_mask; |
29 | u8 num_parents; |
30 | }; |
31 | |
32 | #define to_at91sam9x5_clk_usb(hw) \ |
33 | container_of(hw, struct at91sam9x5_clk_usb, hw) |
34 | |
35 | struct at91rm9200_clk_usb { |
36 | struct clk_hw hw; |
37 | struct regmap *regmap; |
38 | u32 divisors[4]; |
39 | }; |
40 | |
41 | #define to_at91rm9200_clk_usb(hw) \ |
42 | container_of(hw, struct at91rm9200_clk_usb, hw) |
43 | |
44 | static unsigned long at91sam9x5_clk_usb_recalc_rate(struct clk_hw *hw, |
45 | unsigned long parent_rate) |
46 | { |
47 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
48 | unsigned int usbr; |
49 | u8 usbdiv; |
50 | |
51 | regmap_read(map: usb->regmap, AT91_PMC_USB, val: &usbr); |
52 | usbdiv = (usbr & AT91_PMC_OHCIUSBDIV) >> SAM9X5_USB_DIV_SHIFT; |
53 | |
54 | return DIV_ROUND_CLOSEST(parent_rate, (usbdiv + 1)); |
55 | } |
56 | |
57 | static int at91sam9x5_clk_usb_determine_rate(struct clk_hw *hw, |
58 | struct clk_rate_request *req) |
59 | { |
60 | struct clk_hw *parent; |
61 | long best_rate = -EINVAL; |
62 | unsigned long tmp_rate; |
63 | int best_diff = -1; |
64 | int tmp_diff; |
65 | int i; |
66 | |
67 | for (i = 0; i < clk_hw_get_num_parents(hw); i++) { |
68 | int div; |
69 | |
70 | parent = clk_hw_get_parent_by_index(hw, index: i); |
71 | if (!parent) |
72 | continue; |
73 | |
74 | for (div = 1; div < SAM9X5_USB_MAX_DIV + 2; div++) { |
75 | unsigned long tmp_parent_rate; |
76 | |
77 | tmp_parent_rate = req->rate * div; |
78 | tmp_parent_rate = clk_hw_round_rate(hw: parent, |
79 | rate: tmp_parent_rate); |
80 | if (!tmp_parent_rate) |
81 | continue; |
82 | |
83 | tmp_rate = DIV_ROUND_CLOSEST(tmp_parent_rate, div); |
84 | if (tmp_rate < req->rate) |
85 | tmp_diff = req->rate - tmp_rate; |
86 | else |
87 | tmp_diff = tmp_rate - req->rate; |
88 | |
89 | if (best_diff < 0 || best_diff > tmp_diff) { |
90 | best_rate = tmp_rate; |
91 | best_diff = tmp_diff; |
92 | req->best_parent_rate = tmp_parent_rate; |
93 | req->best_parent_hw = parent; |
94 | } |
95 | |
96 | if (!best_diff || tmp_rate < req->rate) |
97 | break; |
98 | } |
99 | |
100 | if (!best_diff) |
101 | break; |
102 | } |
103 | |
104 | if (best_rate < 0) |
105 | return best_rate; |
106 | |
107 | req->rate = best_rate; |
108 | return 0; |
109 | } |
110 | |
111 | static int at91sam9x5_clk_usb_set_parent(struct clk_hw *hw, u8 index) |
112 | { |
113 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
114 | |
115 | if (index >= usb->num_parents) |
116 | return -EINVAL; |
117 | |
118 | regmap_update_bits(map: usb->regmap, AT91_PMC_USB, mask: usb->usbs_mask, val: index); |
119 | |
120 | return 0; |
121 | } |
122 | |
123 | static u8 at91sam9x5_clk_usb_get_parent(struct clk_hw *hw) |
124 | { |
125 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
126 | unsigned int usbr; |
127 | |
128 | regmap_read(map: usb->regmap, AT91_PMC_USB, val: &usbr); |
129 | |
130 | return usbr & usb->usbs_mask; |
131 | } |
132 | |
133 | static int at91sam9x5_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate, |
134 | unsigned long parent_rate) |
135 | { |
136 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
137 | unsigned long div; |
138 | |
139 | if (!rate) |
140 | return -EINVAL; |
141 | |
142 | div = DIV_ROUND_CLOSEST(parent_rate, rate); |
143 | if (div > SAM9X5_USB_MAX_DIV + 1 || !div) |
144 | return -EINVAL; |
145 | |
146 | regmap_update_bits(map: usb->regmap, AT91_PMC_USB, AT91_PMC_OHCIUSBDIV, |
147 | val: (div - 1) << SAM9X5_USB_DIV_SHIFT); |
148 | |
149 | return 0; |
150 | } |
151 | |
152 | static int at91sam9x5_usb_save_context(struct clk_hw *hw) |
153 | { |
154 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
155 | struct clk_hw *parent_hw = clk_hw_get_parent(hw); |
156 | |
157 | usb->pms.parent = at91sam9x5_clk_usb_get_parent(hw); |
158 | usb->pms.parent_rate = clk_hw_get_rate(hw: parent_hw); |
159 | usb->pms.rate = at91sam9x5_clk_usb_recalc_rate(hw, parent_rate: usb->pms.parent_rate); |
160 | |
161 | return 0; |
162 | } |
163 | |
164 | static void at91sam9x5_usb_restore_context(struct clk_hw *hw) |
165 | { |
166 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
167 | int ret; |
168 | |
169 | ret = at91sam9x5_clk_usb_set_parent(hw, index: usb->pms.parent); |
170 | if (ret) |
171 | return; |
172 | |
173 | at91sam9x5_clk_usb_set_rate(hw, rate: usb->pms.rate, parent_rate: usb->pms.parent_rate); |
174 | } |
175 | |
176 | static const struct clk_ops at91sam9x5_usb_ops = { |
177 | .recalc_rate = at91sam9x5_clk_usb_recalc_rate, |
178 | .determine_rate = at91sam9x5_clk_usb_determine_rate, |
179 | .get_parent = at91sam9x5_clk_usb_get_parent, |
180 | .set_parent = at91sam9x5_clk_usb_set_parent, |
181 | .set_rate = at91sam9x5_clk_usb_set_rate, |
182 | .save_context = at91sam9x5_usb_save_context, |
183 | .restore_context = at91sam9x5_usb_restore_context, |
184 | }; |
185 | |
186 | static int at91sam9n12_clk_usb_enable(struct clk_hw *hw) |
187 | { |
188 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
189 | |
190 | regmap_update_bits(map: usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, |
191 | AT91_PMC_USBS); |
192 | |
193 | return 0; |
194 | } |
195 | |
196 | static void at91sam9n12_clk_usb_disable(struct clk_hw *hw) |
197 | { |
198 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
199 | |
200 | regmap_update_bits(map: usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, val: 0); |
201 | } |
202 | |
203 | static int at91sam9n12_clk_usb_is_enabled(struct clk_hw *hw) |
204 | { |
205 | struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw); |
206 | unsigned int usbr; |
207 | |
208 | regmap_read(map: usb->regmap, AT91_PMC_USB, val: &usbr); |
209 | |
210 | return usbr & AT91_PMC_USBS; |
211 | } |
212 | |
213 | static const struct clk_ops at91sam9n12_usb_ops = { |
214 | .enable = at91sam9n12_clk_usb_enable, |
215 | .disable = at91sam9n12_clk_usb_disable, |
216 | .is_enabled = at91sam9n12_clk_usb_is_enabled, |
217 | .recalc_rate = at91sam9x5_clk_usb_recalc_rate, |
218 | .determine_rate = at91sam9x5_clk_usb_determine_rate, |
219 | .set_rate = at91sam9x5_clk_usb_set_rate, |
220 | }; |
221 | |
222 | static struct clk_hw * __init |
223 | _at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name, |
224 | const char **parent_names, u8 num_parents, |
225 | u32 usbs_mask) |
226 | { |
227 | struct at91sam9x5_clk_usb *usb; |
228 | struct clk_hw *hw; |
229 | struct clk_init_data init; |
230 | int ret; |
231 | |
232 | usb = kzalloc(size: sizeof(*usb), GFP_KERNEL); |
233 | if (!usb) |
234 | return ERR_PTR(error: -ENOMEM); |
235 | |
236 | init.name = name; |
237 | init.ops = &at91sam9x5_usb_ops; |
238 | init.parent_names = parent_names; |
239 | init.num_parents = num_parents; |
240 | init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE | |
241 | CLK_SET_RATE_PARENT; |
242 | |
243 | usb->hw.init = &init; |
244 | usb->regmap = regmap; |
245 | usb->usbs_mask = usbs_mask; |
246 | usb->num_parents = num_parents; |
247 | |
248 | hw = &usb->hw; |
249 | ret = clk_hw_register(NULL, hw: &usb->hw); |
250 | if (ret) { |
251 | kfree(objp: usb); |
252 | hw = ERR_PTR(error: ret); |
253 | } |
254 | |
255 | return hw; |
256 | } |
257 | |
258 | struct clk_hw * __init |
259 | at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name, |
260 | const char **parent_names, u8 num_parents) |
261 | { |
262 | return _at91sam9x5_clk_register_usb(regmap, name, parent_names, |
263 | num_parents, SAM9X5_USBS_MASK); |
264 | } |
265 | |
266 | struct clk_hw * __init |
267 | sam9x60_clk_register_usb(struct regmap *regmap, const char *name, |
268 | const char **parent_names, u8 num_parents) |
269 | { |
270 | return _at91sam9x5_clk_register_usb(regmap, name, parent_names, |
271 | num_parents, SAM9X60_USBS_MASK); |
272 | } |
273 | |
274 | struct clk_hw * __init |
275 | at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name, |
276 | const char *parent_name) |
277 | { |
278 | struct at91sam9x5_clk_usb *usb; |
279 | struct clk_hw *hw; |
280 | struct clk_init_data init; |
281 | int ret; |
282 | |
283 | usb = kzalloc(size: sizeof(*usb), GFP_KERNEL); |
284 | if (!usb) |
285 | return ERR_PTR(error: -ENOMEM); |
286 | |
287 | init.name = name; |
288 | init.ops = &at91sam9n12_usb_ops; |
289 | init.parent_names = &parent_name; |
290 | init.num_parents = 1; |
291 | init.flags = CLK_SET_RATE_GATE | CLK_SET_RATE_PARENT; |
292 | |
293 | usb->hw.init = &init; |
294 | usb->regmap = regmap; |
295 | |
296 | hw = &usb->hw; |
297 | ret = clk_hw_register(NULL, hw: &usb->hw); |
298 | if (ret) { |
299 | kfree(objp: usb); |
300 | hw = ERR_PTR(error: ret); |
301 | } |
302 | |
303 | return hw; |
304 | } |
305 | |
306 | static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw, |
307 | unsigned long parent_rate) |
308 | { |
309 | struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); |
310 | unsigned int pllbr; |
311 | u8 usbdiv; |
312 | |
313 | regmap_read(map: usb->regmap, AT91_CKGR_PLLBR, val: &pllbr); |
314 | |
315 | usbdiv = (pllbr & AT91_PMC_USBDIV) >> RM9200_USB_DIV_SHIFT; |
316 | if (usb->divisors[usbdiv]) |
317 | return parent_rate / usb->divisors[usbdiv]; |
318 | |
319 | return 0; |
320 | } |
321 | |
322 | static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate, |
323 | unsigned long *parent_rate) |
324 | { |
325 | struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); |
326 | struct clk_hw *parent = clk_hw_get_parent(hw); |
327 | unsigned long bestrate = 0; |
328 | int bestdiff = -1; |
329 | unsigned long tmprate; |
330 | int tmpdiff; |
331 | int i = 0; |
332 | |
333 | for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) { |
334 | unsigned long tmp_parent_rate; |
335 | |
336 | if (!usb->divisors[i]) |
337 | continue; |
338 | |
339 | tmp_parent_rate = rate * usb->divisors[i]; |
340 | tmp_parent_rate = clk_hw_round_rate(hw: parent, rate: tmp_parent_rate); |
341 | tmprate = DIV_ROUND_CLOSEST(tmp_parent_rate, usb->divisors[i]); |
342 | if (tmprate < rate) |
343 | tmpdiff = rate - tmprate; |
344 | else |
345 | tmpdiff = tmprate - rate; |
346 | |
347 | if (bestdiff < 0 || bestdiff > tmpdiff) { |
348 | bestrate = tmprate; |
349 | bestdiff = tmpdiff; |
350 | *parent_rate = tmp_parent_rate; |
351 | } |
352 | |
353 | if (!bestdiff) |
354 | break; |
355 | } |
356 | |
357 | return bestrate; |
358 | } |
359 | |
360 | static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate, |
361 | unsigned long parent_rate) |
362 | { |
363 | int i; |
364 | struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw); |
365 | unsigned long div; |
366 | |
367 | if (!rate) |
368 | return -EINVAL; |
369 | |
370 | div = DIV_ROUND_CLOSEST(parent_rate, rate); |
371 | |
372 | for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) { |
373 | if (usb->divisors[i] == div) { |
374 | regmap_update_bits(map: usb->regmap, AT91_CKGR_PLLBR, |
375 | AT91_PMC_USBDIV, |
376 | val: i << RM9200_USB_DIV_SHIFT); |
377 | |
378 | return 0; |
379 | } |
380 | } |
381 | |
382 | return -EINVAL; |
383 | } |
384 | |
385 | static const struct clk_ops at91rm9200_usb_ops = { |
386 | .recalc_rate = at91rm9200_clk_usb_recalc_rate, |
387 | .round_rate = at91rm9200_clk_usb_round_rate, |
388 | .set_rate = at91rm9200_clk_usb_set_rate, |
389 | }; |
390 | |
391 | struct clk_hw * __init |
392 | at91rm9200_clk_register_usb(struct regmap *regmap, const char *name, |
393 | const char *parent_name, const u32 *divisors) |
394 | { |
395 | struct at91rm9200_clk_usb *usb; |
396 | struct clk_hw *hw; |
397 | struct clk_init_data init; |
398 | int ret; |
399 | |
400 | usb = kzalloc(size: sizeof(*usb), GFP_KERNEL); |
401 | if (!usb) |
402 | return ERR_PTR(error: -ENOMEM); |
403 | |
404 | init.name = name; |
405 | init.ops = &at91rm9200_usb_ops; |
406 | init.parent_names = &parent_name; |
407 | init.num_parents = 1; |
408 | init.flags = CLK_SET_RATE_PARENT; |
409 | |
410 | usb->hw.init = &init; |
411 | usb->regmap = regmap; |
412 | memcpy(usb->divisors, divisors, sizeof(usb->divisors)); |
413 | |
414 | hw = &usb->hw; |
415 | ret = clk_hw_register(NULL, hw: &usb->hw); |
416 | if (ret) { |
417 | kfree(objp: usb); |
418 | hw = ERR_PTR(error: ret); |
419 | } |
420 | |
421 | return hw; |
422 | } |
423 | |