clk-pll.c 3.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * MMP PLL clock rate calculation
  4. *
  5. * Copyright (C) 2020 Lubomir Rintel <lkundrak@v3.sk>
  6. */
  7. #include <linux/clk-provider.h>
  8. #include <linux/slab.h>
  9. #include <linux/io.h>
  10. #include "clk.h"
  11. #define to_clk_mmp_pll(hw) container_of(hw, struct mmp_clk_pll, hw)
  12. struct mmp_clk_pll {
  13. struct clk_hw hw;
  14. unsigned long default_rate;
  15. void __iomem *enable_reg;
  16. u32 enable;
  17. void __iomem *reg;
  18. u8 shift;
  19. unsigned long input_rate;
  20. void __iomem *postdiv_reg;
  21. u8 postdiv_shift;
  22. };
  23. static int mmp_clk_pll_is_enabled(struct clk_hw *hw)
  24. {
  25. struct mmp_clk_pll *pll = to_clk_mmp_pll(hw);
  26. u32 val;
  27. val = readl_relaxed(pll->enable_reg);
  28. if ((val & pll->enable) == pll->enable)
  29. return 1;
  30. /* Some PLLs, if not software controlled, output default clock. */
  31. if (pll->default_rate > 0)
  32. return 1;
  33. return 0;
  34. }
  35. static unsigned long mmp_clk_pll_recalc_rate(struct clk_hw *hw,
  36. unsigned long parent_rate)
  37. {
  38. struct mmp_clk_pll *pll = to_clk_mmp_pll(hw);
  39. u32 fbdiv, refdiv, postdiv;
  40. u64 rate;
  41. u32 val;
  42. val = readl_relaxed(pll->enable_reg);
  43. if ((val & pll->enable) != pll->enable)
  44. return pll->default_rate;
  45. if (pll->reg) {
  46. val = readl_relaxed(pll->reg);
  47. fbdiv = (val >> pll->shift) & 0x1ff;
  48. refdiv = (val >> (pll->shift + 9)) & 0x1f;
  49. } else {
  50. fbdiv = 2;
  51. refdiv = 1;
  52. }
  53. if (pll->postdiv_reg) {
  54. /* MMP3 clock rate calculation */
  55. static const u8 postdivs[] = {2, 3, 4, 5, 6, 8, 10, 12, 16};
  56. val = readl_relaxed(pll->postdiv_reg);
  57. postdiv = (val >> pll->postdiv_shift) & 0x7;
  58. rate = pll->input_rate;
  59. rate *= 2 * fbdiv;
  60. do_div(rate, refdiv);
  61. do_div(rate, postdivs[postdiv]);
  62. } else {
  63. /* MMP2 clock rate calculation */
  64. if (refdiv == 3) {
  65. rate = 19200000;
  66. } else if (refdiv == 4) {
  67. rate = 26000000;
  68. } else {
  69. pr_err("bad refdiv: %d (0x%08x)\n", refdiv, val);
  70. return 0;
  71. }
  72. rate *= fbdiv + 2;
  73. do_div(rate, refdiv + 2);
  74. }
  75. return (unsigned long)rate;
  76. }
  77. static const struct clk_ops mmp_clk_pll_ops = {
  78. .is_enabled = mmp_clk_pll_is_enabled,
  79. .recalc_rate = mmp_clk_pll_recalc_rate,
  80. };
  81. static struct clk *mmp_clk_register_pll(char *name,
  82. unsigned long default_rate,
  83. void __iomem *enable_reg, u32 enable,
  84. void __iomem *reg, u8 shift,
  85. unsigned long input_rate,
  86. void __iomem *postdiv_reg, u8 postdiv_shift)
  87. {
  88. struct mmp_clk_pll *pll;
  89. struct clk *clk;
  90. struct clk_init_data init;
  91. pll = kzalloc(sizeof(*pll), GFP_KERNEL);
  92. if (!pll)
  93. return ERR_PTR(-ENOMEM);
  94. init.name = name;
  95. init.ops = &mmp_clk_pll_ops;
  96. init.flags = 0;
  97. init.parent_names = NULL;
  98. init.num_parents = 0;
  99. pll->default_rate = default_rate;
  100. pll->enable_reg = enable_reg;
  101. pll->enable = enable;
  102. pll->reg = reg;
  103. pll->shift = shift;
  104. pll->input_rate = input_rate;
  105. pll->postdiv_reg = postdiv_reg;
  106. pll->postdiv_shift = postdiv_shift;
  107. pll->hw.init = &init;
  108. clk = clk_register(NULL, &pll->hw);
  109. if (IS_ERR(clk))
  110. kfree(pll);
  111. return clk;
  112. }
  113. void mmp_register_pll_clks(struct mmp_clk_unit *unit,
  114. struct mmp_param_pll_clk *clks,
  115. void __iomem *base, int size)
  116. {
  117. struct clk *clk;
  118. int i;
  119. for (i = 0; i < size; i++) {
  120. void __iomem *reg = NULL;
  121. if (clks[i].offset)
  122. reg = base + clks[i].offset;
  123. clk = mmp_clk_register_pll(clks[i].name,
  124. clks[i].default_rate,
  125. base + clks[i].enable_offset,
  126. clks[i].enable,
  127. reg, clks[i].shift,
  128. clks[i].input_rate,
  129. base + clks[i].postdiv_offset,
  130. clks[i].postdiv_shift);
  131. if (IS_ERR(clk)) {
  132. pr_err("%s: failed to register clock %s\n",
  133. __func__, clks[i].name);
  134. continue;
  135. }
  136. if (clks[i].id)
  137. unit->clk_table[clks[i].id] = clk;
  138. }
  139. }