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, d_val, not2d_val, n_minus_m;
 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                 /* Calculate 2d value */
 287                 d_val = f->n;
 288
 289                 n_minus_m = f->n - f->m;
 290                 n_minus_m *= 2;
 291
 292                 d_val = clamp_t(u32, d_val, f->m, n_minus_m);
 293                 not2d_val = ~d_val & mask;
 294
 295                 ret = regmap_update_bits(rcg->clkr.regmap,
 296                                 RCG_D_OFFSET(rcg), mask, not2d_val);
 297                 if (ret)
 298                         return ret;
 299         }
 300
 301         mask = BIT(rcg->hid_width) - 1;
 302         mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK | CFG_HW_CLK_CTRL_MASK;
 303         cfg = f->pre_div << CFG_SRC_DIV_SHIFT;
 304         cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
 305         if (rcg->mnd_width && f->n && (f->m != f->n))
 306                 cfg |= CFG_MODE_DUAL_EDGE;
 307         return regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg),
 308                                         mask, cfg);
 309 }
 310
 311 static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f)
 312 {
 313         int ret;
 314
 315         ret = __clk_rcg2_configure(rcg, f);
 316         if (ret)
 317                 return ret;
 318
 319         return update_config(rcg);
 320 }
 321
 322 static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
 323                                enum freq_policy policy)
 324 {
 325         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 326         const struct freq_tbl *f;
 327
 328         switch (policy) {
 329         case FLOOR:
 330                 f = qcom_find_freq_floor(rcg->freq_tbl, rate);
 331                 break;
 332         case CEIL:
 333                 f = qcom_find_freq(rcg->freq_tbl, rate);
 334                 break;
 335         default:
 336                 return -EINVAL;
 337         }
 338
 339         if (!f)
 340                 return -EINVAL;
 341
 342         return clk_rcg2_configure(rcg, f);
 343 }
 344
 345 static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
 346                             unsigned long parent_rate)
 347 {
 348         return __clk_rcg2_set_rate(hw, rate, CEIL);
 349 }
 350
 351 static int clk_rcg2_set_floor_rate(struct clk_hw *hw, unsigned long rate,
 352                                    unsigned long parent_rate)
 353 {
 354         return __clk_rcg2_set_rate(hw, rate, FLOOR);
 355 }
 356
 357 static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw,
 358                 unsigned long rate, unsigned long parent_rate, u8 index)
 359 {
 360         return __clk_rcg2_set_rate(hw, rate, CEIL);
 361 }
 362
 363 static int clk_rcg2_set_floor_rate_and_parent(struct clk_hw *hw,
 364                 unsigned long rate, unsigned long parent_rate, u8 index)
 365 {
 366         return __clk_rcg2_set_rate(hw, rate, FLOOR);
 367 }
 368
 369 const struct clk_ops clk_rcg2_ops = {
 370         .is_enabled = clk_rcg2_is_enabled,
 371         .get_parent = clk_rcg2_get_parent,
 372         .set_parent = clk_rcg2_set_parent,
 373         .recalc_rate = clk_rcg2_recalc_rate,
 374         .determine_rate = clk_rcg2_determine_rate,
 375         .set_rate = clk_rcg2_set_rate,
 376         .set_rate_and_parent = clk_rcg2_set_rate_and_parent,
 377 };
 378 EXPORT_SYMBOL_GPL(clk_rcg2_ops);
 379
 380 const struct clk_ops clk_rcg2_floor_ops = {
 381         .is_enabled = clk_rcg2_is_enabled,
 382         .get_parent = clk_rcg2_get_parent,
 383         .set_parent = clk_rcg2_set_parent,
 384         .recalc_rate = clk_rcg2_recalc_rate,
 385         .determine_rate = clk_rcg2_determine_floor_rate,
 386         .set_rate = clk_rcg2_set_floor_rate,
 387         .set_rate_and_parent = clk_rcg2_set_floor_rate_and_parent,
 388 };
 389 EXPORT_SYMBOL_GPL(clk_rcg2_floor_ops);
 390
 391 struct frac_entry {
 392         int num;
 393         int den;
 394 };
 395
 396 static const struct frac_entry frac_table_675m[] = {    /* link rate of 270M */
 397         { 52, 295 },    /* 119 M */
 398         { 11, 57 },     /* 130.25 M */
 399         { 63, 307 },    /* 138.50 M */
 400         { 11, 50 },     /* 148.50 M */
 401         { 47, 206 },    /* 154 M */
 402         { 31, 100 },    /* 205.25 M */
 403         { 107, 269 },   /* 268.50 M */
 404         { },
 405 };
 406
 407 static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */
 408         { 31, 211 },    /* 119 M */
 409         { 32, 199 },    /* 130.25 M */
 410         { 63, 307 },    /* 138.50 M */
 411         { 11, 60 },     /* 148.50 M */
 412         { 50, 263 },    /* 154 M */
 413         { 31, 120 },    /* 205.25 M */
 414         { 119, 359 },   /* 268.50 M */
 415         { },
 416 };
 417
 418 static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
 419                               unsigned long parent_rate)
 420 {
 421         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 422         struct freq_tbl f = *rcg->freq_tbl;
 423         const struct frac_entry *frac;
 424         int delta = 100000;
 425         s64 src_rate = parent_rate;
 426         s64 request;
 427         u32 mask = BIT(rcg->hid_width) - 1;
 428         u32 hid_div;
 429
 430         if (src_rate == 810000000)
 431                 frac = frac_table_810m;
 432         else
 433                 frac = frac_table_675m;
 434
 435         for (; frac->num; frac++) {
 436                 request = rate;
 437                 request *= frac->den;
 438                 request = div_s64(request, frac->num);
 439                 if ((src_rate < (request - delta)) ||
 440                     (src_rate > (request + delta)))
 441                         continue;
 442
 443                 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
 444                                 &hid_div);
 445                 f.pre_div = hid_div;
 446                 f.pre_div >>= CFG_SRC_DIV_SHIFT;
 447                 f.pre_div &= mask;
 448                 f.m = frac->num;
 449                 f.n = frac->den;
 450
 451                 return clk_rcg2_configure(rcg, &f);
 452         }
 453
 454         return -EINVAL;
 455 }
 456
 457 static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,
 458                 unsigned long rate, unsigned long parent_rate, u8 index)
 459 {
 460         /* Parent index is set statically in frequency table */
 461         return clk_edp_pixel_set_rate(hw, rate, parent_rate);
 462 }
 463
 464 static int clk_edp_pixel_determine_rate(struct clk_hw *hw,
 465                                         struct clk_rate_request *req)
 466 {
 467         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 468         const struct freq_tbl *f = rcg->freq_tbl;
 469         const struct frac_entry *frac;
 470         int delta = 100000;
 471         s64 request;
 472         u32 mask = BIT(rcg->hid_width) - 1;
 473         u32 hid_div;
 474         int index = qcom_find_src_index(hw, rcg->parent_map, f->src);
 475
 476         /* Force the correct parent */
 477         req->best_parent_hw = clk_hw_get_parent_by_index(hw, index);
 478         req->best_parent_rate = clk_hw_get_rate(req->best_parent_hw);
 479
 480         if (req->best_parent_rate == 810000000)
 481                 frac = frac_table_810m;
 482         else
 483                 frac = frac_table_675m;
 484
 485         for (; frac->num; frac++) {
 486                 request = req->rate;
 487                 request *= frac->den;
 488                 request = div_s64(request, frac->num);
 489                 if ((req->best_parent_rate < (request - delta)) ||
 490                     (req->best_parent_rate > (request + delta)))
 491                         continue;
 492
 493                 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
 494                                 &hid_div);
 495                 hid_div >>= CFG_SRC_DIV_SHIFT;
 496                 hid_div &= mask;
 497
 498                 req->rate = calc_rate(req->best_parent_rate,
 499                                       frac->num, frac->den,
 500                                       !!frac->den, hid_div);
 501                 return 0;
 502         }
 503
 504         return -EINVAL;
 505 }
 506
 507 const struct clk_ops clk_edp_pixel_ops = {
 508         .is_enabled = clk_rcg2_is_enabled,
 509         .get_parent = clk_rcg2_get_parent,
 510         .set_parent = clk_rcg2_set_parent,
 511         .recalc_rate = clk_rcg2_recalc_rate,
 512         .set_rate = clk_edp_pixel_set_rate,
 513         .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent,
 514         .determine_rate = clk_edp_pixel_determine_rate,
 515 };
 516 EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);
 517
 518 static int clk_byte_determine_rate(struct clk_hw *hw,
 519                                    struct clk_rate_request *req)
 520 {
 521         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 522         const struct freq_tbl *f = rcg->freq_tbl;
 523         int index = qcom_find_src_index(hw, rcg->parent_map, f->src);
 524         unsigned long parent_rate, div;
 525         u32 mask = BIT(rcg->hid_width) - 1;
 526         struct clk_hw *p;
 527
 528         if (req->rate == 0)
 529                 return -EINVAL;
 530
 531         req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index);
 532         req->best_parent_rate = parent_rate = clk_hw_round_rate(p, req->rate);
 533
 534         div = DIV_ROUND_UP((2 * parent_rate), req->rate) - 1;
 535         div = min_t(u32, div, mask);
 536
 537         req->rate = calc_rate(parent_rate, 0, 0, 0, div);
 538
 539         return 0;
 540 }
 541
 542 static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate,
 543                          unsigned long parent_rate)
 544 {
 545         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 546         struct freq_tbl f = *rcg->freq_tbl;
 547         unsigned long div;
 548         u32 mask = BIT(rcg->hid_width) - 1;
 549
 550         div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
 551         div = min_t(u32, div, mask);
 552
 553         f.pre_div = div;
 554
 555         return clk_rcg2_configure(rcg, &f);
 556 }
 557
 558 static int clk_byte_set_rate_and_parent(struct clk_hw *hw,
 559                 unsigned long rate, unsigned long parent_rate, u8 index)
 560 {
 561         /* Parent index is set statically in frequency table */
 562         return clk_byte_set_rate(hw, rate, parent_rate);
 563 }
 564
 565 const struct clk_ops clk_byte_ops = {
 566         .is_enabled = clk_rcg2_is_enabled,
 567         .get_parent = clk_rcg2_get_parent,
 568         .set_parent = clk_rcg2_set_parent,
 569         .recalc_rate = clk_rcg2_recalc_rate,
 570         .set_rate = clk_byte_set_rate,
 571         .set_rate_and_parent = clk_byte_set_rate_and_parent,
 572         .determine_rate = clk_byte_determine_rate,
 573 };
 574 EXPORT_SYMBOL_GPL(clk_byte_ops);
 575
 576 static int clk_byte2_determine_rate(struct clk_hw *hw,
 577                                     struct clk_rate_request *req)
 578 {
 579         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 580         unsigned long parent_rate, div;
 581         u32 mask = BIT(rcg->hid_width) - 1;
 582         struct clk_hw *p;
 583         unsigned long rate = req->rate;
 584
 585         if (rate == 0)
 586                 return -EINVAL;
 587
 588         p = req->best_parent_hw;
 589         req->best_parent_rate = parent_rate = clk_hw_round_rate(p, rate);
 590
 591         div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
 592         div = min_t(u32, div, mask);
 593
 594         req->rate = calc_rate(parent_rate, 0, 0, 0, div);
 595
 596         return 0;
 597 }
 598
 599 static int clk_byte2_set_rate(struct clk_hw *hw, unsigned long rate,
 600                          unsigned long parent_rate)
 601 {
 602         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 603         struct freq_tbl f = { 0 };
 604         unsigned long div;
 605         int i, num_parents = clk_hw_get_num_parents(hw);
 606         u32 mask = BIT(rcg->hid_width) - 1;
 607         u32 cfg;
 608
 609         div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
 610         div = min_t(u32, div, mask);
 611
 612         f.pre_div = div;
 613
 614         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
 615         cfg &= CFG_SRC_SEL_MASK;
 616         cfg >>= CFG_SRC_SEL_SHIFT;
 617
 618         for (i = 0; i < num_parents; i++) {
 619                 if (cfg == rcg->parent_map[i].cfg) {
 620                         f.src = rcg->parent_map[i].src;
 621                         return clk_rcg2_configure(rcg, &f);
 622                 }
 623         }
 624
 625         return -EINVAL;
 626 }
 627
 628 static int clk_byte2_set_rate_and_parent(struct clk_hw *hw,
 629                 unsigned long rate, unsigned long parent_rate, u8 index)
 630 {
 631         /* Read the hardware to determine parent during set_rate */
 632         return clk_byte2_set_rate(hw, rate, parent_rate);
 633 }
 634
 635 const struct clk_ops clk_byte2_ops = {
 636         .is_enabled = clk_rcg2_is_enabled,
 637         .get_parent = clk_rcg2_get_parent,
 638         .set_parent = clk_rcg2_set_parent,
 639         .recalc_rate = clk_rcg2_recalc_rate,
 640         .set_rate = clk_byte2_set_rate,
 641         .set_rate_and_parent = clk_byte2_set_rate_and_parent,
 642         .determine_rate = clk_byte2_determine_rate,
 643 };
 644 EXPORT_SYMBOL_GPL(clk_byte2_ops);
 645
 646 static const struct frac_entry frac_table_pixel[] = {
 647         { 3, 8 },
 648         { 2, 9 },
 649         { 4, 9 },
 650         { 1, 1 },
 651         { 2, 3 },
 652         { }
 653 };
 654
 655 static int clk_pixel_determine_rate(struct clk_hw *hw,
 656                                     struct clk_rate_request *req)
 657 {
 658         unsigned long request, src_rate;
 659         int delta = 100000;
 660         const struct frac_entry *frac = frac_table_pixel;
 661
 662         for (; frac->num; frac++) {
 663                 request = (req->rate * frac->den) / frac->num;
 664
 665                 src_rate = clk_hw_round_rate(req->best_parent_hw, request);
 666                 if ((src_rate < (request - delta)) ||
 667                         (src_rate > (request + delta)))
 668                         continue;
 669
 670                 req->best_parent_rate = src_rate;
 671                 req->rate = (src_rate * frac->num) / frac->den;
 672                 return 0;
 673         }
 674
 675         return -EINVAL;
 676 }
 677
 678 static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
 679                 unsigned long parent_rate)
 680 {
 681         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 682         struct freq_tbl f = { 0 };
 683         const struct frac_entry *frac = frac_table_pixel;
 684         unsigned long request;
 685         int delta = 100000;
 686         u32 mask = BIT(rcg->hid_width) - 1;
 687         u32 hid_div, cfg;
 688         int i, num_parents = clk_hw_get_num_parents(hw);
 689
 690         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
 691         cfg &= CFG_SRC_SEL_MASK;
 692         cfg >>= CFG_SRC_SEL_SHIFT;
 693
 694         for (i = 0; i < num_parents; i++)
 695                 if (cfg == rcg->parent_map[i].cfg) {
 696                         f.src = rcg->parent_map[i].src;
 697                         break;
 698                 }
 699
 700         for (; frac->num; frac++) {
 701                 request = (rate * frac->den) / frac->num;
 702
 703                 if ((parent_rate < (request - delta)) ||
 704                         (parent_rate > (request + delta)))
 705                         continue;
 706
 707                 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
 708                                 &hid_div);
 709                 f.pre_div = hid_div;
 710                 f.pre_div >>= CFG_SRC_DIV_SHIFT;
 711                 f.pre_div &= mask;
 712                 f.m = frac->num;
 713                 f.n = frac->den;
 714
 715                 return clk_rcg2_configure(rcg, &f);
 716         }
 717         return -EINVAL;
 718 }
 719
 720 static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
 721                 unsigned long parent_rate, u8 index)
 722 {
 723         return clk_pixel_set_rate(hw, rate, parent_rate);
 724 }
 725
 726 const struct clk_ops clk_pixel_ops = {
 727         .is_enabled = clk_rcg2_is_enabled,
 728         .get_parent = clk_rcg2_get_parent,
 729         .set_parent = clk_rcg2_set_parent,
 730         .recalc_rate = clk_rcg2_recalc_rate,
 731         .set_rate = clk_pixel_set_rate,
 732         .set_rate_and_parent = clk_pixel_set_rate_and_parent,
 733         .determine_rate = clk_pixel_determine_rate,
 734 };
 735 EXPORT_SYMBOL_GPL(clk_pixel_ops);
 736
 737 static int clk_gfx3d_determine_rate(struct clk_hw *hw,
 738                                     struct clk_rate_request *req)
 739 {
 740         struct clk_rate_request parent_req = { };
 741         struct clk_hw *p2, *p8, *p9, *xo;
 742         unsigned long p9_rate;
 743         int ret;
 744
 745         xo = clk_hw_get_parent_by_index(hw, 0);
 746         if (req->rate == clk_hw_get_rate(xo)) {
 747                 req->best_parent_hw = xo;
 748                 return 0;
 749         }
 750
 751         p9 = clk_hw_get_parent_by_index(hw, 2);
 752         p2 = clk_hw_get_parent_by_index(hw, 3);
 753         p8 = clk_hw_get_parent_by_index(hw, 4);
 754
 755         /* PLL9 is a fixed rate PLL */
 756         p9_rate = clk_hw_get_rate(p9);
 757
 758         parent_req.rate = req->rate = min(req->rate, p9_rate);
 759         if (req->rate == p9_rate) {
 760                 req->rate = req->best_parent_rate = p9_rate;
 761                 req->best_parent_hw = p9;
 762                 return 0;
 763         }
 764
 765         if (req->best_parent_hw == p9) {
 766                 /* Are we going back to a previously used rate? */
 767                 if (clk_hw_get_rate(p8) == req->rate)
 768                         req->best_parent_hw = p8;
 769                 else
 770                         req->best_parent_hw = p2;
 771         } else if (req->best_parent_hw == p8) {
 772                 req->best_parent_hw = p2;
 773         } else {
 774                 req->best_parent_hw = p8;
 775         }
 776
 777         ret = __clk_determine_rate(req->best_parent_hw, &parent_req);
 778         if (ret)
 779                 return ret;
 780
 781         req->rate = req->best_parent_rate = parent_req.rate;
 782
 783         return 0;
 784 }
 785
 786 static int clk_gfx3d_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
 787                 unsigned long parent_rate, u8 index)
 788 {
 789         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 790         u32 cfg;
 791         int ret;
 792
 793         /* Just mux it, we don't use the division or m/n hardware */
 794         cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
 795         ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg);
 796         if (ret)
 797                 return ret;
 798
 799         return update_config(rcg);
 800 }
 801
 802 static int clk_gfx3d_set_rate(struct clk_hw *hw, unsigned long rate,
 803                               unsigned long parent_rate)
 804 {
 805         /*
 806          * We should never get here; clk_gfx3d_determine_rate() should always
 807          * make us use a different parent than what we're currently using, so
 808          * clk_gfx3d_set_rate_and_parent() should always be called.
 809          */
 810         return 0;
 811 }
 812
 813 const struct clk_ops clk_gfx3d_ops = {
 814         .is_enabled = clk_rcg2_is_enabled,
 815         .get_parent = clk_rcg2_get_parent,
 816         .set_parent = clk_rcg2_set_parent,
 817         .recalc_rate = clk_rcg2_recalc_rate,
 818         .set_rate = clk_gfx3d_set_rate,
 819         .set_rate_and_parent = clk_gfx3d_set_rate_and_parent,
 820         .determine_rate = clk_gfx3d_determine_rate,
 821 };
 822 EXPORT_SYMBOL_GPL(clk_gfx3d_ops);
 823
 824 static int clk_rcg2_set_force_enable(struct clk_hw *hw)
 825 {
 826         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 827         const char *name = clk_hw_get_name(hw);
 828         int ret, count;
 829
 830         ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
 831                                  CMD_ROOT_EN, CMD_ROOT_EN);
 832         if (ret)
 833                 return ret;
 834
 835         /* wait for RCG to turn ON */
 836         for (count = 500; count > 0; count--) {
 837                 if (clk_rcg2_is_enabled(hw))
 838                         return 0;
 839
 840                 udelay(1);
 841         }
 842
 843         pr_err("%s: RCG did not turn on\n", name);
 844         return -ETIMEDOUT;
 845 }
 846
 847 static int clk_rcg2_clear_force_enable(struct clk_hw *hw)
 848 {
 849         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 850
 851         return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
 852                                         CMD_ROOT_EN, 0);
 853 }
 854
 855 static int
 856 clk_rcg2_shared_force_enable_clear(struct clk_hw *hw, const struct freq_tbl *f)
 857 {
 858         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 859         int ret;
 860
 861         ret = clk_rcg2_set_force_enable(hw);
 862         if (ret)
 863                 return ret;
 864
 865         ret = clk_rcg2_configure(rcg, f);
 866         if (ret)
 867                 return ret;
 868
 869         return clk_rcg2_clear_force_enable(hw);
 870 }
 871
 872 static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate,
 873                                     unsigned long parent_rate)
 874 {
 875         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 876         const struct freq_tbl *f;
 877
 878         f = qcom_find_freq(rcg->freq_tbl, rate);
 879         if (!f)
 880                 return -EINVAL;
 881
 882         /*
 883          * In case clock is disabled, update the CFG, M, N and D registers
 884          * and don't hit the update bit of CMD register.
 885          */
 886         if (!__clk_is_enabled(hw->clk))
 887                 return __clk_rcg2_configure(rcg, f);
 888
 889         return clk_rcg2_shared_force_enable_clear(hw, f);
 890 }
 891
 892 static int clk_rcg2_shared_set_rate_and_parent(struct clk_hw *hw,
 893                 unsigned long rate, unsigned long parent_rate, u8 index)
 894 {
 895         return clk_rcg2_shared_set_rate(hw, rate, parent_rate);
 896 }
 897
 898 static int clk_rcg2_shared_enable(struct clk_hw *hw)
 899 {
 900         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 901         int ret;
 902
 903         /*
 904          * Set the update bit because required configuration has already
 905          * been written in clk_rcg2_shared_set_rate()
 906          */
 907         ret = clk_rcg2_set_force_enable(hw);
 908         if (ret)
 909                 return ret;
 910
 911         ret = update_config(rcg);
 912         if (ret)
 913                 return ret;
 914
 915         return clk_rcg2_clear_force_enable(hw);
 916 }
 917
 918 static void clk_rcg2_shared_disable(struct clk_hw *hw)
 919 {
 920         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 921         u32 cfg;
 922
 923         /*
 924          * Store current configuration as switching to safe source would clear
 925          * the SRC and DIV of CFG register
 926          */
 927         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
 928
 929         /*
 930          * Park the RCG at a safe configuration - sourced off of safe source.
 931          * Force enable and disable the RCG while configuring it to safeguard
 932          * against any update signal coming from the downstream clock.
 933          * The current parent is still prepared and enabled at this point, and
 934          * the safe source is always on while application processor subsystem
 935          * is online. Therefore, the RCG can safely switch its parent.
 936          */
 937         clk_rcg2_set_force_enable(hw);
 938
 939         regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
 940                      rcg->safe_src_index << CFG_SRC_SEL_SHIFT);
 941
 942         update_config(rcg);
 943
 944         clk_rcg2_clear_force_enable(hw);
 945
 946         /* Write back the stored configuration corresponding to current rate */
 947         regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg);
 948 }
 949
 950 const struct clk_ops clk_rcg2_shared_ops = {
 951         .enable = clk_rcg2_shared_enable,
 952         .disable = clk_rcg2_shared_disable,
 953         .get_parent = clk_rcg2_get_parent,
 954         .set_parent = clk_rcg2_set_parent,
 955         .recalc_rate = clk_rcg2_recalc_rate,
 956         .determine_rate = clk_rcg2_determine_rate,
 957         .set_rate = clk_rcg2_shared_set_rate,
 958         .set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent,
 959 };
 960 EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops);
 961
 962 /* Common APIs to be used for DFS based RCGR */
 963 static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l,
 964                                        struct freq_tbl *f)
 965 {
 966         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 967         struct clk_hw *p;
 968         unsigned long prate = 0;
 969         u32 val, mask, cfg, mode, src;
 970         int i, num_parents;
 971
 972         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg);
 973
 974         mask = BIT(rcg->hid_width) - 1;
 975         f->pre_div = 1;
 976         if (cfg & mask)
 977                 f->pre_div = cfg & mask;
 978
 979         src = cfg & CFG_SRC_SEL_MASK;
 980         src >>= CFG_SRC_SEL_SHIFT;
 981
 982         num_parents = clk_hw_get_num_parents(hw);
 983         for (i = 0; i < num_parents; i++) {
 984                 if (src == rcg->parent_map[i].cfg) {
 985                         f->src = rcg->parent_map[i].src;
 986                         p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i);
 987                         prate = clk_hw_get_rate(p);
 988                 }
 989         }
 990
 991         mode = cfg & CFG_MODE_MASK;
 992         mode >>= CFG_MODE_SHIFT;
 993         if (mode) {
 994                 mask = BIT(rcg->mnd_width) - 1;
 995                 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l),
 996                             &val);
 997                 val &= mask;
 998                 f->m = val;
 999
1000                 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l),
1001                             &val);
1002                 val = ~val;
1003                 val &= mask;
1004                 val += f->m;
1005                 f->n = val;
1006         }
1007
1008         f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div);
1009 }
1010
1011 static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg)
1012 {
1013         struct freq_tbl *freq_tbl;
1014         int i;
1015
1016         /* Allocate space for 1 extra since table is NULL terminated */
1017         freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL);
1018         if (!freq_tbl)
1019                 return -ENOMEM;
1020         rcg->freq_tbl = freq_tbl;
1021
1022         for (i = 0; i < MAX_PERF_LEVEL; i++)
1023                 clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i);
1024
1025         return 0;
1026 }
1027
1028 static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw,
1029                                    struct clk_rate_request *req)
1030 {
1031         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1032         int ret;
1033
1034         if (!rcg->freq_tbl) {
1035                 ret = clk_rcg2_dfs_populate_freq_table(rcg);
1036                 if (ret) {
1037                         pr_err("Failed to update DFS tables for %s\n",
1038                                         clk_hw_get_name(hw));
1039                         return ret;
1040                 }
1041         }
1042
1043         return clk_rcg2_determine_rate(hw, req);
1044 }
1045
1046 static unsigned long
1047 clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1048 {
1049         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1050         u32 level, mask, cfg, m = 0, n = 0, mode, pre_div;
1051
1052         regmap_read(rcg->clkr.regmap,
1053                     rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level);
1054         level &= GENMASK(4, 1);
1055         level >>= 1;
1056
1057         if (rcg->freq_tbl)
1058                 return rcg->freq_tbl[level].freq;
1059
1060         /*
1061          * Assume that parent_rate is actually the parent because
1062          * we can't do any better at figuring it out when the table
1063          * hasn't been populated yet. We only populate the table
1064          * in determine_rate because we can't guarantee the parents
1065          * will be registered with the framework until then.
1066          */
1067         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level),
1068                     &cfg);
1069
1070         mask = BIT(rcg->hid_width) - 1;
1071         pre_div = 1;
1072         if (cfg & mask)
1073                 pre_div = cfg & mask;
1074
1075         mode = cfg & CFG_MODE_MASK;
1076         mode >>= CFG_MODE_SHIFT;
1077         if (mode) {
1078                 mask = BIT(rcg->mnd_width) - 1;
1079                 regmap_read(rcg->clkr.regmap,
1080                             rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m);
1081                 m &= mask;
1082
1083                 regmap_read(rcg->clkr.regmap,
1084                             rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n);
1085                 n = ~n;
1086                 n &= mask;
1087                 n += m;
1088         }
1089
1090         return calc_rate(parent_rate, m, n, mode, pre_div);
1091 }
1092
1093 static const struct clk_ops clk_rcg2_dfs_ops = {
1094         .is_enabled = clk_rcg2_is_enabled,
1095         .get_parent = clk_rcg2_get_parent,
1096         .determine_rate = clk_rcg2_dfs_determine_rate,
1097         .recalc_rate = clk_rcg2_dfs_recalc_rate,
1098 };
1099
1100 static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data,
1101                                struct regmap *regmap)
1102 {
1103         struct clk_rcg2 *rcg = data->rcg;
1104         struct clk_init_data *init = data->init;
1105         u32 val;
1106         int ret;
1107
1108         ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val);
1109         if (ret)
1110                 return -EINVAL;
1111
1112         if (!(val & SE_CMD_DFS_EN))
1113                 return 0;
1114
1115         /*
1116          * Rate changes with consumer writing a register in
1117          * their own I/O region
1118          */
1119         init->flags |= CLK_GET_RATE_NOCACHE;
1120         init->ops = &clk_rcg2_dfs_ops;
1121
1122         rcg->freq_tbl = NULL;
1123
1124         return 0;
1125 }
1126
1127 int qcom_cc_register_rcg_dfs(struct regmap *regmap,
1128                              const struct clk_rcg_dfs_data *rcgs, size_t len)
1129 {
1130         int i, ret;
1131
1132         for (i = 0; i < len; i++) {
1133                 ret = clk_rcg2_enable_dfs(&rcgs[i], regmap);
1134                 if (ret)
1135                         return ret;
1136         }
1137
1138         return 0;
1139 }
1140 EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs);
1141
1142 static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate,
1143                         unsigned long parent_rate)
1144 {
1145         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1146         struct freq_tbl f = { 0 };
1147         u32 mask = BIT(rcg->hid_width) - 1;
1148         u32 hid_div, cfg;
1149         int i, num_parents = clk_hw_get_num_parents(hw);
1150         unsigned long num, den;
1151
1152         rational_best_approximation(parent_rate, rate,
1153                         GENMASK(rcg->mnd_width - 1, 0),
1154                         GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1155
1156         if (!num || !den)
1157                 return -EINVAL;
1158
1159         regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
1160         hid_div = cfg;
1161         cfg &= CFG_SRC_SEL_MASK;
1162         cfg >>= CFG_SRC_SEL_SHIFT;
1163
1164         for (i = 0; i < num_parents; i++) {
1165                 if (cfg == rcg->parent_map[i].cfg) {
1166                         f.src = rcg->parent_map[i].src;
1167                         break;
1168                 }
1169         }
1170
1171         f.pre_div = hid_div;
1172         f.pre_div >>= CFG_SRC_DIV_SHIFT;
1173         f.pre_div &= mask;
1174
1175         if (num != den) {
1176                 f.m = num;
1177                 f.n = den;
1178         } else {
1179                 f.m = 0;
1180                 f.n = 0;
1181         }
1182
1183         return clk_rcg2_configure(rcg, &f);
1184 }
1185
1186 static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw,
1187                 unsigned long rate, unsigned long parent_rate, u8 index)
1188 {
1189         return clk_rcg2_dp_set_rate(hw, rate, parent_rate);
1190 }
1191
1192 static int clk_rcg2_dp_determine_rate(struct clk_hw *hw,
1193                                 struct clk_rate_request *req)
1194 {
1195         struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1196         unsigned long num, den;
1197         u64 tmp;
1198
1199         /* Parent rate is a fixed phy link rate */
1200         rational_best_approximation(req->best_parent_rate, req->rate,
1201                         GENMASK(rcg->mnd_width - 1, 0),
1202                         GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1203
1204         if (!num || !den)
1205                 return -EINVAL;
1206
1207         tmp = req->best_parent_rate * num;
1208         do_div(tmp, den);
1209         req->rate = tmp;
1210
1211         return 0;
1212 }
1213
1214 const struct clk_ops clk_dp_ops = {
1215         .is_enabled = clk_rcg2_is_enabled,
1216         .get_parent = clk_rcg2_get_parent,
1217         .set_parent = clk_rcg2_set_parent,
1218         .recalc_rate = clk_rcg2_recalc_rate,
1219         .set_rate = clk_rcg2_dp_set_rate,
1220         .set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent,
1221         .determine_rate = clk_rcg2_dp_determine_rate,
1222 };
1223 EXPORT_SYMBOL_GPL(clk_dp_ops);