1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2021, Bootlin
4 * Copyright (c) 2023, STMicroelectronics
5 */
6
7 #include <config.h>
8 #include <drivers/clk.h>
9 #include <kernel/boot.h>
10 #include <kernel/mutex_pm_aware.h>
11 #include <kernel/panic.h>
12 #include <kernel/thread.h>
13 #include <malloc.h>
14 #include <stddef.h>
15 #include <stdio.h>
16
17 /* Global clock tree access protection complying the power state transitions */
18 static struct mutex_pm_aware mu = MUTEX_PM_AWARE_INITIALIZER;
19
20 #ifdef CFG_DRIVERS_CLK_PRINT_TREE
21 static SLIST_HEAD(, clk) clock_list = SLIST_HEAD_INITIALIZER(clock_list);
22 #endif
23
lock_clk(void)24 static void lock_clk(void)
25 {
26 mutex_pm_aware_lock(&mu);
27 }
28
unlock_clk(void)29 static void unlock_clk(void)
30 {
31 mutex_pm_aware_unlock(&mu);
32 }
33
clk_alloc(const char * name,const struct clk_ops * ops,struct clk ** parent_clks,size_t parent_count)34 struct clk *clk_alloc(const char *name, const struct clk_ops *ops,
35 struct clk **parent_clks, size_t parent_count)
36 {
37 struct clk *clk = NULL;
38 size_t parent = 0;
39
40 clk = calloc(1, sizeof(*clk) + parent_count * sizeof(clk));
41 if (!clk)
42 return NULL;
43
44 clk->num_parents = parent_count;
45 for (parent = 0; parent < parent_count; parent++)
46 clk->parents[parent] = parent_clks[parent];
47
48 clk->name = name;
49 clk->ops = ops;
50 refcount_set(&clk->enabled_count, 0);
51
52 return clk;
53 }
54
clk_free(struct clk * clk)55 void clk_free(struct clk *clk)
56 {
57 free(clk);
58 }
59
clk_check(struct clk * clk)60 static bool __maybe_unused clk_check(struct clk *clk)
61 {
62 if (!clk || !clk->ops)
63 return false;
64
65 if (clk->ops->set_parent && !clk->ops->get_parent)
66 return false;
67
68 if (clk->num_parents > 1 && !clk->ops->get_parent)
69 return false;
70
71 return true;
72 }
73
clk_compute_rate_no_lock(struct clk * clk)74 static void clk_compute_rate_no_lock(struct clk *clk)
75 {
76 unsigned long parent_rate = 0;
77
78 if (clk->parent) {
79 clk_compute_rate_no_lock(clk->parent);
80 parent_rate = clk->parent->rate;
81 }
82
83 if (clk->ops->get_rate)
84 clk->rate = clk->ops->get_rate(clk, parent_rate);
85 else
86 clk->rate = parent_rate;
87 }
88
clk_get_parent_by_index(struct clk * clk,size_t pidx)89 struct clk *clk_get_parent_by_index(struct clk *clk, size_t pidx)
90 {
91 if (pidx >= clk->num_parents)
92 return NULL;
93
94 return clk->parents[pidx];
95 }
96
clk_init_parent(struct clk * clk)97 static void clk_init_parent(struct clk *clk)
98 {
99 size_t pidx = 0;
100
101 switch (clk->num_parents) {
102 case 0:
103 break;
104 case 1:
105 clk->parent = clk->parents[0];
106 break;
107 default:
108 pidx = clk->ops->get_parent(clk);
109 assert(pidx < clk->num_parents);
110
111 clk->parent = clk->parents[pidx];
112 break;
113 }
114 }
115
clk_register(struct clk * clk)116 TEE_Result clk_register(struct clk *clk)
117 {
118 assert(clk_check(clk));
119
120 clk_init_parent(clk);
121 clk_compute_rate_no_lock(clk);
122
123 #ifdef CFG_DRIVERS_CLK_PRINT_TREE
124 SLIST_INSERT_HEAD(&clock_list, clk, link);
125 #endif
126
127 DMSG("Registered clock %s, freq %lu", clk->name, clk_get_rate(clk));
128
129 return TEE_SUCCESS;
130 }
131
clk_is_enabled_no_lock(struct clk * clk)132 static bool clk_is_enabled_no_lock(struct clk *clk)
133 {
134 return refcount_val(&clk->enabled_count) != 0;
135 }
136
clk_is_enabled(struct clk * clk)137 bool clk_is_enabled(struct clk *clk)
138 {
139 return clk_is_enabled_no_lock(clk);
140 }
141
clk_disable_no_lock(struct clk * clk)142 static void clk_disable_no_lock(struct clk *clk)
143 {
144 struct clk *parent = NULL;
145
146 if (!refcount_dec(&clk->enabled_count))
147 return;
148
149 if (clk->ops->disable)
150 clk->ops->disable(clk);
151
152 parent = clk_get_parent(clk);
153 if (parent)
154 clk_disable_no_lock(parent);
155 }
156
clk_enable_no_lock(struct clk * clk)157 static TEE_Result clk_enable_no_lock(struct clk *clk)
158 {
159 TEE_Result res = TEE_ERROR_GENERIC;
160 struct clk *parent = NULL;
161
162 if (refcount_inc(&clk->enabled_count))
163 return TEE_SUCCESS;
164
165 parent = clk_get_parent(clk);
166 if (parent) {
167 res = clk_enable_no_lock(parent);
168 if (res)
169 return res;
170 }
171
172 if (clk->ops->enable) {
173 res = clk->ops->enable(clk);
174 if (res) {
175 if (parent)
176 clk_disable_no_lock(parent);
177
178 return res;
179 }
180 }
181
182 refcount_set(&clk->enabled_count, 1);
183
184 return TEE_SUCCESS;
185 }
186
clk_enable(struct clk * clk)187 TEE_Result clk_enable(struct clk *clk)
188 {
189 TEE_Result res = TEE_ERROR_GENERIC;
190
191 lock_clk();
192 res = clk_enable_no_lock(clk);
193 unlock_clk();
194
195 return res;
196 }
197
clk_disable(struct clk * clk)198 void clk_disable(struct clk *clk)
199 {
200 lock_clk();
201 clk_disable_no_lock(clk);
202 unlock_clk();
203 }
204
clk_get_rate(struct clk * clk)205 unsigned long clk_get_rate(struct clk *clk)
206 {
207 clk_compute_rate_no_lock(clk);
208
209 return clk->rate;
210 }
211
clk_set_rate_no_lock(struct clk * clk,unsigned long rate)212 static TEE_Result clk_set_rate_no_lock(struct clk *clk, unsigned long rate)
213 {
214 TEE_Result res = TEE_ERROR_GENERIC;
215 unsigned long parent_rate = 0;
216
217 if (clk->parent)
218 parent_rate = clk_get_rate(clk->parent);
219
220 assert(!(clk->flags & CLK_SET_RATE_PARENT) || clk->parent);
221 if (clk->flags & CLK_SET_RATE_PARENT) {
222 res = clk_set_rate_no_lock(clk->parent, rate);
223 if (res)
224 return res;
225 rate = clk_get_rate(clk->parent);
226 }
227
228 if (clk->ops->set_rate) {
229 if (clk->flags & CLK_SET_RATE_UNGATE) {
230 res = clk_enable_no_lock(clk);
231 if (res)
232 return res;
233 }
234
235 res = clk->ops->set_rate(clk, rate, parent_rate);
236
237 if (clk->flags & CLK_SET_RATE_UNGATE)
238 clk_disable_no_lock(clk);
239
240 if (res)
241 return res;
242 }
243
244 clk_compute_rate_no_lock(clk);
245
246 return TEE_SUCCESS;
247 }
248
clk_set_rate(struct clk * clk,unsigned long rate)249 TEE_Result clk_set_rate(struct clk *clk, unsigned long rate)
250 {
251 TEE_Result res = TEE_ERROR_GENERIC;
252
253 lock_clk();
254
255 if (clk->flags & CLK_SET_RATE_GATE && clk_is_enabled_no_lock(clk))
256 res = TEE_ERROR_BAD_STATE;
257 else
258 res = clk_set_rate_no_lock(clk, rate);
259
260 unlock_clk();
261
262 return res;
263 }
264
clk_get_parent(struct clk * clk)265 struct clk *clk_get_parent(struct clk *clk)
266 {
267 return clk->parent;
268 }
269
clk_get_parent_idx(struct clk * clk,struct clk * parent,size_t * pidx)270 static TEE_Result clk_get_parent_idx(struct clk *clk, struct clk *parent,
271 size_t *pidx)
272 {
273 size_t i = 0;
274
275 for (i = 0; i < clk_get_num_parents(clk); i++) {
276 if (clk_get_parent_by_index(clk, i) == parent) {
277 *pidx = i;
278 return TEE_SUCCESS;
279 }
280 }
281 EMSG("Clock %s is not a parent of clock %s", parent->name, clk->name);
282
283 return TEE_ERROR_BAD_PARAMETERS;
284 }
285
clk_set_parent_no_lock(struct clk * clk,struct clk * parent,size_t pidx)286 static TEE_Result clk_set_parent_no_lock(struct clk *clk, struct clk *parent,
287 size_t pidx)
288 {
289 TEE_Result res = TEE_ERROR_GENERIC;
290 bool was_enabled = false;
291
292 /* Requested parent is already the one set */
293 if (clk->parent == parent)
294 return TEE_SUCCESS;
295
296 was_enabled = clk_is_enabled_no_lock(clk);
297 /* Call is needed to decrement refcount on current parent tree */
298 if (was_enabled) {
299 if (clk->flags & CLK_SET_PARENT_PRE_ENABLE) {
300 res = clk_enable_no_lock(parent);
301 if (res)
302 return res;
303 }
304
305 clk_disable_no_lock(clk);
306 }
307
308 res = clk->ops->set_parent(clk, pidx);
309 if (res)
310 goto out;
311
312 clk->parent = parent;
313
314 /* The parent changed and the rate might also have changed */
315 clk_compute_rate_no_lock(clk);
316
317 out:
318 /* Call is needed to increment refcount on the new parent tree */
319 if (was_enabled) {
320 res = clk_enable_no_lock(clk);
321 if (res)
322 panic("Failed to re-enable clock after setting parent");
323
324 if (clk->flags & CLK_SET_PARENT_PRE_ENABLE) {
325 /* Balance refcount when new parent was pre-enabled */
326 clk_disable_no_lock(parent);
327 }
328 }
329
330 return res;
331 }
332
clk_set_parent(struct clk * clk,struct clk * parent)333 TEE_Result clk_set_parent(struct clk *clk, struct clk *parent)
334 {
335 size_t pidx = 0;
336 TEE_Result res = TEE_ERROR_GENERIC;
337
338 if (clk_get_parent_idx(clk, parent, &pidx) || !clk->ops->set_parent)
339 return TEE_ERROR_BAD_PARAMETERS;
340
341 lock_clk();
342 if (clk->flags & CLK_SET_PARENT_GATE && clk_is_enabled_no_lock(clk)) {
343 res = TEE_ERROR_BAD_STATE;
344 goto out;
345 }
346
347 res = clk_set_parent_no_lock(clk, parent, pidx);
348 out:
349 unlock_clk();
350
351 return res;
352 }
353
clk_get_rates_array(struct clk * clk,size_t start_index,unsigned long * rates,size_t * nb_elts)354 TEE_Result clk_get_rates_array(struct clk *clk, size_t start_index,
355 unsigned long *rates, size_t *nb_elts)
356 {
357 if (!clk->ops->get_rates_array)
358 return TEE_ERROR_NOT_SUPPORTED;
359
360 return clk->ops->get_rates_array(clk, start_index, rates, nb_elts);
361 }
362
clk_get_rates_steps(struct clk * clk,unsigned long * min,unsigned long * max,unsigned long * step)363 TEE_Result clk_get_rates_steps(struct clk *clk, unsigned long *min,
364 unsigned long *max, unsigned long *step)
365 {
366 if (!clk->ops->get_rates_steps)
367 return TEE_ERROR_NOT_SUPPORTED;
368
369 return clk->ops->get_rates_steps(clk, min, max, step);
370 }
371
clk_get_duty_cycle(struct clk * clk,struct clk_duty_cycle * duty_cycle)372 TEE_Result clk_get_duty_cycle(struct clk *clk,
373 struct clk_duty_cycle *duty_cycle)
374 {
375 if (clk->ops->get_duty_cycle)
376 return clk->ops->get_duty_cycle(clk, duty_cycle);
377
378 if (clk->parent && (clk->flags & CLK_DUTY_CYCLE_PARENT))
379 return clk_get_duty_cycle(clk->parent, duty_cycle);
380
381 /* Default set 50% duty cycle */
382 duty_cycle->num = 1;
383 duty_cycle->den = 2;
384
385 return TEE_SUCCESS;
386 }
387
388 /* Return updated message buffer position of NULL on failure */
add_msg(char * cur,char * end,const char * fmt,...)389 static __printf(3, 4) char *add_msg(char *cur, char *end, const char *fmt, ...)
390 {
391 va_list ap = { };
392 int max_len = end - cur;
393 int ret = 0;
394
395 va_start(ap, fmt);
396 ret = vsnprintf(cur, max_len, fmt, ap);
397 va_end(ap);
398
399 if (ret < 0 || ret >= max_len)
400 return NULL;
401
402 return cur + ret;
403 }
404
find_next_clk(struct clk * parent __maybe_unused,struct clk * sibling __maybe_unused)405 static struct clk *find_next_clk(struct clk *parent __maybe_unused,
406 struct clk *sibling __maybe_unused)
407 {
408 struct clk *clk = NULL;
409
410 #ifdef CFG_DRIVERS_CLK_PRINT_TREE
411 if (sibling)
412 clk = SLIST_NEXT(sibling, link);
413 else
414 clk = SLIST_FIRST(&clock_list);
415
416 while (clk && clk->parent != parent)
417 clk = SLIST_NEXT(clk, link);
418 #endif
419
420 return clk;
421 }
422
clk_is_parent_last_child(struct clk * clk)423 static bool clk_is_parent_last_child(struct clk *clk)
424 {
425 return !find_next_clk(clk->parent, clk);
426 }
427
indent_last_node_already_found(struct clk * node_clk,int node_indent,int cur_indent)428 static bool indent_last_node_already_found(struct clk *node_clk,
429 int node_indent, int cur_indent)
430 {
431 struct clk *clk = node_clk;
432 int n = 0;
433
434 /* Find parent clock at level @node_indent - @cur_indent - 1 */
435 for (n = 0; n < node_indent - cur_indent - 1; n++)
436 clk = clk->parent;
437
438 return clk_is_parent_last_child(clk);
439 }
440
print_clk(struct clk * clk,int indent)441 static void __maybe_unused print_clk(struct clk *clk, int indent)
442 {
443 static const char * const rate_unit[] = { "Hz", "kHz", "MHz", "GHz" };
444 int max_unit = ARRAY_SIZE(rate_unit);
445 unsigned long rate = 0;
446 char msg_buf[128] = { };
447 char *msg_end = msg_buf + sizeof(msg_buf);
448 char *msg = msg_buf;
449 int n = 0;
450
451 /*
452 * Currently prints the clock state based on the clock refcount.
453 * A future change could print the hardware clock state when
454 * related clock driver provides a struct clk_ops::is_enabled handler
455 */
456
457 if (indent) {
458 /* Indent for root clock level */
459 msg = add_msg(msg, msg_end, " ");
460 if (!msg)
461 goto out;
462
463 /* Indent for root parent to clock parent levels */
464 for (n = 0; n < indent - 1; n++) {
465 if (indent_last_node_already_found(clk, indent, n))
466 msg = add_msg(msg, msg_end, " ");
467 else
468 msg = add_msg(msg, msg_end, "| ");
469
470 if (!msg)
471 goto out;
472 }
473
474 /* Clock indentation */
475 if (clk_is_parent_last_child(clk))
476 msg = add_msg(msg, msg_end, "`-- ");
477 else
478 msg = add_msg(msg, msg_end, "|-- ");
479 } else {
480 /* Root clock indentation */
481 msg = add_msg(msg, msg_end, "o- ");
482 }
483 if (!msg)
484 goto out;
485
486 rate = clk_get_rate(clk);
487 for (n = 1; rate && !(rate % 1000) && n < max_unit; n++)
488 rate /= 1000;
489
490 msg = add_msg(msg, msg_end, "%s \t(%3s / refcnt %u / %ld %s)",
491 clk_get_name(clk),
492 refcount_val(&clk->enabled_count) ? "on " : "off",
493 refcount_val(&clk->enabled_count),
494 rate, rate_unit[n - 1]);
495 if (!msg)
496 goto out;
497
498 out:
499 if (!msg)
500 snprintf(msg_end - 4, 4, "...");
501
502 IMSG("%s", msg_buf);
503 }
504
print_tree(void)505 static void print_tree(void)
506 {
507 struct clk *clk = NULL;
508 struct clk *parent = NULL;
509 struct clk *next = NULL;
510 int indent = -1;
511
512 #ifdef CFG_DRIVERS_CLK_PRINT_TREE
513 if (SLIST_EMPTY(&clock_list)) {
514 IMSG("-- No registered clock");
515 return;
516 }
517 #endif
518
519 while (true) {
520 next = find_next_clk(parent, clk);
521 if (next) {
522 print_clk(next, indent + 1);
523 /* Enter the subtree of the next clock */
524 parent = next;
525 indent++;
526 clk = NULL;
527 } else {
528 /*
529 * We've processed all children at this level.
530 * If parent is NULL we're at the top and are done.
531 */
532 if (!parent)
533 break;
534 /*
535 * Move up one level to resume with the next
536 * child clock of the parent.
537 */
538 clk = parent;
539 parent = clk->parent;
540 indent--;
541 }
542 }
543 }
544
clk_print_tree(void)545 void clk_print_tree(void)
546 {
547 if (IS_ENABLED(CFG_DRIVERS_CLK_PRINT_TREE) &&
548 TRACE_LEVEL >= TRACE_INFO) {
549 IMSG("Clock tree summary (informative):");
550 print_tree();
551 }
552 }
553