clk-peripheral.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/bitfield.h>
  7#include <linux/bitops.h>
  8#include <linux/clk-provider.h>
  9#include <linux/clkdev.h>
 10#include <linux/clk/at91_pmc.h>
 11#include <linux/of.h>
 12#include <linux/mfd/syscon.h>
 13#include <linux/regmap.h>
 14
 15#include "pmc.h"
 16
 17DEFINE_SPINLOCK(pmc_pcr_lock);
 18
 19#define PERIPHERAL_ID_MIN	2
 20#define PERIPHERAL_ID_MAX	31
 21#define PERIPHERAL_MASK(id)	(1 << ((id) & PERIPHERAL_ID_MAX))
 22
 23#define PERIPHERAL_MAX_SHIFT	3
 24
 25struct clk_peripheral {
 26	struct clk_hw hw;
 27	struct regmap *regmap;
 28	u32 id;
 29};
 30
 31#define to_clk_peripheral(hw) container_of(hw, struct clk_peripheral, hw)
 32
 33struct clk_sam9x5_peripheral {
 34	struct clk_hw hw;
 35	struct regmap *regmap;
 36	struct clk_range range;
 37	spinlock_t *lock;
 38	u32 id;
 39	u32 div;
 40	const struct clk_pcr_layout *layout;
 41	struct at91_clk_pms pms;
 42	bool auto_div;
 43	int chg_pid;
 44};
 45
 46#define to_clk_sam9x5_peripheral(hw) \
 47	container_of(hw, struct clk_sam9x5_peripheral, hw)
 48
 49static int clk_peripheral_enable(struct clk_hw *hw)
 50{
 51	struct clk_peripheral *periph = to_clk_peripheral(hw);
 52	int offset = AT91_PMC_PCER;
 53	u32 id = periph->id;
 54
 55	if (id < PERIPHERAL_ID_MIN)
 56		return 0;
 57	if (id > PERIPHERAL_ID_MAX)
 58		offset = AT91_PMC_PCER1;
 59	regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id));
 60
 61	return 0;
 62}
 63
 64static void clk_peripheral_disable(struct clk_hw *hw)
 65{
 66	struct clk_peripheral *periph = to_clk_peripheral(hw);
 67	int offset = AT91_PMC_PCDR;
 68	u32 id = periph->id;
 69
 70	if (id < PERIPHERAL_ID_MIN)
 71		return;
 72	if (id > PERIPHERAL_ID_MAX)
 73		offset = AT91_PMC_PCDR1;
 74	regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id));
 75}
 76
 77static int clk_peripheral_is_enabled(struct clk_hw *hw)
 78{
 79	struct clk_peripheral *periph = to_clk_peripheral(hw);
 80	int offset = AT91_PMC_PCSR;
 81	unsigned int status;
 82	u32 id = periph->id;
 83
 84	if (id < PERIPHERAL_ID_MIN)
 85		return 1;
 86	if (id > PERIPHERAL_ID_MAX)
 87		offset = AT91_PMC_PCSR1;
 88	regmap_read(periph->regmap, offset, &status);
 89
 90	return status & PERIPHERAL_MASK(id) ? 1 : 0;
 91}
 92
 93static const struct clk_ops peripheral_ops = {
 94	.enable = clk_peripheral_enable,
 95	.disable = clk_peripheral_disable,
 96	.is_enabled = clk_peripheral_is_enabled,
 97};
 98
 99struct clk_hw * __init
100at91_clk_register_peripheral(struct regmap *regmap, const char *name,
101			     const char *parent_name, struct clk_hw *parent_hw,
102			     u32 id)
103{
104	struct clk_peripheral *periph;
105	struct clk_init_data init = {};
106	struct clk_hw *hw;
107	int ret;
108
109	if (!name || !(parent_name || parent_hw) || id > PERIPHERAL_ID_MAX)
110		return ERR_PTR(-EINVAL);
111
112	periph = kzalloc(sizeof(*periph), GFP_KERNEL);
113	if (!periph)
114		return ERR_PTR(-ENOMEM);
115
116	init.name = name;
117	init.ops = &peripheral_ops;
118	if (parent_hw)
119		init.parent_hws = (const struct clk_hw **)&parent_hw;
120	else
121		init.parent_names = &parent_name;
122	init.num_parents = 1;
123	init.flags = 0;
124
125	periph->id = id;
126	periph->hw.init = &init;
127	periph->regmap = regmap;
128
129	hw = &periph->hw;
130	ret = clk_hw_register(NULL, &periph->hw);
131	if (ret) {
132		kfree(periph);
133		hw = ERR_PTR(ret);
134	}
135
136	return hw;
137}
138
139static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph)
140{
141	struct clk_hw *parent;
142	unsigned long parent_rate;
143	int shift = 0;
144
145	if (!periph->auto_div)
146		return;
147
148	if (periph->range.max) {
149		parent = clk_hw_get_parent_by_index(&periph->hw, 0);
150		parent_rate = clk_hw_get_rate(parent);
151		if (!parent_rate)
152			return;
153
154		for (; shift < PERIPHERAL_MAX_SHIFT; shift++) {
155			if (parent_rate >> shift <= periph->range.max)
156				break;
157		}
158	}
159
160	periph->auto_div = false;
161	periph->div = shift;
162}
163
164static int clk_sam9x5_peripheral_set(struct clk_sam9x5_peripheral *periph,
165				     unsigned int status)
166{
167	unsigned long flags;
168	unsigned int enable = status ? AT91_PMC_PCR_EN : 0;
169
170	if (periph->id < PERIPHERAL_ID_MIN)
171		return 0;
172
173	spin_lock_irqsave(periph->lock, flags);
174	regmap_write(periph->regmap, periph->layout->offset,
175		     (periph->id & periph->layout->pid_mask));
176	regmap_update_bits(periph->regmap, periph->layout->offset,
177			   periph->layout->div_mask | periph->layout->cmd |
178			   enable,
179			   field_prep(periph->layout->div_mask, periph->div) |
180			   periph->layout->cmd | enable);
181	spin_unlock_irqrestore(periph->lock, flags);
182
183	return 0;
184}
185
186static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
187{
188	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
189
190	return clk_sam9x5_peripheral_set(periph, 1);
191}
192
193static void clk_sam9x5_peripheral_disable(struct clk_hw *hw)
194{
195	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
196	unsigned long flags;
197
198	if (periph->id < PERIPHERAL_ID_MIN)
199		return;
200
201	spin_lock_irqsave(periph->lock, flags);
202	regmap_write(periph->regmap, periph->layout->offset,
203		     (periph->id & periph->layout->pid_mask));
204	regmap_update_bits(periph->regmap, periph->layout->offset,
205			   AT91_PMC_PCR_EN | periph->layout->cmd,
206			   periph->layout->cmd);
207	spin_unlock_irqrestore(periph->lock, flags);
208}
209
210static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw)
211{
212	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
213	unsigned long flags;
214	unsigned int status;
215
216	if (periph->id < PERIPHERAL_ID_MIN)
217		return 1;
218
219	spin_lock_irqsave(periph->lock, flags);
220	regmap_write(periph->regmap, periph->layout->offset,
221		     (periph->id & periph->layout->pid_mask));
222	regmap_read(periph->regmap, periph->layout->offset, &status);
223	spin_unlock_irqrestore(periph->lock, flags);
224
225	return !!(status & AT91_PMC_PCR_EN);
226}
227
228static unsigned long
229clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw,
230				  unsigned long parent_rate)
231{
232	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
233	unsigned long flags;
234	unsigned int status;
235
236	if (periph->id < PERIPHERAL_ID_MIN)
237		return parent_rate;
238
239	spin_lock_irqsave(periph->lock, flags);
240	regmap_write(periph->regmap, periph->layout->offset,
241		     (periph->id & periph->layout->pid_mask));
242	regmap_read(periph->regmap, periph->layout->offset, &status);
243	spin_unlock_irqrestore(periph->lock, flags);
244
245	if (status & AT91_PMC_PCR_EN) {
246		periph->div = field_get(periph->layout->div_mask, status);
247		periph->auto_div = false;
248	} else {
249		clk_sam9x5_peripheral_autodiv(periph);
250	}
251
252	return parent_rate >> periph->div;
253}
254
255static void clk_sam9x5_peripheral_best_diff(struct clk_rate_request *req,
256					    struct clk_hw *parent,
257					    unsigned long parent_rate,
258					    u32 shift, long *best_diff,
259					    long *best_rate)
260{
261	unsigned long tmp_rate = parent_rate >> shift;
262	unsigned long tmp_diff = abs(req->rate - tmp_rate);
263
264	if (*best_diff < 0 || *best_diff >= tmp_diff) {
265		*best_rate = tmp_rate;
266		*best_diff = tmp_diff;
267		req->best_parent_rate = parent_rate;
268		req->best_parent_hw = parent;
269	}
270}
271
272static int clk_sam9x5_peripheral_determine_rate(struct clk_hw *hw,
273						struct clk_rate_request *req)
274{
275	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
276	struct clk_hw *parent = clk_hw_get_parent(hw);
277	unsigned long parent_rate = clk_hw_get_rate(parent);
278	unsigned long tmp_rate;
279	long best_rate = LONG_MIN;
280	long best_diff = LONG_MIN;
281	u32 shift;
282
283	if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) {
284		req->rate = parent_rate;
285
286		return 0;
287	}
288
289	/* Fist step: check the available dividers. */
290	for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
291		tmp_rate = parent_rate >> shift;
292
293		if (periph->range.max && tmp_rate > periph->range.max)
294			continue;
295
296		clk_sam9x5_peripheral_best_diff(req, parent, parent_rate,
297						shift, &best_diff, &best_rate);
298
299		if (!best_diff || best_rate <= req->rate)
300			break;
301	}
302
303	if (periph->chg_pid < 0)
304		goto end;
305
306	/* Step two: try to request rate from parent. */
307	parent = clk_hw_get_parent_by_index(hw, periph->chg_pid);
308	if (!parent)
309		goto end;
310
311	for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
312		struct clk_rate_request req_parent;
313
314		clk_hw_forward_rate_request(hw, req, parent, &req_parent, req->rate << shift);
315		if (__clk_determine_rate(parent, &req_parent))
316			continue;
317
318		clk_sam9x5_peripheral_best_diff(req, parent, req_parent.rate,
319						shift, &best_diff, &best_rate);
320
321		if (!best_diff)
322			break;
323	}
324end:
325	if (best_rate < 0 ||
326	    (periph->range.max && best_rate > periph->range.max))
327		return -EINVAL;
328
329	pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
330		 __func__, best_rate,
331		 __clk_get_name((req->best_parent_hw)->clk),
332		 req->best_parent_rate);
333
334	req->rate = best_rate;
335
336	return 0;
337}
338
339static int clk_sam9x5_peripheral_no_parent_determine_rate(struct clk_hw *hw,
340							  struct clk_rate_request *req)
341{
342	int shift = 0;
343	unsigned long best_rate;
344	unsigned long best_diff;
345	unsigned long cur_rate = req->best_parent_rate;
346	unsigned long cur_diff;
347	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
348
349	if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) {
350		req->rate = req->best_parent_rate;
351
352		return 0;
353	}
354
355	if (periph->range.max) {
356		for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
357			cur_rate = req->best_parent_rate >> shift;
358			if (cur_rate <= periph->range.max)
359				break;
360		}
361	}
362
363	if (req->rate >= cur_rate) {
364		req->rate = cur_rate;
365
366		return 0;
367	}
368
369	best_diff = cur_rate - req->rate;
370	best_rate = cur_rate;
371	for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
372		cur_rate = req->best_parent_rate >> shift;
373		if (cur_rate < req->rate)
374			cur_diff = req->rate - cur_rate;
375		else
376			cur_diff = cur_rate - req->rate;
377
378		if (cur_diff < best_diff) {
379			best_diff = cur_diff;
380			best_rate = cur_rate;
381		}
382
383		if (!best_diff || cur_rate < req->rate)
384			break;
385	}
386
387	req->rate = best_rate;
388
389	return 0;
390}
391
392static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw,
393					  unsigned long rate,
394					  unsigned long parent_rate)
395{
396	int shift;
397	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
398	if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) {
399		if (parent_rate == rate)
400			return 0;
401		else
402			return -EINVAL;
403	}
404
405	if (periph->range.max && rate > periph->range.max)
406		return -EINVAL;
407
408	for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
409		if (parent_rate >> shift == rate) {
410			periph->auto_div = false;
411			periph->div = shift;
412			return 0;
413		}
414	}
415
416	return -EINVAL;
417}
418
419static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw)
420{
421	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
422
423	periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw);
424
425	return 0;
426}
427
428static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw)
429{
430	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
431
432	if (periph->pms.status)
433		clk_sam9x5_peripheral_set(periph, periph->pms.status);
434}
435
436static const struct clk_ops sam9x5_peripheral_ops = {
437	.enable = clk_sam9x5_peripheral_enable,
438	.disable = clk_sam9x5_peripheral_disable,
439	.is_enabled = clk_sam9x5_peripheral_is_enabled,
440	.recalc_rate = clk_sam9x5_peripheral_recalc_rate,
441	.determine_rate = clk_sam9x5_peripheral_no_parent_determine_rate,
442	.set_rate = clk_sam9x5_peripheral_set_rate,
443	.save_context = clk_sam9x5_peripheral_save_context,
444	.restore_context = clk_sam9x5_peripheral_restore_context,
445};
446
447static const struct clk_ops sam9x5_peripheral_chg_ops = {
448	.enable = clk_sam9x5_peripheral_enable,
449	.disable = clk_sam9x5_peripheral_disable,
450	.is_enabled = clk_sam9x5_peripheral_is_enabled,
451	.recalc_rate = clk_sam9x5_peripheral_recalc_rate,
452	.determine_rate = clk_sam9x5_peripheral_determine_rate,
453	.set_rate = clk_sam9x5_peripheral_set_rate,
454	.save_context = clk_sam9x5_peripheral_save_context,
455	.restore_context = clk_sam9x5_peripheral_restore_context,
456};
457
458struct clk_hw * __init
459at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
460				    const struct clk_pcr_layout *layout,
461				    const char *name, const char *parent_name,
462				    struct clk_hw *parent_hw,
463				    u32 id, const struct clk_range *range,
464				    int chg_pid, unsigned long flags)
465{
466	struct clk_sam9x5_peripheral *periph;
467	struct clk_init_data init = {};
468	struct clk_hw *hw;
469	int ret;
470
471	if (!name || !(parent_name || parent_hw))
472		return ERR_PTR(-EINVAL);
473
474	periph = kzalloc(sizeof(*periph), GFP_KERNEL);
475	if (!periph)
476		return ERR_PTR(-ENOMEM);
477
478	init.name = name;
479	if (parent_hw)
480		init.parent_hws = (const struct clk_hw **)&parent_hw;
481	else
482		init.parent_names = &parent_name;
483	init.num_parents = 1;
484	init.flags = flags;
485	if (chg_pid < 0) {
486		init.ops = &sam9x5_peripheral_ops;
487	} else {
488		init.flags |= CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |
489			      CLK_SET_RATE_PARENT;
490		init.ops = &sam9x5_peripheral_chg_ops;
491	}
492
493	periph->id = id;
494	periph->hw.init = &init;
495	periph->div = 0;
496	periph->regmap = regmap;
497	periph->lock = lock;
498	if (layout->div_mask)
499		periph->auto_div = true;
500	periph->layout = layout;
501	periph->range = *range;
502	periph->chg_pid = chg_pid;
503
504	hw = &periph->hw;
505	ret = clk_hw_register(NULL, &periph->hw);
506	if (ret) {
507		kfree(periph);
508		hw = ERR_PTR(ret);
509	} else {
510		clk_sam9x5_peripheral_autodiv(periph);
511	}
512
513	return hw;
514}