blob: a651b6e152a2f087daa5a377bf0d4b0fd9155b3d [file] [log] [blame]
Aaron Durbinbda35772014-09-19 15:52:31 -05001/*
2 * This file is part of the coreboot project.
3 *
4 * Copyright 2014 Google Inc.
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; version 2 of the License.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA, 02110-1301 USA
18 */
19
20#include <string.h>
21#include <stdlib.h>
22#include <smp/spinlock.h>
23#include <arch/cpu.h>
24#include <arch/psci.h>
25#include <arch/smc.h>
26#include <arch/transition.h>
27#include <arch/lib_helpers.h>
28#include <console/console.h>
29#include "secmon.h"
30
Aaron Durbinbda35772014-09-19 15:52:31 -050031DECLARE_SPIN_LOCK(psci_spinlock);
32
Aaron Durbinb777f3e2014-10-28 15:38:17 -050033/* Root of PSCI node tree. */
34static struct psci_node psci_root;
Aaron Durbinbda35772014-09-19 15:52:31 -050035
Aaron Durbinb777f3e2014-10-28 15:38:17 -050036/* Array of all the psci_nodes in system. */
37static size_t psci_num_nodes;
38static struct psci_node **psci_nodes;
Aaron Durbinbda35772014-09-19 15:52:31 -050039
40static inline void psci_lock(void)
41{
42 spin_lock(&psci_spinlock);
43}
44
45static inline void psci_unlock(void)
46{
47 spin_unlock(&psci_spinlock);
48}
49
Aaron Durbinb777f3e2014-10-28 15:38:17 -050050static inline int psci_state_locked(const struct psci_node *e)
Aaron Durbinbda35772014-09-19 15:52:31 -050051{
Aaron Durbinb777f3e2014-10-28 15:38:17 -050052 return e->state;
Aaron Durbinbda35772014-09-19 15:52:31 -050053}
54
Aaron Durbinb777f3e2014-10-28 15:38:17 -050055static inline void psci_set_state_locked(struct psci_node *e, int s)
Aaron Durbinbda35772014-09-19 15:52:31 -050056{
Aaron Durbinb777f3e2014-10-28 15:38:17 -050057 e->state = s;
Aaron Durbinbda35772014-09-19 15:52:31 -050058}
59
Aaron Durbinb777f3e2014-10-28 15:38:17 -050060static struct psci_node *psci_node_lookup(uint64_t mpidr, int level)
Aaron Durbinbda35772014-09-19 15:52:31 -050061{
Aaron Durbinb777f3e2014-10-28 15:38:17 -050062 size_t i;
Aaron Durbinbda35772014-09-19 15:52:31 -050063
Aaron Durbinb777f3e2014-10-28 15:38:17 -050064 /* The array of node pointers are in depth-first order of the tree. */
65 for (i = 0; i < psci_num_nodes; i++) {
66 struct psci_node *current = psci_nodes[i];
67
68 if (current->mpidr > mpidr)
69 break;
70 if (current->mpidr < mpidr)
71 continue;
72 if (current->level == level)
73 return current;
74 }
75 return NULL;
76}
77
78static inline struct psci_node *node_self(void)
79{
80 return psci_node_lookup(cpu_info()->mpidr, PSCI_AFFINITY_LEVEL_0);
81}
82
83/* Find the ancestor of node affected by a state transition limited by level. */
84static struct psci_node *psci_find_ancestor(struct psci_node *e, int level,
85 int state)
86{
87 struct psci_node *p;
88
89 /* If all siblings of the node are already off then parent can be
90 * set to off as well. */
91 if (state == PSCI_STATE_OFF) {
92 while (1) {
93 size_t i;
94 struct psci_node *s;
95
96 if (psci_root_node(e))
97 return e;
98
99 p = psci_node_parent(e);
100
101 if (p->level > level)
102 return e;
103
104 for (i = 0; i < p->children.num; i++) {
105 s = &p->children.nodes[i];
106 /* Don't check target. */
107 if (s == e)
108 continue;
109 if (psci_state_locked(s) != PSCI_STATE_OFF)
110 return e;
111 }
112
113 e = p;
114 }
Aaron Durbinbda35772014-09-19 15:52:31 -0500115 }
116
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500117 /* All ancestors in state OFF are affected. */
118 if (state == PSCI_STATE_ON_PENDING) {
119 while (1) {
120 /* At the root. Return last affected node. */
121 if (psci_root_node(e))
122 return e;
123
124 p = psci_node_parent(e);
125
126 if (p->level > level)
127 return e;
128
129 /* This parent is already ON. */
130 if (psci_state_locked(p) != PSCI_STATE_OFF)
131 return e;
132
133 e = p;
134 }
135 }
136
137 /* Default to returning node passed in. */
138 return e;
139}
140
141static void psci_set_hierarchy_state(struct psci_node *from,
142 struct psci_node *to,
143 int state)
144{
145 struct psci_node *end;
146
147 end = psci_node_parent(to);
148
149 while (from != end) {
150 /* Raced with another CPU as state is already set. */
151 if (psci_state_locked(from) == state)
152 break;
153 psci_set_state_locked(from, state);
154 from = psci_node_parent(from);
155 }
Aaron Durbinbda35772014-09-19 15:52:31 -0500156}
157
158static void psci_cpu_on_callback(void *arg)
159{
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500160 struct exc_state state;
161 int target_el;
162 struct psci_node *e = arg;
Aaron Durbinbda35772014-09-19 15:52:31 -0500163
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500164 psci_lock();
165 psci_set_hierarchy_state(e, e->cpu_state.ancestor, PSCI_STATE_ON);
166 psci_unlock();
167
168 /* Target EL is determined if HVC is enabled or not. */
169 target_el = (raw_read_scr_el3() & SCR_HVC_ENABLE) ? EL2 : EL1;
170
171 memset(&state, 0, sizeof(state));
172 state.elx.spsr = get_eret_el(target_el, SPSR_USE_H);
Aaron Durbin9fd7b1c2014-11-05 10:45:05 -0600173 transition_with_entry(e->cpu_state.startup.run,
174 e->cpu_state.startup.arg, &state);
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500175}
176
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500177static void psci_cpu_on_prepare(struct psci_cmd *cmd,
178 const struct cpu_action *a)
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500179{
180 struct psci_node *ancestor;
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500181 struct psci_node *e;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500182 int state = PSCI_STATE_ON_PENDING;
183
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500184 e = cmd->target;
Aaron Durbin9fd7b1c2014-11-05 10:45:05 -0600185 e->cpu_state.startup = *a;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500186 ancestor = psci_find_ancestor(e, PSCI_AFFINITY_LEVEL_HIGHEST, state);
187 e->cpu_state.ancestor = ancestor;
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500188 cmd->ancestor = ancestor;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500189}
190
191static int psci_schedule_cpu_on(struct psci_node *e)
192{
Aaron Durbin9fd7b1c2014-11-05 10:45:05 -0600193 struct cpu_info *ci;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500194 struct cpu_action action = {
195 .run = &psci_cpu_on_callback,
196 .arg = e,
197 };
198
Aaron Durbin9fd7b1c2014-11-05 10:45:05 -0600199 ci = e->cpu_state.ci;
200 if (ci == NULL || arch_run_on_cpu_async(ci->id, &action)) {
201 psci_set_hierarchy_state(e, e->cpu_state.ancestor,
202 PSCI_STATE_OFF);
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500203 return PSCI_RET_INTERNAL_FAILURE;
Aaron Durbin9fd7b1c2014-11-05 10:45:05 -0600204 }
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500205
206 return PSCI_RET_SUCCESS;
207}
208
Aaron Durbin9fd7b1c2014-11-05 10:45:05 -0600209void psci_turn_on_self(const struct cpu_action *action)
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500210{
211 struct psci_node *e = node_self();
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500212 struct psci_cmd cmd = {
213 .type = PSCI_CMD_ON,
214 };
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500215
216 if (e == NULL) {
217 printk(BIOS_ERR, "Couldn't turn on self: mpidr %llx\n",
218 cpu_info()->mpidr);
219 return;
220 }
221
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500222 cmd.target = e;
223
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500224 psci_lock();
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500225 psci_cpu_on_prepare(&cmd, action);
226 psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_ON_PENDING);
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500227 psci_unlock();
228
229 psci_schedule_cpu_on(e);
Aaron Durbinbda35772014-09-19 15:52:31 -0500230}
231
Aaron Durbin6cdacb32014-11-06 15:17:33 -0600232void psci_cpu_entry(void)
233{
234 /*
235 * Just wait for an action to be performed. Only CPU_ON is supported
236 * initially. i.e. no power down then wake.
237 */
238 secmon_wait_for_action();
239}
240
Aaron Durbinbda35772014-09-19 15:52:31 -0500241static void psci_cpu_on(struct psci_func *pf)
242{
243 uint64_t entry;
244 uint64_t target_mpidr;
245 uint64_t context_id;
Aaron Durbinbda35772014-09-19 15:52:31 -0500246 int cpu_state;
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500247 int ret;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500248 struct psci_node *e;
Aaron Durbin9fd7b1c2014-11-05 10:45:05 -0600249 struct cpu_action action;
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500250 struct psci_cmd cmd = {
251 .type = PSCI_CMD_ON,
252 };
Aaron Durbinbda35772014-09-19 15:52:31 -0500253
254 target_mpidr = psci64_arg(pf, PSCI_PARAM_0);
255 entry = psci64_arg(pf, PSCI_PARAM_1);
256 context_id = psci64_arg(pf, PSCI_PARAM_2);
257
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500258 e = psci_node_lookup(target_mpidr, PSCI_AFFINITY_LEVEL_0);
Aaron Durbinbda35772014-09-19 15:52:31 -0500259
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500260 if (e == NULL) {
Aaron Durbinbda35772014-09-19 15:52:31 -0500261 psci32_return(pf, PSCI_RET_INVALID_PARAMETERS);
262 return;
263 }
264
265 psci_lock();
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500266 cpu_state = psci_state_locked(e);
Aaron Durbinbda35772014-09-19 15:52:31 -0500267
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500268 if (cpu_state == PSCI_STATE_ON_PENDING) {
Aaron Durbinbda35772014-09-19 15:52:31 -0500269 psci32_return(pf, PSCI_RET_ON_PENDING);
270 psci_unlock();
271 return;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500272 } else if (cpu_state == PSCI_STATE_ON) {
Aaron Durbinbda35772014-09-19 15:52:31 -0500273 psci32_return(pf, PSCI_RET_ALREADY_ON);
274 psci_unlock();
275 return;
276 }
277
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500278 cmd.target = e;
Aaron Durbin9fd7b1c2014-11-05 10:45:05 -0600279 action.run = (void *)entry;
280 action.arg = (void *)context_id;
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500281 psci_cpu_on_prepare(&cmd, &action);
282
283 ret = soc_psci_ops.cmd_prepare(&cmd);
284
285 if (ret == PSCI_RET_SUCCESS)
286 psci_set_hierarchy_state(e, cmd.ancestor,
287 PSCI_STATE_ON_PENDING);
288
Aaron Durbinbda35772014-09-19 15:52:31 -0500289 psci_unlock();
290
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500291 if (ret != PSCI_RET_SUCCESS)
292 return psci32_return(pf, ret);
293
294 ret = soc_psci_ops.cmd_commit(&cmd);
295
296 if (ret != PSCI_RET_SUCCESS) {
297 psci_lock();
298 psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_OFF);
299 psci_unlock();
300 return psci32_return(pf, ret);
301 }
302
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500303 psci32_return(pf, psci_schedule_cpu_on(e));
Aaron Durbinbda35772014-09-19 15:52:31 -0500304}
305
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500306static int psci_turn_off_node(struct psci_node *e, int level,
307 int state_id)
Aaron Durbinbda35772014-09-19 15:52:31 -0500308{
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500309 int ret;
310 struct psci_cmd cmd = {
311 .type = PSCI_CMD_OFF,
312 .state_id = state_id,
313 .target = e,
314 };
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500315
Aaron Durbinbda35772014-09-19 15:52:31 -0500316 psci_lock();
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500317
318 cmd.ancestor = psci_find_ancestor(e, level, PSCI_STATE_OFF);
319
320 ret = soc_psci_ops.cmd_prepare(&cmd);
321
322 if (ret == PSCI_RET_SUCCESS)
323 psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_OFF);
324
Aaron Durbinbda35772014-09-19 15:52:31 -0500325 psci_unlock();
326
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500327 if (ret != PSCI_RET_SUCCESS)
328 return ret;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500329
Aaron Durbin0179fcf2014-10-30 13:13:50 -0500330 /* Should never return. */
331 ret = soc_psci_ops.cmd_commit(&cmd);
332
333 /* Adjust ret to be an error. */
334 if (ret == PSCI_RET_SUCCESS)
335 ret = PSCI_RET_INTERNAL_FAILURE;
336
337 /* Turn things back on. */
338 psci_lock();
339 psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_ON);
340 psci_unlock();
341
342 return ret;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500343}
344
345int psci_turn_off_self(void)
346{
347 struct psci_node *e = node_self();
348
349 if (e == NULL) {
350 printk(BIOS_ERR, "No PSCI node for MPIDR %llx.\n",
351 cpu_info()->mpidr);
352 return PSCI_RET_INTERNAL_FAILURE;
353 }
354
355 /* -1 state id indicates to SoC to make its own decision for
356 * internal state when powering off the node. */
357 return psci_turn_off_node(e, PSCI_AFFINITY_LEVEL_HIGHEST, -1);
Aaron Durbinbda35772014-09-19 15:52:31 -0500358}
359
360static int psci_handler(struct smc_call *smc)
361{
362 struct psci_func pf_storage;
363 struct psci_func *pf = &pf_storage;
364
365 psci_func_init(pf, smc);
366
367 switch (pf->id) {
368 case PSCI_CPU_ON64:
369 psci_cpu_on(pf);
370 break;
Furquan Shaikh3c526b12015-04-09 08:22:57 -0700371 case PSCI_CPU_OFF32:
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500372 psci32_return(pf, psci_turn_off_self());
Aaron Durbinbda35772014-09-19 15:52:31 -0500373 break;
374 default:
375 psci32_return(pf, PSCI_RET_NOT_SUPPORTED);
376 break;
377 }
378
379 return 0;
380}
381
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500382static void psci_link_cpu_info(void *arg)
383{
384 struct psci_node *e = node_self();
385
386 if (e == NULL) {
387 printk(BIOS_ERR, "No PSCI node for MPIDR %llx.\n",
388 cpu_info()->mpidr);
389 return;
390 }
391
392 e->cpu_state.ci = cpu_info();
393}
394
395static int psci_init_node(struct psci_node *e,
396 struct psci_node *parent,
397 int level, uint64_t mpidr)
398{
399 size_t i;
400 uint64_t mpidr_inc;
401 struct psci_node_group *ng;
402 size_t num_children;
403
404 memset(e, 0, sizeof(*e));
405 e->mpidr = mpidr;
406 psci_set_state_locked(e, PSCI_STATE_OFF);
407 e->parent = parent;
408 e->level = level;
409
410 if (level == PSCI_AFFINITY_LEVEL_0)
411 return 0;
412
413 num_children = soc_psci_ops.children_at_level(level, mpidr);
414
415 if (num_children == 0)
416 return 0;
417
418 ng = &e->children;
419 ng->num = num_children;
420 ng->nodes = malloc(ng->num * sizeof(struct psci_node));
421 if (ng->nodes == NULL) {
422 printk(BIOS_DEBUG, "PSCI: Allocation failure at level %d\n",
423 level);
424 return -1;
425 }
426
427 /* Switch to next level below. */
428 level = psci_level_below(level);
429 mpidr_inc = mpidr_mask(!!(level == PSCI_AFFINITY_LEVEL_3),
430 !!(level == PSCI_AFFINITY_LEVEL_2),
431 !!(level == PSCI_AFFINITY_LEVEL_1),
432 !!(level == PSCI_AFFINITY_LEVEL_0));
433
434 for (i = 0; i < ng->num; i++) {
435 struct psci_node *c = &ng->nodes[i];
436
437 /* Recursively initialize the nodes. */
438 if (psci_init_node(c, e, level, mpidr))
439 return -1;
440 mpidr += mpidr_inc;
441 }
442
443 return 0;
444}
445
446static size_t psci_count_children(struct psci_node *e)
447{
448 size_t i;
449 size_t count;
450
451 if (e->level == PSCI_AFFINITY_LEVEL_0)
452 return 0;
453
454 count = e->children.num;
455 for (i = 0; i < e->children.num; i++)
456 count += psci_count_children(&e->children.nodes[i]);
457
458 return count;
459}
460
461static size_t psci_write_nodes(struct psci_node *e, size_t index)
462{
463 size_t i;
464
465 /*
466 * Recursively save node pointers in array. Node pointers are
467 * ordered in ascending mpidr and descending level within same mpidr.
468 * i.e. each node is saved in depth-first order of the tree.
469 */
470 if (e->level != PSCI_AFFINITY_ROOT) {
471 psci_nodes[index] = e;
472 index++;
473 }
474
475 if (e->level == PSCI_AFFINITY_LEVEL_0)
476 return index;
477
478 for (i = 0; i < e->children.num; i++)
479 index = psci_write_nodes(&e->children.nodes[i], index);
480
481 return index;
482}
483
484static int psci_allocate_nodes(void)
485{
486 int level;
487 size_t num_children;
488 uint64_t mpidr;
489 struct psci_node *e;
490
491 mpidr = 0;
492 level = PSCI_AFFINITY_ROOT;
493
494 /* Find where the root should start. */
495 while (psci_level_below(level) >= PSCI_AFFINITY_LEVEL_0) {
496 num_children = soc_psci_ops.children_at_level(level, mpidr);
497
498 if (num_children == 0) {
499 printk(BIOS_ERR, "PSCI: No children at level %d!\n",
500 level);
501 return -1;
502 }
503
504 /* The root starts where the affinity levels branch. */
505 if (num_children > 1)
506 break;
507
508 level = psci_level_below(level);
509 }
510
511 if (psci_init_node(&psci_root, NULL, level, mpidr)) {
512 printk(BIOS_ERR, "PSCI init node failure.\n");
513 return -1;
514 }
515
516 num_children = psci_count_children(&psci_root);
517 /* Count the root node if isn't a fake node. */
518 if (psci_root.level != PSCI_AFFINITY_ROOT)
519 num_children++;
520
521 psci_nodes = malloc(num_children * sizeof(void *));
522 psci_num_nodes = num_children;
523
524 if (psci_nodes == NULL) {
525 printk(BIOS_ERR, "PSCI node pointer array failure.\n");
526 return -1;
527 }
528
529 num_children = psci_write_nodes(&psci_root, 0);
530 if (num_children != psci_num_nodes) {
531 printk(BIOS_ERR, "Wrong nodes written: %zd vs %zd.\n",
532 num_children, psci_num_nodes);
533 return -1;
534 }
535
536 /*
537 * By default all nodes are set to PSCI_STATE_OFF. In order not
538 * to race with other CPUs turning themselves off set the BSPs
539 * affinity node to ON.
540 */
541 e = node_self();
542 if (e == NULL) {
543 printk(BIOS_ERR, "No PSCI node for BSP.\n");
544 return -1;
545 }
546 psci_set_state_locked(e, PSCI_STATE_ON);
547
548 return 0;
549}
550
Aaron Durbin6cdacb32014-11-06 15:17:33 -0600551void psci_init(uintptr_t cpu_on_entry)
Aaron Durbinbda35772014-09-19 15:52:31 -0500552{
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500553 struct cpu_action action = {
554 .run = &psci_link_cpu_info,
555 };
Aaron Durbinbda35772014-09-19 15:52:31 -0500556
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500557 if (psci_allocate_nodes()) {
558 printk(BIOS_ERR, "PSCI support not enabled.\n");
Aaron Durbinbda35772014-09-19 15:52:31 -0500559 return;
Aaron Durbinb777f3e2014-10-28 15:38:17 -0500560 }
561
562 if (arch_run_on_all_cpus_async(&action))
563 printk(BIOS_ERR, "Error linking cpu_info to PSCI nodes.\n");
Aaron Durbinbda35772014-09-19 15:52:31 -0500564
565 /* Register PSCI handlers. */
Furquan Shaikh3c526b12015-04-09 08:22:57 -0700566 if (smc_register_range(PSCI_CPU_SUSPEND32, PSCI_CPU_ON32,
567 &psci_handler))
568 printk(BIOS_ERR, "Couldn't register PSCI handler.\n");
569
570 if (smc_register_range(PSCI_CPU_SUSPEND64, PSCI_CPU_ON64,
571 &psci_handler))
Aaron Durbinbda35772014-09-19 15:52:31 -0500572 printk(BIOS_ERR, "Couldn't register PSCI handler.\n");
Aaron Durbine37c18f2014-11-25 17:23:22 -0600573
574 /* Inform SoC layer of CPU_ON entry point. */
575 psci_soc_init(cpu_on_entry);
Aaron Durbinbda35772014-09-19 15:52:31 -0500576}