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