1ee81f4eeSChao Xie /* 2ee81f4eeSChao Xie * mmp mix(div and mux) clock operation source file 3ee81f4eeSChao Xie * 4ee81f4eeSChao Xie * Copyright (C) 2014 Marvell 5ee81f4eeSChao Xie * Chao Xie <chao.xie@marvell.com> 6ee81f4eeSChao Xie * 7ee81f4eeSChao Xie * This file is licensed under the terms of the GNU General Public 8ee81f4eeSChao Xie * License version 2. This program is licensed "as is" without any 9ee81f4eeSChao Xie * warranty of any kind, whether express or implied. 10ee81f4eeSChao Xie */ 11ee81f4eeSChao Xie 12ee81f4eeSChao Xie #include <linux/clk-provider.h> 13ee81f4eeSChao Xie #include <linux/slab.h> 14ee81f4eeSChao Xie #include <linux/io.h> 15ee81f4eeSChao Xie #include <linux/err.h> 16ee81f4eeSChao Xie 17ee81f4eeSChao Xie #include "clk.h" 18ee81f4eeSChao Xie 19ee81f4eeSChao Xie /* 20ee81f4eeSChao Xie * The mix clock is a clock combined mux and div type clock. 21ee81f4eeSChao Xie * Because the div field and mux field need to be set at same 22ee81f4eeSChao Xie * time, we can not divide it into 2 types of clock 23ee81f4eeSChao Xie */ 24ee81f4eeSChao Xie 25ee81f4eeSChao Xie #define to_clk_mix(hw) container_of(hw, struct mmp_clk_mix, hw) 26ee81f4eeSChao Xie 27ee81f4eeSChao Xie static unsigned int _get_maxdiv(struct mmp_clk_mix *mix) 28ee81f4eeSChao Xie { 29ee81f4eeSChao Xie unsigned int div_mask = (1 << mix->reg_info.width_div) - 1; 30ee81f4eeSChao Xie unsigned int maxdiv = 0; 31ee81f4eeSChao Xie struct clk_div_table *clkt; 32ee81f4eeSChao Xie 33ee81f4eeSChao Xie if (mix->div_flags & CLK_DIVIDER_ONE_BASED) 34ee81f4eeSChao Xie return div_mask; 35ee81f4eeSChao Xie if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO) 36ee81f4eeSChao Xie return 1 << div_mask; 37ee81f4eeSChao Xie if (mix->div_table) { 38ee81f4eeSChao Xie for (clkt = mix->div_table; clkt->div; clkt++) 39ee81f4eeSChao Xie if (clkt->div > maxdiv) 40ee81f4eeSChao Xie maxdiv = clkt->div; 41ee81f4eeSChao Xie return maxdiv; 42ee81f4eeSChao Xie } 43ee81f4eeSChao Xie return div_mask + 1; 44ee81f4eeSChao Xie } 45ee81f4eeSChao Xie 46ee81f4eeSChao Xie static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val) 47ee81f4eeSChao Xie { 48ee81f4eeSChao Xie struct clk_div_table *clkt; 49ee81f4eeSChao Xie 50ee81f4eeSChao Xie if (mix->div_flags & CLK_DIVIDER_ONE_BASED) 51ee81f4eeSChao Xie return val; 52ee81f4eeSChao Xie if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO) 53ee81f4eeSChao Xie return 1 << val; 54ee81f4eeSChao Xie if (mix->div_table) { 55ee81f4eeSChao Xie for (clkt = mix->div_table; clkt->div; clkt++) 56ee81f4eeSChao Xie if (clkt->val == val) 57ee81f4eeSChao Xie return clkt->div; 58ee81f4eeSChao Xie if (clkt->div == 0) 59ee81f4eeSChao Xie return 0; 60ee81f4eeSChao Xie } 61ee81f4eeSChao Xie return val + 1; 62ee81f4eeSChao Xie } 63ee81f4eeSChao Xie 64ee81f4eeSChao Xie static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val) 65ee81f4eeSChao Xie { 66ee81f4eeSChao Xie int num_parents = __clk_get_num_parents(mix->hw.clk); 67ee81f4eeSChao Xie int i; 68ee81f4eeSChao Xie 69ee81f4eeSChao Xie if (mix->mux_flags & CLK_MUX_INDEX_BIT) 70ee81f4eeSChao Xie return ffs(val) - 1; 71ee81f4eeSChao Xie if (mix->mux_flags & CLK_MUX_INDEX_ONE) 72ee81f4eeSChao Xie return val - 1; 73ee81f4eeSChao Xie if (mix->mux_table) { 74ee81f4eeSChao Xie for (i = 0; i < num_parents; i++) 75ee81f4eeSChao Xie if (mix->mux_table[i] == val) 76ee81f4eeSChao Xie return i; 77ee81f4eeSChao Xie if (i == num_parents) 78ee81f4eeSChao Xie return 0; 79ee81f4eeSChao Xie } 80ee81f4eeSChao Xie 81ee81f4eeSChao Xie return val; 82ee81f4eeSChao Xie } 83ee81f4eeSChao Xie static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div) 84ee81f4eeSChao Xie { 85ee81f4eeSChao Xie struct clk_div_table *clkt; 86ee81f4eeSChao Xie 87ee81f4eeSChao Xie if (mix->div_flags & CLK_DIVIDER_ONE_BASED) 88ee81f4eeSChao Xie return div; 89ee81f4eeSChao Xie if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO) 90ee81f4eeSChao Xie return __ffs(div); 91ee81f4eeSChao Xie if (mix->div_table) { 92ee81f4eeSChao Xie for (clkt = mix->div_table; clkt->div; clkt++) 93ee81f4eeSChao Xie if (clkt->div == div) 94ee81f4eeSChao Xie return clkt->val; 95ee81f4eeSChao Xie if (clkt->div == 0) 96ee81f4eeSChao Xie return 0; 97ee81f4eeSChao Xie } 98ee81f4eeSChao Xie 99ee81f4eeSChao Xie return div - 1; 100ee81f4eeSChao Xie } 101ee81f4eeSChao Xie 102ee81f4eeSChao Xie static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux) 103ee81f4eeSChao Xie { 104ee81f4eeSChao Xie if (mix->mux_table) 105ee81f4eeSChao Xie return mix->mux_table[mux]; 106ee81f4eeSChao Xie 107ee81f4eeSChao Xie return mux; 108ee81f4eeSChao Xie } 109ee81f4eeSChao Xie 110ee81f4eeSChao Xie static void _filter_clk_table(struct mmp_clk_mix *mix, 111ee81f4eeSChao Xie struct mmp_clk_mix_clk_table *table, 112ee81f4eeSChao Xie unsigned int table_size) 113ee81f4eeSChao Xie { 114ee81f4eeSChao Xie int i; 115ee81f4eeSChao Xie struct mmp_clk_mix_clk_table *item; 116ee81f4eeSChao Xie struct clk *parent, *clk; 117ee81f4eeSChao Xie unsigned long parent_rate; 118ee81f4eeSChao Xie 119ee81f4eeSChao Xie clk = mix->hw.clk; 120ee81f4eeSChao Xie 121ee81f4eeSChao Xie for (i = 0; i < table_size; i++) { 122ee81f4eeSChao Xie item = &table[i]; 123ee81f4eeSChao Xie parent = clk_get_parent_by_index(clk, item->parent_index); 124ee81f4eeSChao Xie parent_rate = __clk_get_rate(parent); 125ee81f4eeSChao Xie if (parent_rate % item->rate) { 126ee81f4eeSChao Xie item->valid = 0; 127ee81f4eeSChao Xie } else { 128ee81f4eeSChao Xie item->divisor = parent_rate / item->rate; 129ee81f4eeSChao Xie item->valid = 1; 130ee81f4eeSChao Xie } 131ee81f4eeSChao Xie } 132ee81f4eeSChao Xie } 133ee81f4eeSChao Xie 134ee81f4eeSChao Xie static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val, 135ee81f4eeSChao Xie unsigned int change_mux, unsigned int change_div) 136ee81f4eeSChao Xie { 137ee81f4eeSChao Xie struct mmp_clk_mix_reg_info *ri = &mix->reg_info; 138ee81f4eeSChao Xie u8 width, shift; 139ee81f4eeSChao Xie u32 mux_div, fc_req; 140ee81f4eeSChao Xie int ret, timeout = 50; 141ee81f4eeSChao Xie unsigned long flags = 0; 142ee81f4eeSChao Xie 143ee81f4eeSChao Xie if (!change_mux && !change_div) 144ee81f4eeSChao Xie return -EINVAL; 145ee81f4eeSChao Xie 146ee81f4eeSChao Xie if (mix->lock) 147ee81f4eeSChao Xie spin_lock_irqsave(mix->lock, flags); 148ee81f4eeSChao Xie 149ee81f4eeSChao Xie if (mix->type == MMP_CLK_MIX_TYPE_V1 150ee81f4eeSChao Xie || mix->type == MMP_CLK_MIX_TYPE_V2) 151ee81f4eeSChao Xie mux_div = readl(ri->reg_clk_ctrl); 152ee81f4eeSChao Xie else 153ee81f4eeSChao Xie mux_div = readl(ri->reg_clk_sel); 154ee81f4eeSChao Xie 155ee81f4eeSChao Xie if (change_div) { 156ee81f4eeSChao Xie width = ri->width_div; 157ee81f4eeSChao Xie shift = ri->shift_div; 158ee81f4eeSChao Xie mux_div &= ~MMP_CLK_BITS_MASK(width, shift); 159ee81f4eeSChao Xie mux_div |= MMP_CLK_BITS_SET_VAL(div_val, width, shift); 160ee81f4eeSChao Xie } 161ee81f4eeSChao Xie 162ee81f4eeSChao Xie if (change_mux) { 163ee81f4eeSChao Xie width = ri->width_mux; 164ee81f4eeSChao Xie shift = ri->shift_mux; 165ee81f4eeSChao Xie mux_div &= ~MMP_CLK_BITS_MASK(width, shift); 166ee81f4eeSChao Xie mux_div |= MMP_CLK_BITS_SET_VAL(mux_val, width, shift); 167ee81f4eeSChao Xie } 168ee81f4eeSChao Xie 169ee81f4eeSChao Xie if (mix->type == MMP_CLK_MIX_TYPE_V1) { 170ee81f4eeSChao Xie writel(mux_div, ri->reg_clk_ctrl); 171ee81f4eeSChao Xie } else if (mix->type == MMP_CLK_MIX_TYPE_V2) { 172ee81f4eeSChao Xie mux_div |= (1 << ri->bit_fc); 173ee81f4eeSChao Xie writel(mux_div, ri->reg_clk_ctrl); 174ee81f4eeSChao Xie 175ee81f4eeSChao Xie do { 176ee81f4eeSChao Xie fc_req = readl(ri->reg_clk_ctrl); 177ee81f4eeSChao Xie timeout--; 178ee81f4eeSChao Xie if (!(fc_req & (1 << ri->bit_fc))) 179ee81f4eeSChao Xie break; 180ee81f4eeSChao Xie } while (timeout); 181ee81f4eeSChao Xie 182ee81f4eeSChao Xie if (timeout == 0) { 183ee81f4eeSChao Xie pr_err("%s:%s cannot do frequency change\n", 184ee81f4eeSChao Xie __func__, __clk_get_name(mix->hw.clk)); 185ee81f4eeSChao Xie ret = -EBUSY; 186ee81f4eeSChao Xie goto error; 187ee81f4eeSChao Xie } 188ee81f4eeSChao Xie } else { 189ee81f4eeSChao Xie fc_req = readl(ri->reg_clk_ctrl); 190ee81f4eeSChao Xie fc_req |= 1 << ri->bit_fc; 191ee81f4eeSChao Xie writel(fc_req, ri->reg_clk_ctrl); 192ee81f4eeSChao Xie writel(mux_div, ri->reg_clk_sel); 193ee81f4eeSChao Xie fc_req &= ~(1 << ri->bit_fc); 194ee81f4eeSChao Xie } 195ee81f4eeSChao Xie 196ee81f4eeSChao Xie ret = 0; 197ee81f4eeSChao Xie error: 198ee81f4eeSChao Xie if (mix->lock) 199ee81f4eeSChao Xie spin_unlock_irqrestore(mix->lock, flags); 200ee81f4eeSChao Xie 201ee81f4eeSChao Xie return ret; 202ee81f4eeSChao Xie } 203ee81f4eeSChao Xie 204ee81f4eeSChao Xie static long mmp_clk_mix_determine_rate(struct clk_hw *hw, unsigned long rate, 205ee81f4eeSChao Xie unsigned long *best_parent_rate, 206ee81f4eeSChao Xie struct clk **best_parent_clk) 207ee81f4eeSChao Xie { 208ee81f4eeSChao Xie struct mmp_clk_mix *mix = to_clk_mix(hw); 209ee81f4eeSChao Xie struct mmp_clk_mix_clk_table *item; 210ee81f4eeSChao Xie struct clk *parent, *parent_best, *mix_clk; 211ee81f4eeSChao Xie unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best; 212ee81f4eeSChao Xie unsigned long gap, gap_best; 213ee81f4eeSChao Xie u32 div_val_max; 214ee81f4eeSChao Xie unsigned int div; 215ee81f4eeSChao Xie int i, j; 216ee81f4eeSChao Xie 217ee81f4eeSChao Xie mix_clk = hw->clk; 218ee81f4eeSChao Xie 219ee81f4eeSChao Xie parent = NULL; 220ee81f4eeSChao Xie mix_rate_best = 0; 221ee81f4eeSChao Xie parent_rate_best = 0; 222ee81f4eeSChao Xie gap_best = rate; 223ee81f4eeSChao Xie parent_best = NULL; 224ee81f4eeSChao Xie 225ee81f4eeSChao Xie if (mix->table) { 226ee81f4eeSChao Xie for (i = 0; i < mix->table_size; i++) { 227ee81f4eeSChao Xie item = &mix->table[i]; 228ee81f4eeSChao Xie if (item->valid == 0) 229ee81f4eeSChao Xie continue; 230ee81f4eeSChao Xie parent = clk_get_parent_by_index(mix_clk, 231ee81f4eeSChao Xie item->parent_index); 232ee81f4eeSChao Xie parent_rate = __clk_get_rate(parent); 233ee81f4eeSChao Xie mix_rate = parent_rate / item->divisor; 234ee81f4eeSChao Xie gap = abs(mix_rate - rate); 235ee81f4eeSChao Xie if (parent_best == NULL || gap < gap_best) { 236ee81f4eeSChao Xie parent_best = parent; 237ee81f4eeSChao Xie parent_rate_best = parent_rate; 238ee81f4eeSChao Xie mix_rate_best = mix_rate; 239ee81f4eeSChao Xie gap_best = gap; 240ee81f4eeSChao Xie if (gap_best == 0) 241ee81f4eeSChao Xie goto found; 242ee81f4eeSChao Xie } 243ee81f4eeSChao Xie } 244ee81f4eeSChao Xie } else { 245ee81f4eeSChao Xie for (i = 0; i < __clk_get_num_parents(mix_clk); i++) { 246ee81f4eeSChao Xie parent = clk_get_parent_by_index(mix_clk, i); 247ee81f4eeSChao Xie parent_rate = __clk_get_rate(parent); 248ee81f4eeSChao Xie div_val_max = _get_maxdiv(mix); 249ee81f4eeSChao Xie for (j = 0; j < div_val_max; j++) { 250ee81f4eeSChao Xie div = _get_div(mix, j); 251ee81f4eeSChao Xie mix_rate = parent_rate / div; 252ee81f4eeSChao Xie gap = abs(mix_rate - rate); 253ee81f4eeSChao Xie if (parent_best == NULL || gap < gap_best) { 254ee81f4eeSChao Xie parent_best = parent; 255ee81f4eeSChao Xie parent_rate_best = parent_rate; 256ee81f4eeSChao Xie mix_rate_best = mix_rate; 257ee81f4eeSChao Xie gap_best = gap; 258ee81f4eeSChao Xie if (gap_best == 0) 259ee81f4eeSChao Xie goto found; 260ee81f4eeSChao Xie } 261ee81f4eeSChao Xie } 262ee81f4eeSChao Xie } 263ee81f4eeSChao Xie } 264ee81f4eeSChao Xie 265ee81f4eeSChao Xie found: 266ee81f4eeSChao Xie *best_parent_rate = parent_rate_best; 267ee81f4eeSChao Xie *best_parent_clk = parent_best; 268ee81f4eeSChao Xie 269ee81f4eeSChao Xie return mix_rate_best; 270ee81f4eeSChao Xie } 271ee81f4eeSChao Xie 272ee81f4eeSChao Xie static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw, 273ee81f4eeSChao Xie unsigned long rate, 274ee81f4eeSChao Xie unsigned long parent_rate, 275ee81f4eeSChao Xie u8 index) 276ee81f4eeSChao Xie { 277ee81f4eeSChao Xie struct mmp_clk_mix *mix = to_clk_mix(hw); 278ee81f4eeSChao Xie unsigned int div; 279ee81f4eeSChao Xie u32 div_val, mux_val; 280ee81f4eeSChao Xie 281ee81f4eeSChao Xie div = parent_rate / rate; 282ee81f4eeSChao Xie div_val = _get_div_val(mix, div); 283ee81f4eeSChao Xie mux_val = _get_mux_val(mix, index); 284ee81f4eeSChao Xie 285ee81f4eeSChao Xie return _set_rate(mix, mux_val, div_val, 1, 1); 286ee81f4eeSChao Xie } 287ee81f4eeSChao Xie 288ee81f4eeSChao Xie static u8 mmp_clk_mix_get_parent(struct clk_hw *hw) 289ee81f4eeSChao Xie { 290ee81f4eeSChao Xie struct mmp_clk_mix *mix = to_clk_mix(hw); 291ee81f4eeSChao Xie struct mmp_clk_mix_reg_info *ri = &mix->reg_info; 292ee81f4eeSChao Xie unsigned long flags = 0; 293ee81f4eeSChao Xie u32 mux_div = 0; 294ee81f4eeSChao Xie u8 width, shift; 295ee81f4eeSChao Xie u32 mux_val; 296ee81f4eeSChao Xie 297ee81f4eeSChao Xie if (mix->lock) 298ee81f4eeSChao Xie spin_lock_irqsave(mix->lock, flags); 299ee81f4eeSChao Xie 300ee81f4eeSChao Xie if (mix->type == MMP_CLK_MIX_TYPE_V1 301ee81f4eeSChao Xie || mix->type == MMP_CLK_MIX_TYPE_V2) 302ee81f4eeSChao Xie mux_div = readl(ri->reg_clk_ctrl); 303ee81f4eeSChao Xie else 304ee81f4eeSChao Xie mux_div = readl(ri->reg_clk_sel); 305ee81f4eeSChao Xie 306ee81f4eeSChao Xie if (mix->lock) 307ee81f4eeSChao Xie spin_unlock_irqrestore(mix->lock, flags); 308ee81f4eeSChao Xie 309ee81f4eeSChao Xie width = mix->reg_info.width_mux; 310ee81f4eeSChao Xie shift = mix->reg_info.shift_mux; 311ee81f4eeSChao Xie 312ee81f4eeSChao Xie mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift); 313ee81f4eeSChao Xie 314ee81f4eeSChao Xie return _get_mux(mix, mux_val); 315ee81f4eeSChao Xie } 316ee81f4eeSChao Xie 317ee81f4eeSChao Xie static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw, 318ee81f4eeSChao Xie unsigned long parent_rate) 319ee81f4eeSChao Xie { 320ee81f4eeSChao Xie struct mmp_clk_mix *mix = to_clk_mix(hw); 321ee81f4eeSChao Xie struct mmp_clk_mix_reg_info *ri = &mix->reg_info; 322ee81f4eeSChao Xie unsigned long flags = 0; 323ee81f4eeSChao Xie u32 mux_div = 0; 324ee81f4eeSChao Xie u8 width, shift; 325ee81f4eeSChao Xie unsigned int div; 326ee81f4eeSChao Xie 327ee81f4eeSChao Xie if (mix->lock) 328ee81f4eeSChao Xie spin_lock_irqsave(mix->lock, flags); 329ee81f4eeSChao Xie 330ee81f4eeSChao Xie if (mix->type == MMP_CLK_MIX_TYPE_V1 331ee81f4eeSChao Xie || mix->type == MMP_CLK_MIX_TYPE_V2) 332ee81f4eeSChao Xie mux_div = readl(ri->reg_clk_ctrl); 333ee81f4eeSChao Xie else 334ee81f4eeSChao Xie mux_div = readl(ri->reg_clk_sel); 335ee81f4eeSChao Xie 336ee81f4eeSChao Xie if (mix->lock) 337ee81f4eeSChao Xie spin_unlock_irqrestore(mix->lock, flags); 338ee81f4eeSChao Xie 339ee81f4eeSChao Xie width = mix->reg_info.width_div; 340ee81f4eeSChao Xie shift = mix->reg_info.shift_div; 341ee81f4eeSChao Xie 342ee81f4eeSChao Xie div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift)); 343ee81f4eeSChao Xie 344ee81f4eeSChao Xie return parent_rate / div; 345ee81f4eeSChao Xie } 346ee81f4eeSChao Xie 347ee81f4eeSChao Xie static int mmp_clk_set_parent(struct clk_hw *hw, u8 index) 348ee81f4eeSChao Xie { 349ee81f4eeSChao Xie struct mmp_clk_mix *mix = to_clk_mix(hw); 350ee81f4eeSChao Xie struct mmp_clk_mix_clk_table *item; 351ee81f4eeSChao Xie int i; 352ee81f4eeSChao Xie u32 div_val, mux_val; 353ee81f4eeSChao Xie 354ee81f4eeSChao Xie if (mix->table) { 355ee81f4eeSChao Xie for (i = 0; i < mix->table_size; i++) { 356ee81f4eeSChao Xie item = &mix->table[i]; 357ee81f4eeSChao Xie if (item->valid == 0) 358ee81f4eeSChao Xie continue; 359ee81f4eeSChao Xie if (item->parent_index == index) 360ee81f4eeSChao Xie break; 361ee81f4eeSChao Xie } 362ee81f4eeSChao Xie if (i < mix->table_size) { 363ee81f4eeSChao Xie div_val = _get_div_val(mix, item->divisor); 364ee81f4eeSChao Xie mux_val = _get_mux_val(mix, item->parent_index); 365ee81f4eeSChao Xie } else 366ee81f4eeSChao Xie return -EINVAL; 367ee81f4eeSChao Xie } else { 368ee81f4eeSChao Xie mux_val = _get_mux_val(mix, index); 369ee81f4eeSChao Xie div_val = 0; 370ee81f4eeSChao Xie } 371ee81f4eeSChao Xie 372ee81f4eeSChao Xie return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0); 373ee81f4eeSChao Xie } 374ee81f4eeSChao Xie 375ee81f4eeSChao Xie static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate, 376ee81f4eeSChao Xie unsigned long best_parent_rate) 377ee81f4eeSChao Xie { 378ee81f4eeSChao Xie struct mmp_clk_mix *mix = to_clk_mix(hw); 379ee81f4eeSChao Xie struct mmp_clk_mix_clk_table *item; 380ee81f4eeSChao Xie unsigned long parent_rate; 381ee81f4eeSChao Xie unsigned int best_divisor; 382ee81f4eeSChao Xie struct clk *mix_clk, *parent; 383ee81f4eeSChao Xie int i; 384ee81f4eeSChao Xie 385ee81f4eeSChao Xie best_divisor = best_parent_rate / rate; 386ee81f4eeSChao Xie 387ee81f4eeSChao Xie mix_clk = hw->clk; 388ee81f4eeSChao Xie if (mix->table) { 389ee81f4eeSChao Xie for (i = 0; i < mix->table_size; i++) { 390ee81f4eeSChao Xie item = &mix->table[i]; 391ee81f4eeSChao Xie if (item->valid == 0) 392ee81f4eeSChao Xie continue; 393ee81f4eeSChao Xie parent = clk_get_parent_by_index(mix_clk, 394ee81f4eeSChao Xie item->parent_index); 395ee81f4eeSChao Xie parent_rate = __clk_get_rate(parent); 396ee81f4eeSChao Xie if (parent_rate == best_parent_rate 397ee81f4eeSChao Xie && item->divisor == best_divisor) 398ee81f4eeSChao Xie break; 399ee81f4eeSChao Xie } 400ee81f4eeSChao Xie if (i < mix->table_size) 401ee81f4eeSChao Xie return _set_rate(mix, 402ee81f4eeSChao Xie _get_mux_val(mix, item->parent_index), 403ee81f4eeSChao Xie _get_div_val(mix, item->divisor), 404ee81f4eeSChao Xie 1, 1); 405ee81f4eeSChao Xie else 406ee81f4eeSChao Xie return -EINVAL; 407ee81f4eeSChao Xie } else { 408ee81f4eeSChao Xie for (i = 0; i < __clk_get_num_parents(mix_clk); i++) { 409ee81f4eeSChao Xie parent = clk_get_parent_by_index(mix_clk, i); 410ee81f4eeSChao Xie parent_rate = __clk_get_rate(parent); 411ee81f4eeSChao Xie if (parent_rate == best_parent_rate) 412ee81f4eeSChao Xie break; 413ee81f4eeSChao Xie } 414ee81f4eeSChao Xie if (i < __clk_get_num_parents(mix_clk)) 415ee81f4eeSChao Xie return _set_rate(mix, _get_mux_val(mix, i), 416ee81f4eeSChao Xie _get_div_val(mix, best_divisor), 1, 1); 417ee81f4eeSChao Xie else 418ee81f4eeSChao Xie return -EINVAL; 419ee81f4eeSChao Xie } 420ee81f4eeSChao Xie } 421ee81f4eeSChao Xie 422ee81f4eeSChao Xie static void mmp_clk_mix_init(struct clk_hw *hw) 423ee81f4eeSChao Xie { 424ee81f4eeSChao Xie struct mmp_clk_mix *mix = to_clk_mix(hw); 425ee81f4eeSChao Xie 426ee81f4eeSChao Xie if (mix->table) 427ee81f4eeSChao Xie _filter_clk_table(mix, mix->table, mix->table_size); 428ee81f4eeSChao Xie } 429ee81f4eeSChao Xie 430ee81f4eeSChao Xie const struct clk_ops mmp_clk_mix_ops = { 431ee81f4eeSChao Xie .determine_rate = mmp_clk_mix_determine_rate, 432ee81f4eeSChao Xie .set_rate_and_parent = mmp_clk_mix_set_rate_and_parent, 433ee81f4eeSChao Xie .set_rate = mmp_clk_set_rate, 434ee81f4eeSChao Xie .set_parent = mmp_clk_set_parent, 435ee81f4eeSChao Xie .get_parent = mmp_clk_mix_get_parent, 436ee81f4eeSChao Xie .recalc_rate = mmp_clk_mix_recalc_rate, 437ee81f4eeSChao Xie .init = mmp_clk_mix_init, 438ee81f4eeSChao Xie }; 439ee81f4eeSChao Xie 440ee81f4eeSChao Xie struct clk *mmp_clk_register_mix(struct device *dev, 441ee81f4eeSChao Xie const char *name, 442ee81f4eeSChao Xie const char **parent_names, 443ee81f4eeSChao Xie u8 num_parents, 444ee81f4eeSChao Xie unsigned long flags, 445ee81f4eeSChao Xie struct mmp_clk_mix_config *config, 446ee81f4eeSChao Xie spinlock_t *lock) 447ee81f4eeSChao Xie { 448ee81f4eeSChao Xie struct mmp_clk_mix *mix; 449ee81f4eeSChao Xie struct clk *clk; 450ee81f4eeSChao Xie struct clk_init_data init; 451ee81f4eeSChao Xie size_t table_bytes; 452ee81f4eeSChao Xie 453ee81f4eeSChao Xie mix = kzalloc(sizeof(*mix), GFP_KERNEL); 454ee81f4eeSChao Xie if (!mix) { 455ee81f4eeSChao Xie pr_err("%s:%s: could not allocate mmp mix clk\n", 456ee81f4eeSChao Xie __func__, name); 457ee81f4eeSChao Xie return ERR_PTR(-ENOMEM); 458ee81f4eeSChao Xie } 459ee81f4eeSChao Xie 460ee81f4eeSChao Xie init.name = name; 461ee81f4eeSChao Xie init.flags = flags | CLK_GET_RATE_NOCACHE; 462ee81f4eeSChao Xie init.parent_names = parent_names; 463ee81f4eeSChao Xie init.num_parents = num_parents; 464ee81f4eeSChao Xie init.ops = &mmp_clk_mix_ops; 465ee81f4eeSChao Xie 466ee81f4eeSChao Xie memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info)); 467ee81f4eeSChao Xie if (config->table) { 468ee81f4eeSChao Xie table_bytes = sizeof(*config->table) * config->table_size; 469ee81f4eeSChao Xie mix->table = kzalloc(table_bytes, GFP_KERNEL); 470ee81f4eeSChao Xie if (!mix->table) { 471ee81f4eeSChao Xie pr_err("%s:%s: could not allocate mmp mix table\n", 472ee81f4eeSChao Xie __func__, name); 473ee81f4eeSChao Xie kfree(mix); 474ee81f4eeSChao Xie return ERR_PTR(-ENOMEM); 475ee81f4eeSChao Xie } 476ee81f4eeSChao Xie memcpy(mix->table, config->table, table_bytes); 477ee81f4eeSChao Xie mix->table_size = config->table_size; 478ee81f4eeSChao Xie } 479ee81f4eeSChao Xie 480ee81f4eeSChao Xie if (config->mux_table) { 481ee81f4eeSChao Xie table_bytes = sizeof(u32) * num_parents; 482ee81f4eeSChao Xie mix->mux_table = kzalloc(table_bytes, GFP_KERNEL); 483ee81f4eeSChao Xie if (!mix->mux_table) { 484ee81f4eeSChao Xie pr_err("%s:%s: could not allocate mmp mix mux-table\n", 485ee81f4eeSChao Xie __func__, name); 486ee81f4eeSChao Xie kfree(mix->table); 487ee81f4eeSChao Xie kfree(mix); 488ee81f4eeSChao Xie return ERR_PTR(-ENOMEM); 489ee81f4eeSChao Xie } 490ee81f4eeSChao Xie memcpy(mix->mux_table, config->mux_table, table_bytes); 491ee81f4eeSChao Xie } 492ee81f4eeSChao Xie 493ee81f4eeSChao Xie mix->div_flags = config->div_flags; 494ee81f4eeSChao Xie mix->mux_flags = config->mux_flags; 495ee81f4eeSChao Xie mix->lock = lock; 496ee81f4eeSChao Xie mix->hw.init = &init; 497ee81f4eeSChao Xie 498ee81f4eeSChao Xie if (config->reg_info.bit_fc >= 32) 499ee81f4eeSChao Xie mix->type = MMP_CLK_MIX_TYPE_V1; 500ee81f4eeSChao Xie else if (config->reg_info.reg_clk_sel) 501ee81f4eeSChao Xie mix->type = MMP_CLK_MIX_TYPE_V3; 502ee81f4eeSChao Xie else 503ee81f4eeSChao Xie mix->type = MMP_CLK_MIX_TYPE_V2; 504ee81f4eeSChao Xie clk = clk_register(dev, &mix->hw); 505ee81f4eeSChao Xie 506ee81f4eeSChao Xie if (IS_ERR(clk)) { 507ee81f4eeSChao Xie kfree(mix->mux_table); 508ee81f4eeSChao Xie kfree(mix->table); 509ee81f4eeSChao Xie kfree(mix); 510ee81f4eeSChao Xie } 511ee81f4eeSChao Xie 512ee81f4eeSChao Xie return clk; 513ee81f4eeSChao Xie } 514