xref: /rk3399_ARM-atf/drivers/nxp/clk/s32cc/s32cc_clk_drv.c (revision f8490b85b49c92799a792587658eca4cf36fd4f6) 
1 /*
2  * Copyright 2024 NXP
3  *
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 #include <errno.h>
7 
8 #include <s32cc-clk-regs.h>
9 
10 #include <common/debug.h>
11 #include <drivers/clk.h>
12 #include <lib/mmio.h>
13 #include <s32cc-clk-ids.h>
14 #include <s32cc-clk-modules.h>
15 #include <s32cc-clk-utils.h>
16 
17 #define MAX_STACK_DEPTH		(15U)
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 cgm0_base;
28 	uintptr_t cgm1_base;
29 };
30 
31 static int update_stack_depth(unsigned int *depth)
32 {
33 	if (*depth == 0U) {
34 		return -ENOMEM;
35 	}
36 
37 	(*depth)--;
38 	return 0;
39 }
40 
41 static struct s32cc_clk_drv *get_drv(void)
42 {
43 	static struct s32cc_clk_drv driver = {
44 		.fxosc_base = FXOSC_BASE_ADDR,
45 		.armpll_base = ARMPLL_BASE_ADDR,
46 		.periphpll_base = PERIPHPLL_BASE_ADDR,
47 		.armdfs_base = ARM_DFS_BASE_ADDR,
48 		.cgm0_base = CGM0_BASE_ADDR,
49 		.cgm1_base = CGM1_BASE_ADDR,
50 	};
51 
52 	return &driver;
53 }
54 
55 static int enable_module(const struct s32cc_clk_obj *module, unsigned int *depth);
56 
57 static int enable_clk_module(const struct s32cc_clk_obj *module,
58 			     const struct s32cc_clk_drv *drv,
59 			     unsigned int *depth)
60 {
61 	const struct s32cc_clk *clk = s32cc_obj2clk(module);
62 	int ret;
63 
64 	ret = update_stack_depth(depth);
65 	if (ret != 0) {
66 		return ret;
67 	}
68 
69 	if (clk == NULL) {
70 		return -EINVAL;
71 	}
72 
73 	if (clk->module != NULL) {
74 		return enable_module(clk->module, depth);
75 	}
76 
77 	if (clk->pclock != NULL) {
78 		return enable_clk_module(&clk->pclock->desc, drv, depth);
79 	}
80 
81 	return -EINVAL;
82 }
83 
84 static int get_base_addr(enum s32cc_clk_source id, const struct s32cc_clk_drv *drv,
85 			 uintptr_t *base)
86 {
87 	int ret = 0;
88 
89 	switch (id) {
90 	case S32CC_FXOSC:
91 		*base = drv->fxosc_base;
92 		break;
93 	case S32CC_ARM_PLL:
94 		*base = drv->armpll_base;
95 		break;
96 	case S32CC_PERIPH_PLL:
97 		*base = drv->periphpll_base;
98 		break;
99 	case S32CC_ARM_DFS:
100 		*base = drv->armdfs_base;
101 		break;
102 	case S32CC_CGM0:
103 		*base = drv->cgm0_base;
104 		break;
105 	case S32CC_CGM1:
106 		*base = drv->cgm1_base;
107 		break;
108 	case S32CC_FIRC:
109 		break;
110 	case S32CC_SIRC:
111 		break;
112 	default:
113 		ret = -EINVAL;
114 		break;
115 	}
116 
117 	if (ret != 0) {
118 		ERROR("Unknown clock source id: %u\n", id);
119 	}
120 
121 	return ret;
122 }
123 
124 static void enable_fxosc(const struct s32cc_clk_drv *drv)
125 {
126 	uintptr_t fxosc_base = drv->fxosc_base;
127 	uint32_t ctrl;
128 
129 	ctrl = mmio_read_32(FXOSC_CTRL(fxosc_base));
130 	if ((ctrl & FXOSC_CTRL_OSCON) != U(0)) {
131 		return;
132 	}
133 
134 	ctrl = FXOSC_CTRL_COMP_EN;
135 	ctrl &= ~FXOSC_CTRL_OSC_BYP;
136 	ctrl |= FXOSC_CTRL_EOCV(0x1);
137 	ctrl |= FXOSC_CTRL_GM_SEL(0x7);
138 	mmio_write_32(FXOSC_CTRL(fxosc_base), ctrl);
139 
140 	/* Switch ON the crystal oscillator. */
141 	mmio_setbits_32(FXOSC_CTRL(fxosc_base), FXOSC_CTRL_OSCON);
142 
143 	/* Wait until the clock is stable. */
144 	while ((mmio_read_32(FXOSC_STAT(fxosc_base)) & FXOSC_STAT_OSC_STAT) == U(0)) {
145 	}
146 }
147 
148 static int enable_osc(const struct s32cc_clk_obj *module,
149 		      const struct s32cc_clk_drv *drv,
150 		      unsigned int *depth)
151 {
152 	const struct s32cc_osc *osc = s32cc_obj2osc(module);
153 	int ret = 0;
154 
155 	ret = update_stack_depth(depth);
156 	if (ret != 0) {
157 		return ret;
158 	}
159 
160 	switch (osc->source) {
161 	case S32CC_FXOSC:
162 		enable_fxosc(drv);
163 		break;
164 	/* FIRC and SIRC oscillators are enabled by default */
165 	case S32CC_FIRC:
166 		break;
167 	case S32CC_SIRC:
168 		break;
169 	default:
170 		ERROR("Invalid oscillator %d\n", osc->source);
171 		ret = -EINVAL;
172 		break;
173 	};
174 
175 	return ret;
176 }
177 
178 static int get_pll_mfi_mfn(unsigned long pll_vco, unsigned long ref_freq,
179 			   uint32_t *mfi, uint32_t *mfn)
180 
181 {
182 	unsigned long vco;
183 	unsigned long mfn64;
184 
185 	/* FRAC-N mode */
186 	*mfi = (uint32_t)(pll_vco / ref_freq);
187 
188 	/* MFN formula : (double)(pll_vco % ref_freq) / ref_freq * 18432.0 */
189 	mfn64 = pll_vco % ref_freq;
190 	mfn64 *= FP_PRECISION;
191 	mfn64 /= ref_freq;
192 	mfn64 *= 18432UL;
193 	mfn64 /= FP_PRECISION;
194 
195 	if (mfn64 > UINT32_MAX) {
196 		return -EINVAL;
197 	}
198 
199 	*mfn = (uint32_t)mfn64;
200 
201 	vco = ((unsigned long)*mfn * FP_PRECISION) / 18432UL;
202 	vco += (unsigned long)*mfi * FP_PRECISION;
203 	vco *= ref_freq;
204 	vco /= FP_PRECISION;
205 
206 	if (vco != pll_vco) {
207 		ERROR("Failed to find MFI and MFN settings for PLL freq %lu. Nearest freq = %lu\n",
208 		      pll_vco, vco);
209 		return -EINVAL;
210 	}
211 
212 	return 0;
213 }
214 
215 static struct s32cc_clkmux *get_pll_mux(const struct s32cc_pll *pll)
216 {
217 	const struct s32cc_clk_obj *source = pll->source;
218 	const struct s32cc_clk *clk;
219 
220 	if (source == NULL) {
221 		ERROR("Failed to identify PLL's parent\n");
222 		return NULL;
223 	}
224 
225 	if (source->type != s32cc_clk_t) {
226 		ERROR("The parent of the PLL isn't a clock\n");
227 		return NULL;
228 	}
229 
230 	clk = s32cc_obj2clk(source);
231 
232 	if (clk->module == NULL) {
233 		ERROR("The clock isn't connected to a module\n");
234 		return NULL;
235 	}
236 
237 	source = clk->module;
238 
239 	if ((source->type != s32cc_clkmux_t) &&
240 	    (source->type != s32cc_shared_clkmux_t)) {
241 		ERROR("The parent of the PLL isn't a MUX\n");
242 		return NULL;
243 	}
244 
245 	return s32cc_obj2clkmux(source);
246 }
247 
248 static void disable_odiv(uintptr_t pll_addr, uint32_t div_index)
249 {
250 	mmio_clrbits_32(PLLDIG_PLLODIV(pll_addr, div_index), PLLDIG_PLLODIV_DE);
251 }
252 
253 static void enable_odiv(uintptr_t pll_addr, uint32_t div_index)
254 {
255 	mmio_setbits_32(PLLDIG_PLLODIV(pll_addr, div_index), PLLDIG_PLLODIV_DE);
256 }
257 
258 static void disable_odivs(uintptr_t pll_addr, uint32_t ndivs)
259 {
260 	uint32_t i;
261 
262 	for (i = 0; i < ndivs; i++) {
263 		disable_odiv(pll_addr, i);
264 	}
265 }
266 
267 static void enable_pll_hw(uintptr_t pll_addr)
268 {
269 	/* Enable the PLL. */
270 	mmio_write_32(PLLDIG_PLLCR(pll_addr), 0x0);
271 
272 	/* Poll until PLL acquires lock. */
273 	while ((mmio_read_32(PLLDIG_PLLSR(pll_addr)) & PLLDIG_PLLSR_LOCK) == 0U) {
274 	}
275 }
276 
277 static void disable_pll_hw(uintptr_t pll_addr)
278 {
279 	mmio_write_32(PLLDIG_PLLCR(pll_addr), PLLDIG_PLLCR_PLLPD);
280 }
281 
282 static int program_pll(const struct s32cc_pll *pll, uintptr_t pll_addr,
283 		       const struct s32cc_clk_drv *drv, uint32_t sclk_id,
284 		       unsigned long sclk_freq)
285 {
286 	uint32_t rdiv = 1, mfi, mfn;
287 	int ret;
288 
289 	ret = get_pll_mfi_mfn(pll->vco_freq, sclk_freq, &mfi, &mfn);
290 	if (ret != 0) {
291 		return -EINVAL;
292 	}
293 
294 	/* Disable ODIVs*/
295 	disable_odivs(pll_addr, pll->ndividers);
296 
297 	/* Disable PLL */
298 	disable_pll_hw(pll_addr);
299 
300 	/* Program PLLCLKMUX */
301 	mmio_write_32(PLLDIG_PLLCLKMUX(pll_addr), sclk_id);
302 
303 	/* Program VCO */
304 	mmio_clrsetbits_32(PLLDIG_PLLDV(pll_addr),
305 			   PLLDIG_PLLDV_RDIV_MASK | PLLDIG_PLLDV_MFI_MASK,
306 			   PLLDIG_PLLDV_RDIV_SET(rdiv) | PLLDIG_PLLDV_MFI(mfi));
307 
308 	mmio_write_32(PLLDIG_PLLFD(pll_addr),
309 		      PLLDIG_PLLFD_MFN_SET(mfn) | PLLDIG_PLLFD_SMDEN);
310 
311 	enable_pll_hw(pll_addr);
312 
313 	return ret;
314 }
315 
316 static int enable_pll(const struct s32cc_clk_obj *module,
317 		      const struct s32cc_clk_drv *drv,
318 		      unsigned int *depth)
319 {
320 	const struct s32cc_pll *pll = s32cc_obj2pll(module);
321 	const struct s32cc_clkmux *mux;
322 	uintptr_t pll_addr = UL(0x0);
323 	unsigned long sclk_freq;
324 	uint32_t sclk_id;
325 	int ret;
326 
327 	ret = update_stack_depth(depth);
328 	if (ret != 0) {
329 		return ret;
330 	}
331 
332 	mux = get_pll_mux(pll);
333 	if (mux == NULL) {
334 		return -EINVAL;
335 	}
336 
337 	if (pll->instance != mux->module) {
338 		ERROR("MUX type is not in sync with PLL ID\n");
339 		return -EINVAL;
340 	}
341 
342 	ret = get_base_addr(pll->instance, drv, &pll_addr);
343 	if (ret != 0) {
344 		ERROR("Failed to detect PLL instance\n");
345 		return ret;
346 	}
347 
348 	switch (mux->source_id) {
349 	case S32CC_CLK_FIRC:
350 		sclk_freq = 48U * MHZ;
351 		sclk_id = 0;
352 		break;
353 	case S32CC_CLK_FXOSC:
354 		sclk_freq = 40U * MHZ;
355 		sclk_id = 1;
356 		break;
357 	default:
358 		ERROR("Invalid source selection for PLL 0x%lx\n",
359 		      pll_addr);
360 		return -EINVAL;
361 	};
362 
363 	return program_pll(pll, pll_addr, drv, sclk_id, sclk_freq);
364 }
365 
366 static inline struct s32cc_pll *get_div_pll(const struct s32cc_pll_out_div *pdiv)
367 {
368 	const struct s32cc_clk_obj *parent;
369 
370 	parent = pdiv->parent;
371 	if (parent == NULL) {
372 		ERROR("Failed to identify PLL divider's parent\n");
373 		return NULL;
374 	}
375 
376 	if (parent->type != s32cc_pll_t) {
377 		ERROR("The parent of the divider is not a PLL instance\n");
378 		return NULL;
379 	}
380 
381 	return s32cc_obj2pll(parent);
382 }
383 
384 static void config_pll_out_div(uintptr_t pll_addr, uint32_t div_index, uint32_t dc)
385 {
386 	uint32_t pllodiv;
387 	uint32_t pdiv;
388 
389 	pllodiv = mmio_read_32(PLLDIG_PLLODIV(pll_addr, div_index));
390 	pdiv = PLLDIG_PLLODIV_DIV(pllodiv);
391 
392 	if (((pdiv + 1U) == dc) && ((pllodiv & PLLDIG_PLLODIV_DE) != 0U)) {
393 		return;
394 	}
395 
396 	if ((pllodiv & PLLDIG_PLLODIV_DE) != 0U) {
397 		disable_odiv(pll_addr, div_index);
398 	}
399 
400 	pllodiv = PLLDIG_PLLODIV_DIV_SET(dc - 1U);
401 	mmio_write_32(PLLDIG_PLLODIV(pll_addr, div_index), pllodiv);
402 
403 	enable_odiv(pll_addr, div_index);
404 }
405 
406 static int enable_pll_div(const struct s32cc_clk_obj *module,
407 			  const struct s32cc_clk_drv *drv,
408 			  unsigned int *depth)
409 {
410 	const struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module);
411 	uintptr_t pll_addr = 0x0ULL;
412 	const struct s32cc_pll *pll;
413 	uint32_t dc;
414 	int ret;
415 
416 	ret = update_stack_depth(depth);
417 	if (ret != 0) {
418 		return ret;
419 	}
420 
421 	pll = get_div_pll(pdiv);
422 	if (pll == NULL) {
423 		ERROR("The parent of the PLL DIV is invalid\n");
424 		return 0;
425 	}
426 
427 	ret = get_base_addr(pll->instance, drv, &pll_addr);
428 	if (ret != 0) {
429 		ERROR("Failed to detect PLL instance\n");
430 		return -EINVAL;
431 	}
432 
433 	dc = (uint32_t)(pll->vco_freq / pdiv->freq);
434 
435 	config_pll_out_div(pll_addr, pdiv->index, dc);
436 
437 	return 0;
438 }
439 
440 static int cgm_mux_clk_config(uintptr_t cgm_addr, uint32_t mux, uint32_t source,
441 			      bool safe_clk)
442 {
443 	uint32_t css, csc;
444 
445 	css = mmio_read_32(CGM_MUXn_CSS(cgm_addr, mux));
446 
447 	/* Already configured */
448 	if ((MC_CGM_MUXn_CSS_SELSTAT(css) == source) &&
449 	    (MC_CGM_MUXn_CSS_SWTRG(css) == MC_CGM_MUXn_CSS_SWTRG_SUCCESS) &&
450 	    ((css & MC_CGM_MUXn_CSS_SWIP) == 0U) && !safe_clk) {
451 		return 0;
452 	}
453 
454 	/* Ongoing clock switch? */
455 	while ((mmio_read_32(CGM_MUXn_CSS(cgm_addr, mux)) &
456 		MC_CGM_MUXn_CSS_SWIP) != 0U) {
457 	}
458 
459 	csc = mmio_read_32(CGM_MUXn_CSC(cgm_addr, mux));
460 
461 	/* Clear previous source. */
462 	csc &= ~(MC_CGM_MUXn_CSC_SELCTL_MASK);
463 
464 	if (!safe_clk) {
465 		/* Select the clock source and trigger the clock switch. */
466 		csc |= MC_CGM_MUXn_CSC_SELCTL(source) | MC_CGM_MUXn_CSC_CLK_SW;
467 	} else {
468 		/* Switch to safe clock */
469 		csc |= MC_CGM_MUXn_CSC_SAFE_SW;
470 	}
471 
472 	mmio_write_32(CGM_MUXn_CSC(cgm_addr, mux), csc);
473 
474 	/* Wait for configuration bit to auto-clear. */
475 	while ((mmio_read_32(CGM_MUXn_CSC(cgm_addr, mux)) &
476 		MC_CGM_MUXn_CSC_CLK_SW) != 0U) {
477 	}
478 
479 	/* Is the clock switch completed? */
480 	while ((mmio_read_32(CGM_MUXn_CSS(cgm_addr, mux)) &
481 		MC_CGM_MUXn_CSS_SWIP) != 0U) {
482 	}
483 
484 	/*
485 	 * Check if the switch succeeded.
486 	 * Check switch trigger cause and the source.
487 	 */
488 	css = mmio_read_32(CGM_MUXn_CSS(cgm_addr, mux));
489 	if (!safe_clk) {
490 		if ((MC_CGM_MUXn_CSS_SWTRG(css) == MC_CGM_MUXn_CSS_SWTRG_SUCCESS) &&
491 		    (MC_CGM_MUXn_CSS_SELSTAT(css) == source)) {
492 			return 0;
493 		}
494 
495 		ERROR("Failed to change the source of mux %" PRIu32 " to %" PRIu32 " (CGM=%lu)\n",
496 		      mux, source, cgm_addr);
497 	} else {
498 		if (((MC_CGM_MUXn_CSS_SWTRG(css) == MC_CGM_MUXn_CSS_SWTRG_SAFE_CLK) ||
499 		     (MC_CGM_MUXn_CSS_SWTRG(css) == MC_CGM_MUXn_CSS_SWTRG_SAFE_CLK_INACTIVE)) &&
500 		     ((MC_CGM_MUXn_CSS_SAFE_SW & css) != 0U)) {
501 			return 0;
502 		}
503 
504 		ERROR("The switch of mux %" PRIu32 " (CGM=%lu) to safe clock failed\n",
505 		      mux, cgm_addr);
506 	}
507 
508 	return -EINVAL;
509 }
510 
511 static int enable_cgm_mux(const struct s32cc_clkmux *mux,
512 			  const struct s32cc_clk_drv *drv)
513 {
514 	uintptr_t cgm_addr = UL(0x0);
515 	uint32_t mux_hw_clk;
516 	int ret;
517 
518 	ret = get_base_addr(mux->module, drv, &cgm_addr);
519 	if (ret != 0) {
520 		return ret;
521 	}
522 
523 	mux_hw_clk = (uint32_t)S32CC_CLK_ID(mux->source_id);
524 
525 	return cgm_mux_clk_config(cgm_addr, mux->index,
526 				  mux_hw_clk, false);
527 }
528 
529 static int enable_mux(const struct s32cc_clk_obj *module,
530 		      const struct s32cc_clk_drv *drv,
531 		      unsigned int *depth)
532 {
533 	const struct s32cc_clkmux *mux = s32cc_obj2clkmux(module);
534 	const struct s32cc_clk *clk;
535 	int ret = 0;
536 
537 	ret = update_stack_depth(depth);
538 	if (ret != 0) {
539 		return ret;
540 	}
541 
542 	if (mux == NULL) {
543 		return -EINVAL;
544 	}
545 
546 	clk = s32cc_get_arch_clk(mux->source_id);
547 	if (clk == NULL) {
548 		ERROR("Invalid parent (%lu) for mux %" PRIu8 "\n",
549 		      mux->source_id, mux->index);
550 		return -EINVAL;
551 	}
552 
553 	switch (mux->module) {
554 	/* PLL mux will be enabled by PLL setup */
555 	case S32CC_ARM_PLL:
556 	case S32CC_PERIPH_PLL:
557 		break;
558 	case S32CC_CGM1:
559 		ret = enable_cgm_mux(mux, drv);
560 		break;
561 	case S32CC_CGM0:
562 		ret = enable_cgm_mux(mux, drv);
563 		break;
564 	default:
565 		ERROR("Unknown mux parent type: %d\n", mux->module);
566 		ret = -EINVAL;
567 		break;
568 	};
569 
570 	return ret;
571 }
572 
573 static int enable_dfs(const struct s32cc_clk_obj *module,
574 		      const struct s32cc_clk_drv *drv,
575 		      unsigned int *depth)
576 {
577 	int ret = 0;
578 
579 	ret = update_stack_depth(depth);
580 	if (ret != 0) {
581 		return ret;
582 	}
583 
584 	return 0;
585 }
586 
587 static struct s32cc_dfs *get_div_dfs(const struct s32cc_dfs_div *dfs_div)
588 {
589 	const struct s32cc_clk_obj *parent = dfs_div->parent;
590 
591 	if (parent->type != s32cc_dfs_t) {
592 		ERROR("DFS DIV doesn't have a DFS as parent\n");
593 		return NULL;
594 	}
595 
596 	return s32cc_obj2dfs(parent);
597 }
598 
599 static struct s32cc_pll *dfsdiv2pll(const struct s32cc_dfs_div *dfs_div)
600 {
601 	const struct s32cc_clk_obj *parent;
602 	const struct s32cc_dfs *dfs;
603 
604 	dfs = get_div_dfs(dfs_div);
605 	if (dfs == NULL) {
606 		return NULL;
607 	}
608 
609 	parent = dfs->parent;
610 	if (parent->type != s32cc_pll_t) {
611 		return NULL;
612 	}
613 
614 	return s32cc_obj2pll(parent);
615 }
616 
617 static int get_dfs_mfi_mfn(unsigned long dfs_freq, const struct s32cc_dfs_div *dfs_div,
618 			   uint32_t *mfi, uint32_t *mfn)
619 {
620 	uint64_t factor64, tmp64, ofreq;
621 	uint32_t factor32;
622 
623 	unsigned long in = dfs_freq;
624 	unsigned long out = dfs_div->freq;
625 
626 	/**
627 	 * factor = (IN / OUT) / 2
628 	 * MFI = integer(factor)
629 	 * MFN = (factor - MFI) * 36
630 	 */
631 	factor64 = ((((uint64_t)in) * FP_PRECISION) / ((uint64_t)out)) / 2ULL;
632 	tmp64 = factor64 / FP_PRECISION;
633 	if (tmp64 > UINT32_MAX) {
634 		return -EINVAL;
635 	}
636 
637 	factor32 = (uint32_t)tmp64;
638 	*mfi = factor32;
639 
640 	tmp64 = ((factor64 - ((uint64_t)*mfi * FP_PRECISION)) * 36UL) / FP_PRECISION;
641 	if (tmp64 > UINT32_MAX) {
642 		return -EINVAL;
643 	}
644 
645 	*mfn = (uint32_t)tmp64;
646 
647 	/* div_freq = in / (2 * (*mfi + *mfn / 36.0)) */
648 	factor64 = (((uint64_t)*mfn) * FP_PRECISION) / 36ULL;
649 	factor64 += ((uint64_t)*mfi) * FP_PRECISION;
650 	factor64 *= 2ULL;
651 	ofreq = (((uint64_t)in) * FP_PRECISION) / factor64;
652 
653 	if (ofreq != dfs_div->freq) {
654 		ERROR("Failed to find MFI and MFN settings for DFS DIV freq %lu\n",
655 		      dfs_div->freq);
656 		ERROR("Nearest freq = %" PRIx64 "\n", ofreq);
657 		return -EINVAL;
658 	}
659 
660 	return 0;
661 }
662 
663 static int init_dfs_port(uintptr_t dfs_addr, uint32_t port,
664 			 uint32_t mfi, uint32_t mfn)
665 {
666 	uint32_t portsr, portolsr;
667 	uint32_t mask, old_mfi, old_mfn;
668 	uint32_t dvport;
669 	bool init_dfs;
670 
671 	dvport = mmio_read_32(DFS_DVPORTn(dfs_addr, port));
672 
673 	old_mfi = DFS_DVPORTn_MFI(dvport);
674 	old_mfn = DFS_DVPORTn_MFN(dvport);
675 
676 	portsr = mmio_read_32(DFS_PORTSR(dfs_addr));
677 	portolsr = mmio_read_32(DFS_PORTOLSR(dfs_addr));
678 
679 	/* Skip configuration if it's not needed */
680 	if (((portsr & BIT_32(port)) != 0U) &&
681 	    ((portolsr & BIT_32(port)) == 0U) &&
682 	    (mfi == old_mfi) && (mfn == old_mfn)) {
683 		return 0;
684 	}
685 
686 	init_dfs = (portsr == 0U);
687 
688 	if (init_dfs) {
689 		mask = DFS_PORTRESET_MASK;
690 	} else {
691 		mask = DFS_PORTRESET_SET(BIT_32(port));
692 	}
693 
694 	mmio_write_32(DFS_PORTOLSR(dfs_addr), mask);
695 	mmio_write_32(DFS_PORTRESET(dfs_addr), mask);
696 
697 	while ((mmio_read_32(DFS_PORTSR(dfs_addr)) & mask) != 0U) {
698 	}
699 
700 	if (init_dfs) {
701 		mmio_write_32(DFS_CTL(dfs_addr), DFS_CTL_RESET);
702 	}
703 
704 	mmio_write_32(DFS_DVPORTn(dfs_addr, port),
705 		      DFS_DVPORTn_MFI_SET(mfi) | DFS_DVPORTn_MFN_SET(mfn));
706 
707 	if (init_dfs) {
708 		/* DFS clk enable programming */
709 		mmio_clrbits_32(DFS_CTL(dfs_addr), DFS_CTL_RESET);
710 	}
711 
712 	mmio_clrbits_32(DFS_PORTRESET(dfs_addr), BIT_32(port));
713 
714 	while ((mmio_read_32(DFS_PORTSR(dfs_addr)) & BIT_32(port)) != BIT_32(port)) {
715 	}
716 
717 	portolsr = mmio_read_32(DFS_PORTOLSR(dfs_addr));
718 	if ((portolsr & DFS_PORTOLSR_LOL(port)) != 0U) {
719 		ERROR("Failed to lock DFS divider\n");
720 		return -EINVAL;
721 	}
722 
723 	return 0;
724 }
725 
726 static int enable_dfs_div(const struct s32cc_clk_obj *module,
727 			  const struct s32cc_clk_drv *drv,
728 			  unsigned int *depth)
729 {
730 	const struct s32cc_dfs_div *dfs_div = s32cc_obj2dfsdiv(module);
731 	const struct s32cc_pll *pll;
732 	const struct s32cc_dfs *dfs;
733 	uintptr_t dfs_addr = 0UL;
734 	uint32_t mfi, mfn;
735 	int ret = 0;
736 
737 	ret = update_stack_depth(depth);
738 	if (ret != 0) {
739 		return ret;
740 	}
741 
742 	dfs = get_div_dfs(dfs_div);
743 	if (dfs == NULL) {
744 		return -EINVAL;
745 	}
746 
747 	pll = dfsdiv2pll(dfs_div);
748 	if (pll == NULL) {
749 		ERROR("Failed to identify DFS divider's parent\n");
750 		return -EINVAL;
751 	}
752 
753 	ret = get_base_addr(dfs->instance, drv, &dfs_addr);
754 	if ((ret != 0) || (dfs_addr == 0UL)) {
755 		return -EINVAL;
756 	}
757 
758 	ret = get_dfs_mfi_mfn(pll->vco_freq, dfs_div, &mfi, &mfn);
759 	if (ret != 0) {
760 		return -EINVAL;
761 	}
762 
763 	return init_dfs_port(dfs_addr, dfs_div->index, mfi, mfn);
764 }
765 
766 static int enable_module(const struct s32cc_clk_obj *module, unsigned int *depth)
767 {
768 	const struct s32cc_clk_drv *drv = get_drv();
769 	int ret = 0;
770 
771 	ret = update_stack_depth(depth);
772 	if (ret != 0) {
773 		return ret;
774 	}
775 
776 	if (drv == NULL) {
777 		return -EINVAL;
778 	}
779 
780 	switch (module->type) {
781 	case s32cc_osc_t:
782 		ret = enable_osc(module, drv, depth);
783 		break;
784 	case s32cc_clk_t:
785 		ret = enable_clk_module(module, drv, depth);
786 		break;
787 	case s32cc_pll_t:
788 		ret = enable_pll(module, drv, depth);
789 		break;
790 	case s32cc_pll_out_div_t:
791 		ret = enable_pll_div(module, drv, depth);
792 		break;
793 	case s32cc_clkmux_t:
794 		ret = enable_mux(module, drv, depth);
795 		break;
796 	case s32cc_shared_clkmux_t:
797 		ret = enable_mux(module, drv, depth);
798 		break;
799 	case s32cc_fixed_div_t:
800 		ret = -ENOTSUP;
801 		break;
802 	case s32cc_dfs_t:
803 		ret = enable_dfs(module, drv, depth);
804 		break;
805 	case s32cc_dfs_div_t:
806 		ret = enable_dfs_div(module, drv, depth);
807 		break;
808 	default:
809 		ret = -EINVAL;
810 		break;
811 	}
812 
813 	return ret;
814 }
815 
816 static int s32cc_clk_enable(unsigned long id)
817 {
818 	unsigned int depth = MAX_STACK_DEPTH;
819 	const struct s32cc_clk *clk;
820 
821 	clk = s32cc_get_arch_clk(id);
822 	if (clk == NULL) {
823 		return -EINVAL;
824 	}
825 
826 	return enable_module(&clk->desc, &depth);
827 }
828 
829 static void s32cc_clk_disable(unsigned long id)
830 {
831 }
832 
833 static bool s32cc_clk_is_enabled(unsigned long id)
834 {
835 	return false;
836 }
837 
838 static unsigned long s32cc_clk_get_rate(unsigned long id)
839 {
840 	return 0;
841 }
842 
843 static int set_module_rate(const struct s32cc_clk_obj *module,
844 			   unsigned long rate, unsigned long *orate,
845 			   unsigned int *depth);
846 
847 static int set_osc_freq(const struct s32cc_clk_obj *module, unsigned long rate,
848 			unsigned long *orate, unsigned int *depth)
849 {
850 	struct s32cc_osc *osc = s32cc_obj2osc(module);
851 	int ret;
852 
853 	ret = update_stack_depth(depth);
854 	if (ret != 0) {
855 		return ret;
856 	}
857 
858 	if ((osc->freq != 0UL) && (rate != osc->freq)) {
859 		ERROR("Already initialized oscillator. freq = %lu\n",
860 		      osc->freq);
861 		return -EINVAL;
862 	}
863 
864 	osc->freq = rate;
865 	*orate = osc->freq;
866 
867 	return 0;
868 }
869 
870 static int set_clk_freq(const struct s32cc_clk_obj *module, unsigned long rate,
871 			unsigned long *orate, unsigned int *depth)
872 {
873 	const struct s32cc_clk *clk = s32cc_obj2clk(module);
874 	int ret;
875 
876 	ret = update_stack_depth(depth);
877 	if (ret != 0) {
878 		return ret;
879 	}
880 
881 	if ((clk->min_freq != 0UL) && (clk->max_freq != 0UL) &&
882 	    ((rate < clk->min_freq) || (rate > clk->max_freq))) {
883 		ERROR("%lu frequency is out of the allowed range: [%lu:%lu]\n",
884 		      rate, clk->min_freq, clk->max_freq);
885 		return -EINVAL;
886 	}
887 
888 	if (clk->module != NULL) {
889 		return set_module_rate(clk->module, rate, orate, depth);
890 	}
891 
892 	if (clk->pclock != NULL) {
893 		return set_clk_freq(&clk->pclock->desc, rate, orate, depth);
894 	}
895 
896 	return -EINVAL;
897 }
898 
899 static int set_pll_freq(const struct s32cc_clk_obj *module, unsigned long rate,
900 			unsigned long *orate, unsigned int *depth)
901 {
902 	struct s32cc_pll *pll = s32cc_obj2pll(module);
903 	int ret;
904 
905 	ret = update_stack_depth(depth);
906 	if (ret != 0) {
907 		return ret;
908 	}
909 
910 	if ((pll->vco_freq != 0UL) && (pll->vco_freq != rate)) {
911 		ERROR("PLL frequency was already set\n");
912 		return -EINVAL;
913 	}
914 
915 	pll->vco_freq = rate;
916 	*orate = pll->vco_freq;
917 
918 	return 0;
919 }
920 
921 static int set_pll_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
922 			    unsigned long *orate, unsigned int *depth)
923 {
924 	struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module);
925 	const struct s32cc_pll *pll;
926 	unsigned long prate, dc;
927 	int ret;
928 
929 	ret = update_stack_depth(depth);
930 	if (ret != 0) {
931 		return ret;
932 	}
933 
934 	if (pdiv->parent == NULL) {
935 		ERROR("Failed to identify PLL divider's parent\n");
936 		return -EINVAL;
937 	}
938 
939 	pll = s32cc_obj2pll(pdiv->parent);
940 	if (pll == NULL) {
941 		ERROR("The parent of the PLL DIV is invalid\n");
942 		return -EINVAL;
943 	}
944 
945 	prate = pll->vco_freq;
946 
947 	/**
948 	 * The PLL is not initialized yet, so let's take a risk
949 	 * and accept the proposed rate.
950 	 */
951 	if (prate == 0UL) {
952 		pdiv->freq = rate;
953 		*orate = rate;
954 		return 0;
955 	}
956 
957 	/* Decline in case the rate cannot fit PLL's requirements. */
958 	dc = prate / rate;
959 	if ((prate / dc) != rate) {
960 		return -EINVAL;
961 	}
962 
963 	pdiv->freq = rate;
964 	*orate = pdiv->freq;
965 
966 	return 0;
967 }
968 
969 static int set_fixed_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
970 			      unsigned long *orate, unsigned int *depth)
971 {
972 	const struct s32cc_fixed_div *fdiv = s32cc_obj2fixeddiv(module);
973 	int ret;
974 
975 	ret = update_stack_depth(depth);
976 	if (ret != 0) {
977 		return ret;
978 	}
979 
980 	if (fdiv->parent == NULL) {
981 		ERROR("The divider doesn't have a valid parent\b");
982 		return -EINVAL;
983 	}
984 
985 	ret = set_module_rate(fdiv->parent, rate * fdiv->rate_div, orate, depth);
986 
987 	/* Update the output rate based on the parent's rate */
988 	*orate /= fdiv->rate_div;
989 
990 	return ret;
991 }
992 
993 static int set_mux_freq(const struct s32cc_clk_obj *module, unsigned long rate,
994 			unsigned long *orate, unsigned int *depth)
995 {
996 	const struct s32cc_clkmux *mux = s32cc_obj2clkmux(module);
997 	const struct s32cc_clk *clk = s32cc_get_arch_clk(mux->source_id);
998 	int ret;
999 
1000 	ret = update_stack_depth(depth);
1001 	if (ret != 0) {
1002 		return ret;
1003 	}
1004 
1005 	if (clk == NULL) {
1006 		ERROR("Mux (id:%" PRIu8 ") without a valid source (%lu)\n",
1007 		      mux->index, mux->source_id);
1008 		return -EINVAL;
1009 	}
1010 
1011 	return set_module_rate(&clk->desc, rate, orate, depth);
1012 }
1013 
1014 static int set_dfs_div_freq(const struct s32cc_clk_obj *module, unsigned long rate,
1015 			    unsigned long *orate, unsigned int *depth)
1016 {
1017 	struct s32cc_dfs_div *dfs_div = s32cc_obj2dfsdiv(module);
1018 	const struct s32cc_dfs *dfs;
1019 	int ret;
1020 
1021 	ret = update_stack_depth(depth);
1022 	if (ret != 0) {
1023 		return ret;
1024 	}
1025 
1026 	if (dfs_div->parent == NULL) {
1027 		ERROR("Failed to identify DFS divider's parent\n");
1028 		return -EINVAL;
1029 	}
1030 
1031 	/* Sanity check */
1032 	dfs = s32cc_obj2dfs(dfs_div->parent);
1033 	if (dfs->parent == NULL) {
1034 		ERROR("Failed to identify DFS's parent\n");
1035 		return -EINVAL;
1036 	}
1037 
1038 	if ((dfs_div->freq != 0U) && (dfs_div->freq != rate)) {
1039 		ERROR("DFS DIV frequency was already set to %lu\n",
1040 		      dfs_div->freq);
1041 		return -EINVAL;
1042 	}
1043 
1044 	dfs_div->freq = rate;
1045 	*orate = rate;
1046 
1047 	return ret;
1048 }
1049 
1050 static int set_module_rate(const struct s32cc_clk_obj *module,
1051 			   unsigned long rate, unsigned long *orate,
1052 			   unsigned int *depth)
1053 {
1054 	int ret = 0;
1055 
1056 	ret = update_stack_depth(depth);
1057 	if (ret != 0) {
1058 		return ret;
1059 	}
1060 
1061 	ret = -EINVAL;
1062 
1063 	switch (module->type) {
1064 	case s32cc_clk_t:
1065 		ret = set_clk_freq(module, rate, orate, depth);
1066 		break;
1067 	case s32cc_osc_t:
1068 		ret = set_osc_freq(module, rate, orate, depth);
1069 		break;
1070 	case s32cc_pll_t:
1071 		ret = set_pll_freq(module, rate, orate, depth);
1072 		break;
1073 	case s32cc_pll_out_div_t:
1074 		ret = set_pll_div_freq(module, rate, orate, depth);
1075 		break;
1076 	case s32cc_fixed_div_t:
1077 		ret = set_fixed_div_freq(module, rate, orate, depth);
1078 		break;
1079 	case s32cc_clkmux_t:
1080 		ret = set_mux_freq(module, rate, orate, depth);
1081 		break;
1082 	case s32cc_shared_clkmux_t:
1083 		ret = set_mux_freq(module, rate, orate, depth);
1084 		break;
1085 	case s32cc_dfs_t:
1086 		ERROR("Setting the frequency of a DFS is not allowed!");
1087 		break;
1088 	case s32cc_dfs_div_t:
1089 		ret = set_dfs_div_freq(module, rate, orate, depth);
1090 		break;
1091 	default:
1092 		break;
1093 	}
1094 
1095 	return ret;
1096 }
1097 
1098 static int s32cc_clk_set_rate(unsigned long id, unsigned long rate,
1099 			      unsigned long *orate)
1100 {
1101 	unsigned int depth = MAX_STACK_DEPTH;
1102 	const struct s32cc_clk *clk;
1103 	int ret;
1104 
1105 	clk = s32cc_get_arch_clk(id);
1106 	if (clk == NULL) {
1107 		return -EINVAL;
1108 	}
1109 
1110 	ret = set_module_rate(&clk->desc, rate, orate, &depth);
1111 	if (ret != 0) {
1112 		ERROR("Failed to set frequency (%lu MHz) for clock %lu\n",
1113 		      rate, id);
1114 	}
1115 
1116 	return ret;
1117 }
1118 
1119 static int s32cc_clk_get_parent(unsigned long id)
1120 {
1121 	return -ENOTSUP;
1122 }
1123 
1124 static int s32cc_clk_set_parent(unsigned long id, unsigned long parent_id)
1125 {
1126 	const struct s32cc_clk *parent;
1127 	const struct s32cc_clk *clk;
1128 	bool valid_source = false;
1129 	struct s32cc_clkmux *mux;
1130 	uint8_t i;
1131 
1132 	clk = s32cc_get_arch_clk(id);
1133 	if (clk == NULL) {
1134 		return -EINVAL;
1135 	}
1136 
1137 	parent = s32cc_get_arch_clk(parent_id);
1138 	if (parent == NULL) {
1139 		return -EINVAL;
1140 	}
1141 
1142 	if (!is_s32cc_clk_mux(clk)) {
1143 		ERROR("Clock %lu is not a mux\n", id);
1144 		return -EINVAL;
1145 	}
1146 
1147 	mux = s32cc_clk2mux(clk);
1148 	if (mux == NULL) {
1149 		ERROR("Failed to cast clock %lu to clock mux\n", id);
1150 		return -EINVAL;
1151 	}
1152 
1153 	for (i = 0; i < mux->nclks; i++) {
1154 		if (mux->clkids[i] == parent_id) {
1155 			valid_source = true;
1156 			break;
1157 		}
1158 	}
1159 
1160 	if (!valid_source) {
1161 		ERROR("Clock %lu is not a valid clock for mux %lu\n",
1162 		      parent_id, id);
1163 		return -EINVAL;
1164 	}
1165 
1166 	mux->source_id = parent_id;
1167 
1168 	return 0;
1169 }
1170 
1171 void s32cc_clk_register_drv(void)
1172 {
1173 	static const struct clk_ops s32cc_clk_ops = {
1174 		.enable		= s32cc_clk_enable,
1175 		.disable	= s32cc_clk_disable,
1176 		.is_enabled	= s32cc_clk_is_enabled,
1177 		.get_rate	= s32cc_clk_get_rate,
1178 		.set_rate	= s32cc_clk_set_rate,
1179 		.get_parent	= s32cc_clk_get_parent,
1180 		.set_parent	= s32cc_clk_set_parent,
1181 	};
1182 
1183 	clk_register(&s32cc_clk_ops);
1184 }
1185 
1186