sys/arm64/rockchip/clk/rk_clk_composite.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   3  *
   4  * Copyright (c) 2018 Emmanuel Vadot <manu@freebsd.org>
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
  16  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  17  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  18  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
  19  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  20  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  21  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
  22  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  23  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  25  * SUCH DAMAGE.
  26  *
  27  * $FreeBSD$
  28  */
  29
  30 #include <sys/cdefs.h>
  31 __FBSDID("$FreeBSD$");
  32
  33 #include <sys/param.h>
  34 #include <sys/systm.h>
  35 #include <sys/bus.h>
  36
  37 #include <dev/extres/clk/clk.h>
  38 #include <dev/extres/syscon/syscon.h>
  39
  40 #include <arm64/rockchip/clk/rk_clk_composite.h>
  41
  42 #include "clkdev_if.h"
  43 #include "syscon_if.h"
  44
  45 struct rk_clk_composite_sc {
  46         uint32_t        muxdiv_offset;
  47         uint32_t        mux_shift;
  48         uint32_t        mux_width;
  49         uint32_t        mux_mask;
  50
  51         uint32_t        div_shift;
  52         uint32_t        div_width;
  53         uint32_t        div_mask;
  54
  55         uint32_t        gate_offset;
  56         uint32_t        gate_shift;
  57
  58         uint32_t        flags;
  59
  60         struct syscon   *grf;
  61 };
  62
  63 #define WRITE4(_clk, off, val)                                          \
  64         rk_clk_composite_write_4(_clk, off, val)
  65 #define READ4(_clk, off, val)                                           \
  66         rk_clk_composite_read_4(_clk, off, val)
  67 #define DEVICE_LOCK(_clk)                                               \
  68         CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
  69 #define DEVICE_UNLOCK(_clk)                                             \
  70         CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
  71
  72 #define RK_CLK_COMPOSITE_MASK_SHIFT     16
  73
  74 #if 0
  75 #define dprintf(format, arg...)                                         \
  76         printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg)
  77 #else
  78 #define dprintf(format, arg...)
  79 #endif
  80
  81 static void
  82 rk_clk_composite_read_4(struct clknode *clk, bus_addr_t addr, uint32_t *val)
  83 {
  84         struct rk_clk_composite_sc *sc;
  85
  86         sc = clknode_get_softc(clk);
  87         if (sc->grf)
  88                 *val = SYSCON_READ_4(sc->grf, addr);
  89         else
  90                 CLKDEV_READ_4(clknode_get_device(clk), addr, val);
  91 }
  92
  93 static void
  94 rk_clk_composite_write_4(struct clknode *clk, bus_addr_t addr, uint32_t val)
  95 {
  96         struct rk_clk_composite_sc *sc;
  97
  98         sc = clknode_get_softc(clk);
  99         if (sc->grf)
 100                 SYSCON_WRITE_4(sc->grf, addr, val | (0xffff << 16));
 101         else
 102                 CLKDEV_WRITE_4(clknode_get_device(clk), addr, val);
 103 }
 104
 105 static struct syscon *
 106 rk_clk_composite_get_grf(struct clknode *clk)
 107 {
 108         device_t dev;
 109         phandle_t node;
 110         struct syscon *grf;
 111
 112         grf = NULL;
 113         dev = clknode_get_device(clk);
 114         node = ofw_bus_get_node(dev);
 115         if (OF_hasprop(node, "rockchip,grf") &&
 116             syscon_get_by_ofw_property(dev, node,
 117             "rockchip,grf", &grf) != 0) {
 118                 return (NULL);
 119         }
 120
 121         return (grf);
 122 }
 123
 124 static int
 125 rk_clk_composite_init(struct clknode *clk, device_t dev)
 126 {
 127         struct rk_clk_composite_sc *sc;
 128         uint32_t val, idx;
 129
 130         sc = clknode_get_softc(clk);
 131         if ((sc->flags & RK_CLK_COMPOSITE_GRF) != 0) {
 132                 sc->grf = rk_clk_composite_get_grf(clk);
 133                 if (sc->grf == NULL)
 134                         panic("clock %s has GRF flag set but no syscon is available",
 135                             clknode_get_name(clk));
 136         }
 137
 138         idx = 0;
 139         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) != 0) {
 140                 DEVICE_LOCK(clk);
 141                 READ4(clk, sc->muxdiv_offset, &val);
 142                 DEVICE_UNLOCK(clk);
 143
 144                 idx = (val & sc->mux_mask) >> sc->mux_shift;
 145         }
 146
 147         clknode_init_parent_idx(clk, idx);
 148
 149         return (0);
 150 }
 151
 152 static int
 153 rk_clk_composite_set_gate(struct clknode *clk, bool enable)
 154 {
 155         struct rk_clk_composite_sc *sc;
 156         uint32_t val = 0;
 157
 158         sc = clknode_get_softc(clk);
 159
 160         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_GATE) == 0)
 161                 return (0);
 162
 163         dprintf("%sabling gate\n", enable ? "En" : "Dis");
 164         if (!enable)
 165                 val |= 1 << sc->gate_shift;
 166         dprintf("sc->gate_shift: %x\n", sc->gate_shift);
 167         val |= (1 << sc->gate_shift) << RK_CLK_COMPOSITE_MASK_SHIFT;
 168         dprintf("Write: gate_offset=%x, val=%x\n", sc->gate_offset, val);
 169         DEVICE_LOCK(clk);
 170         WRITE4(clk, sc->gate_offset, val);
 171         DEVICE_UNLOCK(clk);
 172
 173         return (0);
 174 }
 175
 176 static int
 177 rk_clk_composite_set_mux(struct clknode *clk, int index)
 178 {
 179         struct rk_clk_composite_sc *sc;
 180         uint32_t val = 0;
 181
 182         sc = clknode_get_softc(clk);
 183
 184         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) == 0)
 185                 return (0);
 186
 187         dprintf("Set mux to %d\n", index);
 188         DEVICE_LOCK(clk);
 189         val |= (index << sc->mux_shift);
 190         val |= sc->mux_mask << RK_CLK_COMPOSITE_MASK_SHIFT;
 191         dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val);
 192         WRITE4(clk, sc->muxdiv_offset, val);
 193         DEVICE_UNLOCK(clk);
 194
 195         return (0);
 196 }
 197
 198 static int
 199 rk_clk_composite_recalc(struct clknode *clk, uint64_t *freq)
 200 {
 201         struct rk_clk_composite_sc *sc;
 202         uint32_t reg, div;
 203
 204         sc = clknode_get_softc(clk);
 205
 206         DEVICE_LOCK(clk);
 207
 208         READ4(clk, sc->muxdiv_offset, &reg);
 209         dprintf("Read: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, reg);
 210
 211         DEVICE_UNLOCK(clk);
 212
 213         div = ((reg & sc->div_mask) >> sc->div_shift);
 214         if (sc->flags & RK_CLK_COMPOSITE_DIV_EXP)
 215                 div = 1 << div;
 216         else
 217                 div += 1;
 218         dprintf("parent_freq=%ju, div=%u\n", *freq, div);
 219         *freq = *freq / div;
 220         dprintf("Final freq=%ju\n", *freq);
 221         return (0);
 222 }
 223
 224 static uint32_t
 225 rk_clk_composite_find_best(struct rk_clk_composite_sc *sc, uint64_t fparent,
 226     uint64_t freq, uint32_t *reg)
 227 {
 228         uint64_t best, cur;
 229         uint32_t best_div, best_div_reg;
 230         uint32_t div, div_reg;
 231
 232         best = 0;
 233         best_div = 0;
 234         best_div_reg = 0;
 235
 236         for (div_reg = 0;  div_reg <= ((sc->div_mask >> sc->div_shift) + 1);
 237             div_reg++) {
 238                 if (sc->flags == RK_CLK_COMPOSITE_DIV_EXP)
 239                         div = 1 << div_reg;
 240                 else
 241                         div = div_reg + 1;
 242                 cur = fparent / div;
 243                 if ((freq - cur) < (freq - best)) {
 244                         best = cur;
 245                         best_div = div;
 246                         best_div_reg = div_reg;
 247                         break;
 248                 }
 249         }
 250         *reg = div_reg;
 251         return (best_div);
 252 }
 253
 254 static int
 255 rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
 256     int flags, int *stop)
 257 {
 258         struct rk_clk_composite_sc *sc;
 259         struct clknode *p_clk;
 260         const char **p_names;
 261         uint64_t best, cur;
 262         uint32_t div, div_reg, best_div, best_div_reg, val;
 263         int p_idx, best_parent;
 264
 265         sc = clknode_get_softc(clk);
 266         dprintf("Finding best parent/div for target freq of %ju\n", *fout);
 267         p_names = clknode_get_parent_names(clk);
 268         for (best_div = 0, best = 0, p_idx = 0;
 269              p_idx != clknode_get_parents_num(clk); p_idx++) {
 270                 p_clk = clknode_find_by_name(p_names[p_idx]);
 271                 clknode_get_freq(p_clk, &fparent);
 272                 dprintf("Testing with parent %s (%d) at freq %ju\n",
 273                     clknode_get_name(p_clk), p_idx, fparent);
 274                 div = rk_clk_composite_find_best(sc, fparent, *fout, &div_reg);
 275                 cur = fparent / div;
 276                 if ((*fout - cur) < (*fout - best)) {
 277                         best = cur;
 278                         best_div = div;
 279                         best_div_reg = div_reg;
 280                         best_parent = p_idx;
 281                         dprintf("Best parent so far %s (%d) with best freq at "
 282                             "%ju\n", clknode_get_name(p_clk), p_idx, best);
 283                 }
 284         }
 285
 286         *stop = 1;
 287         if (best_div == 0)
 288                 return (ERANGE);
 289
 290         if ((best < *fout) && ((flags & CLK_SET_ROUND_DOWN) == 0))
 291                 return (ERANGE);
 292
 293         if ((best > *fout) && ((flags & CLK_SET_ROUND_UP) == 0)) {
 294                 return (ERANGE);
 295         }
 296
 297         if ((flags & CLK_SET_DRYRUN) != 0) {
 298                 *fout = best;
 299                 return (0);
 300         }
 301
 302         p_idx = clknode_get_parent_idx(clk);
 303         if (p_idx != best_parent) {
 304                 dprintf("Switching parent index from %d to %d\n", p_idx,
 305                     best_parent);
 306                 clknode_set_parent_by_idx(clk, best_parent);
 307         }
 308
 309         dprintf("Setting divider to %d (reg: %d)\n", best_div, best_div_reg);
 310         dprintf(" div_mask: 0x%X, div_shift: %d\n", sc->div_mask,
 311             sc->div_shift);
 312
 313         DEVICE_LOCK(clk);
 314         val = best_div_reg << sc->div_shift;
 315         val |= sc->div_mask << RK_CLK_COMPOSITE_MASK_SHIFT;
 316         dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val);
 317         WRITE4(clk, sc->muxdiv_offset, val);
 318         DEVICE_UNLOCK(clk);
 319
 320         *fout = best;
 321         return (0);
 322 }
 323
 324 static clknode_method_t rk_clk_composite_clknode_methods[] = {
 325         /* Device interface */
 326         CLKNODEMETHOD(clknode_init,             rk_clk_composite_init),
 327         CLKNODEMETHOD(clknode_set_gate,         rk_clk_composite_set_gate),
 328         CLKNODEMETHOD(clknode_set_mux,          rk_clk_composite_set_mux),
 329         CLKNODEMETHOD(clknode_recalc_freq,      rk_clk_composite_recalc),
 330         CLKNODEMETHOD(clknode_set_freq,         rk_clk_composite_set_freq),
 331         CLKNODEMETHOD_END
 332 };
 333
 334 DEFINE_CLASS_1(rk_clk_composite_clknode, rk_clk_composite_clknode_class,
 335     rk_clk_composite_clknode_methods, sizeof(struct rk_clk_composite_sc),
 336     clknode_class);
 337
 338 int
 339 rk_clk_composite_register(struct clkdom *clkdom,
 340     struct rk_clk_composite_def *clkdef)
 341 {
 342         struct clknode *clk;
 343         struct rk_clk_composite_sc *sc;
 344
 345         clk = clknode_create(clkdom, &rk_clk_composite_clknode_class,
 346             &clkdef->clkdef);
 347         if (clk == NULL)
 348                 return (1);
 349
 350         sc = clknode_get_softc(clk);
 351
 352         sc->muxdiv_offset = clkdef->muxdiv_offset;
 353
 354         sc->mux_shift = clkdef->mux_shift;
 355         sc->mux_width = clkdef->mux_width;
 356         sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
 357
 358         sc->div_shift = clkdef->div_shift;
 359         sc->div_width = clkdef->div_width;
 360         sc->div_mask = ((1 << clkdef->div_width) - 1) << sc->div_shift;
 361
 362         sc->gate_offset = clkdef->gate_offset;
 363         sc->gate_shift = clkdef->gate_shift;
 364
 365         sc->flags = clkdef->flags;
 366
 367         clknode_register(clkdom, clk);
 368
 369         return (0);
 370 }