sys/arm64/rockchip/clk/rk_clk_armclk.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_armclk.h>
  40
  41 #include "clkdev_if.h"
  42
  43 struct rk_clk_armclk_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         uint32_t        main_parent;
  59         uint32_t        alt_parent;
  60
  61         struct rk_clk_armclk_rates      *rates;
  62         int             nrates;
  63 };
  64
  65 #define WRITE4(_clk, off, val)                                          \
  66         CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
  67 #define READ4(_clk, off, val)                                           \
  68         CLKDEV_READ_4(clknode_get_device(_clk), off, val)
  69 #define DEVICE_LOCK(_clk)                                               \
  70         CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
  71 #define DEVICE_UNLOCK(_clk)                                             \
  72         CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
  73
  74 #define RK_ARMCLK_WRITE_MASK_SHIFT      16
  75
  76 #if 0
  77 #define dprintf(format, arg...)                                         \
  78         printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg)
  79 #else
  80 #define dprintf(format, arg...)
  81 #endif
  82
  83 static int
  84 rk_clk_armclk_init(struct clknode *clk, device_t dev)
  85 {
  86         struct rk_clk_armclk_sc *sc;
  87         uint32_t val, idx;
  88
  89         sc = clknode_get_softc(clk);
  90
  91         idx = 0;
  92         DEVICE_LOCK(clk);
  93         READ4(clk, sc->muxdiv_offset, &val);
  94         DEVICE_UNLOCK(clk);
  95
  96         idx = (val & sc->mux_mask) >> sc->mux_shift;
  97
  98         clknode_init_parent_idx(clk, idx);
  99
 100         return (0);
 101 }
 102
 103 static int
 104 rk_clk_armclk_set_mux(struct clknode *clk, int index)
 105 {
 106         struct rk_clk_armclk_sc *sc;
 107         uint32_t val = 0;
 108
 109         sc = clknode_get_softc(clk);
 110
 111         dprintf("Set mux to %d\n", index);
 112         DEVICE_LOCK(clk);
 113         val |= index << sc->mux_shift;
 114         val |= sc->mux_mask << RK_ARMCLK_WRITE_MASK_SHIFT;
 115         dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val);
 116         WRITE4(clk, sc->muxdiv_offset, val);
 117         DEVICE_UNLOCK(clk);
 118
 119         return (0);
 120 }
 121
 122 static int
 123 rk_clk_armclk_recalc(struct clknode *clk, uint64_t *freq)
 124 {
 125         struct rk_clk_armclk_sc *sc;
 126         uint32_t reg, div;
 127
 128         sc = clknode_get_softc(clk);
 129
 130         DEVICE_LOCK(clk);
 131
 132         READ4(clk, sc->muxdiv_offset, &reg);
 133         dprintf("Read: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, reg);
 134
 135         DEVICE_UNLOCK(clk);
 136
 137         div = ((reg & sc->div_mask) >> sc->div_shift) + 1;
 138         dprintf("parent_freq=%ju, div=%u\n", *freq, div);
 139
 140         *freq = *freq / div;
 141
 142         return (0);
 143 }
 144
 145 static int
 146 rk_clk_armclk_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
 147     int flags, int *stop)
 148 {
 149         struct rk_clk_armclk_sc *sc;
 150         struct clknode *p_main;
 151         const char **p_names;
 152         uint64_t best = 0, best_p = 0;
 153         uint32_t div = 0, val = 0;
 154         int err, i, rate = 0;
 155
 156         sc = clknode_get_softc(clk);
 157
 158         dprintf("Finding best parent/div for target freq of %ju\n", *fout);
 159         p_names = clknode_get_parent_names(clk);
 160         p_main = clknode_find_by_name(p_names[sc->main_parent]);
 161
 162         for (i = 0; i < sc->nrates; i++) {
 163                 if (sc->rates[i].freq == *fout) {
 164                         best = sc->rates[i].freq;
 165                         div = sc->rates[i].div;
 166                         best_p = best * div;
 167                         rate = i;
 168                         dprintf("Best parent %s (%d) with best freq at %ju\n",
 169                             clknode_get_name(p_main),
 170                             sc->main_parent,
 171                             best);
 172                         break;
 173                 }
 174         }
 175
 176         if (rate == sc->nrates)
 177                 return (0);
 178
 179         if ((flags & CLK_SET_DRYRUN) != 0) {
 180                 *fout = best;
 181                 *stop = 1;
 182                 return (0);
 183         }
 184
 185         dprintf("Changing parent (%s) freq to %ju\n", clknode_get_name(p_main),
 186             best_p);
 187         err = clknode_set_freq(p_main, best_p, 0, 1);
 188         if (err != 0)
 189                 printf("Cannot set %s to %ju\n",
 190                     clknode_get_name(p_main),
 191                     best_p);
 192
 193         clknode_set_parent_by_idx(clk, sc->main_parent);
 194
 195         clknode_get_freq(p_main, &best_p);
 196         dprintf("main parent freq at %ju\n", best_p);
 197         DEVICE_LOCK(clk);
 198         val |= (div - 1) << sc->div_shift;
 199         val |= sc->div_mask << RK_ARMCLK_WRITE_MASK_SHIFT;
 200         dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val);
 201         WRITE4(clk, sc->muxdiv_offset, val);
 202         DEVICE_UNLOCK(clk);
 203
 204         *fout = best;
 205         *stop = 1;
 206
 207         return (0);
 208 }
 209
 210 static clknode_method_t rk_clk_armclk_clknode_methods[] = {
 211         /* Device interface */
 212         CLKNODEMETHOD(clknode_init,             rk_clk_armclk_init),
 213         CLKNODEMETHOD(clknode_set_mux,          rk_clk_armclk_set_mux),
 214         CLKNODEMETHOD(clknode_recalc_freq,      rk_clk_armclk_recalc),
 215         CLKNODEMETHOD(clknode_set_freq,         rk_clk_armclk_set_freq),
 216         CLKNODEMETHOD_END
 217 };
 218
 219 DEFINE_CLASS_1(rk_clk_armclk_clknode, rk_clk_armclk_clknode_class,
 220     rk_clk_armclk_clknode_methods, sizeof(struct rk_clk_armclk_sc),
 221     clknode_class);
 222
 223 int
 224 rk_clk_armclk_register(struct clkdom *clkdom, struct rk_clk_armclk_def *clkdef)
 225 {
 226         struct clknode *clk;
 227         struct rk_clk_armclk_sc *sc;
 228
 229         clk = clknode_create(clkdom, &rk_clk_armclk_clknode_class,
 230             &clkdef->clkdef);
 231         if (clk == NULL)
 232                 return (1);
 233
 234         sc = clknode_get_softc(clk);
 235
 236         sc->muxdiv_offset = clkdef->muxdiv_offset;
 237
 238         sc->mux_shift = clkdef->mux_shift;
 239         sc->mux_width = clkdef->mux_width;
 240         sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
 241
 242         sc->div_shift = clkdef->div_shift;
 243         sc->div_width = clkdef->div_width;
 244         sc->div_mask = ((1 << clkdef->div_width) - 1) << sc->div_shift;
 245
 246         sc->flags = clkdef->flags;
 247
 248         sc->main_parent = clkdef->main_parent;
 249         sc->alt_parent = clkdef->alt_parent;
 250
 251         sc->rates = clkdef->rates;
 252         sc->nrates = clkdef->nrates;
 253
 254         clknode_register(clkdom, clk);
 255
 256         return (0);
 257 }