1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2018 Emmanuel Vadot <manu@freebsd.org>
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
16 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
17 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
18 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
19 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
20 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
22 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
23 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 */
27
28 #include <sys/param.h>
29 #include <sys/systm.h>
30 #include <sys/bus.h>
31
32 #include <dev/clk/clk.h>
33
34 #include <arm64/freescale/imx/clk/imx_clk_composite.h>
35
36 #include "clkdev_if.h"
37
38 #define TARGET_ROOT_ENABLE (1 << 28)
39 #define TARGET_ROOT_MUX(n) ((n) << 24)
40 #define TARGET_ROOT_MUX_MASK (7 << 24)
41 #define TARGET_ROOT_MUX_SHIFT 24
42 #define TARGET_ROOT_PRE_PODF(n) ((((n) - 1) & 0x7) << 16)
43 #define TARGET_ROOT_PRE_PODF_MASK (0x7 << 16)
44 #define TARGET_ROOT_PRE_PODF_SHIFT 16
45 #define TARGET_ROOT_PRE_PODF_MAX 7
46 #define TARGET_ROOT_POST_PODF(n) ((((n) - 1) & 0x3f) << 0)
47 #define TARGET_ROOT_POST_PODF_MASK (0x3f << 0)
48 #define TARGET_ROOT_POST_PODF_SHIFT 0
49 #define TARGET_ROOT_POST_PODF_MAX 0x3f
50
51 struct imx_clk_composite_sc {
52 uint32_t offset;
53 uint32_t flags;
54 };
55
56 #define WRITE4(_clk, off, val) \
57 CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
58 #define READ4(_clk, off, val) \
59 CLKDEV_READ_4(clknode_get_device(_clk), off, val)
60 #define DEVICE_LOCK(_clk) \
61 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
62 #define DEVICE_UNLOCK(_clk) \
63 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
64
65 #define IMX_CLK_COMPOSITE_MASK_SHIFT 16
66
67 #if 0
68 #define dprintf(format, arg...) \
69 printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg)
70 #else
71 #define dprintf(format, arg...)
72 #endif
73
74 static int
imx_clk_composite_init(struct clknode * clk,device_t dev)75 imx_clk_composite_init(struct clknode *clk, device_t dev)
76 {
77 struct imx_clk_composite_sc *sc;
78 uint32_t val, idx;
79
80 sc = clknode_get_softc(clk);
81
82 DEVICE_LOCK(clk);
83 READ4(clk, sc->offset, &val);
84 DEVICE_UNLOCK(clk);
85 idx = (val & TARGET_ROOT_MUX_MASK) >> TARGET_ROOT_MUX_SHIFT;
86
87 clknode_init_parent_idx(clk, idx);
88
89 return (0);
90 }
91
92 static int
imx_clk_composite_set_gate(struct clknode * clk,bool enable)93 imx_clk_composite_set_gate(struct clknode *clk, bool enable)
94 {
95 struct imx_clk_composite_sc *sc;
96 uint32_t val = 0;
97
98 sc = clknode_get_softc(clk);
99
100 dprintf("%sabling gate\n", enable ? "En" : "Dis");
101 DEVICE_LOCK(clk);
102 READ4(clk, sc->offset, &val);
103 if (enable)
104 val |= TARGET_ROOT_ENABLE;
105 else
106 val &= ~(TARGET_ROOT_ENABLE);
107 WRITE4(clk, sc->offset, val);
108 DEVICE_UNLOCK(clk);
109
110 return (0);
111 }
112
113 static int
imx_clk_composite_set_mux(struct clknode * clk,int index)114 imx_clk_composite_set_mux(struct clknode *clk, int index)
115 {
116 struct imx_clk_composite_sc *sc;
117 uint32_t val = 0;
118
119 sc = clknode_get_softc(clk);
120
121 dprintf("Set mux to %d\n", index);
122 DEVICE_LOCK(clk);
123 READ4(clk, sc->offset, &val);
124 val &= ~(TARGET_ROOT_MUX_MASK);
125 val |= TARGET_ROOT_MUX(index);
126 WRITE4(clk, sc->offset, val);
127 DEVICE_UNLOCK(clk);
128
129 return (0);
130 }
131
132 static int
imx_clk_composite_recalc(struct clknode * clk,uint64_t * freq)133 imx_clk_composite_recalc(struct clknode *clk, uint64_t *freq)
134 {
135 struct imx_clk_composite_sc *sc;
136 uint32_t reg, pre_div, post_div;
137
138 sc = clknode_get_softc(clk);
139
140 DEVICE_LOCK(clk);
141 READ4(clk, sc->offset, ®);
142 DEVICE_UNLOCK(clk);
143
144 pre_div = ((reg & TARGET_ROOT_PRE_PODF_MASK)
145 >> TARGET_ROOT_PRE_PODF_SHIFT) + 1;
146 post_div = ((reg & TARGET_ROOT_POST_PODF_MASK)
147 >> TARGET_ROOT_POST_PODF_SHIFT) + 1;
148
149 dprintf("parent_freq=%ju, div=%u\n", *freq, div);
150 *freq = *freq / pre_div / post_div;
151 dprintf("Final freq=%ju\n", *freq);
152 return (0);
153 }
154
155 static int
imx_clk_composite_find_best(uint64_t fparent,uint64_t ftarget,uint32_t * pre_div,uint32_t * post_div,int flags)156 imx_clk_composite_find_best(uint64_t fparent, uint64_t ftarget,
157 uint32_t *pre_div, uint32_t *post_div, int flags)
158 {
159 uint32_t prediv, postdiv, best_prediv, best_postdiv;
160 int64_t diff, best_diff;
161 uint64_t cur;
162
163 best_diff = INT64_MAX;
164 for (prediv = 1; prediv <= TARGET_ROOT_PRE_PODF_MAX + 1; prediv++) {
165 for (postdiv = 1; postdiv <= TARGET_ROOT_POST_PODF_MAX + 1; postdiv++) {
166 cur= fparent / prediv / postdiv;
167 diff = (int64_t)ftarget - (int64_t)cur;
168 if (flags & CLK_SET_ROUND_DOWN) {
169 if (diff >= 0 && diff < best_diff) {
170 best_diff = diff;
171 best_prediv = prediv;
172 best_postdiv = postdiv;
173 }
174 }
175 else if (flags & CLK_SET_ROUND_UP) {
176 if (diff <= 0 && abs(diff) < best_diff) {
177 best_diff = diff;
178 best_prediv = prediv;
179 best_postdiv = postdiv;
180 }
181 }
182 else {
183 if (abs(diff) < best_diff) {
184 best_diff = abs(diff);
185 best_prediv = prediv;
186 best_postdiv = postdiv;
187 }
188 }
189 }
190 }
191
192 if (best_diff == INT64_MAX)
193 return (ERANGE);
194
195 *pre_div = best_prediv;
196 *post_div = best_postdiv;
197
198 return (0);
199 }
200
201 static int
imx_clk_composite_set_freq(struct clknode * clk,uint64_t fparent,uint64_t * fout,int flags,int * stop)202 imx_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
203 int flags, int *stop)
204 {
205 struct imx_clk_composite_sc *sc;
206 struct clknode *p_clk;
207 const char **p_names;
208 int p_idx, best_parent;
209 int64_t best_diff, diff;
210 int32_t best_pre_div __unused, best_post_div __unused;
211 int32_t pre_div, post_div;
212 uint64_t cur, best;
213 uint32_t val;
214
215 sc = clknode_get_softc(clk);
216 dprintf("Finding best parent/div for target freq of %ju\n", *fout);
217 p_names = clknode_get_parent_names(clk);
218
219 best_diff = 0;
220 best_parent = -1;
221
222 for (p_idx = 0; p_idx != clknode_get_parents_num(clk); p_idx++) {
223 p_clk = clknode_find_by_name(p_names[p_idx]);
224 clknode_get_freq(p_clk, &fparent);
225 dprintf("Testing with parent %s (%d) at freq %ju\n",
226 clknode_get_name(p_clk), p_idx, fparent);
227
228 if (!imx_clk_composite_find_best(fparent, *fout, &pre_div, &post_div, sc->flags))
229 continue;
230 cur = fparent / pre_div / post_div;
231 diff = abs((int64_t)*fout - (int64_t)cur);
232 if (diff < best_diff) {
233 best = cur;
234 best_diff = diff;
235 best_pre_div = pre_div;
236 best_post_div = post_div;
237 best_parent = p_idx;
238 dprintf("Best parent so far %s (%d) with best freq at "
239 "%ju\n", clknode_get_name(p_clk), p_idx, best);
240 }
241 }
242
243 *stop = 1;
244 if (best_diff == INT64_MAX)
245 return (ERANGE);
246
247 /* If we didn't find a new best_parent just return */
248 if (best_parent == -1)
249 return (0);
250
251 if ((flags & CLK_SET_DRYRUN) != 0) {
252 *fout = best;
253 return (0);
254 }
255
256 p_idx = clknode_get_parent_idx(clk);
257 if (p_idx != best_parent) {
258 dprintf("Switching parent index from %d to %d\n", p_idx,
259 best_parent);
260 clknode_set_parent_by_idx(clk, best_parent);
261 }
262
263 dprintf("Setting dividers to pre=%d, post=%d\n", best_pre_div, best_post_div);
264
265 DEVICE_LOCK(clk);
266 READ4(clk, sc->offset, &val);
267 val &= ~(TARGET_ROOT_PRE_PODF_MASK | TARGET_ROOT_POST_PODF_MASK);
268 val |= TARGET_ROOT_PRE_PODF(pre_div);
269 val |= TARGET_ROOT_POST_PODF(post_div);
270 DEVICE_UNLOCK(clk);
271
272 *fout = best;
273 return (0);
274 }
275
276 static clknode_method_t imx_clk_composite_clknode_methods[] = {
277 /* Device interface */
278 CLKNODEMETHOD(clknode_init, imx_clk_composite_init),
279 CLKNODEMETHOD(clknode_set_gate, imx_clk_composite_set_gate),
280 CLKNODEMETHOD(clknode_set_mux, imx_clk_composite_set_mux),
281 CLKNODEMETHOD(clknode_recalc_freq, imx_clk_composite_recalc),
282 CLKNODEMETHOD(clknode_set_freq, imx_clk_composite_set_freq),
283 CLKNODEMETHOD_END
284 };
285
286 DEFINE_CLASS_1(imx_clk_composite_clknode, imx_clk_composite_clknode_class,
287 imx_clk_composite_clknode_methods, sizeof(struct imx_clk_composite_sc),
288 clknode_class);
289
290 int
imx_clk_composite_register(struct clkdom * clkdom,struct imx_clk_composite_def * clkdef)291 imx_clk_composite_register(struct clkdom *clkdom,
292 struct imx_clk_composite_def *clkdef)
293 {
294 struct clknode *clk;
295 struct imx_clk_composite_sc *sc;
296
297 clk = clknode_create(clkdom, &imx_clk_composite_clknode_class,
298 &clkdef->clkdef);
299 if (clk == NULL)
300 return (1);
301
302 sc = clknode_get_softc(clk);
303
304 sc->offset = clkdef->offset;
305 sc->flags = clkdef->flags;
306
307 clknode_register(clkdom, clk);
308
309 return (0);
310 }
311