drivers/clk/qcom/clk-rcg2.c

   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 = mult_frac(rate, 2, hid_div + 1);
 163
 164         if (mode)
 165                 rate = mult_frac(rate, m, n);
 166
 167         return rate;
 168 }
 169
 170 static unsigned long
 171 __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
 195 static unsigned long
 196 clk_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
 206 static 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
 263 static 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 config
 308          * 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
 316 exit:
 317         return best_conf;
 318 }
 319
 320 static 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
 372 static 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
 380 static 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
 388 static 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
 396 static 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
 448 static 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
 468 static 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
 491 static 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
 515 static 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
 521 static 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
 527 static 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
 533 static 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
 539 static 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
 545 static 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
 551 static 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), &not2d);
 564         regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);
 565         regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), &notn_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
 587 static 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), &notn_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
 635 const 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 };
 646 EXPORT_SYMBOL_GPL(clk_rcg2_ops);
 647
 648 const 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 };
 659 EXPORT_SYMBOL_GPL(clk_rcg2_floor_ops);
 660
 661 const 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 };
 672 EXPORT_SYMBOL_GPL(clk_rcg2_fm_ops);
 673
 674 const 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 };
 679 EXPORT_SYMBOL_GPL(clk_rcg2_mux_closest_ops);
 680
 681 struct frac_entry {
 682         int num;
 683         int den;
 684 };
 685
 686 static 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
 697 static 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
 708 static 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         else
 723                 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
 747 static 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
 754 static 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         else
 773                 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
 797 const 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 };
 806 EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);
 807
 808 static 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
 832 static 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
 848 static 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
 855 const 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 };
 864 EXPORT_SYMBOL_GPL(clk_byte_ops);
 865
 866 static 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
 889 static 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
 918 static 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
 925 const 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 };
 934 EXPORT_SYMBOL_GPL(clk_byte2_ops);
 935
 936 static 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
 945 static 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
 968 static 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
1010 static 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
1016 const 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 };
1025 EXPORT_SYMBOL_GPL(clk_pixel_ops);
1026
1027 static 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't
1042          * 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                 else
1073                         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
1099 static 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
1119 static 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 always
1124          * make us use a different parent than what we're currently using, so
1125          * clk_gfx3d_set_rate_and_parent() should always be called.
1126          */
1127         return 0;
1128 }
1129
1130 const 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 };
1139 EXPORT_SYMBOL_GPL(clk_gfx3d_ops);
1140
1141 static 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
1164 static 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
1172 static int
1173 clk_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
1189 static 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, cache
1201          * the CFG value in parked_cfg and don't hit the update bit of CMD
1202          * 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
1210 static 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
1216 static 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 already
1223          * 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
1241 static 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 clear
1247          * the SRC and DIV of CFG register
1248          */
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 safeguard
1254          * against any update signal coming from the downstream clock.
1255          * The current parent is still prepared and enabled at this point, and
1256          * the safe source is always on while application processor subsystem
1257          * 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
1269 static 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
1280 static 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
1295 static unsigned long
1296 clk_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
1307 static 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 the
1313          *     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 child
1316          *     clks (branches) to turn the RCG on/off with a hardware feedback
1317          *     mechanism and only set the force enable bit in the RCG when we
1318          *     want to make sure the clk stays on for parent switches or
1319          *     parking.
1320          *
1321          *  3. Parks shared RCGs on the safe source at registration because we
1322          *     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 at
1324          *     boot, and the parent is turned off, the RCG will get stuck on. A
1325          *     GDSC can wedge if is turned on and the RCG is stuck on because
1326          *     the GDSC's controller will hang waiting for the clk status to
1327          *     toggle on when it never does.
1328          *
1329          * The safest option here is to "park" the RCG at init so that the clk
1330          * can never get stuck on or off. This ensures the GDSC can't get
1331          * wedged.
1332          */
1333         clk_rcg2_shared_disable(hw);
1334
1335         return 0;
1336 }
1337
1338 const 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 };
1349 EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops);
1350
1351 /* Common APIs to be used for DFS based RCGR */
1352 static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l,
1353                                        struct freq_tbl *f)
1354 {
1355         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1356         struct clk_hw *p;
1357         unsigned long prate = 0;
1358         u32 val, mask, cfg, mode, src;
1359         int i, num_parents;
1360
1361         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg);
1362
1363         mask = BIT(rcg->hid_width) - 1;
1364         f->pre_div = 1;
1365         if (cfg & mask)
1366                 f->pre_div = cfg & mask;
1367
1368         src = cfg & CFG_SRC_SEL_MASK;
1369         src >>= CFG_SRC_SEL_SHIFT;
1370
1371         num_parents = clk_hw_get_num_parents(hw);
1372         for (i = 0; i < num_parents; i++) {
1373                 if (src == rcg->parent_map[i].cfg) {
1374                         f->src = rcg->parent_map[i].src;
1375                         p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i);
1376                         prate = clk_hw_get_rate(p);
1377                 }
1378         }
1379
1380         mode = cfg & CFG_MODE_MASK;
1381         mode >>= CFG_MODE_SHIFT;
1382         if (mode) {
1383                 mask = BIT(rcg->mnd_width) - 1;
1384                 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l),
1385                             &val);
1386                 val &= mask;
1387                 f->m = val;
1388
1389                 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l),
1390                             &val);
1391                 val = ~val;
1392                 val &= mask;
1393                 val += f->m;
1394                 f->n = val;
1395         }
1396
1397         f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div);
1398 }
1399
1400 static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg)
1401 {
1402         struct freq_tbl *freq_tbl;
1403         int i;
1404
1405         /* Allocate space for 1 extra since table is NULL terminated */
1406         freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL);
1407         if (!freq_tbl)
1408                 return -ENOMEM;
1409         rcg->freq_tbl = freq_tbl;
1410
1411         for (i = 0; i < MAX_PERF_LEVEL; i++)
1412                 clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i);
1413
1414         return 0;
1415 }
1416
1417 static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw,
1418                                    struct clk_rate_request *req)
1419 {
1420         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1421         int ret;
1422
1423         if (!rcg->freq_tbl) {
1424                 ret = clk_rcg2_dfs_populate_freq_table(rcg);
1425                 if (ret) {
1426                         pr_err("Failed to update DFS tables for %s\n",
1427                                         clk_hw_get_name(hw));
1428                         return ret;
1429                 }
1430         }
1431
1432         return clk_rcg2_determine_rate(hw, req);
1433 }
1434
1435 static unsigned long
1436 clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1437 {
1438         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1439         u32 level, mask, cfg, m = 0, n = 0, mode, pre_div;
1440
1441         regmap_read(rcg->clkr.regmap,
1442                     rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level);
1443         level &= GENMASK(4, 1);
1444         level >>= 1;
1445
1446         if (rcg->freq_tbl)
1447                 return rcg->freq_tbl[level].freq;
1448
1449         /*
1450          * Assume that parent_rate is actually the parent because
1451          * we can't do any better at figuring it out when the table
1452          * hasn't been populated yet. We only populate the table
1453          * in determine_rate because we can't guarantee the parents
1454          * will be registered with the framework until then.
1455          */
1456         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level),
1457                     &cfg);
1458
1459         mask = BIT(rcg->hid_width) - 1;
1460         pre_div = 1;
1461         if (cfg & mask)
1462                 pre_div = cfg & mask;
1463
1464         mode = cfg & CFG_MODE_MASK;
1465         mode >>= CFG_MODE_SHIFT;
1466         if (mode) {
1467                 mask = BIT(rcg->mnd_width) - 1;
1468                 regmap_read(rcg->clkr.regmap,
1469                             rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m);
1470                 m &= mask;
1471
1472                 regmap_read(rcg->clkr.regmap,
1473                             rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n);
1474                 n = ~n;
1475                 n &= mask;
1476                 n += m;
1477         }
1478
1479         return calc_rate(parent_rate, m, n, mode, pre_div);
1480 }
1481
1482 static const struct clk_ops clk_rcg2_dfs_ops = {
1483         .is_enabled = clk_rcg2_is_enabled,
1484         .get_parent = clk_rcg2_get_parent,
1485         .determine_rate = clk_rcg2_dfs_determine_rate,
1486         .recalc_rate = clk_rcg2_dfs_recalc_rate,
1487 };
1488
1489 static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data,
1490                                struct regmap *regmap)
1491 {
1492         struct clk_rcg2 *rcg = data->rcg;
1493         struct clk_init_data *init = data->init;
1494         u32 val;
1495         int ret;
1496
1497         ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val);
1498         if (ret)
1499                 return -EINVAL;
1500
1501         if (!(val & SE_CMD_DFS_EN))
1502                 return 0;
1503
1504         /*
1505          * Rate changes with consumer writing a register in
1506          * their own I/O region
1507          */
1508         init->flags |= CLK_GET_RATE_NOCACHE;
1509         init->ops = &clk_rcg2_dfs_ops;
1510
1511         rcg->freq_tbl = NULL;
1512
1513         return 0;
1514 }
1515
1516 int qcom_cc_register_rcg_dfs(struct regmap *regmap,
1517                              const struct clk_rcg_dfs_data *rcgs, size_t len)
1518 {
1519         int i, ret;
1520
1521         for (i = 0; i < len; i++) {
1522                 ret = clk_rcg2_enable_dfs(&rcgs[i], regmap);
1523                 if (ret)
1524                         return ret;
1525         }
1526
1527         return 0;
1528 }
1529 EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs);
1530
1531 static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate,
1532                         unsigned long parent_rate)
1533 {
1534         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1535         struct freq_tbl f = { 0 };
1536         u32 mask = BIT(rcg->hid_width) - 1;
1537         u32 hid_div, cfg;
1538         int i, num_parents = clk_hw_get_num_parents(hw);
1539         unsigned long num, den;
1540
1541         rational_best_approximation(parent_rate, rate,
1542                         GENMASK(rcg->mnd_width - 1, 0),
1543                         GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1544
1545         if (!num || !den)
1546                 return -EINVAL;
1547
1548         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
1549         hid_div = cfg;
1550         cfg &= CFG_SRC_SEL_MASK;
1551         cfg >>= CFG_SRC_SEL_SHIFT;
1552
1553         for (i = 0; i < num_parents; i++) {
1554                 if (cfg == rcg->parent_map[i].cfg) {
1555                         f.src = rcg->parent_map[i].src;
1556                         break;
1557                 }
1558         }
1559
1560         f.pre_div = hid_div;
1561         f.pre_div >>= CFG_SRC_DIV_SHIFT;
1562         f.pre_div &= mask;
1563
1564         if (num != den) {
1565                 f.m = num;
1566                 f.n = den;
1567         } else {
1568                 f.m = 0;
1569                 f.n = 0;
1570         }
1571
1572         return clk_rcg2_configure(rcg, &f);
1573 }
1574
1575 static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw,
1576                 unsigned long rate, unsigned long parent_rate, u8 index)
1577 {
1578         return clk_rcg2_dp_set_rate(hw, rate, parent_rate);
1579 }
1580
1581 static int clk_rcg2_dp_determine_rate(struct clk_hw *hw,
1582                                 struct clk_rate_request *req)
1583 {
1584         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1585         unsigned long num, den;
1586         u64 tmp;
1587
1588         /* Parent rate is a fixed phy link rate */
1589         rational_best_approximation(req->best_parent_rate, req->rate,
1590                         GENMASK(rcg->mnd_width - 1, 0),
1591                         GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1592
1593         if (!num || !den)
1594                 return -EINVAL;
1595
1596         tmp = req->best_parent_rate * num;
1597         do_div(tmp, den);
1598         req->rate = tmp;
1599
1600         return 0;
1601 }
1602
1603 const struct clk_ops clk_dp_ops = {
1604         .is_enabled = clk_rcg2_is_enabled,
1605         .get_parent = clk_rcg2_get_parent,
1606         .set_parent = clk_rcg2_set_parent,
1607         .recalc_rate = clk_rcg2_recalc_rate,
1608         .set_rate = clk_rcg2_dp_set_rate,
1609         .set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent,
1610         .determine_rate = clk_rcg2_dp_determine_rate,
1611 };
1612 EXPORT_SYMBOL_GPL(clk_dp_ops);