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