sun4i_hdmi_ddc_clk.c 3.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Copyright (C) 2016 Free Electrons
  4. * Copyright (C) 2016 NextThing Co
  5. *
  6. * Maxime Ripard <maxime.ripard@free-electrons.com>
  7. */
  8. #include <linux/clk-provider.h>
  9. #include <linux/regmap.h>
  10. #include "sun4i_hdmi.h"
  11. struct sun4i_ddc {
  12. struct clk_hw hw;
  13. struct sun4i_hdmi *hdmi;
  14. struct regmap_field *reg;
  15. u8 pre_div;
  16. u8 m_offset;
  17. };
  18. static inline struct sun4i_ddc *hw_to_ddc(struct clk_hw *hw)
  19. {
  20. return container_of(hw, struct sun4i_ddc, hw);
  21. }
  22. static unsigned long sun4i_ddc_calc_divider(unsigned long rate,
  23. unsigned long parent_rate,
  24. const u8 pre_div,
  25. const u8 m_offset,
  26. u8 *m, u8 *n)
  27. {
  28. unsigned long best_rate = 0;
  29. u8 best_m = 0, best_n = 0, _m, _n;
  30. for (_m = 0; _m < 16; _m++) {
  31. for (_n = 0; _n < 8; _n++) {
  32. unsigned long tmp_rate;
  33. tmp_rate = (((parent_rate / pre_div) / 10) >> _n) /
  34. (_m + m_offset);
  35. if (tmp_rate > rate)
  36. continue;
  37. if (abs(rate - tmp_rate) < abs(rate - best_rate)) {
  38. best_rate = tmp_rate;
  39. best_m = _m;
  40. best_n = _n;
  41. }
  42. }
  43. }
  44. if (m && n) {
  45. *m = best_m;
  46. *n = best_n;
  47. }
  48. return best_rate;
  49. }
  50. static long sun4i_ddc_round_rate(struct clk_hw *hw, unsigned long rate,
  51. unsigned long *prate)
  52. {
  53. struct sun4i_ddc *ddc = hw_to_ddc(hw);
  54. return sun4i_ddc_calc_divider(rate, *prate, ddc->pre_div,
  55. ddc->m_offset, NULL, NULL);
  56. }
  57. static unsigned long sun4i_ddc_recalc_rate(struct clk_hw *hw,
  58. unsigned long parent_rate)
  59. {
  60. struct sun4i_ddc *ddc = hw_to_ddc(hw);
  61. unsigned int reg;
  62. u8 m, n;
  63. regmap_field_read(ddc->reg, &reg);
  64. m = (reg >> 3) & 0xf;
  65. n = reg & 0x7;
  66. return (((parent_rate / ddc->pre_div) / 10) >> n) /
  67. (m + ddc->m_offset);
  68. }
  69. static int sun4i_ddc_set_rate(struct clk_hw *hw, unsigned long rate,
  70. unsigned long parent_rate)
  71. {
  72. struct sun4i_ddc *ddc = hw_to_ddc(hw);
  73. u8 div_m, div_n;
  74. sun4i_ddc_calc_divider(rate, parent_rate, ddc->pre_div,
  75. ddc->m_offset, &div_m, &div_n);
  76. regmap_field_write(ddc->reg,
  77. SUN4I_HDMI_DDC_CLK_M(div_m) |
  78. SUN4I_HDMI_DDC_CLK_N(div_n));
  79. return 0;
  80. }
  81. static const struct clk_ops sun4i_ddc_ops = {
  82. .recalc_rate = sun4i_ddc_recalc_rate,
  83. .round_rate = sun4i_ddc_round_rate,
  84. .set_rate = sun4i_ddc_set_rate,
  85. };
  86. int sun4i_ddc_create(struct sun4i_hdmi *hdmi, struct clk *parent)
  87. {
  88. struct clk_init_data init;
  89. struct sun4i_ddc *ddc;
  90. const char *parent_name;
  91. parent_name = __clk_get_name(parent);
  92. if (!parent_name)
  93. return -ENODEV;
  94. ddc = devm_kzalloc(hdmi->dev, sizeof(*ddc), GFP_KERNEL);
  95. if (!ddc)
  96. return -ENOMEM;
  97. ddc->reg = devm_regmap_field_alloc(hdmi->dev, hdmi->regmap,
  98. hdmi->variant->ddc_clk_reg);
  99. if (IS_ERR(ddc->reg))
  100. return PTR_ERR(ddc->reg);
  101. init.name = "hdmi-ddc";
  102. init.ops = &sun4i_ddc_ops;
  103. init.parent_names = &parent_name;
  104. init.num_parents = 1;
  105. ddc->hdmi = hdmi;
  106. ddc->hw.init = &init;
  107. ddc->pre_div = hdmi->variant->ddc_clk_pre_divider;
  108. ddc->m_offset = hdmi->variant->ddc_clk_m_offset;
  109. hdmi->ddc_clk = devm_clk_register(hdmi->dev, &ddc->hw);
  110. if (IS_ERR(hdmi->ddc_clk))
  111. return PTR_ERR(hdmi->ddc_clk);
  112. return 0;
  113. }