1 files changed, 3 insertions, 37 deletions

diff --git a/src/arch/arm64/armv8/secmon/secmon_init.c b/src/arch/arm64/armv8/secmon/secmon_init.c
index 63bd4907d9..7d7f8bb811 100644
--- a/src/arch/arm64/armv8/secmon/secmon_init.c
+++ b/src/arch/arm64/armv8/secmon/secmon_init.c
@@ -27,20 +27,11 @@
 #include <arch/psci.h>
 #include <arch/secmon.h>
 #include <arch/smc.h>
+#include <arch/startup.h>
 #include <console/console.h>
 #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)
@@ -61,27 +52,6 @@ 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();
 }
 
@@ -92,11 +62,7 @@ static void cpu_resume_init(void)
 	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));
+	startup_save_cpu_data();
 }
 
 static void start_up_cpu(void *arg)
@@ -150,7 +116,7 @@ static void secmon_init(struct secmon_params *params, int bsp)
 	wait_for_all_cpus(params->online_cpus);
 
 	smc_init();
-	psci_init((uintptr_t)arm64_cpu_startup);
+	psci_init((uintptr_t)arm64_cpu_startup_resume);
 
 	/* Initialize the resume path. */
 	cpu_resume_init();