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 9
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
clk_master_ready(struct clk_master * master)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(master->regmap, AT91_PMC_SR, &status);
52
53 return !!(status & bit);
54 }
55
clk_master_prepare(struct clk_hw * hw)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(master->lock, flags);
67
68 return 0;
69 }
70
clk_master_is_prepared(struct clk_hw * hw)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(master->lock, flags);
80
81 return status;
82 }
83
clk_master_div_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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(master->regmap, master->layout->offset, &mckr);
97 spin_unlock_irqrestore(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
clk_master_div_save_context(struct clk_hw * hw)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(master->regmap, master->layout->offset, &mckr);
122 spin_unlock_irqrestore(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(parent_hw);
129 master->pms.rate = DIV_ROUND_CLOSEST(master->pms.parent_rate, div);
130
131 return 0;
132 }
133
clk_master_div_restore_context(struct clk_hw * hw)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(master->regmap, master->layout->offset, &mckr);
143 spin_unlock_irqrestore(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. */
clk_master_div_set(struct clk_master * master,unsigned long parent_rate,int div)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(master->regmap, master->layout->offset, &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(master->regmap, master->layout->offset, 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
clk_master_div_recalc_rate_chg(struct clk_hw * hw,unsigned long parent_rate)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
clk_master_div_restore_context_chg(struct clk_hw * hw)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, master->pms.parent_rate,
233 DIV_ROUND_CLOSEST(master->pms.parent_rate,
234 master->pms.rate));
235 spin_unlock_irqrestore(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
clk_master_div_notifier_fn(struct notifier_block * notifier,unsigned long code,void * data)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(master_div->regmap, master_div->layout->offset,
275 &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_div,
286 cnd->old_rate * characteristics->divisors[div],
287 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(master_div->regmap, master_div->layout->offset,
296 &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_div, new_parent_rate,
333 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(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
clk_sama7g5_master_best_diff(struct clk_rate_request * req,struct clk_hw * parent,unsigned long parent_rate,long * best_rate,long * best_diff,u32 div)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
clk_master_pres_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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(master->regmap, master->layout->offset, &val);
388 spin_unlock_irqrestore(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
clk_master_pres_get_parent(struct clk_hw * hw)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(master->regmap, master->layout->offset, &mckr);
408 spin_unlock_irqrestore(master->lock, flags);
409
410 mckr &= master->layout->mask;
411
412 return mckr & AT91_PMC_CSS;
413 }
414
clk_master_pres_save_context(struct clk_hw * hw)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(master->regmap, master->layout->offset, &val);
424 spin_unlock_irqrestore(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(parent_hw);
435 master->pms.rate = DIV_ROUND_CLOSEST_ULL(master->pms.parent_rate, pres);
436
437 return 0;
438 }
439
clk_master_pres_restore_context(struct clk_hw * hw)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(master->regmap, master->layout->offset, &val);
448 spin_unlock_irqrestore(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
at91_clk_register_master_internal(struct regmap * regmap,const char * name,int num_parents,const char ** parent_names,struct clk_hw ** parent_hws,const struct clk_master_layout * layout,const struct clk_master_characteristics * characteristics,const struct clk_ops * ops,spinlock_t * lock,u32 flags)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(-EINVAL);
490
491 master = kzalloc(sizeof(*master), GFP_KERNEL);
492 if (!master)
493 return ERR_PTR(-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(master->regmap, master->layout->offset, &mckr);
513 spin_unlock_irqrestore(master->lock, 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, &master->hw);
522 if (ret) {
523 kfree(master);
524 hw = ERR_PTR(ret);
525 }
526
527 return hw;
528 }
529
530 struct clk_hw * __init
at91_clk_register_master_pres(struct regmap * regmap,const char * name,int num_parents,const char ** parent_names,struct clk_hw ** parent_hws,const struct clk_master_layout * layout,const struct clk_master_characteristics * characteristics,spinlock_t * lock)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 &master_pres_ops,
543 lock, CLK_SET_RATE_GATE);
544 }
545
546 struct clk_hw * __init
at91_clk_register_master_div(struct regmap * regmap,const char * name,const char * parent_name,struct clk_hw * parent_hw,const struct clk_master_layout * layout,const struct clk_master_characteristics * characteristics,spinlock_t * lock,u32 flags,u32 safe_div)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, 1,
562 parent_name ? &parent_name : NULL,
563 parent_hw ? &parent_hw : NULL, layout,
564 characteristics, ops,
565 lock, flags);
566
567 if (!IS_ERR(hw) && safe_div) {
568 master_div = to_clk_master(hw);
569 master_div->safe_div = safe_div;
570 clk_notifier_register(hw->clk,
571 &clk_master_div_notifier);
572 }
573
574 return hw;
575 }
576
577 static unsigned long
clk_sama7g5_master_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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 if (master->div == MASTER_PRES_MAX)
584 return DIV_ROUND_CLOSEST_ULL(parent_rate, 3);
585
586 return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));
587 }
588
clk_sama7g5_master_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)589 static int clk_sama7g5_master_determine_rate(struct clk_hw *hw,
590 struct clk_rate_request *req)
591 {
592 struct clk_master *master = to_clk_master(hw);
593 struct clk_hw *parent;
594 long best_rate = LONG_MIN, best_diff = LONG_MIN;
595 unsigned long parent_rate;
596 unsigned int div, i;
597
598 /* First: check the dividers of MCR. */
599 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
600 parent = clk_hw_get_parent_by_index(hw, i);
601 if (!parent)
602 continue;
603
604 parent_rate = clk_hw_get_rate(parent);
605 if (!parent_rate)
606 continue;
607
608 for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
609 clk_sama7g5_master_best_diff(req, parent, parent_rate,
610 &best_rate, &best_diff,
611 div);
612 if (!best_diff)
613 break;
614 }
615
616 if (!best_diff)
617 break;
618 }
619
620 /* Second: try to request rate form changeable parent. */
621 if (master->chg_pid < 0)
622 goto end;
623
624 parent = clk_hw_get_parent_by_index(hw, master->chg_pid);
625 if (!parent)
626 goto end;
627
628 for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
629 struct clk_rate_request req_parent;
630 unsigned long req_rate;
631
632 if (div == MASTER_PRES_MAX)
633 req_rate = req->rate * 3;
634 else
635 req_rate = req->rate << div;
636
637 clk_hw_forward_rate_request(hw, req, parent, &req_parent, req_rate);
638 if (__clk_determine_rate(parent, &req_parent))
639 continue;
640
641 clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
642 &best_rate, &best_diff, div);
643
644 if (!best_diff)
645 break;
646 }
647
648 end:
649 pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
650 __func__, best_rate,
651 __clk_get_name((req->best_parent_hw)->clk),
652 req->best_parent_rate);
653
654 if (best_rate < 0)
655 return -EINVAL;
656
657 req->rate = best_rate;
658
659 return 0;
660 }
661
clk_sama7g5_master_get_parent(struct clk_hw * hw)662 static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw)
663 {
664 struct clk_master *master = to_clk_master(hw);
665 unsigned long flags;
666 u8 index;
667
668 spin_lock_irqsave(master->lock, flags);
669 index = clk_mux_val_to_index(&master->hw, master->mux_table, 0,
670 master->parent);
671 spin_unlock_irqrestore(master->lock, flags);
672
673 return index;
674 }
675
clk_sama7g5_master_set_parent(struct clk_hw * hw,u8 index)676 static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index)
677 {
678 struct clk_master *master = to_clk_master(hw);
679 unsigned long flags;
680
681 if (index >= clk_hw_get_num_parents(hw))
682 return -EINVAL;
683
684 spin_lock_irqsave(master->lock, flags);
685 master->parent = clk_mux_index_to_val(master->mux_table, 0, index);
686 spin_unlock_irqrestore(master->lock, flags);
687
688 return 0;
689 }
690
clk_sama7g5_master_set(struct clk_master * master,unsigned int status)691 static void clk_sama7g5_master_set(struct clk_master *master,
692 unsigned int status)
693 {
694 unsigned long flags;
695 unsigned int val, cparent;
696 unsigned int enable = status ? AT91_PMC_MCR_V2_EN : 0;
697 unsigned int parent = master->parent << PMC_MCR_CSS_SHIFT;
698 unsigned int div = master->div << MASTER_DIV_SHIFT;
699
700 spin_lock_irqsave(master->lock, flags);
701
702 regmap_write(master->regmap, AT91_PMC_MCR_V2,
703 AT91_PMC_MCR_V2_ID(master->id));
704 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
705 regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,
706 enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV |
707 AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK,
708 enable | parent | div | AT91_PMC_MCR_V2_CMD |
709 AT91_PMC_MCR_V2_ID(master->id));
710
711 cparent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT;
712
713 /* Wait here only if parent is being changed. */
714 while ((cparent != master->parent) && !clk_master_ready(master))
715 cpu_relax();
716
717 spin_unlock_irqrestore(master->lock, flags);
718 }
719
clk_sama7g5_master_enable(struct clk_hw * hw)720 static int clk_sama7g5_master_enable(struct clk_hw *hw)
721 {
722 struct clk_master *master = to_clk_master(hw);
723
724 clk_sama7g5_master_set(master, 1);
725
726 return 0;
727 }
728
clk_sama7g5_master_disable(struct clk_hw * hw)729 static void clk_sama7g5_master_disable(struct clk_hw *hw)
730 {
731 struct clk_master *master = to_clk_master(hw);
732 unsigned long flags;
733
734 spin_lock_irqsave(master->lock, flags);
735
736 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
737 regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,
738 AT91_PMC_MCR_V2_EN | AT91_PMC_MCR_V2_CMD |
739 AT91_PMC_MCR_V2_ID_MSK,
740 AT91_PMC_MCR_V2_CMD |
741 AT91_PMC_MCR_V2_ID(master->id));
742
743 spin_unlock_irqrestore(master->lock, flags);
744 }
745
clk_sama7g5_master_is_enabled(struct clk_hw * hw)746 static int clk_sama7g5_master_is_enabled(struct clk_hw *hw)
747 {
748 struct clk_master *master = to_clk_master(hw);
749 unsigned long flags;
750 unsigned int val;
751
752 spin_lock_irqsave(master->lock, flags);
753
754 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
755 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
756
757 spin_unlock_irqrestore(master->lock, flags);
758
759 return !!(val & AT91_PMC_MCR_V2_EN);
760 }
761
clk_sama7g5_master_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)762 static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate,
763 unsigned long parent_rate)
764 {
765 struct clk_master *master = to_clk_master(hw);
766 unsigned long div, flags;
767
768 div = DIV_ROUND_CLOSEST(parent_rate, rate);
769 if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1)))
770 return -EINVAL;
771
772 if (div == 3)
773 div = MASTER_PRES_MAX;
774 else if (div)
775 div = ffs(div) - 1;
776
777 spin_lock_irqsave(master->lock, flags);
778 master->div = div;
779 spin_unlock_irqrestore(master->lock, flags);
780
781 return 0;
782 }
783
clk_sama7g5_master_save_context(struct clk_hw * hw)784 static int clk_sama7g5_master_save_context(struct clk_hw *hw)
785 {
786 struct clk_master *master = to_clk_master(hw);
787
788 master->pms.status = clk_sama7g5_master_is_enabled(hw);
789
790 return 0;
791 }
792
clk_sama7g5_master_restore_context(struct clk_hw * hw)793 static void clk_sama7g5_master_restore_context(struct clk_hw *hw)
794 {
795 struct clk_master *master = to_clk_master(hw);
796
797 if (master->pms.status)
798 clk_sama7g5_master_set(master, master->pms.status);
799 }
800
801 static const struct clk_ops sama7g5_master_ops = {
802 .enable = clk_sama7g5_master_enable,
803 .disable = clk_sama7g5_master_disable,
804 .is_enabled = clk_sama7g5_master_is_enabled,
805 .recalc_rate = clk_sama7g5_master_recalc_rate,
806 .determine_rate = clk_sama7g5_master_determine_rate,
807 .set_rate = clk_sama7g5_master_set_rate,
808 .get_parent = clk_sama7g5_master_get_parent,
809 .set_parent = clk_sama7g5_master_set_parent,
810 .save_context = clk_sama7g5_master_save_context,
811 .restore_context = clk_sama7g5_master_restore_context,
812 };
813
814 struct clk_hw * __init
at91_clk_sama7g5_register_master(struct regmap * regmap,const char * name,int num_parents,const char ** parent_names,struct clk_hw ** parent_hws,u32 * mux_table,spinlock_t * lock,u8 id,bool critical,int chg_pid)815 at91_clk_sama7g5_register_master(struct regmap *regmap,
816 const char *name, int num_parents,
817 const char **parent_names,
818 struct clk_hw **parent_hws,
819 u32 *mux_table,
820 spinlock_t *lock, u8 id,
821 bool critical, int chg_pid)
822 {
823 struct clk_master *master;
824 struct clk_hw *hw;
825 struct clk_init_data init = {};
826 unsigned long flags;
827 unsigned int val;
828 int ret;
829
830 if (!name || !num_parents || !(parent_names || parent_hws) || !mux_table ||
831 !lock || id > MASTER_MAX_ID)
832 return ERR_PTR(-EINVAL);
833
834 master = kzalloc(sizeof(*master), GFP_KERNEL);
835 if (!master)
836 return ERR_PTR(-ENOMEM);
837
838 init.name = name;
839 init.ops = &sama7g5_master_ops;
840 if (parent_hws)
841 init.parent_hws = (const struct clk_hw **)parent_hws;
842 else
843 init.parent_names = parent_names;
844 init.num_parents = num_parents;
845 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
846 if (chg_pid >= 0)
847 init.flags |= CLK_SET_RATE_PARENT;
848 if (critical)
849 init.flags |= CLK_IS_CRITICAL;
850
851 master->hw.init = &init;
852 master->regmap = regmap;
853 master->id = id;
854 master->chg_pid = chg_pid;
855 master->lock = lock;
856 master->mux_table = mux_table;
857
858 spin_lock_irqsave(master->lock, flags);
859 regmap_write(master->regmap, AT91_PMC_MCR_V2, master->id);
860 regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
861 master->parent = (val & AT91_PMC_MCR_V2_CSS) >> PMC_MCR_CSS_SHIFT;
862 master->div = (val & AT91_PMC_MCR_V2_DIV) >> MASTER_DIV_SHIFT;
863 spin_unlock_irqrestore(master->lock, flags);
864
865 hw = &master->hw;
866 ret = clk_hw_register(NULL, &master->hw);
867 if (ret) {
868 kfree(master);
869 hw = ERR_PTR(ret);
870 }
871
872 return hw;
873 }
874
875 const struct clk_master_layout at91rm9200_master_layout = {
876 .mask = 0x31F,
877 .pres_shift = 2,
878 .offset = AT91_PMC_MCKR,
879 };
880
881 const struct clk_master_layout at91sam9x5_master_layout = {
882 .mask = 0x373,
883 .pres_shift = 4,
884 .offset = AT91_PMC_MCKR,
885 };
886