1 /* 2 * mmp factor clock operation source file 3 * 4 * Copyright (C) 2012 Marvell 5 * Chao Xie <xiechao.mail@gmail.com> 6 * 7 * This file is licensed under the terms of the GNU General Public 8 * License version 2. This program is licensed "as is" without any 9 * warranty of any kind, whether express or implied. 10 */ 11 12 #include <linux/clk-provider.h> 13 #include <linux/slab.h> 14 #include <linux/io.h> 15 #include <linux/err.h> 16 17 #include "clk.h" 18 /* 19 * It is M/N clock 20 * 21 * Fout from synthesizer can be given from two equations: 22 * numerator/denominator = Fin / (Fout * factor) 23 */ 24 25 #define to_clk_factor(hw) container_of(hw, struct mmp_clk_factor, hw) 26 27 static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate, 28 unsigned long *prate) 29 { 30 struct mmp_clk_factor *factor = to_clk_factor(hw); 31 u64 rate = 0, prev_rate; 32 int i; 33 34 for (i = 0; i < factor->ftbl_cnt; i++) { 35 prev_rate = rate; 36 rate = *prate; 37 rate *= factor->ftbl[i].den; 38 do_div(rate, factor->ftbl[i].num * factor->masks->factor); 39 40 if (rate > drate) 41 break; 42 } 43 if ((i == 0) || (i == factor->ftbl_cnt)) { 44 return rate; 45 } else { 46 if ((drate - prev_rate) > (rate - drate)) 47 return rate; 48 else 49 return prev_rate; 50 } 51 } 52 53 static unsigned long clk_factor_recalc_rate(struct clk_hw *hw, 54 unsigned long parent_rate) 55 { 56 struct mmp_clk_factor *factor = to_clk_factor(hw); 57 struct mmp_clk_factor_masks *masks = factor->masks; 58 unsigned int val, num, den; 59 u64 rate; 60 61 val = readl_relaxed(factor->base); 62 63 /* calculate numerator */ 64 num = (val >> masks->num_shift) & masks->num_mask; 65 66 /* calculate denominator */ 67 den = (val >> masks->den_shift) & masks->den_mask; 68 69 if (!den) 70 return 0; 71 72 rate = parent_rate; 73 rate *= den; 74 do_div(rate, num * factor->masks->factor); 75 76 return rate; 77 } 78 79 /* Configures new clock rate*/ 80 static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate, 81 unsigned long prate) 82 { 83 struct mmp_clk_factor *factor = to_clk_factor(hw); 84 struct mmp_clk_factor_masks *masks = factor->masks; 85 int i; 86 unsigned long val; 87 unsigned long flags = 0; 88 u64 rate = 0; 89 90 for (i = 0; i < factor->ftbl_cnt; i++) { 91 rate = prate; 92 rate *= factor->ftbl[i].den; 93 do_div(rate, factor->ftbl[i].num * factor->masks->factor); 94 95 if (rate > drate) 96 break; 97 } 98 if (i > 0) 99 i--; 100 101 if (factor->lock) 102 spin_lock_irqsave(factor->lock, flags); 103 104 val = readl_relaxed(factor->base); 105 106 val &= ~(masks->num_mask << masks->num_shift); 107 val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift; 108 109 val &= ~(masks->den_mask << masks->den_shift); 110 val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift; 111 112 writel_relaxed(val, factor->base); 113 114 if (factor->lock) 115 spin_unlock_irqrestore(factor->lock, flags); 116 117 return 0; 118 } 119 120 static int clk_factor_init(struct clk_hw *hw) 121 { 122 struct mmp_clk_factor *factor = to_clk_factor(hw); 123 struct mmp_clk_factor_masks *masks = factor->masks; 124 u32 val, num, den; 125 int i; 126 unsigned long flags = 0; 127 128 if (factor->lock) 129 spin_lock_irqsave(factor->lock, flags); 130 131 val = readl(factor->base); 132 133 /* calculate numerator */ 134 num = (val >> masks->num_shift) & masks->num_mask; 135 136 /* calculate denominator */ 137 den = (val >> masks->den_shift) & masks->den_mask; 138 139 for (i = 0; i < factor->ftbl_cnt; i++) 140 if (den == factor->ftbl[i].den && num == factor->ftbl[i].num) 141 break; 142 143 if (i >= factor->ftbl_cnt) { 144 val &= ~(masks->num_mask << masks->num_shift); 145 val |= (factor->ftbl[0].num & masks->num_mask) << 146 masks->num_shift; 147 148 val &= ~(masks->den_mask << masks->den_shift); 149 val |= (factor->ftbl[0].den & masks->den_mask) << 150 masks->den_shift; 151 } 152 153 if (!(val & masks->enable_mask) || i >= factor->ftbl_cnt) { 154 val |= masks->enable_mask; 155 writel(val, factor->base); 156 } 157 158 if (factor->lock) 159 spin_unlock_irqrestore(factor->lock, flags); 160 161 return 0; 162 } 163 164 static const struct clk_ops clk_factor_ops = { 165 .recalc_rate = clk_factor_recalc_rate, 166 .round_rate = clk_factor_round_rate, 167 .set_rate = clk_factor_set_rate, 168 .init = clk_factor_init, 169 }; 170 171 struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, 172 unsigned long flags, void __iomem *base, 173 struct mmp_clk_factor_masks *masks, 174 struct mmp_clk_factor_tbl *ftbl, 175 unsigned int ftbl_cnt, spinlock_t *lock) 176 { 177 struct mmp_clk_factor *factor; 178 struct clk_init_data init; 179 struct clk *clk; 180 181 if (!masks) { 182 pr_err("%s: must pass a clk_factor_mask\n", __func__); 183 return ERR_PTR(-EINVAL); 184 } 185 186 factor = kzalloc(sizeof(*factor), GFP_KERNEL); 187 if (!factor) 188 return ERR_PTR(-ENOMEM); 189 190 /* struct clk_aux assignments */ 191 factor->base = base; 192 factor->masks = masks; 193 factor->ftbl = ftbl; 194 factor->ftbl_cnt = ftbl_cnt; 195 factor->hw.init = &init; 196 factor->lock = lock; 197 198 init.name = name; 199 init.ops = &clk_factor_ops; 200 init.flags = flags; 201 init.parent_names = &parent_name; 202 init.num_parents = 1; 203 204 clk = clk_register(NULL, &factor->hw); 205 if (IS_ERR_OR_NULL(clk)) 206 kfree(factor); 207 208 return clk; 209 } 210