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