clk-frac.c 4.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209
  1. /*
  2. * mmp factor clock operation source file
  3. *
  4. * Copyright (C) 2012 Marvell
  5. * Chao Xie <xiechao.mail@gmail.com>
  6. *
  7. * This file is licensed under the terms of the GNU General Public
  8. * License version 2. This program is licensed "as is" without any
  9. * warranty of any kind, whether express or implied.
  10. */
  11. #include <linux/clk-provider.h>
  12. #include <linux/slab.h>
  13. #include <linux/io.h>
  14. #include <linux/err.h>
  15. #include "clk.h"
  16. /*
  17. * It is M/N clock
  18. *
  19. * Fout from synthesizer can be given from two equations:
  20. * numerator/denominator = Fin / (Fout * factor)
  21. */
  22. #define to_clk_factor(hw) container_of(hw, struct mmp_clk_factor, hw)
  23. static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate,
  24. unsigned long *prate)
  25. {
  26. struct mmp_clk_factor *factor = to_clk_factor(hw);
  27. u64 rate = 0, prev_rate;
  28. int i;
  29. for (i = 0; i < factor->ftbl_cnt; i++) {
  30. prev_rate = rate;
  31. rate = *prate;
  32. rate *= factor->ftbl[i].den;
  33. do_div(rate, factor->ftbl[i].num * factor->masks->factor);
  34. if (rate > drate)
  35. break;
  36. }
  37. if ((i == 0) || (i == factor->ftbl_cnt)) {
  38. return rate;
  39. } else {
  40. if ((drate - prev_rate) > (rate - drate))
  41. return rate;
  42. else
  43. return prev_rate;
  44. }
  45. }
  46. static unsigned long clk_factor_recalc_rate(struct clk_hw *hw,
  47. unsigned long parent_rate)
  48. {
  49. struct mmp_clk_factor *factor = to_clk_factor(hw);
  50. struct mmp_clk_factor_masks *masks = factor->masks;
  51. unsigned int val, num, den;
  52. u64 rate;
  53. val = readl_relaxed(factor->base);
  54. /* calculate numerator */
  55. num = (val >> masks->num_shift) & masks->num_mask;
  56. /* calculate denominator */
  57. den = (val >> masks->den_shift) & masks->den_mask;
  58. if (!den)
  59. return 0;
  60. rate = parent_rate;
  61. rate *= den;
  62. do_div(rate, num * factor->masks->factor);
  63. return rate;
  64. }
  65. /* Configures new clock rate*/
  66. static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
  67. unsigned long prate)
  68. {
  69. struct mmp_clk_factor *factor = to_clk_factor(hw);
  70. struct mmp_clk_factor_masks *masks = factor->masks;
  71. int i;
  72. unsigned long val;
  73. unsigned long flags = 0;
  74. u64 rate = 0;
  75. for (i = 0; i < factor->ftbl_cnt; i++) {
  76. rate = prate;
  77. rate *= factor->ftbl[i].den;
  78. do_div(rate, factor->ftbl[i].num * factor->masks->factor);
  79. if (rate > drate)
  80. break;
  81. }
  82. if (i > 0)
  83. i--;
  84. if (factor->lock)
  85. spin_lock_irqsave(factor->lock, flags);
  86. val = readl_relaxed(factor->base);
  87. val &= ~(masks->num_mask << masks->num_shift);
  88. val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift;
  89. val &= ~(masks->den_mask << masks->den_shift);
  90. val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift;
  91. writel_relaxed(val, factor->base);
  92. if (factor->lock)
  93. spin_unlock_irqrestore(factor->lock, flags);
  94. return 0;
  95. }
  96. static int clk_factor_init(struct clk_hw *hw)
  97. {
  98. struct mmp_clk_factor *factor = to_clk_factor(hw);
  99. struct mmp_clk_factor_masks *masks = factor->masks;
  100. u32 val, num, den;
  101. int i;
  102. unsigned long flags = 0;
  103. if (factor->lock)
  104. spin_lock_irqsave(factor->lock, flags);
  105. val = readl(factor->base);
  106. /* calculate numerator */
  107. num = (val >> masks->num_shift) & masks->num_mask;
  108. /* calculate denominator */
  109. den = (val >> masks->den_shift) & masks->den_mask;
  110. for (i = 0; i < factor->ftbl_cnt; i++)
  111. if (den == factor->ftbl[i].den && num == factor->ftbl[i].num)
  112. break;
  113. if (i >= factor->ftbl_cnt) {
  114. val &= ~(masks->num_mask << masks->num_shift);
  115. val |= (factor->ftbl[0].num & masks->num_mask) <<
  116. masks->num_shift;
  117. val &= ~(masks->den_mask << masks->den_shift);
  118. val |= (factor->ftbl[0].den & masks->den_mask) <<
  119. masks->den_shift;
  120. }
  121. if (!(val & masks->enable_mask) || i >= factor->ftbl_cnt) {
  122. val |= masks->enable_mask;
  123. writel(val, factor->base);
  124. }
  125. if (factor->lock)
  126. spin_unlock_irqrestore(factor->lock, flags);
  127. return 0;
  128. }
  129. static const struct clk_ops clk_factor_ops = {
  130. .recalc_rate = clk_factor_recalc_rate,
  131. .round_rate = clk_factor_round_rate,
  132. .set_rate = clk_factor_set_rate,
  133. .init = clk_factor_init,
  134. };
  135. struct clk *mmp_clk_register_factor(const char *name, const char *parent_name,
  136. unsigned long flags, void __iomem *base,
  137. struct mmp_clk_factor_masks *masks,
  138. struct mmp_clk_factor_tbl *ftbl,
  139. unsigned int ftbl_cnt, spinlock_t *lock)
  140. {
  141. struct mmp_clk_factor *factor;
  142. struct clk_init_data init;
  143. struct clk *clk;
  144. if (!masks) {
  145. pr_err("%s: must pass a clk_factor_mask\n", __func__);
  146. return ERR_PTR(-EINVAL);
  147. }
  148. factor = kzalloc(sizeof(*factor), GFP_KERNEL);
  149. if (!factor)
  150. return ERR_PTR(-ENOMEM);
  151. /* struct clk_aux assignments */
  152. factor->base = base;
  153. factor->masks = masks;
  154. factor->ftbl = ftbl;
  155. factor->ftbl_cnt = ftbl_cnt;
  156. factor->hw.init = &init;
  157. factor->lock = lock;
  158. init.name = name;
  159. init.ops = &clk_factor_ops;
  160. init.flags = flags;
  161. init.parent_names = &parent_name;
  162. init.num_parents = 1;
  163. clk = clk_register(NULL, &factor->hw);
  164. if (IS_ERR_OR_NULL(clk))
  165. kfree(factor);
  166. return clk;
  167. }