diff --git a/Inc/control.h b/Inc/control.h
index 173ecee..12dc543 100644
--- a/Inc/control.h
+++ b/Inc/control.h
@@ -18,17 +18,6 @@ typedef enum {
 	CONTROL_CLOCK_HIGH
 } ControlState;
 
-typedef struct {
-	uint8_t programming;
-	uint16_t length;
-	uint8_t* data;
-} Eeprom;
-
-typedef struct {
-	uint8_t received;
-	uint8_t c;
-} Input;
-
 typedef struct {
 	uint8_t dirty;
 	uint8_t action;
@@ -45,19 +34,14 @@ typedef struct {
 	ControlState state;
 	uint8_t memory_config;
 	Storage storage;
-	Input input;
-	Eeprom eeprom;
 } Control;
 
 uint16_t read_address();
 void write_data(uint8_t value);
 uint8_t read_data();
 
+void control_program_eeprom(uint8_t* data, uint16_t length);
 void control_execute_state();
 void control_reset();
 
-uint8_t control_receive_program(uint8_t byte);
-
-void send_key(uint8_t c);
-
 #endif
diff --git a/Src/control.c b/Src/control.c
index 6517f4f..af86441 100644
--- a/Src/control.c
+++ b/Src/control.c
@@ -47,86 +47,47 @@ uint32_t calculate_lba() {
 	return temp;
 }
 
-void program_eeprom() {
-	static int address_counter = 0;
-	static int read_counter = 0;
-	static int write_counter = 0;
-
-	// Check if we are done
-	if (address_counter == control.eeprom.length) {
-		// Mark as done and trigger Z80 reset
-		control.eeprom.programming = 0;
-		control.state = CONTROL_RESET_BEGIN;
-
-		// Reset counters
-		address_counter = 0;
-		read_counter = 0;
-		write_counter = 0;
-		send_busrq(0);
-
-		// Reset signals
-		send_memrq(0);
-		send_wr(0);
-		send_rd(0);
-
-		// Free memory used to store program
-		free(control.eeprom.data);
-		control.eeprom.data = NULL;
-		control.eeprom.length = 0;
-
-		// Notify the user
-		printf("\nEEPROM Programmed!\n\r");
-
-		// Turn of the update light
-		HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
-
-		return;
+void control_program_eeprom(uint8_t* data, uint16_t length) {
+	// Take control of the bus
+	printf("Taking control of bus...\n\r");
+	send_busrq(1);
+	while (!has_busak()) {
+		control_execute_state();
 	}
 
-	// Turn on the update light
-	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_SET);
+	printf("Done!\n\r");
+	printf("Writing to ROM...\n\r");
 
-	// Check if the data is written
-	if (write_counter < 100) {
-		// Select correct device and address
-		select_device(memory_map_0[0]);
-		write_address(address_counter);
+	enable_address_out(1);
 
-		// Write the data
-		write_data(control.eeprom.data[address_counter]);
+	select_device(memory_map_0[0]);
+
+	for (uint16_t i = 0; i < length; ++i) {
+		write_address(i);
+		write_data(data[i]);
 
-		// Send write request
 		enable_data_out(1);
 		send_memrq(1);
 		send_wr(1);
 
-		write_counter++;
-	} else {
-		// Stop writing
-		enable_data_out(0);
-		send_memrq(1);
 		send_wr(0);
+		enable_data_out(0);
 		send_rd(1);
 
-		uint8_t d = read_data();
-		if (read_counter > 5 && d == control.eeprom.data[address_counter]) {
-			address_counter++;
-			write_counter = 0;
-			read_counter = 0;
+		printf("%i/%i\r", i+1, length);
 
-			send_rd(0);
-
-			printf("Progress: %i/%i\r", address_counter, control.eeprom.length);
-		} else if (read_counter > 4000) {
-			// In case we failed, try again
-			printf("Trying again! %x != %x (%i)\n\r", d, control.eeprom.data[address_counter], address_counter);
-			read_counter = 0;
-			write_counter = 0;
-			send_rd(0);
-		} else {
-			read_counter++;
+		for (;;) {
+			uint8_t d = read_data();
+			if (d == data[i]) {
+				break;
+			}
 		}
+
+		send_rd(0);
+		send_memrq(0);
 	}
+
+	printf("\n");
 }
 
 void handle_memrq() {
@@ -139,6 +100,7 @@ void handle_io_read() {
 	uint8_t address = read_address() & 0xFF;
 
 	switch (address) {
+		// @todo This should be detected on startup
 		// Stand in for graphics hardware
 		/* case 0x03: */
 		/* 	write_data(0x01); */
@@ -266,27 +228,13 @@ void handle_ioreq() {
 }
 
 void cycle() {
-	// Make sure data get reset as input
-	if (!control.eeprom.programming) {
-		enable_data_out(0);
-	}
-
 	// We need this not detect IO multiple times
 	static uint8_t had_ioreq = 0;
 	if (!has_ioreq()) {
 		had_ioreq = 0;
 	}
 
-	// @todo Enable this one we start doing things using interrupts again
-	/* send_int(control.interrupt.type != CONTROL_INT_NONE); */
-
-	if (control.eeprom.programming && has_busak()) {
-		if (control.eeprom.programming) {
-			program_eeprom();
-		}
-	} else if (control.eeprom.programming != has_busak()) {
-		send_busrq(control.eeprom.programming);
-	} else if (has_memrq()) {
+	if (has_memrq()) {
 		handle_memrq();
 	} else if (has_ioreq() && !has_m1()) {
 		had_ioreq++;
@@ -347,39 +295,7 @@ void control_execute_state() {
 // @todo Properly reset everything
 void control_reset() {
 	free(control.storage.buffer);
-	Control temp = {CONTROL_RESET_BEGIN, 0, {1, 0, 0, 0, 0, 0, 0, 0, NULL}, {0,0}, {0, 0, NULL}};
+	Control temp = {CONTROL_RESET_BEGIN, 0, {1, 0, 0, 0, 0, 0, 0, 0, NULL}};
 	control = temp;
 	control.storage.buffer = (uint8_t*)malloc(SD_PAGES*SD_PAGE_SIZE);
 }
-
-
-uint8_t control_receive_program(uint8_t byte) {
-	static uint16_t i = 0;
-	static uint16_t c = 0;
-
-	if (c <= 1) {
-		control.eeprom.length += byte << c*8;
-		c++;
-	} else {
-		if (c == 2) {
-			control.eeprom.data = malloc(control.eeprom.length);
-			c++;
-		}
-		control.eeprom.data[i] = byte;
-		i++;
-	}
-
-	if (i >= control.eeprom.length) {
-		i = 0;
-		c = 0;
-		control.eeprom.programming = 1;
-		return 1;
-	}
-
-	return 0;
-}
-
-void send_key(uint8_t c) {
-	control.input.c = c;
-	control.input.received = 1;
-}
diff --git a/Src/firmware.c b/Src/firmware.c
index ec2e5a4..05a9073 100644
--- a/Src/firmware.c
+++ b/Src/firmware.c
@@ -1,7 +1,10 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
+
 #include "firmware.h"
+#include "control.h"
+#include "io.h"
 
 extern I2C_HandleTypeDef hi2c1;
 extern UART_HandleTypeDef huart2;
@@ -34,29 +37,43 @@ void firmware_update() {
 	HAL_UART_Receive(&huart2, data, length, 1000);
 
 	printf("Firmware received!\n\r");
-	printf("Uploading to target...\n\r");
 
 	switch (target) {
+		case TARGET_ROM:
+			control_program_eeprom(data, length);
+			break;
 		case TARGET_I2C: {
-			 // Upload the application
-			 uint8_t upload_command[4 + length];
-			 upload_command[0] = 0x02;
-			 upload_command[1] = 0x01;
+			// Make sure tell the device to enter bootloader mode first
+			// Upload the application
+			printf("Uploading to target...\n\r");
+			uint8_t upload_command[4 + length];
+			upload_command[0] = 0x02;
+			upload_command[1] = 0x01;
 
-			 for (uint16_t offset = 0; offset < 8; ++offset) {
-				 upload_command[2] = ((offset*0x80) >> 8) & 0xFF;
-				 upload_command[3] = (offset*0x80) & 0xFF;
+			printf("0/%i\r", length);
 
-				 // We need to handle the last section properly
-				 memcpy(&upload_command[4], &data[offset*0x80], 0x80);
+			uint16_t remaining = length;
+			for (uint8_t offset = 0; remaining > 0; ++offset) {
+				upload_command[2] = ((offset*0x80) >> 8) & 0xFF;
+				upload_command[3] = (offset*0x80) & 0xFF;
 
-				 HAL_I2C_Master_Transmit(&hi2c1, address << 1, upload_command, sizeof(upload_command), 1000);
-			 }
+				uint8_t amount = remaining > 0x80 ? 0x80 : remaining;
+				remaining -= amount;
 
-			 // Start application
-			 uint8_t start_command[] = {0x01, 0x80};
-			 HAL_I2C_Master_Transmit(&hi2c1, address << 1, start_command, sizeof(start_command), 1000);
-			 break;
+				// We need to handle the last section properly
+				memcpy(&upload_command[4], &data[offset*0x80], amount);
+
+				HAL_I2C_Master_Transmit(&hi2c1, address << 1, upload_command, sizeof(upload_command), 1000);
+
+				printf("%i/%i\r", length-remaining, length);
+			}
+
+			printf("\n");
+
+			// Start application
+			uint8_t start_command[] = {0x01, 0x80};
+			HAL_I2C_Master_Transmit(&hi2c1, address << 1, start_command, sizeof(start_command), 1000);
+			break;
 		}
 
 		default:
diff --git a/Src/main.c b/Src/main.c
index fa915a2..2a183b6 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -73,113 +73,94 @@ static void MX_SDIO_SD_Init(void);
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
-typedef uint8_t (*upload_callback_t)(uint8_t byte);
-upload_callback_t upload_callback = NULL;
-
 uint8_t byte;
 void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
 	if (huart->Instance == USART2) {
-		if (upload_callback) {
-			if (upload_callback(byte)) {
-				upload_callback = NULL;
+		static uint8_t cmd_buffer[128];
+		static uint32_t cmd_buffer_len = 0;
 
-				printf("Done\n\r");
-			}
-		} else {
+		if (cmd_buffer_len+1 > 128) {
+			printf("\n\rCommand is too long!\n\r");
+			cmd_buffer_len = 0;
+			return;
+		}
 
-			static uint8_t cmd_buffer[128];
-			static uint32_t cmd_buffer_len = 0;
-
-			if (cmd_buffer_len+1 > 128) {
-				printf("\n\rCommand is too long!\n\r");
-				cmd_buffer_len = 0;
-				return;
-			}
-
-			int handle_command = 0;
-			static int is_command = 0;
-			switch (byte) {
-				// @todo Handle \t
-				// Backspace
-				case 0x08:
-					if (is_command) {
-						if (cmd_buffer_len) {
-							cmd_buffer_len--;
-						} else {
-							is_command = 0;
-						}
-						break;
-					}
-					// else falltrough
-
-					// Newline
-				case '\r':
-				case '\n':
-					if (is_command) {
-						handle_command = 1;
-						break;
-					}
-					// else falltrough
-
-				default:
-					if (!is_command && cmd_buffer_len == 0 && byte == '#') {
-						is_command = 1;
+		int handle_command = 0;
+		static int is_command = 0;
+		switch (byte) {
+			// @todo Handle \t
+			// Backspace
+			case 0x08:
+				if (is_command) {
+					if (cmd_buffer_len) {
+						cmd_buffer_len--;
 					} else {
-						cmd_buffer[cmd_buffer_len] = byte;
-						cmd_buffer_len++;
+						is_command = 0;
 					}
 					break;
-			}
-
-			if (!is_command) {
-				// ONLY SEND THE FIRST CHAR
-				if (cmd_buffer_len > 0) {
-					send_key(byte);
-					HAL_I2C_Master_Transmit(&hi2c1, 0x04 << 1, &byte, 1, 1000);
 				}
-				cmd_buffer_len = 0;
-			} else if (handle_command && is_command) {
-				if (cmd_buffer_len == 1) {
-					switch (cmd_buffer[0]) {
-						case 'r':
-							control_reset();
-							printf("Starting...\n\r");
-							break;
+				// else falltrough
 
-						case 'b':
-							request_restart_to_bootloader();
-							printf("Restarting... (bootloader)\n\r");
-							break;
+				// Newline
+			case '\r':
+			case '\n':
+				if (is_command) {
+					handle_command = 1;
+					break;
+				}
+				// else falltrough
 
-						case 'u':
-							printf("Receiving program...\n\r");
-							upload_callback = &control_receive_program;
-							break;
-
-						case 'h':
-							printf("#r - (Re)start program\n\r#b - Restart (Bootloader)\n\r#u - Update\n\r#m - Mount the sd card to pc\n\r#h - Show this menu\n\r");
-							break;
-
-						case '#':
-							send_key('#');
-							HAL_I2C_Master_Transmit(&hi2c1, 0x04 << 1, &byte, 1, 1000);
-							break;
-
-						case 'a':
-							firmware_update();
-							break;
-
-						default:
-							printf("Unknown command: %c!\n\r", cmd_buffer[0]);
-							break;
-					}
+			default:
+				if (!is_command && cmd_buffer_len == 0 && byte == '#') {
+					is_command = 1;
 				} else {
-					printf("Unknown command!\n\r");
+					cmd_buffer[cmd_buffer_len] = byte;
+					cmd_buffer_len++;
 				}
+				break;
+		}
 
-				cmd_buffer_len = 0;
-				is_command = 0;
+		if (!is_command) {
+			// ONLY SEND THE FIRST CHAR
+			if (cmd_buffer_len > 0) {
+				HAL_I2C_Master_Transmit(&hi2c1, 0x04 << 1, &byte, 1, 1000);
 			}
+			cmd_buffer_len = 0;
+		} else if (handle_command && is_command) {
+			if (cmd_buffer_len == 1) {
+				switch (cmd_buffer[0]) {
+					case 'r':
+						control_reset();
+						printf("Starting...\n\r");
+						break;
+
+					case 'b':
+						request_restart_to_bootloader();
+						printf("Restarting... (bootloader)\n\r");
+						break;
+
+					case 'u':
+						firmware_update();
+						break;
+
+					case 'h':
+						printf("#r - (Re)start program\n\r#b - Restart (Bootloader)\n\r#u - Update\n\r#m - Mount the sd card to pc\n\r#h - Show this menu\n\r");
+						break;
+
+					case '#':
+						HAL_I2C_Master_Transmit(&hi2c1, 0x04 << 1, &byte, 1, 1000);
+						break;
+
+					default:
+						printf("Unknown command: %c!\n\r", cmd_buffer[0]);
+						break;
+				}
+			} else {
+				printf("Unknown command!\n\r");
+			}
+
+			cmd_buffer_len = 0;
+			is_command = 0;
 		}
 		HAL_UART_Receive_IT(&huart2, &byte, 1);
 	}