Improve PicoSoC demo firmware

1 files changed, 93 insertions, 15 deletions

diff --git a/picosoc/firmware.c b/picosoc/firmware.c
index c26547f..660ef56 100644
--- a/picosoc/firmware.c
+++ b/picosoc/firmware.c
@@ -5,23 +5,101 @@
 #define reg_uart_data (*(volatile uint32_t*)0x02000008)
 #define reg_leds (*(volatile uint32_t*)0x03000000)
 
+void print(const char *p)
+{
+	while (*p) {
+		if (*p == '\n')
+			reg_uart_data = '\r';
+		reg_uart_data = *(p++);
+	}
+}
+
+char getchar_prompt(char *prompt)
+{
+	int32_t c = -1;
+
+	uint32_t cycles_begin, cycles_now, cycles;
+	__asm__ volatile ("rdcycle %0" : "=r"(cycles_begin));
+
+	if (prompt)
+		print(prompt);
+
+	reg_leds = ~0;
+	while (c == -1) {
+		__asm__ volatile ("rdcycle %0" : "=r"(cycles_now));
+		cycles = cycles_now - cycles_begin;
+		if (cycles > 12000000) {
+			if (prompt)
+				print(prompt);
+			cycles_begin = cycles_now;
+			reg_leds = ~reg_leds;
+		}
+		c = reg_uart_data;
+	}
+	reg_leds = 0;
+	return c;
+}
+
+char getchar()
+{
+	return getchar_prompt(0);
+}
+
+// --------------------------------------------------------
+
+extern uint32_t cmd_read_spi_flash_id_worker_begin;
+extern uint32_t cmd_read_spi_flash_id_worker_end;
+
+void cmd_read_spi_flash_id()
+{
+	uint32_t *src_ptr = &cmd_read_spi_flash_id_worker_begin;
+	uint32_t *dst_ptr = (uint32_t*)0;
+
+	while (src_ptr != &cmd_read_spi_flash_id_worker_end)
+		*(dst_ptr++) = *(src_ptr++);
+
+	((void(*)())0)();
+}
+
+// --------------------------------------------------------
+
 void main()
 {
 	reg_uart_clkdiv = 104;
-	reg_leds = 1;
-	reg_uart_data = 'H';
-	reg_leds = 2;
-	reg_uart_data = 'e';
-	reg_leds = 3;
-	reg_uart_data = 'l';
-	reg_leds = 4;
-	reg_uart_data = 'l';
-	reg_leds = 5;
-	reg_uart_data = 'o';
-	reg_leds = 6;
-	reg_uart_data = '\r';
-	reg_leds = 7;
-	reg_uart_data = '\n';
-	reg_leds = 8;
+	while (getchar_prompt("Press ENTER to continue..\n") != '\r') { /* wait */ }
+
+	print("\n");
+	print("  ____  _          ____         ____\n");
+	print(" |  _ \\(_) ___ ___/ ___|  ___  / ___|\n");
+	print(" | |_) | |/ __/ _ \\___ \\ / _ \\| |\n");
+	print(" |  __/| | (_| (_) |__) | (_) | |___\n");
+	print(" |_|   |_|\\___\\___/____/ \\___/ \\____|\n");
+
+	while (1)
+	{
+		print("\n");
+		print("\n");
+		print("Select an action:\n");
+		print("\n");
+		print("   [1] Read SPI Flash ID\n");
+		print("\n");
+
+		for (int rep = 10; rep > 0; rep--)
+		{
+			print("Command> ");
+			char cmd = getchar();
+			if (cmd > 32 && cmd < 127)
+				reg_uart_data = cmd;
+			print("\n");
+
+			switch (cmd)
+			{
+			case '1':
+				cmd_read_spi_flash_id();
+				rep = 0;
+				break;
+			}
+		}
+	}
 }