[PATCH kvm-unit-tests 3/6] lib/arm/smp: introduce smp_run

[Andrew Jones <drjones@redhat.com>]

A common pattern is
 - run a function on all cpus
 - signal each cpu's completion with a cpumask
 - halt the secondaries when they're complete
 - have the primary wait on the cpumask for all to complete

smp_run is a wrapper for that pattern. Also, we were allowing
secondaries to go off in the weeds if they returned from their
secondary entry function, which can be difficult to debug. A
nice side-effect of adding this wrapper is we don't do that
anymore, and can even know when a secondary has halted with the
new cpu_halted_mask.

Signed-off-by: Andrew Jones <drjones@redhat.com>
---
 arm/cstart.S      |  3 ++-
 arm/cstart64.S    |  3 ++-
 lib/arm/asm/smp.h |  2 ++
 lib/arm/smp.c     | 26 ++++++++++++++++++++++++++
 4 files changed, 32 insertions(+), 2 deletions(-)

diff --git a/arm/cstart.S b/arm/cstart.S
index 12461d104dad..b3176b274933 100644
--- a/arm/cstart.S
+++ b/arm/cstart.S
@@ -118,7 +118,8 @@ secondary_entry:
 	bl	secondary_cinit
 
 	/* r0 is now the entry function, run it */
-	mov	pc, r0
+	blx	r0
+	b	secondary_halt
 
 .globl halt
 halt:
diff --git a/arm/cstart64.S b/arm/cstart64.S
index 7738babc4109..30f732f1e99b 100644
--- a/arm/cstart64.S
+++ b/arm/cstart64.S
@@ -79,7 +79,8 @@ secondary_entry:
 	bl	secondary_cinit
 
 	/* x0 is now the entry function, run it */
-	br	x0
+	blr	x0
+	b	secondary_halt
 
 .globl halt
 halt:
diff --git a/lib/arm/asm/smp.h b/lib/arm/asm/smp.h
index 4cb86b6ce342..1d1cd7a29991 100644
--- a/lib/arm/asm/smp.h
+++ b/lib/arm/asm/smp.h
@@ -14,6 +14,7 @@ extern void halt(void);
 
 extern cpumask_t cpu_present_mask;
 extern cpumask_t cpu_online_mask;
+extern cpumask_t cpu_halted_mask;
 #define cpu_present(cpu)		cpumask_test_cpu(cpu, &cpu_present_mask)
 #define cpu_online(cpu)			cpumask_test_cpu(cpu, &cpu_online_mask)
 #define for_each_present_cpu(cpu)	for_each_cpu(cpu, &cpu_present_mask)
@@ -45,5 +46,6 @@ struct secondary_data {
 extern struct secondary_data secondary_data;
 
 extern void smp_boot_secondary(int cpu, secondary_entry_fn entry);
+extern void smp_run(void (*func)(void));
 
 #endif /* _ASMARM_SMP_H_ */
diff --git a/lib/arm/smp.c b/lib/arm/smp.c
index bbaf9e60e950..8528aa454108 100644
--- a/lib/arm/smp.c
+++ b/lib/arm/smp.c
@@ -15,6 +15,7 @@
 
 cpumask_t cpu_present_mask;
 cpumask_t cpu_online_mask;
+cpumask_t cpu_halted_mask;
 struct secondary_data secondary_data;
 
 secondary_entry_fn secondary_cinit(void)
@@ -53,3 +54,28 @@ void smp_boot_secondary(int cpu, secondary_entry_fn entry)
 	while (!cpu_online(cpu))
 		wfe();
 }
+
+void secondary_halt(void)
+{
+	struct thread_info *ti = current_thread_info();
+
+	cpumask_set_cpu(ti->cpu, &cpu_halted_mask);
+	halt();
+}
+
+void smp_run(void (*func)(void))
+{
+	int cpu;
+
+	for_each_present_cpu(cpu) {
+		if (cpu == 0)
+			continue;
+		smp_boot_secondary(cpu, func);
+	}
+	func();
+
+	cpumask_set_cpu(0, &cpu_halted_mask);
+	while (!cpumask_full(&cpu_halted_mask))
+		cpu_relax();
+	cpumask_clear_cpu(0, &cpu_halted_mask);
+}
-- 
2.9.4