[PATCH kvm-unit-tests 1/3] arm/arm64: smp: rename secondary_halt to do_idle

[Andrew Jones <drjones@xxxxxxxxxx>]

Also prepare the newly renamed function to come out of idle
and use sev/wfe rather than a busy wait in smp_run().

Signed-off-by: Andrew Jones <drjones@xxxxxxxxxx>
---
 arm/cstart.S      |  2 +-
 arm/cstart64.S    |  2 +-
 lib/arm/asm/smp.h | 12 +++++++++++-
 lib/arm/smp.c     | 25 ++++++++++++++++---------
 4 files changed, 29 insertions(+), 12 deletions(-)

diff --git a/arm/cstart.S b/arm/cstart.S
index b3176b274933..86d879ec7b35 100644
--- a/arm/cstart.S
+++ b/arm/cstart.S
@@ -119,7 +119,7 @@ secondary_entry:
 
 	/* r0 is now the entry function, run it */
 	blx	r0
-	b	secondary_halt
+	b	do_idle
 
 .globl halt
 halt:
diff --git a/arm/cstart64.S b/arm/cstart64.S
index 30f732f1e99b..0445e133bb62 100644
--- a/arm/cstart64.S
+++ b/arm/cstart64.S
@@ -80,7 +80,7 @@ secondary_entry:
 
 	/* x0 is now the entry function, run it */
 	blr	x0
-	b	secondary_halt
+	b	do_idle
 
 .globl halt
 halt:
diff --git a/lib/arm/asm/smp.h b/lib/arm/asm/smp.h
index a3551a40e4c5..a352d76e710d 100644
--- a/lib/arm/asm/smp.h
+++ b/lib/arm/asm/smp.h
@@ -11,12 +11,14 @@
 #define smp_processor_id()		(current_thread_info()->cpu)
 
 extern void halt(void);
+extern void do_idle(void);
 
 extern cpumask_t cpu_present_mask;
 extern cpumask_t cpu_online_mask;
-extern cpumask_t cpu_halted_mask;
+extern cpumask_t cpu_idle_mask;
 #define cpu_present(cpu)		cpumask_test_cpu(cpu, &cpu_present_mask)
 #define cpu_online(cpu)			cpumask_test_cpu(cpu, &cpu_online_mask)
+#define cpu_idle(cpu)			cpumask_test_cpu(cpu, &cpu_idle_mask)
 #define for_each_present_cpu(cpu)	for_each_cpu(cpu, &cpu_present_mask)
 #define for_each_online_cpu(cpu)	for_each_cpu(cpu, &cpu_online_mask)
 
@@ -36,6 +38,14 @@ static inline void set_cpu_online(int cpu, bool online)
 		cpumask_clear_cpu(cpu, &cpu_online_mask);
 }
 
+static inline void set_cpu_idle(int cpu, bool idle)
+{
+	if (idle)
+		cpumask_set_cpu(cpu, &cpu_idle_mask);
+	else
+		cpumask_clear_cpu(cpu, &cpu_idle_mask);
+}
+
 typedef void (*secondary_entry_fn)(void);
 extern void smp_boot_secondary(int cpu, secondary_entry_fn entry);
 extern void smp_run(void (*func)(void));
diff --git a/lib/arm/smp.c b/lib/arm/smp.c
index fe64ed410377..3f8457cbf3a1 100644
--- a/lib/arm/smp.c
+++ b/lib/arm/smp.c
@@ -16,7 +16,7 @@
 
 cpumask_t cpu_present_mask;
 cpumask_t cpu_online_mask;
-cpumask_t cpu_halted_mask;
+cpumask_t cpu_idle_mask;
 
 struct secondary_data {
 	void *stack;            /* must be first member of struct */
@@ -68,12 +68,19 @@ void smp_boot_secondary(int cpu, secondary_entry_fn entry)
 	spin_unlock(&lock);
 }
 
-void secondary_halt(void)
+void do_idle(void)
 {
-	struct thread_info *ti = current_thread_info();
+	int cpu = smp_processor_id();
+
+	set_cpu_idle(cpu, true);
+	sev();
 
-	cpumask_set_cpu(ti->cpu, &cpu_halted_mask);
-	halt();
+	for (;;) {
+		while (cpu_idle(cpu))
+			wfe();
+		set_cpu_idle(cpu, true);
+		sev();
+	}
 }
 
 void smp_run(void (*func)(void))
@@ -87,8 +94,8 @@ void smp_run(void (*func)(void))
 	}
 	func();
 
-	cpumask_set_cpu(0, &cpu_halted_mask);
-	while (!cpumask_full(&cpu_halted_mask))
-		cpu_relax();
-	cpumask_clear_cpu(0, &cpu_halted_mask);
+	set_cpu_idle(0, true);
+	while (!cpumask_full(&cpu_idle_mask))
+		wfe();
+	set_cpu_idle(0, false);
 }
-- 
2.9.4