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