sys/arm/allwinner/clkng/aw_clk_nm.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_nm.h>
  40
  41 #include "clkdev_if.h"
  42
  43 /*
  44  * clknode for clocks matching the formula :
  45  *
  46  * clk = clkin / n / m
  47  *
  48  */
  49
  50 struct aw_clk_nm_sc {
  51         uint32_t        offset;
  52
  53         struct aw_clk_factor    m;
  54         struct aw_clk_factor    n;
  55         struct aw_clk_frac      frac;
  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
  63         uint32_t        flags;
  64 };
  65
  66 #define WRITE4(_clk, off, val)                                          \
  67         CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
  68 #define READ4(_clk, off, val)                                           \
  69         CLKDEV_READ_4(clknode_get_device(_clk), off, val)
  70 #define DEVICE_LOCK(_clk)                                                       \
  71         CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
  72 #define DEVICE_UNLOCK(_clk)                                             \
  73         CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
  74
  75 static int
  76 aw_clk_nm_init(struct clknode *clk, device_t dev)
  77 {
  78         struct aw_clk_nm_sc *sc;
  79         uint32_t val, idx;
  80
  81         sc = clknode_get_softc(clk);
  82
  83         idx = 0;
  84         if ((sc->flags & AW_CLK_HAS_MUX) != 0) {
  85                 DEVICE_LOCK(clk);
  86                 READ4(clk, sc->offset, &val);
  87                 DEVICE_UNLOCK(clk);
  88
  89                 idx = (val & sc->mux_mask) >> sc->mux_shift;
  90         }
  91
  92         clknode_init_parent_idx(clk, idx);
  93         return (0);
  94 }
  95
  96 static int
  97 aw_clk_nm_set_gate(struct clknode *clk, bool enable)
  98 {
  99         struct aw_clk_nm_sc *sc;
 100         uint32_t val;
 101
 102         sc = clknode_get_softc(clk);
 103
 104         if ((sc->flags & AW_CLK_HAS_GATE) == 0)
 105                 return (0);
 106
 107         DEVICE_LOCK(clk);
 108         READ4(clk, sc->offset, &val);
 109         if (enable)
 110                 val |= (1 << sc->gate_shift);
 111         else
 112                 val &= ~(1 << sc->gate_shift);
 113         WRITE4(clk, sc->offset, val);
 114         DEVICE_UNLOCK(clk);
 115
 116         return (0);
 117 }
 118
 119 static int
 120 aw_clk_nm_set_mux(struct clknode *clk, int index)
 121 {
 122         struct aw_clk_nm_sc *sc;
 123         uint32_t val;
 124
 125         sc = clknode_get_softc(clk);
 126
 127         if ((sc->flags & AW_CLK_HAS_MUX) != 0)
 128                 return (0);
 129
 130         DEVICE_LOCK(clk);
 131         READ4(clk, sc->offset, &val);
 132         val &= ~(sc->mux_mask >> sc->mux_shift);
 133         val |= index << sc->mux_shift;
 134         WRITE4(clk, sc->offset, val);
 135         DEVICE_UNLOCK(clk);
 136
 137         return (0);
 138 }
 139
 140 static uint64_t
 141 aw_clk_nm_find_best(struct aw_clk_nm_sc *sc, uint64_t fparent, uint64_t *fout,
 142     uint32_t *factor_n, uint32_t *factor_m)
 143 {
 144         uint64_t cur, best;
 145         uint32_t m, n, max_m, max_n, min_m, min_n;
 146
 147         *factor_n = *factor_m = 0;
 148
 149         max_m = aw_clk_factor_get_max(&sc->m);
 150         max_n = aw_clk_factor_get_max(&sc->n);
 151         min_m = aw_clk_factor_get_min(&sc->m);
 152         min_n = aw_clk_factor_get_min(&sc->n);
 153
 154         for (m = min_m; m <= max_m; ) {
 155                 for (n = min_m; n <= max_n; ) {
 156                         cur = fparent / n / m;
 157                         if ((*fout - cur) < (*fout - best)) {
 158                                 best = cur;
 159                                 *factor_n = n;
 160                                 *factor_m = m;
 161                         }
 162
 163                         if ((sc->n.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0)
 164                                 n <<= 1;
 165                         else
 166                                 n++;
 167                 }
 168                 if ((sc->m.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0)
 169                         m <<= 1;
 170                 else
 171                         m++;
 172         }
 173
 174         return (best);
 175 }
 176
 177 static int
 178 aw_clk_nm_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
 179     int flags, int *stop)
 180 {
 181         struct aw_clk_nm_sc *sc;
 182         struct clknode *p_clk;
 183         const char **p_names;
 184         uint64_t cur, best, best_frac;
 185         uint32_t val, m, n, best_m, best_n;
 186         int p_idx, best_parent, retry;
 187
 188         sc = clknode_get_softc(clk);
 189
 190         best = best_frac = cur = 0;
 191         best_parent = 0;
 192
 193         if ((sc->flags & AW_CLK_REPARENT) != 0) {
 194                 p_names = clknode_get_parent_names(clk);
 195                 for (p_idx = 0; p_idx != clknode_get_parents_num(clk); p_idx++) {
 196                         p_clk = clknode_find_by_name(p_names[p_idx]);
 197                         clknode_get_freq(p_clk, &fparent);
 198
 199                         cur = aw_clk_nm_find_best(sc, fparent, fout, &n, &m);
 200                         if ((*fout - cur) < (*fout - best)) {
 201                                 best = cur;
 202                                 best_parent = p_idx;
 203                                 best_n = n;
 204                                 best_m = m;
 205                         }
 206                 }
 207
 208                 p_idx = clknode_get_parent_idx(clk);
 209                 p_clk = clknode_get_parent(clk);
 210                 clknode_get_freq(p_clk, &fparent);
 211         } else {
 212                 if (sc->flags & AW_CLK_HAS_FRAC &&
 213                     (*fout == sc->frac.freq0 || *fout == sc->frac.freq1))
 214                         best = best_frac = *fout;
 215
 216                 if (best == 0)
 217                         best = aw_clk_nm_find_best(sc, fparent, fout,
 218                             &best_n, &best_m);
 219         }
 220
 221         if ((flags & CLK_SET_DRYRUN) != 0) {
 222                 *fout = best;
 223                 *stop = 1;
 224                 return (0);
 225         }
 226
 227         if ((best < *fout) &&
 228           ((flags & CLK_SET_ROUND_DOWN) == 0)) {
 229                 *stop = 1;
 230                 return (ERANGE);
 231         }
 232         if ((best > *fout) &&
 233           ((flags & CLK_SET_ROUND_UP) == 0)) {
 234                 *stop = 1;
 235                 return (ERANGE);
 236         }
 237
 238         if (p_idx != best_parent)
 239                 clknode_set_parent_by_idx(clk, best_parent);
 240
 241         DEVICE_LOCK(clk);
 242         READ4(clk, sc->offset, &val);
 243
 244         if (best_frac != 0) {
 245                 val &= ~sc->frac.mode_sel;
 246                 if (best_frac == sc->frac.freq0)
 247                         val &= ~sc->frac.freq_sel;
 248                 else
 249                         val |= sc->frac.freq_sel;
 250         } else {
 251                 n = aw_clk_factor_get_value(&sc->n, best_n);
 252                 m = aw_clk_factor_get_value(&sc->m, best_m);
 253                 val &= ~sc->n.mask;
 254                 val &= ~sc->m.mask;
 255                 val |= n << sc->n.shift;
 256                 val |= m << sc->m.shift;
 257         }
 258
 259         WRITE4(clk, sc->offset, val);
 260         DEVICE_UNLOCK(clk);
 261
 262         if ((sc->flags & AW_CLK_HAS_LOCK) != 0) {
 263                 for (retry = 0; retry < sc->lock_retries; retry++) {
 264                         READ4(clk, sc->offset, &val);
 265                         if ((val & (1 << sc->lock_shift)) != 0)
 266                                 break;
 267                         DELAY(1000);
 268                 }
 269         }
 270
 271         *fout = best;
 272         *stop = 1;
 273
 274         return (0);
 275 }
 276
 277 static int
 278 aw_clk_nm_recalc(struct clknode *clk, uint64_t *freq)
 279 {
 280         struct aw_clk_nm_sc *sc;
 281         uint32_t val, m, n;
 282
 283         sc = clknode_get_softc(clk);
 284
 285         DEVICE_LOCK(clk);
 286         READ4(clk, sc->offset, &val);
 287         DEVICE_UNLOCK(clk);
 288
 289         if (sc->flags & AW_CLK_HAS_FRAC && ((val & sc->frac.mode_sel) == 0)) {
 290                 if (val & sc->frac.freq_sel)
 291                         *freq = sc->frac.freq1;
 292                 else
 293                         *freq = sc->frac.freq0;
 294         } else {
 295                 m = aw_clk_get_factor(val, &sc->m);
 296                 n = aw_clk_get_factor(val, &sc->n);
 297
 298                 *freq = *freq / n / m;
 299         }
 300
 301         return (0);
 302 }
 303
 304 static clknode_method_t aw_nm_clknode_methods[] = {
 305         /* Device interface */
 306         CLKNODEMETHOD(clknode_init,             aw_clk_nm_init),
 307         CLKNODEMETHOD(clknode_set_gate,         aw_clk_nm_set_gate),
 308         CLKNODEMETHOD(clknode_set_mux,          aw_clk_nm_set_mux),
 309         CLKNODEMETHOD(clknode_recalc_freq,      aw_clk_nm_recalc),
 310         CLKNODEMETHOD(clknode_set_freq,         aw_clk_nm_set_freq),
 311         CLKNODEMETHOD_END
 312 };
 313
 314 DEFINE_CLASS_1(aw_nm_clknode, aw_nm_clknode_class, aw_nm_clknode_methods,
 315     sizeof(struct aw_clk_nm_sc), clknode_class);
 316
 317 int
 318 aw_clk_nm_register(struct clkdom *clkdom, struct aw_clk_nm_def *clkdef)
 319 {
 320         struct clknode *clk;
 321         struct aw_clk_nm_sc *sc;
 322
 323         clk = clknode_create(clkdom, &aw_nm_clknode_class, &clkdef->clkdef);
 324         if (clk == NULL)
 325                 return (1);
 326
 327         sc = clknode_get_softc(clk);
 328
 329         sc->offset = clkdef->offset;
 330
 331         sc->m.shift = clkdef->m.shift;
 332         sc->m.width = clkdef->m.width;
 333         sc->m.mask = ((1 << sc->m.width) - 1) << sc->m.shift;
 334         sc->m.value = clkdef->m.value;
 335         sc->m.flags = clkdef->m.flags;
 336
 337         sc->n.shift = clkdef->n.shift;
 338         sc->n.width = clkdef->n.width;
 339         sc->n.mask = ((1 << sc->n.width) - 1) << sc->n.shift;
 340         sc->n.value = clkdef->n.value;
 341         sc->n.flags = clkdef->n.flags;
 342
 343         sc->frac.freq0 = clkdef->frac.freq0;
 344         sc->frac.freq1 = clkdef->frac.freq1;
 345         sc->frac.mode_sel = 1 << clkdef->frac.mode_sel;
 346         sc->frac.freq_sel = 1 << clkdef->frac.freq_sel;
 347
 348         sc->mux_shift = clkdef->mux_shift;
 349         sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
 350
 351         sc->gate_shift = clkdef->gate_shift;
 352
 353         sc->lock_shift = clkdef->lock_shift;
 354         sc->lock_retries = clkdef->lock_retries;
 355
 356         sc->flags = clkdef->flags;
 357
 358         clknode_register(clkdom, clk);
 359
 360         return (0);
 361 }