3 files changed, 104 insertions, 0 deletions

diff --git a/demo/demo.h b/demo/demo.h
index 5cf41da..a8d81ce 100644
--- a/demo/demo.h
+++ b/demo/demo.h
@@ -35,6 +35,10 @@ void halt_cpus(unsigned mask);
 int strcmp(const char *s1, const char *s2);
 char *strtok_input(char **tokens);
 
+int parse_hex(char **tokens, unsigned *val);
+int parse_ptr(char **tokens, void **ptr);
+int parse_aligned(char **tokens, void **ptr);
 int parse_cpu_mask(char **tokens, unsigned *mask);
+int expect_end(char **tokens);
 
 #endif
diff --git a/demo/main.c b/demo/main.c
index efc183b..fe17ec2 100644
--- a/demo/main.c
+++ b/demo/main.c
@@ -22,6 +22,26 @@ static void cmd_halt(char **tokens)
 	halt_cpus(mask);
 }
 
+static void cmd_rd(char **tokens)
+{
+	void *ptr;
+	if (parse_aligned(tokens, &ptr) < 0 || expect_end(tokens) < 0)
+		return;
+
+	print("%p: %x", ptr, *(volatile unsigned *)ptr);
+}
+
+static void cmd_wr(char **tokens)
+{
+	void *ptr;
+	unsigned val;
+
+	if (parse_aligned(tokens, &ptr) < 0 || parse_hex(tokens, &val) < 0 || expect_end(tokens) < 0)
+		return;
+
+	*(volatile unsigned *)ptr = val;
+}
+
 void bsp_main(void)
 {
 	run_cpu(1);
@@ -43,6 +63,10 @@ void bsp_main(void)
 			cmd_run(&tokens);
 		else if (!strcmp(cmd, "halt"))
 			cmd_halt(&tokens);
+		else if (!strcmp(cmd, "rd"))
+			cmd_rd(&tokens);
+		else if (!strcmp(cmd, "wr"))
+			cmd_wr(&tokens);
 		else
 			print("unknown command '%s'", cmd);
 	}
diff --git a/demo/util.c b/demo/util.c
index 6bc1920..b0ef3cb 100644
--- a/demo/util.c
+++ b/demo/util.c
@@ -7,6 +7,11 @@ static void bad_input(void)
 	print("bad input");
 }
 
+static void unexpected_eof(void)
+{
+	print("unexpected end-of-input");
+}
+
 int strcmp(const char *s1, const char *s2)
 {
     while (*s1 && *s1 == *s2)
@@ -58,3 +63,74 @@ int parse_cpu_mask(char **tokens, unsigned *mask)
 
 	return 0;
 }
+
+int parse_hex(char **tokens, unsigned *val)
+{
+	char *token = strtok_input(tokens);
+	if (!token) {
+		unexpected_eof();
+		return -1;
+	} else if (token[0] != '0' || token[1] != 'x') {
+		bad_input();
+		return -1;
+	}
+
+	*val = 0;
+
+	char *c = &token[2];
+	unsigned nibbles = 0;
+
+	while (*c) {
+		*val <<= 4;
+
+		if ('0' <= *c && *c <= '9')
+			*val |= *c - '0';
+		else if ('a' <= *c && *c <= 'f')
+			*val |= *c - 'a' + 10;
+		else if ('A' <= *c && *c <= 'F')
+			*val |= *c - 'A' + 10;
+
+		++c;
+		++nibbles;
+	}
+
+	if (!nibbles || nibbles > 8) {
+		bad_input();
+		return -1;
+	}
+
+	return 0;
+}
+
+int parse_ptr(char **tokens, void **ptr)
+{
+	unsigned ptr_val;
+	if (parse_hex(tokens, &ptr_val) < 0)
+		return -1;
+
+	*ptr = (void *)ptr_val;
+	return 0;
+}
+
+int parse_aligned(char **tokens, void **ptr)
+{
+	if (parse_ptr(tokens, ptr) < 0)
+		return -1;
+	else if ((unsigned)*ptr & 0b11) {
+		print("unaligned address: %p", *ptr);
+		return -1;
+	}
+
+	return 0;
+}
+
+int expect_end(char **tokens)
+{
+	char *token = strtok_input(tokens);
+	if (token) {
+		print("too many arguments starting from '%s'", token);
+		return -1;
+	}
+
+	return 0;
+}