diff options
| -rw-r--r-- | src/arch/arm64/armv8/secmon/psci.c | 133 | ||||
| -rw-r--r-- | src/arch/arm64/include/arch/psci.h | 36 |
2 files changed, 166 insertions, 3 deletions
diff --git a/src/arch/arm64/armv8/secmon/psci.c b/src/arch/arm64/armv8/secmon/psci.c index 70251b2b06..f57a3e98f3 100644 --- a/src/arch/arm64/armv8/secmon/psci.c +++ b/src/arch/arm64/armv8/secmon/psci.c @@ -207,6 +207,34 @@ static int psci_schedule_cpu_on(struct psci_node *e) return PSCI_RET_SUCCESS; } +static void psci_cpu_resume_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.resume = *a; + ancestor = psci_find_ancestor(e, PSCI_AFFINITY_LEVEL_HIGHEST, state); + e->cpu_state.ancestor = ancestor; + cmd->ancestor = ancestor; +} + +static void psci_schedule_cpu_resume(struct psci_node *e) +{ + struct cpu_info *ci; + struct cpu_action *action; + + if (e->cpu_state.resume.run == NULL) + return; + + ci = e->cpu_state.ci; + action = &e->cpu_state.resume; + + arch_run_on_cpu(ci->id, action); +} + void psci_turn_on_self(const struct cpu_action *action) { struct psci_node *e = node_self(); @@ -233,13 +261,111 @@ void psci_turn_on_self(const struct cpu_action *action) void psci_cpu_entry(void) { gic_enable(); + /* - * Just wait for an action to be performed. Only CPU_ON is supported - * initially. i.e. no power down then wake. + * Just wait for an action to be performed. */ + psci_schedule_cpu_resume(node_self()); secmon_wait_for_action(); } +static void psci_cpu_resume(void *arg) +{ + uint64_t power_state = (uint64_t)arg; + struct psci_node *e; + struct psci_power_state state; + struct psci_cmd cmd = { + .type = PSCI_CMD_RESUME, + }; + + psci_power_state_unpack(power_state, &state); + + psci_lock(); + + e = node_self(); + /* clear the resume action after resume */ + e->cpu_state.resume.run = NULL; + e->cpu_state.resume.arg = NULL; + + cmd.target = e; + cmd.state = &state; + soc_psci_ops.cmd_prepare(&cmd); + + psci_unlock(); + + soc_psci_ops.cmd_commit(&cmd); + + psci_lock(); + psci_set_hierarchy_state(e, e->cpu_state.ancestor, PSCI_STATE_ON); + psci_unlock(); + + psci_schedule_cpu_on(node_self()); +} + +static void psci_cpu_suspend(struct psci_func *pf) +{ + uint64_t power_state; + uint64_t entry; + uint64_t context_id; + struct psci_node *e; + struct psci_power_state state; + struct cpu_action action; + struct cpu_action resume_action; + struct psci_cmd cmd = { + .type = PSCI_CMD_SUSPEND, + }; + int ret; + + power_state = psci64_arg(pf, PSCI_PARAM_0); + entry = psci64_arg(pf, PSCI_PARAM_1); + context_id = psci64_arg(pf, PSCI_PARAM_2); + psci_power_state_unpack(power_state, &state); + + psci_lock(); + + e = node_self(); + cmd.target = e; + cmd.state = &state; + action.run = (void *)entry; + action.arg = (void *)context_id; + resume_action.run = &psci_cpu_resume; + resume_action.arg = (void*)power_state; + + psci_cpu_on_prepare(&cmd, &action); + psci_cpu_resume_prepare(&cmd, &resume_action); + + ret = soc_psci_ops.cmd_prepare(&cmd); + + if (ret == PSCI_RET_SUCCESS) + psci_set_hierarchy_state(e, cmd.ancestor, PSCI_STATE_OFF); + + psci_unlock(); + + if (ret != PSCI_RET_SUCCESS) + return psci32_return(pf, ret); + + gic_disable(); + + ret = soc_psci_ops.cmd_commit(&cmd); + + /* PSCI_POWER_STATE_TYPE_STANDBY mode only */ + + psci_lock(); + resume_action.run = NULL; + resume_action.arg = NULL; + psci_cpu_resume_prepare(&cmd, &resume_action); + psci_unlock(); + + if (ret != PSCI_RET_SUCCESS) + return psci32_return(pf, ret); + + psci_lock(); + psci_set_hierarchy_state(e, e->cpu_state.ancestor, PSCI_STATE_ON); + psci_unlock(); + + psci32_return(pf, PSCI_RET_SUCCESS); +} + static void psci_cpu_on(struct psci_func *pf) { uint64_t entry; @@ -369,6 +495,9 @@ static int psci_handler(struct smc_call *smc) psci_func_init(pf, smc); switch (pf->id) { + case PSCI_CPU_SUSPEND64: + psci_cpu_suspend(pf); + break; case PSCI_CPU_ON64: psci_cpu_on(pf); break; diff --git a/src/arch/arm64/include/arch/psci.h b/src/arch/arm64/include/arch/psci.h index 47c902828c..1c28dc45da 100644 --- a/src/arch/arm64/include/arch/psci.h +++ b/src/arch/arm64/include/arch/psci.h @@ -24,6 +24,25 @@ #include <arch/cpu.h> #include <arch/smc.h> +/* PSCI v0.2 power state encoding for CPU_SUSPEND function */ +#define PSCI_0_2_POWER_STATE_ID_MASK 0xffff +#define PSCI_0_2_POWER_STATE_ID_SHIFT 0 +#define PSCI_0_2_POWER_STATE_TYPE_SHIFT 16 +#define PSCI_0_2_POWER_STATE_TYPE_MASK \ + (0x1 << PSCI_0_2_POWER_STATE_TYPE_SHIFT) +#define PSCI_0_2_POWER_STATE_AFFL_SHIFT 24 +#define PSCI_0_2_POWER_STATE_AFFL_MASK \ + (0x3 << PSCI_0_2_POWER_STATE_AFFL_SHIFT) + +#define PSCI_POWER_STATE_TYPE_STANDBY 0 +#define PSCI_POWER_STATE_TYPE_POWER_DOWN 1 + +struct psci_power_state { + u16 id; + u8 type; + u8 affinity_level; +}; + /* Return Values */ enum { PSCI_RET_SUCCESS = 0, @@ -64,6 +83,7 @@ struct psci_node; struct psci_cpu_state { struct cpu_info *ci; struct cpu_action startup; + struct cpu_action resume; /* Ancestor of target to update state in CPU_ON case. */ struct psci_node *ancestor; }; @@ -107,7 +127,8 @@ static inline int psci_root_node(const struct psci_node *n) enum { PSCI_CMD_ON, PSCI_CMD_OFF, - PSCI_CMD_STANDBY, + PSCI_CMD_SUSPEND, + PSCI_CMD_RESUME, }; /* @@ -127,6 +148,7 @@ struct psci_cmd { * A value of -1 indicates a CPU_OFF request. */ int state_id; + struct psci_power_state *state; /* * target is the command's target, but it can affect up to the * ancestor entity. If target == ancestor then it only affects @@ -184,6 +206,18 @@ struct psci_func { struct smc_call *smc; }; +static inline void psci_power_state_unpack(uint32_t power_state, + struct psci_power_state *state) +{ + state->id = (power_state & PSCI_0_2_POWER_STATE_ID_MASK) >> + PSCI_0_2_POWER_STATE_ID_SHIFT; + state->type = (power_state & PSCI_0_2_POWER_STATE_TYPE_MASK) >> + PSCI_0_2_POWER_STATE_TYPE_SHIFT; + state->affinity_level = + (power_state & PSCI_0_2_POWER_STATE_AFFL_MASK) >> + PSCI_0_2_POWER_STATE_AFFL_SHIFT; +} + static inline void psci_func_init(struct psci_func *pf, struct smc_call *smc) { pf->id = smc_function_id(smc); |