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