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