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