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