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/extres/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 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 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 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 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 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 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