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 | |
9 | #include <linux/clk-provider.h> |
10 | #include <linux/regmap.h> |
11 | |
12 | #include "sun4i_hdmi.h" |
13 | |
14 | struct sun4i_ddc { |
15 | struct clk_hw hw; |
16 | struct sun4i_hdmi *hdmi; |
17 | struct regmap_field *reg; |
18 | u8 pre_div; |
19 | u8 m_offset; |
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, |
29 | const u8 pre_div, |
30 | const u8 m_offset, |
31 | u8 *m, u8 *n) |
32 | { |
33 | unsigned long best_rate = 0; |
34 | u8 best_m = 0, best_n = 0, _m, _n; |
35 | |
36 | for (_m = 0; _m < 16; _m++) { |
37 | for (_n = 0; _n < 8; _n++) { |
38 | unsigned long tmp_rate; |
39 | |
40 | tmp_rate = (((parent_rate / pre_div) / 10) >> _n) / |
41 | (_m + m_offset); |
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 | { |
65 | struct sun4i_ddc *ddc = hw_to_ddc(hw); |
66 | |
67 | return sun4i_ddc_calc_divider(rate, parent_rate: *prate, pre_div: ddc->pre_div, |
68 | m_offset: ddc->m_offset, NULL, NULL); |
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); |
75 | unsigned int reg; |
76 | u8 m, n; |
77 | |
78 | regmap_field_read(field: ddc->reg, val: ®); |
79 | m = (reg >> 3) & 0xf; |
80 | n = reg & 0x7; |
81 | |
82 | return (((parent_rate / ddc->pre_div) / 10) >> n) / |
83 | (m + ddc->m_offset); |
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 | |
92 | sun4i_ddc_calc_divider(rate, parent_rate, pre_div: ddc->pre_div, |
93 | m_offset: ddc->m_offset, m: &div_m, n: &div_n); |
94 | |
95 | regmap_field_write(field: ddc->reg, |
96 | SUN4I_HDMI_DDC_CLK_M(div_m) | |
97 | SUN4I_HDMI_DDC_CLK_N(div_n)); |
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(clk: parent); |
115 | if (!parent_name) |
116 | return -ENODEV; |
117 | |
118 | ddc = devm_kzalloc(dev: hdmi->dev, size: sizeof(*ddc), GFP_KERNEL); |
119 | if (!ddc) |
120 | return -ENOMEM; |
121 | |
122 | ddc->reg = devm_regmap_field_alloc(dev: hdmi->dev, regmap: hdmi->regmap, |
123 | reg_field: hdmi->variant->ddc_clk_reg); |
124 | if (IS_ERR(ptr: ddc->reg)) |
125 | return PTR_ERR(ptr: ddc->reg); |
126 | |
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; |
134 | ddc->pre_div = hdmi->variant->ddc_clk_pre_divider; |
135 | ddc->m_offset = hdmi->variant->ddc_clk_m_offset; |
136 | |
137 | hdmi->ddc_clk = devm_clk_register(dev: hdmi->dev, hw: &ddc->hw); |
138 | if (IS_ERR(ptr: hdmi->ddc_clk)) |
139 | return PTR_ERR(ptr: hdmi->ddc_clk); |
140 | |
141 | return 0; |
142 | } |
143 | |