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