1 /*- 2 * Copyright 2016 Michal Meloun <[email protected]> 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions 7 * are met: 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24 * SUCH DAMAGE. 25 */ 26 27 #include <sys/cdefs.h> 28 __FBSDID("$FreeBSD$"); 29 30 31 #include <sys/param.h> 32 #include <sys/conf.h> 33 #include <sys/bus.h> 34 #include <sys/kernel.h> 35 #include <sys/systm.h> 36 37 #include <machine/bus.h> 38 39 #include <dev/extres/clk/clk_div.h> 40 41 #include "clkdev_if.h" 42 43 #define WR4(_clk, off, val) \ 44 CLKDEV_WRITE_4(clknode_get_device(_clk), off, val) 45 #define RD4(_clk, off, val) \ 46 CLKDEV_READ_4(clknode_get_device(_clk), off, val) 47 #define MD4(_clk, off, clr, set ) \ 48 CLKDEV_MODIFY_4(clknode_get_device(_clk), off, clr, set) 49 #define DEVICE_LOCK(_clk) \ 50 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk)) 51 #define DEVICE_UNLOCK(_clk) \ 52 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk)) 53 54 static int clknode_div_init(struct clknode *clk, device_t dev); 55 static int clknode_div_recalc(struct clknode *clk, uint64_t *req); 56 static int clknode_div_set_freq(struct clknode *clknode, uint64_t fin, 57 uint64_t *fout, int flag, int *stop); 58 59 struct clknode_div_sc { 60 struct mtx *mtx; 61 struct resource *mem_res; 62 uint32_t offset; 63 uint32_t i_shift; 64 uint32_t i_mask; 65 uint32_t i_width; 66 uint32_t f_shift; 67 uint32_t f_mask; 68 uint32_t f_width; 69 int div_flags; 70 uint32_t divider; /* in natural form */ 71 72 struct clk_div_table *div_table; 73 }; 74 75 static clknode_method_t clknode_div_methods[] = { 76 /* Device interface */ 77 CLKNODEMETHOD(clknode_init, clknode_div_init), 78 CLKNODEMETHOD(clknode_recalc_freq, clknode_div_recalc), 79 CLKNODEMETHOD(clknode_set_freq, clknode_div_set_freq), 80 CLKNODEMETHOD_END 81 }; 82 DEFINE_CLASS_1(clknode_div, clknode_div_class, clknode_div_methods, 83 sizeof(struct clknode_div_sc), clknode_class); 84 85 static uint32_t 86 clknode_div_table_get_divider(struct clknode_div_sc *sc, uint32_t divider) 87 { 88 struct clk_div_table *table; 89 90 if (!(sc->div_flags & CLK_DIV_WITH_TABLE)) 91 return (divider); 92 93 for (table = sc->div_table; table->divider != 0; table++) 94 if (table->value == sc->divider) 95 return (table->divider); 96 97 return (0); 98 } 99 100 static int 101 clknode_div_table_get_value(struct clknode_div_sc *sc, uint32_t *divider) 102 { 103 struct clk_div_table *table; 104 105 if (!(sc->div_flags & CLK_DIV_WITH_TABLE)) 106 return (0); 107 108 for (table = sc->div_table; table->divider != 0; table++) 109 if (table->divider == *divider) { 110 *divider = table->value; 111 return (0); 112 } 113 114 return (ENOENT); 115 } 116 117 static int 118 clknode_div_init(struct clknode *clk, device_t dev) 119 { 120 uint32_t reg; 121 struct clknode_div_sc *sc; 122 uint32_t i_div, f_div; 123 int rv; 124 125 sc = clknode_get_softc(clk); 126 127 DEVICE_LOCK(clk); 128 rv = RD4(clk, sc->offset, ®); 129 DEVICE_UNLOCK(clk); 130 if (rv != 0) 131 return (rv); 132 133 i_div = (reg >> sc->i_shift) & sc->i_mask; 134 if (!(sc->div_flags & CLK_DIV_ZERO_BASED)) 135 i_div++; 136 f_div = (reg >> sc->f_shift) & sc->f_mask; 137 sc->divider = i_div << sc->f_width | f_div; 138 139 sc->divider = clknode_div_table_get_divider(sc, sc->divider); 140 if (sc->divider == 0) 141 panic("%s: divider is zero!\n", clknode_get_name(clk)); 142 143 clknode_init_parent_idx(clk, 0); 144 return(0); 145 } 146 147 static int 148 clknode_div_recalc(struct clknode *clk, uint64_t *freq) 149 { 150 struct clknode_div_sc *sc; 151 152 sc = clknode_get_softc(clk); 153 if (sc->divider == 0) { 154 printf("%s: %s divider is zero!\n", clknode_get_name(clk), 155 __func__); 156 *freq = 0; 157 return(EINVAL); 158 } 159 *freq = (*freq << sc->f_width) / sc->divider; 160 return (0); 161 } 162 163 static int 164 clknode_div_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout, 165 int flags, int *stop) 166 { 167 struct clknode_div_sc *sc; 168 uint64_t divider, _fin, _fout; 169 uint32_t div_value, reg, i_div, f_div, hw_i_div; 170 int rv; 171 172 sc = clknode_get_softc(clk); 173 174 /* For fractional divider. */ 175 _fin = fin << sc->f_width; 176 divider = (_fin + *fout / 2) / *fout; 177 _fout = _fin / divider; 178 179 /* Rounding. */ 180 if ((flags & CLK_SET_ROUND_UP) && (*fout < _fout)) 181 divider--; 182 else if ((flags & CLK_SET_ROUND_DOWN) && (*fout > _fout)) 183 divider++; 184 185 /* Break divider into integer and fractional parts. */ 186 i_div = divider >> sc->f_width; 187 f_div = divider & sc->f_mask; 188 189 if (i_div == 0) { 190 printf("%s: %s integer divider is zero!\n", 191 clknode_get_name(clk), __func__); 192 return(EINVAL); 193 } 194 195 hw_i_div = i_div; 196 if (!(sc->div_flags & CLK_DIV_ZERO_BASED)) 197 hw_i_div--; 198 199 *stop = 1; 200 if (hw_i_div > sc->i_mask && 201 ((sc->div_flags & CLK_DIV_WITH_TABLE) == 0)) { 202 /* XXX Or only return error? */ 203 printf("%s: %s integer divider is too big: %u\n", 204 clknode_get_name(clk), __func__, hw_i_div); 205 hw_i_div = sc->i_mask; 206 *stop = 0; 207 } 208 209 i_div = hw_i_div; 210 if (!(sc->div_flags & CLK_DIV_ZERO_BASED)) 211 i_div++; 212 divider = i_div << sc->f_width | f_div; 213 214 if ((flags & CLK_SET_DRYRUN) == 0) { 215 if ((*stop != 0) && 216 ((flags & (CLK_SET_ROUND_UP | CLK_SET_ROUND_DOWN)) == 0) && 217 (*fout != (_fin / divider))) 218 return (ERANGE); 219 220 div_value = divider; 221 if (clknode_div_table_get_value(sc, &div_value) != 0) 222 return (ERANGE); 223 if (div_value != divider) 224 i_div = div_value; 225 226 DEVICE_LOCK(clk); 227 rv = MD4(clk, sc->offset, 228 (sc->i_mask << sc->i_shift) | (sc->f_mask << sc->f_shift), 229 (i_div << sc->i_shift) | (f_div << sc->f_shift)); 230 if (rv != 0) { 231 DEVICE_UNLOCK(clk); 232 return (rv); 233 } 234 RD4(clk, sc->offset, ®); 235 DEVICE_UNLOCK(clk); 236 237 sc->divider = divider; 238 } 239 240 *fout = _fin / divider; 241 return (0); 242 } 243 244 int 245 clknode_div_register(struct clkdom *clkdom, struct clk_div_def *clkdef) 246 { 247 struct clknode *clk; 248 struct clknode_div_sc *sc; 249 250 clk = clknode_create(clkdom, &clknode_div_class, &clkdef->clkdef); 251 if (clk == NULL) 252 return (1); 253 254 sc = clknode_get_softc(clk); 255 sc->offset = clkdef->offset; 256 sc->i_shift = clkdef->i_shift; 257 sc->i_width = clkdef->i_width; 258 sc->i_mask = (1 << clkdef->i_width) - 1; 259 sc->f_shift = clkdef->f_shift; 260 sc->f_width = clkdef->f_width; 261 sc->f_mask = (1 << clkdef->f_width) - 1; 262 sc->div_flags = clkdef->div_flags; 263 sc->div_table = clkdef->div_table; 264 265 clknode_register(clkdom, clk); 266 return (0); 267 } 268