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