1 /*-
2 * Copyright (c) 2017 Emmanuel Vadot <[email protected]>
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
15 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
16 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
17 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
18 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
19 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
20 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
21 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
22 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 * $FreeBSD$
27 */
28
29 #include <sys/cdefs.h>
30 __FBSDID("$FreeBSD$");
31
32 #include <sys/param.h>
33 #include <sys/systm.h>
34 #include <sys/bus.h>
35
36 #include <dev/extres/clk/clk.h>
37
38 #include <arm/allwinner/clkng/aw_clk.h>
39 #include <arm/allwinner/clkng/aw_clk_nkmp.h>
40
41 #include "clkdev_if.h"
42
43 /*
44 * clknode for clocks matching the formula :
45 *
46 * clk = (clkin * n * k) / (m * p)
47 *
48 */
49
50 struct aw_clk_nkmp_sc {
51 uint32_t offset;
52
53 struct aw_clk_factor n;
54 struct aw_clk_factor k;
55 struct aw_clk_factor m;
56 struct aw_clk_factor p;
57
58 uint32_t mux_shift;
59 uint32_t mux_mask;
60 uint32_t gate_shift;
61 uint32_t lock_shift;
62 uint32_t lock_retries;
63 uint32_t update_shift;
64
65 uint32_t flags;
66 };
67
68 #define WRITE4(_clk, off, val) \
69 CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
70 #define READ4(_clk, off, val) \
71 CLKDEV_READ_4(clknode_get_device(_clk), off, val)
72 #define MODIFY4(_clk, off, clr, set ) \
73 CLKDEV_MODIFY_4(clknode_get_device(_clk), off, clr, set)
74 #define DEVICE_LOCK(_clk) \
75 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
76 #define DEVICE_UNLOCK(_clk) \
77 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
78
79 static int
aw_clk_nkmp_init(struct clknode * clk,device_t dev)80 aw_clk_nkmp_init(struct clknode *clk, device_t dev)
81 {
82 struct aw_clk_nkmp_sc *sc;
83 uint32_t val, idx;
84
85 sc = clknode_get_softc(clk);
86
87 idx = 0;
88 if ((sc->flags & AW_CLK_HAS_MUX) != 0) {
89 DEVICE_LOCK(clk);
90 READ4(clk, sc->offset, &val);
91 DEVICE_UNLOCK(clk);
92
93 idx = (val & sc->mux_mask) >> sc->mux_shift;
94 }
95
96 clknode_init_parent_idx(clk, idx);
97 return (0);
98 }
99
100 static int
aw_clk_nkmp_set_gate(struct clknode * clk,bool enable)101 aw_clk_nkmp_set_gate(struct clknode *clk, bool enable)
102 {
103 struct aw_clk_nkmp_sc *sc;
104 uint32_t val;
105
106 sc = clknode_get_softc(clk);
107
108 if ((sc->flags & AW_CLK_HAS_GATE) == 0)
109 return (0);
110
111 DEVICE_LOCK(clk);
112 READ4(clk, sc->offset, &val);
113 if (enable)
114 val |= (1 << sc->gate_shift);
115 else
116 val &= ~(1 << sc->gate_shift);
117 WRITE4(clk, sc->offset, val);
118 DEVICE_UNLOCK(clk);
119
120 return (0);
121 }
122
123 static int
aw_clk_nkmp_set_mux(struct clknode * clk,int index)124 aw_clk_nkmp_set_mux(struct clknode *clk, int index)
125 {
126 struct aw_clk_nkmp_sc *sc;
127 uint32_t val;
128
129 sc = clknode_get_softc(clk);
130
131 if ((sc->flags & AW_CLK_HAS_MUX) == 0)
132 return (0);
133
134 DEVICE_LOCK(clk);
135 READ4(clk, sc->offset, &val);
136 val &= ~sc->mux_mask;
137 val |= index << sc->mux_shift;
138 WRITE4(clk, sc->offset, val);
139 DEVICE_UNLOCK(clk);
140
141 return (0);
142 }
143
144 static uint64_t
aw_clk_nkmp_find_best(struct aw_clk_nkmp_sc * sc,uint64_t fparent,uint64_t * fout,uint32_t * factor_n,uint32_t * factor_k,uint32_t * factor_m,uint32_t * factor_p)145 aw_clk_nkmp_find_best(struct aw_clk_nkmp_sc *sc, uint64_t fparent, uint64_t *fout,
146 uint32_t *factor_n, uint32_t *factor_k, uint32_t *factor_m, uint32_t *factor_p)
147 {
148 uint64_t cur, best;
149 uint32_t n, k, m, p;
150
151 best = 0;
152 *factor_n = 0;
153 *factor_k = 0;
154 *factor_m = 0;
155 *factor_p = 0;
156
157 for (n = aw_clk_factor_get_min(&sc->n); n <= aw_clk_factor_get_max(&sc->n); ) {
158 for (k = aw_clk_factor_get_min(&sc->k); k <= aw_clk_factor_get_max(&sc->k); ) {
159 for (m = aw_clk_factor_get_min(&sc->m); m <= aw_clk_factor_get_max(&sc->m); ) {
160 for (p = aw_clk_factor_get_min(&sc->p); p <= aw_clk_factor_get_max(&sc->p); ) {
161 cur = (fparent * n * k) / (m * p);
162 if ((*fout - cur) < (*fout - best)) {
163 best = cur;
164 *factor_n = n;
165 *factor_k = k;
166 *factor_m = m;
167 *factor_p = p;
168 }
169 if (best == *fout)
170 return (best);
171 if ((sc->p.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0)
172 p <<= 1;
173 else
174 p++;
175 }
176 if ((sc->m.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0)
177 m <<= 1;
178 else
179 m++;
180 }
181 if ((sc->k.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0)
182 k <<= 1;
183 else
184 k++;
185 }
186 if ((sc->n.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0)
187 n <<= 1;
188 else
189 n++;
190 }
191
192 return best;
193 }
194
195 static void
aw_clk_nkmp_set_freq_scale(struct clknode * clk,struct aw_clk_nkmp_sc * sc,uint32_t factor_n,uint32_t factor_k,uint32_t factor_m,uint32_t factor_p)196 aw_clk_nkmp_set_freq_scale(struct clknode *clk, struct aw_clk_nkmp_sc *sc,
197 uint32_t factor_n, uint32_t factor_k, uint32_t factor_m, uint32_t factor_p)
198 {
199 uint32_t val, n, k, m, p;
200 int retry;
201
202 DEVICE_LOCK(clk);
203 READ4(clk, sc->offset, &val);
204
205 n = aw_clk_get_factor(val, &sc->n);
206 k = aw_clk_get_factor(val, &sc->k);
207 m = aw_clk_get_factor(val, &sc->m);
208 p = aw_clk_get_factor(val, &sc->p);
209
210 if (p < factor_p) {
211 val &= ~sc->p.mask;
212 val |= aw_clk_factor_get_value(&sc->p, factor_p) << sc->p.shift;
213 WRITE4(clk, sc->offset, val);
214 DELAY(2000);
215 }
216
217 if (m < factor_m) {
218 val &= ~sc->m.mask;
219 val |= aw_clk_factor_get_value(&sc->m, factor_m) << sc->m.shift;
220 WRITE4(clk, sc->offset, val);
221 DELAY(2000);
222 }
223
224 val &= ~sc->n.mask;
225 val &= ~sc->k.mask;
226 val |= aw_clk_factor_get_value(&sc->n, factor_n) << sc->n.shift;
227 val |= aw_clk_factor_get_value(&sc->k, factor_k) << sc->k.shift;
228 WRITE4(clk, sc->offset, val);
229 DELAY(2000);
230
231 if (m > factor_m) {
232 val &= ~sc->m.mask;
233 val |= aw_clk_factor_get_value(&sc->m, factor_m) << sc->m.shift;
234 WRITE4(clk, sc->offset, val);
235 DELAY(2000);
236 }
237
238 if (p > factor_p) {
239 val &= ~sc->p.mask;
240 val |= aw_clk_factor_get_value(&sc->p, factor_p) << sc->p.shift;
241 WRITE4(clk, sc->offset, val);
242 DELAY(2000);
243 }
244
245 if ((sc->flags & AW_CLK_HAS_LOCK) != 0) {
246 for (retry = 0; retry < sc->lock_retries; retry++) {
247 READ4(clk, sc->offset, &val);
248 if ((val & (1 << sc->lock_shift)) != 0)
249 break;
250 DELAY(1000);
251 }
252 }
253
254 DEVICE_UNLOCK(clk);
255 }
256
257 static int
aw_clk_nkmp_set_freq(struct clknode * clk,uint64_t fparent,uint64_t * fout,int flags,int * stop)258 aw_clk_nkmp_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
259 int flags, int *stop)
260 {
261 struct aw_clk_nkmp_sc *sc;
262 uint64_t best;
263 uint32_t val, best_n, best_k, best_m, best_p;
264 int retry;
265
266 sc = clknode_get_softc(clk);
267
268 best = aw_clk_nkmp_find_best(sc, fparent, fout,
269 &best_n, &best_k, &best_m, &best_p);
270 if ((flags & CLK_SET_DRYRUN) != 0) {
271 *fout = best;
272 *stop = 1;
273 return (0);
274 }
275
276 if ((best < *fout) &&
277 ((flags & CLK_SET_ROUND_DOWN) != 0)) {
278 *stop = 1;
279 return (ERANGE);
280 }
281 if ((best > *fout) &&
282 ((flags & CLK_SET_ROUND_UP) != 0)) {
283 *stop = 1;
284 return (ERANGE);
285 }
286
287 if ((sc->flags & AW_CLK_SCALE_CHANGE) != 0)
288 aw_clk_nkmp_set_freq_scale(clk, sc,
289 best_n, best_k, best_m, best_p);
290 else {
291 DEVICE_LOCK(clk);
292 READ4(clk, sc->offset, &val);
293 val &= ~sc->n.mask;
294 val &= ~sc->k.mask;
295 val &= ~sc->m.mask;
296 val &= ~sc->p.mask;
297 val |= aw_clk_factor_get_value(&sc->n, best_n) << sc->n.shift;
298 val |= aw_clk_factor_get_value(&sc->k, best_k) << sc->k.shift;
299 val |= aw_clk_factor_get_value(&sc->m, best_m) << sc->m.shift;
300 val |= aw_clk_factor_get_value(&sc->p, best_p) << sc->p.shift;
301 WRITE4(clk, sc->offset, val);
302 DELAY(2000);
303 DEVICE_UNLOCK(clk);
304
305 if ((sc->flags & AW_CLK_HAS_UPDATE) != 0) {
306 DEVICE_LOCK(clk);
307 READ4(clk, sc->offset, &val);
308 val |= 1 << sc->update_shift;
309 WRITE4(clk, sc->offset, val);
310 DELAY(2000);
311 DEVICE_UNLOCK(clk);
312 }
313
314 if ((sc->flags & AW_CLK_HAS_LOCK) != 0) {
315 for (retry = 0; retry < sc->lock_retries; retry++) {
316 READ4(clk, sc->offset, &val);
317 if ((val & (1 << sc->lock_shift)) != 0)
318 break;
319 DELAY(1000);
320 }
321 }
322 }
323
324 *fout = best;
325 *stop = 1;
326
327 return (0);
328 }
329
330 static int
aw_clk_nkmp_recalc(struct clknode * clk,uint64_t * freq)331 aw_clk_nkmp_recalc(struct clknode *clk, uint64_t *freq)
332 {
333 struct aw_clk_nkmp_sc *sc;
334 uint32_t val, m, n, k, p;
335
336 sc = clknode_get_softc(clk);
337
338 DEVICE_LOCK(clk);
339 READ4(clk, sc->offset, &val);
340 DEVICE_UNLOCK(clk);
341
342 n = aw_clk_get_factor(val, &sc->n);
343 k = aw_clk_get_factor(val, &sc->k);
344 m = aw_clk_get_factor(val, &sc->m);
345 p = aw_clk_get_factor(val, &sc->p);
346
347 *freq = (*freq * n * k) / (m * p);
348
349 return (0);
350 }
351
352 static clknode_method_t aw_nkmp_clknode_methods[] = {
353 /* Device interface */
354 CLKNODEMETHOD(clknode_init, aw_clk_nkmp_init),
355 CLKNODEMETHOD(clknode_set_gate, aw_clk_nkmp_set_gate),
356 CLKNODEMETHOD(clknode_set_mux, aw_clk_nkmp_set_mux),
357 CLKNODEMETHOD(clknode_recalc_freq, aw_clk_nkmp_recalc),
358 CLKNODEMETHOD(clknode_set_freq, aw_clk_nkmp_set_freq),
359 CLKNODEMETHOD_END
360 };
361
362 DEFINE_CLASS_1(aw_nkmp_clknode, aw_nkmp_clknode_class, aw_nkmp_clknode_methods,
363 sizeof(struct aw_clk_nkmp_sc), clknode_class);
364
365 int
aw_clk_nkmp_register(struct clkdom * clkdom,struct aw_clk_nkmp_def * clkdef)366 aw_clk_nkmp_register(struct clkdom *clkdom, struct aw_clk_nkmp_def *clkdef)
367 {
368 struct clknode *clk;
369 struct aw_clk_nkmp_sc *sc;
370
371 clk = clknode_create(clkdom, &aw_nkmp_clknode_class, &clkdef->clkdef);
372 if (clk == NULL)
373 return (1);
374
375 sc = clknode_get_softc(clk);
376
377 sc->offset = clkdef->offset;
378
379 sc->n.shift = clkdef->n.shift;
380 sc->n.width = clkdef->n.width;
381 sc->n.mask = ((1 << clkdef->n.width) - 1) << sc->n.shift;
382 sc->n.value = clkdef->n.value;
383 sc->n.flags = clkdef->n.flags;
384
385 sc->k.shift = clkdef->k.shift;
386 sc->k.width = clkdef->k.width;
387 sc->k.mask = ((1 << clkdef->k.width) - 1) << sc->k.shift;
388 sc->k.value = clkdef->k.value;
389 sc->k.flags = clkdef->k.flags;
390
391 sc->m.shift = clkdef->m.shift;
392 sc->m.width = clkdef->m.width;
393 sc->m.mask = ((1 << clkdef->m.width) - 1) << sc->m.shift;
394 sc->m.value = clkdef->m.value;
395 sc->m.flags = clkdef->m.flags;
396
397 sc->p.shift = clkdef->p.shift;
398 sc->p.width = clkdef->p.width;
399 sc->p.mask = ((1 << clkdef->p.width) - 1) << sc->p.shift;
400 sc->p.value = clkdef->p.value;
401 sc->p.flags = clkdef->p.flags;
402
403 sc->mux_shift = clkdef->mux_shift;
404 sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
405
406 sc->gate_shift = clkdef->gate_shift;
407 sc->lock_shift = clkdef->lock_shift;
408 sc->lock_retries = clkdef->lock_retries;
409 sc->update_shift = clkdef->update_shift;
410 sc->flags = clkdef->flags;
411
412 clknode_register(clkdom, clk);
413
414 return (0);
415 }
416