demo: implement remote cmds

4 files changed, 126 insertions, 10 deletions

diff --git a/demo/lock.S b/demo/atomic.S
index 51805f2..991ad5a 100644
--- a/demo/lock.S
+++ b/demo/atomic.S
@@ -28,3 +28,23 @@ spin_unlock:
 	str     r2, [r0]
 	msr     cpsr_c, r1
 	mov     pc, lr
+
+.global compare_exchange_64
+compare_exchange_64:
+	push    {r4, r5, r6, r7, r8}
+	ldr     r4, [r1]
+	ldr     r5, [r1, #4]
+	add     r8, r0, #4
+	ldrex   r6, [r0]
+	ldrex   r7, [r8]
+	teq     r4, r6
+	teqeq   r5, r7
+	strexeq r4, r2, [r0]
+	strexeq r4, r3, [r8]
+	teqeq   r4, #0
+	moveq   r0, #1
+	movne   r0, #0
+	strne   r6, [r1]
+	strne   r7, [r1, #4]
+	pop     {r4, r5, r6, r7, r8}
+	mov     pc, lr
diff --git a/demo/demo.h b/demo/demo.h
index c889e1a..5ffd4cd 100644
--- a/demo/demo.h
+++ b/demo/demo.h
@@ -3,15 +3,16 @@
 
 #define NUM_CPUS 4
 
-struct lock
+struct __attribute__((aligned(16))) lock
 {
 	volatile unsigned val;
 };
 
-struct cpu
+extern struct __attribute__((aligned(16))) cpu
 {
-	unsigned num;
-};
+	         unsigned           num;
+	volatile unsigned long long mailbox;
+} all_cpus[NUM_CPUS];
 
 /* Esto viola la ABI, pero no importa porque no dependemos de bibliotecas
  * https://gcc.gnu.org/onlinedocs/gcc/Global-Register-Variables.html
@@ -24,7 +25,7 @@ void spin_unlock(struct lock *lock, unsigned irq_save);
 
 void console_init(void);
 void print(const char *fmt, ...);
-void read_line(char *vuf, unsigned size);
+void read_line(char *buf, unsigned size);
 
 void run_cpu(unsigned num);
 void run_cpus(unsigned mask);
@@ -49,4 +50,11 @@ void cache_debug(unsigned cpu, void *ptr);
 void perf_show(unsigned cpu);
 void perf_clear(unsigned cpu);
 
+void do_read(void *ptr);
+void do_write(void *ptr, unsigned val);
+void remote_send(unsigned cpu, void *ptr, int write, unsigned val);
+void remote_recv(void **ptr, int *write, unsigned *val);
+
+int compare_exchange_64(volatile unsigned long long *p, unsigned long long *old, unsigned long long val);
+
 #endif
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);
 	}
 }
 
diff --git a/demo/mem.c b/demo/mem.c
new file mode 100644
index 0000000..2551385
--- /dev/null
+++ b/demo/mem.c
@@ -0,0 +1,31 @@
+#include "demo.h"
+
+void do_read(void *ptr)
+{
+	print("%p -> %x", ptr, *(volatile unsigned *)ptr);
+}
+
+void do_write(void *ptr, unsigned val)
+{
+	*(volatile unsigned *)ptr = val;
+	print("%p <- %x", ptr, val);
+}
+
+void remote_send(unsigned cpu, void *ptr, int write, unsigned val)
+{
+	unsigned long ptr_val = (unsigned)ptr & ~0b11;
+	unsigned long long send_word = ((unsigned long long)ptr_val | (write & 1) << 1 | 1) << 32 | val;
+
+	unsigned long long current = all_cpus[cpu].mailbox;
+	while (!compare_exchange_64(&all_cpus[cpu].mailbox, &current, send_word));
+}
+
+void remote_recv(void **ptr, int *write, unsigned *val)
+{
+	unsigned long long current = this_cpu->mailbox;
+	while (!compare_exchange_64(&this_cpu->mailbox, &current, 0) || !current);
+
+	*val = (unsigned long)current;
+	*ptr = (void *)((unsigned)((current >> 32) & ~0b11));
+	*write = (current >> 33) & 1;
+}