1 files changed, 75 insertions, 19 deletions

diff --git a/src/arch/arm64/armv8/secmon/secmon_init.c b/src/arch/arm64/armv8/secmon/secmon_init.c
index 3405027f2f..63bd4907d9 100644
--- a/src/arch/arm64/armv8/secmon/secmon_init.c
+++ b/src/arch/arm64/armv8/secmon/secmon_init.c
@@ -20,6 +20,7 @@
  */
 
 #include <arch/barrier.h>
+#include <arch/cache.h>
 #include <arch/io.h>
 #include <arch/exception.h>
 #include <arch/lib_helpers.h>
@@ -30,6 +31,74 @@
 #include <stddef.h>
 #include "secmon.h"
 
+/* Common CPU state for all CPUs running in secmon. */
+struct cpu_resume_data {
+	uint64_t mair;
+	uint64_t tcr;
+	uint64_t ttbr0;
+	uint64_t scr;
+};
+
+static struct cpu_resume_data resume_data;
+
+static void secmon_init(struct secmon_params *params, int bsp);
+
+static void secmon_init_bsp(void *arg)
+{
+	secmon_init(arg, 1);
+}
+
+static void secmon_init_nonbsp(void *arg)
+{
+	secmon_init(arg, 0);
+}
+
+/*
+ * This variable holds entry point for secmon init code. Once the stacks are
+ * setup by the stage_entry.S, it jumps to c_entry.
+ */
+void (*c_entry[2])(void *) = { &secmon_init_bsp, &secmon_init_nonbsp };
+
+static void cpu_resume(void *unused)
+{
+	uint32_t sctlr;
+
+	/* Re-enable exception vector. */
+	exception_hwinit();
+
+	tlbiall_el3();
+	raw_write_mair_el3(resume_data.mair);
+	raw_write_tcr_el3(resume_data.tcr);
+	raw_write_ttbr0_el3(resume_data.ttbr0);
+	dsb();
+	isb();
+
+	/* Enable MMU */
+	sctlr = raw_read_sctlr_el3();
+	sctlr |= SCTLR_C | SCTLR_M | SCTLR_I;
+	raw_write_sctlr_el3(sctlr);
+	isb();
+
+	raw_write_scr_el3(resume_data.scr);
+	isb();
+
+	psci_cpu_entry();
+}
+
+static void cpu_resume_init(void)
+{
+	/* Change entry points into secmon. */
+	c_entry[0] = c_entry[1] = cpu_resume;
+	dcache_clean_by_mva(&c_entry, sizeof(c_entry));
+
+	/* Back up state. */
+	resume_data.mair = raw_read_mair_el3();
+	resume_data.tcr = raw_read_tcr_el3();
+	resume_data.ttbr0 = raw_read_ttbr0_el3();
+	resume_data.scr = raw_read_scr_el3();
+	dcache_clean_by_mva(&resume_data, sizeof(resume_data));
+}
+
 static void start_up_cpu(void *arg)
 {
 	struct secmon_params *params = arg;
@@ -81,7 +150,10 @@ static void secmon_init(struct secmon_params *params, int bsp)
 	wait_for_all_cpus(params->online_cpus);
 
 	smc_init();
-	psci_init();
+	psci_init((uintptr_t)arm64_cpu_startup);
+
+	/* Initialize the resume path. */
+	cpu_resume_init();
 
 	/* Make sure all non-BSP CPUs take action before the BSP. */
 	arch_run_on_all_cpus_but_self_async(&action);
@@ -89,27 +161,11 @@ static void secmon_init(struct secmon_params *params, int bsp)
 	start_up_cpu(params);
 
 	printk(BIOS_ERR, "CPU turn on failed for BSP.\n");
-	while (1)
-		;
+
+	secmon_wait_for_action();
 }
 
 void secmon_wait_for_action(void)
 {
 	arch_cpu_wait_for_action();
 }
-
-static void secmon_init_bsp(void *arg)
-{
-	secmon_init(arg, 1);
-}
-
-static void secmon_init_nonbsp(void *arg)
-{
-	secmon_init(arg, 0);
-}
-
-/*
- * This variable holds entry point for secmon init code. Once the stacks are
- * setup by the stage_entry.S, it jumps to c_entry.
- */
-void (*c_entry[2])(void*) = { &secmon_init_bsp, &secmon_init_nonbsp };