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 }; 26 27 static int update_stack_depth(unsigned int *depth) 28 { 29 if (*depth == 0U) { 30 return -ENOMEM; 31 } 32 33 (*depth)--; 34 return 0; 35 } 36 37 static struct s32cc_clk_drv *get_drv(void) 38 { 39 static struct s32cc_clk_drv driver = { 40 .fxosc_base = FXOSC_BASE_ADDR, 41 .armpll_base = ARMPLL_BASE_ADDR, 42 }; 43 44 return &driver; 45 } 46 47 static int enable_module(const struct s32cc_clk_obj *module, unsigned int *depth); 48 49 static int enable_clk_module(const struct s32cc_clk_obj *module, 50 const struct s32cc_clk_drv *drv, 51 unsigned int *depth) 52 { 53 const struct s32cc_clk *clk = s32cc_obj2clk(module); 54 int ret; 55 56 ret = update_stack_depth(depth); 57 if (ret != 0) { 58 return ret; 59 } 60 61 if (clk == NULL) { 62 return -EINVAL; 63 } 64 65 if (clk->module != NULL) { 66 return enable_module(clk->module, depth); 67 } 68 69 if (clk->pclock != NULL) { 70 return enable_clk_module(&clk->pclock->desc, drv, depth); 71 } 72 73 return -EINVAL; 74 } 75 76 static int get_base_addr(enum s32cc_clk_source id, const struct s32cc_clk_drv *drv, 77 uintptr_t *base) 78 { 79 int ret = 0; 80 81 switch (id) { 82 case S32CC_FXOSC: 83 *base = drv->fxosc_base; 84 break; 85 case S32CC_ARM_PLL: 86 *base = drv->armpll_base; 87 break; 88 case S32CC_CGM1: 89 ret = -ENOTSUP; 90 break; 91 case S32CC_FIRC: 92 break; 93 case S32CC_SIRC: 94 break; 95 default: 96 ret = -EINVAL; 97 break; 98 } 99 100 if (ret != 0) { 101 ERROR("Unknown clock source id: %u\n", id); 102 } 103 104 return ret; 105 } 106 107 static void enable_fxosc(const struct s32cc_clk_drv *drv) 108 { 109 uintptr_t fxosc_base = drv->fxosc_base; 110 uint32_t ctrl; 111 112 ctrl = mmio_read_32(FXOSC_CTRL(fxosc_base)); 113 if ((ctrl & FXOSC_CTRL_OSCON) != U(0)) { 114 return; 115 } 116 117 ctrl = FXOSC_CTRL_COMP_EN; 118 ctrl &= ~FXOSC_CTRL_OSC_BYP; 119 ctrl |= FXOSC_CTRL_EOCV(0x1); 120 ctrl |= FXOSC_CTRL_GM_SEL(0x7); 121 mmio_write_32(FXOSC_CTRL(fxosc_base), ctrl); 122 123 /* Switch ON the crystal oscillator. */ 124 mmio_setbits_32(FXOSC_CTRL(fxosc_base), FXOSC_CTRL_OSCON); 125 126 /* Wait until the clock is stable. */ 127 while ((mmio_read_32(FXOSC_STAT(fxosc_base)) & FXOSC_STAT_OSC_STAT) == U(0)) { 128 } 129 } 130 131 static int enable_osc(const struct s32cc_clk_obj *module, 132 const struct s32cc_clk_drv *drv, 133 unsigned int *depth) 134 { 135 const struct s32cc_osc *osc = s32cc_obj2osc(module); 136 int ret = 0; 137 138 ret = update_stack_depth(depth); 139 if (ret != 0) { 140 return ret; 141 } 142 143 switch (osc->source) { 144 case S32CC_FXOSC: 145 enable_fxosc(drv); 146 break; 147 /* FIRC and SIRC oscillators are enabled by default */ 148 case S32CC_FIRC: 149 break; 150 case S32CC_SIRC: 151 break; 152 default: 153 ERROR("Invalid oscillator %d\n", osc->source); 154 ret = -EINVAL; 155 break; 156 }; 157 158 return ret; 159 } 160 161 static int get_pll_mfi_mfn(unsigned long pll_vco, unsigned long ref_freq, 162 uint32_t *mfi, uint32_t *mfn) 163 164 { 165 unsigned long vco; 166 unsigned long mfn64; 167 168 /* FRAC-N mode */ 169 *mfi = (uint32_t)(pll_vco / ref_freq); 170 171 /* MFN formula : (double)(pll_vco % ref_freq) / ref_freq * 18432.0 */ 172 mfn64 = pll_vco % ref_freq; 173 mfn64 *= FP_PRECISION; 174 mfn64 /= ref_freq; 175 mfn64 *= 18432UL; 176 mfn64 /= FP_PRECISION; 177 178 if (mfn64 > UINT32_MAX) { 179 return -EINVAL; 180 } 181 182 *mfn = (uint32_t)mfn64; 183 184 vco = ((unsigned long)*mfn * FP_PRECISION) / 18432UL; 185 vco += (unsigned long)*mfi * FP_PRECISION; 186 vco *= ref_freq; 187 vco /= FP_PRECISION; 188 189 if (vco != pll_vco) { 190 ERROR("Failed to find MFI and MFN settings for PLL freq %lu. Nearest freq = %lu\n", 191 pll_vco, vco); 192 return -EINVAL; 193 } 194 195 return 0; 196 } 197 198 static struct s32cc_clkmux *get_pll_mux(const struct s32cc_pll *pll) 199 { 200 const struct s32cc_clk_obj *source = pll->source; 201 const struct s32cc_clk *clk; 202 203 if (source == NULL) { 204 ERROR("Failed to identify PLL's parent\n"); 205 return NULL; 206 } 207 208 if (source->type != s32cc_clk_t) { 209 ERROR("The parent of the PLL isn't a clock\n"); 210 return NULL; 211 } 212 213 clk = s32cc_obj2clk(source); 214 215 if (clk->module == NULL) { 216 ERROR("The clock isn't connected to a module\n"); 217 return NULL; 218 } 219 220 source = clk->module; 221 222 if ((source->type != s32cc_clkmux_t) && 223 (source->type != s32cc_shared_clkmux_t)) { 224 ERROR("The parent of the PLL isn't a MUX\n"); 225 return NULL; 226 } 227 228 return s32cc_obj2clkmux(source); 229 } 230 231 static void disable_odiv(uintptr_t pll_addr, uint32_t div_index) 232 { 233 mmio_clrbits_32(PLLDIG_PLLODIV(pll_addr, div_index), PLLDIG_PLLODIV_DE); 234 } 235 236 static void enable_odiv(uintptr_t pll_addr, uint32_t div_index) 237 { 238 mmio_setbits_32(PLLDIG_PLLODIV(pll_addr, div_index), PLLDIG_PLLODIV_DE); 239 } 240 241 static void disable_odivs(uintptr_t pll_addr, uint32_t ndivs) 242 { 243 uint32_t i; 244 245 for (i = 0; i < ndivs; i++) { 246 disable_odiv(pll_addr, i); 247 } 248 } 249 250 static void enable_pll_hw(uintptr_t pll_addr) 251 { 252 /* Enable the PLL. */ 253 mmio_write_32(PLLDIG_PLLCR(pll_addr), 0x0); 254 255 /* Poll until PLL acquires lock. */ 256 while ((mmio_read_32(PLLDIG_PLLSR(pll_addr)) & PLLDIG_PLLSR_LOCK) == 0U) { 257 } 258 } 259 260 static void disable_pll_hw(uintptr_t pll_addr) 261 { 262 mmio_write_32(PLLDIG_PLLCR(pll_addr), PLLDIG_PLLCR_PLLPD); 263 } 264 265 static int program_pll(const struct s32cc_pll *pll, uintptr_t pll_addr, 266 const struct s32cc_clk_drv *drv, uint32_t sclk_id, 267 unsigned long sclk_freq) 268 { 269 uint32_t rdiv = 1, mfi, mfn; 270 int ret; 271 272 ret = get_pll_mfi_mfn(pll->vco_freq, sclk_freq, &mfi, &mfn); 273 if (ret != 0) { 274 return -EINVAL; 275 } 276 277 /* Disable ODIVs*/ 278 disable_odivs(pll_addr, pll->ndividers); 279 280 /* Disable PLL */ 281 disable_pll_hw(pll_addr); 282 283 /* Program PLLCLKMUX */ 284 mmio_write_32(PLLDIG_PLLCLKMUX(pll_addr), sclk_id); 285 286 /* Program VCO */ 287 mmio_clrsetbits_32(PLLDIG_PLLDV(pll_addr), 288 PLLDIG_PLLDV_RDIV_MASK | PLLDIG_PLLDV_MFI_MASK, 289 PLLDIG_PLLDV_RDIV_SET(rdiv) | PLLDIG_PLLDV_MFI(mfi)); 290 291 mmio_write_32(PLLDIG_PLLFD(pll_addr), 292 PLLDIG_PLLFD_MFN_SET(mfn) | PLLDIG_PLLFD_SMDEN); 293 294 enable_pll_hw(pll_addr); 295 296 return ret; 297 } 298 299 static int enable_pll(const struct s32cc_clk_obj *module, 300 const struct s32cc_clk_drv *drv, 301 unsigned int *depth) 302 { 303 const struct s32cc_pll *pll = s32cc_obj2pll(module); 304 const struct s32cc_clkmux *mux; 305 uintptr_t pll_addr = UL(0x0); 306 unsigned long sclk_freq; 307 uint32_t sclk_id; 308 int ret; 309 310 ret = update_stack_depth(depth); 311 if (ret != 0) { 312 return ret; 313 } 314 315 mux = get_pll_mux(pll); 316 if (mux == NULL) { 317 return -EINVAL; 318 } 319 320 if (pll->instance != mux->module) { 321 ERROR("MUX type is not in sync with PLL ID\n"); 322 return -EINVAL; 323 } 324 325 ret = get_base_addr(pll->instance, drv, &pll_addr); 326 if (ret != 0) { 327 ERROR("Failed to detect PLL instance\n"); 328 return ret; 329 } 330 331 switch (mux->source_id) { 332 case S32CC_CLK_FIRC: 333 sclk_freq = 48U * MHZ; 334 sclk_id = 0; 335 break; 336 case S32CC_CLK_FXOSC: 337 sclk_freq = 40U * MHZ; 338 sclk_id = 1; 339 break; 340 default: 341 ERROR("Invalid source selection for PLL 0x%lx\n", 342 pll_addr); 343 return -EINVAL; 344 }; 345 346 return program_pll(pll, pll_addr, drv, sclk_id, sclk_freq); 347 } 348 349 static inline struct s32cc_pll *get_div_pll(const struct s32cc_pll_out_div *pdiv) 350 { 351 const struct s32cc_clk_obj *parent; 352 353 parent = pdiv->parent; 354 if (parent == NULL) { 355 ERROR("Failed to identify PLL divider's parent\n"); 356 return NULL; 357 } 358 359 if (parent->type != s32cc_pll_t) { 360 ERROR("The parent of the divider is not a PLL instance\n"); 361 return NULL; 362 } 363 364 return s32cc_obj2pll(parent); 365 } 366 367 static void config_pll_out_div(uintptr_t pll_addr, uint32_t div_index, uint32_t dc) 368 { 369 uint32_t pllodiv; 370 uint32_t pdiv; 371 372 pllodiv = mmio_read_32(PLLDIG_PLLODIV(pll_addr, div_index)); 373 pdiv = PLLDIG_PLLODIV_DIV(pllodiv); 374 375 if (((pdiv + 1U) == dc) && ((pllodiv & PLLDIG_PLLODIV_DE) != 0U)) { 376 return; 377 } 378 379 if ((pllodiv & PLLDIG_PLLODIV_DE) != 0U) { 380 disable_odiv(pll_addr, div_index); 381 } 382 383 pllodiv = PLLDIG_PLLODIV_DIV_SET(dc - 1U); 384 mmio_write_32(PLLDIG_PLLODIV(pll_addr, div_index), pllodiv); 385 386 enable_odiv(pll_addr, div_index); 387 } 388 389 static int enable_pll_div(const struct s32cc_clk_obj *module, 390 const struct s32cc_clk_drv *drv, 391 unsigned int *depth) 392 { 393 const struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module); 394 uintptr_t pll_addr = 0x0ULL; 395 const struct s32cc_pll *pll; 396 uint32_t dc; 397 int ret; 398 399 ret = update_stack_depth(depth); 400 if (ret != 0) { 401 return ret; 402 } 403 404 pll = get_div_pll(pdiv); 405 if (pll == NULL) { 406 ERROR("The parent of the PLL DIV is invalid\n"); 407 return 0; 408 } 409 410 ret = get_base_addr(pll->instance, drv, &pll_addr); 411 if (ret != 0) { 412 ERROR("Failed to detect PLL instance\n"); 413 return -EINVAL; 414 } 415 416 dc = (uint32_t)(pll->vco_freq / pdiv->freq); 417 418 config_pll_out_div(pll_addr, pdiv->index, dc); 419 420 return 0; 421 } 422 423 static int enable_module(const struct s32cc_clk_obj *module, unsigned int *depth) 424 { 425 const struct s32cc_clk_drv *drv = get_drv(); 426 int ret = 0; 427 428 ret = update_stack_depth(depth); 429 if (ret != 0) { 430 return ret; 431 } 432 433 if (drv == NULL) { 434 return -EINVAL; 435 } 436 437 switch (module->type) { 438 case s32cc_osc_t: 439 ret = enable_osc(module, drv, depth); 440 break; 441 case s32cc_clk_t: 442 ret = enable_clk_module(module, drv, depth); 443 break; 444 case s32cc_pll_t: 445 ret = enable_pll(module, drv, depth); 446 break; 447 case s32cc_pll_out_div_t: 448 ret = enable_pll_div(module, drv, depth); 449 break; 450 case s32cc_clkmux_t: 451 ret = -ENOTSUP; 452 break; 453 case s32cc_shared_clkmux_t: 454 ret = -ENOTSUP; 455 break; 456 case s32cc_fixed_div_t: 457 ret = -ENOTSUP; 458 break; 459 default: 460 ret = -EINVAL; 461 break; 462 } 463 464 return ret; 465 } 466 467 static int s32cc_clk_enable(unsigned long id) 468 { 469 unsigned int depth = MAX_STACK_DEPTH; 470 const struct s32cc_clk *clk; 471 472 clk = s32cc_get_arch_clk(id); 473 if (clk == NULL) { 474 return -EINVAL; 475 } 476 477 return enable_module(&clk->desc, &depth); 478 } 479 480 static void s32cc_clk_disable(unsigned long id) 481 { 482 } 483 484 static bool s32cc_clk_is_enabled(unsigned long id) 485 { 486 return false; 487 } 488 489 static unsigned long s32cc_clk_get_rate(unsigned long id) 490 { 491 return 0; 492 } 493 494 static int set_module_rate(const struct s32cc_clk_obj *module, 495 unsigned long rate, unsigned long *orate, 496 unsigned int *depth); 497 498 static int set_osc_freq(const struct s32cc_clk_obj *module, unsigned long rate, 499 unsigned long *orate, unsigned int *depth) 500 { 501 struct s32cc_osc *osc = s32cc_obj2osc(module); 502 int ret; 503 504 ret = update_stack_depth(depth); 505 if (ret != 0) { 506 return ret; 507 } 508 509 if ((osc->freq != 0UL) && (rate != osc->freq)) { 510 ERROR("Already initialized oscillator. freq = %lu\n", 511 osc->freq); 512 return -EINVAL; 513 } 514 515 osc->freq = rate; 516 *orate = osc->freq; 517 518 return 0; 519 } 520 521 static int set_clk_freq(const struct s32cc_clk_obj *module, unsigned long rate, 522 unsigned long *orate, unsigned int *depth) 523 { 524 const struct s32cc_clk *clk = s32cc_obj2clk(module); 525 int ret; 526 527 ret = update_stack_depth(depth); 528 if (ret != 0) { 529 return ret; 530 } 531 532 if ((clk->min_freq != 0UL) && (clk->max_freq != 0UL) && 533 ((rate < clk->min_freq) || (rate > clk->max_freq))) { 534 ERROR("%lu frequency is out of the allowed range: [%lu:%lu]\n", 535 rate, clk->min_freq, clk->max_freq); 536 return -EINVAL; 537 } 538 539 if (clk->module != NULL) { 540 return set_module_rate(clk->module, rate, orate, depth); 541 } 542 543 if (clk->pclock != NULL) { 544 return set_clk_freq(&clk->pclock->desc, rate, orate, depth); 545 } 546 547 return -EINVAL; 548 } 549 550 static int set_pll_freq(const struct s32cc_clk_obj *module, unsigned long rate, 551 unsigned long *orate, unsigned int *depth) 552 { 553 struct s32cc_pll *pll = s32cc_obj2pll(module); 554 int ret; 555 556 ret = update_stack_depth(depth); 557 if (ret != 0) { 558 return ret; 559 } 560 561 if ((pll->vco_freq != 0UL) && (pll->vco_freq != rate)) { 562 ERROR("PLL frequency was already set\n"); 563 return -EINVAL; 564 } 565 566 pll->vco_freq = rate; 567 *orate = pll->vco_freq; 568 569 return 0; 570 } 571 572 static int set_pll_div_freq(const struct s32cc_clk_obj *module, unsigned long rate, 573 unsigned long *orate, unsigned int *depth) 574 { 575 struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module); 576 const struct s32cc_pll *pll; 577 unsigned long prate, dc; 578 int ret; 579 580 ret = update_stack_depth(depth); 581 if (ret != 0) { 582 return ret; 583 } 584 585 if (pdiv->parent == NULL) { 586 ERROR("Failed to identify PLL divider's parent\n"); 587 return -EINVAL; 588 } 589 590 pll = s32cc_obj2pll(pdiv->parent); 591 if (pll == NULL) { 592 ERROR("The parent of the PLL DIV is invalid\n"); 593 return -EINVAL; 594 } 595 596 prate = pll->vco_freq; 597 598 /** 599 * The PLL is not initialized yet, so let's take a risk 600 * and accept the proposed rate. 601 */ 602 if (prate == 0UL) { 603 pdiv->freq = rate; 604 *orate = rate; 605 return 0; 606 } 607 608 /* Decline in case the rate cannot fit PLL's requirements. */ 609 dc = prate / rate; 610 if ((prate / dc) != rate) { 611 return -EINVAL; 612 } 613 614 pdiv->freq = rate; 615 *orate = pdiv->freq; 616 617 return 0; 618 } 619 620 static int set_fixed_div_freq(const struct s32cc_clk_obj *module, unsigned long rate, 621 unsigned long *orate, unsigned int *depth) 622 { 623 const struct s32cc_fixed_div *fdiv = s32cc_obj2fixeddiv(module); 624 int ret; 625 626 ret = update_stack_depth(depth); 627 if (ret != 0) { 628 return ret; 629 } 630 631 if (fdiv->parent == NULL) { 632 ERROR("The divider doesn't have a valid parent\b"); 633 return -EINVAL; 634 } 635 636 ret = set_module_rate(fdiv->parent, rate * fdiv->rate_div, orate, depth); 637 638 /* Update the output rate based on the parent's rate */ 639 *orate /= fdiv->rate_div; 640 641 return ret; 642 } 643 644 static int set_mux_freq(const struct s32cc_clk_obj *module, unsigned long rate, 645 unsigned long *orate, unsigned int *depth) 646 { 647 const struct s32cc_clkmux *mux = s32cc_obj2clkmux(module); 648 const struct s32cc_clk *clk = s32cc_get_arch_clk(mux->source_id); 649 int ret; 650 651 ret = update_stack_depth(depth); 652 if (ret != 0) { 653 return ret; 654 } 655 656 if (clk == NULL) { 657 ERROR("Mux (id:%" PRIu8 ") without a valid source (%lu)\n", 658 mux->index, mux->source_id); 659 return -EINVAL; 660 } 661 662 return set_module_rate(&clk->desc, rate, orate, depth); 663 } 664 665 static int set_module_rate(const struct s32cc_clk_obj *module, 666 unsigned long rate, unsigned long *orate, 667 unsigned int *depth) 668 { 669 int ret = 0; 670 671 ret = update_stack_depth(depth); 672 if (ret != 0) { 673 return ret; 674 } 675 676 switch (module->type) { 677 case s32cc_clk_t: 678 ret = set_clk_freq(module, rate, orate, depth); 679 break; 680 case s32cc_osc_t: 681 ret = set_osc_freq(module, rate, orate, depth); 682 break; 683 case s32cc_pll_t: 684 ret = set_pll_freq(module, rate, orate, depth); 685 break; 686 case s32cc_pll_out_div_t: 687 ret = set_pll_div_freq(module, rate, orate, depth); 688 break; 689 case s32cc_fixed_div_t: 690 ret = set_fixed_div_freq(module, rate, orate, depth); 691 break; 692 case s32cc_clkmux_t: 693 ret = set_mux_freq(module, rate, orate, depth); 694 break; 695 case s32cc_shared_clkmux_t: 696 ret = set_mux_freq(module, rate, orate, depth); 697 break; 698 default: 699 ret = -EINVAL; 700 break; 701 } 702 703 return ret; 704 } 705 706 static int s32cc_clk_set_rate(unsigned long id, unsigned long rate, 707 unsigned long *orate) 708 { 709 unsigned int depth = MAX_STACK_DEPTH; 710 const struct s32cc_clk *clk; 711 int ret; 712 713 clk = s32cc_get_arch_clk(id); 714 if (clk == NULL) { 715 return -EINVAL; 716 } 717 718 ret = set_module_rate(&clk->desc, rate, orate, &depth); 719 if (ret != 0) { 720 ERROR("Failed to set frequency (%lu MHz) for clock %lu\n", 721 rate, id); 722 } 723 724 return ret; 725 } 726 727 static int s32cc_clk_get_parent(unsigned long id) 728 { 729 return -ENOTSUP; 730 } 731 732 static int s32cc_clk_set_parent(unsigned long id, unsigned long parent_id) 733 { 734 const struct s32cc_clk *parent; 735 const struct s32cc_clk *clk; 736 bool valid_source = false; 737 struct s32cc_clkmux *mux; 738 uint8_t i; 739 740 clk = s32cc_get_arch_clk(id); 741 if (clk == NULL) { 742 return -EINVAL; 743 } 744 745 parent = s32cc_get_arch_clk(parent_id); 746 if (parent == NULL) { 747 return -EINVAL; 748 } 749 750 if (!is_s32cc_clk_mux(clk)) { 751 ERROR("Clock %lu is not a mux\n", id); 752 return -EINVAL; 753 } 754 755 mux = s32cc_clk2mux(clk); 756 if (mux == NULL) { 757 ERROR("Failed to cast clock %lu to clock mux\n", id); 758 return -EINVAL; 759 } 760 761 for (i = 0; i < mux->nclks; i++) { 762 if (mux->clkids[i] == parent_id) { 763 valid_source = true; 764 break; 765 } 766 } 767 768 if (!valid_source) { 769 ERROR("Clock %lu is not a valid clock for mux %lu\n", 770 parent_id, id); 771 return -EINVAL; 772 } 773 774 mux->source_id = parent_id; 775 776 return 0; 777 } 778 779 void s32cc_clk_register_drv(void) 780 { 781 static const struct clk_ops s32cc_clk_ops = { 782 .enable = s32cc_clk_enable, 783 .disable = s32cc_clk_disable, 784 .is_enabled = s32cc_clk_is_enabled, 785 .get_rate = s32cc_clk_get_rate, 786 .set_rate = s32cc_clk_set_rate, 787 .get_parent = s32cc_clk_get_parent, 788 .set_parent = s32cc_clk_set_parent, 789 }; 790 791 clk_register(&s32cc_clk_ops); 792 } 793 794