1
2
3
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
16DEFINE_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
24struct 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
32struct 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
47static 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
62static 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
75static 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
91static 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
97struct clk_hw * __init
98at91_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
133static 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
158static 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
180static 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
197static 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
215static unsigned long
216clk_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
242static 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
259static 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
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
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 }
308end:
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
323static 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
369static 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
396static 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
405static 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
414struct clk_hw * __init
415at91_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
