1 /*- 2 * Copyright 2016 Michal Meloun <mmel@FreeBSD.org> 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/param.h> 28 #include <sys/conf.h> 29 #include <sys/bus.h> 30 #include <sys/kernel.h> 31 #include <sys/systm.h> 32 33 #include <machine/bus.h> 34 35 #include <dev/clk/clk_div.h> 36 37 #include "clkdev_if.h" 38 39 #define WR4(_clk, off, val) \ 40 CLKDEV_WRITE_4(clknode_get_device(_clk), off, val) 41 #define RD4(_clk, off, val) \ 42 CLKDEV_READ_4(clknode_get_device(_clk), off, val) 43 #define MD4(_clk, off, clr, set ) \ 44 CLKDEV_MODIFY_4(clknode_get_device(_clk), off, clr, set) 45 #define DEVICE_LOCK(_clk) \ 46 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk)) 47 #define DEVICE_UNLOCK(_clk) \ 48 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk)) 49 50 static int clknode_div_init(struct clknode *clk, device_t dev); 51 static int clknode_div_recalc(struct clknode *clk, uint64_t *req); 52 static int clknode_div_set_freq(struct clknode *clknode, uint64_t fin, 53 uint64_t *fout, int flag, int *stop); 54 55 struct clknode_div_sc { 56 struct mtx *mtx; 57 struct resource *mem_res; 58 uint32_t offset; 59 uint32_t i_shift; 60 uint32_t i_mask; 61 uint32_t i_width; 62 uint32_t f_shift; 63 uint32_t f_mask; 64 uint32_t f_width; 65 int div_flags; 66 uint32_t divider; /* in natural form */ 67 68 struct clk_div_table *div_table; 69 }; 70 71 static clknode_method_t clknode_div_methods[] = { 72 /* Device interface */ 73 CLKNODEMETHOD(clknode_init, clknode_div_init), 74 CLKNODEMETHOD(clknode_recalc_freq, clknode_div_recalc), 75 CLKNODEMETHOD(clknode_set_freq, clknode_div_set_freq), 76 CLKNODEMETHOD_END 77 }; 78 DEFINE_CLASS_1(clknode_div, clknode_div_class, clknode_div_methods, 79 sizeof(struct clknode_div_sc), clknode_class); 80 81 static uint32_t 82 clknode_div_table_get_divider(struct clknode_div_sc *sc, uint32_t divider) 83 { 84 struct clk_div_table *table; 85 86 if (!(sc->div_flags & CLK_DIV_WITH_TABLE)) 87 return (divider); 88 89 for (table = sc->div_table; table->divider != 0; table++) 90 if (table->value == sc->divider) 91 return (table->divider); 92 93 return (0); 94 } 95 96 static int 97 clknode_div_table_get_value(struct clknode_div_sc *sc, uint32_t *divider) 98 { 99 struct clk_div_table *table; 100 101 if (!(sc->div_flags & CLK_DIV_WITH_TABLE)) 102 return (0); 103 104 for (table = sc->div_table; table->divider != 0; table++) 105 if (table->divider == *divider) { 106 *divider = table->value; 107 return (0); 108 } 109 110 return (ENOENT); 111 } 112 113 static int 114 clknode_div_init(struct clknode *clk, device_t dev) 115 { 116 uint32_t reg; 117 struct clknode_div_sc *sc; 118 uint32_t i_div, f_div; 119 int rv; 120 121 sc = clknode_get_softc(clk); 122 123 DEVICE_LOCK(clk); 124 rv = RD4(clk, sc->offset, ®); 125 DEVICE_UNLOCK(clk); 126 if (rv != 0) 127 return (rv); 128 129 i_div = (reg >> sc->i_shift) & sc->i_mask; 130 if (!(sc->div_flags & CLK_DIV_WITH_TABLE) && 131 !(sc->div_flags & CLK_DIV_ZERO_BASED)) 132 i_div++; 133 f_div = (reg >> sc->f_shift) & sc->f_mask; 134 sc->divider = i_div << sc->f_width | f_div; 135 136 sc->divider = clknode_div_table_get_divider(sc, sc->divider); 137 if (sc->divider == 0) 138 panic("%s: divider is zero!\n", clknode_get_name(clk)); 139 140 clknode_init_parent_idx(clk, 0); 141 return(0); 142 } 143 144 static int 145 clknode_div_recalc(struct clknode *clk, uint64_t *freq) 146 { 147 struct clknode_div_sc *sc; 148 149 sc = clknode_get_softc(clk); 150 if (sc->divider == 0) { 151 printf("%s: %s divider is zero!\n", clknode_get_name(clk), 152 __func__); 153 *freq = 0; 154 return(EINVAL); 155 } 156 *freq = (*freq << sc->f_width) / sc->divider; 157 return (0); 158 } 159 160 static int 161 clknode_div_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout, 162 int flags, int *stop) 163 { 164 struct clknode_div_sc *sc; 165 uint64_t divider, _fin, _fout; 166 uint32_t reg, i_div, f_div, hw_i_div; 167 int rv; 168 169 sc = clknode_get_softc(clk); 170 171 /* For fractional divider. */ 172 _fin = fin << sc->f_width; 173 divider = (_fin + *fout / 2) / *fout; 174 _fout = _fin / divider; 175 176 /* Rounding. */ 177 if ((flags & CLK_SET_ROUND_UP) && (*fout < _fout)) 178 divider--; 179 else if ((flags & CLK_SET_ROUND_DOWN) && (*fout > _fout)) 180 divider++; 181 182 /* Break divider into integer and fractional parts. */ 183 i_div = divider >> sc->f_width; 184 f_div = divider & sc->f_mask; 185 186 if (i_div == 0) { 187 printf("%s: %s integer divider is zero!\n", 188 clknode_get_name(clk), __func__); 189 return(EINVAL); 190 } 191 192 *stop = 1; 193 hw_i_div = i_div; 194 if (sc->div_flags & CLK_DIV_WITH_TABLE) { 195 if (clknode_div_table_get_value(sc, &hw_i_div) != 0) 196 return (ERANGE); 197 } else { 198 if (!(sc->div_flags & CLK_DIV_ZERO_BASED)) 199 hw_i_div--; 200 201 if (i_div > sc->i_mask) { 202 /* XXX Pass to parent or return error? */ 203 printf("%s: %s integer divider is too big: %u\n", 204 clknode_get_name(clk), __func__, i_div); 205 hw_i_div = sc->i_mask; 206 *stop = 0; 207 } 208 i_div = hw_i_div; 209 if (!(sc->div_flags & CLK_DIV_ZERO_BASED)) 210 i_div++; 211 } 212 213 divider = i_div << sc->f_width | f_div; 214 215 if ((flags & CLK_SET_DRYRUN) == 0) { 216 if ((*stop != 0) && 217 ((flags & (CLK_SET_ROUND_UP | CLK_SET_ROUND_DOWN)) == 0) && 218 (*fout != (_fin / divider))) 219 return (ERANGE); 220 221 DEVICE_LOCK(clk); 222 rv = MD4(clk, sc->offset, 223 (sc->i_mask << sc->i_shift) | (sc->f_mask << sc->f_shift), 224 (hw_i_div << sc->i_shift) | (f_div << sc->f_shift)); 225 if (rv != 0) { 226 DEVICE_UNLOCK(clk); 227 return (rv); 228 } 229 RD4(clk, sc->offset, ®); 230 DEVICE_UNLOCK(clk); 231 232 sc->divider = divider; 233 } 234 235 *fout = _fin / divider; 236 return (0); 237 } 238 239 int 240 clknode_div_register(struct clkdom *clkdom, struct clk_div_def *clkdef) 241 { 242 struct clknode *clk; 243 struct clknode_div_sc *sc; 244 245 clk = clknode_create(clkdom, &clknode_div_class, &clkdef->clkdef); 246 if (clk == NULL) 247 return (1); 248 249 sc = clknode_get_softc(clk); 250 sc->offset = clkdef->offset; 251 sc->i_shift = clkdef->i_shift; 252 sc->i_width = clkdef->i_width; 253 sc->i_mask = (1 << clkdef->i_width) - 1; 254 sc->f_shift = clkdef->f_shift; 255 sc->f_width = clkdef->f_width; 256 sc->f_mask = (1 << clkdef->f_width) - 1; 257 sc->div_flags = clkdef->div_flags; 258 sc->div_table = clkdef->div_table; 259 260 clknode_register(clkdom, clk); 261 return (0); 262 } 263