clk-fractional-divider.c 5.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copyright (C) 2014 Intel Corporation
  4. *
  5. * Adjustable fractional divider clock implementation.
  6. * Output rate = (m / n) * parent_rate.
  7. * Uses rational best approximation algorithm.
  8. */
  9. #include <linux/clk-provider.h>
  10. #include <linux/io.h>
  11. #include <linux/module.h>
  12. #include <linux/device.h>
  13. #include <linux/slab.h>
  14. #include <linux/rational.h>
  15. static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
  16. {
  17. if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
  18. return ioread32be(fd->reg);
  19. return readl(fd->reg);
  20. }
  21. static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
  22. {
  23. if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
  24. iowrite32be(val, fd->reg);
  25. else
  26. writel(val, fd->reg);
  27. }
  28. static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
  29. unsigned long parent_rate)
  30. {
  31. struct clk_fractional_divider *fd = to_clk_fd(hw);
  32. unsigned long flags = 0;
  33. unsigned long m, n;
  34. u32 val;
  35. u64 ret;
  36. if (fd->lock)
  37. spin_lock_irqsave(fd->lock, flags);
  38. else
  39. __acquire(fd->lock);
  40. val = clk_fd_readl(fd);
  41. if (fd->lock)
  42. spin_unlock_irqrestore(fd->lock, flags);
  43. else
  44. __release(fd->lock);
  45. m = (val & fd->mmask) >> fd->mshift;
  46. n = (val & fd->nmask) >> fd->nshift;
  47. if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
  48. m++;
  49. n++;
  50. }
  51. if (!n || !m)
  52. return parent_rate;
  53. ret = (u64)parent_rate * m;
  54. do_div(ret, n);
  55. return ret;
  56. }
  57. static void clk_fd_general_approximation(struct clk_hw *hw, unsigned long rate,
  58. unsigned long *parent_rate,
  59. unsigned long *m, unsigned long *n)
  60. {
  61. struct clk_fractional_divider *fd = to_clk_fd(hw);
  62. unsigned long scale;
  63. /*
  64. * Get rate closer to *parent_rate to guarantee there is no overflow
  65. * for m and n. In the result it will be the nearest rate left shifted
  66. * by (scale - fd->nwidth) bits.
  67. */
  68. scale = fls_long(*parent_rate / rate - 1);
  69. if (scale > fd->nwidth)
  70. rate <<= scale - fd->nwidth;
  71. rational_best_approximation(rate, *parent_rate,
  72. GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
  73. m, n);
  74. }
  75. static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
  76. unsigned long *parent_rate)
  77. {
  78. struct clk_fractional_divider *fd = to_clk_fd(hw);
  79. unsigned long m, n;
  80. u64 ret;
  81. if (!rate || (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
  82. return *parent_rate;
  83. if (fd->approximation)
  84. fd->approximation(hw, rate, parent_rate, &m, &n);
  85. else
  86. clk_fd_general_approximation(hw, rate, parent_rate, &m, &n);
  87. ret = (u64)*parent_rate * m;
  88. do_div(ret, n);
  89. return ret;
  90. }
  91. static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
  92. unsigned long parent_rate)
  93. {
  94. struct clk_fractional_divider *fd = to_clk_fd(hw);
  95. unsigned long flags = 0;
  96. unsigned long m, n;
  97. u32 val;
  98. rational_best_approximation(rate, parent_rate,
  99. GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
  100. &m, &n);
  101. if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
  102. m--;
  103. n--;
  104. }
  105. if (fd->lock)
  106. spin_lock_irqsave(fd->lock, flags);
  107. else
  108. __acquire(fd->lock);
  109. val = clk_fd_readl(fd);
  110. val &= ~(fd->mmask | fd->nmask);
  111. val |= (m << fd->mshift) | (n << fd->nshift);
  112. clk_fd_writel(fd, val);
  113. if (fd->lock)
  114. spin_unlock_irqrestore(fd->lock, flags);
  115. else
  116. __release(fd->lock);
  117. return 0;
  118. }
  119. const struct clk_ops clk_fractional_divider_ops = {
  120. .recalc_rate = clk_fd_recalc_rate,
  121. .round_rate = clk_fd_round_rate,
  122. .set_rate = clk_fd_set_rate,
  123. };
  124. EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
  125. struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
  126. const char *name, const char *parent_name, unsigned long flags,
  127. void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
  128. u8 clk_divider_flags, spinlock_t *lock)
  129. {
  130. struct clk_fractional_divider *fd;
  131. struct clk_init_data init;
  132. struct clk_hw *hw;
  133. int ret;
  134. fd = kzalloc(sizeof(*fd), GFP_KERNEL);
  135. if (!fd)
  136. return ERR_PTR(-ENOMEM);
  137. init.name = name;
  138. init.ops = &clk_fractional_divider_ops;
  139. init.flags = flags;
  140. init.parent_names = parent_name ? &parent_name : NULL;
  141. init.num_parents = parent_name ? 1 : 0;
  142. fd->reg = reg;
  143. fd->mshift = mshift;
  144. fd->mwidth = mwidth;
  145. fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
  146. fd->nshift = nshift;
  147. fd->nwidth = nwidth;
  148. fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
  149. fd->flags = clk_divider_flags;
  150. fd->lock = lock;
  151. fd->hw.init = &init;
  152. hw = &fd->hw;
  153. ret = clk_hw_register(dev, hw);
  154. if (ret) {
  155. kfree(fd);
  156. hw = ERR_PTR(ret);
  157. }
  158. return hw;
  159. }
  160. EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
  161. struct clk *clk_register_fractional_divider(struct device *dev,
  162. const char *name, const char *parent_name, unsigned long flags,
  163. void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
  164. u8 clk_divider_flags, spinlock_t *lock)
  165. {
  166. struct clk_hw *hw;
  167. hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
  168. reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
  169. lock);
  170. if (IS_ERR(hw))
  171. return ERR_CAST(hw);
  172. return hw->clk;
  173. }
  174. EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
  175. void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
  176. {
  177. struct clk_fractional_divider *fd;
  178. fd = to_clk_fd(hw);
  179. clk_hw_unregister(hw);
  180. kfree(fd);
  181. }