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 rate = 0; 82 unsigned long flags = 0; 83 84 for (i = 0; i < factor->ftbl_cnt; i++) { 85 rate = (((prate / 10000) * factor->ftbl[i].den) / 86 (factor->ftbl[i].num * factor->masks->factor)) * 10000; 87 if (rate > drate) 88 break; 89 } 90 if (i > 0) 91 i--; 92 93 if (factor->lock) 94 spin_lock_irqsave(factor->lock, flags); 95 96 val = readl_relaxed(factor->base); 97 98 val &= ~(masks->num_mask << masks->num_shift); 99 val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift; 100 101 val &= ~(masks->den_mask << masks->den_shift); 102 val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift; 103 104 writel_relaxed(val, factor->base); 105 106 if (factor->lock) 107 spin_unlock_irqrestore(factor->lock, flags); 108 109 return 0; 110 } 111 112 static int clk_factor_init(struct clk_hw *hw) 113 { 114 struct mmp_clk_factor *factor = to_clk_factor(hw); 115 struct mmp_clk_factor_masks *masks = factor->masks; 116 u32 val, num, den; 117 int i; 118 unsigned long flags = 0; 119 120 if (factor->lock) 121 spin_lock_irqsave(factor->lock, flags); 122 123 val = readl(factor->base); 124 125 /* calculate numerator */ 126 num = (val >> masks->num_shift) & masks->num_mask; 127 128 /* calculate denominator */ 129 den = (val >> masks->den_shift) & masks->den_mask; 130 131 for (i = 0; i < factor->ftbl_cnt; i++) 132 if (den == factor->ftbl[i].den && num == factor->ftbl[i].num) 133 break; 134 135 if (i >= factor->ftbl_cnt) { 136 val &= ~(masks->num_mask << masks->num_shift); 137 val |= (factor->ftbl[0].num & masks->num_mask) << 138 masks->num_shift; 139 140 val &= ~(masks->den_mask << masks->den_shift); 141 val |= (factor->ftbl[0].den & masks->den_mask) << 142 masks->den_shift; 143 144 writel(val, factor->base); 145 } 146 147 if (factor->lock) 148 spin_unlock_irqrestore(factor->lock, flags); 149 150 return 0; 151 } 152 153 static const struct clk_ops clk_factor_ops = { 154 .recalc_rate = clk_factor_recalc_rate, 155 .round_rate = clk_factor_round_rate, 156 .set_rate = clk_factor_set_rate, 157 .init = clk_factor_init, 158 }; 159 160 struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, 161 unsigned long flags, void __iomem *base, 162 struct mmp_clk_factor_masks *masks, 163 struct mmp_clk_factor_tbl *ftbl, 164 unsigned int ftbl_cnt, spinlock_t *lock) 165 { 166 struct mmp_clk_factor *factor; 167 struct clk_init_data init; 168 struct clk *clk; 169 170 if (!masks) { 171 pr_err("%s: must pass a clk_factor_mask\n", __func__); 172 return ERR_PTR(-EINVAL); 173 } 174 175 factor = kzalloc(sizeof(*factor), GFP_KERNEL); 176 if (!factor) 177 return ERR_PTR(-ENOMEM); 178 179 /* struct clk_aux assignments */ 180 factor->base = base; 181 factor->masks = masks; 182 factor->ftbl = ftbl; 183 factor->ftbl_cnt = ftbl_cnt; 184 factor->hw.init = &init; 185 factor->lock = lock; 186 187 init.name = name; 188 init.ops = &clk_factor_ops; 189 init.flags = flags; 190 init.parent_names = &parent_name; 191 init.num_parents = 1; 192 193 clk = clk_register(NULL, &factor->hw); 194 if (IS_ERR_OR_NULL(clk)) 195 kfree(factor); 196 197 return clk; 198 } 199