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