1
2
3
4
5
6
7
8
9
10#include <linux/clk-provider.h>
11#include <linux/err.h>
12#include <linux/io.h>
13#include <linux/iopoll.h>
14#include <linux/slab.h>
15#include <linux/spinlock.h>
16#include <asm/div64.h>
17
18#include "clk.h"
19
20#define to_clk_zx_pll(_hw) container_of(_hw, struct clk_zx_pll, hw)
21#define to_clk_zx_audio(_hw) container_of(_hw, struct clk_zx_audio, hw)
22
23#define CFG0_CFG1_OFFSET 4
24#define LOCK_FLAG BIT(30)
25#define POWER_DOWN BIT(31)
26
27static int rate_to_idx(struct clk_zx_pll *zx_pll, unsigned long rate)
28{
29 const struct zx_pll_config *config = zx_pll->lookup_table;
30 int i;
31
32 for (i = 0; i < zx_pll->count; i++) {
33 if (config[i].rate > rate)
34 return i > 0 ? i - 1 : 0;
35
36 if (config[i].rate == rate)
37 return i;
38 }
39
40 return i - 1;
41}
42
43static int hw_to_idx(struct clk_zx_pll *zx_pll)
44{
45 const struct zx_pll_config *config = zx_pll->lookup_table;
46 u32 hw_cfg0, hw_cfg1;
47 int i;
48
49 hw_cfg0 = readl_relaxed(zx_pll->reg_base);
50 hw_cfg1 = readl_relaxed(zx_pll->reg_base + CFG0_CFG1_OFFSET);
51
52
53 hw_cfg0 &= ~LOCK_FLAG;
54 hw_cfg0 |= POWER_DOWN;
55
56 for (i = 0; i < zx_pll->count; i++) {
57 if (hw_cfg0 == config[i].cfg0 && hw_cfg1 == config[i].cfg1)
58 return i;
59 }
60
61 return -EINVAL;
62}
63
64static unsigned long zx_pll_recalc_rate(struct clk_hw *hw,
65 unsigned long parent_rate)
66{
67 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
68 int idx;
69
70 idx = hw_to_idx(zx_pll);
71 if (unlikely(idx == -EINVAL))
72 return 0;
73
74 return zx_pll->lookup_table[idx].rate;
75}
76
77static long zx_pll_round_rate(struct clk_hw *hw, unsigned long rate,
78 unsigned long *prate)
79{
80 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
81 int idx;
82
83 idx = rate_to_idx(zx_pll, rate);
84
85 return zx_pll->lookup_table[idx].rate;
86}
87
88static int zx_pll_set_rate(struct clk_hw *hw, unsigned long rate,
89 unsigned long parent_rate)
90{
91
92 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
93 const struct zx_pll_config *config;
94 int idx;
95
96 idx = rate_to_idx(zx_pll, rate);
97 config = &zx_pll->lookup_table[idx];
98
99 writel_relaxed(config->cfg0, zx_pll->reg_base);
100 writel_relaxed(config->cfg1, zx_pll->reg_base + CFG0_CFG1_OFFSET);
101
102 return 0;
103}
104
105static int zx_pll_enable(struct clk_hw *hw)
106{
107 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
108 u32 reg;
109
110 reg = readl_relaxed(zx_pll->reg_base);
111 writel_relaxed(reg & ~POWER_DOWN, zx_pll->reg_base);
112
113 return readl_relaxed_poll_timeout(zx_pll->reg_base, reg,
114 reg & LOCK_FLAG, 0, 100);
115}
116
117static void zx_pll_disable(struct clk_hw *hw)
118{
119 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
120 u32 reg;
121
122 reg = readl_relaxed(zx_pll->reg_base);
123 writel_relaxed(reg | POWER_DOWN, zx_pll->reg_base);
124}
125
126static int zx_pll_is_enabled(struct clk_hw *hw)
127{
128 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
129 u32 reg;
130
131 reg = readl_relaxed(zx_pll->reg_base);
132
133 return !(reg & POWER_DOWN);
134}
135
136static const struct clk_ops zx_pll_ops = {
137 .recalc_rate = zx_pll_recalc_rate,
138 .round_rate = zx_pll_round_rate,
139 .set_rate = zx_pll_set_rate,
140 .enable = zx_pll_enable,
141 .disable = zx_pll_disable,
142 .is_enabled = zx_pll_is_enabled,
143};
144
145struct clk *clk_register_zx_pll(const char *name, const char *parent_name,
146 unsigned long flags, void __iomem *reg_base,
147 const struct zx_pll_config *lookup_table,
148 int count, spinlock_t *lock)
149{
150 struct clk_zx_pll *zx_pll;
151 struct clk *clk;
152 struct clk_init_data init;
153
154 zx_pll = kzalloc(sizeof(*zx_pll), GFP_KERNEL);
155 if (!zx_pll)
156 return ERR_PTR(-ENOMEM);
157
158 init.name = name;
159 init.ops = &zx_pll_ops;
160 init.flags = flags;
161 init.parent_names = parent_name ? &parent_name : NULL;
162 init.num_parents = parent_name ? 1 : 0;
163
164 zx_pll->reg_base = reg_base;
165 zx_pll->lookup_table = lookup_table;
166 zx_pll->count = count;
167 zx_pll->lock = lock;
168 zx_pll->hw.init = &init;
169
170 clk = clk_register(NULL, &zx_pll->hw);
171 if (IS_ERR(clk))
172 kfree(zx_pll);
173
174 return clk;
175}
176
177#define BPAR 1000000
178static u32 calc_reg(u32 parent_rate, u32 rate)
179{
180 u32 sel, integ, fra_div, tmp;
181 u64 tmp64 = (u64)parent_rate * BPAR;
182
183 do_div(tmp64, rate);
184 integ = (u32)tmp64 / BPAR;
185 integ = integ >> 1;
186
187 tmp = (u32)tmp64 % BPAR;
188 sel = tmp / BPAR;
189
190 tmp = tmp % BPAR;
191 fra_div = tmp * 0xff / BPAR;
192 tmp = (sel << 24) | (integ << 16) | (0xff << 8) | fra_div;
193
194
195
196
197 tmp |= BIT(28);
198 return tmp;
199}
200
201static u32 calc_rate(u32 reg, u32 parent_rate)
202{
203 u32 sel, integ, fra_div, tmp;
204 u64 tmp64 = (u64)parent_rate * BPAR;
205
206 tmp = reg;
207 sel = (tmp >> 24) & BIT(0);
208 integ = (tmp >> 16) & 0xff;
209 fra_div = tmp & 0xff;
210
211 tmp = fra_div * BPAR;
212 tmp = tmp / 0xff;
213 tmp += sel * BPAR;
214 tmp += 2 * integ * BPAR;
215 do_div(tmp64, tmp);
216
217 return (u32)tmp64;
218}
219
220static unsigned long zx_audio_recalc_rate(struct clk_hw *hw,
221 unsigned long parent_rate)
222{
223 struct clk_zx_audio *zx_audio = to_clk_zx_audio(hw);
224 u32 reg;
225
226 reg = readl_relaxed(zx_audio->reg_base);
227 return calc_rate(reg, parent_rate);
228}
229
230static long zx_audio_round_rate(struct clk_hw *hw, unsigned long rate,
231 unsigned long *prate)
232{
233 u32 reg;
234
235 if (rate * 2 > *prate)
236 return -EINVAL;
237
238 reg = calc_reg(*prate, rate);
239 return calc_rate(reg, *prate);
240}
241
242static int zx_audio_set_rate(struct clk_hw *hw, unsigned long rate,
243 unsigned long parent_rate)
244{
245 struct clk_zx_audio *zx_audio = to_clk_zx_audio(hw);
246 u32 reg;
247
248 reg = calc_reg(parent_rate, rate);
249 writel_relaxed(reg, zx_audio->reg_base);
250
251 return 0;
252}
253
254#define ZX_AUDIO_EN BIT(25)
255static int zx_audio_enable(struct clk_hw *hw)
256{
257 struct clk_zx_audio *zx_audio = to_clk_zx_audio(hw);
258 u32 reg;
259
260 reg = readl_relaxed(zx_audio->reg_base);
261 writel_relaxed(reg & ~ZX_AUDIO_EN, zx_audio->reg_base);
262 return 0;
263}
264
265static void zx_audio_disable(struct clk_hw *hw)
266{
267 struct clk_zx_audio *zx_audio = to_clk_zx_audio(hw);
268 u32 reg;
269
270 reg = readl_relaxed(zx_audio->reg_base);
271 writel_relaxed(reg | ZX_AUDIO_EN, zx_audio->reg_base);
272}
273
274static const struct clk_ops zx_audio_ops = {
275 .recalc_rate = zx_audio_recalc_rate,
276 .round_rate = zx_audio_round_rate,
277 .set_rate = zx_audio_set_rate,
278 .enable = zx_audio_enable,
279 .disable = zx_audio_disable,
280};
281
282struct clk *clk_register_zx_audio(const char *name,
283 const char * const parent_name,
284 unsigned long flags,
285 void __iomem *reg_base)
286{
287 struct clk_zx_audio *zx_audio;
288 struct clk *clk;
289 struct clk_init_data init;
290
291 zx_audio = kzalloc(sizeof(*zx_audio), GFP_KERNEL);
292 if (!zx_audio)
293 return ERR_PTR(-ENOMEM);
294
295 init.name = name;
296 init.ops = &zx_audio_ops;
297 init.flags = flags;
298 init.parent_names = parent_name ? &parent_name : NULL;
299 init.num_parents = parent_name ? 1 : 0;
300
301 zx_audio->reg_base = reg_base;
302 zx_audio->hw.init = &init;
303
304 clk = clk_register(NULL, &zx_audio->hw);
305 if (IS_ERR(clk))
306 kfree(zx_audio);
307
308 return clk;
309}
310
