1 /*- 2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD 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 * $FreeBSD$ 28 */ 29 30 #include <sys/cdefs.h> 31 __FBSDID("$FreeBSD$"); 32 33 #include <sys/param.h> 34 #include <sys/systm.h> 35 #include <sys/bus.h> 36 37 #include <dev/extres/clk/clk.h> 38 39 #include <arm64/freescale/imx/clk/imx_clk_composite.h> 40 41 #include "clkdev_if.h" 42 43 #define TARGET_ROOT_ENABLE (1 << 28) 44 #define TARGET_ROOT_MUX(n) ((n) << 24) 45 #define TARGET_ROOT_MUX_MASK (7 << 24) 46 #define TARGET_ROOT_MUX_SHIFT 24 47 #define TARGET_ROOT_PRE_PODF(n) ((((n) - 1) & 0x7) << 16) 48 #define TARGET_ROOT_PRE_PODF_MASK (0x7 << 16) 49 #define TARGET_ROOT_PRE_PODF_SHIFT 16 50 #define TARGET_ROOT_PRE_PODF_MAX 7 51 #define TARGET_ROOT_POST_PODF(n) ((((n) - 1) & 0x3f) << 0) 52 #define TARGET_ROOT_POST_PODF_MASK (0x3f << 0) 53 #define TARGET_ROOT_POST_PODF_SHIFT 0 54 #define TARGET_ROOT_POST_PODF_MAX 0x3f 55 56 struct imx_clk_composite_sc { 57 uint32_t offset; 58 uint32_t flags; 59 }; 60 61 #define WRITE4(_clk, off, val) \ 62 CLKDEV_WRITE_4(clknode_get_device(_clk), off, val) 63 #define READ4(_clk, off, val) \ 64 CLKDEV_READ_4(clknode_get_device(_clk), off, val) 65 #define DEVICE_LOCK(_clk) \ 66 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk)) 67 #define DEVICE_UNLOCK(_clk) \ 68 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk)) 69 70 #define IMX_CLK_COMPOSITE_MASK_SHIFT 16 71 72 #if 0 73 #define dprintf(format, arg...) \ 74 printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg) 75 #else 76 #define dprintf(format, arg...) 77 #endif 78 79 static int 80 imx_clk_composite_init(struct clknode *clk, device_t dev) 81 { 82 struct imx_clk_composite_sc *sc; 83 uint32_t val, idx; 84 85 sc = clknode_get_softc(clk); 86 87 DEVICE_LOCK(clk); 88 READ4(clk, sc->offset, &val); 89 DEVICE_UNLOCK(clk); 90 idx = (val & TARGET_ROOT_MUX_MASK) >> TARGET_ROOT_MUX_SHIFT; 91 92 clknode_init_parent_idx(clk, idx); 93 94 return (0); 95 } 96 97 static int 98 imx_clk_composite_set_gate(struct clknode *clk, bool enable) 99 { 100 struct imx_clk_composite_sc *sc; 101 uint32_t val = 0; 102 103 sc = clknode_get_softc(clk); 104 105 dprintf("%sabling gate\n", enable ? "En" : "Dis"); 106 DEVICE_LOCK(clk); 107 READ4(clk, sc->offset, &val); 108 if (enable) 109 val |= TARGET_ROOT_ENABLE; 110 else 111 val &= ~(TARGET_ROOT_ENABLE); 112 WRITE4(clk, sc->offset, val); 113 DEVICE_UNLOCK(clk); 114 115 return (0); 116 } 117 118 static int 119 imx_clk_composite_set_mux(struct clknode *clk, int index) 120 { 121 struct imx_clk_composite_sc *sc; 122 uint32_t val = 0; 123 124 sc = clknode_get_softc(clk); 125 126 dprintf("Set mux to %d\n", index); 127 DEVICE_LOCK(clk); 128 READ4(clk, sc->offset, &val); 129 val &= ~(TARGET_ROOT_MUX_MASK); 130 val |= TARGET_ROOT_MUX(index); 131 WRITE4(clk, sc->offset, val); 132 DEVICE_UNLOCK(clk); 133 134 return (0); 135 } 136 137 static int 138 imx_clk_composite_recalc(struct clknode *clk, uint64_t *freq) 139 { 140 struct imx_clk_composite_sc *sc; 141 uint32_t reg, pre_div, post_div; 142 143 sc = clknode_get_softc(clk); 144 145 DEVICE_LOCK(clk); 146 READ4(clk, sc->offset, ®); 147 DEVICE_UNLOCK(clk); 148 149 pre_div = ((reg & TARGET_ROOT_PRE_PODF_MASK) 150 >> TARGET_ROOT_PRE_PODF_SHIFT) + 1; 151 post_div = ((reg & TARGET_ROOT_POST_PODF_MASK) 152 >> TARGET_ROOT_POST_PODF_SHIFT) + 1; 153 154 dprintf("parent_freq=%ju, div=%u\n", *freq, div); 155 *freq = *freq / pre_div / post_div; 156 dprintf("Final freq=%ju\n", *freq); 157 return (0); 158 } 159 160 static int 161 imx_clk_composite_find_best(uint64_t fparent, uint64_t ftarget, 162 uint32_t *pre_div, uint32_t *post_div, int flags) 163 { 164 uint32_t prediv, postdiv, best_prediv, best_postdiv; 165 int64_t diff, best_diff; 166 uint64_t cur; 167 168 best_diff = INT64_MAX; 169 for (prediv = 1; prediv <= TARGET_ROOT_PRE_PODF_MAX + 1; prediv++) { 170 for (postdiv = 1; postdiv <= TARGET_ROOT_POST_PODF_MAX + 1; postdiv++) { 171 cur= fparent / prediv / postdiv; 172 diff = (int64_t)ftarget - (int64_t)cur; 173 if (flags & CLK_SET_ROUND_DOWN) { 174 if (diff >= 0 && diff < best_diff) { 175 best_diff = diff; 176 best_prediv = prediv; 177 best_postdiv = postdiv; 178 } 179 } 180 else if (flags & CLK_SET_ROUND_UP) { 181 if (diff <= 0 && abs(diff) < best_diff) { 182 best_diff = diff; 183 best_prediv = prediv; 184 best_postdiv = postdiv; 185 } 186 } 187 else { 188 if (abs(diff) < best_diff) { 189 best_diff = abs(diff); 190 best_prediv = prediv; 191 best_postdiv = postdiv; 192 } 193 } 194 } 195 } 196 197 if (best_diff == INT64_MAX) 198 return (ERANGE); 199 200 *pre_div = best_prediv; 201 *post_div = best_postdiv; 202 203 return (0); 204 } 205 206 static int 207 imx_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout, 208 int flags, int *stop) 209 { 210 struct imx_clk_composite_sc *sc; 211 struct clknode *p_clk; 212 const char **p_names; 213 int p_idx, best_parent; 214 int64_t best_diff, diff; 215 int32_t best_pre_div __unused, best_post_div __unused; 216 int32_t pre_div, post_div; 217 uint64_t cur, best; 218 uint32_t val; 219 220 sc = clknode_get_softc(clk); 221 dprintf("Finding best parent/div for target freq of %ju\n", *fout); 222 p_names = clknode_get_parent_names(clk); 223 224 best_diff = 0; 225 226 for (p_idx = 0; p_idx != clknode_get_parents_num(clk); p_idx++) { 227 p_clk = clknode_find_by_name(p_names[p_idx]); 228 clknode_get_freq(p_clk, &fparent); 229 dprintf("Testing with parent %s (%d) at freq %ju\n", 230 clknode_get_name(p_clk), p_idx, fparent); 231 232 if (!imx_clk_composite_find_best(fparent, *fout, &pre_div, &post_div, sc->flags)) 233 continue; 234 cur = fparent / pre_div / post_div; 235 diff = abs((int64_t)*fout - (int64_t)cur); 236 if (diff < best_diff) { 237 best = cur; 238 best_diff = diff; 239 best_pre_div = pre_div; 240 best_post_div = post_div; 241 best_parent = p_idx; 242 dprintf("Best parent so far %s (%d) with best freq at " 243 "%ju\n", clknode_get_name(p_clk), p_idx, best); 244 } 245 } 246 247 *stop = 1; 248 if (best_diff == INT64_MAX) 249 return (ERANGE); 250 251 if ((flags & CLK_SET_DRYRUN) != 0) { 252 *fout = best; 253 return (0); 254 } 255 256 p_idx = clknode_get_parent_idx(clk); 257 if (p_idx != best_parent) { 258 dprintf("Switching parent index from %d to %d\n", p_idx, 259 best_parent); 260 clknode_set_parent_by_idx(clk, best_parent); 261 } 262 263 dprintf("Setting dividers to pre=%d, post=%d\n", best_pre_div, best_post_div); 264 265 DEVICE_LOCK(clk); 266 READ4(clk, sc->offset, &val); 267 val &= ~(TARGET_ROOT_PRE_PODF_MASK | TARGET_ROOT_POST_PODF_MASK); 268 val |= TARGET_ROOT_PRE_PODF(pre_div); 269 val |= TARGET_ROOT_POST_PODF(post_div); 270 DEVICE_UNLOCK(clk); 271 272 *fout = best; 273 return (0); 274 } 275 276 static clknode_method_t imx_clk_composite_clknode_methods[] = { 277 /* Device interface */ 278 CLKNODEMETHOD(clknode_init, imx_clk_composite_init), 279 CLKNODEMETHOD(clknode_set_gate, imx_clk_composite_set_gate), 280 CLKNODEMETHOD(clknode_set_mux, imx_clk_composite_set_mux), 281 CLKNODEMETHOD(clknode_recalc_freq, imx_clk_composite_recalc), 282 CLKNODEMETHOD(clknode_set_freq, imx_clk_composite_set_freq), 283 CLKNODEMETHOD_END 284 }; 285 286 DEFINE_CLASS_1(imx_clk_composite_clknode, imx_clk_composite_clknode_class, 287 imx_clk_composite_clknode_methods, sizeof(struct imx_clk_composite_sc), 288 clknode_class); 289 290 int 291 imx_clk_composite_register(struct clkdom *clkdom, 292 struct imx_clk_composite_def *clkdef) 293 { 294 struct clknode *clk; 295 struct imx_clk_composite_sc *sc; 296 297 clk = clknode_create(clkdom, &imx_clk_composite_clknode_class, 298 &clkdef->clkdef); 299 if (clk == NULL) 300 return (1); 301 302 sc = clknode_get_softc(clk); 303 304 sc->offset = clkdef->offset; 305 sc->flags = clkdef->flags; 306 307 clknode_register(clkdom, clk); 308 309 return (0); 310 } 311