clk-multiplier.c 3.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copyright (C) 2015 Maxime Ripard <[email protected]>
  4. */
  5. #include <linux/bitops.h>
  6. #include <linux/clk-provider.h>
  7. #include <linux/err.h>
  8. #include <linux/export.h>
  9. #include <linux/io.h>
  10. #include <linux/kernel.h>
  11. #include <linux/of.h>
  12. #include <linux/slab.h>
  13. static inline u32 clk_mult_readl(struct clk_multiplier *mult)
  14. {
  15. if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
  16. return ioread32be(mult->reg);
  17. return readl(mult->reg);
  18. }
  19. static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)
  20. {
  21. if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
  22. iowrite32be(val, mult->reg);
  23. else
  24. writel(val, mult->reg);
  25. }
  26. static unsigned long __get_mult(struct clk_multiplier *mult,
  27. unsigned long rate,
  28. unsigned long parent_rate)
  29. {
  30. if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
  31. return DIV_ROUND_CLOSEST(rate, parent_rate);
  32. return rate / parent_rate;
  33. }
  34. static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
  35. unsigned long parent_rate)
  36. {
  37. struct clk_multiplier *mult = to_clk_multiplier(hw);
  38. unsigned long val;
  39. val = clk_mult_readl(mult) >> mult->shift;
  40. val &= GENMASK(mult->width - 1, 0);
  41. if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
  42. val = 1;
  43. return parent_rate * val;
  44. }
  45. static bool __is_best_rate(unsigned long rate, unsigned long new,
  46. unsigned long best, unsigned long flags)
  47. {
  48. if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
  49. return abs(rate - new) < abs(rate - best);
  50. return new >= rate && new < best;
  51. }
  52. static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
  53. unsigned long *best_parent_rate,
  54. u8 width, unsigned long flags)
  55. {
  56. struct clk_multiplier *mult = to_clk_multiplier(hw);
  57. unsigned long orig_parent_rate = *best_parent_rate;
  58. unsigned long parent_rate, current_rate, best_rate = ~0;
  59. unsigned int i, bestmult = 0;
  60. unsigned int maxmult = (1 << width) - 1;
  61. if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
  62. bestmult = rate / orig_parent_rate;
  63. /* Make sure we don't end up with a 0 multiplier */
  64. if ((bestmult == 0) &&
  65. !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
  66. bestmult = 1;
  67. /* Make sure we don't overflow the multiplier */
  68. if (bestmult > maxmult)
  69. bestmult = maxmult;
  70. return bestmult;
  71. }
  72. for (i = 1; i < maxmult; i++) {
  73. if (rate == orig_parent_rate * i) {
  74. /*
  75. * This is the best case for us if we have a
  76. * perfect match without changing the parent
  77. * rate.
  78. */
  79. *best_parent_rate = orig_parent_rate;
  80. return i;
  81. }
  82. parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
  83. rate / i);
  84. current_rate = parent_rate * i;
  85. if (__is_best_rate(rate, current_rate, best_rate, flags)) {
  86. bestmult = i;
  87. best_rate = current_rate;
  88. *best_parent_rate = parent_rate;
  89. }
  90. }
  91. return bestmult;
  92. }
  93. static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
  94. unsigned long *parent_rate)
  95. {
  96. struct clk_multiplier *mult = to_clk_multiplier(hw);
  97. unsigned long factor = __bestmult(hw, rate, parent_rate,
  98. mult->width, mult->flags);
  99. return *parent_rate * factor;
  100. }
  101. static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
  102. unsigned long parent_rate)
  103. {
  104. struct clk_multiplier *mult = to_clk_multiplier(hw);
  105. unsigned long factor = __get_mult(mult, rate, parent_rate);
  106. unsigned long flags = 0;
  107. unsigned long val;
  108. if (mult->lock)
  109. spin_lock_irqsave(mult->lock, flags);
  110. else
  111. __acquire(mult->lock);
  112. val = clk_mult_readl(mult);
  113. val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
  114. val |= factor << mult->shift;
  115. clk_mult_writel(mult, val);
  116. if (mult->lock)
  117. spin_unlock_irqrestore(mult->lock, flags);
  118. else
  119. __release(mult->lock);
  120. return 0;
  121. }
  122. const struct clk_ops clk_multiplier_ops = {
  123. .recalc_rate = clk_multiplier_recalc_rate,
  124. .round_rate = clk_multiplier_round_rate,
  125. .set_rate = clk_multiplier_set_rate,
  126. };
  127. EXPORT_SYMBOL_GPL(clk_multiplier_ops);