sys/arm/allwinner/clkng/aw_clk_nkmp.c

   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
  50 struct 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
  79 static int
  80 aw_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
 100 static int
 101 aw_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
 123 static int
 124 aw_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 >> sc->mux_shift);
 137         val |= index << sc->mux_shift;
 138         WRITE4(clk, sc->offset, val);
 139         DEVICE_UNLOCK(clk);
 140
 141         return (0);
 142 }
 143
 144 static uint64_t
 145 aw_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
 195 static void
 196 aw_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
 257 static int
 258 aw_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
 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                 }
 311
 312                 if ((sc->flags & AW_CLK_HAS_LOCK) != 0) {
 313                         for (retry = 0; retry < sc->lock_retries; retry++) {
 314                                 READ4(clk, sc->offset, &val);
 315                                 if ((val & (1 << sc->lock_shift)) != 0)
 316                                         break;
 317                                 DELAY(1000);
 318                         }
 319                 }
 320         }
 321
 322         *fout = best;
 323         *stop = 1;
 324
 325         return (0);
 326 }
 327
 328 static int
 329 aw_clk_nkmp_recalc(struct clknode *clk, uint64_t *freq)
 330 {
 331         struct aw_clk_nkmp_sc *sc;
 332         uint32_t val, m, n, k, p;
 333
 334         sc = clknode_get_softc(clk);
 335
 336         DEVICE_LOCK(clk);
 337         READ4(clk, sc->offset, &val);
 338         DEVICE_UNLOCK(clk);
 339
 340         n = aw_clk_get_factor(val, &sc->n);
 341         k = aw_clk_get_factor(val, &sc->k);
 342         m = aw_clk_get_factor(val, &sc->m);
 343         p = aw_clk_get_factor(val, &sc->p);
 344
 345         *freq = (*freq * n * k) / (m * p);
 346
 347         return (0);
 348 }
 349
 350 static clknode_method_t aw_nkmp_clknode_methods[] = {
 351         /* Device interface */
 352         CLKNODEMETHOD(clknode_init,             aw_clk_nkmp_init),
 353         CLKNODEMETHOD(clknode_set_gate,         aw_clk_nkmp_set_gate),
 354         CLKNODEMETHOD(clknode_set_mux,          aw_clk_nkmp_set_mux),
 355         CLKNODEMETHOD(clknode_recalc_freq,      aw_clk_nkmp_recalc),
 356         CLKNODEMETHOD(clknode_set_freq,         aw_clk_nkmp_set_freq),
 357         CLKNODEMETHOD_END
 358 };
 359
 360 DEFINE_CLASS_1(aw_nkmp_clknode, aw_nkmp_clknode_class, aw_nkmp_clknode_methods,
 361     sizeof(struct aw_clk_nkmp_sc), clknode_class);
 362
 363 int
 364 aw_clk_nkmp_register(struct clkdom *clkdom, struct aw_clk_nkmp_def *clkdef)
 365 {
 366         struct clknode *clk;
 367         struct aw_clk_nkmp_sc *sc;
 368
 369         clk = clknode_create(clkdom, &aw_nkmp_clknode_class, &clkdef->clkdef);
 370         if (clk == NULL)
 371                 return (1);
 372
 373         sc = clknode_get_softc(clk);
 374
 375         sc->offset = clkdef->offset;
 376
 377         sc->n.shift = clkdef->n.shift;
 378         sc->n.width = clkdef->n.width;
 379         sc->n.mask = ((1 << clkdef->n.width) - 1) << sc->n.shift;
 380         sc->n.value = clkdef->n.value;
 381         sc->n.flags = clkdef->n.flags;
 382
 383         sc->k.shift = clkdef->k.shift;
 384         sc->k.width = clkdef->k.width;
 385         sc->k.mask = ((1 << clkdef->k.width) - 1) << sc->k.shift;
 386         sc->k.value = clkdef->k.value;
 387         sc->k.flags = clkdef->k.flags;
 388
 389         sc->m.shift = clkdef->m.shift;
 390         sc->m.width = clkdef->m.width;
 391         sc->m.mask = ((1 << clkdef->m.width) - 1) << sc->m.shift;
 392         sc->m.value = clkdef->m.value;
 393         sc->m.flags = clkdef->m.flags;
 394
 395         sc->p.shift = clkdef->p.shift;
 396         sc->p.width = clkdef->p.width;
 397         sc->p.mask = ((1 << clkdef->p.width) - 1) << sc->p.shift;
 398         sc->p.value = clkdef->p.value;
 399         sc->p.flags = clkdef->p.flags;
 400
 401         sc->mux_shift = clkdef->mux_shift;
 402         sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
 403
 404         sc->gate_shift = clkdef->gate_shift;
 405         sc->lock_shift = clkdef->lock_shift;
 406         sc->lock_retries = clkdef->lock_retries;
 407         sc->update_shift = clkdef->update_shift;
 408         sc->flags = clkdef->flags;
 409
 410         clknode_register(clkdom, clk);
 411
 412         return (0);
 413 }