1643 lines · c
1// SPDX-License-Identifier: GPL-2.02/*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 0x025#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 0x434#define CFG_SRC_DIV_SHIFT 035#define CFG_SRC_SEL_SHIFT 836#define CFG_SRC_SEL_MASK (0x7 << CFG_SRC_SEL_SHIFT)37#define CFG_MODE_SHIFT 1238#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 0x843#define N_REG 0xc44#define D_REG 0x1045 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 853#define SE_CMD_DFSR_OFFSET 0x1454#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 59enum freq_policy {60 FLOOR,61 CEIL,62};63 64static 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 77static 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 95static 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 111static 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 137static 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 rate153 *154 * parent_rate m155 * rate = ----------- x ---156 * hid_div n157 */158static unsigned long159calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 hid_div)160{161 if (hid_div)162 rate = mult_frac(rate, 2, hid_div + 1);163 164 if (mode)165 rate = mult_frac(rate, m, n);166 167 return rate;168}169 170static unsigned long171__clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate, u32 cfg)172{173 struct clk_rcg2 *rcg = to_clk_rcg2(hw);174 u32 hid_div, m = 0, n = 0, mode = 0, mask;175 176 if (rcg->mnd_width) {177 mask = BIT(rcg->mnd_width) - 1;178 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);179 m &= mask;180 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), &n);181 n = ~n;182 n &= mask;183 n += m;184 mode = cfg & CFG_MODE_MASK;185 mode >>= CFG_MODE_SHIFT;186 }187 188 mask = BIT(rcg->hid_width) - 1;189 hid_div = cfg >> CFG_SRC_DIV_SHIFT;190 hid_div &= mask;191 192 return calc_rate(parent_rate, m, n, mode, hid_div);193}194 195static unsigned long196clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)197{198 struct clk_rcg2 *rcg = to_clk_rcg2(hw);199 u32 cfg;200 201 regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);202 203 return __clk_rcg2_recalc_rate(hw, parent_rate, cfg);204}205 206static int _freq_tbl_determine_rate(struct clk_hw *hw, const struct freq_tbl *f,207 struct clk_rate_request *req,208 enum freq_policy policy)209{210 unsigned long clk_flags, rate = req->rate;211 struct clk_hw *p;212 struct clk_rcg2 *rcg = to_clk_rcg2(hw);213 int index;214 215 switch (policy) {216 case FLOOR:217 f = qcom_find_freq_floor(f, rate);218 break;219 case CEIL:220 f = qcom_find_freq(f, rate);221 break;222 default:223 return -EINVAL;224 }225 226 if (!f)227 return -EINVAL;228 229 index = qcom_find_src_index(hw, rcg->parent_map, f->src);230 if (index < 0)231 return index;232 233 clk_flags = clk_hw_get_flags(hw);234 p = clk_hw_get_parent_by_index(hw, index);235 if (!p)236 return -EINVAL;237 238 if (clk_flags & CLK_SET_RATE_PARENT) {239 rate = f->freq;240 if (f->pre_div) {241 if (!rate)242 rate = req->rate;243 rate /= 2;244 rate *= f->pre_div + 1;245 }246 247 if (f->n) {248 u64 tmp = rate;249 tmp = tmp * f->n;250 do_div(tmp, f->m);251 rate = tmp;252 }253 } else {254 rate = clk_hw_get_rate(p);255 }256 req->best_parent_hw = p;257 req->best_parent_rate = rate;258 req->rate = f->freq;259 260 return 0;261}262 263static const struct freq_conf *264__clk_rcg2_select_conf(struct clk_hw *hw, const struct freq_multi_tbl *f,265 unsigned long req_rate)266{267 unsigned long rate_diff, best_rate_diff = ULONG_MAX;268 const struct freq_conf *conf, *best_conf = NULL;269 struct clk_rcg2 *rcg = to_clk_rcg2(hw);270 const char *name = clk_hw_get_name(hw);271 unsigned long parent_rate, rate;272 struct clk_hw *p;273 int index, i;274 275 /* Exit early if only one config is defined */276 if (f->num_confs == 1) {277 best_conf = f->confs;278 goto exit;279 }280 281 /* Search in each provided config the one that is near the wanted rate */282 for (i = 0, conf = f->confs; i < f->num_confs; i++, conf++) {283 index = qcom_find_src_index(hw, rcg->parent_map, conf->src);284 if (index < 0)285 continue;286 287 p = clk_hw_get_parent_by_index(hw, index);288 if (!p)289 continue;290 291 parent_rate = clk_hw_get_rate(p);292 rate = calc_rate(parent_rate, conf->n, conf->m, conf->n, conf->pre_div);293 294 if (rate == req_rate) {295 best_conf = conf;296 goto exit;297 }298 299 rate_diff = abs_diff(req_rate, rate);300 if (rate_diff < best_rate_diff) {301 best_rate_diff = rate_diff;302 best_conf = conf;303 }304 }305 306 /*307 * Very unlikely. Warn if we couldn't find a correct config308 * due to parent not found in every config.309 */310 if (unlikely(!best_conf)) {311 WARN(1, "%s: can't find a configuration for rate %lu\n",312 name, req_rate);313 return ERR_PTR(-EINVAL);314 }315 316exit:317 return best_conf;318}319 320static int _freq_tbl_fm_determine_rate(struct clk_hw *hw, const struct freq_multi_tbl *f,321 struct clk_rate_request *req)322{323 unsigned long clk_flags, rate = req->rate;324 struct clk_rcg2 *rcg = to_clk_rcg2(hw);325 const struct freq_conf *conf;326 struct clk_hw *p;327 int index;328 329 f = qcom_find_freq_multi(f, rate);330 if (!f || !f->confs)331 return -EINVAL;332 333 conf = __clk_rcg2_select_conf(hw, f, rate);334 if (IS_ERR(conf))335 return PTR_ERR(conf);336 index = qcom_find_src_index(hw, rcg->parent_map, conf->src);337 if (index < 0)338 return index;339 340 clk_flags = clk_hw_get_flags(hw);341 p = clk_hw_get_parent_by_index(hw, index);342 if (!p)343 return -EINVAL;344 345 if (clk_flags & CLK_SET_RATE_PARENT) {346 rate = f->freq;347 if (conf->pre_div) {348 if (!rate)349 rate = req->rate;350 rate /= 2;351 rate *= conf->pre_div + 1;352 }353 354 if (conf->n) {355 u64 tmp = rate;356 357 tmp = tmp * conf->n;358 do_div(tmp, conf->m);359 rate = tmp;360 }361 } else {362 rate = clk_hw_get_rate(p);363 }364 365 req->best_parent_hw = p;366 req->best_parent_rate = rate;367 req->rate = f->freq;368 369 return 0;370}371 372static int clk_rcg2_determine_rate(struct clk_hw *hw,373 struct clk_rate_request *req)374{375 struct clk_rcg2 *rcg = to_clk_rcg2(hw);376 377 return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, CEIL);378}379 380static int clk_rcg2_determine_floor_rate(struct clk_hw *hw,381 struct clk_rate_request *req)382{383 struct clk_rcg2 *rcg = to_clk_rcg2(hw);384 385 return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, FLOOR);386}387 388static int clk_rcg2_fm_determine_rate(struct clk_hw *hw,389 struct clk_rate_request *req)390{391 struct clk_rcg2 *rcg = to_clk_rcg2(hw);392 393 return _freq_tbl_fm_determine_rate(hw, rcg->freq_multi_tbl, req);394}395 396static int __clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f,397 u32 *_cfg)398{399 u32 cfg, mask, d_val, not2d_val, n_minus_m;400 struct clk_hw *hw = &rcg->clkr.hw;401 int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src);402 403 if (index < 0)404 return index;405 406 if (rcg->mnd_width && f->n) {407 mask = BIT(rcg->mnd_width) - 1;408 ret = regmap_update_bits(rcg->clkr.regmap,409 RCG_M_OFFSET(rcg), mask, f->m);410 if (ret)411 return ret;412 413 ret = regmap_update_bits(rcg->clkr.regmap,414 RCG_N_OFFSET(rcg), mask, ~(f->n - f->m));415 if (ret)416 return ret;417 418 /* Calculate 2d value */419 d_val = f->n;420 421 n_minus_m = f->n - f->m;422 n_minus_m *= 2;423 424 d_val = clamp_t(u32, d_val, f->m, n_minus_m);425 not2d_val = ~d_val & mask;426 427 ret = regmap_update_bits(rcg->clkr.regmap,428 RCG_D_OFFSET(rcg), mask, not2d_val);429 if (ret)430 return ret;431 }432 433 mask = BIT(rcg->hid_width) - 1;434 mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK | CFG_HW_CLK_CTRL_MASK;435 cfg = f->pre_div << CFG_SRC_DIV_SHIFT;436 cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;437 if (rcg->mnd_width && f->n && (f->m != f->n))438 cfg |= CFG_MODE_DUAL_EDGE;439 if (rcg->hw_clk_ctrl)440 cfg |= CFG_HW_CLK_CTRL_MASK;441 442 *_cfg &= ~mask;443 *_cfg |= cfg;444 445 return 0;446}447 448static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f)449{450 u32 cfg;451 int ret;452 453 ret = regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);454 if (ret)455 return ret;456 457 ret = __clk_rcg2_configure(rcg, f, &cfg);458 if (ret)459 return ret;460 461 ret = regmap_write(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), cfg);462 if (ret)463 return ret;464 465 return update_config(rcg);466}467 468static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,469 enum freq_policy policy)470{471 struct clk_rcg2 *rcg = to_clk_rcg2(hw);472 const struct freq_tbl *f;473 474 switch (policy) {475 case FLOOR:476 f = qcom_find_freq_floor(rcg->freq_tbl, rate);477 break;478 case CEIL:479 f = qcom_find_freq(rcg->freq_tbl, rate);480 break;481 default:482 return -EINVAL;483 }484 485 if (!f)486 return -EINVAL;487 488 return clk_rcg2_configure(rcg, f);489}490 491static int __clk_rcg2_fm_set_rate(struct clk_hw *hw, unsigned long rate)492{493 struct clk_rcg2 *rcg = to_clk_rcg2(hw);494 const struct freq_multi_tbl *f;495 const struct freq_conf *conf;496 struct freq_tbl f_tbl = {};497 498 f = qcom_find_freq_multi(rcg->freq_multi_tbl, rate);499 if (!f || !f->confs)500 return -EINVAL;501 502 conf = __clk_rcg2_select_conf(hw, f, rate);503 if (IS_ERR(conf))504 return PTR_ERR(conf);505 506 f_tbl.freq = f->freq;507 f_tbl.src = conf->src;508 f_tbl.pre_div = conf->pre_div;509 f_tbl.m = conf->m;510 f_tbl.n = conf->n;511 512 return clk_rcg2_configure(rcg, &f_tbl);513}514 515static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,516 unsigned long parent_rate)517{518 return __clk_rcg2_set_rate(hw, rate, CEIL);519}520 521static int clk_rcg2_set_floor_rate(struct clk_hw *hw, unsigned long rate,522 unsigned long parent_rate)523{524 return __clk_rcg2_set_rate(hw, rate, FLOOR);525}526 527static int clk_rcg2_fm_set_rate(struct clk_hw *hw, unsigned long rate,528 unsigned long parent_rate)529{530 return __clk_rcg2_fm_set_rate(hw, rate);531}532 533static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw,534 unsigned long rate, unsigned long parent_rate, u8 index)535{536 return __clk_rcg2_set_rate(hw, rate, CEIL);537}538 539static int clk_rcg2_set_floor_rate_and_parent(struct clk_hw *hw,540 unsigned long rate, unsigned long parent_rate, u8 index)541{542 return __clk_rcg2_set_rate(hw, rate, FLOOR);543}544 545static int clk_rcg2_fm_set_rate_and_parent(struct clk_hw *hw,546 unsigned long rate, unsigned long parent_rate, u8 index)547{548 return __clk_rcg2_fm_set_rate(hw, rate);549}550 551static int clk_rcg2_get_duty_cycle(struct clk_hw *hw, struct clk_duty *duty)552{553 struct clk_rcg2 *rcg = to_clk_rcg2(hw);554 u32 notn_m, n, m, d, not2d, mask;555 556 if (!rcg->mnd_width) {557 /* 50 % duty-cycle for Non-MND RCGs */558 duty->num = 1;559 duty->den = 2;560 return 0;561 }562 563 regmap_read(rcg->clkr.regmap, RCG_D_OFFSET(rcg), ¬2d);564 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);565 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), ¬n_m);566 567 if (!not2d && !m && !notn_m) {568 /* 50 % duty-cycle always */569 duty->num = 1;570 duty->den = 2;571 return 0;572 }573 574 mask = BIT(rcg->mnd_width) - 1;575 576 d = ~(not2d) & mask;577 d = DIV_ROUND_CLOSEST(d, 2);578 579 n = (~(notn_m) + m) & mask;580 581 duty->num = d;582 duty->den = n;583 584 return 0;585}586 587static int clk_rcg2_set_duty_cycle(struct clk_hw *hw, struct clk_duty *duty)588{589 struct clk_rcg2 *rcg = to_clk_rcg2(hw);590 u32 notn_m, n, m, d, not2d, mask, duty_per, cfg;591 int ret;592 593 /* Duty-cycle cannot be modified for non-MND RCGs */594 if (!rcg->mnd_width)595 return -EINVAL;596 597 mask = BIT(rcg->mnd_width) - 1;598 599 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), ¬n_m);600 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);601 regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);602 603 /* Duty-cycle cannot be modified if MND divider is in bypass mode. */604 if (!(cfg & CFG_MODE_MASK))605 return -EINVAL;606 607 n = (~(notn_m) + m) & mask;608 609 duty_per = (duty->num * 100) / duty->den;610 611 /* Calculate 2d value */612 d = DIV_ROUND_CLOSEST(n * duty_per * 2, 100);613 614 /*615 * Check bit widths of 2d. If D is too big reduce duty cycle.616 * Also make sure it is never zero.617 */618 d = clamp_val(d, 1, mask);619 620 if ((d / 2) > (n - m))621 d = (n - m) * 2;622 else if ((d / 2) < (m / 2))623 d = m;624 625 not2d = ~d & mask;626 627 ret = regmap_update_bits(rcg->clkr.regmap, RCG_D_OFFSET(rcg), mask,628 not2d);629 if (ret)630 return ret;631 632 return update_config(rcg);633}634 635const struct clk_ops clk_rcg2_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 .determine_rate = clk_rcg2_determine_rate,641 .set_rate = clk_rcg2_set_rate,642 .set_rate_and_parent = clk_rcg2_set_rate_and_parent,643 .get_duty_cycle = clk_rcg2_get_duty_cycle,644 .set_duty_cycle = clk_rcg2_set_duty_cycle,645};646EXPORT_SYMBOL_GPL(clk_rcg2_ops);647 648const struct clk_ops clk_rcg2_floor_ops = {649 .is_enabled = clk_rcg2_is_enabled,650 .get_parent = clk_rcg2_get_parent,651 .set_parent = clk_rcg2_set_parent,652 .recalc_rate = clk_rcg2_recalc_rate,653 .determine_rate = clk_rcg2_determine_floor_rate,654 .set_rate = clk_rcg2_set_floor_rate,655 .set_rate_and_parent = clk_rcg2_set_floor_rate_and_parent,656 .get_duty_cycle = clk_rcg2_get_duty_cycle,657 .set_duty_cycle = clk_rcg2_set_duty_cycle,658};659EXPORT_SYMBOL_GPL(clk_rcg2_floor_ops);660 661const struct clk_ops clk_rcg2_fm_ops = {662 .is_enabled = clk_rcg2_is_enabled,663 .get_parent = clk_rcg2_get_parent,664 .set_parent = clk_rcg2_set_parent,665 .recalc_rate = clk_rcg2_recalc_rate,666 .determine_rate = clk_rcg2_fm_determine_rate,667 .set_rate = clk_rcg2_fm_set_rate,668 .set_rate_and_parent = clk_rcg2_fm_set_rate_and_parent,669 .get_duty_cycle = clk_rcg2_get_duty_cycle,670 .set_duty_cycle = clk_rcg2_set_duty_cycle,671};672EXPORT_SYMBOL_GPL(clk_rcg2_fm_ops);673 674const struct clk_ops clk_rcg2_mux_closest_ops = {675 .determine_rate = __clk_mux_determine_rate_closest,676 .get_parent = clk_rcg2_get_parent,677 .set_parent = clk_rcg2_set_parent,678};679EXPORT_SYMBOL_GPL(clk_rcg2_mux_closest_ops);680 681struct frac_entry {682 int num;683 int den;684};685 686static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */687 { 52, 295 }, /* 119 M */688 { 11, 57 }, /* 130.25 M */689 { 63, 307 }, /* 138.50 M */690 { 11, 50 }, /* 148.50 M */691 { 47, 206 }, /* 154 M */692 { 31, 100 }, /* 205.25 M */693 { 107, 269 }, /* 268.50 M */694 { },695};696 697static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */698 { 31, 211 }, /* 119 M */699 { 32, 199 }, /* 130.25 M */700 { 63, 307 }, /* 138.50 M */701 { 11, 60 }, /* 148.50 M */702 { 50, 263 }, /* 154 M */703 { 31, 120 }, /* 205.25 M */704 { 119, 359 }, /* 268.50 M */705 { },706};707 708static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate,709 unsigned long parent_rate)710{711 struct clk_rcg2 *rcg = to_clk_rcg2(hw);712 struct freq_tbl f = *rcg->freq_tbl;713 const struct frac_entry *frac;714 int delta = 100000;715 s64 src_rate = parent_rate;716 s64 request;717 u32 mask = BIT(rcg->hid_width) - 1;718 u32 hid_div;719 720 if (src_rate == 810000000)721 frac = frac_table_810m;722 else723 frac = frac_table_675m;724 725 for (; frac->num; frac++) {726 request = rate;727 request *= frac->den;728 request = div_s64(request, frac->num);729 if ((src_rate < (request - delta)) ||730 (src_rate > (request + delta)))731 continue;732 733 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,734 &hid_div);735 f.pre_div = hid_div;736 f.pre_div >>= CFG_SRC_DIV_SHIFT;737 f.pre_div &= mask;738 f.m = frac->num;739 f.n = frac->den;740 741 return clk_rcg2_configure(rcg, &f);742 }743 744 return -EINVAL;745}746 747static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,748 unsigned long rate, unsigned long parent_rate, u8 index)749{750 /* Parent index is set statically in frequency table */751 return clk_edp_pixel_set_rate(hw, rate, parent_rate);752}753 754static int clk_edp_pixel_determine_rate(struct clk_hw *hw,755 struct clk_rate_request *req)756{757 struct clk_rcg2 *rcg = to_clk_rcg2(hw);758 const struct freq_tbl *f = rcg->freq_tbl;759 const struct frac_entry *frac;760 int delta = 100000;761 s64 request;762 u32 mask = BIT(rcg->hid_width) - 1;763 u32 hid_div;764 int index = qcom_find_src_index(hw, rcg->parent_map, f->src);765 766 /* Force the correct parent */767 req->best_parent_hw = clk_hw_get_parent_by_index(hw, index);768 req->best_parent_rate = clk_hw_get_rate(req->best_parent_hw);769 770 if (req->best_parent_rate == 810000000)771 frac = frac_table_810m;772 else773 frac = frac_table_675m;774 775 for (; frac->num; frac++) {776 request = req->rate;777 request *= frac->den;778 request = div_s64(request, frac->num);779 if ((req->best_parent_rate < (request - delta)) ||780 (req->best_parent_rate > (request + delta)))781 continue;782 783 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,784 &hid_div);785 hid_div >>= CFG_SRC_DIV_SHIFT;786 hid_div &= mask;787 788 req->rate = calc_rate(req->best_parent_rate,789 frac->num, frac->den,790 !!frac->den, hid_div);791 return 0;792 }793 794 return -EINVAL;795}796 797const struct clk_ops clk_edp_pixel_ops = {798 .is_enabled = clk_rcg2_is_enabled,799 .get_parent = clk_rcg2_get_parent,800 .set_parent = clk_rcg2_set_parent,801 .recalc_rate = clk_rcg2_recalc_rate,802 .set_rate = clk_edp_pixel_set_rate,803 .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent,804 .determine_rate = clk_edp_pixel_determine_rate,805};806EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);807 808static int clk_byte_determine_rate(struct clk_hw *hw,809 struct clk_rate_request *req)810{811 struct clk_rcg2 *rcg = to_clk_rcg2(hw);812 const struct freq_tbl *f = rcg->freq_tbl;813 int index = qcom_find_src_index(hw, rcg->parent_map, f->src);814 unsigned long parent_rate, div;815 u32 mask = BIT(rcg->hid_width) - 1;816 struct clk_hw *p;817 818 if (req->rate == 0)819 return -EINVAL;820 821 req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index);822 req->best_parent_rate = parent_rate = clk_hw_round_rate(p, req->rate);823 824 div = DIV_ROUND_UP((2 * parent_rate), req->rate) - 1;825 div = min_t(u32, div, mask);826 827 req->rate = calc_rate(parent_rate, 0, 0, 0, div);828 829 return 0;830}831 832static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate,833 unsigned long parent_rate)834{835 struct clk_rcg2 *rcg = to_clk_rcg2(hw);836 struct freq_tbl f = *rcg->freq_tbl;837 unsigned long div;838 u32 mask = BIT(rcg->hid_width) - 1;839 840 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;841 div = min_t(u32, div, mask);842 843 f.pre_div = div;844 845 return clk_rcg2_configure(rcg, &f);846}847 848static int clk_byte_set_rate_and_parent(struct clk_hw *hw,849 unsigned long rate, unsigned long parent_rate, u8 index)850{851 /* Parent index is set statically in frequency table */852 return clk_byte_set_rate(hw, rate, parent_rate);853}854 855const struct clk_ops clk_byte_ops = {856 .is_enabled = clk_rcg2_is_enabled,857 .get_parent = clk_rcg2_get_parent,858 .set_parent = clk_rcg2_set_parent,859 .recalc_rate = clk_rcg2_recalc_rate,860 .set_rate = clk_byte_set_rate,861 .set_rate_and_parent = clk_byte_set_rate_and_parent,862 .determine_rate = clk_byte_determine_rate,863};864EXPORT_SYMBOL_GPL(clk_byte_ops);865 866static int clk_byte2_determine_rate(struct clk_hw *hw,867 struct clk_rate_request *req)868{869 struct clk_rcg2 *rcg = to_clk_rcg2(hw);870 unsigned long parent_rate, div;871 u32 mask = BIT(rcg->hid_width) - 1;872 struct clk_hw *p;873 unsigned long rate = req->rate;874 875 if (rate == 0)876 return -EINVAL;877 878 p = req->best_parent_hw;879 req->best_parent_rate = parent_rate = clk_hw_round_rate(p, rate);880 881 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;882 div = min_t(u32, div, mask);883 884 req->rate = calc_rate(parent_rate, 0, 0, 0, div);885 886 return 0;887}888 889static int clk_byte2_set_rate(struct clk_hw *hw, unsigned long rate,890 unsigned long parent_rate)891{892 struct clk_rcg2 *rcg = to_clk_rcg2(hw);893 struct freq_tbl f = { 0 };894 unsigned long div;895 int i, num_parents = clk_hw_get_num_parents(hw);896 u32 mask = BIT(rcg->hid_width) - 1;897 u32 cfg;898 899 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;900 div = min_t(u32, div, mask);901 902 f.pre_div = div;903 904 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);905 cfg &= CFG_SRC_SEL_MASK;906 cfg >>= CFG_SRC_SEL_SHIFT;907 908 for (i = 0; i < num_parents; i++) {909 if (cfg == rcg->parent_map[i].cfg) {910 f.src = rcg->parent_map[i].src;911 return clk_rcg2_configure(rcg, &f);912 }913 }914 915 return -EINVAL;916}917 918static int clk_byte2_set_rate_and_parent(struct clk_hw *hw,919 unsigned long rate, unsigned long parent_rate, u8 index)920{921 /* Read the hardware to determine parent during set_rate */922 return clk_byte2_set_rate(hw, rate, parent_rate);923}924 925const struct clk_ops clk_byte2_ops = {926 .is_enabled = clk_rcg2_is_enabled,927 .get_parent = clk_rcg2_get_parent,928 .set_parent = clk_rcg2_set_parent,929 .recalc_rate = clk_rcg2_recalc_rate,930 .set_rate = clk_byte2_set_rate,931 .set_rate_and_parent = clk_byte2_set_rate_and_parent,932 .determine_rate = clk_byte2_determine_rate,933};934EXPORT_SYMBOL_GPL(clk_byte2_ops);935 936static const struct frac_entry frac_table_pixel[] = {937 { 3, 8 },938 { 2, 9 },939 { 4, 9 },940 { 1, 1 },941 { 2, 3 },942 { }943};944 945static int clk_pixel_determine_rate(struct clk_hw *hw,946 struct clk_rate_request *req)947{948 unsigned long request, src_rate;949 int delta = 100000;950 const struct frac_entry *frac = frac_table_pixel;951 952 for (; frac->num; frac++) {953 request = (req->rate * frac->den) / frac->num;954 955 src_rate = clk_hw_round_rate(req->best_parent_hw, request);956 if ((src_rate < (request - delta)) ||957 (src_rate > (request + delta)))958 continue;959 960 req->best_parent_rate = src_rate;961 req->rate = (src_rate * frac->num) / frac->den;962 return 0;963 }964 965 return -EINVAL;966}967 968static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,969 unsigned long parent_rate)970{971 struct clk_rcg2 *rcg = to_clk_rcg2(hw);972 struct freq_tbl f = { 0 };973 const struct frac_entry *frac = frac_table_pixel;974 unsigned long request;975 int delta = 100000;976 u32 mask = BIT(rcg->hid_width) - 1;977 u32 hid_div, cfg;978 int i, num_parents = clk_hw_get_num_parents(hw);979 980 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);981 cfg &= CFG_SRC_SEL_MASK;982 cfg >>= CFG_SRC_SEL_SHIFT;983 984 for (i = 0; i < num_parents; i++)985 if (cfg == rcg->parent_map[i].cfg) {986 f.src = rcg->parent_map[i].src;987 break;988 }989 990 for (; frac->num; frac++) {991 request = (rate * frac->den) / frac->num;992 993 if ((parent_rate < (request - delta)) ||994 (parent_rate > (request + delta)))995 continue;996 997 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,998 &hid_div);999 f.pre_div = hid_div;1000 f.pre_div >>= CFG_SRC_DIV_SHIFT;1001 f.pre_div &= mask;1002 f.m = frac->num;1003 f.n = frac->den;1004 1005 return clk_rcg2_configure(rcg, &f);1006 }1007 return -EINVAL;1008}1009 1010static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,1011 unsigned long parent_rate, u8 index)1012{1013 return clk_pixel_set_rate(hw, rate, parent_rate);1014}1015 1016const struct clk_ops clk_pixel_ops = {1017 .is_enabled = clk_rcg2_is_enabled,1018 .get_parent = clk_rcg2_get_parent,1019 .set_parent = clk_rcg2_set_parent,1020 .recalc_rate = clk_rcg2_recalc_rate,1021 .set_rate = clk_pixel_set_rate,1022 .set_rate_and_parent = clk_pixel_set_rate_and_parent,1023 .determine_rate = clk_pixel_determine_rate,1024};1025EXPORT_SYMBOL_GPL(clk_pixel_ops);1026 1027static int clk_gfx3d_determine_rate(struct clk_hw *hw,1028 struct clk_rate_request *req)1029{1030 struct clk_rate_request parent_req = { .min_rate = 0, .max_rate = ULONG_MAX };1031 struct clk_rcg2_gfx3d *cgfx = to_clk_rcg2_gfx3d(hw);1032 struct clk_hw *xo, *p0, *p1, *p2;1033 unsigned long p0_rate;1034 u8 mux_div = cgfx->div;1035 int ret;1036 1037 p0 = cgfx->hws[0];1038 p1 = cgfx->hws[1];1039 p2 = cgfx->hws[2];1040 /*1041 * This function does ping-pong the RCG between PLLs: if we don't1042 * have at least one fixed PLL and two variable ones,1043 * then it's not going to work correctly.1044 */1045 if (WARN_ON(!p0 || !p1 || !p2))1046 return -EINVAL;1047 1048 xo = clk_hw_get_parent_by_index(hw, 0);1049 if (req->rate == clk_hw_get_rate(xo)) {1050 req->best_parent_hw = xo;1051 return 0;1052 }1053 1054 if (mux_div == 0)1055 mux_div = 1;1056 1057 parent_req.rate = req->rate * mux_div;1058 1059 /* This has to be a fixed rate PLL */1060 p0_rate = clk_hw_get_rate(p0);1061 1062 if (parent_req.rate == p0_rate) {1063 req->rate = req->best_parent_rate = p0_rate;1064 req->best_parent_hw = p0;1065 return 0;1066 }1067 1068 if (req->best_parent_hw == p0) {1069 /* Are we going back to a previously used rate? */1070 if (clk_hw_get_rate(p2) == parent_req.rate)1071 req->best_parent_hw = p2;1072 else1073 req->best_parent_hw = p1;1074 } else if (req->best_parent_hw == p2) {1075 req->best_parent_hw = p1;1076 } else {1077 req->best_parent_hw = p2;1078 }1079 1080 clk_hw_get_rate_range(req->best_parent_hw,1081 &parent_req.min_rate, &parent_req.max_rate);1082 1083 if (req->min_rate > parent_req.min_rate)1084 parent_req.min_rate = req->min_rate;1085 1086 if (req->max_rate < parent_req.max_rate)1087 parent_req.max_rate = req->max_rate;1088 1089 ret = __clk_determine_rate(req->best_parent_hw, &parent_req);1090 if (ret)1091 return ret;1092 1093 req->rate = req->best_parent_rate = parent_req.rate;1094 req->rate /= mux_div;1095 1096 return 0;1097}1098 1099static int clk_gfx3d_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,1100 unsigned long parent_rate, u8 index)1101{1102 struct clk_rcg2_gfx3d *cgfx = to_clk_rcg2_gfx3d(hw);1103 struct clk_rcg2 *rcg = &cgfx->rcg;1104 u32 cfg;1105 int ret;1106 1107 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;1108 /* On some targets, the GFX3D RCG may need to divide PLL frequency */1109 if (cgfx->div > 1)1110 cfg |= ((2 * cgfx->div) - 1) << CFG_SRC_DIV_SHIFT;1111 1112 ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg);1113 if (ret)1114 return ret;1115 1116 return update_config(rcg);1117}1118 1119static int clk_gfx3d_set_rate(struct clk_hw *hw, unsigned long rate,1120 unsigned long parent_rate)1121{1122 /*1123 * We should never get here; clk_gfx3d_determine_rate() should always1124 * make us use a different parent than what we're currently using, so1125 * clk_gfx3d_set_rate_and_parent() should always be called.1126 */1127 return 0;1128}1129 1130const struct clk_ops clk_gfx3d_ops = {1131 .is_enabled = clk_rcg2_is_enabled,1132 .get_parent = clk_rcg2_get_parent,1133 .set_parent = clk_rcg2_set_parent,1134 .recalc_rate = clk_rcg2_recalc_rate,1135 .set_rate = clk_gfx3d_set_rate,1136 .set_rate_and_parent = clk_gfx3d_set_rate_and_parent,1137 .determine_rate = clk_gfx3d_determine_rate,1138};1139EXPORT_SYMBOL_GPL(clk_gfx3d_ops);1140 1141static int clk_rcg2_set_force_enable(struct clk_hw *hw)1142{1143 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1144 const char *name = clk_hw_get_name(hw);1145 int ret, count;1146 1147 ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,1148 CMD_ROOT_EN, CMD_ROOT_EN);1149 if (ret)1150 return ret;1151 1152 /* wait for RCG to turn ON */1153 for (count = 500; count > 0; count--) {1154 if (clk_rcg2_is_enabled(hw))1155 return 0;1156 1157 udelay(1);1158 }1159 1160 pr_err("%s: RCG did not turn on\n", name);1161 return -ETIMEDOUT;1162}1163 1164static int clk_rcg2_clear_force_enable(struct clk_hw *hw)1165{1166 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1167 1168 return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,1169 CMD_ROOT_EN, 0);1170}1171 1172static int1173clk_rcg2_shared_force_enable_clear(struct clk_hw *hw, const struct freq_tbl *f)1174{1175 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1176 int ret;1177 1178 ret = clk_rcg2_set_force_enable(hw);1179 if (ret)1180 return ret;1181 1182 ret = clk_rcg2_configure(rcg, f);1183 if (ret)1184 return ret;1185 1186 return clk_rcg2_clear_force_enable(hw);1187}1188 1189static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate,1190 unsigned long parent_rate)1191{1192 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1193 const struct freq_tbl *f;1194 1195 f = qcom_find_freq(rcg->freq_tbl, rate);1196 if (!f)1197 return -EINVAL;1198 1199 /*1200 * In case clock is disabled, update the M, N and D registers, cache1201 * the CFG value in parked_cfg and don't hit the update bit of CMD1202 * register.1203 */1204 if (!clk_hw_is_enabled(hw))1205 return __clk_rcg2_configure(rcg, f, &rcg->parked_cfg);1206 1207 return clk_rcg2_shared_force_enable_clear(hw, f);1208}1209 1210static int clk_rcg2_shared_set_rate_and_parent(struct clk_hw *hw,1211 unsigned long rate, unsigned long parent_rate, u8 index)1212{1213 return clk_rcg2_shared_set_rate(hw, rate, parent_rate);1214}1215 1216static int clk_rcg2_shared_enable(struct clk_hw *hw)1217{1218 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1219 int ret;1220 1221 /*1222 * Set the update bit because required configuration has already1223 * been written in clk_rcg2_shared_set_rate()1224 */1225 ret = clk_rcg2_set_force_enable(hw);1226 if (ret)1227 return ret;1228 1229 /* Write back the stored configuration corresponding to current rate */1230 ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, rcg->parked_cfg);1231 if (ret)1232 return ret;1233 1234 ret = update_config(rcg);1235 if (ret)1236 return ret;1237 1238 return clk_rcg2_clear_force_enable(hw);1239}1240 1241static void clk_rcg2_shared_disable(struct clk_hw *hw)1242{1243 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1244 1245 /*1246 * Store current configuration as switching to safe source would clear1247 * the SRC and DIV of CFG register1248 */1249 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &rcg->parked_cfg);1250 1251 /*1252 * Park the RCG at a safe configuration - sourced off of safe source.1253 * Force enable and disable the RCG while configuring it to safeguard1254 * against any update signal coming from the downstream clock.1255 * The current parent is still prepared and enabled at this point, and1256 * the safe source is always on while application processor subsystem1257 * is online. Therefore, the RCG can safely switch its parent.1258 */1259 clk_rcg2_set_force_enable(hw);1260 1261 regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,1262 rcg->safe_src_index << CFG_SRC_SEL_SHIFT);1263 1264 update_config(rcg);1265 1266 clk_rcg2_clear_force_enable(hw);1267}1268 1269static u8 clk_rcg2_shared_get_parent(struct clk_hw *hw)1270{1271 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1272 1273 /* If the shared rcg is parked use the cached cfg instead */1274 if (!clk_hw_is_enabled(hw))1275 return __clk_rcg2_get_parent(hw, rcg->parked_cfg);1276 1277 return clk_rcg2_get_parent(hw);1278}1279 1280static int clk_rcg2_shared_set_parent(struct clk_hw *hw, u8 index)1281{1282 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1283 1284 /* If the shared rcg is parked only update the cached cfg */1285 if (!clk_hw_is_enabled(hw)) {1286 rcg->parked_cfg &= ~CFG_SRC_SEL_MASK;1287 rcg->parked_cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;1288 1289 return 0;1290 }1291 1292 return clk_rcg2_set_parent(hw, index);1293}1294 1295static unsigned long1296clk_rcg2_shared_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)1297{1298 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1299 1300 /* If the shared rcg is parked use the cached cfg instead */1301 if (!clk_hw_is_enabled(hw))1302 return __clk_rcg2_recalc_rate(hw, parent_rate, rcg->parked_cfg);1303 1304 return clk_rcg2_recalc_rate(hw, parent_rate);1305}1306 1307static int clk_rcg2_shared_init(struct clk_hw *hw)1308{1309 /*1310 * This does a few things:1311 *1312 * 1. Sets rcg->parked_cfg to reflect the value at probe so that the1313 * proper parent is reported from clk_rcg2_shared_get_parent().1314 *1315 * 2. Clears the force enable bit of the RCG because we rely on child1316 * clks (branches) to turn the RCG on/off with a hardware feedback1317 * mechanism and only set the force enable bit in the RCG when we1318 * want to make sure the clk stays on for parent switches or1319 * parking.1320 *1321 * 3. Parks shared RCGs on the safe source at registration because we1322 * can't be certain that the parent clk will stay on during boot,1323 * especially if the parent is shared. If this RCG is enabled at1324 * boot, and the parent is turned off, the RCG will get stuck on. A1325 * GDSC can wedge if is turned on and the RCG is stuck on because1326 * the GDSC's controller will hang waiting for the clk status to1327 * toggle on when it never does.1328 *1329 * The safest option here is to "park" the RCG at init so that the clk1330 * can never get stuck on or off. This ensures the GDSC can't get1331 * wedged.1332 */1333 clk_rcg2_shared_disable(hw);1334 1335 return 0;1336}1337 1338const struct clk_ops clk_rcg2_shared_ops = {1339 .init = clk_rcg2_shared_init,1340 .enable = clk_rcg2_shared_enable,1341 .disable = clk_rcg2_shared_disable,1342 .get_parent = clk_rcg2_shared_get_parent,1343 .set_parent = clk_rcg2_shared_set_parent,1344 .recalc_rate = clk_rcg2_shared_recalc_rate,1345 .determine_rate = clk_rcg2_determine_rate,1346 .set_rate = clk_rcg2_shared_set_rate,1347 .set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent,1348};1349EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops);1350 1351static int clk_rcg2_shared_no_init_park(struct clk_hw *hw)1352{1353 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1354 1355 /*1356 * Read the config register so that the parent is properly mapped at1357 * registration time.1358 */1359 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &rcg->parked_cfg);1360 1361 return 0;1362}1363 1364/*1365 * Like clk_rcg2_shared_ops but skip the init so that the clk frequency is left1366 * unchanged at registration time.1367 */1368const struct clk_ops clk_rcg2_shared_no_init_park_ops = {1369 .init = clk_rcg2_shared_no_init_park,1370 .enable = clk_rcg2_shared_enable,1371 .disable = clk_rcg2_shared_disable,1372 .get_parent = clk_rcg2_shared_get_parent,1373 .set_parent = clk_rcg2_shared_set_parent,1374 .recalc_rate = clk_rcg2_shared_recalc_rate,1375 .determine_rate = clk_rcg2_determine_rate,1376 .set_rate = clk_rcg2_shared_set_rate,1377 .set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent,1378};1379EXPORT_SYMBOL_GPL(clk_rcg2_shared_no_init_park_ops);1380 1381/* Common APIs to be used for DFS based RCGR */1382static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l,1383 struct freq_tbl *f)1384{1385 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1386 struct clk_hw *p;1387 unsigned long prate = 0;1388 u32 val, mask, cfg, mode, src;1389 int i, num_parents;1390 1391 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg);1392 1393 mask = BIT(rcg->hid_width) - 1;1394 f->pre_div = 1;1395 if (cfg & mask)1396 f->pre_div = cfg & mask;1397 1398 src = cfg & CFG_SRC_SEL_MASK;1399 src >>= CFG_SRC_SEL_SHIFT;1400 1401 num_parents = clk_hw_get_num_parents(hw);1402 for (i = 0; i < num_parents; i++) {1403 if (src == rcg->parent_map[i].cfg) {1404 f->src = rcg->parent_map[i].src;1405 p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i);1406 prate = clk_hw_get_rate(p);1407 }1408 }1409 1410 mode = cfg & CFG_MODE_MASK;1411 mode >>= CFG_MODE_SHIFT;1412 if (mode) {1413 mask = BIT(rcg->mnd_width) - 1;1414 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l),1415 &val);1416 val &= mask;1417 f->m = val;1418 1419 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l),1420 &val);1421 val = ~val;1422 val &= mask;1423 val += f->m;1424 f->n = val;1425 }1426 1427 f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div);1428}1429 1430static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg)1431{1432 struct freq_tbl *freq_tbl;1433 int i;1434 1435 /* Allocate space for 1 extra since table is NULL terminated */1436 freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL);1437 if (!freq_tbl)1438 return -ENOMEM;1439 rcg->freq_tbl = freq_tbl;1440 1441 for (i = 0; i < MAX_PERF_LEVEL; i++)1442 clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i);1443 1444 return 0;1445}1446 1447static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw,1448 struct clk_rate_request *req)1449{1450 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1451 int ret;1452 1453 if (!rcg->freq_tbl) {1454 ret = clk_rcg2_dfs_populate_freq_table(rcg);1455 if (ret) {1456 pr_err("Failed to update DFS tables for %s\n",1457 clk_hw_get_name(hw));1458 return ret;1459 }1460 }1461 1462 return clk_rcg2_determine_rate(hw, req);1463}1464 1465static unsigned long1466clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)1467{1468 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1469 u32 level, mask, cfg, m = 0, n = 0, mode, pre_div;1470 1471 regmap_read(rcg->clkr.regmap,1472 rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level);1473 level &= GENMASK(4, 1);1474 level >>= 1;1475 1476 if (rcg->freq_tbl)1477 return rcg->freq_tbl[level].freq;1478 1479 /*1480 * Assume that parent_rate is actually the parent because1481 * we can't do any better at figuring it out when the table1482 * hasn't been populated yet. We only populate the table1483 * in determine_rate because we can't guarantee the parents1484 * will be registered with the framework until then.1485 */1486 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level),1487 &cfg);1488 1489 mask = BIT(rcg->hid_width) - 1;1490 pre_div = 1;1491 if (cfg & mask)1492 pre_div = cfg & mask;1493 1494 mode = cfg & CFG_MODE_MASK;1495 mode >>= CFG_MODE_SHIFT;1496 if (mode) {1497 mask = BIT(rcg->mnd_width) - 1;1498 regmap_read(rcg->clkr.regmap,1499 rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m);1500 m &= mask;1501 1502 regmap_read(rcg->clkr.regmap,1503 rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n);1504 n = ~n;1505 n &= mask;1506 n += m;1507 }1508 1509 return calc_rate(parent_rate, m, n, mode, pre_div);1510}1511 1512static const struct clk_ops clk_rcg2_dfs_ops = {1513 .is_enabled = clk_rcg2_is_enabled,1514 .get_parent = clk_rcg2_get_parent,1515 .determine_rate = clk_rcg2_dfs_determine_rate,1516 .recalc_rate = clk_rcg2_dfs_recalc_rate,1517};1518 1519static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data,1520 struct regmap *regmap)1521{1522 struct clk_rcg2 *rcg = data->rcg;1523 struct clk_init_data *init = data->init;1524 u32 val;1525 int ret;1526 1527 ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val);1528 if (ret)1529 return -EINVAL;1530 1531 if (!(val & SE_CMD_DFS_EN))1532 return 0;1533 1534 /*1535 * Rate changes with consumer writing a register in1536 * their own I/O region1537 */1538 init->flags |= CLK_GET_RATE_NOCACHE;1539 init->ops = &clk_rcg2_dfs_ops;1540 1541 rcg->freq_tbl = NULL;1542 1543 return 0;1544}1545 1546int qcom_cc_register_rcg_dfs(struct regmap *regmap,1547 const struct clk_rcg_dfs_data *rcgs, size_t len)1548{1549 int i, ret;1550 1551 for (i = 0; i < len; i++) {1552 ret = clk_rcg2_enable_dfs(&rcgs[i], regmap);1553 if (ret)1554 return ret;1555 }1556 1557 return 0;1558}1559EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs);1560 1561static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate,1562 unsigned long parent_rate)1563{1564 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1565 struct freq_tbl f = { 0 };1566 u32 mask = BIT(rcg->hid_width) - 1;1567 u32 hid_div, cfg;1568 int i, num_parents = clk_hw_get_num_parents(hw);1569 unsigned long num, den;1570 1571 rational_best_approximation(parent_rate, rate,1572 GENMASK(rcg->mnd_width - 1, 0),1573 GENMASK(rcg->mnd_width - 1, 0), &den, &num);1574 1575 if (!num || !den)1576 return -EINVAL;1577 1578 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);1579 hid_div = cfg;1580 cfg &= CFG_SRC_SEL_MASK;1581 cfg >>= CFG_SRC_SEL_SHIFT;1582 1583 for (i = 0; i < num_parents; i++) {1584 if (cfg == rcg->parent_map[i].cfg) {1585 f.src = rcg->parent_map[i].src;1586 break;1587 }1588 }1589 1590 f.pre_div = hid_div;1591 f.pre_div >>= CFG_SRC_DIV_SHIFT;1592 f.pre_div &= mask;1593 1594 if (num != den) {1595 f.m = num;1596 f.n = den;1597 } else {1598 f.m = 0;1599 f.n = 0;1600 }1601 1602 return clk_rcg2_configure(rcg, &f);1603}1604 1605static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw,1606 unsigned long rate, unsigned long parent_rate, u8 index)1607{1608 return clk_rcg2_dp_set_rate(hw, rate, parent_rate);1609}1610 1611static int clk_rcg2_dp_determine_rate(struct clk_hw *hw,1612 struct clk_rate_request *req)1613{1614 struct clk_rcg2 *rcg = to_clk_rcg2(hw);1615 unsigned long num, den;1616 u64 tmp;1617 1618 /* Parent rate is a fixed phy link rate */1619 rational_best_approximation(req->best_parent_rate, req->rate,1620 GENMASK(rcg->mnd_width - 1, 0),1621 GENMASK(rcg->mnd_width - 1, 0), &den, &num);1622 1623 if (!num || !den)1624 return -EINVAL;1625 1626 tmp = req->best_parent_rate * num;1627 do_div(tmp, den);1628 req->rate = tmp;1629 1630 return 0;1631}1632 1633const struct clk_ops clk_dp_ops = {1634 .is_enabled = clk_rcg2_is_enabled,1635 .get_parent = clk_rcg2_get_parent,1636 .set_parent = clk_rcg2_set_parent,1637 .recalc_rate = clk_rcg2_recalc_rate,1638 .set_rate = clk_rcg2_dp_set_rate,1639 .set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent,1640 .determine_rate = clk_rcg2_dp_determine_rate,1641};1642EXPORT_SYMBOL_GPL(clk_dp_ops);1643