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