2 * Copyright (c) 2019 Emmanuel Vadot <manu@freebsd.org>
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
7 * 1. Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * 2. Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
13 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
14 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
15 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
16 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
17 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
18 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
19 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
20 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
21 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
22 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
28 #include <sys/cdefs.h>
29 __FBSDID("$FreeBSD$");
31 #include <sys/param.h>
32 #include <sys/systm.h>
35 #include <dev/extres/clk/clk.h>
37 #include <arm/allwinner/clkng/aw_clk.h>
38 #include <arm/allwinner/clkng/aw_clk_frac.h>
40 #include "clkdev_if.h"
42 /* #define dprintf(format, arg...) printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg) */
43 #define dprintf(format, arg...)
46 * clknode for clocks matching the formula :
48 * clk = (24Mhz * n) / m in integer mode
49 * clk = frac_out1 or frac_out2 in fractional mode
53 struct aw_clk_frac_sc {
56 struct aw_clk_factor m;
57 struct aw_clk_factor n;
58 struct aw_clk_frac frac;
67 uint32_t lock_retries;
72 #define WRITE4(_clk, off, val) \
73 CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
74 #define READ4(_clk, off, val) \
75 CLKDEV_READ_4(clknode_get_device(_clk), off, val)
76 #define DEVICE_LOCK(_clk) \
77 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
78 #define DEVICE_UNLOCK(_clk) \
79 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
82 aw_clk_frac_init(struct clknode *clk, device_t dev)
84 struct aw_clk_frac_sc *sc;
87 sc = clknode_get_softc(clk);
90 if ((sc->flags & AW_CLK_HAS_MUX) != 0) {
92 READ4(clk, sc->offset, &val);
95 idx = (val & sc->mux_mask) >> sc->mux_shift;
98 dprintf("init parent idx %d\n", idx);
99 clknode_init_parent_idx(clk, idx);
104 aw_clk_frac_set_gate(struct clknode *clk, bool enable)
106 struct aw_clk_frac_sc *sc;
109 sc = clknode_get_softc(clk);
111 if ((sc->flags & AW_CLK_HAS_GATE) == 0)
114 dprintf("%sabling gate\n", enable ? "En" : "Dis");
116 READ4(clk, sc->offset, &val);
118 val |= (1 << sc->gate_shift);
120 val &= ~(1 << sc->gate_shift);
121 WRITE4(clk, sc->offset, val);
128 aw_clk_frac_set_mux(struct clknode *clk, int index)
130 struct aw_clk_frac_sc *sc;
133 sc = clknode_get_softc(clk);
135 if ((sc->flags & AW_CLK_HAS_MUX) == 0)
138 dprintf("Set mux to %d\n", index);
140 READ4(clk, sc->offset, &val);
141 val &= ~sc->mux_mask;
142 val |= index << sc->mux_shift;
143 WRITE4(clk, sc->offset, val);
150 aw_clk_frac_find_best(struct aw_clk_frac_sc *sc, uint64_t fparent, uint64_t fout,
151 uint32_t *factor_n, uint32_t *factor_m)
154 uint32_t m, n, max_m, max_n, min_m, min_n;
156 *factor_n = *factor_m = 0;
159 max_m = aw_clk_factor_get_max(&sc->m);
160 max_n = aw_clk_factor_get_max(&sc->n);
161 min_m = aw_clk_factor_get_min(&sc->m);
162 min_n = sc->min_freq / fparent;
164 for (n = min_n; n <= max_n; n++) {
165 for (m = min_m; m <= max_m; m++) {
166 cur = fparent * n / m;
167 if (cur < sc->min_freq) {
170 if (cur > sc->max_freq) {
178 if (abs((fout - cur)) < abs((fout - best))) {
190 aw_clk_frac_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
191 int flags, int *stop)
193 struct aw_clk_frac_sc *sc;
194 uint64_t cur, best, best_frac;
195 uint32_t val, m, n, best_m, best_n;
196 int retry, multiple, max_mult, best_mult;
198 sc = clknode_get_softc(clk);
200 best = best_frac = cur = 0;
204 dprintf("Trying to find freq %ju with parent %ju\n", *fout, fparent);
205 if ((flags & CLK_SET_ROUND_MULTIPLE) != 0)
207 for (multiple = 1; multiple <= max_mult; multiple++) {
208 /* First test the fractional frequencies */
209 dprintf("Testing with multiple %d\n", multiple);
210 if (*fout * multiple == sc->frac.freq0) {
211 best = best_frac = sc->frac.freq0;
212 best_mult = multiple;
213 dprintf("Found with using frac.freq0 and multiple %d\n", multiple);
216 else if (*fout * multiple == sc->frac.freq1) {
217 best = best_frac = sc->frac.freq1;
218 best_mult = multiple;
219 dprintf("Found with using frac.freq1 and multiple %d\n", multiple);
223 cur = aw_clk_frac_find_best(sc, fparent, *fout * multiple,
225 dprintf("Got %ju with n=%d, m=%d\n", cur, n, m);
226 if (cur == (*fout * multiple)) {
228 best_mult = multiple;
231 dprintf("This is the one: n=%d m=%d mult=%d\n", best_n, best_m, best_mult);
234 if (abs(((*fout * multiple) - cur)) < abs(((*fout * multiple) - best))) {
236 best_mult = multiple;
239 dprintf("This is the best for now: n=%d m=%d mult=%d\n", best_n, best_m, best_mult);
244 if (best < sc->min_freq ||
245 best > sc->max_freq) {
246 printf("%s: Cannot set %ju for %s (min=%ju max=%ju)\n",
247 __func__, best, clknode_get_name(clk),
248 sc->min_freq, sc->max_freq);
251 if ((flags & CLK_SET_DRYRUN) != 0) {
257 if ((best < (*fout * best_mult)) &&
258 ((flags & CLK_SET_ROUND_DOWN) == 0)) {
262 if ((best > *fout * best_mult) &&
263 ((flags & CLK_SET_ROUND_UP) == 0)) {
269 READ4(clk, sc->offset, &val);
270 /* Disable clock during freq changes */
271 val &= ~(1 << sc->gate_shift);
272 WRITE4(clk, sc->offset, val);
274 if (best_frac != 0) {
275 val &= ~sc->frac.mode_sel;
276 /* M should be 0 per the manual */
278 if (best_frac == sc->frac.freq0)
279 val &= ~sc->frac.freq_sel;
281 val |= sc->frac.freq_sel;
283 val |= sc->frac.mode_sel; /* Select integer mode */
284 n = aw_clk_factor_get_value(&sc->n, best_n);
285 m = aw_clk_factor_get_value(&sc->m, best_m);
288 val |= n << sc->n.shift;
289 val |= m << sc->m.shift;
292 /* Write the clock changes */
293 WRITE4(clk, sc->offset, val);
295 /* Enable clock now that we've change it */
296 val |= 1 << sc->gate_shift;
297 WRITE4(clk, sc->offset, val);
300 for (retry = 0; retry < sc->lock_retries; retry++) {
301 READ4(clk, sc->offset, &val);
302 if ((val & (1 << sc->lock_shift)) != 0)
314 aw_clk_frac_recalc(struct clknode *clk, uint64_t *freq)
316 struct aw_clk_frac_sc *sc;
319 sc = clknode_get_softc(clk);
322 READ4(clk, sc->offset, &val);
325 if ((val & sc->frac.mode_sel) == 0) {
326 if (val & sc->frac.freq_sel)
327 *freq = sc->frac.freq1;
329 *freq = sc->frac.freq0;
331 m = aw_clk_get_factor(val, &sc->m);
332 n = aw_clk_get_factor(val, &sc->n);
333 *freq = *freq * n / m;
339 static clknode_method_t aw_frac_clknode_methods[] = {
340 /* Device interface */
341 CLKNODEMETHOD(clknode_init, aw_clk_frac_init),
342 CLKNODEMETHOD(clknode_set_gate, aw_clk_frac_set_gate),
343 CLKNODEMETHOD(clknode_set_mux, aw_clk_frac_set_mux),
344 CLKNODEMETHOD(clknode_recalc_freq, aw_clk_frac_recalc),
345 CLKNODEMETHOD(clknode_set_freq, aw_clk_frac_set_freq),
349 DEFINE_CLASS_1(aw_frac_clknode, aw_frac_clknode_class, aw_frac_clknode_methods,
350 sizeof(struct aw_clk_frac_sc), clknode_class);
353 aw_clk_frac_register(struct clkdom *clkdom, struct aw_clk_frac_def *clkdef)
356 struct aw_clk_frac_sc *sc;
358 clk = clknode_create(clkdom, &aw_frac_clknode_class, &clkdef->clkdef);
362 sc = clknode_get_softc(clk);
364 sc->offset = clkdef->offset;
366 sc->m.shift = clkdef->m.shift;
367 sc->m.width = clkdef->m.width;
368 sc->m.mask = ((1 << sc->m.width) - 1) << sc->m.shift;
369 sc->m.value = clkdef->m.value;
370 sc->m.flags = clkdef->m.flags;
372 sc->n.shift = clkdef->n.shift;
373 sc->n.width = clkdef->n.width;
374 sc->n.mask = ((1 << sc->n.width) - 1) << sc->n.shift;
375 sc->n.value = clkdef->n.value;
376 sc->n.flags = clkdef->n.flags;
378 sc->frac.freq0 = clkdef->frac.freq0;
379 sc->frac.freq1 = clkdef->frac.freq1;
380 sc->frac.mode_sel = 1 << clkdef->frac.mode_sel;
381 sc->frac.freq_sel = 1 << clkdef->frac.freq_sel;
383 sc->min_freq = clkdef->min_freq;
384 sc->max_freq = clkdef->max_freq;
386 sc->mux_shift = clkdef->mux_shift;
387 sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
389 sc->gate_shift = clkdef->gate_shift;
391 sc->lock_shift = clkdef->lock_shift;
392 sc->lock_retries = clkdef->lock_retries;
394 sc->flags = clkdef->flags;
396 clknode_register(clkdom, clk);