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