diff --git a/display/firmware/Makefile b/display/firmware/Makefile
index 00a3a27..9532ff5 100644
--- a/display/firmware/Makefile
+++ b/display/firmware/Makefile
@@ -1,7 +1,7 @@
 PRG            = display
 PROJ_ROOT      = ../..
 AVRCANLIB_PATH = $(PROJ_ROOT)/avr-can-lib
-OBJ            = main.o \
+OBJ            = main.o display.o \
 	$(AVRCANLIB_PATH)/src/at90can_buffer.o \
 	$(AVRCANLIB_PATH)/src/at90can.o \
 	$(AVRCANLIB_PATH)/src/at90can_disable_dyn_filter.o \
diff --git a/display/firmware/main.c b/display/firmware/main.c
index 73c0924..8226ad7 100644
--- a/display/firmware/main.c
+++ b/display/firmware/main.c
@@ -5,11 +5,16 @@
 #include <util/delay.h>
 #include <can.h>
 #include <messages.h>
+#include "display.h"
 
 static bool report_change = false;
 
-static void init() {
-	/*
+static void init();
+static uint8_t get_buttons();
+static void send_status();
+static void loop();
+
+/* I/O map:
 	PB2 BUTTON0
 	PB3 BUTTON1
 	PB4 BUTTON2
@@ -31,16 +36,18 @@ static void init() {
 	PD5 VFDDB5
 	PD6 VFDDB6
 	PD7 VFDDB7
-	*/
+*/
+
+static void init() {
+	// initialize buttons
 	// PB2, PB3, PB4, PB5, PB6, PC0, PC1, PC4: input, no pullup (button0..7)
-	// PC5, PC6, PC7: output (VFD RS, RW, E), RS=0, RW=H, E=H
-	// PD: output (VFD DB) - can be switched to input w/ pullup
-	PORTB = 0x00;
-	DDRB = 0x00;
-	PORTC = _BV(PC6) | _BV(PC7);
-	DDRC = _BV(PC5) | _BV(PC6) | _BV(PC7);
-	PORTD = 0x00;
-	DDRD = 0xff;
+	PORTB &= ~(_BV(PB2) | _BV(PB3) | _BV(PB4) | _BV(PB5) | _BV(PB6));
+	DDRB &= ~(_BV(PB2) | _BV(PB3) | _BV(PB4) | _BV(PB5) | _BV(PB6));
+	PORTC &= ~(_BV(PC0) | _BV(PC1) | _BV(PC4));
+	DDRC &= ~(_BV(PC0) | _BV(PC1) | _BV(PC4));
+
+	// initialize VFD
+	display_init();
 
 	// Initialize CAN interface
 	can_init(BITRATE_125_KBPS);
@@ -103,102 +110,6 @@ static void send_status() {
 	}
 }
 
-static uint8_t read_ir() {
-	// switch PD to input
-	DDRD = 0x00;
-	// E=0 (enable/clock) RS=0 (IR) RW=1 (read)
-	PORTC = (PORTC & ~(_BV(PC5) | _BV(PC7))) | _BV(PC6);
-	_delay_us(0.23);
-	// E=1 (enable/clock)
-	PORTC |= _BV(PC7);
-	_delay_us(0.16);
-	uint8_t data = PIND;
-	_delay_us(0.23 - 0.16);
-	// E=0 (enable/clock)
-	PORTC &= ~_BV(PC7);
-	_delay_us(0.01);
-	// reset to idle
-	PORTC = _BV(PC6) | _BV(PC7);
-	DDRD = 0xff;
-	return data;
-}
-
-static uint8_t read_dr() {
-	// switch PD to input
-	DDRD = 0x00;
-	// E=0 (enable/clock) RS=1 (IR) RW=1 (read)
-	PORTC = (PORTC & ~_BV(PC7)) | (_BV(PC5) | _BV(PC6));
-	_delay_us(0.23);
-	// E=1 (enable/clock)
-	PORTC |= _BV(PC7);
-	_delay_us(0.16);
-	uint8_t data = PIND;
-	_delay_us(0.23 - 0.16);
-	// E=0 (enable/clock)
-	PORTC &= ~_BV(PC7);
-	_delay_us(0.01);
-	// reset to idle
-	PORTC = _BV(PC6) | _BV(PC7);
-	DDRD = 0xff;
-	return data;
-}
-
-static void write_ir(uint8_t data) {
-	// E=0 (enable/clock) RS=0 (IR) RW=0 (write)
-	PORTC &= ~(_BV(PC5) | _BV(PC6) | _BV(PC7));
-	_delay_us(0.23);
-	// E=1 (enable/clock)
-	PORTC |= _BV(PC7);
-	// data
-	PORTD = data;
-	_delay_us(0.23);
-	// E=0 (enable/clock)
-	PORTC &= ~_BV(PC7);
-	_delay_us(0.01);
-	// reset to idle
-	PORTC = _BV(PC6) | _BV(PC7);
-	PORTD = 0x00;
-}
-
-static void write_dr(uint8_t data) {
-	// E=0 (enable/clock) RS=1 (IR) RW=0 (write)
-	PORTC = (PORTC & ~(_BV(PC6) | _BV(PC7))) | _BV(PC5);
-	_delay_us(0.23);
-	// E=1 (enable/clock)
-	PORTC |= _BV(PC7);
-	// data
-	PORTD = data;
-	_delay_us(0.23);
-	// E=0 (enable/clock)
-	PORTC &= ~_BV(PC7);
-	_delay_us(0.01);
-	// reset to idle
-	PORTC = _BV(PC6) | _BV(PC7);
-	PORTD = 0x00;
-}
-
-
-static bool wait_ready() {
-	// if the busy flag never goes high, we'll run into a deadlock here.
-	// let's put a deadline on the wait loop.
-	for (uint8_t i = 0; i < 100; i++) {
-		// read_ir() takes about 500ns
-		if (read_ir() & _BV(7)) return true;
-		// wait 10us until the next status read
-		_delay_us(10 - 0.5);
-	}
-	return false;
-}
-
-static void write_display(uint8_t command, const uint8_t *data, uint8_t length) {
-	if (wait_ready()) {
-		write_ir(command);
-		for (uint8_t i = 0; i < length; i++) {
-			write_dr(data[i]);
-		}
-	}
-}
-
 static void loop() {
 	// Check for new messages
 	if (can_check_message()) {
@@ -211,7 +122,7 @@ static void loop() {
 				break;
 			case CAN_MSG_DISPLAY_CMD:
 				if (msg.length >= 1) {
-					write_display(msg.data[0], &msg.data[1], msg.length - 1);
+					display_write(msg.data[0], &msg.data[1], msg.length - 1);
 				}
 				break;
 			case CAN_MSG_AUTO_STATUS: