clk-sparx5.c 6.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Microchip Sparx5 SoC Clock driver.
  4. *
  5. * Copyright (c) 2019 Microchip Inc.
  6. *
  7. * Author: Lars Povlsen <lars.povlsen@microchip.com>
  8. */
  9. #include <linux/io.h>
  10. #include <linux/module.h>
  11. #include <linux/clk-provider.h>
  12. #include <linux/bitfield.h>
  13. #include <linux/of.h>
  14. #include <linux/slab.h>
  15. #include <linux/platform_device.h>
  16. #include <dt-bindings/clock/microchip,sparx5.h>
  17. #define PLL_DIV GENMASK(7, 0)
  18. #define PLL_PRE_DIV GENMASK(10, 8)
  19. #define PLL_ROT_DIR BIT(11)
  20. #define PLL_ROT_SEL GENMASK(13, 12)
  21. #define PLL_ROT_ENA BIT(14)
  22. #define PLL_CLK_ENA BIT(15)
  23. #define MAX_SEL 4
  24. #define MAX_PRE BIT(3)
  25. static const u8 sel_rates[MAX_SEL] = { 0, 2*8, 2*4, 2*2 };
  26. static const char *clk_names[N_CLOCKS] = {
  27. "core", "ddr", "cpu2", "arm2",
  28. "aux1", "aux2", "aux3", "aux4",
  29. "synce",
  30. };
  31. struct s5_hw_clk {
  32. struct clk_hw hw;
  33. void __iomem *reg;
  34. };
  35. struct s5_clk_data {
  36. void __iomem *base;
  37. struct s5_hw_clk s5_hw[N_CLOCKS];
  38. };
  39. struct s5_pll_conf {
  40. unsigned long freq;
  41. u8 div;
  42. bool rot_ena;
  43. u8 rot_sel;
  44. u8 rot_dir;
  45. u8 pre_div;
  46. };
  47. #define to_s5_pll(hw) container_of(hw, struct s5_hw_clk, hw)
  48. static unsigned long s5_calc_freq(unsigned long parent_rate,
  49. const struct s5_pll_conf *conf)
  50. {
  51. unsigned long rate = parent_rate / conf->div;
  52. if (conf->rot_ena) {
  53. int sign = conf->rot_dir ? -1 : 1;
  54. int divt = sel_rates[conf->rot_sel] * (1 + conf->pre_div);
  55. int divb = divt + sign;
  56. rate = mult_frac(rate, divt, divb);
  57. rate = roundup(rate, 1000);
  58. }
  59. return rate;
  60. }
  61. static void s5_search_fractional(unsigned long rate,
  62. unsigned long parent_rate,
  63. int div,
  64. struct s5_pll_conf *conf)
  65. {
  66. struct s5_pll_conf best;
  67. ulong cur_offset, best_offset = rate;
  68. int d, i, j;
  69. memset(conf, 0, sizeof(*conf));
  70. conf->div = div;
  71. conf->rot_ena = 1; /* Fractional rate */
  72. for (d = 0; best_offset > 0 && d <= 1 ; d++) {
  73. conf->rot_dir = !!d;
  74. for (i = 0; best_offset > 0 && i < MAX_PRE; i++) {
  75. conf->pre_div = i;
  76. for (j = 1; best_offset > 0 && j < MAX_SEL; j++) {
  77. conf->rot_sel = j;
  78. conf->freq = s5_calc_freq(parent_rate, conf);
  79. cur_offset = abs(rate - conf->freq);
  80. if (cur_offset < best_offset) {
  81. best_offset = cur_offset;
  82. best = *conf;
  83. }
  84. }
  85. }
  86. }
  87. /* Best match */
  88. *conf = best;
  89. }
  90. static unsigned long s5_calc_params(unsigned long rate,
  91. unsigned long parent_rate,
  92. struct s5_pll_conf *conf)
  93. {
  94. if (parent_rate % rate) {
  95. struct s5_pll_conf alt1, alt2;
  96. int div;
  97. div = DIV_ROUND_CLOSEST_ULL(parent_rate, rate);
  98. s5_search_fractional(rate, parent_rate, div, &alt1);
  99. /* Straight match? */
  100. if (alt1.freq == rate) {
  101. *conf = alt1;
  102. } else {
  103. /* Try without rounding divider */
  104. div = parent_rate / rate;
  105. if (div != alt1.div) {
  106. s5_search_fractional(rate, parent_rate, div,
  107. &alt2);
  108. /* Select the better match */
  109. if (abs(rate - alt1.freq) <
  110. abs(rate - alt2.freq))
  111. *conf = alt1;
  112. else
  113. *conf = alt2;
  114. }
  115. }
  116. } else {
  117. /* Straight fit */
  118. memset(conf, 0, sizeof(*conf));
  119. conf->div = parent_rate / rate;
  120. }
  121. return conf->freq;
  122. }
  123. static int s5_pll_enable(struct clk_hw *hw)
  124. {
  125. struct s5_hw_clk *pll = to_s5_pll(hw);
  126. u32 val = readl(pll->reg);
  127. val |= PLL_CLK_ENA;
  128. writel(val, pll->reg);
  129. return 0;
  130. }
  131. static void s5_pll_disable(struct clk_hw *hw)
  132. {
  133. struct s5_hw_clk *pll = to_s5_pll(hw);
  134. u32 val = readl(pll->reg);
  135. val &= ~PLL_CLK_ENA;
  136. writel(val, pll->reg);
  137. }
  138. static int s5_pll_set_rate(struct clk_hw *hw,
  139. unsigned long rate,
  140. unsigned long parent_rate)
  141. {
  142. struct s5_hw_clk *pll = to_s5_pll(hw);
  143. struct s5_pll_conf conf;
  144. unsigned long eff_rate;
  145. u32 val;
  146. eff_rate = s5_calc_params(rate, parent_rate, &conf);
  147. if (eff_rate != rate)
  148. return -EOPNOTSUPP;
  149. val = readl(pll->reg) & PLL_CLK_ENA;
  150. val |= FIELD_PREP(PLL_DIV, conf.div);
  151. if (conf.rot_ena) {
  152. val |= PLL_ROT_ENA;
  153. val |= FIELD_PREP(PLL_ROT_SEL, conf.rot_sel);
  154. val |= FIELD_PREP(PLL_PRE_DIV, conf.pre_div);
  155. if (conf.rot_dir)
  156. val |= PLL_ROT_DIR;
  157. }
  158. writel(val, pll->reg);
  159. return 0;
  160. }
  161. static unsigned long s5_pll_recalc_rate(struct clk_hw *hw,
  162. unsigned long parent_rate)
  163. {
  164. struct s5_hw_clk *pll = to_s5_pll(hw);
  165. struct s5_pll_conf conf;
  166. u32 val;
  167. val = readl(pll->reg);
  168. if (val & PLL_CLK_ENA) {
  169. conf.div = FIELD_GET(PLL_DIV, val);
  170. conf.pre_div = FIELD_GET(PLL_PRE_DIV, val);
  171. conf.rot_ena = FIELD_GET(PLL_ROT_ENA, val);
  172. conf.rot_dir = FIELD_GET(PLL_ROT_DIR, val);
  173. conf.rot_sel = FIELD_GET(PLL_ROT_SEL, val);
  174. conf.freq = s5_calc_freq(parent_rate, &conf);
  175. } else {
  176. conf.freq = 0;
  177. }
  178. return conf.freq;
  179. }
  180. static long s5_pll_round_rate(struct clk_hw *hw, unsigned long rate,
  181. unsigned long *parent_rate)
  182. {
  183. struct s5_pll_conf conf;
  184. return s5_calc_params(rate, *parent_rate, &conf);
  185. }
  186. static const struct clk_ops s5_pll_ops = {
  187. .enable = s5_pll_enable,
  188. .disable = s5_pll_disable,
  189. .set_rate = s5_pll_set_rate,
  190. .round_rate = s5_pll_round_rate,
  191. .recalc_rate = s5_pll_recalc_rate,
  192. };
  193. static struct clk_hw *s5_clk_hw_get(struct of_phandle_args *clkspec, void *data)
  194. {
  195. struct s5_clk_data *s5_clk = data;
  196. unsigned int idx = clkspec->args[0];
  197. if (idx >= N_CLOCKS) {
  198. pr_err("%s: invalid index %u\n", __func__, idx);
  199. return ERR_PTR(-EINVAL);
  200. }
  201. return &s5_clk->s5_hw[idx].hw;
  202. }
  203. static int s5_clk_probe(struct platform_device *pdev)
  204. {
  205. struct device *dev = &pdev->dev;
  206. int i, ret;
  207. struct s5_clk_data *s5_clk;
  208. struct clk_parent_data pdata = { .index = 0 };
  209. struct clk_init_data init = {
  210. .ops = &s5_pll_ops,
  211. .num_parents = 1,
  212. .parent_data = &pdata,
  213. };
  214. s5_clk = devm_kzalloc(dev, sizeof(*s5_clk), GFP_KERNEL);
  215. if (!s5_clk)
  216. return -ENOMEM;
  217. s5_clk->base = devm_platform_ioremap_resource(pdev, 0);
  218. if (IS_ERR(s5_clk->base))
  219. return PTR_ERR(s5_clk->base);
  220. for (i = 0; i < N_CLOCKS; i++) {
  221. struct s5_hw_clk *s5_hw = &s5_clk->s5_hw[i];
  222. init.name = clk_names[i];
  223. s5_hw->reg = s5_clk->base + (i * 4);
  224. s5_hw->hw.init = &init;
  225. ret = devm_clk_hw_register(dev, &s5_hw->hw);
  226. if (ret) {
  227. dev_err(dev, "failed to register %s clock\n",
  228. init.name);
  229. return ret;
  230. }
  231. }
  232. return devm_of_clk_add_hw_provider(dev, s5_clk_hw_get, s5_clk);
  233. }
  234. static const struct of_device_id s5_clk_dt_ids[] = {
  235. { .compatible = "microchip,sparx5-dpll", },
  236. { }
  237. };
  238. MODULE_DEVICE_TABLE(of, s5_clk_dt_ids);
  239. static struct platform_driver s5_clk_driver = {
  240. .probe = s5_clk_probe,
  241. .driver = {
  242. .name = "sparx5-clk",
  243. .of_match_table = s5_clk_dt_ids,
  244. },
  245. };
  246. builtin_platform_driver(s5_clk_driver);