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 unsigned long rate = 0, prev_rate; 32 int i; 33 34 for (i = 0; i < factor->ftbl_cnt; i++) { 35 prev_rate = rate; 36 rate = (((*prate / 10000) * factor->ftbl[i].den) / 37 (factor->ftbl[i].num * factor->masks->factor)) * 10000; 38 if (rate > drate) 39 break; 40 } 41 if ((i == 0) || (i == factor->ftbl_cnt)) { 42 return rate; 43 } else { 44 if ((drate - prev_rate) > (rate - drate)) 45 return rate; 46 else 47 return prev_rate; 48 } 49 } 50 51 static unsigned long clk_factor_recalc_rate(struct clk_hw *hw, 52 unsigned long parent_rate) 53 { 54 struct mmp_clk_factor *factor = to_clk_factor(hw); 55 struct mmp_clk_factor_masks *masks = factor->masks; 56 unsigned int val, num, den; 57 58 val = readl_relaxed(factor->base); 59 60 /* calculate numerator */ 61 num = (val >> masks->num_shift) & masks->num_mask; 62 63 /* calculate denominator */ 64 den = (val >> masks->den_shift) & masks->den_mask; 65 66 if (!den) 67 return 0; 68 69 return (((parent_rate / 10000) * den) / 70 (num * factor->masks->factor)) * 10000; 71 } 72 73 /* Configures new clock rate*/ 74 static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate, 75 unsigned long prate) 76 { 77 struct mmp_clk_factor *factor = to_clk_factor(hw); 78 struct mmp_clk_factor_masks *masks = factor->masks; 79 int i; 80 unsigned long val; 81 unsigned long prev_rate, rate = 0; 82 unsigned long flags = 0; 83 84 for (i = 0; i < factor->ftbl_cnt; i++) { 85 prev_rate = rate; 86 rate = (((prate / 10000) * factor->ftbl[i].den) / 87 (factor->ftbl[i].num * factor->masks->factor)) * 10000; 88 if (rate > drate) 89 break; 90 } 91 if (i > 0) 92 i--; 93 94 if (factor->lock) 95 spin_lock_irqsave(factor->lock, flags); 96 97 val = readl_relaxed(factor->base); 98 99 val &= ~(masks->num_mask << masks->num_shift); 100 val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift; 101 102 val &= ~(masks->den_mask << masks->den_shift); 103 val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift; 104 105 writel_relaxed(val, factor->base); 106 107 if (factor->lock) 108 spin_unlock_irqrestore(factor->lock, flags); 109 110 return 0; 111 } 112 113 static void clk_factor_init(struct clk_hw *hw) 114 { 115 struct mmp_clk_factor *factor = to_clk_factor(hw); 116 struct mmp_clk_factor_masks *masks = factor->masks; 117 u32 val, num, den; 118 int i; 119 unsigned long flags = 0; 120 121 if (factor->lock) 122 spin_lock_irqsave(factor->lock, flags); 123 124 val = readl(factor->base); 125 126 /* calculate numerator */ 127 num = (val >> masks->num_shift) & masks->num_mask; 128 129 /* calculate denominator */ 130 den = (val >> masks->den_shift) & masks->den_mask; 131 132 for (i = 0; i < factor->ftbl_cnt; i++) 133 if (den == factor->ftbl[i].den && num == factor->ftbl[i].num) 134 break; 135 136 if (i >= factor->ftbl_cnt) { 137 val &= ~(masks->num_mask << masks->num_shift); 138 val |= (factor->ftbl[0].num & masks->num_mask) << 139 masks->num_shift; 140 141 val &= ~(masks->den_mask << masks->den_shift); 142 val |= (factor->ftbl[0].den & masks->den_mask) << 143 masks->den_shift; 144 145 writel(val, factor->base); 146 } 147 148 if (factor->lock) 149 spin_unlock_irqrestore(factor->lock, flags); 150 } 151 152 static struct clk_ops clk_factor_ops = { 153 .recalc_rate = clk_factor_recalc_rate, 154 .round_rate = clk_factor_round_rate, 155 .set_rate = clk_factor_set_rate, 156 .init = clk_factor_init, 157 }; 158 159 struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, 160 unsigned long flags, void __iomem *base, 161 struct mmp_clk_factor_masks *masks, 162 struct mmp_clk_factor_tbl *ftbl, 163 unsigned int ftbl_cnt, spinlock_t *lock) 164 { 165 struct mmp_clk_factor *factor; 166 struct clk_init_data init; 167 struct clk *clk; 168 169 if (!masks) { 170 pr_err("%s: must pass a clk_factor_mask\n", __func__); 171 return ERR_PTR(-EINVAL); 172 } 173 174 factor = kzalloc(sizeof(*factor), GFP_KERNEL); 175 if (!factor) { 176 pr_err("%s: could not allocate factor clk\n", __func__); 177 return ERR_PTR(-ENOMEM); 178 } 179 180 /* struct clk_aux assignments */ 181 factor->base = base; 182 factor->masks = masks; 183 factor->ftbl = ftbl; 184 factor->ftbl_cnt = ftbl_cnt; 185 factor->hw.init = &init; 186 factor->lock = lock; 187 188 init.name = name; 189 init.ops = &clk_factor_ops; 190 init.flags = flags; 191 init.parent_names = &parent_name; 192 init.num_parents = 1; 193 194 clk = clk_register(NULL, &factor->hw); 195 if (IS_ERR_OR_NULL(clk)) 196 kfree(factor); 197 198 return clk; 199 } 200