1 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) 2 /* 3 * Copyright (C) STMicroelectronics 2022 - All Rights Reserved 4 */ 5 6 #include <config.h> 7 #include <drivers/clk.h> 8 #include <drivers/clk_dt.h> 9 #include <drivers/stm32_shared_io.h> 10 #include <io.h> 11 #include <kernel/boot.h> 12 #include <kernel/delay.h> 13 #include <kernel/dt.h> 14 #include <libfdt.h> 15 #include <stdio.h> 16 #include <stm32_util.h> 17 18 #include "clk-stm32-core.h" 19 20 #define RCC_MP_ENCLRR_OFFSET 0x4 21 22 #define TIMEOUT_US_200MS U(200000) 23 #define TIMEOUT_US_1S U(1000000) 24 25 static struct clk_stm32_priv *stm32_clock_data; 26 27 struct clk_stm32_priv *clk_stm32_get_priv(void) 28 { 29 return stm32_clock_data; 30 } 31 32 uintptr_t clk_stm32_get_rcc_base(void) 33 { 34 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 35 36 return priv->base; 37 } 38 39 /* STM32 MUX API */ 40 size_t stm32_mux_get_parent(uint32_t mux_id) 41 { 42 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 43 const struct mux_cfg *mux = &priv->muxes[mux_id]; 44 uint32_t mask = MASK_WIDTH_SHIFT(mux->width, mux->shift); 45 46 return (io_read32(priv->base + mux->offset) & mask) >> mux->shift; 47 } 48 49 TEE_Result stm32_mux_set_parent(uint16_t mux_id, uint8_t sel) 50 { 51 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 52 const struct mux_cfg *mux = &priv->muxes[mux_id]; 53 uint32_t mask = MASK_WIDTH_SHIFT(mux->width, mux->shift); 54 uintptr_t address = priv->base + mux->offset; 55 56 io_clrsetbits32(address, mask, (sel << mux->shift) & mask); 57 58 if (mux->ready != MUX_NO_RDY) 59 return stm32_gate_wait_ready((uint16_t)mux->ready, true); 60 61 return TEE_SUCCESS; 62 } 63 64 /* STM32 GATE API */ 65 static void stm32_gate_endisable(uint16_t gate_id, bool enable) 66 { 67 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 68 const struct gate_cfg *gate = &priv->gates[gate_id]; 69 uintptr_t addr = priv->base + gate->offset; 70 71 if (enable) { 72 if (gate->set_clr) 73 io_write32(addr, BIT(gate->bit_idx)); 74 else 75 io_setbits32_stm32shregs(addr, BIT(gate->bit_idx)); 76 } else { 77 if (gate->set_clr) 78 io_write32(addr + RCC_MP_ENCLRR_OFFSET, 79 BIT(gate->bit_idx)); 80 else 81 io_clrbits32_stm32shregs(addr, BIT(gate->bit_idx)); 82 } 83 } 84 85 void stm32_gate_disable(uint16_t gate_id) 86 { 87 stm32_gate_endisable(gate_id, false); 88 } 89 90 void stm32_gate_enable(uint16_t gate_id) 91 { 92 stm32_gate_endisable(gate_id, true); 93 } 94 95 bool stm32_gate_is_enabled(uint16_t gate_id) 96 { 97 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 98 const struct gate_cfg *gate = &priv->gates[gate_id]; 99 uintptr_t addr = priv->base + gate->offset; 100 101 return (io_read32(addr) & BIT(gate->bit_idx)) != 0U; 102 } 103 104 TEE_Result stm32_gate_wait_ready(uint16_t gate_id, bool ready_on) 105 { 106 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 107 const struct gate_cfg *gate = &priv->gates[gate_id]; 108 uintptr_t address = priv->base + gate->offset; 109 uint32_t mask_rdy = BIT(gate->bit_idx); 110 uint64_t timeout = timeout_init_us(TIMEOUT_US_1S); 111 uint32_t mask = 0U; 112 113 if (ready_on) 114 mask = BIT(gate->bit_idx); 115 116 while ((io_read32(address) & mask_rdy) != mask) 117 if (timeout_elapsed(timeout)) 118 break; 119 120 if ((io_read32(address) & mask_rdy) != mask) 121 return TEE_ERROR_GENERIC; 122 123 return TEE_SUCCESS; 124 } 125 126 /* STM32 GATE READY clock operators */ 127 static TEE_Result stm32_gate_ready_endisable(uint16_t gate_id, bool enable, 128 bool wait_rdy) 129 { 130 stm32_gate_endisable(gate_id, enable); 131 132 if (wait_rdy) 133 return stm32_gate_wait_ready(gate_id + 1, enable); 134 135 return TEE_SUCCESS; 136 } 137 138 TEE_Result stm32_gate_rdy_enable(uint16_t gate_id) 139 { 140 return stm32_gate_ready_endisable(gate_id, true, true); 141 } 142 143 TEE_Result stm32_gate_rdy_disable(uint16_t gate_id) 144 { 145 return stm32_gate_ready_endisable(gate_id, false, true); 146 } 147 148 /* STM32 DIV API */ 149 static unsigned int _get_table_div(const struct div_table_cfg *table, 150 unsigned int val) 151 { 152 const struct div_table_cfg *clkt = NULL; 153 154 for (clkt = table; clkt->div; clkt++) 155 if (clkt->val == val) 156 return clkt->div; 157 158 return 0; 159 } 160 161 static unsigned int _get_table_val(const struct div_table_cfg *table, 162 unsigned int div) 163 { 164 const struct div_table_cfg *clkt = NULL; 165 166 for (clkt = table; clkt->div; clkt++) 167 if (clkt->div == div) 168 return clkt->val; 169 170 return 0; 171 } 172 173 static unsigned int _get_div(const struct div_table_cfg *table, 174 unsigned int val, unsigned long flags, 175 uint8_t width) 176 { 177 if (flags & CLK_DIVIDER_ONE_BASED) 178 return val; 179 180 if (flags & CLK_DIVIDER_POWER_OF_TWO) 181 return BIT(val); 182 183 if (flags & CLK_DIVIDER_MAX_AT_ZERO) 184 return (val != 0U) ? val : BIT(width); 185 186 if (table) 187 return _get_table_div(table, val); 188 189 return val + 1U; 190 } 191 192 static unsigned int _get_val(const struct div_table_cfg *table, 193 unsigned int div, unsigned long flags, 194 uint8_t width) 195 { 196 if (flags & CLK_DIVIDER_ONE_BASED) 197 return div; 198 199 if (flags & CLK_DIVIDER_POWER_OF_TWO) 200 return __builtin_ffs(div) - 1; 201 202 if (flags & CLK_DIVIDER_MAX_AT_ZERO) 203 return (div != 0U) ? div : BIT(width); 204 205 if (table) 206 return _get_table_val(table, div); 207 208 return div - 1U; 209 } 210 211 static bool _is_valid_table_div(const struct div_table_cfg *table, 212 unsigned int div) 213 { 214 const struct div_table_cfg *clkt = NULL; 215 216 for (clkt = table; clkt->div; clkt++) 217 if (clkt->div == div) 218 return true; 219 220 return false; 221 } 222 223 static bool _is_valid_div(const struct div_table_cfg *table, 224 unsigned int div, unsigned long flags) 225 { 226 if (flags & CLK_DIVIDER_POWER_OF_TWO) 227 return IS_POWER_OF_TWO(div); 228 229 if (table) 230 return _is_valid_table_div(table, div); 231 232 return true; 233 } 234 235 static int divider_get_val(unsigned long rate, unsigned long parent_rate, 236 const struct div_table_cfg *table, uint8_t width, 237 unsigned long flags) 238 { 239 unsigned int div = 0U; 240 unsigned int value = 0U; 241 242 div = UDIV_ROUND_NEAREST((uint64_t)parent_rate, rate); 243 244 if (!_is_valid_div(table, div, flags)) 245 return -1; 246 247 value = _get_val(table, div, flags, width); 248 249 return MIN(value, MASK_WIDTH_SHIFT(width, 0)); 250 } 251 252 uint32_t stm32_div_get_value(int div_id) 253 { 254 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 255 const struct div_cfg *divider = &priv->div[div_id]; 256 uint32_t val = 0; 257 258 val = io_read32(priv->base + divider->offset) >> divider->shift; 259 val &= MASK_WIDTH_SHIFT(divider->width, 0); 260 261 return val; 262 } 263 264 TEE_Result stm32_div_set_value(uint32_t div_id, uint32_t value) 265 { 266 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 267 const struct div_cfg *divider = NULL; 268 uintptr_t address = 0; 269 uint32_t mask = 0; 270 271 if (div_id >= priv->nb_div) 272 panic(); 273 274 divider = &priv->div[div_id]; 275 address = priv->base + divider->offset; 276 277 mask = MASK_WIDTH_SHIFT(divider->width, divider->shift); 278 io_clrsetbits32(address, mask, (value << divider->shift) & mask); 279 280 if (divider->ready == DIV_NO_RDY) 281 return TEE_SUCCESS; 282 283 return stm32_gate_wait_ready((uint16_t)divider->ready, true); 284 } 285 286 static unsigned long stm32_div_get_rate(int div_id, unsigned long prate) 287 { 288 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 289 const struct div_cfg *divider = &priv->div[div_id]; 290 uint32_t val = stm32_div_get_value(div_id); 291 unsigned int div = 0U; 292 293 div = _get_div(divider->table, val, divider->flags, divider->width); 294 if (!div) 295 return prate; 296 297 return ROUNDUP_DIV((uint64_t)prate, div); 298 } 299 300 TEE_Result stm32_div_set_rate(int div_id, unsigned long rate, 301 unsigned long prate) 302 { 303 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 304 const struct div_cfg *divider = &priv->div[div_id]; 305 int value = 0; 306 307 value = divider_get_val(rate, prate, divider->table, 308 divider->width, divider->flags); 309 310 if (value < 0) 311 return TEE_ERROR_GENERIC; 312 313 return stm32_div_set_value(div_id, value); 314 } 315 316 /* STM32 MUX clock operators */ 317 static size_t clk_stm32_mux_get_parent(struct clk *clk) 318 { 319 struct clk_stm32_mux_cfg *cfg = clk->priv; 320 321 return stm32_mux_get_parent(cfg->mux_id); 322 } 323 324 static TEE_Result clk_stm32_mux_set_parent(struct clk *clk, size_t pidx) 325 { 326 struct clk_stm32_mux_cfg *cfg = clk->priv; 327 328 return stm32_mux_set_parent(cfg->mux_id, pidx); 329 } 330 331 const struct clk_ops clk_stm32_mux_ops = { 332 .get_parent = clk_stm32_mux_get_parent, 333 .set_parent = clk_stm32_mux_set_parent, 334 }; 335 336 /* STM32 GATE clock operators */ 337 static TEE_Result clk_stm32_gate_enable(struct clk *clk) 338 { 339 struct clk_stm32_gate_cfg *cfg = clk->priv; 340 341 stm32_gate_enable(cfg->gate_id); 342 343 return TEE_SUCCESS; 344 } 345 346 static void clk_stm32_gate_disable(struct clk *clk) 347 { 348 struct clk_stm32_gate_cfg *cfg = clk->priv; 349 350 stm32_gate_disable(cfg->gate_id); 351 } 352 353 const struct clk_ops clk_stm32_gate_ops = { 354 .enable = clk_stm32_gate_enable, 355 .disable = clk_stm32_gate_disable, 356 }; 357 358 static TEE_Result clk_stm32_gate_ready_enable(struct clk *clk) 359 { 360 struct clk_stm32_gate_cfg *cfg = clk->priv; 361 362 return stm32_gate_rdy_enable(cfg->gate_id); 363 } 364 365 static void clk_stm32_gate_ready_disable(struct clk *clk) 366 { 367 struct clk_stm32_gate_cfg *cfg = clk->priv; 368 369 if (stm32_gate_rdy_disable(cfg->gate_id)) 370 panic(); 371 } 372 373 const struct clk_ops clk_stm32_gate_ready_ops = { 374 .enable = clk_stm32_gate_ready_enable, 375 .disable = clk_stm32_gate_ready_disable, 376 }; 377 378 /* STM32 DIV clock operators */ 379 unsigned long clk_stm32_divider_get_rate(struct clk *clk, 380 unsigned long parent_rate) 381 { 382 struct clk_stm32_div_cfg *cfg = clk->priv; 383 384 return stm32_div_get_rate(cfg->div_id, parent_rate); 385 } 386 387 TEE_Result clk_stm32_divider_set_rate(struct clk *clk, 388 unsigned long rate, 389 unsigned long parent_rate) 390 { 391 struct clk_stm32_div_cfg *cfg = clk->priv; 392 393 return stm32_div_set_rate(cfg->div_id, rate, parent_rate); 394 } 395 396 const struct clk_ops clk_stm32_divider_ops = { 397 .get_rate = clk_stm32_divider_get_rate, 398 .set_rate = clk_stm32_divider_set_rate, 399 }; 400 401 /* STM32 COMPOSITE clock operators */ 402 size_t clk_stm32_composite_get_parent(struct clk *clk) 403 { 404 struct clk_stm32_composite_cfg *cfg = clk->priv; 405 406 if (cfg->mux_id == NO_MUX) { 407 /* It could be a normal case */ 408 return 0; 409 } 410 411 return stm32_mux_get_parent(cfg->mux_id); 412 } 413 414 TEE_Result clk_stm32_composite_set_parent(struct clk *clk, size_t pidx) 415 { 416 struct clk_stm32_composite_cfg *cfg = clk->priv; 417 418 if (cfg->mux_id == NO_MUX) 419 panic(); 420 421 return stm32_mux_set_parent(cfg->mux_id, pidx); 422 } 423 424 unsigned long clk_stm32_composite_get_rate(struct clk *clk, 425 unsigned long parent_rate) 426 { 427 struct clk_stm32_composite_cfg *cfg = clk->priv; 428 429 if (cfg->div_id == NO_DIV) 430 return parent_rate; 431 432 return stm32_div_get_rate(cfg->div_id, parent_rate); 433 } 434 435 TEE_Result clk_stm32_composite_set_rate(struct clk *clk, unsigned long rate, 436 unsigned long parent_rate) 437 { 438 struct clk_stm32_composite_cfg *cfg = clk->priv; 439 440 if (cfg->div_id == NO_DIV) 441 return TEE_SUCCESS; 442 443 return stm32_div_set_rate(cfg->div_id, rate, parent_rate); 444 } 445 446 TEE_Result clk_stm32_composite_gate_enable(struct clk *clk) 447 { 448 struct clk_stm32_composite_cfg *cfg = clk->priv; 449 450 stm32_gate_enable(cfg->gate_id); 451 452 return TEE_SUCCESS; 453 } 454 455 void clk_stm32_composite_gate_disable(struct clk *clk) 456 { 457 struct clk_stm32_composite_cfg *cfg = clk->priv; 458 459 stm32_gate_disable(cfg->gate_id); 460 } 461 462 const struct clk_ops clk_stm32_composite_ops = { 463 .get_parent = clk_stm32_composite_get_parent, 464 .set_parent = clk_stm32_composite_set_parent, 465 .get_rate = clk_stm32_composite_get_rate, 466 .set_rate = clk_stm32_composite_set_rate, 467 .enable = clk_stm32_composite_gate_enable, 468 .disable = clk_stm32_composite_gate_disable, 469 }; 470 471 TEE_Result clk_stm32_set_parent_by_index(struct clk *clk, size_t pidx) 472 { 473 struct clk *parent = clk_get_parent_by_index(clk, pidx); 474 TEE_Result res = TEE_ERROR_GENERIC; 475 476 if (parent) 477 res = clk_set_parent(clk, parent); 478 479 return res; 480 } 481 482 int clk_stm32_parse_fdt_by_name(const void *fdt, int node, const char *name, 483 uint32_t *tab, uint32_t *nb) 484 { 485 const fdt32_t *cell = NULL; 486 int len = 0; 487 uint32_t i = 0; 488 489 cell = fdt_getprop(fdt, node, name, &len); 490 if (cell) 491 for (i = 0; i < ((uint32_t)len / sizeof(uint32_t)); i++) 492 tab[i] = fdt32_to_cpu(cell[i]); 493 494 *nb = (uint32_t)len / sizeof(uint32_t); 495 496 return 0; 497 } 498 499 TEE_Result clk_stm32_init(struct clk_stm32_priv *priv, uintptr_t base) 500 { 501 stm32_clock_data = priv; 502 503 priv->base = base; 504 505 return TEE_SUCCESS; 506 } 507 508 static unsigned long fixed_factor_get_rate(struct clk *clk, 509 unsigned long parent_rate) 510 { 511 struct fixed_factor_cfg *d = clk->priv; 512 513 unsigned long long rate = (unsigned long long)parent_rate * d->mult; 514 515 if (d->div == 0U) 516 panic("error division by zero"); 517 518 return (unsigned long)(rate / d->div); 519 }; 520 521 const struct clk_ops clk_fixed_factor_ops = { 522 .get_rate = fixed_factor_get_rate, 523 }; 524 525 static unsigned long clk_fixed_get_rate(struct clk *clk, 526 unsigned long parent_rate __unused) 527 { 528 struct clk_fixed_rate_cfg *cfg = clk->priv; 529 530 return cfg->rate; 531 } 532 533 const struct clk_ops clk_fixed_clk_ops = { 534 .get_rate = clk_fixed_get_rate, 535 }; 536 537 struct clk *stm32mp_rcc_clock_id_to_clk(unsigned long clock_id) 538 { 539 struct clk_stm32_priv *priv = clk_stm32_get_priv(); 540 541 if (clock_id > priv->nb_clk_refs) 542 return NULL; 543 544 return priv->clk_refs[clock_id]; 545 } 546 547 static TEE_Result stm32mp_clk_dt_get_clk(struct dt_pargs *pargs, 548 void *data __unused, 549 struct clk **out_clk) 550 { 551 unsigned long clock_id = pargs->args[0]; 552 struct clk *clk = NULL; 553 554 if (pargs->args_count != 1) 555 return TEE_ERROR_BAD_PARAMETERS; 556 557 clk = stm32mp_rcc_clock_id_to_clk(clock_id); 558 if (!clk) 559 return TEE_ERROR_BAD_PARAMETERS; 560 561 *out_clk = clk; 562 563 return TEE_SUCCESS; 564 } 565 566 static void clk_stm32_register_clocks(struct clk_stm32_priv *priv) 567 { 568 unsigned int i = 0; 569 570 for (i = 0; i < priv->nb_clk_refs; i++) { 571 struct clk *clk = priv->clk_refs[i]; 572 573 if (!clk) 574 continue; 575 576 refcount_set(&clk->enabled_count, 0); 577 578 if (clk_register(clk)) 579 panic(); 580 } 581 582 /* Critical clocks management */ 583 for (i = 0; i < priv->nb_clk_refs; i++) { 584 struct clk *clk = priv->clk_refs[i]; 585 586 if (!clk) 587 continue; 588 589 if (priv->is_critical && priv->is_critical(clk)) 590 clk_enable(clk); 591 } 592 } 593 594 void stm32mp_clk_provider_probe_final(const void *fdt, int node, 595 struct clk_stm32_priv *priv) 596 { 597 TEE_Result res = TEE_ERROR_GENERIC; 598 599 clk_stm32_register_clocks(priv); 600 601 res = clk_dt_register_clk_provider(fdt, node, stm32mp_clk_dt_get_clk, 602 priv); 603 if (res) 604 panic("Couldn't register clock provider"); 605 } 606