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