1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2023, STMicroelectronics
4 */
5
6 #include <assert.h>
7 #include <compiler.h>
8 #include <config.h>
9 #include <drivers/regulator.h>
10 #include <initcall.h>
11 #include <keep.h>
12 #include <kernel/boot.h>
13 #include <kernel/delay.h>
14 #include <kernel/mutex_pm_aware.h>
15 #include <kernel/panic.h>
16 #include <kernel/pm.h>
17 #include <kernel/tee_time.h>
18 #include <kernel/thread.h>
19 #include <libfdt.h>
20 #include <limits.h>
21 #include <stdint.h>
22 #include <stdio.h>
23 #include <string.h>
24 #include <util.h>
25
26 static SLIST_HEAD(, regulator) regulator_device_list =
27 SLIST_HEAD_INITIALIZER(regulator);
28
29 /* Access protection mutex complying the power state transitions context */
lock_regulator(struct regulator * regulator)30 static void lock_regulator(struct regulator *regulator)
31 {
32 mutex_pm_aware_lock(®ulator->mutex);
33 }
34
unlock_regulator(struct regulator * regulator)35 static void unlock_regulator(struct regulator *regulator)
36 {
37 mutex_pm_aware_unlock(®ulator->mutex);
38 }
39
set_state(struct regulator * regulator,bool on_not_off)40 static TEE_Result set_state(struct regulator *regulator, bool on_not_off)
41 {
42 if (!regulator->ops->set_state)
43 return TEE_SUCCESS;
44
45 return regulator->ops->set_state(regulator, on_not_off);
46 }
47
regulator_refcnt_enable(struct regulator * regulator)48 static TEE_Result regulator_refcnt_enable(struct regulator *regulator)
49 {
50 TEE_Result res = TEE_ERROR_GENERIC;
51
52 FMSG("%s", regulator_name(regulator));
53
54 if (regulator->supply) {
55 res = regulator_enable(regulator->supply);
56 if (res)
57 return res;
58 }
59
60 lock_regulator(regulator);
61
62 if (!regulator->refcount) {
63 res = set_state(regulator, true);
64 if (res) {
65 EMSG("regul %s set state failed with %#"PRIx32,
66 regulator_name(regulator), res);
67
68 unlock_regulator(regulator);
69
70 if (regulator->supply &&
71 regulator_disable(regulator->supply))
72 panic();
73
74 return res;
75 }
76
77 udelay(regulator->enable_ramp_delay_us);
78 }
79
80 regulator->refcount++;
81 if (!regulator->refcount)
82 panic();
83
84 FMSG("%s refcount: %u", regulator_name(regulator), regulator->refcount);
85
86 unlock_regulator(regulator);
87
88 return TEE_SUCCESS;
89 }
90
regulator_enable(struct regulator * regulator)91 TEE_Result regulator_enable(struct regulator *regulator)
92 {
93 assert(regulator);
94 FMSG("%s", regulator_name(regulator));
95
96 if (regulator_is_always_on(regulator))
97 return TEE_SUCCESS;
98
99 return regulator_refcnt_enable(regulator);
100 }
101
regulator_refcnt_disable(struct regulator * regulator)102 static TEE_Result regulator_refcnt_disable(struct regulator *regulator)
103 {
104 FMSG("%s", regulator_name(regulator));
105
106 lock_regulator(regulator);
107
108 if (regulator->refcount == 1) {
109 TEE_Result res = set_state(regulator, false);
110
111 if (res) {
112 EMSG("regul %s set state failed with %#"PRIx32,
113 regulator_name(regulator), res);
114 unlock_regulator(regulator);
115 return res;
116 }
117 }
118
119 if (!regulator->refcount) {
120 EMSG("Unbalanced %s", regulator_name(regulator));
121 panic();
122 }
123
124 regulator->refcount--;
125
126 FMSG("%s refcount: %u", regulator_name(regulator), regulator->refcount);
127
128 unlock_regulator(regulator);
129
130 if (regulator->supply && regulator_disable(regulator->supply)) {
131 /* We can't leave this unbalanced */
132 EMSG("Can't disable %s", regulator_name(regulator->supply));
133 panic();
134 }
135
136 return TEE_SUCCESS;
137 }
138
regulator_disable(struct regulator * regulator)139 TEE_Result regulator_disable(struct regulator *regulator)
140 {
141 assert(regulator);
142 FMSG("%s", regulator_name(regulator));
143
144 if (regulator_is_always_on(regulator))
145 return TEE_SUCCESS;
146
147 return regulator_refcnt_disable(regulator);
148 }
149
regulator_is_enabled(struct regulator * regulator)150 bool regulator_is_enabled(struct regulator *regulator)
151 {
152 TEE_Result res = TEE_SUCCESS;
153 bool enabled = false;
154
155 if (!regulator->ops->get_state)
156 return true;
157
158 lock_regulator(regulator);
159 res = regulator->ops->get_state(regulator, &enabled);
160 unlock_regulator(regulator);
161
162 if (res)
163 EMSG("regul %s get state failed with %#"PRIx32,
164 regulator_name(regulator), res);
165
166 return !res && enabled;
167 }
168
regulator_get_voltage(struct regulator * regulator)169 int regulator_get_voltage(struct regulator *regulator)
170 {
171 TEE_Result res = TEE_SUCCESS;
172 int level_uv = regulator->min_uv;
173
174 if (regulator->ops->get_voltage) {
175 res = regulator->ops->get_voltage(regulator, &level_uv);
176 if (res) {
177 EMSG("%s get_voltage failed with %#"PRIx32,
178 regulator_name(regulator), res);
179 level_uv = 0;
180 }
181 }
182
183 return level_uv;
184 }
185
regulator_set_voltage(struct regulator * regulator,int level_uv)186 TEE_Result regulator_set_voltage(struct regulator *regulator, int level_uv)
187 {
188 TEE_Result res = TEE_ERROR_GENERIC;
189 int cur_uv = 0;
190
191 assert(regulator);
192 FMSG("%s %duV", regulator_name(regulator), level_uv);
193
194 if (level_uv < regulator->min_uv || level_uv > regulator->max_uv)
195 return TEE_ERROR_BAD_PARAMETERS;
196
197 cur_uv = regulator_get_voltage(regulator);
198 if (level_uv == cur_uv)
199 return TEE_SUCCESS;
200
201 if (!regulator->ops->set_voltage)
202 return TEE_ERROR_NOT_SUPPORTED;
203
204 lock_regulator(regulator);
205 res = regulator->ops->set_voltage(regulator, level_uv);
206 unlock_regulator(regulator);
207
208 if (res) {
209 EMSG("regul %s set volt failed with %#"PRIx32,
210 regulator_name(regulator), res);
211 return res;
212 }
213
214 if (regulator->ramp_delay_uv_per_us) {
215 unsigned int d = 0;
216
217 if (cur_uv > level_uv)
218 d = cur_uv - level_uv;
219 else
220 d = level_uv - cur_uv;
221
222 d /= regulator->ramp_delay_uv_per_us;
223
224 FMSG("%s %"PRIu32"uS", regulator_name(regulator), d);
225 udelay(d);
226 }
227
228 return TEE_SUCCESS;
229 }
230
regulator_supported_voltages(struct regulator * regulator,struct regulator_voltages_desc ** desc,const int ** levels)231 TEE_Result regulator_supported_voltages(struct regulator *regulator,
232 struct regulator_voltages_desc **desc,
233 const int **levels)
234 {
235 TEE_Result res = TEE_ERROR_NOT_SUPPORTED;
236
237 assert(regulator && desc && levels);
238
239 if (regulator->ops->supported_voltages)
240 res = regulator->ops->supported_voltages(regulator, desc,
241 levels);
242 if (res == TEE_ERROR_NOT_SUPPORTED) {
243 *desc = ®ulator->voltages_fallback.desc;
244 *levels = regulator->voltages_fallback.levels;
245 } else if (res) {
246 return res;
247 }
248
249 if ((*desc)->type == VOLTAGE_TYPE_FULL_LIST) {
250 assert((*desc)->num_levels);
251 assert((*levels)[0] >= regulator->min_uv);
252 assert((*levels)[(*desc)->num_levels - 1] <= regulator->max_uv);
253 } else if ((*desc)->type == VOLTAGE_TYPE_INCREMENT) {
254 assert((*levels)[0] >= regulator->min_uv);
255 assert((*levels)[1] <= regulator->max_uv);
256 } else {
257 assert(0);
258 }
259
260 return TEE_SUCCESS;
261 }
262
regulator_register(struct regulator * regulator)263 TEE_Result regulator_register(struct regulator *regulator)
264 {
265 TEE_Result res = TEE_SUCCESS;
266 int min_uv = 0;
267 int max_uv = 0;
268 int uv = 0;
269
270 if (!regulator || !regulator->ops ||
271 regulator->flags & ~REGULATOR_FLAGS_MASK)
272 return TEE_ERROR_BAD_PARAMETERS;
273
274 mutex_pm_aware_init(®ulator->mutex);
275
276 regulator_get_range(regulator, &min_uv, &max_uv);
277 if (min_uv > max_uv)
278 return TEE_ERROR_BAD_PARAMETERS;
279
280 /* Sanitize regulator effective level */
281 uv = regulator_get_voltage(regulator);
282
283 if (uv < min_uv || uv > max_uv) {
284 res = regulator_set_voltage(regulator, min_uv);
285 if (res)
286 return res;
287 }
288
289 /* Unbalanced enable refcount to keep always-on regulators enabled */
290 if (regulator_is_always_on(regulator)) {
291 res = regulator_refcnt_enable(regulator);
292 if (res)
293 return res;
294 }
295
296 /* Preset voltage list in case ops::supported_voltages is NULL */
297 if (regulator->min_uv == regulator->max_uv) {
298 regulator->voltages_fallback.desc.type = VOLTAGE_TYPE_FULL_LIST;
299 regulator->voltages_fallback.desc.num_levels = 1;
300 regulator->voltages_fallback.levels[0] = regulator->min_uv;
301 } else {
302 regulator->voltages_fallback.desc.type = VOLTAGE_TYPE_INCREMENT;
303 regulator->voltages_fallback.levels[0] = regulator->min_uv;
304 regulator->voltages_fallback.levels[1] = regulator->max_uv;
305 regulator->voltages_fallback.levels[2] = 1;
306 }
307
308 SLIST_INSERT_HEAD(®ulator_device_list, regulator, link);
309
310 return TEE_SUCCESS;
311 }
312
313 /*
314 * Clean-up regulators that are not used.
315 */
regulator_core_cleanup(void)316 static TEE_Result regulator_core_cleanup(void)
317 {
318 struct regulator *regulator = NULL;
319
320 SLIST_FOREACH(regulator, ®ulator_device_list, link) {
321 if (!regulator->refcount) {
322 DMSG("disable %s", regulator_name(regulator));
323 lock_regulator(regulator);
324 set_state(regulator, false /* disable */);
325 unlock_regulator(regulator);
326 }
327 }
328
329 if (TRACE_LEVEL >= TRACE_DEBUG)
330 regulator_print_tree();
331
332 return TEE_SUCCESS;
333 }
334
335 release_init_resource(regulator_core_cleanup);
336
337 /* Return updated message buffer position of NULL on failure */
add_msg(char * cur,char * end,const char * fmt,...)338 static __printf(3, 4) char *add_msg(char *cur, char *end, const char *fmt, ...)
339 {
340 va_list ap = { };
341 int max_len = end - cur;
342 int ret = 0;
343
344 va_start(ap, fmt);
345 ret = vsnprintf(cur, max_len, fmt, ap);
346 va_end(ap);
347
348 if (ret < 0 || ret >= max_len)
349 return NULL;
350
351 return cur + ret;
352 }
353
find_next_regulator(struct regulator * parent,struct regulator * sibling)354 static struct regulator *find_next_regulator(struct regulator *parent,
355 struct regulator *sibling)
356 {
357 struct regulator *regulator = NULL;
358
359 if (sibling)
360 regulator = SLIST_NEXT(sibling, link);
361 else
362 regulator = SLIST_FIRST(®ulator_device_list);
363
364 while (regulator && regulator->supply != parent)
365 regulator = SLIST_NEXT(regulator, link);
366
367 return regulator;
368 }
369
370 /* Regulator is the last supplied one by its supply in the registered list */
regulator_is_supply_last_supplied(struct regulator * regulator)371 static bool regulator_is_supply_last_supplied(struct regulator *regulator)
372 {
373 return !find_next_regulator(regulator->supply, regulator);
374 }
375
376 /* Supply last node may already be printed for indentation level @cur_indent */
indent_with_empty_string(struct regulator * node_regulator,int node_indent,int cur_indent)377 static bool indent_with_empty_string(struct regulator *node_regulator,
378 int node_indent, int cur_indent)
379 {
380 struct regulator *r = node_regulator;
381 int n = 0;
382
383 /* Find supply at indentation level @node_indent - @cur_indent - 1 */
384 for (n = 0; n < node_indent - cur_indent - 1; n++)
385 r = r->supply;
386
387 return regulator_is_supply_last_supplied(r);
388 }
389
print_regulator(struct regulator * regulator,int indent)390 static void __maybe_unused print_regulator(struct regulator *regulator,
391 int indent)
392 {
393 static const char * const level_unit[] = { "uV", "mV", "V" };
394 int max_unit = ARRAY_SIZE(level_unit);
395 int level_max = 0;
396 int level_min = 0;
397 int level_cur = 0;
398 char msg_buf[128] = { };
399 char *msg_end = msg_buf + sizeof(msg_buf);
400 char *msg = msg_buf;
401 int n_max = 0;
402 int n_min = 0;
403 int n_cur = 0;
404 int n = 0;
405
406 if (indent) {
407 /* Indent for root clock level */
408 msg = add_msg(msg, msg_end, " ");
409 if (!msg)
410 goto out;
411
412 /* Indent for root supply to regulator supply levels */
413 for (n = 0; n < indent - 1; n++) {
414 if (indent_with_empty_string(regulator, indent, n))
415 msg = add_msg(msg, msg_end, " ");
416 else
417 msg = add_msg(msg, msg_end, "| ");
418 if (!msg)
419 goto out;
420 }
421
422 /* Regulator indentation */
423 if (regulator_is_supply_last_supplied(regulator))
424 msg = add_msg(msg, msg_end, "`-- ");
425 else
426 msg = add_msg(msg, msg_end, "|-- ");
427
428 if (!msg)
429 goto out;
430 } else {
431 /* Root supply indentation */
432 msg = add_msg(msg, msg_end, "o- ");
433 }
434
435 regulator_get_range(regulator, &level_min, &level_max);
436 level_cur = regulator_get_voltage(regulator);
437
438 for (n_cur = 1; !(level_cur % 1000) && n_cur < max_unit; n_cur++)
439 level_cur /= 1000;
440 for (n_max = 1; !(level_max % 1000) && n_max < max_unit; n_max++)
441 level_max /= 1000;
442 for (n_min = 1; !(level_min % 1000) && n_min < max_unit; n_min++)
443 level_min /= 1000;
444
445 msg = add_msg(msg, msg_end, "%s \t(%3s / refcnt %u / flags %#"PRIx32
446 " / %d %s ", regulator_name(regulator),
447 regulator_is_enabled(regulator) ? "on " : "off",
448 regulator->refcount, regulator->flags,
449 level_cur, level_unit[n_cur - 1]);
450 if (!msg)
451 goto out;
452
453 if (level_min == level_max)
454 msg = add_msg(msg, msg_end, "fixed)");
455 else if (level_max == INT_MAX)
456 msg = add_msg(msg, msg_end, "[%d %s .. MAX])",
457 level_min, level_unit[n_min - 1]);
458 else
459 msg = add_msg(msg, msg_end, "[%d %s .. %d %s])",
460 level_min, level_unit[n_min - 1],
461 level_max, level_unit[n_max - 1]);
462
463 out:
464 if (!msg)
465 snprintf(msg_end - 4, 4, "...");
466
467 IMSG("%s", msg_buf);
468 }
469
print_tree(void)470 static void print_tree(void)
471 {
472 struct regulator *regulator = NULL;
473 struct regulator *parent = NULL;
474 struct regulator *next = NULL;
475 int indent = -1;
476
477 while (true) {
478 next = find_next_regulator(parent, regulator);
479 if (next) {
480 print_regulator(next, indent + 1);
481 /* Enter the subtree of the next regulator */
482 parent = next;
483 indent++;
484 regulator = NULL;
485 } else {
486 /*
487 * We've processed all children at this level.
488 * If parent is NULL we're at the top and are done.
489 */
490 if (!parent)
491 break;
492 /*
493 * Move up one level to resume with the next
494 * regulator of the parent.
495 */
496 regulator = parent;
497 parent = regulator->supply;
498 indent--;
499 }
500 }
501 }
502
regulator_print_tree(void)503 void regulator_print_tree(void)
504 {
505 if (IS_ENABLED(CFG_DRIVERS_REGULATOR_PRINT_TREE) &&
506 TRACE_LEVEL >= TRACE_INFO) {
507 IMSG("Regulator tree summary");
508 if (SLIST_EMPTY(®ulator_device_list))
509 IMSG("-- No registered regulator");
510 else
511 print_tree();
512 }
513 }
514