1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (c) 2013, 2018, The Linux Foundation. All rights reserved. 4 */ 5 6 #include <linux/kernel.h> 7 #include <linux/bitops.h> 8 #include <linux/err.h> 9 #include <linux/bug.h> 10 #include <linux/export.h> 11 #include <linux/clk-provider.h> 12 #include <linux/delay.h> 13 #include <linux/rational.h> 14 #include <linux/regmap.h> 15 #include <linux/math64.h> 16 #include <linux/minmax.h> 17 #include <linux/slab.h> 18 19 #include <asm/div64.h> 20 21 #include "clk-rcg.h" 22 #include "common.h" 23 24 #define CMD_REG 0x0 25 #define CMD_UPDATE BIT(0) 26 #define CMD_ROOT_EN BIT(1) 27 #define CMD_DIRTY_CFG BIT(4) 28 #define CMD_DIRTY_N BIT(5) 29 #define CMD_DIRTY_M BIT(6) 30 #define CMD_DIRTY_D BIT(7) 31 #define CMD_ROOT_OFF BIT(31) 32 33 #define CFG_REG 0x4 34 #define CFG_SRC_DIV_SHIFT 0 35 #define CFG_SRC_SEL_SHIFT 8 36 #define CFG_SRC_SEL_MASK (0x7 << CFG_SRC_SEL_SHIFT) 37 #define CFG_MODE_SHIFT 12 38 #define CFG_MODE_MASK (0x3 << CFG_MODE_SHIFT) 39 #define CFG_MODE_DUAL_EDGE (0x2 << CFG_MODE_SHIFT) 40 #define CFG_HW_CLK_CTRL_MASK BIT(20) 41 42 #define M_REG 0x8 43 #define N_REG 0xc 44 #define D_REG 0x10 45 46 #define RCG_CFG_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + CFG_REG) 47 #define RCG_M_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + M_REG) 48 #define RCG_N_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + N_REG) 49 #define RCG_D_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + D_REG) 50 51 /* Dynamic Frequency Scaling */ 52 #define MAX_PERF_LEVEL 8 53 #define SE_CMD_DFSR_OFFSET 0x14 54 #define SE_CMD_DFS_EN BIT(0) 55 #define SE_PERF_DFSR(level) (0x1c + 0x4 * (level)) 56 #define SE_PERF_M_DFSR(level) (0x5c + 0x4 * (level)) 57 #define SE_PERF_N_DFSR(level) (0x9c + 0x4 * (level)) 58 59 enum freq_policy { 60 FLOOR, 61 CEIL, 62 }; 63 64 static int clk_rcg2_is_enabled(struct clk_hw *hw) 65 { 66 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 67 u32 cmd; 68 int ret; 69 70 ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd); 71 if (ret) 72 return ret; 73 74 return (cmd & CMD_ROOT_OFF) == 0; 75 } 76 77 static u8 __clk_rcg2_get_parent(struct clk_hw *hw, u32 cfg) 78 { 79 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 80 int num_parents = clk_hw_get_num_parents(hw); 81 int i; 82 83 cfg &= CFG_SRC_SEL_MASK; 84 cfg >>= CFG_SRC_SEL_SHIFT; 85 86 for (i = 0; i < num_parents; i++) 87 if (cfg == rcg->parent_map[i].cfg) 88 return i; 89 90 pr_debug("%s: Clock %s has invalid parent, using default.\n", 91 __func__, clk_hw_get_name(hw)); 92 return 0; 93 } 94 95 static u8 clk_rcg2_get_parent(struct clk_hw *hw) 96 { 97 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 98 u32 cfg; 99 int ret; 100 101 ret = regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg); 102 if (ret) { 103 pr_debug("%s: Unable to read CFG register for %s\n", 104 __func__, clk_hw_get_name(hw)); 105 return 0; 106 } 107 108 return __clk_rcg2_get_parent(hw, cfg); 109 } 110 111 static int update_config(struct clk_rcg2 *rcg) 112 { 113 int count, ret; 114 u32 cmd; 115 struct clk_hw *hw = &rcg->clkr.hw; 116 const char *name = clk_hw_get_name(hw); 117 118 ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, 119 CMD_UPDATE, CMD_UPDATE); 120 if (ret) 121 return ret; 122 123 /* Wait for update to take effect */ 124 for (count = 500; count > 0; count--) { 125 ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd); 126 if (ret) 127 return ret; 128 if (!(cmd & CMD_UPDATE)) 129 return 0; 130 udelay(1); 131 } 132 133 WARN(1, "%s: rcg didn't update its configuration.", name); 134 return -EBUSY; 135 } 136 137 static int clk_rcg2_set_parent(struct clk_hw *hw, u8 index) 138 { 139 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 140 int ret; 141 u32 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; 142 143 ret = regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), 144 CFG_SRC_SEL_MASK, cfg); 145 if (ret) 146 return ret; 147 148 return update_config(rcg); 149 } 150 151 /* 152 * Calculate m/n:d rate 153 * 154 * parent_rate m 155 * rate = ----------- x --- 156 * hid_div n 157 */ 158 static unsigned long 159 calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 hid_div) 160 { 161 if (hid_div) { 162 rate *= 2; 163 rate /= hid_div + 1; 164 } 165 166 if (mode) { 167 u64 tmp = rate; 168 tmp *= m; 169 do_div(tmp, n); 170 rate = tmp; 171 } 172 173 return rate; 174 } 175 176 static unsigned long 177 __clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate, u32 cfg) 178 { 179 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 180 u32 hid_div, m = 0, n = 0, mode = 0, mask; 181 182 if (rcg->mnd_width) { 183 mask = BIT(rcg->mnd_width) - 1; 184 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m); 185 m &= mask; 186 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), &n); 187 n = ~n; 188 n &= mask; 189 n += m; 190 mode = cfg & CFG_MODE_MASK; 191 mode >>= CFG_MODE_SHIFT; 192 } 193 194 mask = BIT(rcg->hid_width) - 1; 195 hid_div = cfg >> CFG_SRC_DIV_SHIFT; 196 hid_div &= mask; 197 198 return calc_rate(parent_rate, m, n, mode, hid_div); 199 } 200 201 static unsigned long 202 clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) 203 { 204 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 205 u32 cfg; 206 207 regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg); 208 209 return __clk_rcg2_recalc_rate(hw, parent_rate, cfg); 210 } 211 212 static int _freq_tbl_determine_rate(struct clk_hw *hw, const struct freq_tbl *f, 213 struct clk_rate_request *req, 214 enum freq_policy policy) 215 { 216 unsigned long clk_flags, rate = req->rate; 217 struct clk_hw *p; 218 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 219 int index; 220 221 switch (policy) { 222 case FLOOR: 223 f = qcom_find_freq_floor(f, rate); 224 break; 225 case CEIL: 226 f = qcom_find_freq(f, rate); 227 break; 228 default: 229 return -EINVAL; 230 } 231 232 if (!f) 233 return -EINVAL; 234 235 index = qcom_find_src_index(hw, rcg->parent_map, f->src); 236 if (index < 0) 237 return index; 238 239 clk_flags = clk_hw_get_flags(hw); 240 p = clk_hw_get_parent_by_index(hw, index); 241 if (!p) 242 return -EINVAL; 243 244 if (clk_flags & CLK_SET_RATE_PARENT) { 245 rate = f->freq; 246 if (f->pre_div) { 247 if (!rate) 248 rate = req->rate; 249 rate /= 2; 250 rate *= f->pre_div + 1; 251 } 252 253 if (f->n) { 254 u64 tmp = rate; 255 tmp = tmp * f->n; 256 do_div(tmp, f->m); 257 rate = tmp; 258 } 259 } else { 260 rate = clk_hw_get_rate(p); 261 } 262 req->best_parent_hw = p; 263 req->best_parent_rate = rate; 264 req->rate = f->freq; 265 266 return 0; 267 } 268 269 static int clk_rcg2_determine_rate(struct clk_hw *hw, 270 struct clk_rate_request *req) 271 { 272 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 273 274 return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, CEIL); 275 } 276 277 static int clk_rcg2_determine_floor_rate(struct clk_hw *hw, 278 struct clk_rate_request *req) 279 { 280 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 281 282 return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, FLOOR); 283 } 284 285 static int __clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f, 286 u32 *_cfg) 287 { 288 u32 cfg, mask, d_val, not2d_val, n_minus_m; 289 struct clk_hw *hw = &rcg->clkr.hw; 290 int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src); 291 292 if (index < 0) 293 return index; 294 295 if (rcg->mnd_width && f->n) { 296 mask = BIT(rcg->mnd_width) - 1; 297 ret = regmap_update_bits(rcg->clkr.regmap, 298 RCG_M_OFFSET(rcg), mask, f->m); 299 if (ret) 300 return ret; 301 302 ret = regmap_update_bits(rcg->clkr.regmap, 303 RCG_N_OFFSET(rcg), mask, ~(f->n - f->m)); 304 if (ret) 305 return ret; 306 307 /* Calculate 2d value */ 308 d_val = f->n; 309 310 n_minus_m = f->n - f->m; 311 n_minus_m *= 2; 312 313 d_val = clamp_t(u32, d_val, f->m, n_minus_m); 314 not2d_val = ~d_val & mask; 315 316 ret = regmap_update_bits(rcg->clkr.regmap, 317 RCG_D_OFFSET(rcg), mask, not2d_val); 318 if (ret) 319 return ret; 320 } 321 322 mask = BIT(rcg->hid_width) - 1; 323 mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK | CFG_HW_CLK_CTRL_MASK; 324 cfg = f->pre_div << CFG_SRC_DIV_SHIFT; 325 cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; 326 if (rcg->mnd_width && f->n && (f->m != f->n)) 327 cfg |= CFG_MODE_DUAL_EDGE; 328 if (rcg->hw_clk_ctrl) 329 cfg |= CFG_HW_CLK_CTRL_MASK; 330 331 *_cfg &= ~mask; 332 *_cfg |= cfg; 333 334 return 0; 335 } 336 337 static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f) 338 { 339 u32 cfg; 340 int ret; 341 342 ret = regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg); 343 if (ret) 344 return ret; 345 346 ret = __clk_rcg2_configure(rcg, f, &cfg); 347 if (ret) 348 return ret; 349 350 ret = regmap_write(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), cfg); 351 if (ret) 352 return ret; 353 354 return update_config(rcg); 355 } 356 357 static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, 358 enum freq_policy policy) 359 { 360 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 361 const struct freq_tbl *f; 362 363 switch (policy) { 364 case FLOOR: 365 f = qcom_find_freq_floor(rcg->freq_tbl, rate); 366 break; 367 case CEIL: 368 f = qcom_find_freq(rcg->freq_tbl, rate); 369 break; 370 default: 371 return -EINVAL; 372 } 373 374 if (!f) 375 return -EINVAL; 376 377 return clk_rcg2_configure(rcg, f); 378 } 379 380 static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, 381 unsigned long parent_rate) 382 { 383 return __clk_rcg2_set_rate(hw, rate, CEIL); 384 } 385 386 static int clk_rcg2_set_floor_rate(struct clk_hw *hw, unsigned long rate, 387 unsigned long parent_rate) 388 { 389 return __clk_rcg2_set_rate(hw, rate, FLOOR); 390 } 391 392 static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw, 393 unsigned long rate, unsigned long parent_rate, u8 index) 394 { 395 return __clk_rcg2_set_rate(hw, rate, CEIL); 396 } 397 398 static int clk_rcg2_set_floor_rate_and_parent(struct clk_hw *hw, 399 unsigned long rate, unsigned long parent_rate, u8 index) 400 { 401 return __clk_rcg2_set_rate(hw, rate, FLOOR); 402 } 403 404 static int clk_rcg2_get_duty_cycle(struct clk_hw *hw, struct clk_duty *duty) 405 { 406 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 407 u32 notn_m, n, m, d, not2d, mask; 408 409 if (!rcg->mnd_width) { 410 /* 50 % duty-cycle for Non-MND RCGs */ 411 duty->num = 1; 412 duty->den = 2; 413 return 0; 414 } 415 416 regmap_read(rcg->clkr.regmap, RCG_D_OFFSET(rcg), ¬2d); 417 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m); 418 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), ¬n_m); 419 420 if (!not2d && !m && !notn_m) { 421 /* 50 % duty-cycle always */ 422 duty->num = 1; 423 duty->den = 2; 424 return 0; 425 } 426 427 mask = BIT(rcg->mnd_width) - 1; 428 429 d = ~(not2d) & mask; 430 d = DIV_ROUND_CLOSEST(d, 2); 431 432 n = (~(notn_m) + m) & mask; 433 434 duty->num = d; 435 duty->den = n; 436 437 return 0; 438 } 439 440 static int clk_rcg2_set_duty_cycle(struct clk_hw *hw, struct clk_duty *duty) 441 { 442 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 443 u32 notn_m, n, m, d, not2d, mask, duty_per, cfg; 444 int ret; 445 446 /* Duty-cycle cannot be modified for non-MND RCGs */ 447 if (!rcg->mnd_width) 448 return -EINVAL; 449 450 mask = BIT(rcg->mnd_width) - 1; 451 452 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), ¬n_m); 453 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m); 454 regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg); 455 456 /* Duty-cycle cannot be modified if MND divider is in bypass mode. */ 457 if (!(cfg & CFG_MODE_MASK)) 458 return -EINVAL; 459 460 n = (~(notn_m) + m) & mask; 461 462 duty_per = (duty->num * 100) / duty->den; 463 464 /* Calculate 2d value */ 465 d = DIV_ROUND_CLOSEST(n * duty_per * 2, 100); 466 467 /* 468 * Check bit widths of 2d. If D is too big reduce duty cycle. 469 * Also make sure it is never zero. 470 */ 471 d = clamp_val(d, 1, mask); 472 473 if ((d / 2) > (n - m)) 474 d = (n - m) * 2; 475 else if ((d / 2) < (m / 2)) 476 d = m; 477 478 not2d = ~d & mask; 479 480 ret = regmap_update_bits(rcg->clkr.regmap, RCG_D_OFFSET(rcg), mask, 481 not2d); 482 if (ret) 483 return ret; 484 485 return update_config(rcg); 486 } 487 488 const struct clk_ops clk_rcg2_ops = { 489 .is_enabled = clk_rcg2_is_enabled, 490 .get_parent = clk_rcg2_get_parent, 491 .set_parent = clk_rcg2_set_parent, 492 .recalc_rate = clk_rcg2_recalc_rate, 493 .determine_rate = clk_rcg2_determine_rate, 494 .set_rate = clk_rcg2_set_rate, 495 .set_rate_and_parent = clk_rcg2_set_rate_and_parent, 496 .get_duty_cycle = clk_rcg2_get_duty_cycle, 497 .set_duty_cycle = clk_rcg2_set_duty_cycle, 498 }; 499 EXPORT_SYMBOL_GPL(clk_rcg2_ops); 500 501 const struct clk_ops clk_rcg2_floor_ops = { 502 .is_enabled = clk_rcg2_is_enabled, 503 .get_parent = clk_rcg2_get_parent, 504 .set_parent = clk_rcg2_set_parent, 505 .recalc_rate = clk_rcg2_recalc_rate, 506 .determine_rate = clk_rcg2_determine_floor_rate, 507 .set_rate = clk_rcg2_set_floor_rate, 508 .set_rate_and_parent = clk_rcg2_set_floor_rate_and_parent, 509 .get_duty_cycle = clk_rcg2_get_duty_cycle, 510 .set_duty_cycle = clk_rcg2_set_duty_cycle, 511 }; 512 EXPORT_SYMBOL_GPL(clk_rcg2_floor_ops); 513 514 const struct clk_ops clk_rcg2_mux_closest_ops = { 515 .determine_rate = __clk_mux_determine_rate_closest, 516 .get_parent = clk_rcg2_get_parent, 517 .set_parent = clk_rcg2_set_parent, 518 }; 519 EXPORT_SYMBOL_GPL(clk_rcg2_mux_closest_ops); 520 521 struct frac_entry { 522 int num; 523 int den; 524 }; 525 526 static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */ 527 { 52, 295 }, /* 119 M */ 528 { 11, 57 }, /* 130.25 M */ 529 { 63, 307 }, /* 138.50 M */ 530 { 11, 50 }, /* 148.50 M */ 531 { 47, 206 }, /* 154 M */ 532 { 31, 100 }, /* 205.25 M */ 533 { 107, 269 }, /* 268.50 M */ 534 { }, 535 }; 536 537 static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */ 538 { 31, 211 }, /* 119 M */ 539 { 32, 199 }, /* 130.25 M */ 540 { 63, 307 }, /* 138.50 M */ 541 { 11, 60 }, /* 148.50 M */ 542 { 50, 263 }, /* 154 M */ 543 { 31, 120 }, /* 205.25 M */ 544 { 119, 359 }, /* 268.50 M */ 545 { }, 546 }; 547 548 static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate, 549 unsigned long parent_rate) 550 { 551 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 552 struct freq_tbl f = *rcg->freq_tbl; 553 const struct frac_entry *frac; 554 int delta = 100000; 555 s64 src_rate = parent_rate; 556 s64 request; 557 u32 mask = BIT(rcg->hid_width) - 1; 558 u32 hid_div; 559 560 if (src_rate == 810000000) 561 frac = frac_table_810m; 562 else 563 frac = frac_table_675m; 564 565 for (; frac->num; frac++) { 566 request = rate; 567 request *= frac->den; 568 request = div_s64(request, frac->num); 569 if ((src_rate < (request - delta)) || 570 (src_rate > (request + delta))) 571 continue; 572 573 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, 574 &hid_div); 575 f.pre_div = hid_div; 576 f.pre_div >>= CFG_SRC_DIV_SHIFT; 577 f.pre_div &= mask; 578 f.m = frac->num; 579 f.n = frac->den; 580 581 return clk_rcg2_configure(rcg, &f); 582 } 583 584 return -EINVAL; 585 } 586 587 static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw, 588 unsigned long rate, unsigned long parent_rate, u8 index) 589 { 590 /* Parent index is set statically in frequency table */ 591 return clk_edp_pixel_set_rate(hw, rate, parent_rate); 592 } 593 594 static int clk_edp_pixel_determine_rate(struct clk_hw *hw, 595 struct clk_rate_request *req) 596 { 597 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 598 const struct freq_tbl *f = rcg->freq_tbl; 599 const struct frac_entry *frac; 600 int delta = 100000; 601 s64 request; 602 u32 mask = BIT(rcg->hid_width) - 1; 603 u32 hid_div; 604 int index = qcom_find_src_index(hw, rcg->parent_map, f->src); 605 606 /* Force the correct parent */ 607 req->best_parent_hw = clk_hw_get_parent_by_index(hw, index); 608 req->best_parent_rate = clk_hw_get_rate(req->best_parent_hw); 609 610 if (req->best_parent_rate == 810000000) 611 frac = frac_table_810m; 612 else 613 frac = frac_table_675m; 614 615 for (; frac->num; frac++) { 616 request = req->rate; 617 request *= frac->den; 618 request = div_s64(request, frac->num); 619 if ((req->best_parent_rate < (request - delta)) || 620 (req->best_parent_rate > (request + delta))) 621 continue; 622 623 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, 624 &hid_div); 625 hid_div >>= CFG_SRC_DIV_SHIFT; 626 hid_div &= mask; 627 628 req->rate = calc_rate(req->best_parent_rate, 629 frac->num, frac->den, 630 !!frac->den, hid_div); 631 return 0; 632 } 633 634 return -EINVAL; 635 } 636 637 const struct clk_ops clk_edp_pixel_ops = { 638 .is_enabled = clk_rcg2_is_enabled, 639 .get_parent = clk_rcg2_get_parent, 640 .set_parent = clk_rcg2_set_parent, 641 .recalc_rate = clk_rcg2_recalc_rate, 642 .set_rate = clk_edp_pixel_set_rate, 643 .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent, 644 .determine_rate = clk_edp_pixel_determine_rate, 645 }; 646 EXPORT_SYMBOL_GPL(clk_edp_pixel_ops); 647 648 static int clk_byte_determine_rate(struct clk_hw *hw, 649 struct clk_rate_request *req) 650 { 651 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 652 const struct freq_tbl *f = rcg->freq_tbl; 653 int index = qcom_find_src_index(hw, rcg->parent_map, f->src); 654 unsigned long parent_rate, div; 655 u32 mask = BIT(rcg->hid_width) - 1; 656 struct clk_hw *p; 657 658 if (req->rate == 0) 659 return -EINVAL; 660 661 req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index); 662 req->best_parent_rate = parent_rate = clk_hw_round_rate(p, req->rate); 663 664 div = DIV_ROUND_UP((2 * parent_rate), req->rate) - 1; 665 div = min_t(u32, div, mask); 666 667 req->rate = calc_rate(parent_rate, 0, 0, 0, div); 668 669 return 0; 670 } 671 672 static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate, 673 unsigned long parent_rate) 674 { 675 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 676 struct freq_tbl f = *rcg->freq_tbl; 677 unsigned long div; 678 u32 mask = BIT(rcg->hid_width) - 1; 679 680 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; 681 div = min_t(u32, div, mask); 682 683 f.pre_div = div; 684 685 return clk_rcg2_configure(rcg, &f); 686 } 687 688 static int clk_byte_set_rate_and_parent(struct clk_hw *hw, 689 unsigned long rate, unsigned long parent_rate, u8 index) 690 { 691 /* Parent index is set statically in frequency table */ 692 return clk_byte_set_rate(hw, rate, parent_rate); 693 } 694 695 const struct clk_ops clk_byte_ops = { 696 .is_enabled = clk_rcg2_is_enabled, 697 .get_parent = clk_rcg2_get_parent, 698 .set_parent = clk_rcg2_set_parent, 699 .recalc_rate = clk_rcg2_recalc_rate, 700 .set_rate = clk_byte_set_rate, 701 .set_rate_and_parent = clk_byte_set_rate_and_parent, 702 .determine_rate = clk_byte_determine_rate, 703 }; 704 EXPORT_SYMBOL_GPL(clk_byte_ops); 705 706 static int clk_byte2_determine_rate(struct clk_hw *hw, 707 struct clk_rate_request *req) 708 { 709 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 710 unsigned long parent_rate, div; 711 u32 mask = BIT(rcg->hid_width) - 1; 712 struct clk_hw *p; 713 unsigned long rate = req->rate; 714 715 if (rate == 0) 716 return -EINVAL; 717 718 p = req->best_parent_hw; 719 req->best_parent_rate = parent_rate = clk_hw_round_rate(p, rate); 720 721 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; 722 div = min_t(u32, div, mask); 723 724 req->rate = calc_rate(parent_rate, 0, 0, 0, div); 725 726 return 0; 727 } 728 729 static int clk_byte2_set_rate(struct clk_hw *hw, unsigned long rate, 730 unsigned long parent_rate) 731 { 732 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 733 struct freq_tbl f = { 0 }; 734 unsigned long div; 735 int i, num_parents = clk_hw_get_num_parents(hw); 736 u32 mask = BIT(rcg->hid_width) - 1; 737 u32 cfg; 738 739 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; 740 div = min_t(u32, div, mask); 741 742 f.pre_div = div; 743 744 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg); 745 cfg &= CFG_SRC_SEL_MASK; 746 cfg >>= CFG_SRC_SEL_SHIFT; 747 748 for (i = 0; i < num_parents; i++) { 749 if (cfg == rcg->parent_map[i].cfg) { 750 f.src = rcg->parent_map[i].src; 751 return clk_rcg2_configure(rcg, &f); 752 } 753 } 754 755 return -EINVAL; 756 } 757 758 static int clk_byte2_set_rate_and_parent(struct clk_hw *hw, 759 unsigned long rate, unsigned long parent_rate, u8 index) 760 { 761 /* Read the hardware to determine parent during set_rate */ 762 return clk_byte2_set_rate(hw, rate, parent_rate); 763 } 764 765 const struct clk_ops clk_byte2_ops = { 766 .is_enabled = clk_rcg2_is_enabled, 767 .get_parent = clk_rcg2_get_parent, 768 .set_parent = clk_rcg2_set_parent, 769 .recalc_rate = clk_rcg2_recalc_rate, 770 .set_rate = clk_byte2_set_rate, 771 .set_rate_and_parent = clk_byte2_set_rate_and_parent, 772 .determine_rate = clk_byte2_determine_rate, 773 }; 774 EXPORT_SYMBOL_GPL(clk_byte2_ops); 775 776 static const struct frac_entry frac_table_pixel[] = { 777 { 3, 8 }, 778 { 2, 9 }, 779 { 4, 9 }, 780 { 1, 1 }, 781 { 2, 3 }, 782 { } 783 }; 784 785 static int clk_pixel_determine_rate(struct clk_hw *hw, 786 struct clk_rate_request *req) 787 { 788 unsigned long request, src_rate; 789 int delta = 100000; 790 const struct frac_entry *frac = frac_table_pixel; 791 792 for (; frac->num; frac++) { 793 request = (req->rate * frac->den) / frac->num; 794 795 src_rate = clk_hw_round_rate(req->best_parent_hw, request); 796 if ((src_rate < (request - delta)) || 797 (src_rate > (request + delta))) 798 continue; 799 800 req->best_parent_rate = src_rate; 801 req->rate = (src_rate * frac->num) / frac->den; 802 return 0; 803 } 804 805 return -EINVAL; 806 } 807 808 static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate, 809 unsigned long parent_rate) 810 { 811 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 812 struct freq_tbl f = { 0 }; 813 const struct frac_entry *frac = frac_table_pixel; 814 unsigned long request; 815 int delta = 100000; 816 u32 mask = BIT(rcg->hid_width) - 1; 817 u32 hid_div, cfg; 818 int i, num_parents = clk_hw_get_num_parents(hw); 819 820 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg); 821 cfg &= CFG_SRC_SEL_MASK; 822 cfg >>= CFG_SRC_SEL_SHIFT; 823 824 for (i = 0; i < num_parents; i++) 825 if (cfg == rcg->parent_map[i].cfg) { 826 f.src = rcg->parent_map[i].src; 827 break; 828 } 829 830 for (; frac->num; frac++) { 831 request = (rate * frac->den) / frac->num; 832 833 if ((parent_rate < (request - delta)) || 834 (parent_rate > (request + delta))) 835 continue; 836 837 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, 838 &hid_div); 839 f.pre_div = hid_div; 840 f.pre_div >>= CFG_SRC_DIV_SHIFT; 841 f.pre_div &= mask; 842 f.m = frac->num; 843 f.n = frac->den; 844 845 return clk_rcg2_configure(rcg, &f); 846 } 847 return -EINVAL; 848 } 849 850 static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, 851 unsigned long parent_rate, u8 index) 852 { 853 return clk_pixel_set_rate(hw, rate, parent_rate); 854 } 855 856 const struct clk_ops clk_pixel_ops = { 857 .is_enabled = clk_rcg2_is_enabled, 858 .get_parent = clk_rcg2_get_parent, 859 .set_parent = clk_rcg2_set_parent, 860 .recalc_rate = clk_rcg2_recalc_rate, 861 .set_rate = clk_pixel_set_rate, 862 .set_rate_and_parent = clk_pixel_set_rate_and_parent, 863 .determine_rate = clk_pixel_determine_rate, 864 }; 865 EXPORT_SYMBOL_GPL(clk_pixel_ops); 866 867 static int clk_gfx3d_determine_rate(struct clk_hw *hw, 868 struct clk_rate_request *req) 869 { 870 struct clk_rate_request parent_req = { .min_rate = 0, .max_rate = ULONG_MAX }; 871 struct clk_rcg2_gfx3d *cgfx = to_clk_rcg2_gfx3d(hw); 872 struct clk_hw *xo, *p0, *p1, *p2; 873 unsigned long p0_rate; 874 u8 mux_div = cgfx->div; 875 int ret; 876 877 p0 = cgfx->hws[0]; 878 p1 = cgfx->hws[1]; 879 p2 = cgfx->hws[2]; 880 /* 881 * This function does ping-pong the RCG between PLLs: if we don't 882 * have at least one fixed PLL and two variable ones, 883 * then it's not going to work correctly. 884 */ 885 if (WARN_ON(!p0 || !p1 || !p2)) 886 return -EINVAL; 887 888 xo = clk_hw_get_parent_by_index(hw, 0); 889 if (req->rate == clk_hw_get_rate(xo)) { 890 req->best_parent_hw = xo; 891 return 0; 892 } 893 894 if (mux_div == 0) 895 mux_div = 1; 896 897 parent_req.rate = req->rate * mux_div; 898 899 /* This has to be a fixed rate PLL */ 900 p0_rate = clk_hw_get_rate(p0); 901 902 if (parent_req.rate == p0_rate) { 903 req->rate = req->best_parent_rate = p0_rate; 904 req->best_parent_hw = p0; 905 return 0; 906 } 907 908 if (req->best_parent_hw == p0) { 909 /* Are we going back to a previously used rate? */ 910 if (clk_hw_get_rate(p2) == parent_req.rate) 911 req->best_parent_hw = p2; 912 else 913 req->best_parent_hw = p1; 914 } else if (req->best_parent_hw == p2) { 915 req->best_parent_hw = p1; 916 } else { 917 req->best_parent_hw = p2; 918 } 919 920 clk_hw_get_rate_range(req->best_parent_hw, 921 &parent_req.min_rate, &parent_req.max_rate); 922 923 if (req->min_rate > parent_req.min_rate) 924 parent_req.min_rate = req->min_rate; 925 926 if (req->max_rate < parent_req.max_rate) 927 parent_req.max_rate = req->max_rate; 928 929 ret = __clk_determine_rate(req->best_parent_hw, &parent_req); 930 if (ret) 931 return ret; 932 933 req->rate = req->best_parent_rate = parent_req.rate; 934 req->rate /= mux_div; 935 936 return 0; 937 } 938 939 static int clk_gfx3d_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, 940 unsigned long parent_rate, u8 index) 941 { 942 struct clk_rcg2_gfx3d *cgfx = to_clk_rcg2_gfx3d(hw); 943 struct clk_rcg2 *rcg = &cgfx->rcg; 944 u32 cfg; 945 int ret; 946 947 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; 948 /* On some targets, the GFX3D RCG may need to divide PLL frequency */ 949 if (cgfx->div > 1) 950 cfg |= ((2 * cgfx->div) - 1) << CFG_SRC_DIV_SHIFT; 951 952 ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg); 953 if (ret) 954 return ret; 955 956 return update_config(rcg); 957 } 958 959 static int clk_gfx3d_set_rate(struct clk_hw *hw, unsigned long rate, 960 unsigned long parent_rate) 961 { 962 /* 963 * We should never get here; clk_gfx3d_determine_rate() should always 964 * make us use a different parent than what we're currently using, so 965 * clk_gfx3d_set_rate_and_parent() should always be called. 966 */ 967 return 0; 968 } 969 970 const struct clk_ops clk_gfx3d_ops = { 971 .is_enabled = clk_rcg2_is_enabled, 972 .get_parent = clk_rcg2_get_parent, 973 .set_parent = clk_rcg2_set_parent, 974 .recalc_rate = clk_rcg2_recalc_rate, 975 .set_rate = clk_gfx3d_set_rate, 976 .set_rate_and_parent = clk_gfx3d_set_rate_and_parent, 977 .determine_rate = clk_gfx3d_determine_rate, 978 }; 979 EXPORT_SYMBOL_GPL(clk_gfx3d_ops); 980 981 static int clk_rcg2_set_force_enable(struct clk_hw *hw) 982 { 983 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 984 const char *name = clk_hw_get_name(hw); 985 int ret, count; 986 987 ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, 988 CMD_ROOT_EN, CMD_ROOT_EN); 989 if (ret) 990 return ret; 991 992 /* wait for RCG to turn ON */ 993 for (count = 500; count > 0; count--) { 994 if (clk_rcg2_is_enabled(hw)) 995 return 0; 996 997 udelay(1); 998 } 999 1000 pr_err("%s: RCG did not turn on\n", name); 1001 return -ETIMEDOUT; 1002 } 1003 1004 static int clk_rcg2_clear_force_enable(struct clk_hw *hw) 1005 { 1006 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1007 1008 return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, 1009 CMD_ROOT_EN, 0); 1010 } 1011 1012 static int 1013 clk_rcg2_shared_force_enable_clear(struct clk_hw *hw, const struct freq_tbl *f) 1014 { 1015 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1016 int ret; 1017 1018 ret = clk_rcg2_set_force_enable(hw); 1019 if (ret) 1020 return ret; 1021 1022 ret = clk_rcg2_configure(rcg, f); 1023 if (ret) 1024 return ret; 1025 1026 return clk_rcg2_clear_force_enable(hw); 1027 } 1028 1029 static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate, 1030 unsigned long parent_rate) 1031 { 1032 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1033 const struct freq_tbl *f; 1034 1035 f = qcom_find_freq(rcg->freq_tbl, rate); 1036 if (!f) 1037 return -EINVAL; 1038 1039 /* 1040 * In case clock is disabled, update the M, N and D registers, cache 1041 * the CFG value in parked_cfg and don't hit the update bit of CMD 1042 * register. 1043 */ 1044 if (!clk_hw_is_enabled(hw)) 1045 return __clk_rcg2_configure(rcg, f, &rcg->parked_cfg); 1046 1047 return clk_rcg2_shared_force_enable_clear(hw, f); 1048 } 1049 1050 static int clk_rcg2_shared_set_rate_and_parent(struct clk_hw *hw, 1051 unsigned long rate, unsigned long parent_rate, u8 index) 1052 { 1053 return clk_rcg2_shared_set_rate(hw, rate, parent_rate); 1054 } 1055 1056 static int clk_rcg2_shared_enable(struct clk_hw *hw) 1057 { 1058 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1059 int ret; 1060 1061 /* 1062 * Set the update bit because required configuration has already 1063 * been written in clk_rcg2_shared_set_rate() 1064 */ 1065 ret = clk_rcg2_set_force_enable(hw); 1066 if (ret) 1067 return ret; 1068 1069 /* Write back the stored configuration corresponding to current rate */ 1070 ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, rcg->parked_cfg); 1071 if (ret) 1072 return ret; 1073 1074 ret = update_config(rcg); 1075 if (ret) 1076 return ret; 1077 1078 return clk_rcg2_clear_force_enable(hw); 1079 } 1080 1081 static void clk_rcg2_shared_disable(struct clk_hw *hw) 1082 { 1083 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1084 1085 /* 1086 * Store current configuration as switching to safe source would clear 1087 * the SRC and DIV of CFG register 1088 */ 1089 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &rcg->parked_cfg); 1090 1091 /* 1092 * Park the RCG at a safe configuration - sourced off of safe source. 1093 * Force enable and disable the RCG while configuring it to safeguard 1094 * against any update signal coming from the downstream clock. 1095 * The current parent is still prepared and enabled at this point, and 1096 * the safe source is always on while application processor subsystem 1097 * is online. Therefore, the RCG can safely switch its parent. 1098 */ 1099 clk_rcg2_set_force_enable(hw); 1100 1101 regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, 1102 rcg->safe_src_index << CFG_SRC_SEL_SHIFT); 1103 1104 update_config(rcg); 1105 1106 clk_rcg2_clear_force_enable(hw); 1107 } 1108 1109 static u8 clk_rcg2_shared_get_parent(struct clk_hw *hw) 1110 { 1111 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1112 1113 /* If the shared rcg is parked use the cached cfg instead */ 1114 if (!clk_hw_is_enabled(hw)) 1115 return __clk_rcg2_get_parent(hw, rcg->parked_cfg); 1116 1117 return clk_rcg2_get_parent(hw); 1118 } 1119 1120 static int clk_rcg2_shared_set_parent(struct clk_hw *hw, u8 index) 1121 { 1122 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1123 1124 /* If the shared rcg is parked only update the cached cfg */ 1125 if (!clk_hw_is_enabled(hw)) { 1126 rcg->parked_cfg &= ~CFG_SRC_SEL_MASK; 1127 rcg->parked_cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; 1128 1129 return 0; 1130 } 1131 1132 return clk_rcg2_set_parent(hw, index); 1133 } 1134 1135 static unsigned long 1136 clk_rcg2_shared_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) 1137 { 1138 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1139 1140 /* If the shared rcg is parked use the cached cfg instead */ 1141 if (!clk_hw_is_enabled(hw)) 1142 return __clk_rcg2_recalc_rate(hw, parent_rate, rcg->parked_cfg); 1143 1144 return clk_rcg2_recalc_rate(hw, parent_rate); 1145 } 1146 1147 const struct clk_ops clk_rcg2_shared_ops = { 1148 .enable = clk_rcg2_shared_enable, 1149 .disable = clk_rcg2_shared_disable, 1150 .get_parent = clk_rcg2_shared_get_parent, 1151 .set_parent = clk_rcg2_shared_set_parent, 1152 .recalc_rate = clk_rcg2_shared_recalc_rate, 1153 .determine_rate = clk_rcg2_determine_rate, 1154 .set_rate = clk_rcg2_shared_set_rate, 1155 .set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent, 1156 }; 1157 EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops); 1158 1159 /* Common APIs to be used for DFS based RCGR */ 1160 static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l, 1161 struct freq_tbl *f) 1162 { 1163 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1164 struct clk_hw *p; 1165 unsigned long prate = 0; 1166 u32 val, mask, cfg, mode, src; 1167 int i, num_parents; 1168 1169 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg); 1170 1171 mask = BIT(rcg->hid_width) - 1; 1172 f->pre_div = 1; 1173 if (cfg & mask) 1174 f->pre_div = cfg & mask; 1175 1176 src = cfg & CFG_SRC_SEL_MASK; 1177 src >>= CFG_SRC_SEL_SHIFT; 1178 1179 num_parents = clk_hw_get_num_parents(hw); 1180 for (i = 0; i < num_parents; i++) { 1181 if (src == rcg->parent_map[i].cfg) { 1182 f->src = rcg->parent_map[i].src; 1183 p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i); 1184 prate = clk_hw_get_rate(p); 1185 } 1186 } 1187 1188 mode = cfg & CFG_MODE_MASK; 1189 mode >>= CFG_MODE_SHIFT; 1190 if (mode) { 1191 mask = BIT(rcg->mnd_width) - 1; 1192 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l), 1193 &val); 1194 val &= mask; 1195 f->m = val; 1196 1197 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l), 1198 &val); 1199 val = ~val; 1200 val &= mask; 1201 val += f->m; 1202 f->n = val; 1203 } 1204 1205 f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div); 1206 } 1207 1208 static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg) 1209 { 1210 struct freq_tbl *freq_tbl; 1211 int i; 1212 1213 /* Allocate space for 1 extra since table is NULL terminated */ 1214 freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL); 1215 if (!freq_tbl) 1216 return -ENOMEM; 1217 rcg->freq_tbl = freq_tbl; 1218 1219 for (i = 0; i < MAX_PERF_LEVEL; i++) 1220 clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i); 1221 1222 return 0; 1223 } 1224 1225 static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw, 1226 struct clk_rate_request *req) 1227 { 1228 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1229 int ret; 1230 1231 if (!rcg->freq_tbl) { 1232 ret = clk_rcg2_dfs_populate_freq_table(rcg); 1233 if (ret) { 1234 pr_err("Failed to update DFS tables for %s\n", 1235 clk_hw_get_name(hw)); 1236 return ret; 1237 } 1238 } 1239 1240 return clk_rcg2_determine_rate(hw, req); 1241 } 1242 1243 static unsigned long 1244 clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) 1245 { 1246 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1247 u32 level, mask, cfg, m = 0, n = 0, mode, pre_div; 1248 1249 regmap_read(rcg->clkr.regmap, 1250 rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level); 1251 level &= GENMASK(4, 1); 1252 level >>= 1; 1253 1254 if (rcg->freq_tbl) 1255 return rcg->freq_tbl[level].freq; 1256 1257 /* 1258 * Assume that parent_rate is actually the parent because 1259 * we can't do any better at figuring it out when the table 1260 * hasn't been populated yet. We only populate the table 1261 * in determine_rate because we can't guarantee the parents 1262 * will be registered with the framework until then. 1263 */ 1264 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level), 1265 &cfg); 1266 1267 mask = BIT(rcg->hid_width) - 1; 1268 pre_div = 1; 1269 if (cfg & mask) 1270 pre_div = cfg & mask; 1271 1272 mode = cfg & CFG_MODE_MASK; 1273 mode >>= CFG_MODE_SHIFT; 1274 if (mode) { 1275 mask = BIT(rcg->mnd_width) - 1; 1276 regmap_read(rcg->clkr.regmap, 1277 rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m); 1278 m &= mask; 1279 1280 regmap_read(rcg->clkr.regmap, 1281 rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n); 1282 n = ~n; 1283 n &= mask; 1284 n += m; 1285 } 1286 1287 return calc_rate(parent_rate, m, n, mode, pre_div); 1288 } 1289 1290 static const struct clk_ops clk_rcg2_dfs_ops = { 1291 .is_enabled = clk_rcg2_is_enabled, 1292 .get_parent = clk_rcg2_get_parent, 1293 .determine_rate = clk_rcg2_dfs_determine_rate, 1294 .recalc_rate = clk_rcg2_dfs_recalc_rate, 1295 }; 1296 1297 static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data, 1298 struct regmap *regmap) 1299 { 1300 struct clk_rcg2 *rcg = data->rcg; 1301 struct clk_init_data *init = data->init; 1302 u32 val; 1303 int ret; 1304 1305 ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val); 1306 if (ret) 1307 return -EINVAL; 1308 1309 if (!(val & SE_CMD_DFS_EN)) 1310 return 0; 1311 1312 /* 1313 * Rate changes with consumer writing a register in 1314 * their own I/O region 1315 */ 1316 init->flags |= CLK_GET_RATE_NOCACHE; 1317 init->ops = &clk_rcg2_dfs_ops; 1318 1319 rcg->freq_tbl = NULL; 1320 1321 return 0; 1322 } 1323 1324 int qcom_cc_register_rcg_dfs(struct regmap *regmap, 1325 const struct clk_rcg_dfs_data *rcgs, size_t len) 1326 { 1327 int i, ret; 1328 1329 for (i = 0; i < len; i++) { 1330 ret = clk_rcg2_enable_dfs(&rcgs[i], regmap); 1331 if (ret) 1332 return ret; 1333 } 1334 1335 return 0; 1336 } 1337 EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs); 1338 1339 static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate, 1340 unsigned long parent_rate) 1341 { 1342 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1343 struct freq_tbl f = { 0 }; 1344 u32 mask = BIT(rcg->hid_width) - 1; 1345 u32 hid_div, cfg; 1346 int i, num_parents = clk_hw_get_num_parents(hw); 1347 unsigned long num, den; 1348 1349 rational_best_approximation(parent_rate, rate, 1350 GENMASK(rcg->mnd_width - 1, 0), 1351 GENMASK(rcg->mnd_width - 1, 0), &den, &num); 1352 1353 if (!num || !den) 1354 return -EINVAL; 1355 1356 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg); 1357 hid_div = cfg; 1358 cfg &= CFG_SRC_SEL_MASK; 1359 cfg >>= CFG_SRC_SEL_SHIFT; 1360 1361 for (i = 0; i < num_parents; i++) { 1362 if (cfg == rcg->parent_map[i].cfg) { 1363 f.src = rcg->parent_map[i].src; 1364 break; 1365 } 1366 } 1367 1368 f.pre_div = hid_div; 1369 f.pre_div >>= CFG_SRC_DIV_SHIFT; 1370 f.pre_div &= mask; 1371 1372 if (num != den) { 1373 f.m = num; 1374 f.n = den; 1375 } else { 1376 f.m = 0; 1377 f.n = 0; 1378 } 1379 1380 return clk_rcg2_configure(rcg, &f); 1381 } 1382 1383 static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw, 1384 unsigned long rate, unsigned long parent_rate, u8 index) 1385 { 1386 return clk_rcg2_dp_set_rate(hw, rate, parent_rate); 1387 } 1388 1389 static int clk_rcg2_dp_determine_rate(struct clk_hw *hw, 1390 struct clk_rate_request *req) 1391 { 1392 struct clk_rcg2 *rcg = to_clk_rcg2(hw); 1393 unsigned long num, den; 1394 u64 tmp; 1395 1396 /* Parent rate is a fixed phy link rate */ 1397 rational_best_approximation(req->best_parent_rate, req->rate, 1398 GENMASK(rcg->mnd_width - 1, 0), 1399 GENMASK(rcg->mnd_width - 1, 0), &den, &num); 1400 1401 if (!num || !den) 1402 return -EINVAL; 1403 1404 tmp = req->best_parent_rate * num; 1405 do_div(tmp, den); 1406 req->rate = tmp; 1407 1408 return 0; 1409 } 1410 1411 const struct clk_ops clk_dp_ops = { 1412 .is_enabled = clk_rcg2_is_enabled, 1413 .get_parent = clk_rcg2_get_parent, 1414 .set_parent = clk_rcg2_set_parent, 1415 .recalc_rate = clk_rcg2_recalc_rate, 1416 .set_rate = clk_rcg2_dp_set_rate, 1417 .set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent, 1418 .determine_rate = clk_rcg2_dp_determine_rate, 1419 }; 1420 EXPORT_SYMBOL_GPL(clk_dp_ops); 1421