5 files changed, 23 insertions, 16 deletions
diff --git a/src/arch/arm64/armv8/secmon/secmon_init.c b/src/arch/arm64/armv8/secmon/secmon_init.c
index 9e104c759b..3405027f2f 100644
--- a/src/arch/arm64/armv8/secmon/secmon_init.c
+++ b/src/arch/arm64/armv8/secmon/secmon_init.c
@@ -58,22 +58,10 @@ static void cpu_init(int bsp)
 		cpu_set_bsp();
 }
 
-static void wait_for_all_cpus(void)
+static void wait_for_all_cpus(size_t expected)
 {
-	int all_online;
-
-	while (1) {
-		int i;
-
-		all_online = 1;
-		for (i = 0; i < CONFIG_MAX_CPUS; i++) {
-			if (!cpu_online(cpu_info_for_cpu(i)))
-				all_online = 0;
-		}
-
-		if (all_online)
-			break;
-	}
+	while (cpus_online() != expected)
+		;
 }
 
 static void secmon_init(struct secmon_params *params, int bsp)
@@ -90,7 +78,7 @@ static void secmon_init(struct secmon_params *params, int bsp)
 		secmon_wait_for_action();
 
 	/* Wait for all CPUs to enter secmon. */
-	wait_for_all_cpus();
+	wait_for_all_cpus(params->online_cpus);
 
 	smc_init();
 	psci_init();
diff --git a/src/arch/arm64/armv8/secmon_loader.c b/src/arch/arm64/armv8/secmon_loader.c
index 59a6ba7e6d..7a6e3ee738 100644
--- a/src/arch/arm64/armv8/secmon_loader.c
+++ b/src/arch/arm64/armv8/secmon_loader.c
@@ -116,6 +116,8 @@ static void fill_secmon_params(struct secmon_params *p,
 
 	memset(p, 0, sizeof(*p));
 
+	p->online_cpus = cpus_online();
+
 	spin_attrs = spintable_get_attributes();
 
 	if (spin_attrs != NULL) {
diff --git a/src/arch/arm64/cpu.c b/src/arch/arm64/cpu.c
index eb08735292..cd8dd3820a 100644
--- a/src/arch/arm64/cpu.c
+++ b/src/arch/arm64/cpu.c
@@ -32,6 +32,19 @@ struct cpu_info *cpu_info(void)
 	return cpu_info_for_cpu(smp_processor_id());
 }
 
+size_t cpus_online(void)
+{
+	int i;
+	size_t num = 0;
+
+	for (i = 0; i < ARRAY_SIZE(cpu_infos); i++) {
+		if (cpu_online(cpu_info_for_cpu(i)))
+			num++;
+	}
+
+	return num;
+}
+
 static inline int action_queue_empty(struct cpu_action_queue *q)
 {
 	return load_acquire_exclusive(&q->todo) == NULL;
diff --git a/src/arch/arm64/include/armv8/arch/cpu.h b/src/arch/arm64/include/armv8/arch/cpu.h
index d4ff907495..c6ca7d38c6 100644
--- a/src/arch/arm64/include/armv8/arch/cpu.h
+++ b/src/arch/arm64/include/armv8/arch/cpu.h
@@ -99,6 +99,9 @@ static inline void cpu_mark_online(struct cpu_info *ci)
 	store_release(&ci->online, 1);
 }
 
+/* Provide number of CPUs online. */
+size_t cpus_online(void);
+
 /* Control routines for starting CPUs. */
 struct cpu_control_ops {
 	/* Return the maximum number of CPUs supported. */
diff --git a/src/arch/arm64/include/armv8/arch/secmon.h b/src/arch/arm64/include/armv8/arch/secmon.h
index dc4b2e06ce..3d368a70e5 100644
--- a/src/arch/arm64/include/armv8/arch/secmon.h
+++ b/src/arch/arm64/include/armv8/arch/secmon.h
@@ -25,6 +25,7 @@
 #if IS_ENABLED(CONFIG_ARCH_USE_SECURE_MONITOR)
 
 struct secmon_params {
+	size_t online_cpus;
 	struct cpu_action bsp;
 	struct cpu_action secondary;
 };