1/*- 2 * Copyright (c) 2017 Emmanuel Vadot <manu@freebsd.org> 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions 7 * are met: 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 15 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 16 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 17 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 18 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 19 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 21 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 22 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24 * SUCH DAMAGE. 25 * 26 * $FreeBSD$ 27 */ 28 29#include <sys/cdefs.h> 30__FBSDID("$FreeBSD$"); 31 32#include <sys/param.h> 33#include <sys/systm.h> 34#include <sys/bus.h> 35 36#include <dev/extres/clk/clk.h> 37 38#include <arm/allwinner/clkng/aw_clk.h> 39#include <arm/allwinner/clkng/aw_clk_nkmp.h> 40 41#include "clkdev_if.h" 42 43/* 44 * clknode for clocks matching the formula : 45 * 46 * clk = (clkin * n * k) / (m * p) 47 * 48 */ 49 50struct aw_clk_nkmp_sc { 51 uint32_t offset; 52 53 struct aw_clk_factor n; 54 struct aw_clk_factor k; 55 struct aw_clk_factor m; 56 struct aw_clk_factor p; 57 58 uint32_t mux_shift; 59 uint32_t mux_mask; 60 uint32_t gate_shift; 61 uint32_t lock_shift; 62 uint32_t lock_retries; 63 uint32_t update_shift; 64 65 uint32_t flags; 66}; 67 68#define WRITE4(_clk, off, val) \ 69 CLKDEV_WRITE_4(clknode_get_device(_clk), off, val) 70#define READ4(_clk, off, val) \ 71 CLKDEV_READ_4(clknode_get_device(_clk), off, val) 72#define MODIFY4(_clk, off, clr, set ) \ 73 CLKDEV_MODIFY_4(clknode_get_device(_clk), off, clr, set) 74#define DEVICE_LOCK(_clk) \ 75 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk)) 76#define DEVICE_UNLOCK(_clk) \ 77 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk)) 78 79static int 80aw_clk_nkmp_init(struct clknode *clk, device_t dev) 81{ 82 struct aw_clk_nkmp_sc *sc; 83 uint32_t val, idx; 84 85 sc = clknode_get_softc(clk); 86 87 idx = 0; 88 if ((sc->flags & AW_CLK_HAS_MUX) != 0) { 89 DEVICE_LOCK(clk); 90 READ4(clk, sc->offset, &val); 91 DEVICE_UNLOCK(clk); 92 93 idx = (val & sc->mux_mask) >> sc->mux_shift; 94 } 95 96 clknode_init_parent_idx(clk, idx); 97 return (0); 98} 99 100static int 101aw_clk_nkmp_set_gate(struct clknode *clk, bool enable) 102{ 103 struct aw_clk_nkmp_sc *sc; 104 uint32_t val; 105 106 sc = clknode_get_softc(clk); 107 108 if ((sc->flags & AW_CLK_HAS_GATE) == 0) 109 return (0); 110 111 DEVICE_LOCK(clk); 112 READ4(clk, sc->offset, &val); 113 if (enable) 114 val |= (1 << sc->gate_shift); 115 else 116 val &= ~(1 << sc->gate_shift); 117 WRITE4(clk, sc->offset, val); 118 DEVICE_UNLOCK(clk); 119 120 return (0); 121} 122 123static int 124aw_clk_nkmp_set_mux(struct clknode *clk, int index) 125{ 126 struct aw_clk_nkmp_sc *sc; 127 uint32_t val; 128 129 sc = clknode_get_softc(clk); 130 131 if ((sc->flags & AW_CLK_HAS_MUX) == 0) 132 return (0); 133 134 DEVICE_LOCK(clk); 135 READ4(clk, sc->offset, &val); 136 val &= ~sc->mux_mask; 137 val |= index << sc->mux_shift; 138 WRITE4(clk, sc->offset, val); 139 DEVICE_UNLOCK(clk); 140 141 return (0); 142} 143 144static uint64_t 145aw_clk_nkmp_find_best(struct aw_clk_nkmp_sc *sc, uint64_t fparent, uint64_t *fout, 146 uint32_t *factor_n, uint32_t *factor_k, uint32_t *factor_m, uint32_t *factor_p) 147{ 148 uint64_t cur, best; 149 uint32_t n, k, m, p; 150 151 best = 0; 152 *factor_n = 0; 153 *factor_k = 0; 154 *factor_m = 0; 155 *factor_p = 0; 156 157 for (n = aw_clk_factor_get_min(&sc->n); n <= aw_clk_factor_get_max(&sc->n); ) { 158 for (k = aw_clk_factor_get_min(&sc->k); k <= aw_clk_factor_get_max(&sc->k); ) { 159 for (m = aw_clk_factor_get_min(&sc->m); m <= aw_clk_factor_get_max(&sc->m); ) { 160 for (p = aw_clk_factor_get_min(&sc->p); p <= aw_clk_factor_get_max(&sc->p); ) { 161 cur = (fparent * n * k) / (m * p); 162 if ((*fout - cur) < (*fout - best)) { 163 best = cur; 164 *factor_n = n; 165 *factor_k = k; 166 *factor_m = m; 167 *factor_p = p; 168 } 169 if (best == *fout) 170 return (best); 171 if ((sc->p.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0) 172 p <<= 1; 173 else 174 p++; 175 } 176 if ((sc->m.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0) 177 m <<= 1; 178 else 179 m++; 180 } 181 if ((sc->k.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0) 182 k <<= 1; 183 else 184 k++; 185 } 186 if ((sc->n.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0) 187 n <<= 1; 188 else 189 n++; 190 } 191 192 return best; 193} 194 195static void 196aw_clk_nkmp_set_freq_scale(struct clknode *clk, struct aw_clk_nkmp_sc *sc, 197 uint32_t factor_n, uint32_t factor_k, uint32_t factor_m, uint32_t factor_p) 198{ 199 uint32_t val, n, k, m, p; 200 int retry; 201 202 DEVICE_LOCK(clk); 203 READ4(clk, sc->offset, &val); 204 205 n = aw_clk_get_factor(val, &sc->n); 206 k = aw_clk_get_factor(val, &sc->k); 207 m = aw_clk_get_factor(val, &sc->m); 208 p = aw_clk_get_factor(val, &sc->p); 209 210 if (p < factor_p) { 211 val &= ~sc->p.mask; 212 val |= aw_clk_factor_get_value(&sc->p, factor_p) << sc->p.shift; 213 WRITE4(clk, sc->offset, val); 214 DELAY(2000); 215 } 216 217 if (m < factor_m) { 218 val &= ~sc->m.mask; 219 val |= aw_clk_factor_get_value(&sc->m, factor_m) << sc->m.shift; 220 WRITE4(clk, sc->offset, val); 221 DELAY(2000); 222 } 223 224 val &= ~sc->n.mask; 225 val &= ~sc->k.mask; 226 val |= aw_clk_factor_get_value(&sc->n, factor_n) << sc->n.shift; 227 val |= aw_clk_factor_get_value(&sc->k, factor_k) << sc->k.shift; 228 WRITE4(clk, sc->offset, val); 229 DELAY(2000); 230 231 if (m > factor_m) { 232 val &= ~sc->m.mask; 233 val |= aw_clk_factor_get_value(&sc->m, factor_m) << sc->m.shift; 234 WRITE4(clk, sc->offset, val); 235 DELAY(2000); 236 } 237 238 if (p > factor_p) { 239 val &= ~sc->p.mask; 240 val |= aw_clk_factor_get_value(&sc->p, factor_p) << sc->p.shift; 241 WRITE4(clk, sc->offset, val); 242 DELAY(2000); 243 } 244 245 if ((sc->flags & AW_CLK_HAS_LOCK) != 0) { 246 for (retry = 0; retry < sc->lock_retries; retry++) { 247 READ4(clk, sc->offset, &val); 248 if ((val & (1 << sc->lock_shift)) != 0) 249 break; 250 DELAY(1000); 251 } 252 } 253 254 DEVICE_UNLOCK(clk); 255} 256 257static int 258aw_clk_nkmp_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout, 259 int flags, int *stop) 260{ 261 struct aw_clk_nkmp_sc *sc; 262 uint64_t best; 263 uint32_t val, best_n, best_k, best_m, best_p; 264 int retry; 265 266 sc = clknode_get_softc(clk); 267 268 best = aw_clk_nkmp_find_best(sc, fparent, fout, 269 &best_n, &best_k, &best_m, &best_p); 270 if ((flags & CLK_SET_DRYRUN) != 0) { 271 *fout = best; 272 *stop = 1; 273 return (0); 274 } 275 276 if ((best < *fout) && 277 ((flags & CLK_SET_ROUND_DOWN) != 0)) { 278 *stop = 1; 279 return (ERANGE); 280 } 281 if ((best > *fout) && 282 ((flags & CLK_SET_ROUND_UP) != 0)) { 283 *stop = 1; 284 return (ERANGE); 285 } 286 287 if ((sc->flags & AW_CLK_SCALE_CHANGE) != 0) 288 aw_clk_nkmp_set_freq_scale(clk, sc, 289 best_n, best_k, best_m, best_p); 290 else { 291 DEVICE_LOCK(clk); 292 READ4(clk, sc->offset, &val); 293 val &= ~sc->n.mask; 294 val &= ~sc->k.mask; 295 val &= ~sc->m.mask; 296 val &= ~sc->p.mask; 297 val |= aw_clk_factor_get_value(&sc->n, best_n) << sc->n.shift; 298 val |= aw_clk_factor_get_value(&sc->k, best_k) << sc->k.shift; 299 val |= aw_clk_factor_get_value(&sc->m, best_m) << sc->m.shift; 300 val |= aw_clk_factor_get_value(&sc->p, best_p) << sc->p.shift; 301 WRITE4(clk, sc->offset, val); 302 DELAY(2000); 303 DEVICE_UNLOCK(clk); 304 305 if ((sc->flags & AW_CLK_HAS_UPDATE) != 0) { 306 DEVICE_LOCK(clk); 307 READ4(clk, sc->offset, &val); 308 val |= 1 << sc->update_shift; 309 WRITE4(clk, sc->offset, val); 310 DELAY(2000); 311 DEVICE_UNLOCK(clk); 312 } 313 314 if ((sc->flags & AW_CLK_HAS_LOCK) != 0) { 315 for (retry = 0; retry < sc->lock_retries; retry++) { 316 READ4(clk, sc->offset, &val); 317 if ((val & (1 << sc->lock_shift)) != 0) 318 break; 319 DELAY(1000); 320 } 321 } 322 } 323 324 *fout = best; 325 *stop = 1; 326 327 return (0); 328} 329 330static int 331aw_clk_nkmp_recalc(struct clknode *clk, uint64_t *freq) 332{ 333 struct aw_clk_nkmp_sc *sc; 334 uint32_t val, m, n, k, p; 335 336 sc = clknode_get_softc(clk); 337 338 DEVICE_LOCK(clk); 339 READ4(clk, sc->offset, &val); 340 DEVICE_UNLOCK(clk); 341 342 n = aw_clk_get_factor(val, &sc->n); 343 k = aw_clk_get_factor(val, &sc->k); 344 m = aw_clk_get_factor(val, &sc->m); 345 p = aw_clk_get_factor(val, &sc->p); 346 347 *freq = (*freq * n * k) / (m * p); 348 349 return (0); 350} 351 352static clknode_method_t aw_nkmp_clknode_methods[] = { 353 /* Device interface */ 354 CLKNODEMETHOD(clknode_init, aw_clk_nkmp_init), 355 CLKNODEMETHOD(clknode_set_gate, aw_clk_nkmp_set_gate), 356 CLKNODEMETHOD(clknode_set_mux, aw_clk_nkmp_set_mux), 357 CLKNODEMETHOD(clknode_recalc_freq, aw_clk_nkmp_recalc), 358 CLKNODEMETHOD(clknode_set_freq, aw_clk_nkmp_set_freq), 359 CLKNODEMETHOD_END 360}; 361 362DEFINE_CLASS_1(aw_nkmp_clknode, aw_nkmp_clknode_class, aw_nkmp_clknode_methods, 363 sizeof(struct aw_clk_nkmp_sc), clknode_class); 364 365int 366aw_clk_nkmp_register(struct clkdom *clkdom, struct aw_clk_nkmp_def *clkdef) 367{ 368 struct clknode *clk; 369 struct aw_clk_nkmp_sc *sc; 370 371 clk = clknode_create(clkdom, &aw_nkmp_clknode_class, &clkdef->clkdef); 372 if (clk == NULL) 373 return (1); 374 375 sc = clknode_get_softc(clk); 376 377 sc->offset = clkdef->offset; 378 379 sc->n.shift = clkdef->n.shift; 380 sc->n.width = clkdef->n.width; 381 sc->n.mask = ((1 << clkdef->n.width) - 1) << sc->n.shift; 382 sc->n.value = clkdef->n.value; 383 sc->n.flags = clkdef->n.flags; 384 385 sc->k.shift = clkdef->k.shift; 386 sc->k.width = clkdef->k.width; 387 sc->k.mask = ((1 << clkdef->k.width) - 1) << sc->k.shift; 388 sc->k.value = clkdef->k.value; 389 sc->k.flags = clkdef->k.flags; 390 391 sc->m.shift = clkdef->m.shift; 392 sc->m.width = clkdef->m.width; 393 sc->m.mask = ((1 << clkdef->m.width) - 1) << sc->m.shift; 394 sc->m.value = clkdef->m.value; 395 sc->m.flags = clkdef->m.flags; 396 397 sc->p.shift = clkdef->p.shift; 398 sc->p.width = clkdef->p.width; 399 sc->p.mask = ((1 << clkdef->p.width) - 1) << sc->p.shift; 400 sc->p.value = clkdef->p.value; 401 sc->p.flags = clkdef->p.flags; 402 403 sc->mux_shift = clkdef->mux_shift; 404 sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift; 405 406 sc->gate_shift = clkdef->gate_shift; 407 sc->lock_shift = clkdef->lock_shift; 408 sc->lock_retries = clkdef->lock_retries; 409 sc->update_shift = clkdef->update_shift; 410 sc->flags = clkdef->flags; 411 412 clknode_register(clkdom, clk); 413 414 return (0); 415} 416