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_COMPOSITE_WRITE_MASK 0xFFFF0000
  70
  71 static int
  72 rk_clk_composite_init(struct clknode *clk, device_t dev)
  73 {
  74         struct rk_clk_composite_sc *sc;
  75         uint32_t val, idx;
  76
  77         sc = clknode_get_softc(clk);
  78
  79         idx = 0;
  80         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) != 0) {
  81                 DEVICE_LOCK(clk);
  82                 READ4(clk, sc->muxdiv_offset, &val);
  83                 DEVICE_UNLOCK(clk);
  84
  85                 idx = (val & sc->mux_mask) >> sc->mux_shift;
  86         }
  87
  88         clknode_init_parent_idx(clk, idx);
  89
  90         return (0);
  91 }
  92
  93 static int
  94 rk_clk_composite_set_gate(struct clknode *clk, bool enable)
  95 {
  96         struct rk_clk_composite_sc *sc;
  97         uint32_t val;
  98
  99         sc = clknode_get_softc(clk);
 100
 101         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_GATE) == 0)
 102                 return (0);
 103
 104         DEVICE_LOCK(clk);
 105         READ4(clk, sc->gate_offset, &val);
 106         if (enable)
 107                 val &= ~(1 << sc->gate_shift);
 108         else
 109                 val |= 1 << sc->gate_shift;
 110         WRITE4(clk, sc->gate_offset, val | RK_CLK_COMPOSITE_MASK);
 111         DEVICE_UNLOCK(clk);
 112
 113         return (0);
 114 }
 115
 116 static int
 117 rk_clk_composite_set_mux(struct clknode *clk, int index)
 118 {
 119         struct rk_clk_composite_sc *sc;
 120         uint32_t val;
 121
 122         sc = clknode_get_softc(clk);
 123
 124         if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) == 0)
 125                 return (0);
 126
 127         DEVICE_LOCK(clk);
 128         READ4(clk, sc->muxdiv_offset, &val);
 129         val &= ~sc->mux_mask;
 130         val |= index << sc->mux_shift;
 131         WRITE4(clk, sc->muxdiv_offset, val | RK_CLK_COMPOSITE_MASK);
 132         DEVICE_UNLOCK(clk);
 133
 134         return (0);
 135 }
 136
 137 static int
 138 rk_clk_composite_recalc(struct clknode *clk, uint64_t *freq)
 139 {
 140         struct rk_clk_composite_sc *sc;
 141         uint32_t reg, div;
 142
 143         sc = clknode_get_softc(clk);
 144
 145         DEVICE_LOCK(clk);
 146
 147         READ4(clk, sc->muxdiv_offset, &reg);
 148
 149         DEVICE_UNLOCK(clk);
 150
 151         div = ((reg & sc->div_mask) >> sc->div_shift) + 1;
 152
 153         *freq = *freq / div;
 154
 155         return (0);
 156 }
 157
 158 static uint32_t
 159 rk_clk_composite_find_best(struct rk_clk_composite_sc *sc, uint64_t fparent,
 160     uint64_t freq)
 161 {
 162         uint64_t best, cur;
 163         uint32_t best_div, div;
 164
 165         for (best = 0, best_div = 0, div = 0;
 166              div <= ((sc->div_mask >> sc->div_shift) + 1); div++) {
 167                 cur = fparent / div;
 168                 if ((freq - cur) < (freq - best)) {
 169                         best = cur;
 170                         best_div = div;
 171                         break;
 172                 }
 173         }
 174
 175         return (best_div);
 176 }
 177
 178 static int
 179 rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
 180     int flags, int *stop)
 181 {
 182         struct rk_clk_composite_sc *sc;
 183         struct clknode *p_clk;
 184         const char **p_names;
 185         uint64_t best, cur;
 186         uint32_t div, best_div, val;
 187         int p_idx, best_parent;
 188
 189         sc = clknode_get_softc(clk);
 190
 191         p_names = clknode_get_parent_names(clk);
 192         for (best_div = 0, best = 0, p_idx = 0;
 193              p_idx != clknode_get_parents_num(clk); p_idx++) {
 194                 p_clk = clknode_find_by_name(p_names[p_idx]);
 195                 clknode_get_freq(p_clk, &fparent);
 196                 div = rk_clk_composite_find_best(sc, fparent, *fout);
 197                 cur = fparent / div;
 198                 if ((*fout - cur) < (*fout - best)) {
 199                         best = cur;
 200                         best_div = div;
 201                         best_parent = p_idx;
 202                 }
 203         }
 204
 205         if (best_div == 0)
 206                 return (0);
 207
 208         if ((best < *fout) &&
 209           ((flags & CLK_SET_ROUND_DOWN) == 0)) {
 210                 *stop = 1;
 211                 return (ERANGE);
 212         }
 213         if ((best > *fout) &&
 214           ((flags & CLK_SET_ROUND_UP) == 0)) {
 215                 *stop = 1;
 216                 return (ERANGE);
 217         }
 218
 219         if ((flags & CLK_SET_DRYRUN) != 0) {
 220                 *fout = best;
 221                 *stop = 1;
 222                 return (0);
 223         }
 224
 225         p_idx = clknode_get_parent_idx(clk);
 226         if (p_idx != best_parent)
 227                 clknode_set_parent_by_idx(clk, best_parent);
 228
 229         DEVICE_LOCK(clk);
 230         READ4(clk, sc->muxdiv_offset, &val);
 231         val &= ~sc->div_mask;
 232         val |= (best_div - 1) << sc->div_shift;
 233         WRITE4(clk, sc->muxdiv_offset, val | RK_CLK_COMPOSITE_MASK);
 234         DEVICE_UNLOCK(clk);
 235
 236         *fout = best;
 237         *stop = 1;
 238
 239         return (0);
 240 }
 241
 242 static clknode_method_t rk_clk_composite_clknode_methods[] = {
 243         /* Device interface */
 244         CLKNODEMETHOD(clknode_init,             rk_clk_composite_init),
 245         CLKNODEMETHOD(clknode_set_gate,         rk_clk_composite_set_gate),
 246         CLKNODEMETHOD(clknode_set_mux,          rk_clk_composite_set_mux),
 247         CLKNODEMETHOD(clknode_recalc_freq,      rk_clk_composite_recalc),
 248         CLKNODEMETHOD(clknode_set_freq,         rk_clk_composite_set_freq),
 249         CLKNODEMETHOD_END
 250 };
 251
 252 DEFINE_CLASS_1(rk_clk_composite_clknode, rk_clk_composite_clknode_class,
 253     rk_clk_composite_clknode_methods, sizeof(struct rk_clk_composite_sc),
 254     clknode_class);
 255
 256 int
 257 rk_clk_composite_register(struct clkdom *clkdom, struct rk_clk_composite_def *clkdef)
 258 {
 259         struct clknode *clk;
 260         struct rk_clk_composite_sc *sc;
 261
 262         clk = clknode_create(clkdom, &rk_clk_composite_clknode_class,
 263             &clkdef->clkdef);
 264         if (clk == NULL)
 265                 return (1);
 266
 267         sc = clknode_get_softc(clk);
 268
 269         sc->muxdiv_offset = clkdef->muxdiv_offset;
 270
 271         sc->mux_shift = clkdef->mux_shift;
 272         sc->mux_width = clkdef->mux_width;
 273         sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
 274
 275         sc->div_shift = clkdef->div_shift;
 276         sc->div_width = clkdef->div_width;
 277         sc->div_mask = ((1 << clkdef->div_width) - 1) << sc->div_shift;
 278
 279         sc->gate_offset = clkdef->gate_offset;
 280         sc->gate_shift = clkdef->gate_shift;
 281
 282         sc->flags = clkdef->flags;
 283
 284         clknode_register(clkdom, clk);
 285
 286         return (0);
 287 }