sys/arm/allwinner/clkng/aw_clk_nkmp.c

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