diff options
Diffstat (limited to 'src/arch/arm64/armv8/secmon')
-rw-r--r-- | src/arch/arm64/armv8/secmon/psci.c | 76 |
1 files changed, 66 insertions, 10 deletions
diff --git a/src/arch/arm64/armv8/secmon/psci.c b/src/arch/arm64/armv8/secmon/psci.c index 93c5bddcb9..3ba7e73e61 100644 --- a/src/arch/arm64/armv8/secmon/psci.c +++ b/src/arch/arm64/armv8/secmon/psci.c @@ -174,15 +174,18 @@ static void psci_cpu_on_callback(void *arg) e->cpu_state.startup.arg, &state); } -static void psci_cpu_on_prepare(struct psci_node *e, const struct cpu_action *a) +static void psci_cpu_on_prepare(struct psci_cmd *cmd, + const struct cpu_action *a) { struct psci_node *ancestor; + struct psci_node *e; int state = PSCI_STATE_ON_PENDING; + e = cmd->target; e->cpu_state.startup = *a; ancestor = psci_find_ancestor(e, PSCI_AFFINITY_LEVEL_HIGHEST, state); e->cpu_state.ancestor = ancestor; - psci_set_hierarchy_state(e, ancestor, state); + cmd->ancestor = ancestor; } static int psci_schedule_cpu_on(struct psci_node *e) @@ -206,6 +209,9 @@ static int psci_schedule_cpu_on(struct psci_node *e) void psci_turn_on_self(const struct cpu_action *action) { struct psci_node *e = node_self(); + struct psci_cmd cmd = { + .type = PSCI_CMD_ON, + }; if (e == NULL) { printk(BIOS_ERR, "Couldn't turn on self: mpidr %llx\n", @@ -213,8 +219,11 @@ void psci_turn_on_self(const struct cpu_action *action) return; } + cmd.target = e; + psci_lock(); - psci_cpu_on_prepare(e, action); + psci_cpu_on_prepare(&cmd, action); + psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_ON_PENDING); psci_unlock(); psci_schedule_cpu_on(e); @@ -235,8 +244,12 @@ static void psci_cpu_on(struct psci_func *pf) uint64_t target_mpidr; uint64_t context_id; int cpu_state; + int ret; struct psci_node *e; struct cpu_action action; + struct psci_cmd cmd = { + .type = PSCI_CMD_ON, + }; target_mpidr = psci64_arg(pf, PSCI_PARAM_0); entry = psci64_arg(pf, PSCI_PARAM_1); @@ -262,28 +275,71 @@ static void psci_cpu_on(struct psci_func *pf) return; } + cmd.target = e; action.run = (void *)entry; action.arg = (void *)context_id; - psci_cpu_on_prepare(e, &action); + psci_cpu_on_prepare(&cmd, &action); + + ret = soc_psci_ops.cmd_prepare(&cmd); + + if (ret == PSCI_RET_SUCCESS) + psci_set_hierarchy_state(e, cmd.ancestor, + PSCI_STATE_ON_PENDING); + psci_unlock(); + if (ret != PSCI_RET_SUCCESS) + return psci32_return(pf, ret); + + ret = soc_psci_ops.cmd_commit(&cmd); + + if (ret != PSCI_RET_SUCCESS) { + psci_lock(); + psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_OFF); + psci_unlock(); + return psci32_return(pf, ret); + } + psci32_return(pf, psci_schedule_cpu_on(e)); } static int psci_turn_off_node(struct psci_node *e, int level, int state_id) { - struct psci_node *ancestor; + int ret; + struct psci_cmd cmd = { + .type = PSCI_CMD_OFF, + .state_id = state_id, + .target = e, + }; psci_lock(); - ancestor = psci_find_ancestor(e, level, PSCI_STATE_OFF); - psci_set_hierarchy_state(e, ancestor, PSCI_STATE_OFF); + + cmd.ancestor = psci_find_ancestor(e, level, PSCI_STATE_OFF); + + ret = soc_psci_ops.cmd_prepare(&cmd); + + if (ret == PSCI_RET_SUCCESS) + psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_OFF); + psci_unlock(); - /* TODO(adurbin): writeback cache and actually turn off CPU. */ - secmon_trampoline(&secmon_wait_for_action, NULL); + if (ret != PSCI_RET_SUCCESS) + return ret; - return PSCI_RET_SUCCESS; + /* Should never return. */ + ret = soc_psci_ops.cmd_commit(&cmd); + + /* Adjust ret to be an error. */ + if (ret == PSCI_RET_SUCCESS) + ret = PSCI_RET_INTERNAL_FAILURE; + + /* Turn things back on. */ + psci_lock(); + psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_ON); + psci_unlock(); + + return ret; } int psci_turn_off_self(void) |