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/io.h> |
11 | |
12 | #include "sun4i_hdmi.h" |
13 | |
14 | struct sun4i_tmds { |
15 | struct clk_hw hw; |
16 | struct sun4i_hdmi *hdmi; |
17 | |
18 | u8 div_offset; |
19 | }; |
20 | |
21 | static inline struct sun4i_tmds *hw_to_tmds(struct clk_hw *hw) |
22 | { |
23 | return container_of(hw, struct sun4i_tmds, hw); |
24 | } |
25 | |
26 | |
27 | static unsigned long sun4i_tmds_calc_divider(unsigned long rate, |
28 | unsigned long parent_rate, |
29 | u8 div_offset, |
30 | u8 *div, |
31 | bool *half) |
32 | { |
33 | unsigned long best_rate = 0; |
34 | u8 best_m = 0, m; |
35 | bool is_double = false; |
36 | |
37 | for (m = div_offset ?: 1; m < (16 + div_offset); m++) { |
38 | u8 d; |
39 | |
40 | for (d = 1; d < 3; d++) { |
41 | unsigned long tmp_rate; |
42 | |
43 | tmp_rate = parent_rate / m / d; |
44 | |
45 | if (tmp_rate > rate) |
46 | continue; |
47 | |
48 | if (!best_rate || |
49 | (rate - tmp_rate) < (rate - best_rate)) { |
50 | best_rate = tmp_rate; |
51 | best_m = m; |
52 | is_double = (d == 2) ? true : false; |
53 | } |
54 | } |
55 | } |
56 | |
57 | if (div && half) { |
58 | *div = best_m; |
59 | *half = is_double; |
60 | } |
61 | |
62 | return best_rate; |
63 | } |
64 | |
65 | |
66 | static int sun4i_tmds_determine_rate(struct clk_hw *hw, |
67 | struct clk_rate_request *req) |
68 | { |
69 | struct sun4i_tmds *tmds = hw_to_tmds(hw); |
70 | struct clk_hw *parent = NULL; |
71 | unsigned long best_parent = 0; |
72 | unsigned long rate = req->rate; |
73 | int best_div = 1, best_half = 1; |
74 | int i, j, p; |
75 | |
76 | /* |
77 | * We only consider PLL3, since the TCON is very likely to be |
78 | * clocked from it, and to have the same rate than our HDMI |
79 | * clock, so we should not need to do anything. |
80 | */ |
81 | |
82 | for (p = 0; p < clk_hw_get_num_parents(hw); p++) { |
83 | parent = clk_hw_get_parent_by_index(hw, index: p); |
84 | if (!parent) |
85 | continue; |
86 | |
87 | for (i = 1; i < 3; i++) { |
88 | for (j = tmds->div_offset ?: 1; |
89 | j < (16 + tmds->div_offset); j++) { |
90 | unsigned long ideal = rate * i * j; |
91 | unsigned long rounded; |
92 | |
93 | rounded = clk_hw_round_rate(hw: parent, rate: ideal); |
94 | |
95 | if (rounded == ideal) { |
96 | best_parent = rounded; |
97 | best_half = i; |
98 | best_div = j; |
99 | goto out; |
100 | } |
101 | |
102 | if (!best_parent || |
103 | abs(rate - rounded / i / j) < |
104 | abs(rate - best_parent / best_half / |
105 | best_div)) { |
106 | best_parent = rounded; |
107 | best_half = i; |
108 | best_div = j; |
109 | } |
110 | } |
111 | } |
112 | } |
113 | |
114 | if (!parent) |
115 | return -EINVAL; |
116 | |
117 | out: |
118 | req->rate = best_parent / best_half / best_div; |
119 | req->best_parent_rate = best_parent; |
120 | req->best_parent_hw = parent; |
121 | |
122 | return 0; |
123 | } |
124 | |
125 | static unsigned long sun4i_tmds_recalc_rate(struct clk_hw *hw, |
126 | unsigned long parent_rate) |
127 | { |
128 | struct sun4i_tmds *tmds = hw_to_tmds(hw); |
129 | u32 reg; |
130 | |
131 | reg = readl(addr: tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); |
132 | if (reg & SUN4I_HDMI_PAD_CTRL1_HALVE_CLK) |
133 | parent_rate /= 2; |
134 | |
135 | reg = readl(addr: tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); |
136 | reg = ((reg >> 4) & 0xf) + tmds->div_offset; |
137 | if (!reg) |
138 | reg = 1; |
139 | |
140 | return parent_rate / reg; |
141 | } |
142 | |
143 | static int sun4i_tmds_set_rate(struct clk_hw *hw, unsigned long rate, |
144 | unsigned long parent_rate) |
145 | { |
146 | struct sun4i_tmds *tmds = hw_to_tmds(hw); |
147 | bool half; |
148 | u32 reg; |
149 | u8 div; |
150 | |
151 | sun4i_tmds_calc_divider(rate, parent_rate, div_offset: tmds->div_offset, |
152 | div: &div, half: &half); |
153 | |
154 | reg = readl(addr: tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); |
155 | reg &= ~SUN4I_HDMI_PAD_CTRL1_HALVE_CLK; |
156 | if (half) |
157 | reg |= SUN4I_HDMI_PAD_CTRL1_HALVE_CLK; |
158 | writel(val: reg, addr: tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); |
159 | |
160 | reg = readl(addr: tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); |
161 | reg &= ~SUN4I_HDMI_PLL_CTRL_DIV_MASK; |
162 | writel(val: reg | SUN4I_HDMI_PLL_CTRL_DIV(div - tmds->div_offset), |
163 | addr: tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); |
164 | |
165 | return 0; |
166 | } |
167 | |
168 | static u8 sun4i_tmds_get_parent(struct clk_hw *hw) |
169 | { |
170 | struct sun4i_tmds *tmds = hw_to_tmds(hw); |
171 | u32 reg; |
172 | |
173 | reg = readl(addr: tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); |
174 | return ((reg & SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_MASK) >> |
175 | SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_SHIFT); |
176 | } |
177 | |
178 | static int sun4i_tmds_set_parent(struct clk_hw *hw, u8 index) |
179 | { |
180 | struct sun4i_tmds *tmds = hw_to_tmds(hw); |
181 | u32 reg; |
182 | |
183 | if (index > 1) |
184 | return -EINVAL; |
185 | |
186 | reg = readl(addr: tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); |
187 | reg &= ~SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_MASK; |
188 | writel(val: reg | SUN4I_HDMI_PLL_DBG0_TMDS_PARENT(index), |
189 | addr: tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); |
190 | |
191 | return 0; |
192 | } |
193 | |
194 | static const struct clk_ops sun4i_tmds_ops = { |
195 | .determine_rate = sun4i_tmds_determine_rate, |
196 | .recalc_rate = sun4i_tmds_recalc_rate, |
197 | .set_rate = sun4i_tmds_set_rate, |
198 | |
199 | .get_parent = sun4i_tmds_get_parent, |
200 | .set_parent = sun4i_tmds_set_parent, |
201 | }; |
202 | |
203 | int sun4i_tmds_create(struct sun4i_hdmi *hdmi) |
204 | { |
205 | struct clk_init_data init; |
206 | struct sun4i_tmds *tmds; |
207 | const char *parents[2]; |
208 | |
209 | parents[0] = __clk_get_name(clk: hdmi->pll0_clk); |
210 | if (!parents[0]) |
211 | return -ENODEV; |
212 | |
213 | parents[1] = __clk_get_name(clk: hdmi->pll1_clk); |
214 | if (!parents[1]) |
215 | return -ENODEV; |
216 | |
217 | tmds = devm_kzalloc(dev: hdmi->dev, size: sizeof(*tmds), GFP_KERNEL); |
218 | if (!tmds) |
219 | return -ENOMEM; |
220 | |
221 | init.name = "hdmi-tmds" ; |
222 | init.ops = &sun4i_tmds_ops; |
223 | init.parent_names = parents; |
224 | init.num_parents = 2; |
225 | init.flags = CLK_SET_RATE_PARENT; |
226 | |
227 | tmds->hdmi = hdmi; |
228 | tmds->hw.init = &init; |
229 | tmds->div_offset = hdmi->variant->tmds_clk_div_offset; |
230 | |
231 | hdmi->tmds_clk = devm_clk_register(dev: hdmi->dev, hw: &tmds->hw); |
232 | if (IS_ERR(ptr: hdmi->tmds_clk)) |
233 | return PTR_ERR(ptr: hdmi->tmds_clk); |
234 | |
235 | return 0; |
236 | } |
237 | |