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, ®); 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); 518