1 /*- 2 * Copyright (c) 2019 Emmanuel Vadot <manu@freebsd.org> 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 1. Redistributions of source code must retain the above copyright 8 * notice, this list of conditions and the following disclaimer. 9 * 2. Redistributions in binary form must reproduce the above copyright 10 * notice, this list of conditions and the following disclaimer in the 11 * documentation and/or other materials provided with the distribution. 12 * 13 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 14 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 15 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 16 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 17 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 18 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 20 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 21 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 22 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 23 * SUCH DAMAGE. 24 */ 25 26 #include <sys/param.h> 27 #include <sys/systm.h> 28 #include <sys/bus.h> 29 30 #include <dev/clk/clk.h> 31 32 #include <dev/clk/allwinner/aw_clk.h> 33 #include <dev/clk/allwinner/aw_clk_m.h> 34 35 #include "clkdev_if.h" 36 37 /* 38 * clknode for clocks matching the formula : 39 * 40 * clk = clkin / m 41 * And that needs to potentially : 42 * 1) Set the parent freq 43 * 2) Support Setting the parent to a multiple 44 * 45 */ 46 47 struct aw_clk_m_sc { 48 uint32_t offset; 49 50 struct aw_clk_factor m; 51 52 uint32_t mux_shift; 53 uint32_t mux_mask; 54 uint32_t gate_shift; 55 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 static int 69 aw_clk_m_init(struct clknode *clk, device_t dev) 70 { 71 struct aw_clk_m_sc *sc; 72 uint32_t val, idx; 73 74 sc = clknode_get_softc(clk); 75 76 idx = 0; 77 if ((sc->flags & AW_CLK_HAS_MUX) != 0) { 78 DEVICE_LOCK(clk); 79 READ4(clk, sc->offset, &val); 80 DEVICE_UNLOCK(clk); 81 82 idx = (val & sc->mux_mask) >> sc->mux_shift; 83 } 84 85 clknode_init_parent_idx(clk, idx); 86 return (0); 87 } 88 89 static int 90 aw_clk_m_set_gate(struct clknode *clk, bool enable) 91 { 92 struct aw_clk_m_sc *sc; 93 uint32_t val; 94 95 sc = clknode_get_softc(clk); 96 97 if ((sc->flags & AW_CLK_HAS_GATE) == 0) 98 return (0); 99 100 DEVICE_LOCK(clk); 101 READ4(clk, sc->offset, &val); 102 if (enable) 103 val |= (1 << sc->gate_shift); 104 else 105 val &= ~(1 << sc->gate_shift); 106 WRITE4(clk, sc->offset, val); 107 DEVICE_UNLOCK(clk); 108 109 return (0); 110 } 111 112 static int 113 aw_clk_m_set_mux(struct clknode *clk, int index) 114 { 115 struct aw_clk_m_sc *sc; 116 uint32_t val; 117 118 sc = clknode_get_softc(clk); 119 120 if ((sc->flags & AW_CLK_HAS_MUX) == 0) 121 return (0); 122 123 DEVICE_LOCK(clk); 124 READ4(clk, sc->offset, &val); 125 val &= ~sc->mux_mask; 126 val |= index << sc->mux_shift; 127 WRITE4(clk, sc->offset, val); 128 DEVICE_UNLOCK(clk); 129 130 return (0); 131 } 132 133 static uint64_t 134 aw_clk_m_find_best(struct aw_clk_m_sc *sc, uint64_t fparent, uint64_t *fout, 135 uint32_t *factor_m) 136 { 137 uint64_t cur, best = 0; 138 uint32_t m, max_m, min_m; 139 140 *factor_m = 0; 141 142 max_m = aw_clk_factor_get_max(&sc->m); 143 min_m = aw_clk_factor_get_min(&sc->m); 144 145 for (m = min_m; m <= max_m; ) { 146 cur = fparent / m; 147 if (abs(*fout - cur) < abs(*fout - best)) { 148 best = cur; 149 *factor_m = m; 150 } 151 if ((sc->m.flags & AW_CLK_FACTOR_POWER_OF_TWO) != 0) 152 m <<= 1; 153 else 154 m++; 155 } 156 157 return (best); 158 } 159 160 static int 161 aw_clk_m_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout, 162 int flags, int *stop) 163 { 164 struct aw_clk_m_sc *sc; 165 struct clknode *p_clk; 166 uint64_t cur, best; 167 uint32_t val, m, best_m; 168 169 sc = clknode_get_softc(clk); 170 171 best = cur = 0; 172 173 best = aw_clk_m_find_best(sc, fparent, fout, 174 &best_m); 175 if ((best != *fout) && ((sc->flags & AW_CLK_SET_PARENT) != 0)) { 176 p_clk = clknode_get_parent(clk); 177 if (p_clk == NULL) { 178 printf("%s: Cannot get parent for clock %s\n", 179 __func__, 180 clknode_get_name(clk)); 181 return (ENXIO); 182 } 183 clknode_set_freq(p_clk, *fout, CLK_SET_ROUND_MULTIPLE, 0); 184 clknode_get_freq(p_clk, &fparent); 185 best = aw_clk_m_find_best(sc, fparent, fout, 186 &best_m); 187 } 188 189 if ((flags & CLK_SET_DRYRUN) != 0) { 190 *fout = best; 191 *stop = 1; 192 return (0); 193 } 194 195 if ((best < *fout) && 196 ((flags & CLK_SET_ROUND_DOWN) == 0)) { 197 *stop = 1; 198 return (ERANGE); 199 } 200 if ((best > *fout) && 201 ((flags & CLK_SET_ROUND_UP) == 0)) { 202 *stop = 1; 203 return (ERANGE); 204 } 205 206 DEVICE_LOCK(clk); 207 READ4(clk, sc->offset, &val); 208 209 m = aw_clk_factor_get_value(&sc->m, best_m); 210 val &= ~sc->m.mask; 211 val |= m << sc->m.shift; 212 213 WRITE4(clk, sc->offset, val); 214 DEVICE_UNLOCK(clk); 215 216 *fout = best; 217 *stop = 1; 218 219 return (0); 220 } 221 222 static int 223 aw_clk_m_recalc(struct clknode *clk, uint64_t *freq) 224 { 225 struct aw_clk_m_sc *sc; 226 uint32_t val, m; 227 228 sc = clknode_get_softc(clk); 229 230 DEVICE_LOCK(clk); 231 READ4(clk, sc->offset, &val); 232 DEVICE_UNLOCK(clk); 233 234 m = aw_clk_get_factor(val, &sc->m); 235 236 *freq = *freq / m; 237 238 return (0); 239 } 240 241 static clknode_method_t aw_m_clknode_methods[] = { 242 /* Device interface */ 243 CLKNODEMETHOD(clknode_init, aw_clk_m_init), 244 CLKNODEMETHOD(clknode_set_gate, aw_clk_m_set_gate), 245 CLKNODEMETHOD(clknode_set_mux, aw_clk_m_set_mux), 246 CLKNODEMETHOD(clknode_recalc_freq, aw_clk_m_recalc), 247 CLKNODEMETHOD(clknode_set_freq, aw_clk_m_set_freq), 248 CLKNODEMETHOD_END 249 }; 250 251 DEFINE_CLASS_1(aw_m_clknode, aw_m_clknode_class, aw_m_clknode_methods, 252 sizeof(struct aw_clk_m_sc), clknode_class); 253 254 int 255 aw_clk_m_register(struct clkdom *clkdom, struct aw_clk_m_def *clkdef) 256 { 257 struct clknode *clk; 258 struct aw_clk_m_sc *sc; 259 260 clk = clknode_create(clkdom, &aw_m_clknode_class, &clkdef->clkdef); 261 if (clk == NULL) 262 return (1); 263 264 sc = clknode_get_softc(clk); 265 266 sc->offset = clkdef->offset; 267 268 sc->m.shift = clkdef->m.shift; 269 sc->m.width = clkdef->m.width; 270 sc->m.mask = ((1 << sc->m.width) - 1) << sc->m.shift; 271 sc->m.value = clkdef->m.value; 272 sc->m.flags = clkdef->m.flags; 273 274 sc->mux_shift = clkdef->mux_shift; 275 sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift; 276 277 sc->gate_shift = clkdef->gate_shift; 278 279 sc->flags = clkdef->flags; 280 281 clknode_register(clkdom, clk); 282 283 return (0); 284 } 285