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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 372 static void disable_pll_hw(uintptr_t pll_addr) 373 { 374 mmio_write_32(PLLDIG_PLLCR(pll_addr), PLLDIG_PLLCR_PLLPD); 375 } 376 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 * 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 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 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 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 * 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 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 * 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 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 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 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 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 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 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 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 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 * 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 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 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 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 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 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 1443 static void s32cc_clk_disable(unsigned long id) 1444 { 1445 } 1446 1447 static bool s32cc_clk_is_enabled(unsigned long id) 1448 { 1449 return false; 1450 } 1451 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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