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