diff options
| -rw-r--r-- | src/arch/arm64/armv8/secmon/psci.c | 440 | ||||
| -rw-r--r-- | src/arch/arm64/armv8/secmon/secmon_init.c | 29 | ||||
| -rw-r--r-- | src/arch/arm64/armv8/secmon/smc.c | 12 | ||||
| -rw-r--r-- | src/arch/arm64/include/arch/psci.h | 82 | ||||
| -rw-r--r-- | src/soc/nvidia/tegra132/Makefile.inc | 1 | ||||
| -rw-r--r-- | src/soc/nvidia/tegra132/psci.c | 46 |
6 files changed, 527 insertions, 83 deletions
diff --git a/src/arch/arm64/armv8/secmon/psci.c b/src/arch/arm64/armv8/secmon/psci.c index 9764cac4d9..0051031e22 100644 --- a/src/arch/arm64/armv8/secmon/psci.c +++ b/src/arch/arm64/armv8/secmon/psci.c @@ -28,23 +28,14 @@ #include <console/console.h> #include "secmon.h" -enum { - PSCI_CPU_STATE_OFF = 0, - PSCI_CPU_STATE_ON_PENDING, - PSCI_CPU_STATE_ON, -}; - -struct psci_cpu_state { - uint64_t mpidr; - void *entry; - void *arg; - int state; -}; - DECLARE_SPIN_LOCK(psci_spinlock); -static struct psci_cpu_state psci_state[CONFIG_MAX_CPUS]; +/* Root of PSCI node tree. */ +static struct psci_node psci_root; +/* Array of all the psci_nodes in system. */ +static size_t psci_num_nodes; +static struct psci_node **psci_nodes; static inline void psci_lock(void) { @@ -56,33 +47,173 @@ static inline void psci_unlock(void) spin_unlock(&psci_spinlock); } -static inline int psci_cpu_state_locked(int i) +static inline int psci_state_locked(const struct psci_node *e) { - return psci_state[i].state; + return e->state; } -static inline void psci_cpu_set_state_locked(int i, int s) +static inline void psci_set_state_locked(struct psci_node *e, int s) { - psci_state[i].state = s; + e->state = s; } -static struct cpu_info *mpidr_to_cpu_info(uint64_t mpidr) +static struct psci_node *psci_node_lookup(uint64_t mpidr, int level) { - int i; + size_t i; + + /* The array of node pointers are in depth-first order of the tree. */ + for (i = 0; i < psci_num_nodes; i++) { + struct psci_node *current = psci_nodes[i]; + + if (current->mpidr > mpidr) + break; + if (current->mpidr < mpidr) + continue; + if (current->level == level) + return current; + } + return NULL; +} + +static inline struct psci_node *node_self(void) +{ + return psci_node_lookup(cpu_info()->mpidr, PSCI_AFFINITY_LEVEL_0); +} - for (i = 0; i < ARRAY_SIZE(psci_state); i++) { - if (mpidr == psci_state[i].mpidr) - return cpu_info_for_cpu(i); +/* Find the ancestor of node affected by a state transition limited by level. */ +static struct psci_node *psci_find_ancestor(struct psci_node *e, int level, + int state) +{ + struct psci_node *p; + + /* If all siblings of the node are already off then parent can be + * set to off as well. */ + if (state == PSCI_STATE_OFF) { + while (1) { + size_t i; + struct psci_node *s; + + if (psci_root_node(e)) + return e; + + p = psci_node_parent(e); + + if (p->level > level) + return e; + + for (i = 0; i < p->children.num; i++) { + s = &p->children.nodes[i]; + /* Don't check target. */ + if (s == e) + continue; + if (psci_state_locked(s) != PSCI_STATE_OFF) + return e; + } + + e = p; + } } - return NULL; + /* All ancestors in state OFF are affected. */ + if (state == PSCI_STATE_ON_PENDING) { + while (1) { + /* At the root. Return last affected node. */ + if (psci_root_node(e)) + return e; + + p = psci_node_parent(e); + + if (p->level > level) + return e; + + /* This parent is already ON. */ + if (psci_state_locked(p) != PSCI_STATE_OFF) + return e; + + e = p; + } + } + + /* Default to returning node passed in. */ + return e; +} + +static void psci_set_hierarchy_state(struct psci_node *from, + struct psci_node *to, + int state) +{ + struct psci_node *end; + + end = psci_node_parent(to); + + while (from != end) { + /* Raced with another CPU as state is already set. */ + if (psci_state_locked(from) == state) + break; + psci_set_state_locked(from, state); + from = psci_node_parent(from); + } } static void psci_cpu_on_callback(void *arg) { - struct psci_cpu_state *s = arg; + struct exc_state state; + int target_el; + struct psci_node *e = arg; - psci_turn_on_self(s->entry, s->arg); + psci_lock(); + psci_set_hierarchy_state(e, e->cpu_state.ancestor, PSCI_STATE_ON); + psci_unlock(); + + /* Target EL is determined if HVC is enabled or not. */ + target_el = (raw_read_scr_el3() & SCR_HVC_ENABLE) ? EL2 : EL1; + + memset(&state, 0, sizeof(state)); + state.elx.spsr = get_eret_el(target_el, SPSR_USE_H); + transition_with_entry(e->cpu_state.entry, e->cpu_state.arg, &state); +} + +static void psci_cpu_on_prepare(struct psci_node *e, + void *entry, void *arg) +{ + struct psci_node *ancestor; + int state = PSCI_STATE_ON_PENDING; + + e->cpu_state.entry = entry; + e->cpu_state.arg = arg; + ancestor = psci_find_ancestor(e, PSCI_AFFINITY_LEVEL_HIGHEST, state); + e->cpu_state.ancestor = ancestor; + psci_set_hierarchy_state(e, ancestor, state); +} + +static int psci_schedule_cpu_on(struct psci_node *e) +{ + struct cpu_action action = { + .run = &psci_cpu_on_callback, + .arg = e, + }; + + if (arch_run_on_cpu_async(e->cpu_state.ci->id, &action)) + return PSCI_RET_INTERNAL_FAILURE; + + return PSCI_RET_SUCCESS; +} + +void psci_turn_on_self(void *entry, void *arg) +{ + struct psci_node *e = node_self(); + + if (e == NULL) { + printk(BIOS_ERR, "Couldn't turn on self: mpidr %llx\n", + cpu_info()->mpidr); + return; + } + + psci_lock(); + psci_cpu_on_prepare(e, entry, arg); + psci_unlock(); + + psci_schedule_cpu_on(e); } static void psci_cpu_on(struct psci_func *pf) @@ -90,60 +221,68 @@ static void psci_cpu_on(struct psci_func *pf) uint64_t entry; uint64_t target_mpidr; uint64_t context_id; - struct cpu_info *ci; int cpu_state; - struct cpu_action action; + struct psci_node *e; target_mpidr = psci64_arg(pf, PSCI_PARAM_0); entry = psci64_arg(pf, PSCI_PARAM_1); context_id = psci64_arg(pf, PSCI_PARAM_2); - ci = mpidr_to_cpu_info(target_mpidr); + e = psci_node_lookup(target_mpidr, PSCI_AFFINITY_LEVEL_0); - if (ci == NULL) { + if (e == NULL) { psci32_return(pf, PSCI_RET_INVALID_PARAMETERS); return; } psci_lock(); - cpu_state = psci_cpu_state_locked(ci->id); + cpu_state = psci_state_locked(e); - if (cpu_state == PSCI_CPU_STATE_ON_PENDING) { + if (cpu_state == PSCI_STATE_ON_PENDING) { psci32_return(pf, PSCI_RET_ON_PENDING); psci_unlock(); return; - } else if (cpu_state == PSCI_CPU_STATE_ON) { + } else if (cpu_state == PSCI_STATE_ON) { psci32_return(pf, PSCI_RET_ALREADY_ON); psci_unlock(); return; } - psci_cpu_set_state_locked(ci->id, PSCI_CPU_STATE_ON_PENDING); - /* Set the parameters and initialize the action. */ - psci_state[ci->id].entry = (void *)(uintptr_t)entry; - psci_state[ci->id].arg = (void *)(uintptr_t)context_id; - action.run = &psci_cpu_on_callback; - action.arg = &psci_state[ci->id]; - - if (arch_run_on_cpu_async(ci->id, &action)) { - psci32_return(pf, PSCI_RET_INTERNAL_FAILURE); - psci_unlock(); - return; - } - + psci_cpu_on_prepare(e, (void *)entry, (void *)context_id); psci_unlock(); - psci32_return(pf, PSCI_RET_SUCCESS); + psci32_return(pf, psci_schedule_cpu_on(e)); } -static void psci_cpu_off(struct psci_func *pf) +static int psci_turn_off_node(struct psci_node *e, int level, + int state_id) { + struct psci_node *ancestor; + psci_lock(); - psci_cpu_set_state_locked(cpu_info()->id, PSCI_CPU_STATE_OFF); + ancestor = psci_find_ancestor(e, level, PSCI_STATE_OFF); + psci_set_hierarchy_state(e, ancestor, PSCI_STATE_OFF); psci_unlock(); /* TODO(adurbin): writeback cache and actually turn off CPU. */ secmon_trampoline(&secmon_wait_for_action, NULL); + + return PSCI_RET_SUCCESS; +} + +int psci_turn_off_self(void) +{ + struct psci_node *e = node_self(); + + if (e == NULL) { + printk(BIOS_ERR, "No PSCI node for MPIDR %llx.\n", + cpu_info()->mpidr); + return PSCI_RET_INTERNAL_FAILURE; + } + + /* -1 state id indicates to SoC to make its own decision for + * internal state when powering off the node. */ + return psci_turn_off_node(e, PSCI_AFFINITY_LEVEL_HIGHEST, -1); } static int psci_handler(struct smc_call *smc) @@ -158,7 +297,7 @@ static int psci_handler(struct smc_call *smc) psci_cpu_on(pf); break; case PSCI_CPU_OFF64: - psci_cpu_off(pf); + psci32_return(pf, psci_turn_off_self()); break; default: psci32_return(pf, PSCI_RET_NOT_SUPPORTED); @@ -168,41 +307,190 @@ static int psci_handler(struct smc_call *smc) return 0; } -void psci_init(void) +static void psci_link_cpu_info(void *arg) { - struct cpu_info *ci; + struct psci_node *e = node_self(); + + if (e == NULL) { + printk(BIOS_ERR, "No PSCI node for MPIDR %llx.\n", + cpu_info()->mpidr); + return; + } + + e->cpu_state.ci = cpu_info(); +} + +static int psci_init_node(struct psci_node *e, + struct psci_node *parent, + int level, uint64_t mpidr) +{ + size_t i; + uint64_t mpidr_inc; + struct psci_node_group *ng; + size_t num_children; + + memset(e, 0, sizeof(*e)); + e->mpidr = mpidr; + psci_set_state_locked(e, PSCI_STATE_OFF); + e->parent = parent; + e->level = level; + + if (level == PSCI_AFFINITY_LEVEL_0) + return 0; + + num_children = soc_psci_ops.children_at_level(level, mpidr); + + if (num_children == 0) + return 0; + + ng = &e->children; + ng->num = num_children; + ng->nodes = malloc(ng->num * sizeof(struct psci_node)); + if (ng->nodes == NULL) { + printk(BIOS_DEBUG, "PSCI: Allocation failure at level %d\n", + level); + return -1; + } + + /* Switch to next level below. */ + level = psci_level_below(level); + mpidr_inc = mpidr_mask(!!(level == PSCI_AFFINITY_LEVEL_3), + !!(level == PSCI_AFFINITY_LEVEL_2), + !!(level == PSCI_AFFINITY_LEVEL_1), + !!(level == PSCI_AFFINITY_LEVEL_0)); + + for (i = 0; i < ng->num; i++) { + struct psci_node *c = &ng->nodes[i]; + + /* Recursively initialize the nodes. */ + if (psci_init_node(c, e, level, mpidr)) + return -1; + mpidr += mpidr_inc; + } + + return 0; +} + +static size_t psci_count_children(struct psci_node *e) +{ + size_t i; + size_t count; + + if (e->level == PSCI_AFFINITY_LEVEL_0) + return 0; + + count = e->children.num; + for (i = 0; i < e->children.num; i++) + count += psci_count_children(&e->children.nodes[i]); + + return count; +} + +static size_t psci_write_nodes(struct psci_node *e, size_t index) +{ + size_t i; + + /* + * Recursively save node pointers in array. Node pointers are + * ordered in ascending mpidr and descending level within same mpidr. + * i.e. each node is saved in depth-first order of the tree. + */ + if (e->level != PSCI_AFFINITY_ROOT) { + psci_nodes[index] = e; + index++; + } + + if (e->level == PSCI_AFFINITY_LEVEL_0) + return index; + + for (i = 0; i < e->children.num; i++) + index = psci_write_nodes(&e->children.nodes[i], index); + + return index; +} + +static int psci_allocate_nodes(void) +{ + int level; + size_t num_children; uint64_t mpidr; + struct psci_node *e; - /* Set this CPUs MPIDR clearing the bits that are not per-cpu. */ - ci = cpu_info(); - mpidr = raw_read_mpidr_el1(); - mpidr &= ~(1ULL << 31); /* RES1 */ - mpidr &= ~(1ULL << 30); /* U */ - mpidr &= ~(1ULL << 24); /* MT */ - psci_state[ci->id].mpidr = mpidr; + mpidr = 0; + level = PSCI_AFFINITY_ROOT; - if (!cpu_is_bsp()) - return; + /* Find where the root should start. */ + while (psci_level_below(level) >= PSCI_AFFINITY_LEVEL_0) { + num_children = soc_psci_ops.children_at_level(level, mpidr); - /* Register PSCI handlers. */ - if (smc_register_range(PSCI_CPU_OFF64, PSCI_CPU_ON64, &psci_handler)) - printk(BIOS_ERR, "Couldn't register PSCI handler.\n"); + if (num_children == 0) { + printk(BIOS_ERR, "PSCI: No children at level %d!\n", + level); + return -1; + } + + /* The root starts where the affinity levels branch. */ + if (num_children > 1) + break; + + level = psci_level_below(level); + } + + if (psci_init_node(&psci_root, NULL, level, mpidr)) { + printk(BIOS_ERR, "PSCI init node failure.\n"); + return -1; + } + + num_children = psci_count_children(&psci_root); + /* Count the root node if isn't a fake node. */ + if (psci_root.level != PSCI_AFFINITY_ROOT) + num_children++; + + psci_nodes = malloc(num_children * sizeof(void *)); + psci_num_nodes = num_children; + + if (psci_nodes == NULL) { + printk(BIOS_ERR, "PSCI node pointer array failure.\n"); + return -1; + } + + num_children = psci_write_nodes(&psci_root, 0); + if (num_children != psci_num_nodes) { + printk(BIOS_ERR, "Wrong nodes written: %zd vs %zd.\n", + num_children, psci_num_nodes); + return -1; + } + + /* + * By default all nodes are set to PSCI_STATE_OFF. In order not + * to race with other CPUs turning themselves off set the BSPs + * affinity node to ON. + */ + e = node_self(); + if (e == NULL) { + printk(BIOS_ERR, "No PSCI node for BSP.\n"); + return -1; + } + psci_set_state_locked(e, PSCI_STATE_ON); + + return 0; } -void psci_turn_on_self(void *entry, void *arg) +void psci_init(void) { - struct exc_state state; - int target_el; - struct cpu_info *ci = cpu_info(); + struct cpu_action action = { + .run = &psci_link_cpu_info, + }; - psci_lock(); - psci_cpu_set_state_locked(ci->id, PSCI_CPU_STATE_ON); - psci_unlock(); + if (psci_allocate_nodes()) { + printk(BIOS_ERR, "PSCI support not enabled.\n"); + return; + } - /* Target EL is determined if HVC is enabled or not. */ - target_el = (raw_read_scr_el3() & SCR_HVC_ENABLE) ? EL2 : EL1; + if (arch_run_on_all_cpus_async(&action)) + printk(BIOS_ERR, "Error linking cpu_info to PSCI nodes.\n"); - memset(&state, 0, sizeof(state)); - state.elx.spsr = get_eret_el(target_el, SPSR_USE_H); - transition_with_entry(entry, arg, &state); + /* Register PSCI handlers. */ + if (smc_register_range(PSCI_CPU_OFF64, PSCI_CPU_ON64, &psci_handler)) + printk(BIOS_ERR, "Couldn't register PSCI handler.\n"); } diff --git a/src/arch/arm64/armv8/secmon/secmon_init.c b/src/arch/arm64/armv8/secmon/secmon_init.c index 56ed4f14bc..9097a08b36 100644 --- a/src/arch/arm64/armv8/secmon/secmon_init.c +++ b/src/arch/arm64/armv8/secmon/secmon_init.c @@ -30,6 +30,18 @@ #include <stddef.h> #include "secmon.h" +/* Save initial secmon params per CPU to handle turn up. */ +static struct secmon_params *init_params[CONFIG_MAX_CPUS]; + +static void start_up_cpu(void *arg) +{ + struct secmon_params *params = init_params[cpu_info()->id]; + + if (params == NULL) + psci_turn_off_self(); + psci_turn_on_self(params->entry, params->arg); +} + static void cpu_init(int bsp) { struct cpu_info *ci = cpu_info(); @@ -43,17 +55,26 @@ static void cpu_init(int bsp) static void secmon_init(struct secmon_params *params, int bsp) { + struct cpu_action action = { + .run = start_up_cpu, + }; + exception_hwinit(); cpu_init(bsp); + init_params[cpu_info()->id] = params; + + if (!cpu_is_bsp()) + secmon_wait_for_action(); + smc_init(); psci_init(); - /* Turn on CPU if params are not NULL. */ - if (params != NULL) - psci_turn_on_self(params->entry, params->arg); + arch_run_on_all_cpus_async(&action); - secmon_wait_for_action(); + printk(BIOS_ERR, "CPU turn on failed for BSP.\n"); + while (1) + ; } void secmon_wait_for_action(void) diff --git a/src/arch/arm64/armv8/secmon/smc.c b/src/arch/arm64/armv8/secmon/smc.c index 8aa6d41e7d..ae0f130179 100644 --- a/src/arch/arm64/armv8/secmon/smc.c +++ b/src/arch/arm64/armv8/secmon/smc.c @@ -140,7 +140,7 @@ static struct exception_handler smc_handler32 = { .handler = &smc_handler, }; -void smc_init(void) +static void enable_smc(void *arg) { uint32_t scr; @@ -149,9 +149,15 @@ void smc_init(void) scr &= ~(SCR_SMC_MASK); scr |= SCR_SMC_ENABLE; raw_write_scr_el3(scr); +} + +void smc_init(void) +{ + struct cpu_action action = { + .run = enable_smc, + }; - if (!cpu_is_bsp()) - return; + arch_run_on_all_cpus_async(&action); /* Register SMC handlers. */ exception_handler_register(EXC_VID_LOW64_SYNC, &smc_handler64); diff --git a/src/arch/arm64/include/arch/psci.h b/src/arch/arm64/include/arch/psci.h index c39f13a088..afa4c4209c 100644 --- a/src/arch/arm64/include/arch/psci.h +++ b/src/arch/arm64/include/arch/psci.h @@ -20,6 +20,8 @@ #ifndef __ARCH_PSCI_H__ #define __ARCH_PSCI_H__ +#include <stdint.h> +#include <arch/cpu.h> #include <arch/smc.h> /* Return Values */ @@ -35,6 +37,85 @@ enum { PSCI_RET_DISABLED = -8, }; +/* Generic PSCI state. */ +enum { + PSCI_STATE_OFF = 0, + PSCI_STATE_ON_PENDING, + PSCI_STATE_ON, +}; + +/* Affinity level support. */ +enum { + PSCI_AFFINITY_LEVEL_0, + PSCI_AFFINITY_LEVEL_1, + PSCI_AFFINITY_LEVEL_2, + PSCI_AFFINITY_LEVEL_3, + PSCI_AFFINITY_ROOT, + PSCI_AFFINITY_LEVEL_HIGHEST = PSCI_AFFINITY_ROOT, +}; + +static inline int psci_level_below(int level) +{ + return level - 1; +} + +struct psci_node; + +struct psci_cpu_state { + struct cpu_info *ci; + void *entry; + void *arg; + /* Ancestor of target to update state in CPU_ON case. */ + struct psci_node *ancestor; +}; + +struct psci_node_group { + size_t num; + struct psci_node *nodes; +}; + +struct psci_node { + uint64_t mpidr; + /* Affinity level of node. */ + int level; + /* Generic power state of this entity. */ + int state; + /* The SoC can stash its own state accounting in here. */ + int soc_state; + /* Parent of curernt entity. */ + struct psci_node *parent; + /* + * CPUs are leaves in the tree. They don't have children. The + * CPU-specific bits of storage can be shared with the children + * storage. + */ + union { + struct psci_node_group children; + struct psci_cpu_state cpu_state; + }; +}; + +static inline struct psci_node *psci_node_parent(const struct psci_node *n) +{ + return n->parent; +} + +static inline int psci_root_node(const struct psci_node *n) +{ + return psci_node_parent(n) == NULL; +} + +struct psci_soc_ops { + /* + * Return number of entities one level below given parent affinitly + * level and mpidr. + */ + size_t (*children_at_level)(int parent_level, uint64_t mpidr); +}; + +/* Each SoC needs to provide the functions in the psci_soc_ops structure. */ +extern struct psci_soc_ops soc_psci_ops; + /* PSCI Functions. */ enum { /* 32-bit System level functions. */ @@ -111,5 +192,6 @@ void psci_init(void); /* Turn on the current CPU within the PSCI subsystem. */ void psci_turn_on_self(void *entry, void *arg); +int psci_turn_off_self(void); #endif /* __ARCH_PSCI_H__ */ diff --git a/src/soc/nvidia/tegra132/Makefile.inc b/src/soc/nvidia/tegra132/Makefile.inc index 5cdf8e2003..117f629b46 100644 --- a/src/soc/nvidia/tegra132/Makefile.inc +++ b/src/soc/nvidia/tegra132/Makefile.inc @@ -88,6 +88,7 @@ ramstage-y += ../tegra/usb.c ramstage-$(CONFIG_ARCH_USE_SECURE_MONITOR) += secmon.c secmon-$(CONFIG_ARCH_USE_SECURE_MONITOR) += cpu_lib.S +secmon-$(CONFIG_ARCH_USE_SECURE_MONITOR) += psci.c secmon-$(CONFIG_ARCH_USE_SECURE_MONITOR) += uart.c modules_arm-y += monotonic_timer.c diff --git a/src/soc/nvidia/tegra132/psci.c b/src/soc/nvidia/tegra132/psci.c new file mode 100644 index 0000000000..b039dfbe6f --- /dev/null +++ b/src/soc/nvidia/tegra132/psci.c @@ -0,0 +1,46 @@ +/* + * This file is part of the coreboot project. + * + * Copyright 2014 Google Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA, 02110-1301 USA + */ + +#include <arch/psci.h> + +static size_t children_at_level(int parent_level, uint64_t mpidr) +{ + if (mpidr != 0) + return 0; + + /* T132 just has 2 cores. 0. Level 1 has 2 children at level 0. */ + switch (parent_level) { + case PSCI_AFFINITY_ROOT: + return 1; + case PSCI_AFFINITY_LEVEL_3: + return 1; + case PSCI_AFFINITY_LEVEL_2: + return 1; + case PSCI_AFFINITY_LEVEL_1: + return 2; + case PSCI_AFFINITY_LEVEL_0: + return 0; + default: + return 0; + } +} + +struct psci_soc_ops soc_psci_ops = { + .children_at_level = &children_at_level, +}; |