2 files changed, 101 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)
diff --git a/src/arch/arm64/include/arch/psci.h b/src/arch/arm64/include/arch/psci.h
index 555333a44d..b408f3d28c 100644
--- a/src/arch/arm64/include/arch/psci.h
+++ b/src/arch/arm64/include/arch/psci.h
@@ -104,12 +104,47 @@ static inline int psci_root_node(const struct psci_node *n)
 	return psci_node_parent(n) == NULL;
 }
 
+enum {
+	PSCI_CMD_ON,
+	PSCI_CMD_OFF,
+	PSCI_CMD_STANDBY,
+};
+
+/*
+ * PSCI actions are serialized into a command for the SoC to process. There are
+ * 2 phases of a command being processed: prepare and commit. The prepare() is
+ * called with the PSCI locks held for the state of the PSCI nodes. If
+ * successful, the appropriate locks will be dropped and commit() will be
+ * called with the same structure. It is permissible for the SoC support code
+ * to modify the struture passed in (e.g. to update the requested state_id to
+ * reflect dynamic constraints on how deep of a state to enter).
+ */
+struct psci_cmd {
+	/* Command type. */
+	int type;
+	/*
+	 * PSCI state id for PSCI_CMD_OFF and PSCI_CMD_STANDBY commands.
+	 * A value of -1 indicates a CPU_OFF request.
+	 */
+	int state_id;
+	/*
+	 * target is the command's target, but it can affect up to the
+	 * ancestor entity. If target == ancestor then it only affects
+	 * target, otherwise all entites up the hierarchy including ancestor.
+	 */
+	struct psci_node *target;
+	struct psci_node *ancestor;
+};
+
 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);
+
+	int (*cmd_prepare)(struct psci_cmd *cmd);
+	int (*cmd_commit)(struct psci_cmd *cmd);
 };
 
 /* Each SoC needs to provide the functions in the psci_soc_ops structure. */