drivers/clk/qcom/clk-rcg.c

   1 /*
   2  * Copyright (c) 2013, The Linux Foundation. All rights reserved.
   3  *
   4  * This software is licensed under the terms of the GNU General Public
   5  * License version 2, as published by the Free Software Foundation, and
   6  * may be copied, distributed, and modified under those terms.
   7  *
   8  * This program is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  */
  13
  14 #include <linux/kernel.h>
  15 #include <linux/bitops.h>
  16 #include <linux/err.h>
  17 #include <linux/export.h>
  18 #include <linux/clk-provider.h>
  19 #include <linux/regmap.h>
  20
  21 #include <asm/div64.h>
  22
  23 #include "clk-rcg.h"
  24 #include "common.h"
  25
  26 static u32 ns_to_src(struct src_sel *s, u32 ns)
  27 {
  28         ns >>= s->src_sel_shift;
  29         ns &= SRC_SEL_MASK;
  30         return ns;
  31 }
  32
  33 static u32 src_to_ns(struct src_sel *s, u8 src, u32 ns)
  34 {
  35         u32 mask;
  36
  37         mask = SRC_SEL_MASK;
  38         mask <<= s->src_sel_shift;
  39         ns &= ~mask;
  40
  41         ns |= src << s->src_sel_shift;
  42         return ns;
  43 }
  44
  45 static u8 clk_rcg_get_parent(struct clk_hw *hw)
  46 {
  47         struct clk_rcg *rcg = to_clk_rcg(hw);
  48         int num_parents = __clk_get_num_parents(hw->clk);
  49         u32 ns;
  50         int i;
  51
  52         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
  53         ns = ns_to_src(&rcg->s, ns);
  54         for (i = 0; i < num_parents; i++)
  55                 if (ns == rcg->s.parent_map[i])
  56                         return i;
  57
  58         return -EINVAL;
  59 }
  60
  61 static int reg_to_bank(struct clk_dyn_rcg *rcg, u32 bank)
  62 {
  63         bank &= BIT(rcg->mux_sel_bit);
  64         return !!bank;
  65 }
  66
  67 static u8 clk_dyn_rcg_get_parent(struct clk_hw *hw)
  68 {
  69         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
  70         int num_parents = __clk_get_num_parents(hw->clk);
  71         u32 ns, reg;
  72         int bank;
  73         int i;
  74         struct src_sel *s;
  75
  76         regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
  77         bank = reg_to_bank(rcg, reg);
  78         s = &rcg->s[bank];
  79
  80         regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
  81         ns = ns_to_src(s, ns);
  82
  83         for (i = 0; i < num_parents; i++)
  84                 if (ns == s->parent_map[i])
  85                         return i;
  86
  87         return -EINVAL;
  88 }
  89
  90 static int clk_rcg_set_parent(struct clk_hw *hw, u8 index)
  91 {
  92         struct clk_rcg *rcg = to_clk_rcg(hw);
  93         u32 ns;
  94
  95         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
  96         ns = src_to_ns(&rcg->s, rcg->s.parent_map[index], ns);
  97         regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
  98
  99         return 0;
 100 }
 101
 102 static u32 md_to_m(struct mn *mn, u32 md)
 103 {
 104         md >>= mn->m_val_shift;
 105         md &= BIT(mn->width) - 1;
 106         return md;
 107 }
 108
 109 static u32 ns_to_pre_div(struct pre_div *p, u32 ns)
 110 {
 111         ns >>= p->pre_div_shift;
 112         ns &= BIT(p->pre_div_width) - 1;
 113         return ns;
 114 }
 115
 116 static u32 pre_div_to_ns(struct pre_div *p, u8 pre_div, u32 ns)
 117 {
 118         u32 mask;
 119
 120         mask = BIT(p->pre_div_width) - 1;
 121         mask <<= p->pre_div_shift;
 122         ns &= ~mask;
 123
 124         ns |= pre_div << p->pre_div_shift;
 125         return ns;
 126 }
 127
 128 static u32 mn_to_md(struct mn *mn, u32 m, u32 n, u32 md)
 129 {
 130         u32 mask, mask_w;
 131
 132         mask_w = BIT(mn->width) - 1;
 133         mask = (mask_w << mn->m_val_shift) | mask_w;
 134         md &= ~mask;
 135
 136         if (n) {
 137                 m <<= mn->m_val_shift;
 138                 md |= m;
 139                 md |= ~n & mask_w;
 140         }
 141
 142         return md;
 143 }
 144
 145 static u32 ns_m_to_n(struct mn *mn, u32 ns, u32 m)
 146 {
 147         ns = ~ns >> mn->n_val_shift;
 148         ns &= BIT(mn->width) - 1;
 149         return ns + m;
 150 }
 151
 152 static u32 reg_to_mnctr_mode(struct mn *mn, u32 val)
 153 {
 154         val >>= mn->mnctr_mode_shift;
 155         val &= MNCTR_MODE_MASK;
 156         return val;
 157 }
 158
 159 static u32 mn_to_ns(struct mn *mn, u32 m, u32 n, u32 ns)
 160 {
 161         u32 mask;
 162
 163         mask = BIT(mn->width) - 1;
 164         mask <<= mn->n_val_shift;
 165         ns &= ~mask;
 166
 167         if (n) {
 168                 n = n - m;
 169                 n = ~n;
 170                 n &= BIT(mn->width) - 1;
 171                 n <<= mn->n_val_shift;
 172                 ns |= n;
 173         }
 174
 175         return ns;
 176 }
 177
 178 static u32 mn_to_reg(struct mn *mn, u32 m, u32 n, u32 val)
 179 {
 180         u32 mask;
 181
 182         mask = MNCTR_MODE_MASK << mn->mnctr_mode_shift;
 183         mask |= BIT(mn->mnctr_en_bit);
 184         val &= ~mask;
 185
 186         if (n) {
 187                 val |= BIT(mn->mnctr_en_bit);
 188                 val |= MNCTR_MODE_DUAL << mn->mnctr_mode_shift;
 189         }
 190
 191         return val;
 192 }
 193
 194 static void configure_bank(struct clk_dyn_rcg *rcg, const struct freq_tbl *f)
 195 {
 196         u32 ns, md, reg;
 197         int bank, new_bank;
 198         struct mn *mn;
 199         struct pre_div *p;
 200         struct src_sel *s;
 201         bool enabled;
 202         u32 md_reg, ns_reg;
 203         bool banked_mn = !!rcg->mn[1].width;
 204         bool banked_p = !!rcg->p[1].pre_div_width;
 205         struct clk_hw *hw = &rcg->clkr.hw;
 206
 207         enabled = __clk_is_enabled(hw->clk);
 208
 209         regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
 210         bank = reg_to_bank(rcg, reg);
 211         new_bank = enabled ? !bank : bank;
 212
 213         ns_reg = rcg->ns_reg[new_bank];
 214         regmap_read(rcg->clkr.regmap, ns_reg, &ns);
 215
 216         if (banked_mn) {
 217                 mn = &rcg->mn[new_bank];
 218                 md_reg = rcg->md_reg[new_bank];
 219
 220                 ns |= BIT(mn->mnctr_reset_bit);
 221                 regmap_write(rcg->clkr.regmap, ns_reg, ns);
 222
 223                 regmap_read(rcg->clkr.regmap, md_reg, &md);
 224                 md = mn_to_md(mn, f->m, f->n, md);
 225                 regmap_write(rcg->clkr.regmap, md_reg, md);
 226
 227                 ns = mn_to_ns(mn, f->m, f->n, ns);
 228                 regmap_write(rcg->clkr.regmap, ns_reg, ns);
 229
 230                 /* Two NS registers means mode control is in NS register */
 231                 if (rcg->ns_reg[0] != rcg->ns_reg[1]) {
 232                         ns = mn_to_reg(mn, f->m, f->n, ns);
 233                         regmap_write(rcg->clkr.regmap, ns_reg, ns);
 234                 } else {
 235                         reg = mn_to_reg(mn, f->m, f->n, reg);
 236                         regmap_write(rcg->clkr.regmap, rcg->bank_reg, reg);
 237                 }
 238
 239                 ns &= ~BIT(mn->mnctr_reset_bit);
 240                 regmap_write(rcg->clkr.regmap, ns_reg, ns);
 241         }
 242
 243         if (banked_p) {
 244                 p = &rcg->p[new_bank];
 245                 ns = pre_div_to_ns(p, f->pre_div - 1, ns);
 246         }
 247
 248         s = &rcg->s[new_bank];
 249         ns = src_to_ns(s, s->parent_map[f->src], ns);
 250         regmap_write(rcg->clkr.regmap, ns_reg, ns);
 251
 252         if (enabled) {
 253                 regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
 254                 reg ^= BIT(rcg->mux_sel_bit);
 255                 regmap_write(rcg->clkr.regmap, rcg->bank_reg, reg);
 256         }
 257 }
 258
 259 static int clk_dyn_rcg_set_parent(struct clk_hw *hw, u8 index)
 260 {
 261         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
 262         u32 ns, md, reg;
 263         int bank;
 264         struct freq_tbl f = { 0 };
 265         bool banked_mn = !!rcg->mn[1].width;
 266         bool banked_p = !!rcg->p[1].pre_div_width;
 267
 268         regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
 269         bank = reg_to_bank(rcg, reg);
 270
 271         regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
 272
 273         if (banked_mn) {
 274                 regmap_read(rcg->clkr.regmap, rcg->md_reg[bank], &md);
 275                 f.m = md_to_m(&rcg->mn[bank], md);
 276                 f.n = ns_m_to_n(&rcg->mn[bank], ns, f.m);
 277         }
 278
 279         if (banked_p)
 280                 f.pre_div = ns_to_pre_div(&rcg->p[bank], ns) + 1;
 281
 282         f.src = index;
 283         configure_bank(rcg, &f);
 284
 285         return 0;
 286 }
 287
 288 /*
 289  * Calculate m/n:d rate
 290  *
 291  *          parent_rate     m
 292  *   rate = ----------- x  ---
 293  *            pre_div       n
 294  */
 295 static unsigned long
 296 calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 pre_div)
 297 {
 298         if (pre_div)
 299                 rate /= pre_div + 1;
 300
 301         if (mode) {
 302                 u64 tmp = rate;
 303                 tmp *= m;
 304                 do_div(tmp, n);
 305                 rate = tmp;
 306         }
 307
 308         return rate;
 309 }
 310
 311 static unsigned long
 312 clk_rcg_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 313 {
 314         struct clk_rcg *rcg = to_clk_rcg(hw);
 315         u32 pre_div, m = 0, n = 0, ns, md, mode = 0;
 316         struct mn *mn = &rcg->mn;
 317
 318         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
 319         pre_div = ns_to_pre_div(&rcg->p, ns);
 320
 321         if (rcg->mn.width) {
 322                 regmap_read(rcg->clkr.regmap, rcg->md_reg, &md);
 323                 m = md_to_m(mn, md);
 324                 n = ns_m_to_n(mn, ns, m);
 325                 /* MN counter mode is in hw.enable_reg sometimes */
 326                 if (rcg->clkr.enable_reg != rcg->ns_reg)
 327                         regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &mode);
 328                 else
 329                         mode = ns;
 330                 mode = reg_to_mnctr_mode(mn, mode);
 331         }
 332
 333         return calc_rate(parent_rate, m, n, mode, pre_div);
 334 }
 335
 336 static unsigned long
 337 clk_dyn_rcg_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 338 {
 339         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
 340         u32 m, n, pre_div, ns, md, mode, reg;
 341         int bank;
 342         struct mn *mn;
 343         bool banked_p = !!rcg->p[1].pre_div_width;
 344         bool banked_mn = !!rcg->mn[1].width;
 345
 346         regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
 347         bank = reg_to_bank(rcg, reg);
 348
 349         regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
 350         m = n = pre_div = mode = 0;
 351
 352         if (banked_mn) {
 353                 mn = &rcg->mn[bank];
 354                 regmap_read(rcg->clkr.regmap, rcg->md_reg[bank], &md);
 355                 m = md_to_m(mn, md);
 356                 n = ns_m_to_n(mn, ns, m);
 357                 /* Two NS registers means mode control is in NS register */
 358                 if (rcg->ns_reg[0] != rcg->ns_reg[1])
 359                         reg = ns;
 360                 mode = reg_to_mnctr_mode(mn, reg);
 361         }
 362
 363         if (banked_p)
 364                 pre_div = ns_to_pre_div(&rcg->p[bank], ns);
 365
 366         return calc_rate(parent_rate, m, n, mode, pre_div);
 367 }
 368
 369 static long _freq_tbl_determine_rate(struct clk_hw *hw,
 370                 const struct freq_tbl *f, unsigned long rate,
 371                 unsigned long min_rate, unsigned long max_rate,
 372                 unsigned long *p_rate, struct clk_hw **p_hw)
 373 {
 374         unsigned long clk_flags;
 375         struct clk *p;
 376
 377         f = qcom_find_freq(f, rate);
 378         if (!f)
 379                 return -EINVAL;
 380
 381         clk_flags = __clk_get_flags(hw->clk);
 382         p = clk_get_parent_by_index(hw->clk, f->src);
 383         if (clk_flags & CLK_SET_RATE_PARENT) {
 384                 rate = rate * f->pre_div;
 385                 if (f->n) {
 386                         u64 tmp = rate;
 387                         tmp = tmp * f->n;
 388                         do_div(tmp, f->m);
 389                         rate = tmp;
 390                 }
 391         } else {
 392                 rate =  __clk_get_rate(p);
 393         }
 394         *p_hw = __clk_get_hw(p);
 395         *p_rate = rate;
 396
 397         return f->freq;
 398 }
 399
 400 static long clk_rcg_determine_rate(struct clk_hw *hw, unsigned long rate,
 401                 unsigned long min_rate, unsigned long max_rate,
 402                 unsigned long *p_rate, struct clk_hw **p)
 403 {
 404         struct clk_rcg *rcg = to_clk_rcg(hw);
 405
 406         return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, min_rate,
 407                         max_rate, p_rate, p);
 408 }
 409
 410 static long clk_dyn_rcg_determine_rate(struct clk_hw *hw, unsigned long rate,
 411                 unsigned long min_rate, unsigned long max_rate,
 412                 unsigned long *p_rate, struct clk_hw **p)
 413 {
 414         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
 415
 416         return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, min_rate,
 417                         max_rate, p_rate, p);
 418 }
 419
 420 static long clk_rcg_bypass_determine_rate(struct clk_hw *hw, unsigned long rate,
 421                 unsigned long min_rate, unsigned long max_rate,
 422                 unsigned long *p_rate, struct clk_hw **p_hw)
 423 {
 424         struct clk_rcg *rcg = to_clk_rcg(hw);
 425         const struct freq_tbl *f = rcg->freq_tbl;
 426         struct clk *p;
 427
 428         p = clk_get_parent_by_index(hw->clk, f->src);
 429         *p_hw = __clk_get_hw(p);
 430         *p_rate = __clk_round_rate(p, rate);
 431
 432         return *p_rate;
 433 }
 434
 435 static int __clk_rcg_set_rate(struct clk_rcg *rcg, const struct freq_tbl *f)
 436 {
 437         u32 ns, md, ctl;
 438         struct mn *mn = &rcg->mn;
 439         u32 mask = 0;
 440         unsigned int reset_reg;
 441
 442         if (rcg->mn.reset_in_cc)
 443                 reset_reg = rcg->clkr.enable_reg;
 444         else
 445                 reset_reg = rcg->ns_reg;
 446
 447         if (rcg->mn.width) {
 448                 mask = BIT(mn->mnctr_reset_bit);
 449                 regmap_update_bits(rcg->clkr.regmap, reset_reg, mask, mask);
 450
 451                 regmap_read(rcg->clkr.regmap, rcg->md_reg, &md);
 452                 md = mn_to_md(mn, f->m, f->n, md);
 453                 regmap_write(rcg->clkr.regmap, rcg->md_reg, md);
 454
 455                 regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
 456                 /* MN counter mode is in hw.enable_reg sometimes */
 457                 if (rcg->clkr.enable_reg != rcg->ns_reg) {
 458                         regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
 459                         ctl = mn_to_reg(mn, f->m, f->n, ctl);
 460                         regmap_write(rcg->clkr.regmap, rcg->clkr.enable_reg, ctl);
 461                 } else {
 462                         ns = mn_to_reg(mn, f->m, f->n, ns);
 463                 }
 464                 ns = mn_to_ns(mn, f->m, f->n, ns);
 465         } else {
 466                 regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
 467         }
 468
 469         ns = pre_div_to_ns(&rcg->p, f->pre_div - 1, ns);
 470         regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
 471
 472         regmap_update_bits(rcg->clkr.regmap, reset_reg, mask, 0);
 473
 474         return 0;
 475 }
 476
 477 static int clk_rcg_set_rate(struct clk_hw *hw, unsigned long rate,
 478                             unsigned long parent_rate)
 479 {
 480         struct clk_rcg *rcg = to_clk_rcg(hw);
 481         const struct freq_tbl *f;
 482
 483         f = qcom_find_freq(rcg->freq_tbl, rate);
 484         if (!f)
 485                 return -EINVAL;
 486
 487         return __clk_rcg_set_rate(rcg, f);
 488 }
 489
 490 static int clk_rcg_bypass_set_rate(struct clk_hw *hw, unsigned long rate,
 491                                 unsigned long parent_rate)
 492 {
 493         struct clk_rcg *rcg = to_clk_rcg(hw);
 494
 495         return __clk_rcg_set_rate(rcg, rcg->freq_tbl);
 496 }
 497
 498 static int __clk_dyn_rcg_set_rate(struct clk_hw *hw, unsigned long rate)
 499 {
 500         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
 501         const struct freq_tbl *f;
 502
 503         f = qcom_find_freq(rcg->freq_tbl, rate);
 504         if (!f)
 505                 return -EINVAL;
 506
 507         configure_bank(rcg, f);
 508
 509         return 0;
 510 }
 511
 512 static int clk_dyn_rcg_set_rate(struct clk_hw *hw, unsigned long rate,
 513                             unsigned long parent_rate)
 514 {
 515         return __clk_dyn_rcg_set_rate(hw, rate);
 516 }
 517
 518 static int clk_dyn_rcg_set_rate_and_parent(struct clk_hw *hw,
 519                 unsigned long rate, unsigned long parent_rate, u8 index)
 520 {
 521         return __clk_dyn_rcg_set_rate(hw, rate);
 522 }
 523
 524 const struct clk_ops clk_rcg_ops = {
 525         .enable = clk_enable_regmap,
 526         .disable = clk_disable_regmap,
 527         .get_parent = clk_rcg_get_parent,
 528         .set_parent = clk_rcg_set_parent,
 529         .recalc_rate = clk_rcg_recalc_rate,
 530         .determine_rate = clk_rcg_determine_rate,
 531         .set_rate = clk_rcg_set_rate,
 532 };
 533 EXPORT_SYMBOL_GPL(clk_rcg_ops);
 534
 535 const struct clk_ops clk_rcg_bypass_ops = {
 536         .enable = clk_enable_regmap,
 537         .disable = clk_disable_regmap,
 538         .get_parent = clk_rcg_get_parent,
 539         .set_parent = clk_rcg_set_parent,
 540         .recalc_rate = clk_rcg_recalc_rate,
 541         .determine_rate = clk_rcg_bypass_determine_rate,
 542         .set_rate = clk_rcg_bypass_set_rate,
 543 };
 544 EXPORT_SYMBOL_GPL(clk_rcg_bypass_ops);
 545
 546 const struct clk_ops clk_dyn_rcg_ops = {
 547         .enable = clk_enable_regmap,
 548         .is_enabled = clk_is_enabled_regmap,
 549         .disable = clk_disable_regmap,
 550         .get_parent = clk_dyn_rcg_get_parent,
 551         .set_parent = clk_dyn_rcg_set_parent,
 552         .recalc_rate = clk_dyn_rcg_recalc_rate,
 553         .determine_rate = clk_dyn_rcg_determine_rate,
 554         .set_rate = clk_dyn_rcg_set_rate,
 555         .set_rate_and_parent = clk_dyn_rcg_set_rate_and_parent,
 556 };
 557 EXPORT_SYMBOL_GPL(clk_dyn_rcg_ops);