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