demo: implement run, halt cmds

4 files changed, 72 insertions, 9 deletions

diff --git a/demo/demo.h b/demo/demo.h
index 28677f6..5cf41da 100644
--- a/demo/demo.h
+++ b/demo/demo.h
@@ -35,4 +35,6 @@ void halt_cpus(unsigned mask);
 int strcmp(const char *s1, const char *s2);
 char *strtok_input(char **tokens);
 
+int parse_cpu_mask(char **tokens, unsigned *mask);
+
 #endif
diff --git a/demo/main.c b/demo/main.c
index f75f6f1..efc183b 100644
--- a/demo/main.c
+++ b/demo/main.c
@@ -4,6 +4,24 @@ static struct cpu cpu0, cpu1, cpu2, cpu3;
 
 struct cpu *__cpus[] = { &cpu0, &cpu1, &cpu2, &cpu3 };
 
+static void cmd_run(char **tokens)
+{
+	unsigned mask;
+	if (parse_cpu_mask(tokens, &mask) < 0)
+		return;
+
+	run_cpus(mask);
+}
+
+static void cmd_halt(char **tokens)
+{
+	unsigned mask;
+	if (parse_cpu_mask(tokens, &mask) < 0)
+		return;
+
+	halt_cpus(mask);
+}
+
 void bsp_main(void)
 {
 	run_cpu(1);
@@ -21,7 +39,12 @@ void bsp_main(void)
 		if (!cmd)
 			continue;
 
-		print("unknown command '%s'", cmd);
+		if (!strcmp(cmd, "run"))
+			cmd_run(&tokens);
+		else if (!strcmp(cmd, "halt"))
+			cmd_halt(&tokens);
+		else
+			print("unknown command '%s'", cmd);
 	}
 }
 
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;
 }
diff --git a/demo/util.c b/demo/util.c
index 99566bd..6bc1920 100644
--- a/demo/util.c
+++ b/demo/util.c
@@ -2,6 +2,11 @@
 
 #include "demo.h"
 
+static void bad_input(void)
+{
+	print("bad input");
+}
+
 int strcmp(const char *s1, const char *s2)
 {
     while (*s1 && *s1 == *s2)
@@ -30,3 +35,26 @@ char *strtok_input(char **tokens)
 
 	return start;
 }
+
+int parse_cpu_mask(char **tokens, unsigned *mask)
+{
+	*mask = 0;
+
+	char *token;
+	while ((token = strtok_input(tokens))) {
+		if (token[0] != 'c' || token[1] != 'p' || token[2] != 'u'
+		|| !('0' <= token[3] && token[3] < '0' + NUM_CPUS) || token[4]) {
+			bad_input();
+			return -1;
+		}
+
+		*mask |= 1 << (token[3] - '0');
+	}
+
+	if (!*mask) {
+		print("must specify at least one cpu");
+		return -1;
+	}
+
+	return 0;
+}