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