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