3 files changed, 43 insertions, 20 deletions

diff --git a/src/arch/arm64/armv8/secmon/Makefile.inc b/src/arch/arm64/armv8/secmon/Makefile.inc
index b140941e8b..5dadfa9a63 100644
--- a/src/arch/arm64/armv8/secmon/Makefile.inc
+++ b/src/arch/arm64/armv8/secmon/Makefile.inc
@@ -32,6 +32,8 @@ secmon-c-ccopts += -I$(src)/arch/arm64/include/armv8/ -include $(src)/include/kc
 secmon-S-ccopts += -I$(src)/arch/arm64/include/armv8/ -include $(src)/include/kconfig.h -D__SECMON__
 
 secmon-y += secmon_init.c
+secmon-y += ../exception.c
+secmon-y += ../../cpu.c
 secmon-y += ../../transition_asm.S ../../transition.c
 
 ramstage-srcs += $(SECMON_SRC)
diff --git a/src/arch/arm64/armv8/secmon/secmon_init.c b/src/arch/arm64/armv8/secmon/secmon_init.c
index e8bdd82d29..660d6d432b 100644
--- a/src/arch/arm64/armv8/secmon/secmon_init.c
+++ b/src/arch/arm64/armv8/secmon/secmon_init.c
@@ -21,6 +21,7 @@
 
 #include <arch/barrier.h>
 #include <arch/io.h>
+#include <arch/exception.h>
 #include <arch/lib_helpers.h>
 #include <arch/secmon.h>
 #include <arch/transition.h>
@@ -28,14 +29,15 @@
 #include <rmodule.h>
 #include <stddef.h>
 
-static void secmon_wait(void)
+static void cpu_init(int bsp)
 {
-	/*
-	 * TODO(furquan): This should be a point of no-return. Once we have PSCI
-	 * support we need to respond to kernel calls
-	 */
-	while (1)
-		wfe();
+	struct cpu_info *ci = cpu_info();
+
+	ci->id = smp_processor_id();
+	cpu_mark_online(ci);
+
+	if (bsp)
+		cpu_set_bsp();
 }
 
 static void secmon_el3_init(void)
@@ -52,12 +54,12 @@ static void secmon_el3_init(void)
 	isb();
 }
 
-static void secmon_init(void *arg)
+static void secmon_init(struct secmon_params *params, int bsp)
 {
 	struct exc_state exc_state;
-	struct secmon_params *params = arg;
 
-	printk(BIOS_DEBUG, "ARM64: secmon in %s\n", __func__);
+	exception_hwinit();
+	cpu_init(bsp);
 
 	secmon_el3_init();
 
@@ -66,18 +68,29 @@ static void secmon_init(void *arg)
 	 * 1) If yes, we make an EL2 transition to that entry point
 	 * 2) If no, we just wait
 	 */
-	if (params == NULL) {
-		secmon_wait();
+	if (params != NULL) {
+		memset(&exc_state, 0, sizeof(exc_state));
+		exc_state.elx.spsr =
+			get_eret_el(params->elx_el, params->elx_mode);
+
+		transition_with_entry(params->entry, params->arg, &exc_state);
 	}
 
-	memset(&exc_state, 0, sizeof(exc_state));
-	exc_state.elx.spsr = get_eret_el(params->elx_el, params->elx_mode);
+	arch_cpu_wait_for_action();
+}
 
-	transition_with_entry(params->entry, params->arg, &exc_state);
+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)(void*) = &secmon_init;
+void (*c_entry[2])(void*) = { &secmon_init_bsp, &secmon_init_nonbsp };
diff --git a/src/arch/arm64/armv8/secmon_loader.c b/src/arch/arm64/armv8/secmon_loader.c
index 4d83764234..e571b51a6a 100644
--- a/src/arch/arm64/armv8/secmon_loader.c
+++ b/src/arch/arm64/armv8/secmon_loader.c
@@ -25,6 +25,7 @@
 #include <arch/lib_helpers.h>
 #include <arch/secmon.h>
 #include <arch/spintable.h>
+#include <arch/stages.h>
 #include <console/console.h>
 #include <rmodule.h>
 #include <string.h>
@@ -85,15 +86,22 @@ struct secmon_runit {
 static void secmon_start(void *arg)
 {
 	uint32_t scr;
+	secmon_entry_t entry;
 	struct secmon_params *p = NULL;
 	struct secmon_runit *r = arg;
 
+	entry = r->entry;
+
 	if (cpu_is_bsp())
 		p = &r->bsp_params;
-	else if (r->secondary_params.entry != NULL)
-		p = &r->secondary_params;
+	else {
+		entry = secondary_entry_point(entry);
+		if (r->secondary_params.entry != NULL)
+			p = &r->secondary_params;
+	}
 
-	printk(BIOS_DEBUG, "CPU%x entering secure monitor.\n", cpu_info()->id);
+	printk(BIOS_DEBUG, "CPU%x entering secure monitor %p.\n",
+		cpu_info()->id, entry);
 
 	/* We want to enforce the following policies:
 	 * NS bit is set for lower EL
@@ -102,7 +110,7 @@ static void secmon_start(void *arg)
 	scr |= SCR_NS;
 	raw_write_scr_el3(scr);
 
-	r->entry(p);
+	entry(p);
 }
 
 void secmon_run(void (*entry)(void *), void *cb_tables)