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