arm64: psci: add cpu_suspend support

Implement the cpu_suspend for the PSCI service in secmon.

BRANCH=none
BUG=chrome-os-partner:39620
TEST=test with CPU idle driver that invoke the cpu_suspend of PSCI

Change-Id: I4cdfab88bf36bf432fb33c56c1ea114b384528f8
Signed-off-by: Patrick Georgi <pgeorgi@chromium.org>
Original-Commit-Id: 90b3ea3fcb21cb393e30a8359f0328054961f6d5
Original-Change-Id: Ieb76abc017b9c3e074cc018903cef72020306a8f
Original-Signed-off-by: Joseph Lo <josephl@nvidia.com>
Original-Reviewed-on: https://chromium-review.googlesource.com/269115
Original-Reviewed-by: Aaron Durbin <adurbin@chromium.org>
Reviewed-on: http://review.coreboot.org/10171
Tested-by: build bot (Jenkins)
Reviewed-by: Paul Menzel <paulepanter@users.sourceforge.net>
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>

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);