xref: /linux/drivers/clk/at91/clk-master.c (revision cdd5b5a9761fd66d17586e4f4ba6588c70e640ea)
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 
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 	return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));
584 }
585 
clk_sama7g5_master_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)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, i);
598 		if (!parent)
599 			continue;
600 
601 		parent_rate = clk_hw_get_rate(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_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, 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(hw, req, parent, &req_parent, req_rate);
635 		if (__clk_determine_rate(parent, &req_parent))
636 			continue;
637 
638 		clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
639 					     &best_rate, &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 
clk_sama7g5_master_get_parent(struct clk_hw * hw)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(&master->hw, master->mux_table, 0,
667 				     master->parent);
668 	spin_unlock_irqrestore(master->lock, flags);
669 
670 	return index;
671 }
672 
clk_sama7g5_master_set_parent(struct clk_hw * hw,u8 index)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(master->mux_table, 0, index);
683 	spin_unlock_irqrestore(master->lock, flags);
684 
685 	return 0;
686 }
687 
clk_sama7g5_master_set(struct clk_master * master,unsigned int status)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(master->regmap, AT91_PMC_MCR_V2,
700 		     AT91_PMC_MCR_V2_ID(master->id));
701 	regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
702 	regmap_update_bits(master->regmap, AT91_PMC_MCR_V2,
703 			   enable | AT91_PMC_MCR_V2_CSS | AT91_PMC_MCR_V2_DIV |
704 			   AT91_PMC_MCR_V2_CMD | AT91_PMC_MCR_V2_ID_MSK,
705 			   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(master->lock, flags);
715 }
716 
clk_sama7g5_master_enable(struct clk_hw * hw)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, 1);
722 
723 	return 0;
724 }
725 
clk_sama7g5_master_disable(struct clk_hw * hw)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(master->regmap, AT91_PMC_MCR_V2, master->id);
734 	regmap_update_bits(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(master->lock, flags);
741 }
742 
clk_sama7g5_master_is_enabled(struct clk_hw * hw)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(master->regmap, AT91_PMC_MCR_V2, master->id);
752 	regmap_read(master->regmap, AT91_PMC_MCR_V2, &val);
753 
754 	spin_unlock_irqrestore(master->lock, flags);
755 
756 	return !!(val & AT91_PMC_MCR_V2_EN);
757 }
758 
clk_sama7g5_master_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)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(master->lock, flags);
777 
778 	return 0;
779 }
780 
clk_sama7g5_master_save_context(struct clk_hw * hw)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 
clk_sama7g5_master_restore_context(struct clk_hw * hw)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, 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
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)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(-EINVAL);
830 
831 	master = kzalloc(sizeof(*master), GFP_KERNEL);
832 	if (!master)
833 		return ERR_PTR(-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(master->regmap, AT91_PMC_MCR_V2, master->id);
857 	regmap_read(master->regmap, AT91_PMC_MCR_V2, &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(master->lock, flags);
861 
862 	hw = &master->hw;
863 	ret = clk_hw_register(NULL, &master->hw);
864 	if (ret) {
865 		kfree(master);
866 		hw = ERR_PTR(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