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