diff --git a/picosoc/firmware.c b/picosoc/firmware.c
index e3eed76..4a86615 100644
--- a/picosoc/firmware.c
+++ b/picosoc/firmware.c
@@ -20,7 +20,11 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-#if !defined(ICEBREAKER) && !defined(HX8KDEMO)
+#ifdef ICEBREAKER
+  #define MEM_TOTAL 0x20000
+#elif HX8KDEMO
+  #define MEM_TOTAL 0x200
+#else
 #  error "Set -DICEBREAKER or -DHX8KDEMO when compiling firmware.c"
 #endif
 
@@ -176,11 +180,21 @@ void print_hex(uint32_t v, int digits)
 
 void print_dec(uint32_t v)
 {
-	if (v >= 100) {
-		print(">=100");
+	if (v >= 1000) {
+		print(">=1000");
 		return;
 	}
 
+	if      (v >= 900) { putchar('9'); v -= 900; }
+	else if (v >= 800) { putchar('8'); v -= 800; }
+	else if (v >= 700) { putchar('7'); v -= 700; }
+	else if (v >= 600) { putchar('6'); v -= 600; }
+	else if (v >= 500) { putchar('5'); v -= 500; }
+	else if (v >= 400) { putchar('4'); v -= 400; }
+	else if (v >= 300) { putchar('3'); v -= 300; }
+	else if (v >= 200) { putchar('2'); v -= 200; }
+	else if (v >= 100) { putchar('1'); v -= 100; }
+
 	if      (v >= 90) { putchar('9'); v -= 90; }
 	else if (v >= 80) { putchar('8'); v -= 80; }
 	else if (v >= 70) { putchar('7'); v -= 70; }
@@ -236,6 +250,95 @@ char getchar()
 	return getchar_prompt(0);
 }
 
+void cmd_print_spi_state()
+{
+	print("SPI State:\n");
+
+	print("  LATENCY ");
+	print_dec((reg_spictrl >> 16) & 15);
+	print("\n");
+
+	print("  DDR ");
+	if ((reg_spictrl & (1 << 22)) != 0)
+		print("ON\n");
+	else
+		print("OFF\n");
+
+	print("  QSPI ");
+	if ((reg_spictrl & (1 << 21)) != 0)
+		print("ON\n");
+	else
+		print("OFF\n");
+
+	print("  CRM ");
+	if ((reg_spictrl & (1 << 20)) != 0)
+		print("ON\n");
+	else
+		print("OFF\n");
+}
+
+uint32_t xorshift32(uint32_t *state)
+{
+	/* Algorithm "xor" from p. 4 of Marsaglia, "Xorshift RNGs" */
+	uint32_t x = *state;
+	x ^= x << 13;
+	x ^= x >> 17;
+	x ^= x << 5;
+	*state = x;
+
+	return x;
+}
+
+void cmd_memtest()
+{
+	int cyc_count = 5;
+	int stride = 256;
+	uint32_t state;
+
+	volatile uint32_t *base_word = (uint32_t *) 0;
+	volatile uint8_t *base_byte = (uint8_t *) 0;
+
+	print("Running memtest ");
+
+	// Walk in stride increments, word access
+	for (int i = 1; i <= cyc_count; i++) {
+		state = i;
+
+		for (int word = 0; word < MEM_TOTAL / sizeof(int); word += stride) {
+			*(base_word + word) = xorshift32(&state);
+		}
+
+		state = i;
+
+		for (int word = 0; word < MEM_TOTAL / sizeof(int); word += stride) {
+			if (*(base_word + word) != xorshift32(&state)) {
+				print(" ***FAILED WORD*** at ");
+				print_hex(4*word, 4);
+				print("\n");
+				return;
+			}
+		}
+
+		print(".");
+	}
+
+	// Byte access
+	for (int byte = 0; byte < 128; byte++) {
+		*(base_byte + byte) = (uint8_t) byte;
+	}
+
+	for (int byte = 0; byte < 128; byte++) {
+		if (*(base_byte + byte) != (uint8_t) byte) {
+			print(" ***FAILED BYTE*** at ");
+			print_hex(byte, 4);
+			print("\n");
+			return;
+		}
+	}
+
+	print(" passed\n");
+}
+
 // --------------------------------------------------------
 
 void cmd_read_flash_id()
@@ -570,36 +673,23 @@ void main()
 	print(" | |_) | |/ __/ _ \\___ \\ / _ \\| |\n");
 	print(" |  __/| | (_| (_) |__) | (_) | |___\n");
 	print(" |_|   |_|\\___\\___/____/ \\___/ \\____|\n");
+	print("\n");
+
+	print("Total memory: ");
+	print_dec(MEM_TOTAL / 1024);
+	print(" KiB\n");
+	print("\n");
+
+	cmd_memtest();
+	print("\n");
+
+	cmd_print_spi_state();
+	print("\n");
 
 	while (1)
 	{
 		print("\n");
-		print("\n");
-		print("SPI State:\n");
 
-		print("  LATENCY ");
-		print_dec((reg_spictrl >> 16) & 15);
-		print("\n");
-
-		print("  DDR ");
-		if ((reg_spictrl & (1 << 22)) != 0)
-			print("ON\n");
-		else
-			print("OFF\n");
-
-		print("  QSPI ");
-		if ((reg_spictrl & (1 << 21)) != 0)
-			print("ON\n");
-		else
-			print("OFF\n");
-
-		print("  CRM ");
-		if ((reg_spictrl & (1 << 20)) != 0)
-			print("ON\n");
-		else
-			print("OFF\n");
-
-		print("\n");
 		print("Select an action:\n");
 		print("\n");
 		print("   [1] Read SPI Flash ID\n");
@@ -611,6 +701,8 @@ void main()
 		print("   [7] Toggle continuous read mode\n");
 		print("   [9] Run simplistic benchmark\n");
 		print("   [0] Benchmark all configs\n");
+		print("   [M] Run Memtest\n");
+		print("   [S] Print SPI state\n");
 		print("\n");
 
 		for (int rep = 10; rep > 0; rep--)
@@ -650,6 +742,12 @@ void main()
 			case '0':
 				cmd_benchmark_all();
 				break;
+			case 'M':
+				cmd_memtest();
+				break;
+			case 'P':
+				cmd_print_spi_state();
+				break;
 			default:
 				continue;
 			}