ccu_nk.c 3.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Copyright (C) 2016 Maxime Ripard
  4. * Maxime Ripard <[email protected]>
  5. */
  6. #include <linux/clk-provider.h>
  7. #include <linux/io.h>
  8. #include "ccu_gate.h"
  9. #include "ccu_nk.h"
  10. struct _ccu_nk {
  11. unsigned long n, min_n, max_n;
  12. unsigned long k, min_k, max_k;
  13. };
  14. static void ccu_nk_find_best(unsigned long parent, unsigned long rate,
  15. struct _ccu_nk *nk)
  16. {
  17. unsigned long best_rate = 0;
  18. unsigned int best_k = 0, best_n = 0;
  19. unsigned int _k, _n;
  20. for (_k = nk->min_k; _k <= nk->max_k; _k++) {
  21. for (_n = nk->min_n; _n <= nk->max_n; _n++) {
  22. unsigned long tmp_rate = parent * _n * _k;
  23. if (tmp_rate > rate)
  24. continue;
  25. if ((rate - tmp_rate) < (rate - best_rate)) {
  26. best_rate = tmp_rate;
  27. best_k = _k;
  28. best_n = _n;
  29. }
  30. }
  31. }
  32. nk->k = best_k;
  33. nk->n = best_n;
  34. }
  35. static void ccu_nk_disable(struct clk_hw *hw)
  36. {
  37. struct ccu_nk *nk = hw_to_ccu_nk(hw);
  38. return ccu_gate_helper_disable(&nk->common, nk->enable);
  39. }
  40. static int ccu_nk_enable(struct clk_hw *hw)
  41. {
  42. struct ccu_nk *nk = hw_to_ccu_nk(hw);
  43. return ccu_gate_helper_enable(&nk->common, nk->enable);
  44. }
  45. static int ccu_nk_is_enabled(struct clk_hw *hw)
  46. {
  47. struct ccu_nk *nk = hw_to_ccu_nk(hw);
  48. return ccu_gate_helper_is_enabled(&nk->common, nk->enable);
  49. }
  50. static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw,
  51. unsigned long parent_rate)
  52. {
  53. struct ccu_nk *nk = hw_to_ccu_nk(hw);
  54. unsigned long rate, n, k;
  55. u32 reg;
  56. reg = readl(nk->common.base + nk->common.reg);
  57. n = reg >> nk->n.shift;
  58. n &= (1 << nk->n.width) - 1;
  59. n += nk->n.offset;
  60. if (!n)
  61. n++;
  62. k = reg >> nk->k.shift;
  63. k &= (1 << nk->k.width) - 1;
  64. k += nk->k.offset;
  65. if (!k)
  66. k++;
  67. rate = parent_rate * n * k;
  68. if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  69. rate /= nk->fixed_post_div;
  70. return rate;
  71. }
  72. static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
  73. unsigned long *parent_rate)
  74. {
  75. struct ccu_nk *nk = hw_to_ccu_nk(hw);
  76. struct _ccu_nk _nk;
  77. if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  78. rate *= nk->fixed_post_div;
  79. _nk.min_n = nk->n.min ?: 1;
  80. _nk.max_n = nk->n.max ?: 1 << nk->n.width;
  81. _nk.min_k = nk->k.min ?: 1;
  82. _nk.max_k = nk->k.max ?: 1 << nk->k.width;
  83. ccu_nk_find_best(*parent_rate, rate, &_nk);
  84. rate = *parent_rate * _nk.n * _nk.k;
  85. if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  86. rate = rate / nk->fixed_post_div;
  87. return rate;
  88. }
  89. static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
  90. unsigned long parent_rate)
  91. {
  92. struct ccu_nk *nk = hw_to_ccu_nk(hw);
  93. unsigned long flags;
  94. struct _ccu_nk _nk;
  95. u32 reg;
  96. if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  97. rate = rate * nk->fixed_post_div;
  98. _nk.min_n = nk->n.min ?: 1;
  99. _nk.max_n = nk->n.max ?: 1 << nk->n.width;
  100. _nk.min_k = nk->k.min ?: 1;
  101. _nk.max_k = nk->k.max ?: 1 << nk->k.width;
  102. ccu_nk_find_best(parent_rate, rate, &_nk);
  103. spin_lock_irqsave(nk->common.lock, flags);
  104. reg = readl(nk->common.base + nk->common.reg);
  105. reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
  106. reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);
  107. reg |= (_nk.k - nk->k.offset) << nk->k.shift;
  108. reg |= (_nk.n - nk->n.offset) << nk->n.shift;
  109. writel(reg, nk->common.base + nk->common.reg);
  110. spin_unlock_irqrestore(nk->common.lock, flags);
  111. ccu_helper_wait_for_lock(&nk->common, nk->lock);
  112. return 0;
  113. }
  114. const struct clk_ops ccu_nk_ops = {
  115. .disable = ccu_nk_disable,
  116. .enable = ccu_nk_enable,
  117. .is_enabled = ccu_nk_is_enabled,
  118. .recalc_rate = ccu_nk_recalc_rate,
  119. .round_rate = ccu_nk_round_rate,
  120. .set_rate = ccu_nk_set_rate,
  121. };
  122. EXPORT_SYMBOL_NS_GPL(ccu_nk_ops, SUNXI_CCU);