diff --git a/src/main.c b/src/main.c
index 34bac94..e37c006 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,22 +1,12 @@
 #include "common.h"
 #include "serial.h"
 
-#include <stdio.h>
 #include <avr/interrupt.h>
 
-#define MAX_CMDBUF 256
-
 int main(void)
 {
 	unsigned long i;
-	char buf[128];
-	char *cmd;
-
-	// UART command buffer
-	cmd = malloc(MAX_CMDBUF);
-	if (cmd == NULL) {
-		return 1;
-	}
+	char ch;
 
 	// Handle USART_RXC interrupt
 	UCSRB |= (1 << RXCIE);
@@ -27,13 +17,10 @@ int main(void)
 	for (i = 1;; i++) {
 		if (i >= 99999) i = 1;
 
-		sprintf(buf, "[CORE] Fetching sensors #%05lu...\r\n", i);
-		USART_Write(buf);
-		USART_Update();
-
-		// USART_Read(&cmd, MAX_CMDBUF);
-		// sprintf(buf, "[CORE] Checking cmdbuf=\"%s\"\r\n", cmd);
-		// USART_Write(buf);
+		ch = USART_GetChar();
+		USART_Printf("[CORE] Fetching sensors #%05lu...\r\n", i);
+		USART_Printf("[CORE] USART_GetChar() = '%c'\r\n",
+			(ch) ? ch : ' ');
 
 		Sleep(3000);
 	}
diff --git a/src/serial.c b/src/serial.c
index 9c0acb6..b011fda 100644
--- a/src/serial.c
+++ b/src/serial.c
@@ -1,62 +1,77 @@
-#include "common.h"
 #include "serial.h"
+#include "common.h"
 
+#include <stdio.h>
+#include <stdarg.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
-#include <stdio.h>
 
+#define USART_BUFSIZE  512
 #define USART_BAUDRATE 9600
+#define USART_RXBUF_MASK (USART_BUFSIZE - 1)
+#define USART_BAUD_PRESCALE ((((F_CPU / 16) + \
+	(USART_BAUDRATE / 2)) / (USART_BAUDRATE)) - 1)
 
-// TODO: Use ring buffer! https://stackoverflow.com/a/1771607
-
-// Ensure closest fit for target baudrate
-//#define BAUD_PRESCALE (((F_CPU / (USART_BAUDRATE * 16UL))) - 1)
-#define BAUD_PRESCALE ((((F_CPU / 16) + (USART_BAUDRATE / 2)) \
-        / (USART_BAUDRATE)) - 1)
-
-static size_t cmdlen;
-static char cmdbuf[256];
+static unsigned char rxbuf[USART_BUFSIZE];
+static unsigned char rxhead, rxtail;
 
 void USART_Init(void)
 {
+	rxhead = 0;
+	rxtail = 0;
+
 	UCSRB |= BIT(RXEN) | BIT(TXEN); // Enable rx and tx circuitry
 	UCSRC |= BIT(URSEL) | BIT(UCSZ0) | BIT(UCSZ1); // 8-bit chars
-	UBRRH = (BAUD_PRESCALE >> 8); // Baud Rate upper byte
-	UBRRL = BAUD_PRESCALE; // Baud rate lower byte
+	UBRRH = (USART_BAUD_PRESCALE >> 8); // Baud Rate upper byte
+	UBRRL = USART_BAUD_PRESCALE; // Baud rate lower byte
 }
 
-void USART_Update(void)
+char USART_GetChar(void)
 {
-	char buf[512];
-	sprintf(buf, "[CORE] CMDBUF='%s'\r\n", cmdbuf);
-	USART_Write(buf);
+	unsigned char newtail;
+
+	if (rxhead == rxtail) {
+		return '\0'; // Non-blocking
+	}
+
+	newtail = (rxtail + 1) & USART_RXBUF_MASK;
+	rxtail = newtail;
+
+	return rxbuf[newtail];
 }
 
-void USART_Write(const char *buf)
+void USART_PutChar(const char ch)
 {
-	int n = 0;
+	// Wait until UDR can receive data
+	while ((UCSRA & BIT(UDRE)) == 0);
+	UDR = ch;
+}
 
-	while (buf[n] != '\0') {
-		// Wait until UDR can receive data
-		while ((UCSRA & BIT(UDRE)) == 0);
-		UDR = buf[n++];
+void USART_Printf(const char *fmt, ...)
+{
+	char buf[256], *ptr;
+	va_list args;
+
+	va_start(args, fmt);
+	vsprintf(buf, fmt, args);
+	va_end(args);
+
+	ptr = buf;
+	while (*ptr != '\0') {
+		USART_PutChar(*ptr++);
 	}
 }
 
-// USART RX Interrupt
+// Interrupt Handler
 ISR(USART_RXC_vect)
 {
-	char ch;
+	unsigned char newhead;
 
-	ch = UDR;
-
-	// Exceeded buffer capacity?
-	if (cmdlen >= sizeof(cmdbuf)) {
-		if (ch == '\n') {
-			cmdlen = 0;
-		}
-		return;
+	newhead = (rxhead + 1) & USART_RXBUF_MASK;
+	if (newhead == rxtail) {
+		// TODO: Handle overflow
+	} else {
+		rxhead = newhead;
+		rxbuf[newhead] = UDR;
 	}
-
-	cmdbuf[cmdlen++] = ch;
 }
diff --git a/src/serial.h b/src/serial.h
index cf19b41..abd7145 100644
--- a/src/serial.h
+++ b/src/serial.h
@@ -2,8 +2,8 @@
 #define MAD_SERIAL_H
 
 void USART_Init(void);
-void USART_Update(void);
-//void USART_Read(char **buf, int size);
-void USART_Write(const char *buf);
+char USART_GetChar(void);
+void USART_PutChar(const char ch);
+void USART_Printf(const char *fmt, ...);
 
 #endif // MAD_SERIAL_H