demo: implement run, halt cmds

1 files changed, 18 insertions, 8 deletions

diff --git a/demo/smp.c b/demo/smp.c
index f0f83e7..6f8e54a 100644
--- a/demo/smp.c
+++ b/demo/smp.c
@@ -15,12 +15,17 @@ void run_cpu(unsigned num)
 
 void run_cpus(unsigned mask)
 {
-	unsigned ctrl_word = 0;
-	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu)
-		if (mask & (1 << cpu)) {
+	unsigned ctrl_word = 0, ctrl_read = SMP_CTRL;
+	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu) {
+		if (!(mask & (1 << cpu)))
+			continue;
+
+		if (ctrl_read & (0b001 << (cpu * 8))) {
 			print("run cpu%u", cpu);
 			ctrl_word |= 0b001 << (cpu * 8);
-		}
+		} else
+			print("cpu%u already running", cpu);
+	}
 
 	SMP_CTRL = ctrl_word;
 }
@@ -32,12 +37,17 @@ void halt_cpu(unsigned num)
 
 void halt_cpus(unsigned mask)
 {
-	unsigned ctrl_word = 0;
-	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu)
-		if (mask & (1 << cpu)) {
+	unsigned ctrl_word = 0, ctrl_read = SMP_CTRL;
+	for (unsigned cpu = 0; cpu < NUM_CPUS; ++cpu) {
+		if (!(mask & (1 << cpu)))
+			continue;
+
+		if (!(ctrl_read & (0b001 << (cpu * 8)))) {
 			print("halt cpu%u%s", cpu, cpu == this_cpu->num ? " (myself)" : "");
 			ctrl_word |= 0b010 << (cpu * 8);
-		}
+		} else
+			print("cpu%u already halted", cpu);
+	}
 
 	SMP_CTRL = ctrl_word;
 }