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