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