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 <mm/core_memprot.h>
10 #include <types_ext.h>
11
12 #include "at91_clk.h"
13
14 #define MASTER_PRES_MASK 0x7
15 #define MASTER_PRES_MAX MASTER_PRES_MASK
16 #define MASTER_DIV_SHIFT 8
17 #define MASTER_DIV_MASK 0x7
18 #define MASTER_MAX_ID 4 /* Total 5 MCK clocks for SAMA7G5 */
19
20 struct clk_master {
21 vaddr_t base;
22 const struct clk_master_layout *layout;
23 const struct clk_master_charac *charac;
24 uint32_t *mux_table;
25 uint32_t mckr;
26 int chg_pid;
27 uint8_t div;
28 uint8_t id; /* ID of MCK clocks for SAMA7G5, MCK0 ~ MCK4 */
29 uint8_t parent; /* the source clock for SAMA7G5 MCKx */
30 };
31
clk_master_ready(struct clk_master * master)32 static bool clk_master_ready(struct clk_master *master)
33 {
34 uint32_t status = io_read32(master->base + AT91_PMC_SR);
35
36 return status & AT91_PMC_MCKRDY;
37 }
38
clk_master_enable(struct clk * clk)39 static TEE_Result clk_master_enable(struct clk *clk)
40 {
41 struct clk_master *master = clk->priv;
42
43 while (!clk_master_ready(master))
44 ;
45
46 return TEE_SUCCESS;
47 }
48
clk_master_div_get_rate(struct clk * clk,unsigned long parent_rate)49 static unsigned long clk_master_div_get_rate(struct clk *clk,
50 unsigned long parent_rate)
51 {
52 uint8_t div = 1;
53 uint32_t mckr = 0;
54 unsigned long rate = parent_rate;
55 struct clk_master *master = clk->priv;
56 const struct clk_master_layout *layout = master->layout;
57 const struct clk_master_charac *charac = master->charac;
58
59 mckr = io_read32(master->base + master->layout->offset);
60
61 mckr &= layout->mask;
62
63 div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
64
65 rate /= charac->divisors[div];
66
67 if (rate < charac->output.min)
68 IMSG("master clk div is underclocked");
69 else if (rate > charac->output.max)
70 IMSG("master clk div is overclocked");
71
72 return rate;
73 }
74
75 static const struct clk_ops master_div_ops = {
76 .enable = clk_master_enable,
77 .get_rate = clk_master_div_get_rate,
78 };
79
clk_master_pres_get_rate(struct clk * clk,unsigned long parent_rate)80 static unsigned long clk_master_pres_get_rate(struct clk *clk,
81 unsigned long parent_rate)
82 {
83 struct clk_master *master = clk->priv;
84 const struct clk_master_charac *charac = master->charac;
85 uint32_t val = 0;
86 unsigned int pres = 0;
87
88 val = io_read32(master->base + master->layout->offset);
89
90 pres = (val >> master->layout->pres_shift) & MASTER_PRES_MASK;
91 if (pres != 3 || !charac->have_div3_pres)
92 pres = BIT(pres);
93
94 return UDIV_ROUND_NEAREST(parent_rate, pres);
95 }
96
clk_master_pres_get_parent(struct clk * clk)97 static size_t clk_master_pres_get_parent(struct clk *clk)
98 {
99 struct clk_master *master = clk->priv;
100 uint32_t mckr = 0;
101
102 mckr = io_read32(master->base + master->layout->offset);
103
104 return mckr & AT91_PMC_CSS;
105 }
106
107 static const struct clk_ops master_pres_ops = {
108 .enable = clk_master_enable,
109 .get_rate = clk_master_pres_get_rate,
110 .get_parent = clk_master_pres_get_parent,
111 };
112
113 static struct clk *
at91_clk_register_master_internal(struct pmc_data * pmc,const char * name,int num_parents,struct clk ** parents,const struct clk_master_layout * layout,const struct clk_master_charac * charac,const struct clk_ops * ops,int chg_pid)114 at91_clk_register_master_internal(struct pmc_data *pmc,
115 const char *name, int num_parents,
116 struct clk **parents,
117 const struct clk_master_layout *layout,
118 const struct clk_master_charac *charac,
119 const struct clk_ops *ops, int chg_pid)
120 {
121 struct clk_master *master = NULL;
122 struct clk *clk = NULL;
123
124 if (!name || !num_parents || !parents)
125 return NULL;
126
127 clk = clk_alloc(name, ops, parents, num_parents);
128 if (!clk)
129 return NULL;
130
131 master = calloc(1, sizeof(*master));
132 if (!master) {
133 clk_free(clk);
134 return NULL;
135 }
136
137 master->layout = layout;
138 master->charac = charac;
139 master->base = pmc->base;
140 master->chg_pid = chg_pid;
141
142 clk->priv = master;
143 clk->flags = CLK_SET_RATE_GATE;
144
145 if (clk_register(clk)) {
146 clk_free(clk);
147 free(master);
148 return NULL;
149 }
150
151 return clk;
152 }
153
154 struct clk *
at91_clk_register_master_pres(struct pmc_data * pmc,const char * name,int num_parents,struct clk ** parents,const struct clk_master_layout * layout,const struct clk_master_charac * charac,int chg_pid)155 at91_clk_register_master_pres(struct pmc_data *pmc,
156 const char *name, int num_parents,
157 struct clk **parents,
158 const struct clk_master_layout *layout,
159 const struct clk_master_charac *charac,
160 int chg_pid)
161 {
162 return at91_clk_register_master_internal(pmc, name, num_parents,
163 parents, layout,
164 charac,
165 &master_pres_ops, chg_pid);
166 }
167
168 struct clk *
at91_clk_register_master_div(struct pmc_data * pmc,const char * name,struct clk * parent,const struct clk_master_layout * layout,const struct clk_master_charac * charac)169 at91_clk_register_master_div(struct pmc_data *pmc,
170 const char *name, struct clk *parent,
171 const struct clk_master_layout *layout,
172 const struct clk_master_charac *charac)
173 {
174 return at91_clk_register_master_internal(pmc, name, 1,
175 &parent, layout,
176 charac,
177 &master_div_ops, -1);
178 }
179
180 const struct clk_master_layout at91sam9x5_master_layout = {
181 .mask = 0x373,
182 .pres_shift = 4,
183 .offset = AT91_PMC_MCKR,
184 };
185
clk_sama7g5_master_get_parent(struct clk * hw)186 static size_t clk_sama7g5_master_get_parent(struct clk *hw)
187 {
188 struct clk_master *master = hw->priv;
189 size_t i = 0;
190
191 for (i = 0; i < hw->num_parents; i++)
192 if (master->mux_table[i] == master->parent)
193 return i;
194
195 panic("Can't get correct parent of clock");
196 }
197
clk_sama7g5_master_set_parent(struct clk * hw,size_t index)198 static TEE_Result clk_sama7g5_master_set_parent(struct clk *hw, size_t index)
199 {
200 struct clk_master *master = hw->priv;
201
202 if (index >= hw->num_parents)
203 return TEE_ERROR_BAD_PARAMETERS;
204
205 master->parent = master->mux_table[index];
206
207 return TEE_SUCCESS;
208 }
209
clk_sama7g5_master_set_rate(struct clk * hw,unsigned long rate,unsigned long parent_rate)210 static TEE_Result clk_sama7g5_master_set_rate(struct clk *hw,
211 unsigned long rate,
212 unsigned long parent_rate)
213 {
214 struct clk_master *master = hw->priv;
215 unsigned long div = 0;
216
217 div = UDIV_ROUND_NEAREST(parent_rate, rate);
218 if (div > (1 << (MASTER_PRES_MAX - 1)) ||
219 (!IS_POWER_OF_TWO(div) && div != 3))
220 return TEE_ERROR_BAD_PARAMETERS;
221
222 /*
223 * Divisor Value: Select the division ratio to be applied to the
224 * selected clock to generate the corresponding MCKx.
225 * Value | Description
226 * 0 | Selected clock divided by 1
227 * 1 | Selected clock divided by 2
228 * 2 | Selected clock divided by 4
229 * 3 | Selected clock divided by 8
230 * 4 | Selected clock divided by 16
231 * 5 | Selected clock divided by 32
232 * 6 | Selected clock divided by 64
233 * 7 | Selected clock divided by 3
234 */
235 if (div == 3)
236 master->div = MASTER_PRES_MAX;
237 else
238 master->div = ffs(div) - 1;
239
240 return TEE_SUCCESS;
241 }
242
clk_sama7g5_master_get_rate(struct clk * hw,unsigned long parent_rate)243 static unsigned long clk_sama7g5_master_get_rate(struct clk *hw,
244 unsigned long parent_rate)
245 {
246 struct clk_master *master = hw->priv;
247 unsigned long rate = parent_rate >> master->div;
248
249 if (master->div == 7)
250 rate = parent_rate / 3;
251
252 return rate;
253 }
254
255 static const struct clk_ops sama7g5_master_ops = {
256 .set_rate = clk_sama7g5_master_set_rate,
257 .get_rate = clk_sama7g5_master_get_rate,
258 .get_parent = clk_sama7g5_master_get_parent,
259 .set_parent = clk_sama7g5_master_set_parent,
260 };
261
at91_clk_sama7g5_register_master(struct pmc_data * pmc,const char * name,int num_parents,struct clk ** parent,uint32_t * mux_table,uint8_t id,int chg_pid)262 struct clk *at91_clk_sama7g5_register_master(struct pmc_data *pmc,
263 const char *name,
264 int num_parents,
265 struct clk **parent,
266 uint32_t *mux_table,
267 uint8_t id,
268 int chg_pid)
269 {
270 struct clk_master *master = NULL;
271 struct clk *hw = NULL;
272 unsigned int val = 0;
273
274 if (!name || !num_parents || !parent || !mux_table ||
275 id > MASTER_MAX_ID)
276 return NULL;
277
278 master = calloc(1, sizeof(*master));
279 if (!master)
280 return NULL;
281
282 hw = clk_alloc(name, &sama7g5_master_ops, parent, num_parents);
283 if (!hw) {
284 free(master);
285 return NULL;
286 }
287
288 hw->priv = master;
289 master->base = pmc->base;
290 master->id = id;
291 master->chg_pid = chg_pid;
292 master->mux_table = mux_table;
293
294 io_write32(master->base + AT91_PMC_MCR_V2, master->id);
295 val = io_read32(master->base + AT91_PMC_MCR_V2);
296 master->parent = (val & AT91_PMC_MCR_V2_CSS_MASK) >>
297 AT91_PMC_MCR_V2_CSS_SHIFT;
298 master->div = (val & AT91_PMC_MCR_V2_DIV_MASK) >> MASTER_DIV_SHIFT;
299
300 if (clk_register(hw)) {
301 clk_free(hw);
302 free(master);
303 return NULL;
304 }
305
306 return hw;
307 }
308