1 // SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause
2 /*
3 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
4 * Copyright (C) 2021 Microchip
5 */
6
7 #include <io.h>
8 #include <kernel/delay.h>
9 #include <kernel/panic.h>
10 #include <util.h>
11 #include <mm/core_memprot.h>
12 #include <types_ext.h>
13
14 #include "at91_clk.h"
15
16 #define PLL_STATUS_MASK(id) BIT(1 + (id))
17 #define PLL_REG(id) (AT91_CKGR_PLLAR + ((id) * 4))
18 #define PLL_DIV_MASK 0xff
19 #define PLL_DIV_MAX PLL_DIV_MASK
20 #define PLL_DIV(reg) ((reg) & PLL_DIV_MASK)
21 #define PLL_MUL(reg, layout) \
22 ({ \
23 typeof(layout) __layout = layout; \
24 \
25 (((reg) >> (__layout)->mul_shift) & (__layout)->mul_mask); \
26 })
27 #define PLL_MUL_MIN 2
28 #define PLL_MUL_MASK(layout) ((layout)->mul_mask)
29 #define PLL_MUL_MAX(layout) (PLL_MUL_MASK(layout) + 1)
30 #define PLL_ICPR_SHIFT(id) ((id) * 16)
31 #define PLL_ICPR_MASK(id) (0xffff << PLL_ICPR_SHIFT(id))
32 #define PLL_MAX_COUNT 0x3f
33 #define PLL_COUNT_SHIFT 8
34 #define PLL_OUT_SHIFT 14
35 #define PLL_MAX_ID 1
36
37 struct clk_pll {
38 vaddr_t base;
39 uint8_t id;
40 uint8_t div;
41 uint8_t range;
42 uint16_t mul;
43 const struct clk_pll_layout *layout;
44 const struct clk_pll_charac *charac;
45 };
46
clk_pll_ready(vaddr_t base,int id)47 static bool clk_pll_ready(vaddr_t base, int id)
48 {
49 unsigned int status = io_read32(base + AT91_PMC_SR);
50
51 return status & PLL_STATUS_MASK(id);
52 }
53
clk_pll_enable(struct clk * clk)54 static TEE_Result clk_pll_enable(struct clk *clk)
55 {
56 struct clk_pll *pll = clk->priv;
57 const struct clk_pll_layout *layout = pll->layout;
58 const struct clk_pll_charac *charac = pll->charac;
59 uint8_t id = pll->id;
60 uint32_t mask = PLL_STATUS_MASK(id);
61 int offset = PLL_REG(id);
62 uint8_t out = 0;
63 unsigned int pllr = 0;
64 unsigned int status = 0;
65 uint8_t div = 0;
66 uint16_t mul = 0;
67
68 pllr = io_read32(pll->base + offset);
69 div = PLL_DIV(pllr);
70 mul = PLL_MUL(pllr, layout);
71
72 status = io_read32(pll->base + AT91_PMC_SR);
73 if ((status & mask) &&
74 (div == pll->div && mul == pll->mul))
75 return TEE_SUCCESS;
76
77 if (charac->out)
78 out = charac->out[pll->range];
79
80 if (charac->icpll)
81 io_clrsetbits32(pll->base + AT91_PMC_PLLICPR, PLL_ICPR_MASK(id),
82 charac->icpll[pll->range] <<
83 PLL_ICPR_SHIFT(id));
84
85 io_clrsetbits32(pll->base + offset, layout->pllr_mask,
86 pll->div | (PLL_MAX_COUNT << PLL_COUNT_SHIFT) |
87 (out << PLL_OUT_SHIFT) |
88 ((pll->mul & layout->mul_mask) << layout->mul_shift));
89
90 while (!clk_pll_ready(pll->base, pll->id))
91 ;
92
93 return TEE_SUCCESS;
94 }
95
clk_pll_disable(struct clk * clk)96 static void clk_pll_disable(struct clk *clk)
97 {
98 struct clk_pll *pll = clk->priv;
99 unsigned int mask = pll->layout->pllr_mask;
100
101 io_clrsetbits32(pll->base + PLL_REG(pll->id), mask, ~mask);
102 }
103
clk_pll_get_rate(struct clk * clk,unsigned long parent_rate)104 static unsigned long clk_pll_get_rate(struct clk *clk,
105 unsigned long parent_rate)
106 {
107 struct clk_pll *pll = clk->priv;
108
109 if (!pll->div || !pll->mul)
110 return 0;
111
112 return (parent_rate / pll->div) * (pll->mul + 1);
113 }
114
clk_pll_get_best_div_mul(struct clk_pll * pll,unsigned long rate,unsigned long parent_rate,uint32_t * div,uint32_t * mul,uint32_t * index)115 static long clk_pll_get_best_div_mul(struct clk_pll *pll, unsigned long rate,
116 unsigned long parent_rate,
117 uint32_t *div, uint32_t *mul,
118 uint32_t *index)
119 {
120 const struct clk_pll_layout *layout = pll->layout;
121 const struct clk_pll_charac *charac = pll->charac;
122 unsigned long bestremainder = ULONG_MAX;
123 unsigned long maxdiv = 1;
124 unsigned long mindiv = 1;
125 unsigned long tmpdiv = 1;
126 long bestrate = -1;
127 unsigned long bestdiv = 0;
128 unsigned long bestmul = 0;
129 int i = 0;
130
131 /* Check if parent_rate is a valid input rate */
132 if (parent_rate < charac->input.min)
133 return -1;
134
135 /*
136 * Calculate minimum divider based on the minimum multiplier, the
137 * parent_rate and the requested rate.
138 * Should always be 2 according to the input and output charac
139 * of the PLL blocks.
140 */
141 mindiv = (parent_rate * PLL_MUL_MIN) / rate;
142 if (!mindiv)
143 mindiv = 1;
144
145 if (parent_rate > charac->input.max) {
146 tmpdiv = DIV_ROUND_UP(parent_rate, charac->input.max);
147 if (tmpdiv > PLL_DIV_MAX)
148 return -1;
149
150 if (tmpdiv > mindiv)
151 mindiv = tmpdiv;
152 }
153
154 /*
155 * Calculate the maximum divider which is limited by PLL register
156 * layout (limited by the MUL or DIV field size).
157 */
158 maxdiv = DIV_ROUND_UP(parent_rate * PLL_MUL_MAX(layout), rate);
159 if (maxdiv > PLL_DIV_MAX)
160 maxdiv = PLL_DIV_MAX;
161
162 /*
163 * Iterate over the acceptable divider values to find the best
164 * divider/multiplier pair (the one that generates the closest
165 * rate to the requested one).
166 */
167 for (tmpdiv = mindiv; tmpdiv <= maxdiv; tmpdiv++) {
168 unsigned long remainder = 0;
169 unsigned long tmprate = 0;
170 unsigned long tmpmul = 0;
171
172 /*
173 * Calculate the multiplier associated with the current
174 * divider that provide the closest rate to the requested one.
175 */
176 tmpmul = UDIV_ROUND_NEAREST(rate, parent_rate / tmpdiv);
177 tmprate = (parent_rate / tmpdiv) * tmpmul;
178 if (tmprate > rate)
179 remainder = tmprate - rate;
180 else
181 remainder = rate - tmprate;
182
183 /*
184 * Compare the remainder with the best remainder found until
185 * now and elect a new best multiplier/divider pair if the
186 * current remainder is smaller than the best one.
187 */
188 if (remainder < bestremainder) {
189 bestremainder = remainder;
190 bestdiv = tmpdiv;
191 bestmul = tmpmul;
192 bestrate = tmprate;
193 }
194
195 /*
196 * We've found a perfect match!
197 * Stop searching now and use this multiplier/divider pair.
198 */
199 if (!remainder)
200 break;
201 }
202
203 /* We haven't found any multiplier/divider pair => return -ERANGE */
204 if (bestrate < 0)
205 return bestrate;
206
207 /* Check if bestrate is a valid output rate */
208 for (i = 0; i < charac->num_output; i++) {
209 if (bestrate >= (long)charac->output[i].min &&
210 bestrate <= (long)charac->output[i].max)
211 break;
212 }
213
214 if (i >= charac->num_output)
215 return -1;
216
217 if (div)
218 *div = bestdiv;
219 if (mul)
220 *mul = bestmul - 1;
221 if (index)
222 *index = i;
223
224 return bestrate;
225 }
226
clk_pll_set_rate(struct clk * clk,unsigned long rate,unsigned long parent_rate)227 static TEE_Result clk_pll_set_rate(struct clk *clk, unsigned long rate,
228 unsigned long parent_rate)
229 {
230 struct clk_pll *pll = clk->priv;
231 long ret = -1;
232 uint32_t div = 1;
233 uint32_t mul = 0;
234 uint32_t index = 0;
235
236 ret = clk_pll_get_best_div_mul(pll, rate, parent_rate,
237 &div, &mul, &index);
238 if (ret < 0)
239 return TEE_ERROR_BAD_PARAMETERS;
240
241 pll->range = index;
242 pll->div = div;
243 pll->mul = mul;
244
245 return TEE_SUCCESS;
246 }
247
248 static const struct clk_ops pll_ops = {
249 .enable = clk_pll_enable,
250 .disable = clk_pll_disable,
251 .get_rate = clk_pll_get_rate,
252 .set_rate = clk_pll_set_rate,
253 };
254
255 struct clk *
at91_clk_register_pll(struct pmc_data * pmc,const char * name,struct clk * parent,uint8_t id,const struct clk_pll_layout * layout,const struct clk_pll_charac * charac)256 at91_clk_register_pll(struct pmc_data *pmc, const char *name,
257 struct clk *parent, uint8_t id,
258 const struct clk_pll_layout *layout,
259 const struct clk_pll_charac *charac)
260 {
261 struct clk *clk = NULL;
262 struct clk_pll *pll = NULL;
263 int offset = PLL_REG(id);
264 uint32_t pllr = 0;
265
266 if (!name || !parent)
267 return NULL;
268
269 clk = clk_alloc(name, &pll_ops, &parent, 1);
270 if (!clk)
271 return NULL;
272
273 if (id > PLL_MAX_ID)
274 return NULL;
275
276 pll = calloc(1, sizeof(*pll));
277 if (!pll) {
278 clk_free(clk);
279 return NULL;
280 }
281
282 pll->id = id;
283 pll->layout = layout;
284 pll->charac = charac;
285 pll->base = pmc->base;
286 pllr = io_read32(pmc->base + offset);
287 pll->div = PLL_DIV(pllr);
288 pll->mul = PLL_MUL(pllr, layout);
289
290 clk->flags = CLK_SET_RATE_GATE;
291 clk->priv = pll;
292
293 if (clk_register(clk)) {
294 clk_free(clk);
295 free(pll);
296 return NULL;
297 }
298
299 return clk;
300 }
301
302 const struct clk_pll_layout sama5d3_pll_layout = {
303 .pllr_mask = 0x1FFFFFF,
304 .mul_shift = 18,
305 .mul_mask = 0x7F,
306 };
307