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
  39 #include <arm64/rockchip/clk/rk_clk_composite.h>
  40
  41 #include "clkdev_if.h"
  42
  43 struct rk_clk_composite_sc {
  44         uint32_t        muxdiv_offset;
  45         uint32_t        mux_shift;
  46         uint32_t        mux_width;
  47         uint32_t        mux_mask;
  48
  49         uint32_t        div_shift;
  50         uint32_t        div_width;
  51         uint32_t        div_mask;
  52
  53         uint32_t        gate_offset;
  54         uint32_t        gate_shift;
  55
  56         uint32_t        flags;
  57 };
  58
  59 #define WRITE4(_clk, off, val)                                          \
  60         CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
  61 #define READ4(_clk, off, val)                                           \
  62         CLKDEV_READ_4(clknode_get_device(_clk), off, val)
  63 #define DEVICE_LOCK(_clk)                                               \
  64         CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
  65 #define DEVICE_UNLOCK(_clk)                                             \
  66         CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
  67
  68 #define RK_CLK_COMPOSITE_MASK_SHIFT     16
  69
  70 #if 0
  71 #define dprintf(format, arg...)                                         \
  72         printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg)
  73 #else
  74 #define dprintf(format, arg...)
  75 #endif
  76
  77 static int
  78 rk_clk_composite_init(struct clknode *clk, device_t dev)
  79 {
  80         struct rk_clk_composite_sc *sc;
  81         uint32_t val, idx;
  82
  83         sc = clknode_get_softc(clk);
  84
  85         idx = 0;
  86         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) != 0) {
  87                 DEVICE_LOCK(clk);
  88                 READ4(clk, sc->muxdiv_offset, &val);
  89                 DEVICE_UNLOCK(clk);
  90
  91                 idx = (val & sc->mux_mask) >> sc->mux_shift;
  92         }
  93
  94         clknode_init_parent_idx(clk, idx);
  95
  96         return (0);
  97 }
  98
  99 static int
 100 rk_clk_composite_set_gate(struct clknode *clk, bool enable)
 101 {
 102         struct rk_clk_composite_sc *sc;
 103         uint32_t val = 0;
 104
 105         sc = clknode_get_softc(clk);
 106
 107         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_GATE) == 0)
 108                 return (0);
 109
 110         dprintf("%sabling gate\n", enable ? "En" : "Dis");
 111         if (!enable)
 112                 val |= 1 << sc->gate_shift;
 113         dprintf("sc->gate_shift: %x\n", sc->gate_shift);
 114         val |= (1 << sc->gate_shift) << RK_CLK_COMPOSITE_MASK_SHIFT;
 115         dprintf("Write: gate_offset=%x, val=%x\n", sc->gate_offset, val);
 116         DEVICE_LOCK(clk);
 117         WRITE4(clk, sc->gate_offset, val);
 118         DEVICE_UNLOCK(clk);
 119
 120         return (0);
 121 }
 122
 123 static int
 124 rk_clk_composite_set_mux(struct clknode *clk, int index)
 125 {
 126         struct rk_clk_composite_sc *sc;
 127         uint32_t val = 0;
 128
 129         sc = clknode_get_softc(clk);
 130
 131         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) == 0)
 132                 return (0);
 133
 134         dprintf("Set mux to %d\n", index);
 135         DEVICE_LOCK(clk);
 136         val |= (index << sc->mux_shift);
 137         val |= sc->mux_mask << RK_CLK_COMPOSITE_MASK_SHIFT;
 138         dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val);
 139         WRITE4(clk, sc->muxdiv_offset, val);
 140         DEVICE_UNLOCK(clk);
 141
 142         return (0);
 143 }
 144
 145 static int
 146 rk_clk_composite_recalc(struct clknode *clk, uint64_t *freq)
 147 {
 148         struct rk_clk_composite_sc *sc;
 149         uint32_t reg, div;
 150
 151         sc = clknode_get_softc(clk);
 152
 153         DEVICE_LOCK(clk);
 154
 155         READ4(clk, sc->muxdiv_offset, &reg);
 156         dprintf("Read: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, reg);
 157
 158         DEVICE_UNLOCK(clk);
 159
 160         div = ((reg & sc->div_mask) >> sc->div_shift);
 161         if (sc->flags & RK_CLK_COMPOSITE_DIV_EXP)
 162                 div = 1 << div;
 163         else
 164                 div += 1;
 165         dprintf("parent_freq=%ju, div=%u\n", *freq, div);
 166         *freq = *freq / div;
 167         dprintf("Final freq=%ju\n", *freq);
 168         return (0);
 169 }
 170
 171 static uint32_t
 172 rk_clk_composite_find_best(struct rk_clk_composite_sc *sc, uint64_t fparent,
 173     uint64_t freq, uint32_t *reg)
 174 {
 175         uint64_t best, cur;
 176         uint32_t best_div, best_div_reg;
 177         uint32_t div, div_reg;
 178
 179         best = 0;
 180         best_div = 0;
 181         best_div_reg = 0;
 182
 183         for (div_reg = 0;  div_reg <= ((sc->div_mask >> sc->div_shift) + 1);
 184             div_reg++) {
 185                 if (sc->flags == RK_CLK_COMPOSITE_DIV_EXP)
 186                         div = 1 << div_reg;
 187                 else
 188                         div = div_reg + 1;
 189                 cur = fparent / div;
 190                 if ((freq - cur) < (freq - best)) {
 191                         best = cur;
 192                         best_div = div;
 193                         best_div_reg = div_reg;
 194                         break;
 195                 }
 196         }
 197         *reg = div_reg;
 198         return (best_div);
 199 }
 200
 201 static int
 202 rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
 203     int flags, int *stop)
 204 {
 205         struct rk_clk_composite_sc *sc;
 206         struct clknode *p_clk;
 207         const char **p_names;
 208         uint64_t best, cur;
 209         uint32_t div, div_reg, best_div, best_div_reg, val;
 210         int p_idx, best_parent;
 211
 212         sc = clknode_get_softc(clk);
 213         dprintf("Finding best parent/div for target freq of %ju\n", *fout);
 214         p_names = clknode_get_parent_names(clk);
 215         for (best_div = 0, best = 0, p_idx = 0;
 216              p_idx != clknode_get_parents_num(clk); p_idx++) {
 217                 p_clk = clknode_find_by_name(p_names[p_idx]);
 218                 clknode_get_freq(p_clk, &fparent);
 219                 dprintf("Testing with parent %s (%d) at freq %ju\n",
 220                     clknode_get_name(p_clk), p_idx, fparent);
 221                 div = rk_clk_composite_find_best(sc, fparent, *fout, &div_reg);
 222                 cur = fparent / div;
 223                 if ((*fout - cur) < (*fout - best)) {
 224                         best = cur;
 225                         best_div = div;
 226                         best_div_reg = div_reg;
 227                         best_parent = p_idx;
 228                         dprintf("Best parent so far %s (%d) with best freq at "
 229                             "%ju\n", clknode_get_name(p_clk), p_idx, best);
 230                 }
 231         }
 232
 233         *stop = 1;
 234         if (best_div == 0)
 235                 return (ERANGE);
 236
 237         if ((best < *fout) && ((flags & CLK_SET_ROUND_DOWN) == 0))
 238                 return (ERANGE);
 239
 240         if ((best > *fout) && ((flags & CLK_SET_ROUND_UP) == 0)) {
 241                 return (ERANGE);
 242         }
 243
 244         if ((flags & CLK_SET_DRYRUN) != 0) {
 245                 *fout = best;
 246                 return (0);
 247         }
 248
 249         p_idx = clknode_get_parent_idx(clk);
 250         if (p_idx != best_parent) {
 251                 dprintf("Switching parent index from %d to %d\n", p_idx,
 252                     best_parent);
 253                 clknode_set_parent_by_idx(clk, best_parent);
 254         }
 255
 256         dprintf("Setting divider to %d (reg: %d)\n", best_div, best_div_reg);
 257         dprintf(" div_mask: 0x%X, div_shift: %d\n", sc->div_mask,
 258             sc->div_shift);
 259
 260         DEVICE_LOCK(clk);
 261         val = best_div_reg << sc->div_shift;
 262         val |= sc->div_mask << RK_CLK_COMPOSITE_MASK_SHIFT;
 263         dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val);
 264         WRITE4(clk, sc->muxdiv_offset, val);
 265         DEVICE_UNLOCK(clk);
 266
 267         *fout = best;
 268         return (0);
 269 }
 270
 271 static clknode_method_t rk_clk_composite_clknode_methods[] = {
 272         /* Device interface */
 273         CLKNODEMETHOD(clknode_init,             rk_clk_composite_init),
 274         CLKNODEMETHOD(clknode_set_gate,         rk_clk_composite_set_gate),
 275         CLKNODEMETHOD(clknode_set_mux,          rk_clk_composite_set_mux),
 276         CLKNODEMETHOD(clknode_recalc_freq,      rk_clk_composite_recalc),
 277         CLKNODEMETHOD(clknode_set_freq,         rk_clk_composite_set_freq),
 278         CLKNODEMETHOD_END
 279 };
 280
 281 DEFINE_CLASS_1(rk_clk_composite_clknode, rk_clk_composite_clknode_class,
 282     rk_clk_composite_clknode_methods, sizeof(struct rk_clk_composite_sc),
 283     clknode_class);
 284
 285 int
 286 rk_clk_composite_register(struct clkdom *clkdom,
 287     struct rk_clk_composite_def *clkdef)
 288 {
 289         struct clknode *clk;
 290         struct rk_clk_composite_sc *sc;
 291
 292         clk = clknode_create(clkdom, &rk_clk_composite_clknode_class,
 293             &clkdef->clkdef);
 294         if (clk == NULL)
 295                 return (1);
 296
 297         sc = clknode_get_softc(clk);
 298
 299         sc->muxdiv_offset = clkdef->muxdiv_offset;
 300
 301         sc->mux_shift = clkdef->mux_shift;
 302         sc->mux_width = clkdef->mux_width;
 303         sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
 304
 305         sc->div_shift = clkdef->div_shift;
 306         sc->div_width = clkdef->div_width;
 307         sc->div_mask = ((1 << clkdef->div_width) - 1) << sc->div_shift;
 308
 309         sc->gate_offset = clkdef->gate_offset;
 310         sc->gate_shift = clkdef->gate_shift;
 311
 312         sc->flags = clkdef->flags;
 313
 314         clknode_register(clkdom, clk);
 315
 316         return (0);
 317 }