demo: implement remote cmds

1 files changed, 62 insertions, 5 deletions

diff --git a/demo/main.c b/demo/main.c
index 89347ad..c5b7353 100644
--- a/demo/main.c
+++ b/demo/main.c
@@ -1,8 +1,7 @@
 #include "demo.h"
 
-static struct cpu cpu0, cpu1, cpu2, cpu3;
-
-struct cpu *__cpus[] = { &cpu0, &cpu1, &cpu2, &cpu3 };
+struct cpu all_cpus[NUM_CPUS];
+struct cpu *__cpus[] = { &all_cpus[0], &all_cpus[1], &all_cpus[2], &all_cpus[3] };
 
 static volatile unsigned boot_done;
 
@@ -35,7 +34,7 @@ static void cmd_read(char **tokens)
 	if (parse_aligned(tokens, &ptr) < 0 || expect_end(tokens) < 0)
 		return;
 
-	print("%p: %x", ptr, *(volatile unsigned *)ptr);
+	do_read(ptr);
 }
 
 static void cmd_write(char **tokens)
@@ -46,7 +45,7 @@ static void cmd_write(char **tokens)
 	if (parse_aligned(tokens, &ptr) < 0 || parse_hex(tokens, &val) < 0 || expect_end(tokens) < 0)
 		return;
 
-	*(volatile unsigned *)ptr = val;
+	do_write(ptr, val);
 }
 
 static void cmd_cache(char **tokens)
@@ -81,8 +80,54 @@ static void cmd_perf(char **tokens)
 		unknown_command(cmd);
 }
 
+static void cmd_remote_read(char **tokens, unsigned cpu)
+{
+	void *ptr;
+	if (parse_aligned(tokens, &ptr) < 0 || expect_end(tokens) < 0)
+		return;
+
+	remote_send(cpu, ptr, 0, 0);
+}
+
+static void cmd_remote_write(char **tokens, unsigned cpu)
+{
+	void *ptr;
+	unsigned val;
+
+	if (parse_aligned(tokens, &ptr) < 0 || parse_hex(tokens, &val) < 0 || expect_end(tokens) < 0)
+		return;
+
+	remote_send(cpu, ptr, 1, val);
+}
+
+static void cmd_remote(char **tokens)
+{
+	unsigned cpu;
+	const char *cmd;
+
+	if (parse_cpu(tokens, &cpu) < 0)
+		return;
+	else if (cpu == 0) {
+		print("cannot send remote cmd to bsp");
+		return;
+	} else if (!(cmd = strtok_input(tokens))) {
+		unexpected_eof();
+		return;
+	}
+
+	if (!strcmp(cmd, "read"))
+		cmd_remote_read(tokens, cpu);
+	else if (!strcmp(cmd, "write"))
+		cmd_remote_write(tokens, cpu);
+	else
+		unknown_command(cmd);
+}
+
 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);
 
@@ -111,6 +156,8 @@ static void bsp_main(void)
 			cmd_cache(&tokens);
 		else if (!strcmp(cmd, "perf"))
 			cmd_perf(&tokens);
+		else if (!strcmp(cmd, "remote"))
+			cmd_remote(&tokens);
 		else
 			unknown_command(cmd);
 	}
@@ -126,6 +173,16 @@ static void ap_main(void)
 	halt_cpu(this_cpu->num);
 
 	while (1) {
+		int write;
+		void *ptr;
+		unsigned val;
+
+		remote_recv(&ptr, &write, &val);
+
+		if (write)
+			do_write(ptr, val);
+		else
+			do_read(ptr);
 	}
 }