1 /*
2 * Copyright 2024-2025 NXP
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6 #include <errno.h>
7 #include <common/debug.h>
8 #include <drivers/clk.h>
9 #include <lib/mmio.h>
10 #include <lib/xlat_tables/xlat_tables_v2.h>
11 #include <s32cc-clk-ids.h>
12 #include <s32cc-clk-modules.h>
13 #include <s32cc-clk-regs.h>
14 #include <s32cc-clk-utils.h>
15 #include <s32cc-mc-me.h>
16
17 #define MAX_STACK_DEPTH (40U)
18
19 /* This is used for floating-point precision calculations. */
20 #define FP_PRECISION (100000000UL)
21
22 struct s32cc_clk_drv {
23 uintptr_t fxosc_base;
24 uintptr_t armpll_base;
25 uintptr_t periphpll_base;
26 uintptr_t armdfs_base;
27 uintptr_t periphdfs_base;
28 uintptr_t cgm0_base;
29 uintptr_t cgm1_base;
30 uintptr_t cgm5_base;
31 uintptr_t ddrpll_base;
32 uintptr_t mc_me;
33 uintptr_t mc_rgm;
34 uintptr_t rdc;
35 };
36
37 static int set_module_rate(const struct s32cc_clk_obj *module,
38 unsigned long rate, unsigned long *orate,
39 unsigned int *depth);
40 static int get_module_rate(const struct s32cc_clk_obj *module,
41 const struct s32cc_clk_drv *drv,
42 unsigned long *rate,
43 unsigned int depth);
44
update_stack_depth(unsigned int * depth)45 static int update_stack_depth(unsigned int *depth)
46 {
47 if (*depth == 0U) {
48 return -ENOMEM;
49 }
50
51 (*depth)--;
52 return 0;
53 }
54
get_drv(void)55 static struct s32cc_clk_drv *get_drv(void)
56 {
57 static struct s32cc_clk_drv driver = {
58 .fxosc_base = FXOSC_BASE_ADDR,
59 .armpll_base = ARMPLL_BASE_ADDR,
60 .periphpll_base = PERIPHPLL_BASE_ADDR,
61 .armdfs_base = ARM_DFS_BASE_ADDR,
62 .periphdfs_base = PERIPH_DFS_BASE_ADDR,
63 .cgm0_base = CGM0_BASE_ADDR,
64 .cgm1_base = CGM1_BASE_ADDR,
65 .cgm5_base = MC_CGM5_BASE_ADDR,
66 .ddrpll_base = DDRPLL_BASE_ADDR,
67 .mc_me = MC_ME_BASE_ADDR,
68 .mc_rgm = MC_RGM_BASE_ADDR,
69 .rdc = RDC_BASE_ADDR,
70 };
71
72 return &driver;
73 }
74
75 static int enable_module(struct s32cc_clk_obj *module,
76 const struct s32cc_clk_drv *drv,
77 unsigned int depth);
78
get_clk_parent(const struct s32cc_clk_obj * module)79 static struct s32cc_clk_obj *get_clk_parent(const struct s32cc_clk_obj *module)
80 {
81 const struct s32cc_clk *clk = s32cc_obj2clk(module);
82
83 if (clk->module != NULL) {
84 return clk->module;
85 }
86
87 if (clk->pclock != NULL) {
88 return &clk->pclock->desc;
89 }
90
91 return NULL;
92 }
93
get_base_addr(enum s32cc_clk_source id,const struct s32cc_clk_drv * drv,uintptr_t * base)94 static int get_base_addr(enum s32cc_clk_source id, const struct s32cc_clk_drv *drv,
95 uintptr_t *base)
96 {
97 int ret = 0;
98
99 switch (id) {
100 case S32CC_FXOSC:
101 *base = drv->fxosc_base;
102 break;
103 case S32CC_ARM_PLL:
104 *base = drv->armpll_base;
105 break;
106 case S32CC_PERIPH_PLL:
107 *base = drv->periphpll_base;
108 break;
109 case S32CC_DDR_PLL:
110 *base = drv->ddrpll_base;
111 break;
112 case S32CC_ARM_DFS:
113 *base = drv->armdfs_base;
114 break;
115 case S32CC_PERIPH_DFS:
116 *base = drv->periphdfs_base;
117 break;
118 case S32CC_CGM0:
119 *base = drv->cgm0_base;
120 break;
121 case S32CC_CGM1:
122 *base = drv->cgm1_base;
123 break;
124 case S32CC_CGM5:
125 *base = drv->cgm5_base;
126 break;
127 case S32CC_FIRC:
128 break;
129 case S32CC_SIRC:
130 break;
131 default:
132 ret = -EINVAL;
133 break;
134 }
135
136 if (ret != 0) {
137 ERROR("Unknown clock source id: %u\n", id);
138 }
139
140 return ret;
141 }
142
enable_fxosc(const struct s32cc_clk_drv * drv)143 static void enable_fxosc(const struct s32cc_clk_drv *drv)
144 {
145 uintptr_t fxosc_base = drv->fxosc_base;
146 uint32_t ctrl;
147
148 ctrl = mmio_read_32(FXOSC_CTRL(fxosc_base));
149 if ((ctrl & FXOSC_CTRL_OSCON) != U(0)) {
150 return;
151 }
152
153 ctrl = FXOSC_CTRL_COMP_EN;
154 ctrl &= ~FXOSC_CTRL_OSC_BYP;
155 ctrl |= FXOSC_CTRL_EOCV(0x1);
156 ctrl |= FXOSC_CTRL_GM_SEL(0x7);
157 mmio_write_32(FXOSC_CTRL(fxosc_base), ctrl);
158
159 /* Switch ON the crystal oscillator. */
160 mmio_setbits_32(FXOSC_CTRL(fxosc_base), FXOSC_CTRL_OSCON);
161
162 /* Wait until the clock is stable. */
163 while ((mmio_read_32(FXOSC_STAT(fxosc_base)) & FXOSC_STAT_OSC_STAT) == U(0)) {
164 }
165 }
166
enable_osc(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)167 static int enable_osc(struct s32cc_clk_obj *module,
168 const struct s32cc_clk_drv *drv,
169 unsigned int depth)
170 {
171 const struct s32cc_osc *osc = s32cc_obj2osc(module);
172 unsigned int ldepth = depth;
173 int ret = 0;
174
175 ret = update_stack_depth(&ldepth);
176 if (ret != 0) {
177 return ret;
178 }
179
180 switch (osc->source) {
181 case S32CC_FXOSC:
182 enable_fxosc(drv);
183 break;
184 /* FIRC and SIRC oscillators are enabled by default */
185 case S32CC_FIRC:
186 break;
187 case S32CC_SIRC:
188 break;
189 default:
190 ERROR("Invalid oscillator %d\n", osc->source);
191 ret = -EINVAL;
192 break;
193 };
194
195 return ret;
196 }
197
get_pll_parent(const struct s32cc_clk_obj * module)198 static struct s32cc_clk_obj *get_pll_parent(const struct s32cc_clk_obj *module)
199 {
200 const struct s32cc_pll *pll = s32cc_obj2pll(module);
201
202 if (pll->source == NULL) {
203 ERROR("Failed to identify PLL's parent\n");
204 }
205
206 return pll->source;
207 }
208
get_pll_mfi_mfn(unsigned long pll_vco,unsigned long ref_freq,uint32_t * mfi,uint32_t * mfn)209 static int get_pll_mfi_mfn(unsigned long pll_vco, unsigned long ref_freq,
210 uint32_t *mfi, uint32_t *mfn)
211
212 {
213 unsigned long vco;
214 unsigned long mfn64;
215
216 /* FRAC-N mode */
217 *mfi = (uint32_t)(pll_vco / ref_freq);
218
219 /* MFN formula : (double)(pll_vco % ref_freq) / ref_freq * 18432.0 */
220 mfn64 = pll_vco % ref_freq;
221 mfn64 *= FP_PRECISION;
222 mfn64 /= ref_freq;
223 mfn64 *= 18432UL;
224 mfn64 /= FP_PRECISION;
225
226 if (mfn64 > UINT32_MAX) {
227 return -EINVAL;
228 }
229
230 *mfn = (uint32_t)mfn64;
231
232 vco = ((unsigned long)*mfn * FP_PRECISION) / 18432UL;
233 vco += (unsigned long)*mfi * FP_PRECISION;
234 vco *= ref_freq;
235 vco /= FP_PRECISION;
236
237 if (vco != pll_vco) {
238 ERROR("Failed to find MFI and MFN settings for PLL freq %lu. Nearest freq = %lu\n",
239 pll_vco, vco);
240 return -EINVAL;
241 }
242
243 return 0;
244 }
245
get_pll_mux(const struct s32cc_pll * pll)246 static struct s32cc_clkmux *get_pll_mux(const struct s32cc_pll *pll)
247 {
248 const struct s32cc_clk_obj *source = pll->source;
249 const struct s32cc_clk *clk;
250
251 if (source == NULL) {
252 ERROR("Failed to identify PLL's parent\n");
253 return NULL;
254 }
255
256 if (source->type != s32cc_clk_t) {
257 ERROR("The parent of the PLL isn't a clock\n");
258 return NULL;
259 }
260
261 clk = s32cc_obj2clk(source);
262
263 if (clk->module == NULL) {
264 ERROR("The clock isn't connected to a module\n");
265 return NULL;
266 }
267
268 source = clk->module;
269
270 if ((source->type != s32cc_clkmux_t) &&
271 (source->type != s32cc_shared_clkmux_t)) {
272 ERROR("The parent of the PLL isn't a MUX\n");
273 return NULL;
274 }
275
276 return s32cc_obj2clkmux(source);
277 }
278
disable_odiv(uintptr_t pll_addr,uint32_t div_index)279 static void disable_odiv(uintptr_t pll_addr, uint32_t div_index)
280 {
281 mmio_clrbits_32(PLLDIG_PLLODIV(pll_addr, div_index), PLLDIG_PLLODIV_DE);
282 }
283
enable_odiv(uintptr_t pll_addr,uint32_t div_index)284 static void enable_odiv(uintptr_t pll_addr, uint32_t div_index)
285 {
286 mmio_setbits_32(PLLDIG_PLLODIV(pll_addr, div_index), PLLDIG_PLLODIV_DE);
287 }
288
enable_odivs(uintptr_t pll_addr,uint32_t ndivs,uint32_t mask)289 static void enable_odivs(uintptr_t pll_addr, uint32_t ndivs, uint32_t mask)
290 {
291 uint32_t i;
292
293 for (i = 0; i < ndivs; i++) {
294 if ((mask & BIT_32(i)) != 0U) {
295 enable_odiv(pll_addr, i);
296 }
297 }
298 }
299
adjust_odiv_settings(const struct s32cc_pll * pll,uintptr_t pll_addr,uint32_t odivs_mask,unsigned long old_vco)300 static int adjust_odiv_settings(const struct s32cc_pll *pll, uintptr_t pll_addr,
301 uint32_t odivs_mask, unsigned long old_vco)
302 {
303 uint64_t old_odiv_freq, odiv_freq;
304 uint32_t i, pllodiv, pdiv;
305 int ret = 0;
306
307 if (old_vco == 0UL) {
308 return 0;
309 }
310
311 for (i = 0; i < pll->ndividers; i++) {
312 if ((odivs_mask & BIT_32(i)) == 0U) {
313 continue;
314 }
315
316 pllodiv = mmio_read_32(PLLDIG_PLLODIV(pll_addr, i));
317
318 pdiv = PLLDIG_PLLODIV_DIV(pllodiv);
319
320 old_odiv_freq = ((old_vco * FP_PRECISION) / (pdiv + 1U)) / FP_PRECISION;
321 pdiv = (uint32_t)(pll->vco_freq * FP_PRECISION / old_odiv_freq / FP_PRECISION);
322
323 odiv_freq = pll->vco_freq * FP_PRECISION / pdiv / FP_PRECISION;
324
325 if (old_odiv_freq != odiv_freq) {
326 ERROR("Failed to adjust ODIV %" PRIu32 " to match previous frequency\n",
327 i);
328 }
329
330 pllodiv = PLLDIG_PLLODIV_DIV_SET(pdiv - 1U);
331 mmio_write_32(PLLDIG_PLLODIV(pll_addr, i), pllodiv);
332 }
333
334 return ret;
335 }
336
get_enabled_odivs(uintptr_t pll_addr,uint32_t ndivs)337 static uint32_t get_enabled_odivs(uintptr_t pll_addr, uint32_t ndivs)
338 {
339 uint32_t mask = 0;
340 uint32_t pllodiv;
341 uint32_t i;
342
343 for (i = 0; i < ndivs; i++) {
344 pllodiv = mmio_read_32(PLLDIG_PLLODIV(pll_addr, i));
345 if ((pllodiv & PLLDIG_PLLODIV_DE) != 0U) {
346 mask |= BIT_32(i);
347 }
348 }
349
350 return mask;
351 }
352
disable_odivs(uintptr_t pll_addr,uint32_t ndivs)353 static void disable_odivs(uintptr_t pll_addr, uint32_t ndivs)
354 {
355 uint32_t i;
356
357 for (i = 0; i < ndivs; i++) {
358 disable_odiv(pll_addr, i);
359 }
360 }
361
enable_pll_hw(uintptr_t pll_addr)362 static void enable_pll_hw(uintptr_t pll_addr)
363 {
364 /* Enable the PLL. */
365 mmio_write_32(PLLDIG_PLLCR(pll_addr), 0x0);
366
367 /* Poll until PLL acquires lock. */
368 while ((mmio_read_32(PLLDIG_PLLSR(pll_addr)) & PLLDIG_PLLSR_LOCK) == 0U) {
369 }
370 }
371
disable_pll_hw(uintptr_t pll_addr)372 static void disable_pll_hw(uintptr_t pll_addr)
373 {
374 mmio_write_32(PLLDIG_PLLCR(pll_addr), PLLDIG_PLLCR_PLLPD);
375 }
376
is_pll_enabled(uintptr_t pll_base)377 static bool is_pll_enabled(uintptr_t pll_base)
378 {
379 uint32_t pllcr, pllsr;
380
381 pllcr = mmio_read_32(PLLDIG_PLLCR(pll_base));
382 pllsr = mmio_read_32(PLLDIG_PLLSR(pll_base));
383
384 /* Enabled and locked PLL */
385 if ((pllcr & PLLDIG_PLLCR_PLLPD) != 0U) {
386 return false;
387 }
388
389 if ((pllsr & PLLDIG_PLLSR_LOCK) == 0U) {
390 return false;
391 }
392
393 return true;
394 }
395
program_pll(const struct s32cc_pll * pll,uintptr_t pll_addr,const struct s32cc_clk_drv * drv,uint32_t sclk_id,unsigned long sclk_freq,unsigned int depth)396 static int program_pll(const struct s32cc_pll *pll, uintptr_t pll_addr,
397 const struct s32cc_clk_drv *drv, uint32_t sclk_id,
398 unsigned long sclk_freq, unsigned int depth)
399 {
400 uint32_t rdiv = 1, mfi, mfn;
401 unsigned long old_vco = 0UL;
402 unsigned int ldepth = depth;
403 uint32_t odivs_mask;
404 int ret;
405
406 ret = update_stack_depth(&ldepth);
407 if (ret != 0) {
408 return ret;
409 }
410
411 ret = get_pll_mfi_mfn(pll->vco_freq, sclk_freq, &mfi, &mfn);
412 if (ret != 0) {
413 return -EINVAL;
414 }
415
416 odivs_mask = get_enabled_odivs(pll_addr, pll->ndividers);
417
418 if (is_pll_enabled(pll_addr)) {
419 ret = get_module_rate(&pll->desc, drv, &old_vco, ldepth);
420 if (ret != 0) {
421 return ret;
422 }
423 }
424
425 /* Disable ODIVs*/
426 disable_odivs(pll_addr, pll->ndividers);
427
428 /* Disable PLL */
429 disable_pll_hw(pll_addr);
430
431 /* Program PLLCLKMUX */
432 mmio_write_32(PLLDIG_PLLCLKMUX(pll_addr), sclk_id);
433
434 /* Program VCO */
435 mmio_clrsetbits_32(PLLDIG_PLLDV(pll_addr),
436 PLLDIG_PLLDV_RDIV_MASK | PLLDIG_PLLDV_MFI_MASK,
437 PLLDIG_PLLDV_RDIV_SET(rdiv) | PLLDIG_PLLDV_MFI(mfi));
438
439 mmio_write_32(PLLDIG_PLLFD(pll_addr),
440 PLLDIG_PLLFD_MFN_SET(mfn) | PLLDIG_PLLFD_SMDEN);
441
442 ret = adjust_odiv_settings(pll, pll_addr, odivs_mask, old_vco);
443 if (ret != 0) {
444 return ret;
445 }
446
447 enable_pll_hw(pll_addr);
448
449 /* Enable out dividers */
450 enable_odivs(pll_addr, pll->ndividers, odivs_mask);
451
452 return ret;
453 }
454
enable_pll(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)455 static int enable_pll(struct s32cc_clk_obj *module,
456 const struct s32cc_clk_drv *drv,
457 unsigned int depth)
458 {
459 const struct s32cc_pll *pll = s32cc_obj2pll(module);
460 unsigned int clk_src, ldepth = depth;
461 unsigned long sclk_freq, pll_vco;
462 const struct s32cc_clkmux *mux;
463 uintptr_t pll_addr = UL(0x0);
464 bool pll_enabled;
465 uint32_t sclk_id;
466 int ret;
467
468 ret = update_stack_depth(&ldepth);
469 if (ret != 0) {
470 return ret;
471 }
472
473 mux = get_pll_mux(pll);
474 if (mux == NULL) {
475 return -EINVAL;
476 }
477
478 if (pll->instance != mux->module) {
479 ERROR("MUX type is not in sync with PLL ID\n");
480 return -EINVAL;
481 }
482
483 ret = get_base_addr(pll->instance, drv, &pll_addr);
484 if (ret != 0) {
485 ERROR("Failed to detect PLL instance\n");
486 return ret;
487 }
488
489 switch (mux->source_id) {
490 case S32CC_CLK_FIRC:
491 sclk_freq = 48U * MHZ;
492 sclk_id = 0;
493 break;
494 case S32CC_CLK_FXOSC:
495 sclk_freq = 40U * MHZ;
496 sclk_id = 1;
497 break;
498 default:
499 ERROR("Invalid source selection for PLL 0x%lx\n",
500 pll_addr);
501 return -EINVAL;
502 };
503
504 ret = get_module_rate(&pll->desc, drv, &pll_vco, depth);
505 if (ret != 0) {
506 return ret;
507 }
508
509 pll_enabled = is_pll_enabled(pll_addr);
510 clk_src = mmio_read_32(PLLDIG_PLLCLKMUX(pll_addr));
511
512 if ((clk_src == sclk_id) && pll_enabled &&
513 (pll_vco == pll->vco_freq)) {
514 return 0;
515 }
516
517 return program_pll(pll, pll_addr, drv, sclk_id, sclk_freq, ldepth);
518 }
519
get_div_pll(const struct s32cc_pll_out_div * pdiv)520 static inline struct s32cc_pll *get_div_pll(const struct s32cc_pll_out_div *pdiv)
521 {
522 const struct s32cc_clk_obj *parent;
523
524 parent = pdiv->parent;
525 if (parent == NULL) {
526 ERROR("Failed to identify PLL divider's parent\n");
527 return NULL;
528 }
529
530 if (parent->type != s32cc_pll_t) {
531 ERROR("The parent of the divider is not a PLL instance\n");
532 return NULL;
533 }
534
535 return s32cc_obj2pll(parent);
536 }
537
config_pll_out_div(uintptr_t pll_addr,uint32_t div_index,uint32_t dc)538 static void config_pll_out_div(uintptr_t pll_addr, uint32_t div_index, uint32_t dc)
539 {
540 uint32_t pllodiv;
541 uint32_t pdiv;
542
543 pllodiv = mmio_read_32(PLLDIG_PLLODIV(pll_addr, div_index));
544 pdiv = PLLDIG_PLLODIV_DIV(pllodiv);
545
546 if (((pdiv + 1U) == dc) && ((pllodiv & PLLDIG_PLLODIV_DE) != 0U)) {
547 return;
548 }
549
550 if ((pllodiv & PLLDIG_PLLODIV_DE) != 0U) {
551 disable_odiv(pll_addr, div_index);
552 }
553
554 pllodiv = PLLDIG_PLLODIV_DIV_SET(dc - 1U);
555 mmio_write_32(PLLDIG_PLLODIV(pll_addr, div_index), pllodiv);
556
557 enable_odiv(pll_addr, div_index);
558 }
559
get_pll_div_parent(const struct s32cc_clk_obj * module)560 static struct s32cc_clk_obj *get_pll_div_parent(const struct s32cc_clk_obj *module)
561 {
562 const struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module);
563
564 if (pdiv->parent == NULL) {
565 ERROR("Failed to identify PLL DIV's parent\n");
566 }
567
568 return pdiv->parent;
569 }
570
enable_pll_div(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)571 static int enable_pll_div(struct s32cc_clk_obj *module,
572 const struct s32cc_clk_drv *drv,
573 unsigned int depth)
574 {
575 const struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module);
576 uintptr_t pll_addr = 0x0ULL;
577 unsigned int ldepth = depth;
578 const struct s32cc_pll *pll;
579 unsigned long pll_vco;
580 uint32_t dc;
581 int ret;
582
583 ret = update_stack_depth(&ldepth);
584 if (ret != 0) {
585 return ret;
586 }
587
588 pll = get_div_pll(pdiv);
589 if (pll == NULL) {
590 ERROR("The parent of the PLL DIV is invalid\n");
591 return 0;
592 }
593
594 ret = get_base_addr(pll->instance, drv, &pll_addr);
595 if (ret != 0) {
596 ERROR("Failed to detect PLL instance\n");
597 return -EINVAL;
598 }
599
600 ret = get_module_rate(&pll->desc, drv, &pll_vco, ldepth);
601 if (ret != 0) {
602 ERROR("Failed to enable the PLL due to unknown rate for 0x%" PRIxPTR "\n",
603 pll_addr);
604 return ret;
605 }
606
607 dc = (uint32_t)(pll_vco / pdiv->freq);
608
609 config_pll_out_div(pll_addr, pdiv->index, dc);
610
611 return 0;
612 }
613
cgm_mux_clk_config(uintptr_t cgm_addr,uint32_t mux,uint32_t source,bool safe_clk)614 static int cgm_mux_clk_config(uintptr_t cgm_addr, uint32_t mux, uint32_t source,
615 bool safe_clk)
616 {
617 uint32_t css, csc;
618
619 css = mmio_read_32(CGM_MUXn_CSS(cgm_addr, mux));
620
621 /* Already configured */
622 if ((MC_CGM_MUXn_CSS_SELSTAT(css) == source) &&
623 (MC_CGM_MUXn_CSS_SWTRG(css) == MC_CGM_MUXn_CSS_SWTRG_SUCCESS) &&
624 ((css & MC_CGM_MUXn_CSS_SWIP) == 0U) && !safe_clk) {
625 return 0;
626 }
627
628 /* Ongoing clock switch? */
629 while ((mmio_read_32(CGM_MUXn_CSS(cgm_addr, mux)) &
630 MC_CGM_MUXn_CSS_SWIP) != 0U) {
631 }
632
633 csc = mmio_read_32(CGM_MUXn_CSC(cgm_addr, mux));
634
635 /* Clear previous source. */
636 csc &= ~(MC_CGM_MUXn_CSC_SELCTL_MASK);
637
638 if (!safe_clk) {
639 /* Select the clock source and trigger the clock switch. */
640 csc |= MC_CGM_MUXn_CSC_SELCTL(source) | MC_CGM_MUXn_CSC_CLK_SW;
641 } else {
642 /* Switch to safe clock */
643 csc |= MC_CGM_MUXn_CSC_SAFE_SW;
644 }
645
646 mmio_write_32(CGM_MUXn_CSC(cgm_addr, mux), csc);
647
648 /* Wait for configuration bit to auto-clear. */
649 while ((mmio_read_32(CGM_MUXn_CSC(cgm_addr, mux)) &
650 MC_CGM_MUXn_CSC_CLK_SW) != 0U) {
651 }
652
653 /* Is the clock switch completed? */
654 while ((mmio_read_32(CGM_MUXn_CSS(cgm_addr, mux)) &
655 MC_CGM_MUXn_CSS_SWIP) != 0U) {
656 }
657
658 /*
659 * Check if the switch succeeded.
660 * Check switch trigger cause and the source.
661 */
662 css = mmio_read_32(CGM_MUXn_CSS(cgm_addr, mux));
663 if (!safe_clk) {
664 if ((MC_CGM_MUXn_CSS_SWTRG(css) == MC_CGM_MUXn_CSS_SWTRG_SUCCESS) &&
665 (MC_CGM_MUXn_CSS_SELSTAT(css) == source)) {
666 return 0;
667 }
668
669 ERROR("Failed to change the source of mux %" PRIu32 " to %" PRIu32 " (CGM=%lu)\n",
670 mux, source, cgm_addr);
671 } else {
672 if (((MC_CGM_MUXn_CSS_SWTRG(css) == MC_CGM_MUXn_CSS_SWTRG_SAFE_CLK) ||
673 (MC_CGM_MUXn_CSS_SWTRG(css) == MC_CGM_MUXn_CSS_SWTRG_SAFE_CLK_INACTIVE)) &&
674 ((MC_CGM_MUXn_CSS_SAFE_SW & css) != 0U)) {
675 return 0;
676 }
677
678 ERROR("The switch of mux %" PRIu32 " (CGM=%lu) to safe clock failed\n",
679 mux, cgm_addr);
680 }
681
682 return -EINVAL;
683 }
684
enable_cgm_mux(const struct s32cc_clkmux * mux,const struct s32cc_clk_drv * drv)685 static int enable_cgm_mux(const struct s32cc_clkmux *mux,
686 const struct s32cc_clk_drv *drv)
687 {
688 uintptr_t cgm_addr = UL(0x0);
689 uint32_t mux_hw_clk;
690 int ret;
691
692 ret = get_base_addr(mux->module, drv, &cgm_addr);
693 if (ret != 0) {
694 return ret;
695 }
696
697 mux_hw_clk = (uint32_t)S32CC_CLK_ID(mux->source_id);
698
699 return cgm_mux_clk_config(cgm_addr, mux->index,
700 mux_hw_clk, false);
701 }
702
get_mux_parent(const struct s32cc_clk_obj * module)703 static struct s32cc_clk_obj *get_mux_parent(const struct s32cc_clk_obj *module)
704 {
705 const struct s32cc_clkmux *mux = s32cc_obj2clkmux(module);
706 struct s32cc_clk *clk;
707
708 if (mux == NULL) {
709 return NULL;
710 }
711
712 clk = s32cc_get_arch_clk(mux->source_id);
713 if (clk == NULL) {
714 ERROR("Invalid parent (%lu) for mux %" PRIu8 "\n",
715 mux->source_id, mux->index);
716 return NULL;
717 }
718
719 return &clk->desc;
720 }
721
enable_mux(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)722 static int enable_mux(struct s32cc_clk_obj *module,
723 const struct s32cc_clk_drv *drv,
724 unsigned int depth)
725 {
726 const struct s32cc_clkmux *mux = s32cc_obj2clkmux(module);
727 unsigned int ldepth = depth;
728 const struct s32cc_clk *clk;
729 int ret = 0;
730
731 ret = update_stack_depth(&ldepth);
732 if (ret != 0) {
733 return ret;
734 }
735
736 if (mux == NULL) {
737 return -EINVAL;
738 }
739
740 clk = s32cc_get_arch_clk(mux->source_id);
741 if (clk == NULL) {
742 ERROR("Invalid parent (%lu) for mux %" PRIu8 "\n",
743 mux->source_id, mux->index);
744 return -EINVAL;
745 }
746
747 switch (mux->module) {
748 /* PLL mux will be enabled by PLL setup */
749 case S32CC_ARM_PLL:
750 case S32CC_PERIPH_PLL:
751 case S32CC_DDR_PLL:
752 break;
753 case S32CC_CGM1:
754 ret = enable_cgm_mux(mux, drv);
755 break;
756 case S32CC_CGM0:
757 ret = enable_cgm_mux(mux, drv);
758 break;
759 case S32CC_CGM5:
760 ret = enable_cgm_mux(mux, drv);
761 break;
762 default:
763 ERROR("Unknown mux parent type: %d\n", mux->module);
764 ret = -EINVAL;
765 break;
766 };
767
768 return ret;
769 }
770
get_dfs_parent(const struct s32cc_clk_obj * module)771 static struct s32cc_clk_obj *get_dfs_parent(const struct s32cc_clk_obj *module)
772 {
773 const struct s32cc_dfs *dfs = s32cc_obj2dfs(module);
774
775 if (dfs->parent == NULL) {
776 ERROR("Failed to identify DFS's parent\n");
777 }
778
779 return dfs->parent;
780 }
781
enable_dfs(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)782 static int enable_dfs(struct s32cc_clk_obj *module,
783 const struct s32cc_clk_drv *drv,
784 unsigned int depth)
785 {
786 unsigned int ldepth = depth;
787 int ret = 0;
788
789 ret = update_stack_depth(&ldepth);
790 if (ret != 0) {
791 return ret;
792 }
793
794 return 0;
795 }
796
get_dfs_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)797 static int get_dfs_freq(const struct s32cc_clk_obj *module,
798 const struct s32cc_clk_drv *drv,
799 unsigned long *rate, unsigned int depth)
800 {
801 const struct s32cc_dfs *dfs = s32cc_obj2dfs(module);
802 unsigned int ldepth = depth;
803 uintptr_t dfs_addr;
804 int ret;
805
806 ret = update_stack_depth(&ldepth);
807 if (ret != 0) {
808 return ret;
809 }
810
811 ret = get_base_addr(dfs->instance, drv, &dfs_addr);
812 if (ret != 0) {
813 ERROR("Failed to detect the DFS instance\n");
814 return ret;
815 }
816
817 return get_module_rate(dfs->parent, drv, rate, ldepth);
818 }
819
get_div_dfs(const struct s32cc_dfs_div * dfs_div)820 static struct s32cc_dfs *get_div_dfs(const struct s32cc_dfs_div *dfs_div)
821 {
822 const struct s32cc_clk_obj *parent = dfs_div->parent;
823
824 if (parent->type != s32cc_dfs_t) {
825 ERROR("DFS DIV doesn't have a DFS as parent\n");
826 return NULL;
827 }
828
829 return s32cc_obj2dfs(parent);
830 }
831
get_dfs_mfi_mfn(unsigned long dfs_freq,const struct s32cc_dfs_div * dfs_div,uint32_t * mfi,uint32_t * mfn)832 static int get_dfs_mfi_mfn(unsigned long dfs_freq, const struct s32cc_dfs_div *dfs_div,
833 uint32_t *mfi, uint32_t *mfn)
834 {
835 uint64_t factor64, tmp64, ofreq;
836 uint32_t factor32;
837
838 unsigned long in = dfs_freq;
839 unsigned long out = dfs_div->freq;
840
841 /**
842 * factor = (IN / OUT) / 2
843 * MFI = integer(factor)
844 * MFN = (factor - MFI) * 36
845 */
846 factor64 = ((((uint64_t)in) * FP_PRECISION) / ((uint64_t)out)) / 2ULL;
847 tmp64 = factor64 / FP_PRECISION;
848 if (tmp64 > UINT32_MAX) {
849 return -EINVAL;
850 }
851
852 factor32 = (uint32_t)tmp64;
853 *mfi = factor32;
854
855 tmp64 = ((factor64 - ((uint64_t)*mfi * FP_PRECISION)) * 36UL) / FP_PRECISION;
856 if (tmp64 > UINT32_MAX) {
857 return -EINVAL;
858 }
859
860 *mfn = (uint32_t)tmp64;
861
862 /* div_freq = in / (2 * (*mfi + *mfn / 36.0)) */
863 factor64 = (((uint64_t)*mfn) * FP_PRECISION) / 36ULL;
864 factor64 += ((uint64_t)*mfi) * FP_PRECISION;
865 factor64 *= 2ULL;
866 ofreq = (((uint64_t)in) * FP_PRECISION) / factor64;
867
868 if (ofreq != dfs_div->freq) {
869 ERROR("Failed to find MFI and MFN settings for DFS DIV freq %lu\n",
870 dfs_div->freq);
871 ERROR("Nearest freq = %" PRIx64 "\n", ofreq);
872 return -EINVAL;
873 }
874
875 return 0;
876 }
877
init_dfs_port(uintptr_t dfs_addr,uint32_t port,uint32_t mfi,uint32_t mfn)878 static int init_dfs_port(uintptr_t dfs_addr, uint32_t port,
879 uint32_t mfi, uint32_t mfn)
880 {
881 uint32_t portsr, portolsr;
882 uint32_t mask, old_mfi, old_mfn;
883 uint32_t dvport;
884 bool init_dfs;
885
886 dvport = mmio_read_32(DFS_DVPORTn(dfs_addr, port));
887
888 old_mfi = DFS_DVPORTn_MFI(dvport);
889 old_mfn = DFS_DVPORTn_MFN(dvport);
890
891 portsr = mmio_read_32(DFS_PORTSR(dfs_addr));
892 portolsr = mmio_read_32(DFS_PORTOLSR(dfs_addr));
893
894 /* Skip configuration if it's not needed */
895 if (((portsr & BIT_32(port)) != 0U) &&
896 ((portolsr & BIT_32(port)) == 0U) &&
897 (mfi == old_mfi) && (mfn == old_mfn)) {
898 return 0;
899 }
900
901 init_dfs = (portsr == 0U);
902
903 if (init_dfs) {
904 mask = DFS_PORTRESET_MASK;
905 } else {
906 mask = DFS_PORTRESET_SET(BIT_32(port));
907 }
908
909 mmio_write_32(DFS_PORTOLSR(dfs_addr), mask);
910 mmio_write_32(DFS_PORTRESET(dfs_addr), mask);
911
912 while ((mmio_read_32(DFS_PORTSR(dfs_addr)) & mask) != 0U) {
913 }
914
915 if (init_dfs) {
916 mmio_write_32(DFS_CTL(dfs_addr), DFS_CTL_RESET);
917 }
918
919 mmio_write_32(DFS_DVPORTn(dfs_addr, port),
920 DFS_DVPORTn_MFI_SET(mfi) | DFS_DVPORTn_MFN_SET(mfn));
921
922 if (init_dfs) {
923 /* DFS clk enable programming */
924 mmio_clrbits_32(DFS_CTL(dfs_addr), DFS_CTL_RESET);
925 }
926
927 mmio_clrbits_32(DFS_PORTRESET(dfs_addr), BIT_32(port));
928
929 while ((mmio_read_32(DFS_PORTSR(dfs_addr)) & BIT_32(port)) != BIT_32(port)) {
930 }
931
932 portolsr = mmio_read_32(DFS_PORTOLSR(dfs_addr));
933 if ((portolsr & DFS_PORTOLSR_LOL(port)) != 0U) {
934 ERROR("Failed to lock DFS divider\n");
935 return -EINVAL;
936 }
937
938 return 0;
939 }
940
941 static struct s32cc_clk_obj *
get_dfs_div_parent(const struct s32cc_clk_obj * module)942 get_dfs_div_parent(const struct s32cc_clk_obj *module)
943 {
944 const struct s32cc_dfs_div *dfs_div = s32cc_obj2dfsdiv(module);
945
946 if (dfs_div->parent == NULL) {
947 ERROR("Failed to identify DFS divider's parent\n");
948 }
949
950 return dfs_div->parent;
951 }
952
enable_dfs_div(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)953 static int enable_dfs_div(struct s32cc_clk_obj *module,
954 const struct s32cc_clk_drv *drv,
955 unsigned int depth)
956 {
957 const struct s32cc_dfs_div *dfs_div = s32cc_obj2dfsdiv(module);
958 unsigned int ldepth = depth;
959 const struct s32cc_dfs *dfs;
960 uintptr_t dfs_addr = 0UL;
961 unsigned long dfs_freq;
962 uint32_t mfi, mfn;
963 int ret = 0;
964
965 ret = update_stack_depth(&ldepth);
966 if (ret != 0) {
967 return ret;
968 }
969
970 dfs = get_div_dfs(dfs_div);
971 if (dfs == NULL) {
972 return -EINVAL;
973 }
974
975 ret = get_base_addr(dfs->instance, drv, &dfs_addr);
976 if ((ret != 0) || (dfs_addr == 0UL)) {
977 return -EINVAL;
978 }
979
980 ret = get_module_rate(&dfs->desc, drv, &dfs_freq, depth);
981 if (ret != 0) {
982 return ret;
983 }
984
985 ret = get_dfs_mfi_mfn(dfs_freq, dfs_div, &mfi, &mfn);
986 if (ret != 0) {
987 return -EINVAL;
988 }
989
990 return init_dfs_port(dfs_addr, dfs_div->index, mfi, mfn);
991 }
992
993 typedef int (*enable_clk_t)(struct s32cc_clk_obj *module,
994 const struct s32cc_clk_drv *drv,
995 unsigned int depth);
996
enable_part(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)997 static int enable_part(struct s32cc_clk_obj *module,
998 const struct s32cc_clk_drv *drv,
999 unsigned int depth)
1000 {
1001 const struct s32cc_part *part = s32cc_obj2part(module);
1002 uint32_t part_no = part->partition_id;
1003
1004 if ((drv->mc_me == 0UL) || (drv->mc_rgm == 0UL) || (drv->rdc == 0UL)) {
1005 return -EINVAL;
1006 }
1007
1008 return mc_me_enable_partition(drv->mc_me, drv->mc_rgm, drv->rdc, part_no);
1009 }
1010
enable_part_block(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)1011 static int enable_part_block(struct s32cc_clk_obj *module,
1012 const struct s32cc_clk_drv *drv,
1013 unsigned int depth)
1014 {
1015 const struct s32cc_part_block *block = s32cc_obj2partblock(module);
1016 const struct s32cc_part *part = block->part;
1017 uint32_t part_no = part->partition_id;
1018 unsigned int ldepth = depth;
1019 uint32_t cofb;
1020 int ret;
1021
1022 ret = update_stack_depth(&ldepth);
1023 if (ret != 0) {
1024 return ret;
1025 }
1026
1027 if ((block->block >= s32cc_part_block0) &&
1028 (block->block <= s32cc_part_block15)) {
1029 cofb = (uint32_t)block->block - (uint32_t)s32cc_part_block0;
1030 mc_me_enable_part_cofb(drv->mc_me, part_no, cofb, block->status);
1031 } else {
1032 ERROR("Unknown partition block type: %d\n", block->block);
1033 return -EINVAL;
1034 }
1035
1036 return 0;
1037 }
1038
1039 static struct s32cc_clk_obj *
get_part_block_parent(const struct s32cc_clk_obj * module)1040 get_part_block_parent(const struct s32cc_clk_obj *module)
1041 {
1042 const struct s32cc_part_block *block = s32cc_obj2partblock(module);
1043
1044 return &block->part->desc;
1045 }
1046
1047 static int enable_module_with_refcount(struct s32cc_clk_obj *module,
1048 const struct s32cc_clk_drv *drv,
1049 unsigned int depth);
1050
enable_part_block_link(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)1051 static int enable_part_block_link(struct s32cc_clk_obj *module,
1052 const struct s32cc_clk_drv *drv,
1053 unsigned int depth)
1054 {
1055 const struct s32cc_part_block_link *link = s32cc_obj2partblocklink(module);
1056 struct s32cc_part_block *block = link->block;
1057 unsigned int ldepth = depth;
1058 int ret;
1059
1060 ret = update_stack_depth(&ldepth);
1061 if (ret != 0) {
1062 return ret;
1063 }
1064
1065 /* Move the enablement algorithm to partition tree */
1066 return enable_module_with_refcount(&block->desc, drv, ldepth);
1067 }
1068
1069 static struct s32cc_clk_obj *
get_part_block_link_parent(const struct s32cc_clk_obj * module)1070 get_part_block_link_parent(const struct s32cc_clk_obj *module)
1071 {
1072 const struct s32cc_part_block_link *link = s32cc_obj2partblocklink(module);
1073
1074 return link->parent;
1075 }
1076
get_part_block_link_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1077 static int get_part_block_link_freq(const struct s32cc_clk_obj *module,
1078 const struct s32cc_clk_drv *drv,
1079 unsigned long *rate, unsigned int depth)
1080 {
1081 const struct s32cc_part_block_link *block = s32cc_obj2partblocklink(module);
1082 unsigned int ldepth = depth;
1083 int ret;
1084
1085 ret = update_stack_depth(&ldepth);
1086 if (ret != 0) {
1087 return ret;
1088 }
1089
1090 return get_module_rate(block->parent, drv, rate, ldepth);
1091 }
1092
cgm_mux_div_config(uintptr_t cgm_addr,uint32_t mux,uint32_t dc,uint32_t div_index)1093 static void cgm_mux_div_config(uintptr_t cgm_addr, uint32_t mux,
1094 uint32_t dc, uint32_t div_index)
1095 {
1096 uint32_t updstat;
1097 uint32_t dc_val = mmio_read_32(MC_CGM_MUXn_DCm(cgm_addr, mux, div_index));
1098
1099 dc_val &= (MC_CGM_MUXn_DCm_DIV_MASK | MC_CGM_MUXn_DCm_DE);
1100
1101 if (dc_val == (MC_CGM_MUXn_DCm_DE | MC_CGM_MUXn_DCm_DIV_SET(dc))) {
1102 return;
1103 }
1104
1105 /* Set the divider */
1106 mmio_write_32(MC_CGM_MUXn_DCm(cgm_addr, mux, div_index),
1107 MC_CGM_MUXn_DCm_DE | MC_CGM_MUXn_DCm_DIV_SET(dc));
1108
1109 /* Wait for divider to get updated */
1110 do {
1111 updstat = mmio_read_32(MC_CGM_MUXn_DIV_UPD_STAT(cgm_addr, mux));
1112 } while (MC_CGM_MUXn_DIV_UPD_STAT_DIVSTAT(updstat) != 0U);
1113 }
1114
get_cgm_div_mux(const struct s32cc_cgm_div * cgm_div)1115 static inline struct s32cc_clkmux *get_cgm_div_mux(const struct s32cc_cgm_div *cgm_div)
1116 {
1117 const struct s32cc_clk_obj *parent = cgm_div->parent;
1118 const struct s32cc_clk_obj *mux_obj;
1119 const struct s32cc_clk *clk;
1120
1121 if (parent == NULL) {
1122 ERROR("Failed to identify CGM DIV's parent\n");
1123 return NULL;
1124 }
1125
1126 if (parent->type != s32cc_clk_t) {
1127 ERROR("The parent of the CGM DIV isn't a clock\n");
1128 return NULL;
1129 }
1130
1131 clk = s32cc_obj2clk(parent);
1132
1133 if (clk->module == NULL) {
1134 ERROR("The clock isn't connected to a module\n");
1135 return NULL;
1136 }
1137
1138 mux_obj = clk->module;
1139
1140 if ((mux_obj->type != s32cc_clkmux_t) &&
1141 (mux_obj->type != s32cc_shared_clkmux_t)) {
1142 ERROR("The parent of the CGM DIV isn't a MUX\n");
1143 return NULL;
1144 }
1145
1146 return s32cc_obj2clkmux(mux_obj);
1147 }
1148
enable_cgm_div(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)1149 static int enable_cgm_div(struct s32cc_clk_obj *module,
1150 const struct s32cc_clk_drv *drv, unsigned int depth)
1151 {
1152 const struct s32cc_cgm_div *cgm_div = s32cc_obj2cgmdiv(module);
1153 const struct s32cc_clkmux *mux;
1154 unsigned int ldepth = depth;
1155 uintptr_t cgm_addr = 0ULL;
1156 uint64_t pfreq, dc64;
1157 uint32_t dc;
1158 int ret;
1159
1160 ret = update_stack_depth(&ldepth);
1161 if (ret != 0) {
1162 return ret;
1163 }
1164
1165 if (cgm_div->parent == NULL) {
1166 ERROR("Failed to identify CGM divider's parent\n");
1167 return -EINVAL;
1168 }
1169
1170 if (cgm_div->freq == 0U) {
1171 ERROR("The frequency of the divider %" PRIu32 " is not set\n",
1172 cgm_div->index);
1173 return -EINVAL;
1174 }
1175
1176 mux = get_cgm_div_mux(cgm_div);
1177 if (mux == NULL) {
1178 return -EINVAL;
1179 }
1180
1181 ret = get_base_addr(mux->module, drv, &cgm_addr);
1182 if (ret != 0) {
1183 ERROR("Failed to get CGM base address of the MUX module %d\n",
1184 mux->module);
1185 return ret;
1186 }
1187
1188 ret = get_module_rate(cgm_div->parent, drv, &pfreq, ldepth);
1189 if (ret != 0) {
1190 ERROR("Failed to enable the div due to unknown frequency of "
1191 "the CGM MUX %" PRIu8 "(CGM=%" PRIxPTR ")\n",
1192 mux->index, cgm_addr);
1193 return -EINVAL;
1194 }
1195
1196 dc64 = ((pfreq * FP_PRECISION) / cgm_div->freq) / FP_PRECISION;
1197 dc = (uint32_t)dc64;
1198
1199 if ((pfreq / dc64) != cgm_div->freq) {
1200 ERROR("Cannot set CGM divider (mux:%" PRIu8 ", div:%" PRIu32
1201 ") for input = %lu & output = %lu, Nearest freq = %lu\n",
1202 mux->index, cgm_div->index, (unsigned long)pfreq,
1203 cgm_div->freq, (unsigned long)(pfreq / dc));
1204 return -EINVAL;
1205 }
1206
1207 cgm_mux_div_config(cgm_addr, mux->index, dc - 1U, cgm_div->index);
1208 return 0;
1209 }
1210
set_cgm_div_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1211 static int set_cgm_div_freq(const struct s32cc_clk_obj *module,
1212 unsigned long rate, unsigned long *orate,
1213 unsigned int *depth)
1214 {
1215 struct s32cc_cgm_div *cgm_div = s32cc_obj2cgmdiv(module);
1216 int ret;
1217
1218 ret = update_stack_depth(depth);
1219 if (ret != 0) {
1220 return ret;
1221 }
1222
1223 if (cgm_div->parent == NULL) {
1224 ERROR("Failed to identify the CGM divider's parent\n");
1225 return -EINVAL;
1226 }
1227
1228 cgm_div->freq = rate;
1229 *orate = rate;
1230
1231 return 0;
1232 }
1233
is_cgm_div_enabled(uintptr_t cgm_addr,uint32_t mux,uint32_t div_index)1234 static inline bool is_cgm_div_enabled(uintptr_t cgm_addr, uint32_t mux,
1235 uint32_t div_index)
1236 {
1237 uint32_t dc_val;
1238
1239 dc_val = mmio_read_32(MC_CGM_MUXn_DCm(cgm_addr, mux, div_index));
1240
1241 return ((dc_val & MC_CGM_MUXn_DCm_DE) != 0U);
1242 }
1243
calc_cgm_div_freq(uintptr_t cgm_addr,uint32_t mux,uint32_t div_index,unsigned long pfreq)1244 static unsigned long calc_cgm_div_freq(uintptr_t cgm_addr, uint32_t mux,
1245 uint32_t div_index, unsigned long pfreq)
1246 {
1247 uint32_t dc_val;
1248 uint32_t dc_div;
1249
1250 dc_val = mmio_read_32(MC_CGM_MUXn_DCm(cgm_addr, mux, div_index));
1251 dc_div = MC_CGM_MUXn_DCm_DIV(dc_val) + 1U;
1252
1253 return pfreq * FP_PRECISION / dc_div / FP_PRECISION;
1254 }
1255
get_cgm_div_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1256 static int get_cgm_div_freq(const struct s32cc_clk_obj *module,
1257 const struct s32cc_clk_drv *drv,
1258 unsigned long *rate, unsigned int depth)
1259 {
1260 const struct s32cc_cgm_div *cgm_div = s32cc_obj2cgmdiv(module);
1261 const struct s32cc_clkmux *mux;
1262 unsigned int ldepth = depth;
1263 uintptr_t cgm_addr = 0ULL;
1264 unsigned long pfreq;
1265 int ret;
1266
1267 ret = update_stack_depth(&ldepth);
1268 if (ret != 0) {
1269 return ret;
1270 }
1271
1272 if (cgm_div->parent == NULL) {
1273 ERROR("Failed to identify CGM divider's parent\n");
1274 return -EINVAL;
1275 }
1276
1277 mux = get_cgm_div_mux(cgm_div);
1278 if (mux == NULL) {
1279 return -EINVAL;
1280 }
1281
1282 ret = get_base_addr(mux->module, drv, &cgm_addr);
1283 if (ret != 0) {
1284 ERROR("Failed to get CGM base address of the MUX module %d\n",
1285 mux->module);
1286 return ret;
1287 }
1288
1289 if (!is_cgm_div_enabled(cgm_addr, mux->index, cgm_div->index)) {
1290 *rate = cgm_div->freq;
1291 return 0;
1292 }
1293
1294 ret = get_module_rate(cgm_div->parent, drv, &pfreq, ldepth);
1295 if (ret != 0) {
1296 ERROR("Failed to get the frequency of CGM MUX %" PRIu8 "(CGM=0x%" PRIxPTR ")\n",
1297 mux->index, cgm_addr);
1298 return ret;
1299 }
1300
1301 *rate = calc_cgm_div_freq(cgm_addr, mux->index, cgm_div->index, pfreq);
1302
1303 return 0;
1304 }
1305
1306 static struct s32cc_clk_obj *
get_cgm_div_parent(const struct s32cc_clk_obj * module)1307 get_cgm_div_parent(const struct s32cc_clk_obj *module)
1308 {
1309 const struct s32cc_cgm_div *cgm_div = s32cc_obj2cgmdiv(module);
1310
1311 if (cgm_div->parent == NULL) {
1312 ERROR("Failed to identify the CGM divider's parent\n");
1313 return NULL;
1314 }
1315
1316 return cgm_div->parent;
1317 }
1318
no_enable(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)1319 static int no_enable(struct s32cc_clk_obj *module,
1320 const struct s32cc_clk_drv *drv,
1321 unsigned int depth)
1322 {
1323 return 0;
1324 }
1325
exec_cb_with_refcount(enable_clk_t en_cb,struct s32cc_clk_obj * mod,const struct s32cc_clk_drv * drv,bool leaf_node,unsigned int depth)1326 static int exec_cb_with_refcount(enable_clk_t en_cb, struct s32cc_clk_obj *mod,
1327 const struct s32cc_clk_drv *drv, bool leaf_node,
1328 unsigned int depth)
1329 {
1330 unsigned int ldepth = depth;
1331 int ret = 0;
1332
1333 if (mod == NULL) {
1334 return 0;
1335 }
1336
1337 ret = update_stack_depth(&ldepth);
1338 if (ret != 0) {
1339 return ret;
1340 }
1341
1342 /* Refcount will be updated as part of the recursivity */
1343 if (leaf_node) {
1344 return en_cb(mod, drv, ldepth);
1345 }
1346
1347 if (mod->refcount == 0U) {
1348 ret = en_cb(mod, drv, ldepth);
1349 }
1350
1351 if (ret == 0) {
1352 mod->refcount++;
1353 }
1354
1355 return ret;
1356 }
1357
1358 static struct s32cc_clk_obj *get_module_parent(const struct s32cc_clk_obj *module);
1359
enable_module(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)1360 static int enable_module(struct s32cc_clk_obj *module,
1361 const struct s32cc_clk_drv *drv,
1362 unsigned int depth)
1363 {
1364 struct s32cc_clk_obj *parent = get_module_parent(module);
1365 static const enable_clk_t enable_clbs[13] = {
1366 [s32cc_clk_t] = no_enable,
1367 [s32cc_osc_t] = enable_osc,
1368 [s32cc_pll_t] = enable_pll,
1369 [s32cc_pll_out_div_t] = enable_pll_div,
1370 [s32cc_clkmux_t] = enable_mux,
1371 [s32cc_shared_clkmux_t] = enable_mux,
1372 [s32cc_dfs_t] = enable_dfs,
1373 [s32cc_dfs_div_t] = enable_dfs_div,
1374 [s32cc_part_t] = enable_part,
1375 [s32cc_part_block_t] = enable_part_block,
1376 [s32cc_part_block_link_t] = enable_part_block_link,
1377 [s32cc_cgm_div_t] = enable_cgm_div,
1378 };
1379 unsigned int ldepth = depth;
1380 uint32_t index;
1381 int ret = 0;
1382
1383 ret = update_stack_depth(&ldepth);
1384 if (ret != 0) {
1385 return ret;
1386 }
1387
1388 if (drv == NULL) {
1389 return -EINVAL;
1390 }
1391
1392 index = (uint32_t)module->type;
1393
1394 if (index >= ARRAY_SIZE(enable_clbs)) {
1395 ERROR("Undefined module type: %d\n", module->type);
1396 return -EINVAL;
1397 }
1398
1399 if (enable_clbs[index] == NULL) {
1400 ERROR("Undefined callback for the clock type: %d\n",
1401 module->type);
1402 return -EINVAL;
1403 }
1404
1405 parent = get_module_parent(module);
1406
1407 ret = exec_cb_with_refcount(enable_module, parent, drv,
1408 false, ldepth);
1409 if (ret != 0) {
1410 return ret;
1411 }
1412
1413 ret = exec_cb_with_refcount(enable_clbs[index], module, drv,
1414 true, ldepth);
1415 if (ret != 0) {
1416 return ret;
1417 }
1418
1419 return ret;
1420 }
1421
enable_module_with_refcount(struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned int depth)1422 static int enable_module_with_refcount(struct s32cc_clk_obj *module,
1423 const struct s32cc_clk_drv *drv,
1424 unsigned int depth)
1425 {
1426 return exec_cb_with_refcount(enable_module, module, drv, false, depth);
1427 }
1428
s32cc_clk_enable(unsigned long id)1429 static int s32cc_clk_enable(unsigned long id)
1430 {
1431 const struct s32cc_clk_drv *drv = get_drv();
1432 unsigned int depth = MAX_STACK_DEPTH;
1433 struct s32cc_clk *clk;
1434
1435 clk = s32cc_get_arch_clk(id);
1436 if (clk == NULL) {
1437 return -EINVAL;
1438 }
1439
1440 return enable_module_with_refcount(&clk->desc, drv, depth);
1441 }
1442
s32cc_clk_disable(unsigned long id)1443 static void s32cc_clk_disable(unsigned long id)
1444 {
1445 }
1446
s32cc_clk_is_enabled(unsigned long id)1447 static bool s32cc_clk_is_enabled(unsigned long id)
1448 {
1449 return false;
1450 }
1451
set_osc_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1452 static int set_osc_freq(const struct s32cc_clk_obj *module, unsigned long rate,
1453 unsigned long *orate, unsigned int *depth)
1454 {
1455 struct s32cc_osc *osc = s32cc_obj2osc(module);
1456 int ret;
1457
1458 ret = update_stack_depth(depth);
1459 if (ret != 0) {
1460 return ret;
1461 }
1462
1463 if ((osc->freq != 0UL) && (rate != osc->freq)) {
1464 ERROR("Already initialized oscillator. freq = %lu\n",
1465 osc->freq);
1466 return -EINVAL;
1467 }
1468
1469 osc->freq = rate;
1470 *orate = osc->freq;
1471
1472 return 0;
1473 }
1474
get_osc_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1475 static int get_osc_freq(const struct s32cc_clk_obj *module,
1476 const struct s32cc_clk_drv *drv,
1477 unsigned long *rate, unsigned int depth)
1478 {
1479 const struct s32cc_osc *osc = s32cc_obj2osc(module);
1480 unsigned int ldepth = depth;
1481 int ret;
1482
1483 ret = update_stack_depth(&ldepth);
1484 if (ret != 0) {
1485 return ret;
1486 }
1487
1488 if (osc->freq == 0UL) {
1489 ERROR("Uninitialized oscillator\n");
1490 return -EINVAL;
1491 }
1492
1493 *rate = osc->freq;
1494
1495 return 0;
1496 }
1497
set_clk_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1498 static int set_clk_freq(const struct s32cc_clk_obj *module, unsigned long rate,
1499 unsigned long *orate, unsigned int *depth)
1500 {
1501 const struct s32cc_clk *clk = s32cc_obj2clk(module);
1502 int ret;
1503
1504 ret = update_stack_depth(depth);
1505 if (ret != 0) {
1506 return ret;
1507 }
1508
1509 if ((clk->min_freq != 0UL) && (clk->max_freq != 0UL) &&
1510 ((rate < clk->min_freq) || (rate > clk->max_freq))) {
1511 ERROR("%lu frequency is out of the allowed range: [%lu:%lu]\n",
1512 rate, clk->min_freq, clk->max_freq);
1513 return -EINVAL;
1514 }
1515
1516 if (clk->module != NULL) {
1517 return set_module_rate(clk->module, rate, orate, depth);
1518 }
1519
1520 if (clk->pclock != NULL) {
1521 return set_clk_freq(&clk->pclock->desc, rate, orate, depth);
1522 }
1523
1524 return -EINVAL;
1525 }
1526
get_clk_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1527 static int get_clk_freq(const struct s32cc_clk_obj *module,
1528 const struct s32cc_clk_drv *drv, unsigned long *rate,
1529 unsigned int depth)
1530 {
1531 const struct s32cc_clk *clk = s32cc_obj2clk(module);
1532 unsigned int ldepth = depth;
1533 int ret;
1534
1535 ret = update_stack_depth(&ldepth);
1536 if (ret != 0) {
1537 return ret;
1538 }
1539
1540 if (clk == NULL) {
1541 ERROR("Invalid clock\n");
1542 return -EINVAL;
1543 }
1544
1545 if (clk->module != NULL) {
1546 return get_module_rate(clk->module, drv, rate, ldepth);
1547 }
1548
1549 if (clk->pclock == NULL) {
1550 ERROR("Invalid clock parent\n");
1551 return -EINVAL;
1552 }
1553
1554 return get_clk_freq(&clk->pclock->desc, drv, rate, ldepth);
1555 }
1556
set_pll_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1557 static int set_pll_freq(const struct s32cc_clk_obj *module, unsigned long rate,
1558 unsigned long *orate, unsigned int *depth)
1559 {
1560 struct s32cc_pll *pll = s32cc_obj2pll(module);
1561 int ret;
1562
1563 ret = update_stack_depth(depth);
1564 if (ret != 0) {
1565 return ret;
1566 }
1567
1568 if ((pll->vco_freq != 0UL) && (pll->vco_freq != rate)) {
1569 ERROR("PLL frequency was already set\n");
1570 return -EINVAL;
1571 }
1572
1573 pll->vco_freq = rate;
1574 *orate = pll->vco_freq;
1575
1576 return 0;
1577 }
1578
get_pll_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1579 static int get_pll_freq(const struct s32cc_clk_obj *module,
1580 const struct s32cc_clk_drv *drv,
1581 unsigned long *rate, unsigned int depth)
1582 {
1583 const struct s32cc_pll *pll = s32cc_obj2pll(module);
1584 const struct s32cc_clk *source;
1585 uint32_t mfi, mfn, rdiv, plldv;
1586 unsigned long prate, clk_src;
1587 unsigned int ldepth = depth;
1588 uintptr_t pll_addr = 0UL;
1589 uint64_t t1, t2;
1590 int ret;
1591
1592 ret = update_stack_depth(&ldepth);
1593 if (ret != 0) {
1594 return ret;
1595 }
1596
1597 ret = get_base_addr(pll->instance, drv, &pll_addr);
1598 if (ret != 0) {
1599 ERROR("Failed to detect PLL instance\n");
1600 return ret;
1601 }
1602
1603 /* Disabled PLL */
1604 if (!is_pll_enabled(pll_addr)) {
1605 *rate = pll->vco_freq;
1606 return 0;
1607 }
1608
1609 clk_src = mmio_read_32(PLLDIG_PLLCLKMUX(pll_addr));
1610 switch (clk_src) {
1611 case 0:
1612 clk_src = S32CC_CLK_FIRC;
1613 break;
1614 case 1:
1615 clk_src = S32CC_CLK_FXOSC;
1616 break;
1617 default:
1618 ERROR("Failed to identify PLL source id %" PRIu64 "\n", clk_src);
1619 return -EINVAL;
1620 };
1621
1622 source = s32cc_get_arch_clk(clk_src);
1623 if (source == NULL) {
1624 ERROR("Failed to get PLL source clock\n");
1625 return -EINVAL;
1626 }
1627
1628 ret = get_module_rate(&source->desc, drv, &prate, ldepth);
1629 if (ret != 0) {
1630 ERROR("Failed to get PLL's parent frequency\n");
1631 return ret;
1632 }
1633
1634 plldv = mmio_read_32(PLLDIG_PLLDV(pll_addr));
1635 mfi = PLLDIG_PLLDV_MFI(plldv);
1636 rdiv = PLLDIG_PLLDV_RDIV(plldv);
1637 if (rdiv == 0U) {
1638 rdiv = 1;
1639 }
1640
1641 /* Frac-N mode */
1642 mfn = PLLDIG_PLLFD_MFN_SET(mmio_read_32(PLLDIG_PLLFD(pll_addr)));
1643
1644 /* PLL VCO frequency in Fractional mode when PLLDV[RDIV] is not 0 */
1645 t1 = prate / rdiv;
1646 t2 = (mfi * FP_PRECISION) + (mfn * FP_PRECISION / 18432U);
1647
1648 *rate = t1 * t2 / FP_PRECISION;
1649
1650 return 0;
1651 }
1652
set_pll_div_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1653 static int set_pll_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
1654 unsigned long *orate, unsigned int *depth)
1655 {
1656 struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module);
1657 const struct s32cc_pll *pll;
1658 unsigned long prate, dc;
1659 int ret;
1660
1661 ret = update_stack_depth(depth);
1662 if (ret != 0) {
1663 return ret;
1664 }
1665
1666 if (pdiv->parent == NULL) {
1667 ERROR("Failed to identify PLL divider's parent\n");
1668 return -EINVAL;
1669 }
1670
1671 pll = s32cc_obj2pll(pdiv->parent);
1672 if (pll == NULL) {
1673 ERROR("The parent of the PLL DIV is invalid\n");
1674 return -EINVAL;
1675 }
1676
1677 prate = pll->vco_freq;
1678
1679 /**
1680 * The PLL is not initialized yet, so let's take a risk
1681 * and accept the proposed rate.
1682 */
1683 if (prate == 0UL) {
1684 pdiv->freq = rate;
1685 *orate = rate;
1686 return 0;
1687 }
1688
1689 /* Decline in case the rate cannot fit PLL's requirements. */
1690 dc = prate / rate;
1691 if ((prate / dc) != rate) {
1692 return -EINVAL;
1693 }
1694
1695 pdiv->freq = rate;
1696 *orate = pdiv->freq;
1697
1698 return 0;
1699 }
1700
get_pll_div_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1701 static int get_pll_div_freq(const struct s32cc_clk_obj *module,
1702 const struct s32cc_clk_drv *drv,
1703 unsigned long *rate, unsigned int depth)
1704 {
1705 const struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module);
1706 const struct s32cc_pll *pll;
1707 unsigned int ldepth = depth;
1708 uintptr_t pll_addr = 0UL;
1709 unsigned long pfreq;
1710 uint32_t pllodiv;
1711 uint32_t dc;
1712 int ret;
1713
1714 ret = update_stack_depth(&ldepth);
1715 if (ret != 0) {
1716 return ret;
1717 }
1718
1719 pll = get_div_pll(pdiv);
1720 if (pll == NULL) {
1721 ERROR("The parent of the PLL DIV is invalid\n");
1722 return -EINVAL;
1723 }
1724
1725 ret = get_base_addr(pll->instance, drv, &pll_addr);
1726 if (ret != 0) {
1727 ERROR("Failed to detect PLL instance\n");
1728 return -EINVAL;
1729 }
1730
1731 ret = get_module_rate(pdiv->parent, drv, &pfreq, ldepth);
1732 if (ret != 0) {
1733 ERROR("Failed to get the frequency of PLL %" PRIxPTR "\n",
1734 pll_addr);
1735 return ret;
1736 }
1737
1738 pllodiv = mmio_read_32(PLLDIG_PLLODIV(pll_addr, pdiv->index));
1739
1740 /* Disabled module */
1741 if ((pllodiv & PLLDIG_PLLODIV_DE) == 0U) {
1742 *rate = pdiv->freq;
1743 return 0;
1744 }
1745
1746 dc = PLLDIG_PLLODIV_DIV(pllodiv);
1747 *rate = (pfreq * FP_PRECISION) / (dc + 1U) / FP_PRECISION;
1748
1749 return 0;
1750 }
1751
set_fixed_div_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1752 static int set_fixed_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
1753 unsigned long *orate, unsigned int *depth)
1754 {
1755 const struct s32cc_fixed_div *fdiv = s32cc_obj2fixeddiv(module);
1756 int ret;
1757
1758 ret = update_stack_depth(depth);
1759 if (ret != 0) {
1760 return ret;
1761 }
1762
1763 if (fdiv->parent == NULL) {
1764 ERROR("The divider doesn't have a valid parent\b");
1765 return -EINVAL;
1766 }
1767
1768 ret = set_module_rate(fdiv->parent, rate * fdiv->rate_div, orate, depth);
1769
1770 /* Update the output rate based on the parent's rate */
1771 *orate /= fdiv->rate_div;
1772
1773 return ret;
1774 }
1775
get_fixed_div_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1776 static int get_fixed_div_freq(const struct s32cc_clk_obj *module,
1777 const struct s32cc_clk_drv *drv,
1778 unsigned long *rate, unsigned int depth)
1779 {
1780 const struct s32cc_fixed_div *fdiv = s32cc_obj2fixeddiv(module);
1781 unsigned long pfreq;
1782 int ret;
1783
1784 ret = get_module_rate(fdiv->parent, drv, &pfreq, depth);
1785 if (ret != 0) {
1786 return ret;
1787 }
1788
1789 *rate = (pfreq * FP_PRECISION / fdiv->rate_div) / FP_PRECISION;
1790 return 0;
1791 }
1792
get_fixed_div_parent(const struct s32cc_clk_obj * module)1793 static inline struct s32cc_clk_obj *get_fixed_div_parent(const struct s32cc_clk_obj *module)
1794 {
1795 const struct s32cc_fixed_div *fdiv = s32cc_obj2fixeddiv(module);
1796
1797 return fdiv->parent;
1798 }
1799
set_mux_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1800 static int set_mux_freq(const struct s32cc_clk_obj *module, unsigned long rate,
1801 unsigned long *orate, unsigned int *depth)
1802 {
1803 const struct s32cc_clkmux *mux = s32cc_obj2clkmux(module);
1804 const struct s32cc_clk *clk = s32cc_get_arch_clk(mux->source_id);
1805 int ret;
1806
1807 ret = update_stack_depth(depth);
1808 if (ret != 0) {
1809 return ret;
1810 }
1811
1812 if (clk == NULL) {
1813 ERROR("Mux (id:%" PRIu8 ") without a valid source (%lu)\n",
1814 mux->index, mux->source_id);
1815 return -EINVAL;
1816 }
1817
1818 return set_module_rate(&clk->desc, rate, orate, depth);
1819 }
1820
get_mux_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1821 static int get_mux_freq(const struct s32cc_clk_obj *module,
1822 const struct s32cc_clk_drv *drv,
1823 unsigned long *rate, unsigned int depth)
1824 {
1825 const struct s32cc_clkmux *mux = s32cc_obj2clkmux(module);
1826 const struct s32cc_clk *clk = s32cc_get_arch_clk(mux->source_id);
1827 unsigned int ldepth = depth;
1828 int ret;
1829
1830 ret = update_stack_depth(&ldepth);
1831 if (ret != 0) {
1832 return ret;
1833 }
1834
1835 if (clk == NULL) {
1836 ERROR("Mux (id:%" PRIu8 ") without a valid source (%lu)\n",
1837 mux->index, mux->source_id);
1838 return -EINVAL;
1839 }
1840
1841 return get_clk_freq(&clk->desc, drv, rate, ldepth);
1842 }
1843
set_dfs_div_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1844 static int set_dfs_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
1845 unsigned long *orate, unsigned int *depth)
1846 {
1847 struct s32cc_dfs_div *dfs_div = s32cc_obj2dfsdiv(module);
1848 const struct s32cc_dfs *dfs;
1849 int ret;
1850
1851 ret = update_stack_depth(depth);
1852 if (ret != 0) {
1853 return ret;
1854 }
1855
1856 if (dfs_div->parent == NULL) {
1857 ERROR("Failed to identify DFS divider's parent\n");
1858 return -EINVAL;
1859 }
1860
1861 /* Sanity check */
1862 dfs = s32cc_obj2dfs(dfs_div->parent);
1863 if (dfs->parent == NULL) {
1864 ERROR("Failed to identify DFS's parent\n");
1865 return -EINVAL;
1866 }
1867
1868 if ((dfs_div->freq != 0U) && (dfs_div->freq != rate)) {
1869 ERROR("DFS DIV frequency was already set to %lu\n",
1870 dfs_div->freq);
1871 return -EINVAL;
1872 }
1873
1874 dfs_div->freq = rate;
1875 *orate = rate;
1876
1877 return ret;
1878 }
1879
compute_dfs_div_freq(unsigned long pfreq,uint32_t mfi,uint32_t mfn)1880 static unsigned long compute_dfs_div_freq(unsigned long pfreq, uint32_t mfi, uint32_t mfn)
1881 {
1882 unsigned long freq;
1883
1884 /**
1885 * Formula for input and output clocks of each port divider.
1886 * See 'Digital Frequency Synthesizer' chapter from Reference Manual.
1887 *
1888 * freq = pfreq / (2 * (mfi + mfn / 36.0));
1889 */
1890 freq = (mfi * FP_PRECISION) + (mfn * FP_PRECISION / 36UL);
1891 freq *= 2UL;
1892 freq = pfreq * FP_PRECISION / freq;
1893
1894 return freq;
1895 }
1896
get_dfs_div_freq(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)1897 static int get_dfs_div_freq(const struct s32cc_clk_obj *module,
1898 const struct s32cc_clk_drv *drv,
1899 unsigned long *rate, unsigned int depth)
1900 {
1901 const struct s32cc_dfs_div *dfs_div = s32cc_obj2dfsdiv(module);
1902 unsigned int ldepth = depth;
1903 const struct s32cc_dfs *dfs;
1904 uint32_t dvport, mfi, mfn;
1905 uintptr_t dfs_addr = 0UL;
1906 unsigned long pfreq;
1907 int ret;
1908
1909 ret = update_stack_depth(&ldepth);
1910 if (ret != 0) {
1911 return ret;
1912 }
1913
1914 dfs = get_div_dfs(dfs_div);
1915 if (dfs == NULL) {
1916 return -EINVAL;
1917 }
1918
1919 ret = get_module_rate(dfs_div->parent, drv, &pfreq, ldepth);
1920 if (ret != 0) {
1921 return ret;
1922 }
1923
1924 ret = get_base_addr(dfs->instance, drv, &dfs_addr);
1925 if (ret != 0) {
1926 ERROR("Failed to detect the DFS instance\n");
1927 return ret;
1928 }
1929
1930 dvport = mmio_read_32(DFS_DVPORTn(dfs_addr, dfs_div->index));
1931
1932 mfi = DFS_DVPORTn_MFI(dvport);
1933 mfn = DFS_DVPORTn_MFN(dvport);
1934
1935 /* Disabled port */
1936 if ((mfi == 0U) && (mfn == 0U)) {
1937 *rate = dfs_div->freq;
1938 return 0;
1939 }
1940
1941 *rate = compute_dfs_div_freq(pfreq, mfi, mfn);
1942 return 0;
1943 }
1944
set_part_block_link_freq(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,const unsigned int * depth)1945 static int set_part_block_link_freq(const struct s32cc_clk_obj *module,
1946 unsigned long rate, unsigned long *orate,
1947 const unsigned int *depth)
1948 {
1949 const struct s32cc_part_block_link *link = s32cc_obj2partblocklink(module);
1950 const struct s32cc_clk_obj *parent = link->parent;
1951 unsigned int ldepth = *depth;
1952 int ret;
1953
1954 ret = update_stack_depth(&ldepth);
1955 if (ret != 0) {
1956 return ret;
1957 }
1958
1959 if (parent == NULL) {
1960 ERROR("Partition block link with no parent\n");
1961 return -EINVAL;
1962 }
1963
1964 return set_module_rate(parent, rate, orate, &ldepth);
1965 }
1966
set_module_rate(const struct s32cc_clk_obj * module,unsigned long rate,unsigned long * orate,unsigned int * depth)1967 static int set_module_rate(const struct s32cc_clk_obj *module,
1968 unsigned long rate, unsigned long *orate,
1969 unsigned int *depth)
1970 {
1971 int ret = 0;
1972
1973 ret = update_stack_depth(depth);
1974 if (ret != 0) {
1975 return ret;
1976 }
1977
1978 ret = -EINVAL;
1979
1980 switch (module->type) {
1981 case s32cc_clk_t:
1982 ret = set_clk_freq(module, rate, orate, depth);
1983 break;
1984 case s32cc_osc_t:
1985 ret = set_osc_freq(module, rate, orate, depth);
1986 break;
1987 case s32cc_pll_t:
1988 ret = set_pll_freq(module, rate, orate, depth);
1989 break;
1990 case s32cc_pll_out_div_t:
1991 ret = set_pll_div_freq(module, rate, orate, depth);
1992 break;
1993 case s32cc_fixed_div_t:
1994 ret = set_fixed_div_freq(module, rate, orate, depth);
1995 break;
1996 case s32cc_clkmux_t:
1997 ret = set_mux_freq(module, rate, orate, depth);
1998 break;
1999 case s32cc_shared_clkmux_t:
2000 ret = set_mux_freq(module, rate, orate, depth);
2001 break;
2002 case s32cc_cgm_div_t:
2003 ret = set_cgm_div_freq(module, rate, orate, depth);
2004 break;
2005 case s32cc_dfs_t:
2006 ERROR("Setting the frequency of a DFS is not allowed!");
2007 break;
2008 case s32cc_dfs_div_t:
2009 ret = set_dfs_div_freq(module, rate, orate, depth);
2010 break;
2011 case s32cc_part_block_link_t:
2012 ret = set_part_block_link_freq(module, rate, orate, depth);
2013 break;
2014 case s32cc_part_t:
2015 ERROR("It's not allowed to set the frequency of a partition !");
2016 break;
2017 case s32cc_part_block_t:
2018 ERROR("It's not allowed to set the frequency of a partition block !");
2019 break;
2020 default:
2021 break;
2022 }
2023
2024 return ret;
2025 }
2026
get_module_rate(const struct s32cc_clk_obj * module,const struct s32cc_clk_drv * drv,unsigned long * rate,unsigned int depth)2027 static int get_module_rate(const struct s32cc_clk_obj *module,
2028 const struct s32cc_clk_drv *drv,
2029 unsigned long *rate,
2030 unsigned int depth)
2031 {
2032 unsigned int ldepth = depth;
2033 int ret = 0;
2034
2035 ret = update_stack_depth(&ldepth);
2036 if (ret != 0) {
2037 return ret;
2038 }
2039
2040 switch (module->type) {
2041 case s32cc_osc_t:
2042 ret = get_osc_freq(module, drv, rate, ldepth);
2043 break;
2044 case s32cc_clk_t:
2045 ret = get_clk_freq(module, drv, rate, ldepth);
2046 break;
2047 case s32cc_pll_t:
2048 ret = get_pll_freq(module, drv, rate, ldepth);
2049 break;
2050 case s32cc_dfs_t:
2051 ret = get_dfs_freq(module, drv, rate, ldepth);
2052 break;
2053 case s32cc_dfs_div_t:
2054 ret = get_dfs_div_freq(module, drv, rate, ldepth);
2055 break;
2056 case s32cc_fixed_div_t:
2057 ret = get_fixed_div_freq(module, drv, rate, ldepth);
2058 break;
2059 case s32cc_pll_out_div_t:
2060 ret = get_pll_div_freq(module, drv, rate, ldepth);
2061 break;
2062 case s32cc_clkmux_t:
2063 ret = get_mux_freq(module, drv, rate, ldepth);
2064 break;
2065 case s32cc_shared_clkmux_t:
2066 ret = get_mux_freq(module, drv, rate, ldepth);
2067 break;
2068 case s32cc_part_t:
2069 ERROR("s32cc_part_t cannot be used to get rate\n");
2070 break;
2071 case s32cc_part_block_t:
2072 ERROR("s32cc_part_block_t cannot be used to get rate\n");
2073 break;
2074 case s32cc_part_block_link_t:
2075 ret = get_part_block_link_freq(module, drv, rate, ldepth);
2076 break;
2077 case s32cc_cgm_div_t:
2078 ret = get_cgm_div_freq(module, drv, rate, ldepth);
2079 break;
2080 default:
2081 ret = -EINVAL;
2082 break;
2083 }
2084
2085 return ret;
2086 }
2087
s32cc_clk_set_rate(unsigned long id,unsigned long rate,unsigned long * orate)2088 static int s32cc_clk_set_rate(unsigned long id, unsigned long rate,
2089 unsigned long *orate)
2090 {
2091 unsigned int depth = MAX_STACK_DEPTH;
2092 const struct s32cc_clk *clk;
2093 int ret;
2094
2095 clk = s32cc_get_arch_clk(id);
2096 if (clk == NULL) {
2097 return -EINVAL;
2098 }
2099
2100 ret = set_module_rate(&clk->desc, rate, orate, &depth);
2101 if (ret != 0) {
2102 ERROR("Failed to set frequency (%lu MHz) for clock %lu\n",
2103 rate, id);
2104 }
2105
2106 return ret;
2107 }
2108
s32cc_clk_get_rate(unsigned long id)2109 static unsigned long s32cc_clk_get_rate(unsigned long id)
2110 {
2111 const struct s32cc_clk_drv *drv = get_drv();
2112 unsigned int depth = MAX_STACK_DEPTH;
2113 const struct s32cc_clk *clk;
2114 unsigned long rate = 0UL;
2115 int ret;
2116
2117 clk = s32cc_get_arch_clk(id);
2118 if (clk == NULL) {
2119 return 0;
2120 }
2121
2122 ret = get_module_rate(&clk->desc, drv, &rate, depth);
2123 if (ret != 0) {
2124 ERROR("Failed to get frequency (%lu MHz) for clock %lu\n",
2125 rate, id);
2126 return 0;
2127 }
2128
2129 return rate;
2130 }
2131
get_no_parent(const struct s32cc_clk_obj * module)2132 static struct s32cc_clk_obj *get_no_parent(const struct s32cc_clk_obj *module)
2133 {
2134 return NULL;
2135 }
2136
2137 typedef struct s32cc_clk_obj *(*get_parent_clb_t)(const struct s32cc_clk_obj *clk_obj);
2138
get_module_parent(const struct s32cc_clk_obj * module)2139 static struct s32cc_clk_obj *get_module_parent(const struct s32cc_clk_obj *module)
2140 {
2141 static const get_parent_clb_t parents_clbs[13] = {
2142 [s32cc_clk_t] = get_clk_parent,
2143 [s32cc_osc_t] = get_no_parent,
2144 [s32cc_pll_t] = get_pll_parent,
2145 [s32cc_pll_out_div_t] = get_pll_div_parent,
2146 [s32cc_clkmux_t] = get_mux_parent,
2147 [s32cc_shared_clkmux_t] = get_mux_parent,
2148 [s32cc_dfs_t] = get_dfs_parent,
2149 [s32cc_dfs_div_t] = get_dfs_div_parent,
2150 [s32cc_part_t] = get_no_parent,
2151 [s32cc_fixed_div_t] = get_fixed_div_parent,
2152 [s32cc_part_block_t] = get_part_block_parent,
2153 [s32cc_part_block_link_t] = get_part_block_link_parent,
2154 [s32cc_cgm_div_t] = get_cgm_div_parent,
2155 };
2156 uint32_t index;
2157
2158 if (module == NULL) {
2159 return NULL;
2160 }
2161
2162 index = (uint32_t)module->type;
2163
2164 if (index >= ARRAY_SIZE(parents_clbs)) {
2165 ERROR("Undefined module type: %d\n", module->type);
2166 return NULL;
2167 }
2168
2169 if (parents_clbs[index] == NULL) {
2170 ERROR("Undefined parent getter for type: %d\n", module->type);
2171 return NULL;
2172 }
2173
2174 return parents_clbs[index](module);
2175 }
2176
s32cc_clk_get_parent(unsigned long id)2177 static int s32cc_clk_get_parent(unsigned long id)
2178 {
2179 struct s32cc_clk *parent_clk;
2180 const struct s32cc_clk_obj *parent;
2181 const struct s32cc_clk *clk;
2182 unsigned long parent_id;
2183 int ret;
2184
2185 clk = s32cc_get_arch_clk(id);
2186 if (clk == NULL) {
2187 return -EINVAL;
2188 }
2189
2190 parent = get_module_parent(clk->module);
2191 if (parent == NULL) {
2192 return -EINVAL;
2193 }
2194
2195 parent_clk = s32cc_obj2clk(parent);
2196 if (parent_clk == NULL) {
2197 return -EINVAL;
2198 }
2199
2200 ret = s32cc_get_clk_id(parent_clk, &parent_id);
2201 if (ret != 0) {
2202 return ret;
2203 }
2204
2205 if (parent_id > (unsigned long)INT_MAX) {
2206 return -E2BIG;
2207 }
2208
2209 return (int)parent_id;
2210 }
2211
s32cc_clk_set_parent(unsigned long id,unsigned long parent_id)2212 static int s32cc_clk_set_parent(unsigned long id, unsigned long parent_id)
2213 {
2214 const struct s32cc_clk *parent;
2215 const struct s32cc_clk *clk;
2216 bool valid_source = false;
2217 struct s32cc_clkmux *mux;
2218 uint8_t i;
2219
2220 clk = s32cc_get_arch_clk(id);
2221 if (clk == NULL) {
2222 return -EINVAL;
2223 }
2224
2225 parent = s32cc_get_arch_clk(parent_id);
2226 if (parent == NULL) {
2227 return -EINVAL;
2228 }
2229
2230 if (!is_s32cc_clk_mux(clk)) {
2231 ERROR("Clock %lu is not a mux\n", id);
2232 return -EINVAL;
2233 }
2234
2235 mux = s32cc_clk2mux(clk);
2236 if (mux == NULL) {
2237 ERROR("Failed to cast clock %lu to clock mux\n", id);
2238 return -EINVAL;
2239 }
2240
2241 for (i = 0; i < mux->nclks; i++) {
2242 if (mux->clkids[i] == parent_id) {
2243 valid_source = true;
2244 break;
2245 }
2246 }
2247
2248 if (!valid_source) {
2249 ERROR("Clock %lu is not a valid clock for mux %lu\n",
2250 parent_id, id);
2251 return -EINVAL;
2252 }
2253
2254 mux->source_id = parent_id;
2255
2256 return 0;
2257 }
2258
s32cc_clk_mmap_regs(const struct s32cc_clk_drv * drv)2259 static int s32cc_clk_mmap_regs(const struct s32cc_clk_drv *drv)
2260 {
2261 const uintptr_t base_addrs[12] = {
2262 drv->fxosc_base,
2263 drv->armpll_base,
2264 drv->periphpll_base,
2265 drv->armdfs_base,
2266 drv->periphdfs_base,
2267 drv->cgm0_base,
2268 drv->cgm1_base,
2269 drv->cgm5_base,
2270 drv->ddrpll_base,
2271 drv->mc_me,
2272 drv->mc_rgm,
2273 drv->rdc,
2274 };
2275 size_t i;
2276 int ret;
2277
2278 for (i = 0U; i < ARRAY_SIZE(base_addrs); i++) {
2279 ret = mmap_add_dynamic_region(base_addrs[i], base_addrs[i],
2280 PAGE_SIZE,
2281 MT_DEVICE | MT_RW | MT_SECURE);
2282 if (ret != 0) {
2283 ERROR("Failed to map clock module 0x%" PRIuPTR "\n",
2284 base_addrs[i]);
2285 return ret;
2286 }
2287 }
2288
2289 return 0;
2290 }
2291
s32cc_clk_register_drv(bool mmap_regs)2292 int s32cc_clk_register_drv(bool mmap_regs)
2293 {
2294 static const struct clk_ops s32cc_clk_ops = {
2295 .enable = s32cc_clk_enable,
2296 .disable = s32cc_clk_disable,
2297 .is_enabled = s32cc_clk_is_enabled,
2298 .get_rate = s32cc_clk_get_rate,
2299 .set_rate = s32cc_clk_set_rate,
2300 .get_parent = s32cc_clk_get_parent,
2301 .set_parent = s32cc_clk_set_parent,
2302 };
2303 const struct s32cc_clk_drv *drv;
2304
2305 clk_register(&s32cc_clk_ops);
2306
2307 drv = get_drv();
2308 if (drv == NULL) {
2309 return -EINVAL;
2310 }
2311
2312 if (mmap_regs) {
2313 return s32cc_clk_mmap_regs(drv);
2314 }
2315
2316 return 0;
2317 }
2318
2319