1 files changed, 53 insertions, 16 deletions

diff --git a/src/arch/arm64/armv8/secmon_loader.c b/src/arch/arm64/armv8/secmon_loader.c
index 066f1c18a8..4d83764234 100644
--- a/src/arch/arm64/armv8/secmon_loader.c
+++ b/src/arch/arm64/armv8/secmon_loader.c
@@ -24,6 +24,7 @@
 
 #include <arch/lib_helpers.h>
 #include <arch/secmon.h>
+#include <arch/spintable.h>
 #include <console/console.h>
 #include <rmodule.h>
 #include <string.h>
@@ -75,10 +76,43 @@ static secmon_entry_t secmon_load_rmodule(void)
 	return rmodule_entry(&secmon_mod);
 }
 
-void secmon_run(void (*entry)(void *), void *cb_tables)
+struct secmon_runit {
+	secmon_entry_t entry;
+	struct secmon_params bsp_params;
+	struct secmon_params secondary_params;
+};
+
+static void secmon_start(void *arg)
 {
-	struct secmon_params params;
 	uint32_t scr;
+	struct secmon_params *p = NULL;
+	struct secmon_runit *r = arg;
+
+	if (cpu_is_bsp())
+		p = &r->bsp_params;
+	else if (r->secondary_params.entry != NULL)
+		p = &r->secondary_params;
+
+	printk(BIOS_DEBUG, "CPU%x entering secure monitor.\n", cpu_info()->id);
+
+	/* We want to enforce the following policies:
+	 * NS bit is set for lower EL
+	 */
+	scr = raw_read_scr_el3();
+	scr |= SCR_NS;
+	raw_write_scr_el3(scr);
+
+	r->entry(p);
+}
+
+void secmon_run(void (*entry)(void *), void *cb_tables)
+{
+	const struct spintable_attributes *spin_attrs;
+	static struct secmon_runit runit;
+	struct cpu_action action = {
+		.run = secmon_start,
+		.arg = &runit,
+	};
 
 	printk(BIOS_SPEW, "payload jump @ %p\n", entry);
 
@@ -87,25 +121,28 @@ void secmon_run(void (*entry)(void *), void *cb_tables)
 		return;
 	}
 
-	secmon_entry_t doit = secmon_load_rmodule();
+	runit.entry = secmon_load_rmodule();
 
-	if (doit == NULL)
+	if (runit.entry == NULL)
 		die("ARM64 Error: secmon load error");
 
 	printk(BIOS_DEBUG, "ARM64: Loaded the el3 monitor...jumping to %p\n",
-	       doit);
+	       runit.entry);
 
-	params.entry = entry;
-	params.arg = cb_tables;
-	params.elx_el = EL2;
-	params.elx_mode = SPSR_USE_L;
+	runit.bsp_params.entry = entry;
+	runit.bsp_params.arg = cb_tables;
+	runit.bsp_params.elx_el = EL2;
+	runit.bsp_params.elx_mode = SPSR_USE_L;
+	runit.secondary_params.elx_el = EL2;
+	runit.secondary_params.elx_mode = SPSR_USE_L;
 
-	/* We want to enforce the following policies:
-	 * NS bit is set for lower EL
-	 */
-	scr = raw_read_scr_el3();
-	scr |= SCR_NS;
-	raw_write_scr_el3(scr);
+	spin_attrs = spintable_get_attributes();
+
+	if (spin_attrs != NULL) {
+		runit.secondary_params.entry = spin_attrs->entry;
+		runit.secondary_params.arg = spin_attrs->addr;
+	}
 
-	doit(&params);
+	arch_run_on_all_cpus_but_self_async(&action);
+	secmon_start(&runit);
 }