1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
3 *
4 * Copyright 2019 Michal Meloun <[email protected]>
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 AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, 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/cdefs.h>
29 __FBSDID("$FreeBSD$");
30
31 #include <sys/param.h>
32 #include <sys/systm.h>
33 #include <sys/bus.h>
34
35 #include <dev/extres/clk/clk.h>
36
37 #include <arm64/rockchip/clk/rk_clk_fract.h>
38
39 #include "clkdev_if.h"
40
41 #define WR4(_clk, off, val) \
42 CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
43 #define RD4(_clk, off, val) \
44 CLKDEV_READ_4(clknode_get_device(_clk), off, val)
45 #define MD4(_clk, off, clr, set ) \
46 CLKDEV_MODIFY_4(clknode_get_device(_clk), off, clr, set)
47 #define DEVICE_LOCK(_clk) \
48 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
49 #define DEVICE_UNLOCK(_clk) \
50 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
51
52 #define RK_CLK_FRACT_MASK_SHIFT 16
53
54 static int rk_clk_fract_init(struct clknode *clk, device_t dev);
55 static int rk_clk_fract_recalc(struct clknode *clk, uint64_t *req);
56 static int rk_clk_fract_set_freq(struct clknode *clknode, uint64_t fin,
57 uint64_t *fout, int flag, int *stop);
58 static int rk_clk_fract_set_gate(struct clknode *clk, bool enable);
59
60 struct rk_clk_fract_sc {
61 uint32_t flags;
62 uint32_t offset;
63 uint32_t numerator;
64 uint32_t denominator;
65 uint32_t gate_offset;
66 uint32_t gate_shift;
67 };
68
69 static clknode_method_t rk_clk_fract_methods[] = {
70 /* Device interface */
71 CLKNODEMETHOD(clknode_init, rk_clk_fract_init),
72 CLKNODEMETHOD(clknode_set_gate, rk_clk_fract_set_gate),
73 CLKNODEMETHOD(clknode_recalc_freq, rk_clk_fract_recalc),
74 CLKNODEMETHOD(clknode_set_freq, rk_clk_fract_set_freq),
75 CLKNODEMETHOD_END
76 };
77 DEFINE_CLASS_1(rk_clk_fract, rk_clk_fract_class, rk_clk_fract_methods,
78 sizeof(struct rk_clk_fract_sc), clknode_class);
79
80 /*
81 * Compute best rational approximation of input fraction
82 * for fixed sized fractional divider registers.
83 * http://en.wikipedia.org/wiki/Continued_fraction
84 *
85 * - n_input, d_input Given input fraction
86 * - n_max, d_max Maximum vaues of divider registers
87 * - n_out, d_out Computed approximation
88 */
89
90 static void
clk_compute_fract_div(uint64_t n_input,uint64_t d_input,uint64_t n_max,uint64_t d_max,uint64_t * n_out,uint64_t * d_out)91 clk_compute_fract_div(
92 uint64_t n_input, uint64_t d_input,
93 uint64_t n_max, uint64_t d_max,
94 uint64_t *n_out, uint64_t *d_out)
95 {
96 uint64_t n_prev, d_prev; /* previous convergents */
97 uint64_t n_cur, d_cur; /* current convergents */
98 uint64_t n_rem, d_rem; /* fraction remainder */
99 uint64_t tmp, fact;
100
101 /* Initialize fraction reminder */
102 n_rem = n_input;
103 d_rem = d_input;
104
105 /* Init convergents to 0/1 and 1/0 */
106 n_prev = 0;
107 d_prev = 1;
108 n_cur = 1;
109 d_cur = 0;
110
111 while (d_rem != 0 && n_cur < n_max && d_cur < d_max) {
112 /* Factor for this step. */
113 fact = n_rem / d_rem;
114
115 /* Adjust fraction reminder */
116 tmp = d_rem;
117 d_rem = n_rem % d_rem;
118 n_rem = tmp;
119
120 /* Compute new nominator and save last one */
121 tmp = n_prev + fact * n_cur;
122 n_prev = n_cur;
123 n_cur = tmp;
124
125 /* Compute new denominator and save last one */
126 tmp = d_prev + fact * d_cur;
127 d_prev = d_cur;
128 d_cur = tmp;
129 }
130
131 if (n_cur > n_max || d_cur > d_max) {
132 *n_out = n_prev;
133 *d_out = d_prev;
134 } else {
135 *n_out = n_cur;
136 *d_out = d_cur;
137 }
138 }
139
140 static int
rk_clk_fract_init(struct clknode * clk,device_t dev)141 rk_clk_fract_init(struct clknode *clk, device_t dev)
142 {
143 uint32_t reg;
144 struct rk_clk_fract_sc *sc;
145
146 sc = clknode_get_softc(clk);
147 DEVICE_LOCK(clk);
148 RD4(clk, sc->offset, ®);
149 DEVICE_UNLOCK(clk);
150
151 sc->numerator = (reg >> 16) & 0xFFFF;
152 sc->denominator = reg & 0xFFFF;
153 clknode_init_parent_idx(clk, 0);
154
155 return(0);
156 }
157
158 static int
rk_clk_fract_set_gate(struct clknode * clk,bool enable)159 rk_clk_fract_set_gate(struct clknode *clk, bool enable)
160 {
161 struct rk_clk_fract_sc *sc;
162 uint32_t val = 0;
163
164 sc = clknode_get_softc(clk);
165
166 if ((sc->flags & RK_CLK_FRACT_HAVE_GATE) == 0)
167 return (0);
168
169 RD4(clk, sc->gate_offset, &val);
170
171 val = 0;
172 if (!enable)
173 val |= 1 << sc->gate_shift;
174 val |= (1 << sc->gate_shift) << RK_CLK_FRACT_MASK_SHIFT;
175 DEVICE_LOCK(clk);
176 WR4(clk, sc->gate_offset, val);
177 DEVICE_UNLOCK(clk);
178
179 return (0);
180 }
181
182 static int
183 rk_clk_fract_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
184 int flags, int *stop);
185 static int
rk_clk_fract_recalc(struct clknode * clk,uint64_t * freq)186 rk_clk_fract_recalc(struct clknode *clk, uint64_t *freq)
187 {
188 struct rk_clk_fract_sc *sc;
189
190 sc = clknode_get_softc(clk);
191 if (sc->denominator == 0) {
192 printf("%s: %s denominator is zero!\n", clknode_get_name(clk),
193 __func__);
194 *freq = 0;
195 return(EINVAL);
196 }
197
198 *freq *= sc->numerator;
199 *freq /= sc->denominator;
200
201 return (0);
202 }
203
204 static int
rk_clk_fract_set_freq(struct clknode * clk,uint64_t fin,uint64_t * fout,int flags,int * stop)205 rk_clk_fract_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
206 int flags, int *stop)
207 {
208 struct rk_clk_fract_sc *sc;
209 uint64_t div_n, div_d, _fout;
210
211 sc = clknode_get_softc(clk);
212
213 clk_compute_fract_div(*fout, fin, 0xFFFF, 0xFFFF, &div_n, &div_d);
214 _fout = fin * div_n;
215 _fout /= div_d;
216
217 /* Rounding. */
218 if ((flags & CLK_SET_ROUND_UP) && (_fout < *fout)) {
219 if (div_n > div_d && div_d > 1)
220 div_n++;
221 else
222 div_d--;
223 } else if ((flags & CLK_SET_ROUND_DOWN) && (_fout > *fout)) {
224 if (div_n > div_d && div_n > 1)
225 div_n--;
226 else
227 div_d++;
228 }
229
230 /* Check range after rounding */
231 if (div_n > 0xFFFF || div_d > 0xFFFF)
232 return (ERANGE);
233
234 if (div_d == 0) {
235 printf("%s: %s divider is zero!\n",
236 clknode_get_name(clk), __func__);
237 return(EINVAL);
238 }
239 /* Recompute final output frequency */
240 _fout = fin * div_n;
241 _fout /= div_d;
242
243 *stop = 1;
244
245 if ((flags & CLK_SET_DRYRUN) == 0) {
246 if (*stop != 0 &&
247 (flags & (CLK_SET_ROUND_UP | CLK_SET_ROUND_DOWN)) == 0 &&
248 *fout != _fout)
249 return (ERANGE);
250
251 sc->numerator = (uint32_t)div_n;
252 sc->denominator = (uint32_t)div_d;
253
254 DEVICE_LOCK(clk);
255 WR4(clk, sc->offset, sc->numerator << 16 | sc->denominator);
256 DEVICE_UNLOCK(clk);
257 }
258
259 *fout = _fout;
260 return (0);
261 }
262
263 int
rk_clk_fract_register(struct clkdom * clkdom,struct rk_clk_fract_def * clkdef)264 rk_clk_fract_register(struct clkdom *clkdom, struct rk_clk_fract_def *clkdef)
265 {
266 struct clknode *clk;
267 struct rk_clk_fract_sc *sc;
268
269 clk = clknode_create(clkdom, &rk_clk_fract_class, &clkdef->clkdef);
270 if (clk == NULL)
271 return (1);
272
273 sc = clknode_get_softc(clk);
274 sc->flags = clkdef->flags;
275 sc->offset = clkdef->offset;
276 sc->gate_offset = clkdef->gate_offset;
277 sc->gate_shift = clkdef->gate_shift;
278
279 clknode_register(clkdom, clk);
280 return (0);
281 }
282