sys/arm/allwinner/clkng/aw_clk_m.c

   1 /*-
   2  * Copyright (c) 2019 Emmanuel Vadot <manu@freebsd.org>
   3  *
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions
   6  * are met:
   7  * 1. Redistributions of source code must retain the above copyright
   8  *    notice, this list of conditions and the following disclaimer.
   9  * 2. Redistributions in binary form must reproduce the above copyright
  10  *    notice, this list of conditions and the following disclaimer in the
  11  *    documentation and/or other materials provided with the distribution.
  12  *
  13  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
  14  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  15  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  16  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
  17  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  18  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  19  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
  20  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  21  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  22  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  23  * SUCH DAMAGE.
  24  *
  25  * $FreeBSD$
  26  */
  27
  28 #include <sys/cdefs.h>
  29 __FBSDID("$FreeBSD$");
  30
  31 #include <sys/param.h>
  32 #include <sys/systm.h>
  33 #include <sys/bus.h>
  34
  35 #include <dev/extres/clk/clk.h>
  36
  37 #include <arm/allwinner/clkng/aw_clk.h>
  38 #include <arm/allwinner/clkng/aw_clk_m.h>
  39
  40 #include "clkdev_if.h"
  41
  42 /*
  43  * clknode for clocks matching the formula :
  44  *
  45  * clk = clkin / m
  46  * And that needs to potentially :
  47  * 1) Set the parent freq
  48  * 2) Support Setting the parent to a multiple
  49  *
  50  */
  51
  52 struct aw_clk_m_sc {
  53         uint32_t        offset;
  54
  55         struct aw_clk_factor    m;
  56
  57         uint32_t        mux_shift;
  58         uint32_t        mux_mask;
  59         uint32_t        gate_shift;
  60
  61         uint32_t        flags;
  62 };
  63
  64 #define WRITE4(_clk, off, val)                                          \
  65         CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
  66 #define READ4(_clk, off, val)                                           \
  67         CLKDEV_READ_4(clknode_get_device(_clk), off, val)
  68 #define DEVICE_LOCK(_clk)                                                       \
  69         CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
  70 #define DEVICE_UNLOCK(_clk)                                             \
  71         CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
  72
  73 static int
  74 aw_clk_m_init(struct clknode *clk, device_t dev)
  75 {
  76         struct aw_clk_m_sc *sc;
  77         uint32_t val, idx;
  78
  79         sc = clknode_get_softc(clk);
  80
  81         idx = 0;
  82         if ((sc->flags & AW_CLK_HAS_MUX) != 0) {
  83                 DEVICE_LOCK(clk);
  84                 READ4(clk, sc->offset, &val);
  85                 DEVICE_UNLOCK(clk);
  86
  87                 idx = (val & sc->mux_mask) >> sc->mux_shift;
  88         }
  89
  90         clknode_init_parent_idx(clk, idx);
  91         return (0);
  92 }
  93
  94 static int
  95 aw_clk_m_set_gate(struct clknode *clk, bool enable)
  96 {
  97         struct aw_clk_m_sc *sc;
  98         uint32_t val;
  99
 100         sc = clknode_get_softc(clk);
 101
 102         if ((sc->flags & AW_CLK_HAS_GATE) == 0)
 103                 return (0);
 104
 105         DEVICE_LOCK(clk);
 106         READ4(clk, sc->offset, &val);
 107         if (enable)
 108                 val |= (1 << sc->gate_shift);
 109         else
 110                 val &= ~(1 << sc->gate_shift);
 111         WRITE4(clk, sc->offset, val);
 112         DEVICE_UNLOCK(clk);
 113
 114         return (0);
 115 }
 116
 117 static int
 118 aw_clk_m_set_mux(struct clknode *clk, int index)
 119 {
 120         struct aw_clk_m_sc *sc;
 121         uint32_t val;
 122
 123         sc = clknode_get_softc(clk);
 124
 125         if ((sc->flags & AW_CLK_HAS_MUX) == 0)
 126                 return (0);
 127
 128         DEVICE_LOCK(clk);
 129         READ4(clk, sc->offset, &val);
 130         val &= ~sc->mux_mask;
 131         val |= index << sc->mux_shift;
 132         WRITE4(clk, sc->offset, val);
 133         DEVICE_UNLOCK(clk);
 134
 135         return (0);
 136 }
 137
 138 static uint64_t
 139 aw_clk_m_find_best(struct aw_clk_m_sc *sc, uint64_t fparent, uint64_t *fout,
 140     uint32_t *factor_m)
 141 {
 142         uint64_t cur, best;
 143         uint32_t m, max_m, min_m;
 144
 145         *factor_m = 0;
 146
 147         max_m = aw_clk_factor_get_max(&sc->m);
 148         min_m = aw_clk_factor_get_min(&sc->m);
 149
 150         for (m = min_m; m <= max_m; ) {
 151                 cur = fparent / m;
 152                 if (abs(*fout - cur) < abs(*fout - best)) {
 153                         best = cur;
 154                         *factor_m = m;
 155                 }
 156                 if ((sc->m.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0)
 157                         m <<= 1;
 158                 else
 159                         m++;
 160         }
 161
 162         return (best);
 163 }
 164
 165 static int
 166 aw_clk_m_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
 167     int flags, int *stop)
 168 {
 169         struct aw_clk_m_sc *sc;
 170         struct clknode *p_clk;
 171         uint64_t cur, best;
 172         uint32_t val, m, best_m;
 173
 174         sc = clknode_get_softc(clk);
 175
 176         best = cur = 0;
 177
 178         if ((sc->flags & AW_CLK_SET_PARENT) != 0) {
 179                 p_clk = clknode_get_parent(clk);
 180                 if (p_clk == NULL) {
 181                         printf("%s: Cannot get parent for clock %s\n",
 182                             __func__,
 183                             clknode_get_name(clk));
 184                         return (ENXIO);
 185                 }
 186                 clknode_set_freq(p_clk, *fout, CLK_SET_ROUND_MULTIPLE, 0);
 187                 clknode_get_freq(p_clk, &fparent);
 188                 best = aw_clk_m_find_best(sc, fparent, fout,
 189                     &best_m);
 190         } else {
 191                 best = aw_clk_m_find_best(sc, fparent, fout,
 192                     &best_m);
 193         }
 194
 195         if ((flags & CLK_SET_DRYRUN) != 0) {
 196                 *fout = best;
 197                 *stop = 1;
 198                 return (0);
 199         }
 200
 201         if ((best < *fout) &&
 202           ((flags & CLK_SET_ROUND_DOWN) == 0)) {
 203                 *stop = 1;
 204                 return (ERANGE);
 205         }
 206         if ((best > *fout) &&
 207           ((flags & CLK_SET_ROUND_UP) == 0)) {
 208                 *stop = 1;
 209                 return (ERANGE);
 210         }
 211
 212         DEVICE_LOCK(clk);
 213         READ4(clk, sc->offset, &val);
 214
 215         m = aw_clk_factor_get_value(&sc->m, best_m);
 216         val &= ~sc->m.mask;
 217         val |= m << sc->m.shift;
 218
 219         WRITE4(clk, sc->offset, val);
 220         DEVICE_UNLOCK(clk);
 221
 222         *fout = best;
 223         *stop = 1;
 224
 225         return (0);
 226 }
 227
 228 static int
 229 aw_clk_m_recalc(struct clknode *clk, uint64_t *freq)
 230 {
 231         struct aw_clk_m_sc *sc;
 232         uint32_t val, m;
 233
 234         sc = clknode_get_softc(clk);
 235
 236         DEVICE_LOCK(clk);
 237         READ4(clk, sc->offset, &val);
 238         DEVICE_UNLOCK(clk);
 239
 240         m = aw_clk_get_factor(val, &sc->m);
 241
 242         *freq = *freq / m;
 243
 244         return (0);
 245 }
 246
 247 static clknode_method_t aw_m_clknode_methods[] = {
 248         /* Device interface */
 249         CLKNODEMETHOD(clknode_init,             aw_clk_m_init),
 250         CLKNODEMETHOD(clknode_set_gate,         aw_clk_m_set_gate),
 251         CLKNODEMETHOD(clknode_set_mux,          aw_clk_m_set_mux),
 252         CLKNODEMETHOD(clknode_recalc_freq,      aw_clk_m_recalc),
 253         CLKNODEMETHOD(clknode_set_freq,         aw_clk_m_set_freq),
 254         CLKNODEMETHOD_END
 255 };
 256
 257 DEFINE_CLASS_1(aw_m_clknode, aw_m_clknode_class, aw_m_clknode_methods,
 258     sizeof(struct aw_clk_m_sc), clknode_class);
 259
 260 int
 261 aw_clk_m_register(struct clkdom *clkdom, struct aw_clk_m_def *clkdef)
 262 {
 263         struct clknode *clk;
 264         struct aw_clk_m_sc *sc;
 265
 266         clk = clknode_create(clkdom, &aw_m_clknode_class, &clkdef->clkdef);
 267         if (clk == NULL)
 268                 return (1);
 269
 270         sc = clknode_get_softc(clk);
 271
 272         sc->offset = clkdef->offset;
 273
 274         sc->m.shift = clkdef->m.shift;
 275         sc->m.width = clkdef->m.width;
 276         sc->m.mask = ((1 << sc->m.width) - 1) << sc->m.shift;
 277         sc->m.value = clkdef->m.value;
 278         sc->m.flags = clkdef->m.flags;
 279
 280         sc->mux_shift = clkdef->mux_shift;
 281         sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
 282
 283         sc->gate_shift = clkdef->gate_shift;
 284
 285         sc->flags = clkdef->flags;
 286
 287         clknode_register(clkdom, clk);
 288
 289         return (0);
 290 }