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 disable_odivs(uintptr_t pll_addr, uint32_t ndivs) 237 { 238 uint32_t i; 239 240 for (i = 0; i < ndivs; i++) { 241 disable_odiv(pll_addr, i); 242 } 243 } 244 245 static void enable_pll_hw(uintptr_t pll_addr) 246 { 247 /* Enable the PLL. */ 248 mmio_write_32(PLLDIG_PLLCR(pll_addr), 0x0); 249 250 /* Poll until PLL acquires lock. */ 251 while ((mmio_read_32(PLLDIG_PLLSR(pll_addr)) & PLLDIG_PLLSR_LOCK) == 0U) { 252 } 253 } 254 255 static void disable_pll_hw(uintptr_t pll_addr) 256 { 257 mmio_write_32(PLLDIG_PLLCR(pll_addr), PLLDIG_PLLCR_PLLPD); 258 } 259 260 static int program_pll(const struct s32cc_pll *pll, uintptr_t pll_addr, 261 const struct s32cc_clk_drv *drv, uint32_t sclk_id, 262 unsigned long sclk_freq) 263 { 264 uint32_t rdiv = 1, mfi, mfn; 265 int ret; 266 267 ret = get_pll_mfi_mfn(pll->vco_freq, sclk_freq, &mfi, &mfn); 268 if (ret != 0) { 269 return -EINVAL; 270 } 271 272 /* Disable ODIVs*/ 273 disable_odivs(pll_addr, pll->ndividers); 274 275 /* Disable PLL */ 276 disable_pll_hw(pll_addr); 277 278 /* Program PLLCLKMUX */ 279 mmio_write_32(PLLDIG_PLLCLKMUX(pll_addr), sclk_id); 280 281 /* Program VCO */ 282 mmio_clrsetbits_32(PLLDIG_PLLDV(pll_addr), 283 PLLDIG_PLLDV_RDIV_MASK | PLLDIG_PLLDV_MFI_MASK, 284 PLLDIG_PLLDV_RDIV_SET(rdiv) | PLLDIG_PLLDV_MFI(mfi)); 285 286 mmio_write_32(PLLDIG_PLLFD(pll_addr), 287 PLLDIG_PLLFD_MFN_SET(mfn) | PLLDIG_PLLFD_SMDEN); 288 289 enable_pll_hw(pll_addr); 290 291 return ret; 292 } 293 294 static int enable_pll(const struct s32cc_clk_obj *module, 295 const struct s32cc_clk_drv *drv, 296 unsigned int *depth) 297 { 298 const struct s32cc_pll *pll = s32cc_obj2pll(module); 299 const struct s32cc_clkmux *mux; 300 uintptr_t pll_addr = UL(0x0); 301 unsigned long sclk_freq; 302 uint32_t sclk_id; 303 int ret; 304 305 ret = update_stack_depth(depth); 306 if (ret != 0) { 307 return ret; 308 } 309 310 mux = get_pll_mux(pll); 311 if (mux == NULL) { 312 return -EINVAL; 313 } 314 315 if (pll->instance != mux->module) { 316 ERROR("MUX type is not in sync with PLL ID\n"); 317 return -EINVAL; 318 } 319 320 ret = get_base_addr(pll->instance, drv, &pll_addr); 321 if (ret != 0) { 322 ERROR("Failed to detect PLL instance\n"); 323 return ret; 324 } 325 326 switch (mux->source_id) { 327 case S32CC_CLK_FIRC: 328 sclk_freq = 48U * MHZ; 329 sclk_id = 0; 330 break; 331 case S32CC_CLK_FXOSC: 332 sclk_freq = 40U * MHZ; 333 sclk_id = 1; 334 break; 335 default: 336 ERROR("Invalid source selection for PLL 0x%lx\n", 337 pll_addr); 338 return -EINVAL; 339 }; 340 341 return program_pll(pll, pll_addr, drv, sclk_id, sclk_freq); 342 } 343 344 static int enable_module(const struct s32cc_clk_obj *module, unsigned int *depth) 345 { 346 const struct s32cc_clk_drv *drv = get_drv(); 347 int ret = 0; 348 349 ret = update_stack_depth(depth); 350 if (ret != 0) { 351 return ret; 352 } 353 354 if (drv == NULL) { 355 return -EINVAL; 356 } 357 358 switch (module->type) { 359 case s32cc_osc_t: 360 ret = enable_osc(module, drv, depth); 361 break; 362 case s32cc_clk_t: 363 ret = enable_clk_module(module, drv, depth); 364 break; 365 case s32cc_pll_t: 366 ret = enable_pll(module, drv, depth); 367 break; 368 case s32cc_clkmux_t: 369 ret = -ENOTSUP; 370 break; 371 case s32cc_shared_clkmux_t: 372 ret = -ENOTSUP; 373 break; 374 case s32cc_pll_out_div_t: 375 ret = -ENOTSUP; 376 break; 377 case s32cc_fixed_div_t: 378 ret = -ENOTSUP; 379 break; 380 default: 381 ret = -EINVAL; 382 break; 383 } 384 385 return ret; 386 } 387 388 static int s32cc_clk_enable(unsigned long id) 389 { 390 unsigned int depth = MAX_STACK_DEPTH; 391 const struct s32cc_clk *clk; 392 393 clk = s32cc_get_arch_clk(id); 394 if (clk == NULL) { 395 return -EINVAL; 396 } 397 398 return enable_module(&clk->desc, &depth); 399 } 400 401 static void s32cc_clk_disable(unsigned long id) 402 { 403 } 404 405 static bool s32cc_clk_is_enabled(unsigned long id) 406 { 407 return false; 408 } 409 410 static unsigned long s32cc_clk_get_rate(unsigned long id) 411 { 412 return 0; 413 } 414 415 static int set_module_rate(const struct s32cc_clk_obj *module, 416 unsigned long rate, unsigned long *orate, 417 unsigned int *depth); 418 419 static int set_osc_freq(const struct s32cc_clk_obj *module, unsigned long rate, 420 unsigned long *orate, unsigned int *depth) 421 { 422 struct s32cc_osc *osc = s32cc_obj2osc(module); 423 int ret; 424 425 ret = update_stack_depth(depth); 426 if (ret != 0) { 427 return ret; 428 } 429 430 if ((osc->freq != 0UL) && (rate != osc->freq)) { 431 ERROR("Already initialized oscillator. freq = %lu\n", 432 osc->freq); 433 return -EINVAL; 434 } 435 436 osc->freq = rate; 437 *orate = osc->freq; 438 439 return 0; 440 } 441 442 static int set_clk_freq(const struct s32cc_clk_obj *module, unsigned long rate, 443 unsigned long *orate, unsigned int *depth) 444 { 445 const struct s32cc_clk *clk = s32cc_obj2clk(module); 446 int ret; 447 448 ret = update_stack_depth(depth); 449 if (ret != 0) { 450 return ret; 451 } 452 453 if ((clk->min_freq != 0UL) && (clk->max_freq != 0UL) && 454 ((rate < clk->min_freq) || (rate > clk->max_freq))) { 455 ERROR("%lu frequency is out of the allowed range: [%lu:%lu]\n", 456 rate, clk->min_freq, clk->max_freq); 457 return -EINVAL; 458 } 459 460 if (clk->module != NULL) { 461 return set_module_rate(clk->module, rate, orate, depth); 462 } 463 464 if (clk->pclock != NULL) { 465 return set_clk_freq(&clk->pclock->desc, rate, orate, depth); 466 } 467 468 return -EINVAL; 469 } 470 471 static int set_pll_freq(const struct s32cc_clk_obj *module, unsigned long rate, 472 unsigned long *orate, unsigned int *depth) 473 { 474 struct s32cc_pll *pll = s32cc_obj2pll(module); 475 int ret; 476 477 ret = update_stack_depth(depth); 478 if (ret != 0) { 479 return ret; 480 } 481 482 if ((pll->vco_freq != 0UL) && (pll->vco_freq != rate)) { 483 ERROR("PLL frequency was already set\n"); 484 return -EINVAL; 485 } 486 487 pll->vco_freq = rate; 488 *orate = pll->vco_freq; 489 490 return 0; 491 } 492 493 static int set_pll_div_freq(const struct s32cc_clk_obj *module, unsigned long rate, 494 unsigned long *orate, unsigned int *depth) 495 { 496 struct s32cc_pll_out_div *pdiv = s32cc_obj2plldiv(module); 497 const struct s32cc_pll *pll; 498 unsigned long prate, dc; 499 int ret; 500 501 ret = update_stack_depth(depth); 502 if (ret != 0) { 503 return ret; 504 } 505 506 if (pdiv->parent == NULL) { 507 ERROR("Failed to identify PLL divider's parent\n"); 508 return -EINVAL; 509 } 510 511 pll = s32cc_obj2pll(pdiv->parent); 512 if (pll == NULL) { 513 ERROR("The parent of the PLL DIV is invalid\n"); 514 return -EINVAL; 515 } 516 517 prate = pll->vco_freq; 518 519 /** 520 * The PLL is not initialized yet, so let's take a risk 521 * and accept the proposed rate. 522 */ 523 if (prate == 0UL) { 524 pdiv->freq = rate; 525 *orate = rate; 526 return 0; 527 } 528 529 /* Decline in case the rate cannot fit PLL's requirements. */ 530 dc = prate / rate; 531 if ((prate / dc) != rate) { 532 return -EINVAL; 533 } 534 535 pdiv->freq = rate; 536 *orate = pdiv->freq; 537 538 return 0; 539 } 540 541 static int set_fixed_div_freq(const struct s32cc_clk_obj *module, unsigned long rate, 542 unsigned long *orate, unsigned int *depth) 543 { 544 const struct s32cc_fixed_div *fdiv = s32cc_obj2fixeddiv(module); 545 int ret; 546 547 ret = update_stack_depth(depth); 548 if (ret != 0) { 549 return ret; 550 } 551 552 if (fdiv->parent == NULL) { 553 ERROR("The divider doesn't have a valid parent\b"); 554 return -EINVAL; 555 } 556 557 ret = set_module_rate(fdiv->parent, rate * fdiv->rate_div, orate, depth); 558 559 /* Update the output rate based on the parent's rate */ 560 *orate /= fdiv->rate_div; 561 562 return ret; 563 } 564 565 static int set_mux_freq(const struct s32cc_clk_obj *module, unsigned long rate, 566 unsigned long *orate, unsigned int *depth) 567 { 568 const struct s32cc_clkmux *mux = s32cc_obj2clkmux(module); 569 const struct s32cc_clk *clk = s32cc_get_arch_clk(mux->source_id); 570 int ret; 571 572 ret = update_stack_depth(depth); 573 if (ret != 0) { 574 return ret; 575 } 576 577 if (clk == NULL) { 578 ERROR("Mux (id:%" PRIu8 ") without a valid source (%lu)\n", 579 mux->index, mux->source_id); 580 return -EINVAL; 581 } 582 583 return set_module_rate(&clk->desc, rate, orate, depth); 584 } 585 586 static int set_module_rate(const struct s32cc_clk_obj *module, 587 unsigned long rate, unsigned long *orate, 588 unsigned int *depth) 589 { 590 int ret = 0; 591 592 ret = update_stack_depth(depth); 593 if (ret != 0) { 594 return ret; 595 } 596 597 switch (module->type) { 598 case s32cc_clk_t: 599 ret = set_clk_freq(module, rate, orate, depth); 600 break; 601 case s32cc_osc_t: 602 ret = set_osc_freq(module, rate, orate, depth); 603 break; 604 case s32cc_pll_t: 605 ret = set_pll_freq(module, rate, orate, depth); 606 break; 607 case s32cc_pll_out_div_t: 608 ret = set_pll_div_freq(module, rate, orate, depth); 609 break; 610 case s32cc_fixed_div_t: 611 ret = set_fixed_div_freq(module, rate, orate, depth); 612 break; 613 case s32cc_clkmux_t: 614 ret = set_mux_freq(module, rate, orate, depth); 615 break; 616 case s32cc_shared_clkmux_t: 617 ret = set_mux_freq(module, rate, orate, depth); 618 break; 619 default: 620 ret = -EINVAL; 621 break; 622 } 623 624 return ret; 625 } 626 627 static int s32cc_clk_set_rate(unsigned long id, unsigned long rate, 628 unsigned long *orate) 629 { 630 unsigned int depth = MAX_STACK_DEPTH; 631 const struct s32cc_clk *clk; 632 int ret; 633 634 clk = s32cc_get_arch_clk(id); 635 if (clk == NULL) { 636 return -EINVAL; 637 } 638 639 ret = set_module_rate(&clk->desc, rate, orate, &depth); 640 if (ret != 0) { 641 ERROR("Failed to set frequency (%lu MHz) for clock %lu\n", 642 rate, id); 643 } 644 645 return ret; 646 } 647 648 static int s32cc_clk_get_parent(unsigned long id) 649 { 650 return -ENOTSUP; 651 } 652 653 static int s32cc_clk_set_parent(unsigned long id, unsigned long parent_id) 654 { 655 const struct s32cc_clk *parent; 656 const struct s32cc_clk *clk; 657 bool valid_source = false; 658 struct s32cc_clkmux *mux; 659 uint8_t i; 660 661 clk = s32cc_get_arch_clk(id); 662 if (clk == NULL) { 663 return -EINVAL; 664 } 665 666 parent = s32cc_get_arch_clk(parent_id); 667 if (parent == NULL) { 668 return -EINVAL; 669 } 670 671 if (!is_s32cc_clk_mux(clk)) { 672 ERROR("Clock %lu is not a mux\n", id); 673 return -EINVAL; 674 } 675 676 mux = s32cc_clk2mux(clk); 677 if (mux == NULL) { 678 ERROR("Failed to cast clock %lu to clock mux\n", id); 679 return -EINVAL; 680 } 681 682 for (i = 0; i < mux->nclks; i++) { 683 if (mux->clkids[i] == parent_id) { 684 valid_source = true; 685 break; 686 } 687 } 688 689 if (!valid_source) { 690 ERROR("Clock %lu is not a valid clock for mux %lu\n", 691 parent_id, id); 692 return -EINVAL; 693 } 694 695 mux->source_id = parent_id; 696 697 return 0; 698 } 699 700 void s32cc_clk_register_drv(void) 701 { 702 static const struct clk_ops s32cc_clk_ops = { 703 .enable = s32cc_clk_enable, 704 .disable = s32cc_clk_disable, 705 .is_enabled = s32cc_clk_is_enabled, 706 .get_rate = s32cc_clk_get_rate, 707 .set_rate = s32cc_clk_set_rate, 708 .get_parent = s32cc_clk_get_parent, 709 .set_parent = s32cc_clk_set_parent, 710 }; 711 712 clk_register(&s32cc_clk_ops); 713 } 714 715