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.h> |
9 | #include <linux/clk/at91_pmc.h> |
10 | #include <linux/of.h> |
11 | #include <linux/mfd/syscon.h> |
12 | #include <linux/regmap.h> |
13 | |
14 | #include "pmc.h" |
15 | |
16 | #define MASTER_PRES_MASK 0x7 |
17 | #define MASTER_PRES_MAX MASTER_PRES_MASK |
18 | #define MASTER_DIV_SHIFT 8 |
19 | #define MASTER_DIV_MASK 0x7 |
20 | |
21 | #define PMC_MCR_CSS_SHIFT (16) |
22 | |
23 | #define MASTER_MAX_ID 4 |
24 | |
25 | #define to_clk_master(hw) container_of(hw, struct clk_master, hw) |
26 | |
27 | struct clk_master { |
28 | struct clk_hw hw; |
29 | struct regmap *regmap; |
30 | spinlock_t *lock; |
31 | const struct clk_master_layout *layout; |
32 | const struct clk_master_characteristics *characteristics; |
33 | struct at91_clk_pms pms; |
34 | u32 *mux_table; |
35 | u32 mckr; |
36 | int chg_pid; |
37 | u8 id; |
38 | u8 parent; |
39 | u8 div; |
40 | u32 safe_div; |
41 | }; |
42 | |
43 | /* MCK div reference to be used by notifier. */ |
44 | static struct clk_master *master_div; |
45 | |
46 | static inline bool clk_master_ready(struct clk_master *master) |
47 | { |
48 | unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY; |
49 | unsigned int status; |
50 | |
51 | regmap_read(map: master->regmap, AT91_PMC_SR, val: &status); |
52 | |
53 | return !!(status & bit); |
54 | } |
55 | |
56 | static int clk_master_prepare(struct clk_hw *hw) |
57 | { |
58 | struct clk_master *master = to_clk_master(hw); |
59 | unsigned long flags; |
60 | |
61 | spin_lock_irqsave(master->lock, flags); |
62 | |
63 | while (!clk_master_ready(master)) |
64 | cpu_relax(); |
65 | |
66 | spin_unlock_irqrestore(lock: master->lock, flags); |
67 | |
68 | return 0; |
69 | } |
70 | |
71 | static int clk_master_is_prepared(struct clk_hw *hw) |
72 | { |
73 | struct clk_master *master = to_clk_master(hw); |
74 | unsigned long flags; |
75 | bool status; |
76 | |
77 | spin_lock_irqsave(master->lock, flags); |
78 | status = clk_master_ready(master); |
79 | spin_unlock_irqrestore(lock: master->lock, flags); |
80 | |
81 | return status; |
82 | } |
83 | |
84 | static unsigned long clk_master_div_recalc_rate(struct clk_hw *hw, |
85 | unsigned long parent_rate) |
86 | { |
87 | u8 div; |
88 | unsigned long flags, rate = parent_rate; |
89 | struct clk_master *master = to_clk_master(hw); |
90 | const struct clk_master_layout *layout = master->layout; |
91 | const struct clk_master_characteristics *characteristics = |
92 | master->characteristics; |
93 | unsigned int mckr; |
94 | |
95 | spin_lock_irqsave(master->lock, flags); |
96 | regmap_read(map: master->regmap, reg: master->layout->offset, val: &mckr); |
97 | spin_unlock_irqrestore(lock: master->lock, flags); |
98 | |
99 | mckr &= layout->mask; |
100 | |
101 | div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; |
102 | |
103 | rate /= characteristics->divisors[div]; |
104 | |
105 | if (rate < characteristics->output.min) |
106 | pr_warn("master clk div is underclocked" ); |
107 | else if (rate > characteristics->output.max) |
108 | pr_warn("master clk div is overclocked" ); |
109 | |
110 | return rate; |
111 | } |
112 | |
113 | static int clk_master_div_save_context(struct clk_hw *hw) |
114 | { |
115 | struct clk_master *master = to_clk_master(hw); |
116 | struct clk_hw *parent_hw = clk_hw_get_parent(hw); |
117 | unsigned long flags; |
118 | unsigned int mckr, div; |
119 | |
120 | spin_lock_irqsave(master->lock, flags); |
121 | regmap_read(map: master->regmap, reg: master->layout->offset, val: &mckr); |
122 | spin_unlock_irqrestore(lock: master->lock, flags); |
123 | |
124 | mckr &= master->layout->mask; |
125 | div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; |
126 | div = master->characteristics->divisors[div]; |
127 | |
128 | master->pms.parent_rate = clk_hw_get_rate(hw: parent_hw); |
129 | master->pms.rate = DIV_ROUND_CLOSEST(master->pms.parent_rate, div); |
130 | |
131 | return 0; |
132 | } |
133 | |
134 | static void clk_master_div_restore_context(struct clk_hw *hw) |
135 | { |
136 | struct clk_master *master = to_clk_master(hw); |
137 | unsigned long flags; |
138 | unsigned int mckr; |
139 | u8 div; |
140 | |
141 | spin_lock_irqsave(master->lock, flags); |
142 | regmap_read(map: master->regmap, reg: master->layout->offset, val: &mckr); |
143 | spin_unlock_irqrestore(lock: master->lock, flags); |
144 | |
145 | mckr &= master->layout->mask; |
146 | div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; |
147 | div = master->characteristics->divisors[div]; |
148 | |
149 | if (div != DIV_ROUND_CLOSEST(master->pms.parent_rate, master->pms.rate)) |
150 | pr_warn("MCKR DIV not configured properly by firmware!\n" ); |
151 | } |
152 | |
153 | static const struct clk_ops master_div_ops = { |
154 | .prepare = clk_master_prepare, |
155 | .is_prepared = clk_master_is_prepared, |
156 | .recalc_rate = clk_master_div_recalc_rate, |
157 | .save_context = clk_master_div_save_context, |
158 | .restore_context = clk_master_div_restore_context, |
159 | }; |
160 | |
161 | /* This function must be called with lock acquired. */ |
162 | static int clk_master_div_set(struct clk_master *master, |
163 | unsigned long parent_rate, int div) |
164 | { |
165 | const struct clk_master_characteristics *characteristics = |
166 | master->characteristics; |
167 | unsigned long rate = parent_rate; |
168 | unsigned int max_div = 0, div_index = 0, max_div_index = 0; |
169 | unsigned int i, mckr, tmp; |
170 | int ret; |
171 | |
172 | for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) { |
173 | if (!characteristics->divisors[i]) |
174 | break; |
175 | |
176 | if (div == characteristics->divisors[i]) |
177 | div_index = i; |
178 | |
179 | if (max_div < characteristics->divisors[i]) { |
180 | max_div = characteristics->divisors[i]; |
181 | max_div_index = i; |
182 | } |
183 | } |
184 | |
185 | if (div > max_div) |
186 | div_index = max_div_index; |
187 | |
188 | ret = regmap_read(map: master->regmap, reg: master->layout->offset, val: &mckr); |
189 | if (ret) |
190 | return ret; |
191 | |
192 | mckr &= master->layout->mask; |
193 | tmp = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; |
194 | if (tmp == div_index) |
195 | return 0; |
196 | |
197 | rate /= characteristics->divisors[div_index]; |
198 | if (rate < characteristics->output.min) |
199 | pr_warn("master clk div is underclocked" ); |
200 | else if (rate > characteristics->output.max) |
201 | pr_warn("master clk div is overclocked" ); |
202 | |
203 | mckr &= ~(MASTER_DIV_MASK << MASTER_DIV_SHIFT); |
204 | mckr |= (div_index << MASTER_DIV_SHIFT); |
205 | ret = regmap_write(map: master->regmap, reg: master->layout->offset, val: mckr); |
206 | if (ret) |
207 | return ret; |
208 | |
209 | while (!clk_master_ready(master)) |
210 | cpu_relax(); |
211 | |
212 | master->div = characteristics->divisors[div_index]; |
213 | |
214 | return 0; |
215 | } |
216 | |
217 | static unsigned long clk_master_div_recalc_rate_chg(struct clk_hw *hw, |
218 | unsigned long parent_rate) |
219 | { |
220 | struct clk_master *master = to_clk_master(hw); |
221 | |
222 | return DIV_ROUND_CLOSEST_ULL(parent_rate, master->div); |
223 | } |
224 | |
225 | static void clk_master_div_restore_context_chg(struct clk_hw *hw) |
226 | { |
227 | struct clk_master *master = to_clk_master(hw); |
228 | unsigned long flags; |
229 | int ret; |
230 | |
231 | spin_lock_irqsave(master->lock, flags); |
232 | ret = clk_master_div_set(master, parent_rate: master->pms.parent_rate, |
233 | DIV_ROUND_CLOSEST(master->pms.parent_rate, |
234 | master->pms.rate)); |
235 | spin_unlock_irqrestore(lock: master->lock, flags); |
236 | if (ret) |
237 | pr_warn("Failed to restore MCK DIV clock\n" ); |
238 | } |
239 | |
240 | static const struct clk_ops master_div_ops_chg = { |
241 | .prepare = clk_master_prepare, |
242 | .is_prepared = clk_master_is_prepared, |
243 | .recalc_rate = clk_master_div_recalc_rate_chg, |
244 | .save_context = clk_master_div_save_context, |
245 | .restore_context = clk_master_div_restore_context_chg, |
246 | }; |
247 | |
248 | static int clk_master_div_notifier_fn(struct notifier_block *notifier, |
249 | unsigned long code, void *data) |
250 | { |
251 | const struct clk_master_characteristics *characteristics = |
252 | master_div->characteristics; |
253 | struct clk_notifier_data *cnd = data; |
254 | unsigned long flags, new_parent_rate, new_rate; |
255 | unsigned int mckr, div, new_div = 0; |
256 | int ret, i; |
257 | long tmp_diff; |
258 | long best_diff = -1; |
259 | |
260 | spin_lock_irqsave(master_div->lock, flags); |
261 | switch (code) { |
262 | case PRE_RATE_CHANGE: |
263 | /* |
264 | * We want to avoid any overclocking of MCK DIV domain. To do |
265 | * this we set a safe divider (the underclocking is not of |
266 | * interest as we can go as low as 32KHz). The relation |
267 | * b/w this clock and its parents are as follows: |
268 | * |
269 | * FRAC PLL -> DIV PLL -> MCK DIV |
270 | * |
271 | * With the proper safe divider we should be good even with FRAC |
272 | * PLL at its maximum value. |
273 | */ |
274 | ret = regmap_read(map: master_div->regmap, reg: master_div->layout->offset, |
275 | val: &mckr); |
276 | if (ret) { |
277 | ret = NOTIFY_STOP_MASK; |
278 | goto unlock; |
279 | } |
280 | |
281 | mckr &= master_div->layout->mask; |
282 | div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; |
283 | |
284 | /* Switch to safe divider. */ |
285 | clk_master_div_set(master: master_div, |
286 | parent_rate: cnd->old_rate * characteristics->divisors[div], |
287 | div: master_div->safe_div); |
288 | break; |
289 | |
290 | case POST_RATE_CHANGE: |
291 | /* |
292 | * At this point we want to restore MCK DIV domain to its maximum |
293 | * allowed rate. |
294 | */ |
295 | ret = regmap_read(map: master_div->regmap, reg: master_div->layout->offset, |
296 | val: &mckr); |
297 | if (ret) { |
298 | ret = NOTIFY_STOP_MASK; |
299 | goto unlock; |
300 | } |
301 | |
302 | mckr &= master_div->layout->mask; |
303 | div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; |
304 | new_parent_rate = cnd->new_rate * characteristics->divisors[div]; |
305 | |
306 | for (i = 0; i < ARRAY_SIZE(characteristics->divisors); i++) { |
307 | if (!characteristics->divisors[i]) |
308 | break; |
309 | |
310 | new_rate = DIV_ROUND_CLOSEST_ULL(new_parent_rate, |
311 | characteristics->divisors[i]); |
312 | |
313 | tmp_diff = characteristics->output.max - new_rate; |
314 | if (tmp_diff < 0) |
315 | continue; |
316 | |
317 | if (best_diff < 0 || best_diff > tmp_diff) { |
318 | new_div = characteristics->divisors[i]; |
319 | best_diff = tmp_diff; |
320 | } |
321 | |
322 | if (!tmp_diff) |
323 | break; |
324 | } |
325 | |
326 | if (!new_div) { |
327 | ret = NOTIFY_STOP_MASK; |
328 | goto unlock; |
329 | } |
330 | |
331 | /* Update the div to preserve MCK DIV clock rate. */ |
332 | clk_master_div_set(master: master_div, parent_rate: new_parent_rate, |
333 | div: new_div); |
334 | |
335 | ret = NOTIFY_OK; |
336 | break; |
337 | |
338 | default: |
339 | ret = NOTIFY_DONE; |
340 | break; |
341 | } |
342 | |
343 | unlock: |
344 | spin_unlock_irqrestore(lock: master_div->lock, flags); |
345 | |
346 | return ret; |
347 | } |
348 | |
349 | static struct notifier_block clk_master_div_notifier = { |
350 | .notifier_call = clk_master_div_notifier_fn, |
351 | }; |
352 | |
353 | static void clk_sama7g5_master_best_diff(struct clk_rate_request *req, |
354 | struct clk_hw *parent, |
355 | unsigned long parent_rate, |
356 | long *best_rate, |
357 | long *best_diff, |
358 | u32 div) |
359 | { |
360 | unsigned long tmp_rate, tmp_diff; |
361 | |
362 | if (div == MASTER_PRES_MAX) |
363 | tmp_rate = parent_rate / 3; |
364 | else |
365 | tmp_rate = parent_rate >> div; |
366 | |
367 | tmp_diff = abs(req->rate - tmp_rate); |
368 | |
369 | if (*best_diff < 0 || *best_diff >= tmp_diff) { |
370 | *best_rate = tmp_rate; |
371 | *best_diff = tmp_diff; |
372 | req->best_parent_rate = parent_rate; |
373 | req->best_parent_hw = parent; |
374 | } |
375 | } |
376 | |
377 | static unsigned long clk_master_pres_recalc_rate(struct clk_hw *hw, |
378 | unsigned long parent_rate) |
379 | { |
380 | struct clk_master *master = to_clk_master(hw); |
381 | const struct clk_master_characteristics *characteristics = |
382 | master->characteristics; |
383 | unsigned long flags; |
384 | unsigned int val, pres; |
385 | |
386 | spin_lock_irqsave(master->lock, flags); |
387 | regmap_read(map: master->regmap, reg: master->layout->offset, val: &val); |
388 | spin_unlock_irqrestore(lock: master->lock, flags); |
389 | |
390 | val &= master->layout->mask; |
391 | pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; |
392 | if (pres == MASTER_PRES_MAX && characteristics->have_div3_pres) |
393 | pres = 3; |
394 | else |
395 | pres = (1 << pres); |
396 | |
397 | return DIV_ROUND_CLOSEST_ULL(parent_rate, pres); |
398 | } |
399 | |
400 | static u8 clk_master_pres_get_parent(struct clk_hw *hw) |
401 | { |
402 | struct clk_master *master = to_clk_master(hw); |
403 | unsigned long flags; |
404 | unsigned int mckr; |
405 | |
406 | spin_lock_irqsave(master->lock, flags); |
407 | regmap_read(map: master->regmap, reg: master->layout->offset, val: &mckr); |
408 | spin_unlock_irqrestore(lock: master->lock, flags); |
409 | |
410 | mckr &= master->layout->mask; |
411 | |
412 | return mckr & AT91_PMC_CSS; |
413 | } |
414 | |
415 | static int clk_master_pres_save_context(struct clk_hw *hw) |
416 | { |
417 | struct clk_master *master = to_clk_master(hw); |
418 | struct clk_hw *parent_hw = clk_hw_get_parent(hw); |
419 | unsigned long flags; |
420 | unsigned int val, pres; |
421 | |
422 | spin_lock_irqsave(master->lock, flags); |
423 | regmap_read(map: master->regmap, reg: master->layout->offset, val: &val); |
424 | spin_unlock_irqrestore(lock: master->lock, flags); |
425 | |
426 | val &= master->layout->mask; |
427 | pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; |
428 | if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres) |
429 | pres = 3; |
430 | else |
431 | pres = (1 << pres); |
432 | |
433 | master->pms.parent = val & AT91_PMC_CSS; |
434 | master->pms.parent_rate = clk_hw_get_rate(hw: parent_hw); |
435 | master->pms.rate = DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres); |
436 | |
437 | return 0; |
438 | } |
439 | |
440 | static void clk_master_pres_restore_context(struct clk_hw *hw) |
441 | { |
442 | struct clk_master *master = to_clk_master(hw); |
443 | unsigned long flags; |
444 | unsigned int val, pres; |
445 | |
446 | spin_lock_irqsave(master->lock, flags); |
447 | regmap_read(map: master->regmap, reg: master->layout->offset, val: &val); |
448 | spin_unlock_irqrestore(lock: master->lock, flags); |
449 | |
450 | val &= master->layout->mask; |
451 | pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK; |
452 | if (pres == MASTER_PRES_MAX && master->characteristics->have_div3_pres) |
453 | pres = 3; |
454 | else |
455 | pres = (1 << pres); |
456 | |
457 | if (master->pms.rate != |
458 | DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres) || |
459 | (master->pms.parent != (val & AT91_PMC_CSS))) |
460 | pr_warn("MCKR PRES was not configured properly by firmware!\n" ); |
461 | } |
462 | |
463 | static const struct clk_ops master_pres_ops = { |
464 | .prepare = clk_master_prepare, |
465 | .is_prepared = clk_master_is_prepared, |
466 | .recalc_rate = clk_master_pres_recalc_rate, |
467 | .get_parent = clk_master_pres_get_parent, |
468 | .save_context = clk_master_pres_save_context, |
469 | .restore_context = clk_master_pres_restore_context, |
470 | }; |
471 | |
472 | static struct clk_hw * __init |
473 | at91_clk_register_master_internal(struct regmap *regmap, |
474 | const char *name, int num_parents, |
475 | const char **parent_names, |
476 | struct clk_hw **parent_hws, |
477 | const struct clk_master_layout *layout, |
478 | const struct clk_master_characteristics *characteristics, |
479 | const struct clk_ops *ops, spinlock_t *lock, u32 flags) |
480 | { |
481 | struct clk_master *master; |
482 | struct clk_init_data init = {}; |
483 | struct clk_hw *hw; |
484 | unsigned int mckr; |
485 | unsigned long irqflags; |
486 | int ret; |
487 | |
488 | if (!name || !num_parents || !(parent_names || parent_hws) || !lock) |
489 | return ERR_PTR(error: -EINVAL); |
490 | |
491 | master = kzalloc(size: sizeof(*master), GFP_KERNEL); |
492 | if (!master) |
493 | return ERR_PTR(error: -ENOMEM); |
494 | |
495 | init.name = name; |
496 | init.ops = ops; |
497 | if (parent_hws) |
498 | init.parent_hws = (const struct clk_hw **)parent_hws; |
499 | else |
500 | init.parent_names = parent_names; |
501 | init.num_parents = num_parents; |
502 | init.flags = flags; |
503 | |
504 | master->hw.init = &init; |
505 | master->layout = layout; |
506 | master->characteristics = characteristics; |
507 | master->regmap = regmap; |
508 | master->lock = lock; |
509 | |
510 | if (ops == &master_div_ops_chg) { |
511 | spin_lock_irqsave(master->lock, irqflags); |
512 | regmap_read(map: master->regmap, reg: master->layout->offset, val: &mckr); |
513 | spin_unlock_irqrestore(lock: master->lock, flags: irqflags); |
514 | |
515 | mckr &= layout->mask; |
516 | mckr = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK; |
517 | master->div = characteristics->divisors[mckr]; |
518 | } |
519 | |
520 | hw = &master->hw; |
521 | ret = clk_hw_register(NULL, hw: &master->hw); |
522 | if (ret) { |
523 | kfree(objp: master); |
524 | hw = ERR_PTR(error: ret); |
525 | } |
526 | |
527 | return hw; |
528 | } |
529 | |
530 | struct clk_hw * __init |
531 | at91_clk_register_master_pres(struct regmap *regmap, |
532 | const char *name, int num_parents, |
533 | const char **parent_names, |
534 | struct clk_hw **parent_hws, |
535 | const struct clk_master_layout *layout, |
536 | const struct clk_master_characteristics *characteristics, |
537 | spinlock_t *lock) |
538 | { |
539 | return at91_clk_register_master_internal(regmap, name, num_parents, |
540 | parent_names, parent_hws, layout, |
541 | characteristics, |
542 | ops: &master_pres_ops, |
543 | lock, CLK_SET_RATE_GATE); |
544 | } |
545 | |
546 | struct clk_hw * __init |
547 | at91_clk_register_master_div(struct regmap *regmap, |
548 | const char *name, const char *parent_name, |
549 | struct clk_hw *parent_hw, const struct clk_master_layout *layout, |
550 | const struct clk_master_characteristics *characteristics, |
551 | spinlock_t *lock, u32 flags, u32 safe_div) |
552 | { |
553 | const struct clk_ops *ops; |
554 | struct clk_hw *hw; |
555 | |
556 | if (flags & CLK_SET_RATE_GATE) |
557 | ops = &master_div_ops; |
558 | else |
559 | ops = &master_div_ops_chg; |
560 | |
561 | hw = at91_clk_register_master_internal(regmap, name, num_parents: 1, |
562 | parent_names: parent_name ? &parent_name : NULL, |
563 | parent_hws: parent_hw ? &parent_hw : NULL, layout, |
564 | characteristics, ops, |
565 | lock, flags); |
566 | |
567 | if (!IS_ERR(ptr: hw) && safe_div) { |
568 | master_div = to_clk_master(hw); |
569 | master_div->safe_div = safe_div; |
570 | clk_notifier_register(clk: hw->clk, |
571 | nb: &clk_master_div_notifier); |
572 | } |
573 | |
574 | return hw; |
575 | } |
576 | |
577 | static unsigned long |
578 | clk_sama7g5_master_recalc_rate(struct clk_hw *hw, |
579 | unsigned long parent_rate) |
580 | { |
581 | struct clk_master *master = to_clk_master(hw); |
582 | |
583 | return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div)); |
584 | } |
585 | |
586 | static int clk_sama7g5_master_determine_rate(struct clk_hw *hw, |
587 | struct clk_rate_request *req) |
588 | { |
589 | struct clk_master *master = to_clk_master(hw); |
590 | struct clk_hw *parent; |
591 | long best_rate = LONG_MIN, best_diff = LONG_MIN; |
592 | unsigned long parent_rate; |
593 | unsigned int div, i; |
594 | |
595 | /* First: check the dividers of MCR. */ |
596 | for (i = 0; i < clk_hw_get_num_parents(hw); i++) { |
597 | parent = clk_hw_get_parent_by_index(hw, index: i); |
598 | if (!parent) |
599 | continue; |
600 | |
601 | parent_rate = clk_hw_get_rate(hw: parent); |
602 | if (!parent_rate) |
603 | continue; |
604 | |
605 | for (div = 0; div < MASTER_PRES_MAX + 1; div++) { |
606 | clk_sama7g5_master_best_diff(req, parent, parent_rate, |
607 | best_rate: &best_rate, best_diff: &best_diff, |
608 | div); |
609 | if (!best_diff) |
610 | break; |
611 | } |
612 | |
613 | if (!best_diff) |
614 | break; |
615 | } |
616 | |
617 | /* Second: try to request rate form changeable parent. */ |
618 | if (master->chg_pid < 0) |
619 | goto end; |
620 | |
621 | parent = clk_hw_get_parent_by_index(hw, index: master->chg_pid); |
622 | if (!parent) |
623 | goto end; |
624 | |
625 | for (div = 0; div < MASTER_PRES_MAX + 1; div++) { |
626 | struct clk_rate_request req_parent; |
627 | unsigned long req_rate; |
628 | |
629 | if (div == MASTER_PRES_MAX) |
630 | req_rate = req->rate * 3; |
631 | else |
632 | req_rate = req->rate << div; |
633 | |
634 | clk_hw_forward_rate_request(core: hw, old_req: req, parent, req: &req_parent, parent_rate: req_rate); |
635 | if (__clk_determine_rate(core: parent, req: &req_parent)) |
636 | continue; |
637 | |
638 | clk_sama7g5_master_best_diff(req, parent, parent_rate: req_parent.rate, |
639 | best_rate: &best_rate, best_diff: &best_diff, div); |
640 | |
641 | if (!best_diff) |
642 | break; |
643 | } |
644 | |
645 | end: |
646 | pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n" , |
647 | __func__, best_rate, |
648 | __clk_get_name((req->best_parent_hw)->clk), |
649 | req->best_parent_rate); |
650 | |
651 | if (best_rate < 0) |
652 | return -EINVAL; |
653 | |
654 | req->rate = best_rate; |
655 | |
656 | return 0; |
657 | } |
658 | |
659 | static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw) |
660 | { |
661 | struct clk_master *master = to_clk_master(hw); |
662 | unsigned long flags; |
663 | u8 index; |
664 | |
665 | spin_lock_irqsave(master->lock, flags); |
666 | index = clk_mux_val_to_index(hw: &master->hw, table: master->mux_table, flags: 0, |
667 | val: master->parent); |
668 | spin_unlock_irqrestore(lock: master->lock, flags); |
669 | |
670 | return index; |
671 | } |
672 | |
673 | static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index) |
674 | { |
675 | struct clk_master *master = to_clk_master(hw); |
676 | unsigned long flags; |
677 | |
678 | if (index >= clk_hw_get_num_parents(hw)) |
679 | return -EINVAL; |
680 | |
681 | spin_lock_irqsave(master->lock, flags); |
682 | master->parent = clk_mux_index_to_val(table: master->mux_table, flags: 0, index); |
683 | spin_unlock_irqrestore(lock: master->lock, flags); |
684 | |
685 | return 0; |
686 | } |
687 | |
688 | static void clk_sama7g5_master_set(struct clk_master *master, |
689 | unsigned int status) |
690 | { |
691 | unsigned long flags; |
692 | unsigned int val, cparent; |
693 | unsigned int enable = status ? AT91_PMC_MCR_V2_EN : 0; |
694 | unsigned int parent = master->parent << PMC_MCR_CSS_SHIFT; |
695 | unsigned int div = master->div << MASTER_DIV_SHIFT; |
696 | |
697 | spin_lock_irqsave(master->lock, flags); |
698 | |
699 | regmap_write(map: master->regmap, AT91_PMC_MCR_V2, |
700 | AT91_PMC_MCR_V2_ID(master->id)); |
701 | regmap_read(map: master->regmap, AT91_PMC_MCR_V2, val: &val); |
702 | regmap_update_bits(map: master->regmap, AT91_PMC_MCR_V2, |
703 | mask: enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV | |
704 | AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK, |
705 | val: enable | parent | div | AT91_PMC_MCR_V2_CMD | |
706 | AT91_PMC_MCR_V2_ID(master->id)); |
707 | |
708 | cparent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT; |
709 | |
710 | /* Wait here only if parent is being changed. */ |
711 | while ((cparent != master->parent) && !clk_master_ready(master)) |
712 | cpu_relax(); |
713 | |
714 | spin_unlock_irqrestore(lock: master->lock, flags); |
715 | } |
716 | |
717 | static int clk_sama7g5_master_enable(struct clk_hw *hw) |
718 | { |
719 | struct clk_master *master = to_clk_master(hw); |
720 | |
721 | clk_sama7g5_master_set(master, status: 1); |
722 | |
723 | return 0; |
724 | } |
725 | |
726 | static void clk_sama7g5_master_disable(struct clk_hw *hw) |
727 | { |
728 | struct clk_master *master = to_clk_master(hw); |
729 | unsigned long flags; |
730 | |
731 | spin_lock_irqsave(master->lock, flags); |
732 | |
733 | regmap_write(map: master->regmap, AT91_PMC_MCR_V2, val: master->id); |
734 | regmap_update_bits(map: master->regmap, AT91_PMC_MCR_V2, |
735 | AT91_PMC_MCR_V2_EN | AT91_PMC_MCR_V2_CMD | |
736 | AT91_PMC_MCR_V2_ID_MSK, |
737 | AT91_PMC_MCR_V2_CMD | |
738 | AT91_PMC_MCR_V2_ID(master->id)); |
739 | |
740 | spin_unlock_irqrestore(lock: master->lock, flags); |
741 | } |
742 | |
743 | static int clk_sama7g5_master_is_enabled(struct clk_hw *hw) |
744 | { |
745 | struct clk_master *master = to_clk_master(hw); |
746 | unsigned long flags; |
747 | unsigned int val; |
748 | |
749 | spin_lock_irqsave(master->lock, flags); |
750 | |
751 | regmap_write(map: master->regmap, AT91_PMC_MCR_V2, val: master->id); |
752 | regmap_read(map: master->regmap, AT91_PMC_MCR_V2, val: &val); |
753 | |
754 | spin_unlock_irqrestore(lock: master->lock, flags); |
755 | |
756 | return !!(val & AT91_PMC_MCR_V2_EN); |
757 | } |
758 | |
759 | static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate, |
760 | unsigned long parent_rate) |
761 | { |
762 | struct clk_master *master = to_clk_master(hw); |
763 | unsigned long div, flags; |
764 | |
765 | div = DIV_ROUND_CLOSEST(parent_rate, rate); |
766 | if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1))) |
767 | return -EINVAL; |
768 | |
769 | if (div == 3) |
770 | div = MASTER_PRES_MAX; |
771 | else if (div) |
772 | div = ffs(div) - 1; |
773 | |
774 | spin_lock_irqsave(master->lock, flags); |
775 | master->div = div; |
776 | spin_unlock_irqrestore(lock: master->lock, flags); |
777 | |
778 | return 0; |
779 | } |
780 | |
781 | static int clk_sama7g5_master_save_context(struct clk_hw *hw) |
782 | { |
783 | struct clk_master *master = to_clk_master(hw); |
784 | |
785 | master->pms.status = clk_sama7g5_master_is_enabled(hw); |
786 | |
787 | return 0; |
788 | } |
789 | |
790 | static void clk_sama7g5_master_restore_context(struct clk_hw *hw) |
791 | { |
792 | struct clk_master *master = to_clk_master(hw); |
793 | |
794 | if (master->pms.status) |
795 | clk_sama7g5_master_set(master, status: master->pms.status); |
796 | } |
797 | |
798 | static const struct clk_ops sama7g5_master_ops = { |
799 | .enable = clk_sama7g5_master_enable, |
800 | .disable = clk_sama7g5_master_disable, |
801 | .is_enabled = clk_sama7g5_master_is_enabled, |
802 | .recalc_rate = clk_sama7g5_master_recalc_rate, |
803 | .determine_rate = clk_sama7g5_master_determine_rate, |
804 | .set_rate = clk_sama7g5_master_set_rate, |
805 | .get_parent = clk_sama7g5_master_get_parent, |
806 | .set_parent = clk_sama7g5_master_set_parent, |
807 | .save_context = clk_sama7g5_master_save_context, |
808 | .restore_context = clk_sama7g5_master_restore_context, |
809 | }; |
810 | |
811 | struct clk_hw * __init |
812 | at91_clk_sama7g5_register_master(struct regmap *regmap, |
813 | const char *name, int num_parents, |
814 | const char **parent_names, |
815 | struct clk_hw **parent_hws, |
816 | u32 *mux_table, |
817 | spinlock_t *lock, u8 id, |
818 | bool critical, int chg_pid) |
819 | { |
820 | struct clk_master *master; |
821 | struct clk_hw *hw; |
822 | struct clk_init_data init = {}; |
823 | unsigned long flags; |
824 | unsigned int val; |
825 | int ret; |
826 | |
827 | if (!name || !num_parents || !(parent_names || parent_hws) || !mux_table || |
828 | !lock || id > MASTER_MAX_ID) |
829 | return ERR_PTR(error: -EINVAL); |
830 | |
831 | master = kzalloc(size: sizeof(*master), GFP_KERNEL); |
832 | if (!master) |
833 | return ERR_PTR(error: -ENOMEM); |
834 | |
835 | init.name = name; |
836 | init.ops = &sama7g5_master_ops; |
837 | if (parent_hws) |
838 | init.parent_hws = (const struct clk_hw **)parent_hws; |
839 | else |
840 | init.parent_names = parent_names; |
841 | init.num_parents = num_parents; |
842 | init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; |
843 | if (chg_pid >= 0) |
844 | init.flags |= CLK_SET_RATE_PARENT; |
845 | if (critical) |
846 | init.flags |= CLK_IS_CRITICAL; |
847 | |
848 | master->hw.init = &init; |
849 | master->regmap = regmap; |
850 | master->id = id; |
851 | master->chg_pid = chg_pid; |
852 | master->lock = lock; |
853 | master->mux_table = mux_table; |
854 | |
855 | spin_lock_irqsave(master->lock, flags); |
856 | regmap_write(map: master->regmap, AT91_PMC_MCR_V2, val: master->id); |
857 | regmap_read(map: master->regmap, AT91_PMC_MCR_V2, val: &val); |
858 | master->parent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT; |
859 | master->div = (val & AT91_PMC_MCR_V2_DIV) >> MASTER_DIV_SHIFT; |
860 | spin_unlock_irqrestore(lock: master->lock, flags); |
861 | |
862 | hw = &master->hw; |
863 | ret = clk_hw_register(NULL, hw: &master->hw); |
864 | if (ret) { |
865 | kfree(objp: master); |
866 | hw = ERR_PTR(error: ret); |
867 | } |
868 | |
869 | return hw; |
870 | } |
871 | |
872 | const struct clk_master_layout at91rm9200_master_layout = { |
873 | .mask = 0x31F, |
874 | .pres_shift = 2, |
875 | .offset = AT91_PMC_MCKR, |
876 | }; |
877 | |
878 | const struct clk_master_layout at91sam9x5_master_layout = { |
879 | .mask = 0x373, |
880 | .pres_shift = 4, |
881 | .offset = AT91_PMC_MCKR, |
882 | }; |
883 | |