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 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
clk_peripheral_enable(struct clk_hw * hw)47 static 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
clk_peripheral_disable(struct clk_hw * hw)62 static 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
clk_peripheral_is_enabled(struct clk_hw * hw)75 static 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
91 static 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
97 struct clk_hw * __init
at91_clk_register_peripheral(struct regmap * regmap,const char * name,const char * parent_name,u32 id)98 at91_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
clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral * periph)133 static 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
clk_sam9x5_peripheral_enable(struct clk_hw * hw)158 static 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
clk_sam9x5_peripheral_disable(struct clk_hw * hw)180 static 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
clk_sam9x5_peripheral_is_enabled(struct clk_hw * hw)197 static 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
215 static unsigned long
clk_sam9x5_peripheral_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)216 clk_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
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)242 static 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
clk_sam9x5_peripheral_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)259 static 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 }
308 end:
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
clk_sam9x5_peripheral_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)323 static 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
clk_sam9x5_peripheral_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)369 static 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
396 static 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
405 static 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
414 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)415 at91_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