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