diff --git a/Makefile b/Makefile
index cc2204f..b0fb9f4 100755
--- a/Makefile
+++ b/Makefile
@@ -6,7 +6,7 @@ TARGET = images/OpenModem
 OPT = s
 FORMAT = ihex
 
-SRC = main.c hardware/Serial.c hardware/AFSK.c hardware/VREF.c hardware/LED.c hardware/SD.c hardware/sdcard/diskio.c hardware/sdcard/ff.c hardware/sdcard/ffsystem.c hardware/sdcard/ffunicode.c util/CRC-CCIT.c protocol/AX25.c protocol/KISS.c
+SRC = main.c hardware/Serial.c hardware/AFSK.c hardware/VREF.c hardware/LED.c hardware/UserIO.c hardware/SD.c hardware/sdcard/diskio.c hardware/sdcard/ff.c hardware/sdcard/ffsystem.c hardware/sdcard/ffunicode.c hardware/Bluetooth.c util/CRC-CCIT.c protocol/AX25.c protocol/KISS.c
 
 
 # List Assembler source files here.
@@ -22,7 +22,7 @@ EXTRAINCDIRS =
 #  -Wall...:  warning level
 #  -Wa,...:   tell GCC to pass this to the assembler.
 #    -ahlms:  create assembler listing
-CFLAGS = -g -O$(OPT) \
+CFLAGS = -O$(OPT) \
 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \
 -Wall -Wstrict-prototypes -fno-strict-aliasing \
 -Wa,-adhlns=$(<:.c=.lst) \
diff --git a/device.h b/device.h
index cb92d29..19907e0 100755
--- a/device.h
+++ b/device.h
@@ -23,10 +23,9 @@
 #define OPEN_SQUELCH true
 
 // Serial settings
-#define BAUD 115200
 #define SERIAL_DEBUG false
 #define TX_MAXWAIT 2UL
-#define CONFIG_QUEUE_SIZE 5000 // TODO: Optimise this by saving ram other places
+#define CONFIG_QUEUE_SIZE 3000 // TODO: Optimise this by saving ram other places, add SD queue
 #define CONFIG_QUEUE_MAX_LENGTH 15
 #define CONFIG_SERIAL_BUFFER_SIZE 1532 // TODO: Tune this, what is actually required?
 #define CONFIG_SERIAL_TIMEOUT_MS 10
@@ -74,6 +73,19 @@
 	#define SD_DETECT_PORT  PORTA
 	#define SD_DETECT_INPUT PINA
 	#define SD_DETECT_PIN	7
+
+	#define BT_DDR		DDRA
+	#define BT_PORT		PORTA
+	#define BT_INPUT	PINA
+	#define BT_MODE		3
+	#define BT_RTS		4
+
+	#define USR_IO_DDR	DDRA
+	#define USR_IO_PORT	PORTA
+	#define USR_IO_1	1
+	#define USR_IO_2	2
+	#define USR_IO_3	3
+	#define USR_IO_4	4
 #endif
 
 #endif
diff --git a/hardware/AFSK.c b/hardware/AFSK.c
index bd08ba5..8ee360f 100755
--- a/hardware/AFSK.c
+++ b/hardware/AFSK.c
@@ -3,6 +3,7 @@
 #include "util/time.h"
 #include "hardware/LED.h"
 #include "protocol/KISS.h"
+#include "hardware/SD.h"
 
 // TODO: Remove testing vars ////
 #define SAMPLES_TO_CAPTURE 128
@@ -603,7 +604,7 @@ void AFSK_adc_isr(Afsk *afsk, int8_t currentSample) {
 inline void timed_functions(void) {
     update_led_status();
     if (_clock % CLOCK_TICKS_PER_10_MS == 0) {
-        disk_timerproc();
+        sd_scheduler();
     }
 }
 
diff --git a/hardware/Bluetooth.c b/hardware/Bluetooth.c
new file mode 100644
index 0000000..5dfd92f
--- /dev/null
+++ b/hardware/Bluetooth.c
@@ -0,0 +1,60 @@
+#include "Bluetooth.h"
+#include <avr/pgmspace.h>
+
+bool bluetooth_installed = false;
+
+#define BT_AT_DELAY 100
+void bluetooth_init(void) {
+	BT_DDR &= ~(_BV(BT_MODE) | _BV(BT_RTS));
+	BT_PORT |= _BV(BT_RTS);
+	
+	// Check module is in command mode
+	if (BT_INPUT & _BV(BT_MODE)) {
+		// Check module RTS pin is pulled low
+		if (!(BT_INPUT &_BV(BT_RTS))) {
+			// Reconfigure UART to 9600 baud and issue reset
+			serial_setbaudrate_9600();
+			delay_ms(BT_AT_DELAY*2);
+			printf(PSTR("AT"));
+			printf(PSTR("AT+FACTORYRESET\r\n"));
+			delay_ms(BT_AT_DELAY);
+			
+			printf(PSTR("AT+GAPDEVNAME=OpenModem\r\n"));
+			delay_ms(BT_AT_DELAY);
+
+			printf(PSTR("AT+MODESWITCHEN=ble,0\r\n"));
+			delay_ms(BT_AT_DELAY);
+
+			printf(PSTR("AT+MODESWITCHEN=local,0\r\n"));
+			delay_ms(BT_AT_DELAY);
+
+			printf(PSTR("AT+HWMODELED=0\r\n"));
+			delay_ms(BT_AT_DELAY);
+
+			printf(PSTR("AT+BLEPOWERLEVEL=4\r\n"));
+			delay_ms(BT_AT_DELAY);
+
+			printf(PSTR("ATZ\r\n"));
+			delay_ms(BT_AT_DELAY);
+			
+			// Reconfigure mode pin to output
+			BT_DDR |= _BV(BT_MODE);
+
+			// Enable DATA mode
+			BT_PORT &= ~_BV(BT_MODE);
+
+			bluetooth_installed = true;
+		}
+	}
+
+	if (bluetooth_installed) {
+		
+	} else {
+		
+	}
+
+}
+
+bool bluetooth_enabled() {
+	return bluetooth_installed;
+}
\ No newline at end of file
diff --git a/hardware/Bluetooth.h b/hardware/Bluetooth.h
new file mode 100644
index 0000000..8cc3dd7
--- /dev/null
+++ b/hardware/Bluetooth.h
@@ -0,0 +1,19 @@
+#ifndef BT_H
+#define BT_H
+
+#include <avr/io.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include "device.h"
+#include "util/time.h"
+#include "hardware/serial.h"
+
+void bluetooth_init(void);
+bool bluetooth_enabled(void);
+
+
+#endif
+
+// CMD/DATA sequence: 0x2b2b2b0d
diff --git a/hardware/SD.c b/hardware/SD.c
index 9f0eaf0..64becaf 100644
--- a/hardware/SD.c
+++ b/hardware/SD.c
@@ -1,6 +1,16 @@
 #include "SD.h"
 
-FATFS sdfs;    // FatFs work area 
+FATFS sdfs;
+
+uint8_t sd_scheduler_timer = 0;
+uint8_t sd_status = SD_STATUS_UNKNOWN;
+bool sd_mountstate = false;
+
+// Job flags
+bool sd_flag_needs_automount = false;
+bool sd_flag_needs_autounmount = false;
+bool sd_flag_was_automounted = false;
+bool sd_flag_was_autounmounted = false;
 
 void sd_init(void) {
     SPI_DDR |= _BV(SPI_MOSI) | _BV(SPI_CLK);
@@ -12,48 +22,144 @@ void sd_init(void) {
     SD_DETECT_PORT |= _BV(SD_DETECT_PIN); // Enable pull-up
 }
 
-// TODO: Remove this
-void sd_test(void) {
-    printf("Testing SD card functions, waiting for card...\r\n");
+void sd_automounted_hook(void) {
+    sd_statuschange_indication(1);
+}
 
-    FRESULT res = 0xFF;
-    while (res != 0) {
-        res = f_mount(&sdfs, "", 1);
-        printf("SD Detect: %d\r\n", (SD_DETECT_INPUT & _BV(SD_DETECT_PIN)));
-        printf("Res: %d\r\n", res);
-        delay_ms(500);
+void sd_autounmounted_hook(void) {
+    sd_statuschange_indication(0);
+}
+
+void sd_jobs(void) {
+    if (sd_flag_needs_automount) {
+        sd_flag_needs_automount = false;
+        sd_automount();
     }
 
-    char str[12];
-    DWORD sn;
-    f_getlabel("", str, &sn);
-
-    printf("Label: %s, SN: %lu\r\n", str, sn);
-
-    FIL fil;       
-    char line[100];
-    FRESULT fr;    
-
-    
-    fr = f_open(&fil, "file1.txt", FA_READ);
-    printf("File open result: %d\r\nContents:\r\n", fr);
-
-    
-    while (f_gets(line, sizeof line, &fil)) {
-        printf(line);
+    if (sd_flag_needs_autounmount) {
+        sd_flag_needs_autounmount = false;
+        sd_autounmount();
     }
-    printf("\r\n");
 
-    f_close(&fil);
+    if (sd_flag_was_automounted) {
+        sd_flag_was_automounted = false;
+        sd_automounted_hook();
+    }
 
-    printf("Returning from SD test\r\n");
+    if (sd_flag_was_autounmounted) {
+        sd_flag_was_autounmounted = false;
+        sd_autounmounted_hook();
+    }
+}
+
+void sd_statuschange_indication(uint8_t pattern) {
+    if (pattern == 1) {
+        for (uint8_t i = 0; i < 4; i++) {
+            LED_STATUS_OFF();
+            delay_ms(65);
+            LED_STATUS_ON();
+            delay_ms(30);
+        }
+    }
+    
+    if (pattern == 0) {
+        for (uint8_t i = 0; i < 2; i++) {
+            LED_STATUS_OFF();
+            delay_ms(250);
+            LED_STATUS_ON();
+            delay_ms(20);
+        }
+    }
+    
+}
+
+bool sd_inserted(void) {
+    if (sd_status & SD_STATUS_NODISK) {
+        return false;
+    } else {
+        return true;
+    }
+
+}
+
+bool sd_card_ready(void) {
+    if (sd_status == SD_STATUS_READY) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool sd_mounted(void) {
+    return sd_mountstate;
+}
+
+bool sd_mount(void) {
+    if (sd_inserted()) {
+        FRESULT mount_result = 0xFF;
+        mount_result = f_mount(&sdfs, "", 1);
+
+        if (mount_result == 0) {
+            sd_mountstate = true;
+            
+        } else {
+            sd_mountstate = false;
+        }
+
+        return sd_mountstate;
+    } else {
+        return false;
+    }
+}
+
+bool sd_unmount(void) {
+    FRESULT unmount_result = 0xFF;
+    unmount_result = f_mount(0, "", 0);
+
+    if (unmount_result == 0) {
+        sd_mountstate = false;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void sd_automount(void) {
+    if (sd_mount()) {
+        sd_automounted_hook();
+    }
+}
+
+void sd_autounmount(void) {
+    if (sd_unmount()) {
+        sd_autounmounted_hook();
+    }
+}
+
+void sd_scheduler(void) {
+    sd_scheduler_timer++;
+
+    disk_timerproc();
+    sd_status = disk_status(0);
+    
+    if (sd_scheduler_timer % 50 == 0) {
+        sd_scheduler_timer = 0;
+
+        if (!sd_mounted() && sd_inserted()) {
+            sd_flag_needs_automount = true;
+        }
+
+        if (!sd_inserted() && sd_mounted()) {
+            sd_flag_needs_autounmount = true;
+        }
+    }
 }
 
 // TODO: Get time from RTC or host
 DWORD get_fattime (void)
 {
     // Returns current time packed into a DWORD variable 
-    return    ((DWORD)(2013 - 1980) << 25)  // Year 2013 
+    return    ((DWORD)(2018 - 1980) << 25)  // Year 2013 
     | ((DWORD)8 << 21)              // Month 7 
     | ((DWORD)2 << 16)              // Mday 28 
     | ((DWORD)20 << 11)             // Hour 0..24
diff --git a/hardware/SD.h b/hardware/SD.h
index 440edbd..f56c80c 100644
--- a/hardware/SD.h
+++ b/hardware/SD.h
@@ -6,9 +6,21 @@
 #include <stdlib.h>
 #include "device.h"
 #include "util/time.h"
+#include "hardware/UserIO.h"
 #include "hardware/sdcard/ff.h"
 
+#define SD_STATUS_READY			  0x00
+#define SD_STATUS_NOINIT		  0x01
+#define SD_STATUS_NODISK		  0x02
+#define SD_STATUS_WRITEPROTECTED  0x04
+#define SD_STATUS_UNKNOWN		  0xFF
+
 void sd_init(void);
 void sd_test(void);
+void sd_scheduler(void);
+void sd_jobs(void);
+void sd_automount(void);
+void sd_autounmount(void);
+void sd_statuschange_indication(uint8_t pattern);
 
 #endif
\ No newline at end of file
diff --git a/hardware/Serial.c b/hardware/Serial.c
index 9efe7d8..7dc417c 100755
--- a/hardware/Serial.c
+++ b/hardware/Serial.c
@@ -1,5 +1,4 @@
 #include "Serial.h"
-#include <util/setbaud.h>
 #include <stdio.h>
 #include <string.h>
 
@@ -7,14 +6,7 @@ void serial_init(Serial *serial) {
     memset(serial, 0, sizeof(*serial));
     memset(serialBuf, 0, sizeof(serialBuf));
 
-    UBRR0H = UBRRH_VALUE;
-    UBRR0L = UBRRL_VALUE;
-
-    #if USE_2X
-        UCSR0A |= _BV(U2X0);
-    #else
-        UCSR0A &= ~(_BV(U2X0));
-    #endif
+    serial_setbaudrate_115200();
 
     // Set to 8-bit data, enable RX and TX, enable receive interrupt
     UCSR0C = _BV(UCSZ01) | _BV(UCSZ00);
@@ -58,4 +50,151 @@ ISR(USART0_RX_vect) {
         char c = uart0_getchar_nowait();
         fifo_push(&serialFIFO, c);
     }
-}
\ No newline at end of file
+}
+
+void serial_setbaudrate_1200(void) {
+    #undef BAUD
+    #define BAUD 1200
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_2400(void) {
+    #undef BAUD
+    #define BAUD 2400
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_4800(void) {
+    #undef BAUD
+    #define BAUD 4800
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+
+
+void serial_setbaudrate_9600(void) {
+    #undef BAUD
+    #define BAUD 9600
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_14400(void) {
+    #undef BAUD
+    #define BAUD 14400
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_19200(void) {
+    #undef BAUD
+    #define BAUD 19200
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_28800(void) {
+    #undef BAUD
+    #define BAUD 28800
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_38400(void) {
+    #undef BAUD
+    #define BAUD 38400
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_57600(void) {
+    #undef BAUD
+    #define BAUD 57600
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_76800(void) {
+    #undef BAUD
+    #define BAUD 76800
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_115200(void) {
+    #undef BAUD
+    #define BAUD 115200
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
+void serial_setbaudrate_230400(void) {
+    #undef BAUD
+    #define BAUD 230400
+    #include <util/setbaud.h>
+    UBRR0H = UBRRH_VALUE; UBRR0L = UBRRL_VALUE;
+    #if USE_2X
+        UCSR0A |= _BV(U2X0);
+    #else
+        UCSR0A &= ~(_BV(U2X0));
+    #endif
+}
+
diff --git a/hardware/Serial.h b/hardware/Serial.h
index 98da57a..a044a55 100755
--- a/hardware/Serial.h
+++ b/hardware/Serial.h
@@ -22,4 +22,17 @@ int uart0_putchar(char c, FILE *stream);
 int uart0_getchar(FILE *stream);
 char uart0_getchar_nowait(void);
 
+void serial_setbaudrate_1200(void);
+void serial_setbaudrate_2400(void);
+void serial_setbaudrate_4800(void);
+void serial_setbaudrate_9600(void);
+void serial_setbaudrate_14400(void);
+void serial_setbaudrate_19200(void);
+void serial_setbaudrate_28800(void);
+void serial_setbaudrate_38400(void);
+void serial_setbaudrate_57600(void);
+void serial_setbaudrate_76800(void);
+void serial_setbaudrate_115200(void);
+void serial_setbaudrate_230400(void);
+
 #endif
\ No newline at end of file
diff --git a/hardware/UserIO.c b/hardware/UserIO.c
new file mode 100644
index 0000000..f4e8287
--- /dev/null
+++ b/hardware/UserIO.c
@@ -0,0 +1,89 @@
+#include "UserIO.h"
+#include "hardware/Bluetooth.h"
+
+void usrio_init(void) {
+	USR_IO_DDR |= _BV(USR_IO_1) | _BV(USR_IO_2); 
+	// TODO: Add BT module detect and set up other pins accordingly
+}
+
+bool usrio_1(void) {
+	if (USR_IO_PORT & _BV(USR_IO_1)) {
+		return true;
+	} else {
+		return false;
+	}
+}
+
+bool usrio_2(void) {
+	if (USR_IO_PORT & _BV(USR_IO_2)) {
+		return true;
+	} else {
+		return false;
+	}
+}
+
+bool usrio_3(void) {
+	if (!bluetooth_enabled()) {
+		if (USR_IO_PORT & _BV(USR_IO_3)) {
+			return true;
+		} else {
+			return false;
+		}
+	} else {
+		return false;
+	}
+}
+
+bool usrio_4(void) {
+	if (!bluetooth_enabled()) {
+		if (USR_IO_PORT & _BV(USR_IO_4)) {
+			return true;
+		} else {
+			return false;
+		}
+	} else {
+		return false;
+	}
+}
+
+void usrio_1_on(void) {
+	USR_IO_PORT |= _BV(USR_IO_1);
+}
+
+void usrio_2_on(void) {
+	USR_IO_PORT |= _BV(USR_IO_2);
+}
+
+void usrio_3_on(void) {
+	if (!bluetooth_enabled()) { USR_IO_PORT |= _BV(USR_IO_3); }
+}
+
+void usrio_4_on(void) {
+	if (!bluetooth_enabled()) { USR_IO_PORT |= _BV(USR_IO_4); }
+}
+
+void usrio_1_off(void) {
+	USR_IO_PORT &= ~_BV(USR_IO_1);
+}
+
+void usrio_2_off(void) {
+	USR_IO_PORT &= ~_BV(USR_IO_2);
+}
+
+void usrio_3_off(void) {
+	if (!bluetooth_enabled()) { USR_IO_PORT |= _BV(USR_IO_3); }
+}
+
+void usrio_4_off(void) {
+	if (!bluetooth_enabled()) { USR_IO_PORT |= _BV(USR_IO_4); }
+}
+
+void usrio_1_toggle(void) {
+	if (usrio_1()) {
+		usrio_1_off();
+	} else {
+		usrio_1_on();
+	}
+}
+
+// TODO: Add ADC read support to UserIO
\ No newline at end of file
diff --git a/hardware/UserIO.h b/hardware/UserIO.h
new file mode 100644
index 0000000..617abfd
--- /dev/null
+++ b/hardware/UserIO.h
@@ -0,0 +1,31 @@
+#ifndef USRIO_H
+#define USRIO_H
+
+#include <avr/io.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include "device.h"
+
+void usrio_init(void);
+
+bool usrio_1(void);
+bool usrio_2(void);
+bool usrio_3(void);
+bool usrio_4(void);
+
+void usrio_1_on(void);
+void usrio_2_on(void);
+void usrio_3_on(void);
+void usrio_4_on(void);
+
+void usrio_1_off(void);
+void usrio_2_off(void);
+void usrio_3_off(void);
+void usrio_4_off(void);
+
+void usrio_1_toggle(void);
+void usrio_2_toggle(void);
+void usrio_3_toggle(void);
+void usrio_4_toggle(void);
+
+#endif
\ No newline at end of file
diff --git a/hardware/sdcard/diskio.c b/hardware/sdcard/diskio.c
index ba2b730..2830ffd 100755
--- a/hardware/sdcard/diskio.c
+++ b/hardware/sdcard/diskio.c
@@ -56,14 +56,9 @@
 #define CMD58	(58)		/* READ_OCR */
 
 
-static volatile
-DSTATUS Stat = STA_NOINIT;	/* Disk status */
-
-static volatile
-BYTE Timer1, Timer2;	/* 100Hz decrement timer */
-
-static
-BYTE CardType;			/* Card type flags */
+static volatile DSTATUS Stat = STA_NOINIT;	/* Disk status */
+static BYTE CardType;			/* Card type flags */
+static volatile BYTE Timer1, Timer2;	/* 100Hz decrement timer */
 
 
 /*-----------------------------------------------------------------------*/
diff --git a/hardware/sdcard/diskio.h b/hardware/sdcard/diskio.h
index 8ad1b2d..c986669 100755
--- a/hardware/sdcard/diskio.h
+++ b/hardware/sdcard/diskio.h
@@ -37,7 +37,7 @@ DRESULT disk_read (BYTE pdrv, BYTE* buff, DWORD sector, UINT count);
 DRESULT disk_write (BYTE pdrv, const BYTE* buff, DWORD sector, UINT count);
 DRESULT disk_ioctl (BYTE pdrv, BYTE cmd, void* buff);
 
-void disk_timerproc (void);
+void disk_timerproc(void);
 
 
 /* Disk Status Bits (DSTATUS) */
diff --git a/main.c b/main.c
index 52f1004..681a6e7 100755
--- a/main.c
+++ b/main.c
@@ -1,6 +1,8 @@
 #include <stdbool.h>
 #include <avr/io.h>
 #include <avr/wdt.h>
+#include <avr/pgmspace.h>
+#include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
 
@@ -9,7 +11,9 @@
 #include "hardware/AFSK.h"
 #include "hardware/Serial.h"
 #include "hardware/LED.h"
+#include "hardware/UserIO.h"
 #include "hardware/SD.h"
+#include "hardware/Bluetooth.h"
 #include "protocol/AX25.h"
 #include "protocol/KISS.h"
 #include "util/time.h"
@@ -40,8 +44,8 @@ void system_check(void) {
     } else if (OPTIBOOT_MCUSR & (1<<WDRF)) {
       boot_vector = START_FROM_BOOTLOADER;
     } else {
-        printf("Error, indeterminate boot vector %d\r\n", OPTIBOOT_MCUSR);
-        printf("System start has been halted\r\n");
+        printf(PSTR("Error, indeterminate boot vector %d\r\n"), OPTIBOOT_MCUSR);
+        printf(PSTR("System start has been halted\r\n"));
         while (true) {
             LED_TX_ON();
             LED_COM_ON();
@@ -64,7 +68,8 @@ void init(void) {
     ax25_init(&AX25, &modem, &modem.fd, ax25_callback);
     kiss_init(&AX25, &modem, &serial);
     sd_init();
-    //sd_test(); // TODO: Remove
+    bluetooth_init();
+    usrio_init();
 
     system_check();
 }
@@ -77,6 +82,7 @@ int main (void) {
         ax25_poll(&AX25);
         kiss_poll();
         kiss_csma();
+        sd_jobs();
     }
 
     return(0);