demo: implement gfx commands

4 files changed, 248 insertions, 1 deletions

diff --git a/demo/demo.h b/demo/demo.h
index d8b9da6..d5f6065 100644
--- a/demo/demo.h
+++ b/demo/demo.h
@@ -41,10 +41,12 @@ void unexpected_eof();
 
 int parse_hex(char **tokens, unsigned *val);
 int parse_ptr(char **tokens, void **ptr);
+int parse_fp16(char **tokens, short *ptr);
 int parse_aligned(char **tokens, void **ptr);
 
 int parse_cpu(char **tokens, unsigned *cpu);
 int parse_cpu_mask(char **tokens, unsigned *mask);
+int parse_lane(char **tokens, unsigned *lane);
 
 void cache_debug(unsigned cpu, void *ptr);
 
@@ -58,4 +60,11 @@ 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);
 
+void gfx_init(void);
+void gfx_draw(void);
+void gfx_swap(void);
+void gfx_clear(void);
+void gfx_bg(unsigned color);
+void gfx_data(unsigned block, unsigned lane, short data[static 4]);
+
 #endif
diff --git a/demo/gfx.c b/demo/gfx.c
new file mode 100644
index 0000000..a0128e6
--- /dev/null
+++ b/demo/gfx.c
@@ -0,0 +1,105 @@
+#include <stdarg.h>
+#include <stddef.h>
+
+#include "demo.h"
+#include "float16.h"
+
+#define GFX_VRAM_BASE 0x38000000
+#define GFX_CMD_BASE  0x3c000000
+
+#define GFX_CMD_SCAN        (*(volatile unsigned *)(GFX_CMD_BASE + 4))
+#define GFX_CMD_HEADER_BASE (*(volatile unsigned *)(GFX_CMD_BASE + 8))
+#define GFX_CMD_HEADER_SIZE (*(volatile unsigned *)(GFX_CMD_BASE + 12))
+#define GFX_CMD_FB_BASE_A   (*(volatile unsigned *)(GFX_CMD_BASE + 16))
+#define GFX_CMD_FB_BASE_B   (*(volatile unsigned *)(GFX_CMD_BASE + 20))
+
+#define FB_BASE_A   0x000000
+#define FB_BASE_B   0x200000
+#define HEADER_BASE 0x400000
+#define HEADER_SIZE (1 - 1)
+#define CODE_BASE   0x500000
+#define CODE_SIZE   (3 - 1)
+#define DATA_BASE   0x600000
+
+#define VRAM_HEADER_CODE_BASE ((unsigned *)(GFX_VRAM_BASE + HEADER_BASE + 0))
+#define VRAM_HEADER_CODE_SIZE ((unsigned *)(GFX_VRAM_BASE + HEADER_BASE + 4))
+#define VRAM_HEADER_DATA_BASE ((unsigned *)(GFX_VRAM_BASE + HEADER_BASE + 8))
+#define VRAM_HEADER_DATA_SIZE ((unsigned *)(GFX_VRAM_BASE + HEADER_BASE + 12))
+
+#define VRAM_CODE ((unsigned *)(GFX_VRAM_BASE + CODE_BASE))
+#define VRAM_DATA ((short *)(GFX_VRAM_BASE + DATA_BASE))
+
+static int swap_buffers;
+static unsigned clear_color;
+static unsigned data_size;
+
+static void gfx_write_scan(int do_clear)
+{
+	unsigned word = 0x02000000;
+
+	word |= clear_color;
+	if (swap_buffers)
+		word |= 0x01000000;
+
+	if (do_clear)
+		word |= 0x04000000;
+
+	GFX_CMD_SCAN = word;
+}
+
+void gfx_init(void)
+{
+	data_size = 0;
+	clear_color = 0;
+	swap_buffers = 0;
+	gfx_write_scan(1);
+
+	GFX_CMD_FB_BASE_A = FB_BASE_A;
+	GFX_CMD_FB_BASE_B = FB_BASE_B;
+	GFX_CMD_HEADER_BASE = HEADER_BASE;
+
+	*VRAM_HEADER_CODE_BASE = CODE_BASE;
+	*VRAM_HEADER_CODE_SIZE = CODE_SIZE;
+	*VRAM_HEADER_DATA_BASE = DATA_BASE;
+	*VRAM_HEADER_DATA_SIZE = 0;
+
+	VRAM_CODE[0] = 0x00000120; // recv m1
+	VRAM_CODE[1] = 0x00001010; // send m1
+	VRAM_CODE[2] = 0x00001010; // send m1
+}
+
+void gfx_clear(void)
+{
+	data_size = 0;
+	*VRAM_HEADER_DATA_SIZE = 0;
+	gfx_write_scan(1);
+}
+
+void gfx_swap(void)
+{
+	swap_buffers = !swap_buffers;
+	gfx_write_scan(0);
+}
+
+void gfx_draw(void)
+{
+	GFX_CMD_HEADER_SIZE = HEADER_SIZE;
+}
+
+void gfx_bg(unsigned color)
+{
+	clear_color = color;
+	gfx_write_scan(0);
+}
+
+void gfx_data(unsigned block, unsigned lane, short data[static 4])
+{
+	unsigned vec_num = block << 2 | lane;
+	for (unsigned i = 0; i < 4; ++i)
+		VRAM_DATA[vec_num << 2 | i] = data[i];
+
+	if (vec_num + 1 > data_size) {
+		data_size = vec_num + 1;
+		*VRAM_HEADER_DATA_SIZE = data_size;
+	}
+}
diff --git a/demo/main.c b/demo/main.c
index bc95579..7430884 100644
--- a/demo/main.c
+++ b/demo/main.c
@@ -123,6 +123,74 @@ static void cmd_remote(char **tokens)
 		unknown_command(cmd);
 }
 
+static void cmd_gfx_swap(char **tokens)
+{
+	if (!expect_end(tokens))
+		gfx_swap();
+}
+
+static void cmd_gfx_draw(char **tokens)
+{
+	if (!expect_end(tokens))
+		gfx_draw();
+}
+
+static void cmd_gfx_clear(char **tokens)
+{
+	if (!expect_end(tokens))
+		gfx_clear();
+}
+
+static void cmd_gfx_data(char **tokens)
+{
+	short data[4];
+	unsigned block, lane;
+
+	if (parse_hex(tokens, &block) < 0 || parse_lane(tokens, &lane) < 0
+	 || parse_fp16(tokens, &data[3]) < 0 || parse_fp16(tokens, &data[2]) < 0
+	 || parse_fp16(tokens, &data[1]) < 0 || parse_fp16(tokens, &data[0]) < 0
+	 || expect_end(tokens) < 0)
+		return;
+
+	gfx_data(block, lane, data);
+}
+
+static void cmd_gfx_bg(char **tokens)
+{
+	unsigned color;
+	if (parse_hex(tokens, &color) < 0 || expect_end(tokens) < 0)
+		return;
+	else if (color & 0xff000000) {
+		print("bad color: %x", color);
+		return;
+	}
+
+	gfx_bg(color);
+}
+
+static void cmd_gfx(char **tokens)
+{
+	const char *cmd;
+
+	if (!(cmd = strtok_input(tokens))) {
+		unexpected_eof();
+		return;
+	}
+
+	if (!strcmp(cmd, "swap"))
+		cmd_gfx_swap(tokens);
+	else if (!strcmp(cmd, "draw"))
+		cmd_gfx_draw(tokens);
+	else if (!strcmp(cmd, "clear"))
+		cmd_gfx_clear(tokens);
+	else if (!strcmp(cmd, "data"))
+		cmd_gfx_data(tokens);
+	else if (!strcmp(cmd, "bg"))
+		cmd_gfx_bg(tokens);
+	else
+		unknown_command(cmd);
+}
+
 static void kick_cpus(void)
 {
 	for (unsigned i = this_cpu->num + 1; i < NUM_CPUS; ++i) {
@@ -175,6 +243,8 @@ static void bsp_main(void)
 			cmd_perf(&tokens);
 		else if (!strcmp(cmd, "remote"))
 			cmd_remote(&tokens);
+		else if (!strcmp(cmd, "gfx"))
+			cmd_gfx(&tokens);
 		else
 			unknown_command(cmd);
 	}
@@ -201,8 +271,10 @@ static void ap_main(void)
 
 void reset(void)
 {
-	if (this_cpu->num == 0)
+	if (this_cpu->num == 0) {
 		console_init();
+		gfx_init();
+	}
 
 	print("exited reset");
 
diff --git a/demo/util.c b/demo/util.c
index d728fac..6ec23f8 100644
--- a/demo/util.c
+++ b/demo/util.c
@@ -1,6 +1,7 @@
 #include <stddef.h>
 
 #include "demo.h"
+#include "float16.h"
 
 static void bad_input(void)
 {
@@ -64,6 +65,24 @@ int parse_cpu(char **tokens, unsigned *cpu)
 	return parse_cpu_token(token, cpu);
 }
 
+int parse_lane(char **tokens, unsigned *lane)
+{
+	char *token = strtok_input(tokens);
+	if (!token) {
+		unexpected_eof();
+		return -1;
+	}
+
+	if (token[0] != 'l' || token[1] != 'a' || token[2] != 'n' || token[3] != 'e'
+	|| !('0' <= token[4] && token[4] <= '3') || token[5]) {
+		bad_input();
+		return -1;
+	}
+
+	*lane = token[4] - '0';
+	return 0;
+}
+
 int parse_cpu_mask(char **tokens, unsigned *mask)
 {
 	*mask = 0;
@@ -123,6 +142,48 @@ int parse_hex(char **tokens, unsigned *val)
 	return 0;
 }
 
+int parse_fp16(char **tokens, short *val)
+{
+	char *token = strtok_input(tokens);
+	if (!token) {
+		unexpected_eof();
+		return -1;
+	}
+
+	int neg = token[0] == '-';
+	if (neg)
+		++token;
+
+	int in_denom = 0;
+	int32_t num = 0, denom = 1;
+
+	do {
+		if (*token == '.') {
+			if (in_denom) {
+				bad_input();
+				return -1;
+			}
+
+			in_denom = 1;
+			continue;
+		} else if (!(*token >= '0' && *token <= '9')) {
+			bad_input();
+			return -1;
+		}
+
+		num = num * 10 + (*token - '0');
+		if (in_denom)
+			denom *= 10;
+	} while (*++token);
+
+	short fp = f16_div(f16_from_int(num), f16_from_int(denom));
+	if (neg)
+		fp = f16_neg(fp);
+
+	*val = fp;
+	return 0;
+}
+
 int parse_ptr(char **tokens, void **ptr)
 {
 	unsigned ptr_val;