1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (C) 2019 Microchip Technology Inc.
4 */
5
6 #include <io.h>
7 #include <kernel/misc.h>
8 #include <mm/core_memprot.h>
9 #include <types_ext.h>
10 #include <util.h>
11 #include "at91_clk.h"
12
13 #define PMC_PLL_CTRL0_DIV_MASK 0xf
14 #define PMC_PLL_CTRL0_DIV_POS 0
15 #define PMC_PLL_CTRL1_MUL_MASK 0xff
16 #define PMC_PLL_CTRL1_MUL_POS 24
17 #define PMC_PLL_CTRL1_FRACR_MASK 0x3fffff
18 #define PMC_PLL_CTRL1_FRACR_POS 0
19
20 #define PLL_STATUS_MASK(id) BIT(1 + (id))
21 #define PLL_REG(id) (AT91_CKGR_PLLAR + ((id) * 4))
22 #define PLL_DIV_MASK 0xff
23 #define PLL_DIV_MAX PLL_DIV_MASK
24 #define PLL_DIV(reg) ((reg) & PLL_DIV_MASK)
25 #define PLL_MUL(reg, layout) \
26 ({ \
27 typeof(layout) __layout = layout; \
28 \
29 (((reg) >> (__layout)->mul_shift) & (__layout)->mul_mask); \
30 })
31 #define PLL_MUL_MIN 2
32 #define PLL_MUL_MASK(layout) ((layout)->mul_mask)
33 #define PLL_MUL_MAX(layout) (PLL_MUL_MASK(layout) + 1)
34 #define PLL_ICPR_SHIFT(id) ((id) * 16)
35 #define PLL_ICPR_MASK(id) SHIFT_U64(0xffff, PLL_ICPR_SHIFT(id))
36 #define PLL_MAX_COUNT 0x3f
37 #define PLL_COUNT_SHIFT 8
38 #define PLL_OUT_SHIFT 14
39
40 struct sam9x60_pll_core {
41 vaddr_t base;
42 const struct clk_pll_charac *charac;
43 const struct clk_pll_layout *layout;
44 struct clk *hw;
45 uint8_t id;
46 };
47
48 struct sam9x60_frac {
49 struct sam9x60_pll_core core;
50 uint32_t frac;
51 uint16_t mul;
52 };
53
54 struct sam9x60_div {
55 struct sam9x60_pll_core core;
56 uint8_t div;
57 uint8_t safe_div;
58 };
59
60 #define WAIT_PLL_READY_TIMEOUT(_base, _id) \
61 ({ \
62 uint32_t __timeout = 0; \
63 uint32_t _c = 0; \
64 \
65 while (__timeout++ < 1000) { \
66 _c = io_read32((_base) + AT91_PMC_PLL_ISR0) & \
67 BIT(_id); \
68 if (_c) \
69 break; \
70 wait_cycles(100); \
71 } \
72 !(_c); \
73 })
74
sam9x60_pll_ready(vaddr_t base,int id)75 static bool sam9x60_pll_ready(vaddr_t base, int id)
76 {
77 return io_read32(base + AT91_PMC_PLL_ISR0) & BIT(id);
78 }
79
sam9x60_frac_pll_ready(vaddr_t regmap,uint8_t id)80 static bool sam9x60_frac_pll_ready(vaddr_t regmap, uint8_t id)
81 {
82 return sam9x60_pll_ready(regmap, id);
83 }
84
sam9x60_frac_pll_recalc_rate(struct clk * hw,unsigned long parent_rate)85 static unsigned long sam9x60_frac_pll_recalc_rate(struct clk *hw,
86 unsigned long parent_rate)
87 {
88 struct sam9x60_frac *frac = hw->priv;
89
90 return parent_rate * (frac->mul + 1) +
91 UDIV_ROUND_NEAREST((unsigned long long)parent_rate * frac->frac,
92 1 << 22);
93 }
94
sam9x60_frac_pll_set(struct sam9x60_frac * frac)95 static TEE_Result sam9x60_frac_pll_set(struct sam9x60_frac *frac)
96 {
97 struct sam9x60_pll_core *core = &frac->core;
98 vaddr_t regmap = frac->core.base;
99 unsigned int val = 0;
100 unsigned int cfrac = 0;
101 unsigned int cmul = 0;
102
103 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
104 AT91_PMC_PLL_UPDT_ID_MASK, core->id);
105 val = io_read32(regmap + AT91_PMC_PLL_CTRL1);
106 cmul = (val & core->layout->mul_mask) >> core->layout->mul_shift;
107 cfrac = (val & core->layout->frac_mask) >> core->layout->frac_shift;
108
109 if (sam9x60_frac_pll_ready(regmap, core->id) &&
110 cmul == frac->mul && cfrac == frac->frac)
111 return TEE_SUCCESS;
112
113 /* Recommended value for PMC_PLL_ACR */
114 if (core->charac->upll)
115 val = AT91_PMC_PLL_ACR_DEFAULT_UPLL;
116 else
117 val = AT91_PMC_PLL_ACR_DEFAULT_PLLA;
118 io_write32(regmap + AT91_PMC_PLL_ACR, val);
119
120 io_write32(regmap + AT91_PMC_PLL_CTRL1,
121 SHIFT_U32(frac->mul, core->layout->mul_shift) |
122 SHIFT_U32(frac->frac, core->layout->frac_shift));
123
124 if (core->charac->upll) {
125 /* Enable the UTMI internal bandgap */
126 val |= AT91_PMC_PLL_ACR_UTMIBG;
127 io_write32(regmap + AT91_PMC_PLL_ACR, val);
128
129 udelay(10);
130
131 /* Enable the UTMI internal regulator */
132 val |= AT91_PMC_PLL_ACR_UTMIVR;
133 io_write32(regmap + AT91_PMC_PLL_ACR, val);
134
135 udelay(10);
136 }
137
138 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
139 AT91_PMC_PLL_UPDT_UPDATE | AT91_PMC_PLL_UPDT_ID_MASK,
140 AT91_PMC_PLL_UPDT_UPDATE | core->id);
141
142 io_setbits32(regmap + AT91_PMC_PLL_CTRL0,
143 AT91_PMC_PLL_CTRL0_ENLOCK | AT91_PMC_PLL_CTRL0_ENPLL);
144
145 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
146 AT91_PMC_PLL_UPDT_UPDATE | AT91_PMC_PLL_UPDT_ID_MASK,
147 AT91_PMC_PLL_UPDT_UPDATE | core->id);
148
149 if (WAIT_PLL_READY_TIMEOUT(regmap, core->id)) {
150 EMSG("PLL not ready");
151 return TEE_ERROR_BUSY;
152 }
153
154 return TEE_SUCCESS;
155 }
156
sam9x60_frac_pll_prepare(struct clk * hw)157 static TEE_Result sam9x60_frac_pll_prepare(struct clk *hw)
158 {
159 struct sam9x60_frac *frac = hw->priv;
160
161 return sam9x60_frac_pll_set(frac);
162 }
163
sam9x60_frac_pll_unprepare(struct clk * hw)164 static void sam9x60_frac_pll_unprepare(struct clk *hw)
165 {
166 struct sam9x60_frac *frac = hw->priv;
167
168 io_clrsetbits32(frac->core.base + AT91_PMC_PLL_UPDT,
169 AT91_PMC_PLL_UPDT_ID_MASK, frac->core.id);
170
171 io_clrbits32(frac->core.base + AT91_PMC_PLL_CTRL0,
172 AT91_PMC_PLL_CTRL0_ENPLL);
173
174 if (frac->core.charac->upll)
175 io_clrbits32(frac->core.base + AT91_PMC_PLL_ACR,
176 AT91_PMC_PLL_ACR_UTMIBG |
177 AT91_PMC_PLL_ACR_UTMIVR);
178
179 io_clrsetbits32(frac->core.base + AT91_PMC_PLL_UPDT,
180 AT91_PMC_PLL_UPDT_UPDATE | AT91_PMC_PLL_UPDT_ID_MASK,
181 AT91_PMC_PLL_UPDT_UPDATE | frac->core.id);
182 }
183
sam9x60_frac_pll_compute_mul_frac(struct sam9x60_frac * frac,unsigned long rate,unsigned long parent_rate,bool update)184 static TEE_Result sam9x60_frac_pll_compute_mul_frac(struct sam9x60_frac *frac,
185 unsigned long rate,
186 unsigned long parent_rate,
187 bool update)
188 {
189 unsigned long tmprate = 0;
190 unsigned long remainder = 0;
191 unsigned long nmul = 0;
192 unsigned long nfrac = 0;
193
194 if (rate < frac->core.charac->output[0].min ||
195 rate > frac->core.charac->output[0].max)
196 return TEE_ERROR_GENERIC;
197
198 /*
199 * Calculate the multiplier associated with the current
200 * divider that provide the closest rate to the requested one.
201 */
202 nmul = rate / parent_rate;
203 tmprate = parent_rate * nmul;
204 remainder = rate - tmprate;
205
206 if (remainder) {
207 nfrac = UDIV_ROUND_NEAREST((uint64_t)remainder * (1 << 22),
208 parent_rate);
209
210 tmprate += UDIV_ROUND_NEAREST((uint64_t)nfrac * parent_rate,
211 1 << 22);
212 }
213
214 /* Check if resulted rate is a valid. */
215 if (tmprate < frac->core.charac->output[0].min ||
216 tmprate > frac->core.charac->output[0].max)
217 return TEE_ERROR_GENERIC;
218
219 if (update) {
220 frac->mul = nmul - 1;
221 frac->frac = nfrac;
222 }
223
224 return TEE_SUCCESS;
225 }
226
sam9x60_frac_pll_set_rate_chg(struct clk * hw,unsigned long rate,unsigned long parent_rate)227 static TEE_Result sam9x60_frac_pll_set_rate_chg(struct clk *hw,
228 unsigned long rate,
229 unsigned long parent_rate)
230 {
231 TEE_Result ret = TEE_SUCCESS;
232 struct sam9x60_frac *frac = hw->priv;
233 struct sam9x60_pll_core *core = &frac->core;
234 vaddr_t regmap = core->base;
235
236 ret = sam9x60_frac_pll_compute_mul_frac(frac, rate, parent_rate, true);
237 if (ret == TEE_SUCCESS) {
238 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
239 AT91_PMC_PLL_UPDT_ID_MASK, core->id);
240
241 io_write32(regmap + AT91_PMC_PLL_CTRL1,
242 SHIFT_U32(frac->mul, core->layout->mul_shift) |
243 SHIFT_U32(frac->frac, core->layout->frac_shift));
244
245 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
246 AT91_PMC_PLL_UPDT_UPDATE |
247 AT91_PMC_PLL_UPDT_ID_MASK,
248 AT91_PMC_PLL_UPDT_UPDATE | core->id);
249
250 io_setbits32(regmap + AT91_PMC_PLL_CTRL0,
251 AT91_PMC_PLL_CTRL0_ENLOCK |
252 AT91_PMC_PLL_CTRL0_ENPLL);
253
254 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
255 AT91_PMC_PLL_UPDT_UPDATE |
256 AT91_PMC_PLL_UPDT_ID_MASK,
257 AT91_PMC_PLL_UPDT_UPDATE | core->id);
258
259 if (WAIT_PLL_READY_TIMEOUT(regmap, core->id)) {
260 EMSG("PLL not ready");
261 return TEE_ERROR_BUSY;
262 }
263 }
264
265 return ret;
266 }
267
268 static const struct clk_ops sam9x60_frac_pll_ops_chg = {
269 .enable = sam9x60_frac_pll_prepare,
270 .disable = sam9x60_frac_pll_unprepare,
271 .get_rate = sam9x60_frac_pll_recalc_rate,
272 .set_rate = sam9x60_frac_pll_set_rate_chg,
273 };
274
sam9x60_div_pll_set_div(struct sam9x60_pll_core * core,uint32_t div,bool enable)275 static TEE_Result sam9x60_div_pll_set_div(struct sam9x60_pll_core *core,
276 uint32_t div,
277 bool enable)
278 {
279 vaddr_t regmap = core->base;
280 uint32_t enable_mask = enable ? core->layout->endiv_mask : 0;
281 uint32_t ena_val = enable ? BIT(core->layout->endiv_shift) : 0;
282
283 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
284 AT91_PMC_PLL_UPDT_ID_MASK, core->id);
285
286 io_clrsetbits32(regmap + AT91_PMC_PLL_CTRL0,
287 core->layout->div_mask | enable_mask,
288 SHIFT_U32(div, core->layout->div_shift) | ena_val);
289
290 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
291 AT91_PMC_PLL_UPDT_UPDATE | AT91_PMC_PLL_UPDT_ID_MASK,
292 AT91_PMC_PLL_UPDT_UPDATE | core->id);
293
294 if (WAIT_PLL_READY_TIMEOUT(regmap, core->id)) {
295 EMSG("PLL not ready");
296 return TEE_ERROR_BUSY;
297 }
298
299 return TEE_SUCCESS;
300 }
301
sam9x60_div_pll_set(struct sam9x60_div * div)302 static TEE_Result sam9x60_div_pll_set(struct sam9x60_div *div)
303 {
304 struct sam9x60_pll_core *core = &div->core;
305 vaddr_t regmap = core->base;
306 unsigned int val = 0;
307 unsigned int cdiv = 0;
308
309 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
310 AT91_PMC_PLL_UPDT_ID_MASK, core->id);
311 val = io_read32(regmap + AT91_PMC_PLL_CTRL0);
312 cdiv = (val & core->layout->div_mask) >> core->layout->div_shift;
313
314 /* Stop if enabled an nothing changed. */
315 if ((val & core->layout->endiv_mask) && cdiv == div->div)
316 return TEE_SUCCESS;
317
318 return sam9x60_div_pll_set_div(core, div->div, 1);
319 }
320
sam9x60_div_pll_prepare(struct clk * hw)321 static TEE_Result sam9x60_div_pll_prepare(struct clk *hw)
322 {
323 struct sam9x60_div *div = hw->priv;
324
325 return sam9x60_div_pll_set(div);
326 }
327
sam9x60_div_pll_unprepare(struct clk * hw)328 static void sam9x60_div_pll_unprepare(struct clk *hw)
329 {
330 struct sam9x60_div *div = hw->priv;
331 struct sam9x60_pll_core *core = &div->core;
332 vaddr_t regmap = core->base;
333
334 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
335 AT91_PMC_PLL_UPDT_ID_MASK, core->id);
336
337 io_clrbits32(regmap + AT91_PMC_PLL_CTRL0, core->layout->endiv_mask);
338
339 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
340 AT91_PMC_PLL_UPDT_UPDATE | AT91_PMC_PLL_UPDT_ID_MASK,
341 AT91_PMC_PLL_UPDT_UPDATE | core->id);
342 }
343
sam9x60_div_pll_recalc_rate(struct clk * hw,unsigned long parent_rate)344 static unsigned long sam9x60_div_pll_recalc_rate(struct clk *hw,
345 unsigned long parent_rate)
346 {
347 struct sam9x60_div *div = hw->priv;
348
349 return UDIV_ROUND_NEAREST(parent_rate, div->div + 1);
350 }
351
sam9x60_div_pll_set_rate(struct clk * hw,unsigned long rate,unsigned long parent_rate)352 static TEE_Result sam9x60_div_pll_set_rate(struct clk *hw,
353 unsigned long rate,
354 unsigned long parent_rate)
355 {
356 struct sam9x60_div *div = hw->priv;
357
358 if (parent_rate > rate)
359 div->div = UDIV_ROUND_NEAREST(parent_rate, rate) - 1;
360 else
361 div->div = 0;
362
363 return TEE_SUCCESS;
364 }
365
sam9x60_div_pll_set_rate_chg(struct clk * hw,unsigned long rate,unsigned long parent_rate)366 static TEE_Result sam9x60_div_pll_set_rate_chg(struct clk *hw,
367 unsigned long rate,
368 unsigned long parent_rate)
369 {
370 struct sam9x60_div *div = hw->priv;
371 struct sam9x60_pll_core *core = &div->core;
372 vaddr_t regmap = core->base;
373 unsigned int val = 0;
374 unsigned int cdiv = 0;
375
376 if (parent_rate > rate)
377 div->div = UDIV_ROUND_NEAREST(parent_rate, rate) - 1;
378 else
379 div->div = 0;
380
381 io_clrsetbits32(regmap + AT91_PMC_PLL_UPDT,
382 AT91_PMC_PLL_UPDT_ID_MASK,
383 core->id);
384 val = io_read32(regmap + AT91_PMC_PLL_CTRL0);
385 cdiv = (val & core->layout->div_mask) >> core->layout->div_shift;
386
387 /* Stop if nothing changed. */
388 if (cdiv == div->div)
389 return TEE_SUCCESS;
390
391 return sam9x60_div_pll_set_div(core, div->div, 0);
392 }
393
394 static const struct clk_ops sam9x60_div_pll_ops = {
395 .enable = sam9x60_div_pll_prepare,
396 .disable = sam9x60_div_pll_unprepare,
397 .set_rate = sam9x60_div_pll_set_rate,
398 .get_rate = sam9x60_div_pll_recalc_rate,
399 };
400
401 static const struct clk_ops sam9x60_div_pll_ops_chg = {
402 .enable = sam9x60_div_pll_prepare,
403 .disable = sam9x60_div_pll_unprepare,
404 .set_rate = sam9x60_div_pll_set_rate_chg,
405 .get_rate = sam9x60_div_pll_recalc_rate,
406 };
407
sam9x60_clk_register_frac_pll(struct pmc_data * pmc,const char * name,struct clk * parent,uint8_t id,const struct clk_pll_charac * charac,const struct clk_pll_layout * layout,uint32_t flags)408 struct clk *sam9x60_clk_register_frac_pll(struct pmc_data *pmc,
409 const char *name,
410 struct clk *parent,
411 uint8_t id,
412 const struct clk_pll_charac *charac,
413 const struct clk_pll_layout *layout,
414 uint32_t flags)
415 {
416 struct sam9x60_frac *frac = NULL;
417 struct clk *hw = NULL;
418 unsigned long parent_rate = 0;
419 unsigned int val = 0;
420 TEE_Result ret = TEE_SUCCESS;
421
422 frac = calloc(1, sizeof(*frac));
423 if (!frac)
424 return NULL;
425
426 hw = clk_alloc(name, &sam9x60_frac_pll_ops_chg, &parent, 1);
427 if (!hw) {
428 free(frac);
429 return NULL;
430 }
431
432 hw->priv = frac;
433 hw->flags = flags;
434 frac->core.id = id;
435 frac->core.charac = charac;
436 frac->core.layout = layout;
437 frac->core.base = pmc->base;
438
439 if (sam9x60_pll_ready(pmc->base, id)) {
440 io_clrsetbits32(frac->core.base + AT91_PMC_PLL_UPDT,
441 AT91_PMC_PLL_UPDT_ID_MASK, id);
442 val = io_read32(pmc->base + AT91_PMC_PLL_CTRL1);
443 frac->mul = (val >> PMC_PLL_CTRL1_MUL_POS) &
444 PMC_PLL_CTRL1_MUL_MASK;
445 frac->frac = (val >> PMC_PLL_CTRL1_FRACR_POS) &
446 PMC_PLL_CTRL1_FRACR_MASK;
447 } else {
448 /*
449 * This means the PLL is not setup by bootloaders. In this
450 * case we need to set the minimum rate for it. Otherwise
451 * a clock child of this PLL may be enabled before setting
452 * its rate leading to enabling this PLL with unsupported
453 * rate. This will lead to PLL not being locked at all.
454 */
455 parent_rate = clk_get_rate(parent);
456 if (!parent_rate) {
457 clk_free(hw);
458 free(frac);
459 return NULL;
460 }
461
462 ret = sam9x60_frac_pll_compute_mul_frac(frac,
463 charac->output[0].min,
464 parent_rate, true);
465 if (ret != TEE_SUCCESS) {
466 clk_free(hw);
467 free(frac);
468 return NULL;
469 }
470 }
471
472 frac->core.hw = hw;
473 if (clk_register(hw)) {
474 clk_free(hw);
475 free(frac);
476 return NULL;
477 }
478
479 return hw;
480 }
481
sam9x60_clk_register_div_pll(struct pmc_data * pmc,const char * name,struct clk * parent,uint8_t id,const struct clk_pll_charac * charac,const struct clk_pll_layout * layout,uint32_t flags,uint32_t safe_div)482 struct clk *sam9x60_clk_register_div_pll(struct pmc_data *pmc,
483 const char *name,
484 struct clk *parent,
485 uint8_t id,
486 const struct clk_pll_charac *charac,
487 const struct clk_pll_layout *layout,
488 uint32_t flags,
489 uint32_t safe_div)
490 {
491 struct sam9x60_div *div = NULL;
492 struct clk *hw = NULL;
493 unsigned int val = 0;
494
495 if (safe_div >= PLL_DIV_MAX)
496 safe_div = PLL_DIV_MAX - 1;
497
498 div = calloc(1, sizeof(*div));
499 if (!div)
500 return NULL;
501
502 if (flags & CLK_SET_RATE_GATE)
503 hw = clk_alloc(name, &sam9x60_div_pll_ops, &parent, 1);
504 else
505 hw = clk_alloc(name, &sam9x60_div_pll_ops_chg, &parent, 1);
506 if (!hw) {
507 free(div);
508 return NULL;
509 }
510
511 hw->priv = div;
512 hw->flags = flags;
513 div->core.id = id;
514 div->core.charac = charac;
515 div->core.layout = layout;
516 div->core.base = pmc->base;
517 div->safe_div = safe_div;
518
519 io_clrsetbits32(pmc->base + AT91_PMC_PLL_UPDT,
520 AT91_PMC_PLL_UPDT_ID_MASK, id);
521 val = io_read32(pmc->base + AT91_PMC_PLL_CTRL0);
522 div->div = (val >> PMC_PLL_CTRL0_DIV_POS) & PMC_PLL_CTRL0_DIV_MASK;
523
524 div->core.hw = hw;
525 if (clk_register(hw)) {
526 clk_free(hw);
527 free(div);
528 return NULL;
529 }
530
531 return hw;
532 }
533