rtl/smp: implement SMP dead/alive handling

3 files changed, 22 insertions, 6 deletions

diff --git a/demo/demo.h b/demo/demo.h
index 5ffd4cd..d8b9da6 100644
--- a/demo/demo.h
+++ b/demo/demo.h
@@ -27,6 +27,7 @@ void console_init(void);
 void print(const char *fmt, ...);
 void read_line(char *buf, unsigned size);
 
+int cpu_is_alive(unsigned num);
 void run_cpu(unsigned num);
 void run_cpus(unsigned mask);
 void halt_cpu(unsigned num);
diff --git a/demo/main.c b/demo/main.c
index c5b7353..d8f38d4 100644
--- a/demo/main.c
+++ b/demo/main.c
@@ -123,13 +123,27 @@ static void cmd_remote(char **tokens)
 		unknown_command(cmd);
 }
 
+static void kick_cpus(void)
+{
+	for (unsigned i = this_cpu->num + 1; i < NUM_CPUS; ++i) {
+		if (cpu_is_alive(i)) {
+			run_cpu(i);
+			return;
+		}
+
+		print("cpu%u is dead", i);
+	}
+
+	boot_done = 1;
+}
+
 static void bsp_main(void)
 {
 	for (struct cpu *cpu = all_cpus; cpu < all_cpus + NUM_CPUS; ++cpu)
 		cpu->mailbox = 0;
 
 	boot_done = 0;
-	run_cpu(1);
+	kick_cpus();
 
 	while (!boot_done);
 	print("booted %u cpus", NUM_CPUS);
@@ -165,11 +179,7 @@ static void bsp_main(void)
 
 static void ap_main(void)
 {
-	if (this_cpu->num < NUM_CPUS - 1)
-		run_cpu(this_cpu->num + 1);
-	else
-		boot_done = 1;
-
+	kick_cpus();
 	halt_cpu(this_cpu->num);
 
 	while (1) {
diff --git a/demo/smp.c b/demo/smp.c
index 4357d35..8bf220e 100644
--- a/demo/smp.c
+++ b/demo/smp.c
@@ -3,6 +3,11 @@
 #define SMP_CTRL_BASE 0x30140000
 #define SMP_CTRL      (*(volatile unsigned *)SMP_CTRL_BASE)
 
+int cpu_is_alive(unsigned num)
+{
+	return !!(SMP_CTRL & (0b100 << (num * 8)));
+}
+
 void run_cpu(unsigned num)
 {
 	run_cpus(1 << num);