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/extres/clk/clk.h> 33 #include <dev/extres/syscon/syscon.h> 34 35 #include <dev/clk/rockchip/rk_clk_composite.h> 36 37 #include "clkdev_if.h" 38 #include "syscon_if.h" 39 40 struct rk_clk_composite_sc { 41 uint32_t muxdiv_offset; 42 uint32_t mux_shift; 43 uint32_t mux_width; 44 uint32_t mux_mask; 45 46 uint32_t div_shift; 47 uint32_t div_width; 48 uint32_t div_mask; 49 50 uint32_t flags; 51 52 struct syscon *grf; 53 }; 54 55 #define WRITE4(_clk, off, val) \ 56 rk_clk_composite_write_4(_clk, off, val) 57 #define READ4(_clk, off, val) \ 58 rk_clk_composite_read_4(_clk, off, val) 59 #define DEVICE_LOCK(_clk) \ 60 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk)) 61 #define DEVICE_UNLOCK(_clk) \ 62 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk)) 63 64 #define RK_CLK_COMPOSITE_MASK_SHIFT 16 65 66 #if 0 67 #define dprintf(format, arg...) \ 68 printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg) 69 #else 70 #define dprintf(format, arg...) 71 #endif 72 73 static void 74 rk_clk_composite_read_4(struct clknode *clk, bus_addr_t addr, uint32_t *val) 75 { 76 struct rk_clk_composite_sc *sc; 77 78 sc = clknode_get_softc(clk); 79 if (sc->grf) 80 *val = SYSCON_READ_4(sc->grf, addr); 81 else 82 CLKDEV_READ_4(clknode_get_device(clk), addr, val); 83 } 84 85 static void 86 rk_clk_composite_write_4(struct clknode *clk, bus_addr_t addr, uint32_t val) 87 { 88 struct rk_clk_composite_sc *sc; 89 90 sc = clknode_get_softc(clk); 91 if (sc->grf) 92 SYSCON_WRITE_4(sc->grf, addr, val | (0xffff << 16)); 93 else 94 CLKDEV_WRITE_4(clknode_get_device(clk), addr, val); 95 } 96 97 static struct syscon * 98 rk_clk_composite_get_grf(struct clknode *clk) 99 { 100 device_t dev; 101 phandle_t node; 102 struct syscon *grf; 103 104 grf = NULL; 105 dev = clknode_get_device(clk); 106 node = ofw_bus_get_node(dev); 107 if (OF_hasprop(node, "rockchip,grf") && 108 syscon_get_by_ofw_property(dev, node, 109 "rockchip,grf", &grf) != 0) { 110 return (NULL); 111 } 112 113 return (grf); 114 } 115 116 static int 117 rk_clk_composite_init(struct clknode *clk, device_t dev) 118 { 119 struct rk_clk_composite_sc *sc; 120 uint32_t val, idx; 121 122 sc = clknode_get_softc(clk); 123 if ((sc->flags & RK_CLK_COMPOSITE_GRF) != 0) { 124 sc->grf = rk_clk_composite_get_grf(clk); 125 if (sc->grf == NULL) 126 panic("clock %s has GRF flag set but no syscon is available", 127 clknode_get_name(clk)); 128 } 129 130 idx = 0; 131 if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) != 0) { 132 DEVICE_LOCK(clk); 133 READ4(clk, sc->muxdiv_offset, &val); 134 DEVICE_UNLOCK(clk); 135 136 idx = (val & sc->mux_mask) >> sc->mux_shift; 137 } 138 139 clknode_init_parent_idx(clk, idx); 140 141 return (0); 142 } 143 144 static int 145 rk_clk_composite_set_mux(struct clknode *clk, int index) 146 { 147 struct rk_clk_composite_sc *sc; 148 uint32_t val = 0; 149 150 sc = clknode_get_softc(clk); 151 152 if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) == 0) 153 return (0); 154 155 dprintf("Set mux to %d\n", index); 156 DEVICE_LOCK(clk); 157 val |= (index << sc->mux_shift); 158 val |= sc->mux_mask << RK_CLK_COMPOSITE_MASK_SHIFT; 159 dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val); 160 WRITE4(clk, sc->muxdiv_offset, val); 161 DEVICE_UNLOCK(clk); 162 163 return (0); 164 } 165 166 static int 167 rk_clk_composite_recalc(struct clknode *clk, uint64_t *freq) 168 { 169 struct rk_clk_composite_sc *sc; 170 uint32_t reg, div; 171 172 sc = clknode_get_softc(clk); 173 174 DEVICE_LOCK(clk); 175 176 READ4(clk, sc->muxdiv_offset, ®); 177 dprintf("Read: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, reg); 178 179 DEVICE_UNLOCK(clk); 180 181 div = ((reg & sc->div_mask) >> sc->div_shift); 182 if (sc->flags & RK_CLK_COMPOSITE_DIV_EXP) 183 div = 1 << div; 184 else 185 div += 1; 186 dprintf("parent_freq=%ju, div=%u\n", *freq, div); 187 *freq = *freq / div; 188 dprintf("Final freq=%ju\n", *freq); 189 return (0); 190 } 191 192 static uint32_t 193 rk_clk_composite_find_best(struct rk_clk_composite_sc *sc, uint64_t fparent, 194 uint64_t freq, uint32_t *reg) 195 { 196 uint64_t best, cur; 197 uint32_t best_div, best_div_reg; 198 uint32_t div, div_reg; 199 200 best = 0; 201 best_div = 0; 202 best_div_reg = 0; 203 204 for (div_reg = 0; div_reg <= ((sc->div_mask >> sc->div_shift) + 1); 205 div_reg++) { 206 if (sc->flags == RK_CLK_COMPOSITE_DIV_EXP) 207 div = 1 << div_reg; 208 else 209 div = div_reg + 1; 210 cur = fparent / div; 211 if ((freq - cur) < (freq - best)) { 212 best = cur; 213 best_div = div; 214 best_div_reg = div_reg; 215 break; 216 } 217 } 218 *reg = best_div_reg; 219 return (best_div); 220 } 221 222 static int 223 rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout, 224 int flags, int *stop) 225 { 226 struct rk_clk_composite_sc *sc; 227 struct clknode *p_clk; 228 const char **p_names; 229 uint64_t best, cur; 230 uint32_t div, div_reg, best_div, best_div_reg, val; 231 int p_idx, best_parent; 232 233 sc = clknode_get_softc(clk); 234 dprintf("Finding best parent/div for target freq of %ju\n", *fout); 235 p_names = clknode_get_parent_names(clk); 236 for (best_div = 0, best = 0, p_idx = 0; 237 p_idx != clknode_get_parents_num(clk); p_idx++) { 238 p_clk = clknode_find_by_name(p_names[p_idx]); 239 clknode_get_freq(p_clk, &fparent); 240 dprintf("Testing with parent %s (%d) at freq %ju\n", 241 clknode_get_name(p_clk), p_idx, fparent); 242 div = rk_clk_composite_find_best(sc, fparent, *fout, &div_reg); 243 cur = fparent / div; 244 if ((*fout - cur) < (*fout - best)) { 245 best = cur; 246 best_div = div; 247 best_div_reg = div_reg; 248 best_parent = p_idx; 249 dprintf("Best parent so far %s (%d) with best freq at " 250 "%ju\n", clknode_get_name(p_clk), p_idx, best); 251 } 252 } 253 254 *stop = 1; 255 if (best_div == 0) 256 return (ERANGE); 257 258 if ((best < *fout) && ((flags & CLK_SET_ROUND_DOWN) == 0)) 259 return (ERANGE); 260 261 if ((best > *fout) && ((flags & CLK_SET_ROUND_UP) == 0)) { 262 return (ERANGE); 263 } 264 265 if ((flags & CLK_SET_DRYRUN) != 0) { 266 *fout = best; 267 return (0); 268 } 269 270 p_idx = clknode_get_parent_idx(clk); 271 if (p_idx != best_parent) { 272 dprintf("Switching parent index from %d to %d\n", p_idx, 273 best_parent); 274 clknode_set_parent_by_idx(clk, best_parent); 275 } 276 277 dprintf("Setting divider to %d (reg: %d)\n", best_div, best_div_reg); 278 dprintf(" div_mask: 0x%X, div_shift: %d\n", sc->div_mask, 279 sc->div_shift); 280 281 DEVICE_LOCK(clk); 282 val = best_div_reg << sc->div_shift; 283 val |= sc->div_mask << RK_CLK_COMPOSITE_MASK_SHIFT; 284 dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val); 285 WRITE4(clk, sc->muxdiv_offset, val); 286 DEVICE_UNLOCK(clk); 287 288 *fout = best; 289 return (0); 290 } 291 292 static clknode_method_t rk_clk_composite_clknode_methods[] = { 293 /* Device interface */ 294 CLKNODEMETHOD(clknode_init, rk_clk_composite_init), 295 CLKNODEMETHOD(clknode_set_mux, rk_clk_composite_set_mux), 296 CLKNODEMETHOD(clknode_recalc_freq, rk_clk_composite_recalc), 297 CLKNODEMETHOD(clknode_set_freq, rk_clk_composite_set_freq), 298 CLKNODEMETHOD_END 299 }; 300 301 DEFINE_CLASS_1(rk_clk_composite_clknode, rk_clk_composite_clknode_class, 302 rk_clk_composite_clknode_methods, sizeof(struct rk_clk_composite_sc), 303 clknode_class); 304 305 int 306 rk_clk_composite_register(struct clkdom *clkdom, 307 struct rk_clk_composite_def *clkdef) 308 { 309 struct clknode *clk; 310 struct rk_clk_composite_sc *sc; 311 312 clk = clknode_create(clkdom, &rk_clk_composite_clknode_class, 313 &clkdef->clkdef); 314 if (clk == NULL) 315 return (1); 316 317 sc = clknode_get_softc(clk); 318 319 sc->muxdiv_offset = clkdef->muxdiv_offset; 320 321 sc->mux_shift = clkdef->mux_shift; 322 sc->mux_width = clkdef->mux_width; 323 sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift; 324 325 sc->div_shift = clkdef->div_shift; 326 sc->div_width = clkdef->div_width; 327 sc->div_mask = ((1 << clkdef->div_width) - 1) << sc->div_shift; 328 329 sc->flags = clkdef->flags; 330 331 clknode_register(clkdom, clk); 332 333 return (0); 334 } 335