sys/arm/allwinner/clkng/aw_clk_frac.c

   1 /*-
   2  * Copyright (c) 2019 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_frac.h>
  39
  40 #include "clkdev_if.h"
  41
  42 /* #define      dprintf(format, arg...) printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg) */
  43 #define dprintf(format, arg...)
  44
  45 /*
  46  * clknode for clocks matching the formula :
  47  *
  48  * clk = (24Mhz * n) / m in integer mode
  49  * clk = frac_out1 or frac_out2 in fractional mode
  50  *
  51  */
  52
  53 struct aw_clk_frac_sc {
  54         uint32_t        offset;
  55
  56         struct aw_clk_factor    m;
  57         struct aw_clk_factor    n;
  58         struct aw_clk_frac      frac;
  59
  60         uint64_t                min_freq;
  61         uint64_t                max_freq;
  62
  63         uint32_t        mux_shift;
  64         uint32_t        mux_mask;
  65         uint32_t        gate_shift;
  66         uint32_t        lock_shift;
  67         uint32_t        lock_retries;
  68
  69         uint32_t        flags;
  70 };
  71
  72 #define WRITE4(_clk, off, val)                                          \
  73         CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
  74 #define READ4(_clk, off, val)                                           \
  75         CLKDEV_READ_4(clknode_get_device(_clk), off, val)
  76 #define DEVICE_LOCK(_clk)                                                       \
  77         CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
  78 #define DEVICE_UNLOCK(_clk)                                             \
  79         CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
  80
  81 static int
  82 aw_clk_frac_init(struct clknode *clk, device_t dev)
  83 {
  84         struct aw_clk_frac_sc *sc;
  85         uint32_t val, idx;
  86
  87         sc = clknode_get_softc(clk);
  88
  89         idx = 0;
  90         if ((sc->flags & AW_CLK_HAS_MUX) != 0) {
  91                 DEVICE_LOCK(clk);
  92                 READ4(clk, sc->offset, &val);
  93                 DEVICE_UNLOCK(clk);
  94
  95                 idx = (val & sc->mux_mask) >> sc->mux_shift;
  96         }
  97
  98         dprintf("init parent idx %d\n", idx);
  99         clknode_init_parent_idx(clk, idx);
 100         return (0);
 101 }
 102
 103 static int
 104 aw_clk_frac_set_gate(struct clknode *clk, bool enable)
 105 {
 106         struct aw_clk_frac_sc *sc;
 107         uint32_t val;
 108
 109         sc = clknode_get_softc(clk);
 110
 111         if ((sc->flags & AW_CLK_HAS_GATE) == 0)
 112                 return (0);
 113
 114         dprintf("%sabling gate\n", enable ? "En" : "Dis");
 115         DEVICE_LOCK(clk);
 116         READ4(clk, sc->offset, &val);
 117         if (enable)
 118                 val |= (1 << sc->gate_shift);
 119         else
 120                 val &= ~(1 << sc->gate_shift);
 121         WRITE4(clk, sc->offset, val);
 122         DEVICE_UNLOCK(clk);
 123
 124         return (0);
 125 }
 126
 127 static int
 128 aw_clk_frac_set_mux(struct clknode *clk, int index)
 129 {
 130         struct aw_clk_frac_sc *sc;
 131         uint32_t val;
 132
 133         sc = clknode_get_softc(clk);
 134
 135         if ((sc->flags & AW_CLK_HAS_MUX) == 0)
 136                 return (0);
 137
 138         dprintf("Set mux to %d\n", index);
 139         DEVICE_LOCK(clk);
 140         READ4(clk, sc->offset, &val);
 141         val &= ~sc->mux_mask;
 142         val |= index << sc->mux_shift;
 143         WRITE4(clk, sc->offset, val);
 144         DEVICE_UNLOCK(clk);
 145
 146         return (0);
 147 }
 148
 149 static uint64_t
 150 aw_clk_frac_find_best(struct aw_clk_frac_sc *sc, uint64_t fparent, uint64_t fout,
 151     uint32_t *factor_n, uint32_t *factor_m)
 152 {
 153         uint64_t cur, best;
 154         uint32_t m, n, max_m, max_n, min_m, min_n;
 155
 156         *factor_n = *factor_m = 0;
 157         best = cur = 0;
 158
 159         max_m = aw_clk_factor_get_max(&sc->m);
 160         max_n = aw_clk_factor_get_max(&sc->n);
 161         min_m = aw_clk_factor_get_min(&sc->m);
 162         min_n = sc->min_freq / fparent;
 163
 164         for (n = min_n; n <= max_n; n++) {
 165                 for (m = min_m; m <= max_m; m++) {
 166                         cur = fparent * n / m;
 167                         if (cur < sc->min_freq) {
 168                                 continue;
 169                         }
 170                         if (cur > sc->max_freq) {
 171                                 continue;
 172                         }
 173                         if (cur == fout) {
 174                                 *factor_n = n;
 175                                 *factor_m = m;
 176                                 return (cur);
 177                         }
 178                         if (abs((fout - cur)) < abs((fout - best))) {
 179                                 best = cur;
 180                                 *factor_n = n;
 181                                 *factor_m = m;
 182                         }
 183                 }
 184         }
 185
 186         return (best);
 187 }
 188
 189 static int
 190 aw_clk_frac_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
 191     int flags, int *stop)
 192 {
 193         struct aw_clk_frac_sc *sc;
 194         uint64_t cur, best, best_frac;
 195         uint32_t val, m, n, best_m, best_n;
 196         int retry, multiple, max_mult, best_mult;
 197
 198         sc = clknode_get_softc(clk);
 199
 200         best = best_frac = cur = 0;
 201         best_mult = 0;
 202         max_mult = 1;
 203
 204         dprintf("Trying to find freq %ju with parent %ju\n", *fout, fparent);
 205         if ((flags & CLK_SET_ROUND_MULTIPLE) != 0)
 206                 max_mult = 10;
 207         for (multiple = 1; multiple <= max_mult; multiple++) {
 208                 /* First test the fractional frequencies */
 209                 dprintf("Testing with multiple %d\n", multiple);
 210                 if (*fout * multiple == sc->frac.freq0) {
 211                         best = best_frac = sc->frac.freq0;
 212                         best_mult = multiple;
 213                         dprintf("Found with using frac.freq0 and multiple %d\n", multiple);
 214                         break;
 215                 }
 216                 else if (*fout * multiple == sc->frac.freq1) {
 217                         best = best_frac = sc->frac.freq1;
 218                         best_mult = multiple;
 219                         dprintf("Found with using frac.freq1 and multiple %d\n", multiple);
 220                         break;
 221                 }
 222                 else {
 223                         cur = aw_clk_frac_find_best(sc, fparent, *fout * multiple,
 224                           &n, &m);
 225                         dprintf("Got %ju with n=%d, m=%d\n", cur, n, m);
 226                         if (cur == (*fout * multiple)) {
 227                                 best = cur;
 228                                 best_mult = multiple;
 229                                 best_n = n;
 230                                 best_m = m;
 231                                 dprintf("This is the one: n=%d m=%d mult=%d\n", best_n, best_m, best_mult);
 232                                 break;
 233                         }
 234                         if (abs(((*fout * multiple) - cur)) < abs(((*fout * multiple) - best))) {
 235                                 best = cur;
 236                                 best_mult = multiple;
 237                                 best_n = n;
 238                                 best_m = m;
 239                                 dprintf("This is the best for now: n=%d m=%d mult=%d\n", best_n, best_m, best_mult);
 240                         }
 241                 }
 242         }
 243
 244         if (best < sc->min_freq ||
 245             best > sc->max_freq) {
 246                 printf("%s: Cannot set %ju for %s (min=%ju max=%ju)\n",
 247                     __func__, best, clknode_get_name(clk),
 248                     sc->min_freq, sc->max_freq);
 249                 return (ERANGE);
 250         }
 251         if ((flags & CLK_SET_DRYRUN) != 0) {
 252                 *fout = best;
 253                 *stop = 1;
 254                 return (0);
 255         }
 256
 257         if ((best < (*fout * best_mult)) &&
 258           ((flags & CLK_SET_ROUND_DOWN) == 0)) {
 259                 *stop = 1;
 260                 return (ERANGE);
 261         }
 262         if ((best > *fout * best_mult) &&
 263           ((flags & CLK_SET_ROUND_UP) == 0)) {
 264                 *stop = 1;
 265                 return (ERANGE);
 266         }
 267
 268         DEVICE_LOCK(clk);
 269         READ4(clk, sc->offset, &val);
 270         /* Disable clock during freq changes */
 271         val &= ~(1 << sc->gate_shift);
 272         WRITE4(clk, sc->offset, val);
 273
 274         if (best_frac != 0) {
 275                 val &= ~sc->frac.mode_sel;
 276                 /* M should be 0 per the manual */
 277                 val &= ~sc->m.mask;
 278                 if (best_frac == sc->frac.freq0)
 279                         val &= ~sc->frac.freq_sel;
 280                 else
 281                         val |= sc->frac.freq_sel;
 282         } else {
 283                 val |= sc->frac.mode_sel; /* Select integer mode */
 284                 n = aw_clk_factor_get_value(&sc->n, best_n);
 285                 m = aw_clk_factor_get_value(&sc->m, best_m);
 286                 val &= ~sc->n.mask;
 287                 val &= ~sc->m.mask;
 288                 val |= n << sc->n.shift;
 289                 val |= m << sc->m.shift;
 290         }
 291
 292         /* Write the clock changes */
 293         WRITE4(clk, sc->offset, val);
 294
 295         /* Enable clock now that we've change it */
 296         val |= 1 << sc->gate_shift;
 297         WRITE4(clk, sc->offset, val);
 298         DEVICE_UNLOCK(clk);
 299
 300         for (retry = 0; retry < sc->lock_retries; retry++) {
 301                 READ4(clk, sc->offset, &val);
 302                 if ((val & (1 << sc->lock_shift)) != 0)
 303                         break;
 304                 DELAY(1000);
 305         }
 306
 307         *fout = best;
 308         *stop = 1;
 309
 310         return (0);
 311 }
 312
 313 static int
 314 aw_clk_frac_recalc(struct clknode *clk, uint64_t *freq)
 315 {
 316         struct aw_clk_frac_sc *sc;
 317         uint32_t val, m, n;
 318
 319         sc = clknode_get_softc(clk);
 320
 321         DEVICE_LOCK(clk);
 322         READ4(clk, sc->offset, &val);
 323         DEVICE_UNLOCK(clk);
 324
 325         if ((val & sc->frac.mode_sel) == 0) {
 326                 if (val & sc->frac.freq_sel)
 327                         *freq = sc->frac.freq1;
 328                 else
 329                         *freq = sc->frac.freq0;
 330         } else {
 331                 m = aw_clk_get_factor(val, &sc->m);
 332                 n = aw_clk_get_factor(val, &sc->n);
 333                 *freq = *freq * n / m;
 334         }
 335
 336         return (0);
 337 }
 338
 339 static clknode_method_t aw_frac_clknode_methods[] = {
 340         /* Device interface */
 341         CLKNODEMETHOD(clknode_init,             aw_clk_frac_init),
 342         CLKNODEMETHOD(clknode_set_gate,         aw_clk_frac_set_gate),
 343         CLKNODEMETHOD(clknode_set_mux,          aw_clk_frac_set_mux),
 344         CLKNODEMETHOD(clknode_recalc_freq,      aw_clk_frac_recalc),
 345         CLKNODEMETHOD(clknode_set_freq,         aw_clk_frac_set_freq),
 346         CLKNODEMETHOD_END
 347 };
 348
 349 DEFINE_CLASS_1(aw_frac_clknode, aw_frac_clknode_class, aw_frac_clknode_methods,
 350     sizeof(struct aw_clk_frac_sc), clknode_class);
 351
 352 int
 353 aw_clk_frac_register(struct clkdom *clkdom, struct aw_clk_frac_def *clkdef)
 354 {
 355         struct clknode *clk;
 356         struct aw_clk_frac_sc *sc;
 357
 358         clk = clknode_create(clkdom, &aw_frac_clknode_class, &clkdef->clkdef);
 359         if (clk == NULL)
 360                 return (1);
 361
 362         sc = clknode_get_softc(clk);
 363
 364         sc->offset = clkdef->offset;
 365
 366         sc->m.shift = clkdef->m.shift;
 367         sc->m.width = clkdef->m.width;
 368         sc->m.mask = ((1 << sc->m.width) - 1) << sc->m.shift;
 369         sc->m.value = clkdef->m.value;
 370         sc->m.flags = clkdef->m.flags;
 371
 372         sc->n.shift = clkdef->n.shift;
 373         sc->n.width = clkdef->n.width;
 374         sc->n.mask = ((1 << sc->n.width) - 1) << sc->n.shift;
 375         sc->n.value = clkdef->n.value;
 376         sc->n.flags = clkdef->n.flags;
 377
 378         sc->frac.freq0 = clkdef->frac.freq0;
 379         sc->frac.freq1 = clkdef->frac.freq1;
 380         sc->frac.mode_sel = 1 << clkdef->frac.mode_sel;
 381         sc->frac.freq_sel = 1 << clkdef->frac.freq_sel;
 382
 383         sc->min_freq = clkdef->min_freq;
 384         sc->max_freq = clkdef->max_freq;
 385
 386         sc->mux_shift = clkdef->mux_shift;
 387         sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
 388
 389         sc->gate_shift = clkdef->gate_shift;
 390
 391         sc->lock_shift = clkdef->lock_shift;
 392         sc->lock_retries = clkdef->lock_retries;
 393
 394         sc->flags = clkdef->flags;
 395
 396         clknode_register(clkdom, clk);
 397
 398         return (0);
 399 }