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 | |
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(map: periph->regmap, reg: offset, PERIPHERAL_MASK(id)); |
59 | |
60 | return 0; |
61 | } |
62 | |
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(map: periph->regmap, reg: offset, PERIPHERAL_MASK(id)); |
74 | } |
75 | |
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(map: periph->regmap, reg: offset, val: &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 |
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(error: -EINVAL); |
110 | |
111 | periph = kzalloc(size: sizeof(*periph), GFP_KERNEL); |
112 | if (!periph) |
113 | return ERR_PTR(error: -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, hw: &periph->hw); |
130 | if (ret) { |
131 | kfree(objp: periph); |
132 | hw = ERR_PTR(error: ret); |
133 | } |
134 | |
135 | return hw; |
136 | } |
137 | |
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(hw: &periph->hw, index: 0); |
149 | parent_rate = clk_hw_get_rate(hw: 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 | |
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(map: periph->regmap, reg: periph->layout->offset, |
174 | val: (periph->id & periph->layout->pid_mask)); |
175 | regmap_update_bits(map: periph->regmap, reg: periph->layout->offset, |
176 | mask: 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(lock: periph->lock, flags); |
181 | |
182 | return 0; |
183 | } |
184 | |
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, status: 1); |
190 | } |
191 | |
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(map: periph->regmap, reg: periph->layout->offset, |
202 | val: (periph->id & periph->layout->pid_mask)); |
203 | regmap_update_bits(map: periph->regmap, reg: periph->layout->offset, |
204 | AT91_PMC_PCR_EN | periph->layout->cmd, |
205 | val: periph->layout->cmd); |
206 | spin_unlock_irqrestore(lock: periph->lock, flags); |
207 | } |
208 | |
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(map: periph->regmap, reg: periph->layout->offset, |
220 | val: (periph->id & periph->layout->pid_mask)); |
221 | regmap_read(map: periph->regmap, reg: periph->layout->offset, val: &status); |
222 | spin_unlock_irqrestore(lock: periph->lock, flags); |
223 | |
224 | return !!(status & AT91_PMC_PCR_EN); |
225 | } |
226 | |
227 | static unsigned long |
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(map: periph->regmap, reg: periph->layout->offset, |
240 | val: (periph->id & periph->layout->pid_mask)); |
241 | regmap_read(map: periph->regmap, reg: periph->layout->offset, val: &status); |
242 | spin_unlock_irqrestore(lock: 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 | |
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 | |
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(hw: 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 | return parent_rate; |
284 | |
285 | /* Fist step: check the available dividers. */ |
286 | for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { |
287 | tmp_rate = parent_rate >> shift; |
288 | |
289 | if (periph->range.max && tmp_rate > periph->range.max) |
290 | continue; |
291 | |
292 | clk_sam9x5_peripheral_best_diff(req, parent, parent_rate, |
293 | shift, best_diff: &best_diff, best_rate: &best_rate); |
294 | |
295 | if (!best_diff || best_rate <= req->rate) |
296 | break; |
297 | } |
298 | |
299 | if (periph->chg_pid < 0) |
300 | goto end; |
301 | |
302 | /* Step two: try to request rate from parent. */ |
303 | parent = clk_hw_get_parent_by_index(hw, index: periph->chg_pid); |
304 | if (!parent) |
305 | goto end; |
306 | |
307 | for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { |
308 | struct clk_rate_request req_parent; |
309 | |
310 | clk_hw_forward_rate_request(core: hw, old_req: req, parent, req: &req_parent, parent_rate: req->rate << shift); |
311 | if (__clk_determine_rate(core: parent, req: &req_parent)) |
312 | continue; |
313 | |
314 | clk_sam9x5_peripheral_best_diff(req, parent, parent_rate: req_parent.rate, |
315 | shift, best_diff: &best_diff, best_rate: &best_rate); |
316 | |
317 | if (!best_diff) |
318 | break; |
319 | } |
320 | end: |
321 | if (best_rate < 0 || |
322 | (periph->range.max && best_rate > periph->range.max)) |
323 | return -EINVAL; |
324 | |
325 | pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n" , |
326 | __func__, best_rate, |
327 | __clk_get_name((req->best_parent_hw)->clk), |
328 | req->best_parent_rate); |
329 | |
330 | req->rate = best_rate; |
331 | |
332 | return 0; |
333 | } |
334 | |
335 | static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw, |
336 | unsigned long rate, |
337 | unsigned long *parent_rate) |
338 | { |
339 | int shift = 0; |
340 | unsigned long best_rate; |
341 | unsigned long best_diff; |
342 | unsigned long cur_rate = *parent_rate; |
343 | unsigned long cur_diff; |
344 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
345 | |
346 | if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) |
347 | return *parent_rate; |
348 | |
349 | if (periph->range.max) { |
350 | for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { |
351 | cur_rate = *parent_rate >> shift; |
352 | if (cur_rate <= periph->range.max) |
353 | break; |
354 | } |
355 | } |
356 | |
357 | if (rate >= cur_rate) |
358 | return cur_rate; |
359 | |
360 | best_diff = cur_rate - rate; |
361 | best_rate = cur_rate; |
362 | for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { |
363 | cur_rate = *parent_rate >> shift; |
364 | if (cur_rate < rate) |
365 | cur_diff = rate - cur_rate; |
366 | else |
367 | cur_diff = cur_rate - rate; |
368 | |
369 | if (cur_diff < best_diff) { |
370 | best_diff = cur_diff; |
371 | best_rate = cur_rate; |
372 | } |
373 | |
374 | if (!best_diff || cur_rate < rate) |
375 | break; |
376 | } |
377 | |
378 | return best_rate; |
379 | } |
380 | |
381 | static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw, |
382 | unsigned long rate, |
383 | unsigned long parent_rate) |
384 | { |
385 | int shift; |
386 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
387 | if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) { |
388 | if (parent_rate == rate) |
389 | return 0; |
390 | else |
391 | return -EINVAL; |
392 | } |
393 | |
394 | if (periph->range.max && rate > periph->range.max) |
395 | return -EINVAL; |
396 | |
397 | for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { |
398 | if (parent_rate >> shift == rate) { |
399 | periph->auto_div = false; |
400 | periph->div = shift; |
401 | return 0; |
402 | } |
403 | } |
404 | |
405 | return -EINVAL; |
406 | } |
407 | |
408 | static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw) |
409 | { |
410 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
411 | |
412 | periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw); |
413 | |
414 | return 0; |
415 | } |
416 | |
417 | static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw) |
418 | { |
419 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
420 | |
421 | if (periph->pms.status) |
422 | clk_sam9x5_peripheral_set(periph, status: periph->pms.status); |
423 | } |
424 | |
425 | static const struct clk_ops sam9x5_peripheral_ops = { |
426 | .enable = clk_sam9x5_peripheral_enable, |
427 | .disable = clk_sam9x5_peripheral_disable, |
428 | .is_enabled = clk_sam9x5_peripheral_is_enabled, |
429 | .recalc_rate = clk_sam9x5_peripheral_recalc_rate, |
430 | .round_rate = clk_sam9x5_peripheral_round_rate, |
431 | .set_rate = clk_sam9x5_peripheral_set_rate, |
432 | .save_context = clk_sam9x5_peripheral_save_context, |
433 | .restore_context = clk_sam9x5_peripheral_restore_context, |
434 | }; |
435 | |
436 | static const struct clk_ops sam9x5_peripheral_chg_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_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 | struct clk_hw * __init |
448 | at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock, |
449 | const struct clk_pcr_layout *layout, |
450 | const char *name, const char *parent_name, |
451 | struct clk_hw *parent_hw, |
452 | u32 id, const struct clk_range *range, |
453 | int chg_pid, unsigned long flags) |
454 | { |
455 | struct clk_sam9x5_peripheral *periph; |
456 | struct clk_init_data init = {}; |
457 | struct clk_hw *hw; |
458 | int ret; |
459 | |
460 | if (!name || !(parent_name || parent_hw)) |
461 | return ERR_PTR(error: -EINVAL); |
462 | |
463 | periph = kzalloc(size: sizeof(*periph), GFP_KERNEL); |
464 | if (!periph) |
465 | return ERR_PTR(error: -ENOMEM); |
466 | |
467 | init.name = name; |
468 | if (parent_hw) |
469 | init.parent_hws = (const struct clk_hw **)&parent_hw; |
470 | else |
471 | init.parent_names = &parent_name; |
472 | init.num_parents = 1; |
473 | init.flags = flags; |
474 | if (chg_pid < 0) { |
475 | init.ops = &sam9x5_peripheral_ops; |
476 | } else { |
477 | init.flags |= CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE | |
478 | CLK_SET_RATE_PARENT; |
479 | init.ops = &sam9x5_peripheral_chg_ops; |
480 | } |
481 | |
482 | periph->id = id; |
483 | periph->hw.init = &init; |
484 | periph->div = 0; |
485 | periph->regmap = regmap; |
486 | periph->lock = lock; |
487 | if (layout->div_mask) |
488 | periph->auto_div = true; |
489 | periph->layout = layout; |
490 | periph->range = *range; |
491 | periph->chg_pid = chg_pid; |
492 | |
493 | hw = &periph->hw; |
494 | ret = clk_hw_register(NULL, hw: &periph->hw); |
495 | if (ret) { |
496 | kfree(objp: periph); |
497 | hw = ERR_PTR(error: ret); |
498 | } else { |
499 | clk_sam9x5_peripheral_autodiv(periph); |
500 | } |
501 | |
502 | return hw; |
503 | } |
504 | |