diff --git a/Makefile b/Makefile
index 79cbd17..da3b94d 100644
--- a/Makefile
+++ b/Makefile
@@ -20,6 +20,7 @@ SRC_C = \
 	cmd-load.c \
 	cmd-bl.c \
 	cmd-lcd.c \
+	cmd-keypad.c \
 	emi.c \
 	irq.c \
 	lcd.c \
@@ -36,6 +37,7 @@ SRC_S = \
 	scriptic/enable-psram.S \
 	scriptic/spi.S \
 	scriptic/spi-blockmode.S \
+	scriptic/keypad.S \
 	_lshrdi3.S \
 	_udivsi3.S \
 	_divsi3.S \
diff --git a/cmd-keypad.c b/cmd-keypad.c
new file mode 100644
index 0000000..9d72d91
--- /dev/null
+++ b/cmd-keypad.c
@@ -0,0 +1,82 @@
+#include <string.h>
+#include "bionic.h"
+#include "memio.h"
+#include "printf.h"
+#include "serial.h"
+
+#include "fernvale-kbd.h"
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof(*x))
+#endif
+
+const char key_vals[] = "LRUDAB123456789*0#";
+
+const uint32_t key_addr[]={
+	KBD_MEM2,
+	KBD_MEM2,
+	KBD_MEM1,
+	KBD_MEM1,
+	KBD_MEM3,
+	KBD_MEM3,
+	KBD_MEM2,
+	KBD_MEM1,
+	KBD_MEM3,
+	KBD_MEM2,
+	KBD_MEM1,
+	KBD_MEM3,
+	KBD_MEM2,
+	KBD_MEM1,
+	KBD_MEM3,
+	KBD_MEM2,
+	KBD_MEM2,
+	KBD_MEM2
+};
+
+const uint32_t key_mask[] = {
+	0x0000fffb,
+	0x0000fff7,
+	0x0000fdff,
+	0x0000fbff,
+	0x0000ffef,
+	0x0000ffdf,
+	0x0000ffef,
+	0x0000f7ff,
+	0x0000ffbf,
+	0x0000ffdf,
+	0x0000efff,
+	0x0000ff7f,
+	0x0000ffbf,
+	0x0000dfff,
+	0x0000feff,
+	0x0000dfff,
+	0x0000bfff,
+	0x00007fff
+};
+
+int cmd_keypad(int argc, char **argv)
+{
+	int end = 0;
+	uint32_t key_state[18] = {0};
+
+	printf("Press %c on keypad to exit\n",
+		key_vals[ARRAY_SIZE(key_vals) - 2]);
+	while (!end) {
+		int key;
+
+		for (key = 0; key < (ARRAY_SIZE(key_vals) - 1); key++) {
+			int v = readl(key_addr[key]);
+			int newstate = (v == key_mask[key]);
+
+			if (newstate && !key_state[key]) {
+				serial_putc(key_vals[key]);
+				if (key == (ARRAY_SIZE(key_vals) - 2))
+					end = 1;
+			}
+			key_state[key] = newstate;		
+		}
+	}
+
+	printf("\n");
+	return 0;
+}
diff --git a/include/fernvale-kbd.h b/include/fernvale-kbd.h
index b087eb6..0cf19cf 100644
--- a/include/fernvale-kbd.h
+++ b/include/fernvale-kbd.h
@@ -11,4 +11,17 @@
 #define BIG_LED_ON    (0x3)
 #define BIG_LED_OFF   (0x0)
 
+#define KBD_ADDR		0xA00D0000
+#define KBD_STATUS		(KBD_ADDR + 0x0000)
+#define KBD_MEM1		(KBD_ADDR + 0x4)
+#define KBD_MEM2		(KBD_ADDR + 0x8)
+#define KBD_MEM3		(KBD_ADDR + 0xc)
+#define KBD_MEM4		(KBD_ADDR + 0x10)
+#define KBD_MEM5		(KBD_ADDR + 0x14)
+#define KBD_DEBOUNCING		(KBD_ADDR + 0x18)
+#define KBD_SCAN_TIME_ADJ	(KBD_ADDR + 0x1c)
+#define KBD_1OR2		(KBD_ADDR + 0x20)
+#define KBD_ENABLE		(KBD_ADDR + 0x24)
+
+
 #endif /* __FV_KBD_H__ */
diff --git a/main.c b/main.c
index 6087887..b38e490 100644
--- a/main.c
+++ b/main.c
@@ -201,12 +201,14 @@ static int do_init(void)
 
 	scriptic_run("set_plls");
 	scriptic_run("enable_psram");
-
 	serial_puts("\n\nFernly shell\n");
 	shell_run_command("bl 5");
 	shell_run_command("lcd init");
 	shell_run_command("lcd tpd");
 
+	/* Initialize the keypad */
+	scriptic_run("set_kbd");
+
 	return 0;
 }
 
@@ -352,6 +354,7 @@ extern int cmd_bl(int argc, char **argv);
 extern int cmd_lcd(int argc, char **argv);
 extern int cmd_load(int argc, char **argv);
 extern int cmd_loadjump(int argc, char **argv);
+extern int cmd_keypad(int argc, char **argv);
 
 static const struct {
 	int (*func)(int argc, char **argv);
@@ -434,6 +437,11 @@ static const struct {
 		.help = "Load data to a specific area in memory, "
 			"then jump to it",
 	},
+	{
+		.func = cmd_keypad,
+		.name = "keypad",
+		.help = "Read keys from keypad until # is pressed ",
+	},
 };
 
 int cmd_help(int argc, char **argv)
diff --git a/scriptic.c b/scriptic.c
index 23db4fa..9d46509 100644
--- a/scriptic.c
+++ b/scriptic.c
@@ -10,12 +10,14 @@ extern struct scriptic enable_psram;
 extern struct scriptic spi_run;
 extern struct scriptic spi_run;
 extern struct scriptic spi_init;
+extern struct scriptic set_kbd;
 
 static struct scriptic *scripts[] = {
 	&set_plls,
 	&enable_psram,
 	&spi_run,
 	&spi_init,
+	&set_kbd,
 };
 
 #ifdef SCRIPTIC_DEBUG
diff --git a/scriptic/keypad.S b/scriptic/keypad.S
new file mode 100644
index 0000000..23c878d
--- /dev/null
+++ b/scriptic/keypad.S
@@ -0,0 +1,17 @@
+#include "scriptic.h"
+#include "fernvale-gpio.h"
+
+	sc_new "set_kbd", 1, 0, 0
+
+	/* Set pinmux to use as Keypad instead of GPIO */
+	sc_write32 0x11100000, 0, GPIO_CTRL_MODE0
+	sc_usleep 100
+	sc_read32 0x0, 0, GPIO_CTRL_MODE0
+	sc_write32 0x11111011, 0, GPIO_CTRL_MODE1
+	sc_usleep 100
+	sc_read32 0x0, 0, GPIO_CTRL_MODE1
+
+	/* The keyboard controller itself is setup correctly by default,
+	 * no initialisation necessary. */
+
+	sc_end