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
17 DEFINE_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
25 struct 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
33 struct 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
clk_peripheral_enable(struct clk_hw * hw)49 static 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
clk_peripheral_disable(struct clk_hw * hw)64 static 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
clk_peripheral_is_enabled(struct clk_hw * hw)77 static 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
93 static 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
99 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)100 at91_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
clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral * periph)139 static 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
clk_sam9x5_peripheral_set(struct clk_sam9x5_peripheral * periph,unsigned int status)164 static 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
clk_sam9x5_peripheral_enable(struct clk_hw * hw)186 static 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
clk_sam9x5_peripheral_disable(struct clk_hw * hw)193 static 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
clk_sam9x5_peripheral_is_enabled(struct clk_hw * hw)210 static 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
228 static unsigned long
clk_sam9x5_peripheral_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)229 clk_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
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)255 static 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
clk_sam9x5_peripheral_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)272 static 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 }
324 end:
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
clk_sam9x5_peripheral_no_parent_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)339 static 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
clk_sam9x5_peripheral_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)392 static 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
clk_sam9x5_peripheral_save_context(struct clk_hw * hw)419 static 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
clk_sam9x5_peripheral_restore_context(struct clk_hw * hw)428 static 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
436 static 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
447 static 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
458 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)459 at91_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 }
515