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