clk-master.c

  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
 27struct 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. */
 44static struct clk_master *master_div;
 45
 46static 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
 56static 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
 71static 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
 84static 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
113static 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
134static 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
153static 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. */
162static 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
217static 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
225static 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
240static 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
248static 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
343unlock:
344	spin_unlock_irqrestore(master_div->lock, flags);
345
346	return ret;
347}
348
349static struct notifier_block clk_master_div_notifier = {
350	.notifier_call = clk_master_div_notifier_fn,
351};
352
353static 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
377static 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
400static 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
415static 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
440static 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
463static 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
472static struct clk_hw * __init
473at91_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
530struct clk_hw * __init
531at91_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
546struct clk_hw * __init
547at91_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
577static unsigned long
578clk_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
589static 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
648end:
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
662static 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
676static 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
691static 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
720static 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
729static 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
746static 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
762static 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
784static 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
793static 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
801static 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
814struct clk_hw * __init
815at91_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
875const struct clk_master_layout at91rm9200_master_layout = {
876	.mask = 0x31F,
877	.pres_shift = 2,
878	.offset = AT91_PMC_MCKR,
879};
880
881const struct clk_master_layout at91sam9x5_master_layout = {
882	.mask = 0x373,
883	.pres_shift = 4,
884	.offset = AT91_PMC_MCKR,
885};