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