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 void 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 151 static const struct clk_ops clk_factor_ops = { 152 .recalc_rate = clk_factor_recalc_rate, 153 .round_rate = clk_factor_round_rate, 154 .set_rate = clk_factor_set_rate, 155 .init = clk_factor_init, 156 }; 157 158 struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, 159 unsigned long flags, void __iomem *base, 160 struct mmp_clk_factor_masks *masks, 161 struct mmp_clk_factor_tbl *ftbl, 162 unsigned int ftbl_cnt, spinlock_t *lock) 163 { 164 struct mmp_clk_factor *factor; 165 struct clk_init_data init; 166 struct clk *clk; 167 168 if (!masks) { 169 pr_err("%s: must pass a clk_factor_mask\n", __func__); 170 return ERR_PTR(-EINVAL); 171 } 172 173 factor = kzalloc(sizeof(*factor), GFP_KERNEL); 174 if (!factor) 175 return ERR_PTR(-ENOMEM); 176 177 /* struct clk_aux assignments */ 178 factor->base = base; 179 factor->masks = masks; 180 factor->ftbl = ftbl; 181 factor->ftbl_cnt = ftbl_cnt; 182 factor->hw.init = &init; 183 factor->lock = lock; 184 185 init.name = name; 186 init.ops = &clk_factor_ops; 187 init.flags = flags; 188 init.parent_names = &parent_name; 189 init.num_parents = 1; 190 191 clk = clk_register(NULL, &factor->hw); 192 if (IS_ERR_OR_NULL(clk)) 193 kfree(factor); 194 195 return clk; 196 } 197