diff --git a/device.h b/device.h
index e060705..c45b7c2 100755
--- a/device.h
+++ b/device.h
@@ -5,7 +5,7 @@
 
 // Version info
 #define MAJ_VERSION  0x01
-#define MIN_VERSION  0x02
+#define MIN_VERSION  0x03
 
 // CPU settings
 #define TARGET_CPU m1284p
diff --git a/hardware/Serial.c b/hardware/Serial.c
index b0291d3..cbffdb0 100755
--- a/hardware/Serial.c
+++ b/hardware/Serial.c
@@ -26,8 +26,8 @@ void serial_init(Serial *serial) {
     serial->uart0 = uart0_fd;
     serial->uart1 = uart1_fd;
 
-    fifo_init(&uart0FIFO, uart0Buf, sizeof(uart0Buf));
-    fifo_init(&uart1FIFO, uart1Buf, sizeof(uart1Buf));
+    fifo_init(&uart0FIFO, uart0Buf, CONFIG_UART0_BUFFER_SIZE);
+    fifo_init(&uart1FIFO, uart1Buf, CONFIG_UART1_BUFFER_SIZE);
 
 }
 
@@ -82,7 +82,7 @@ ISR(USART0_RX_vect) {
         LED_COM_ON();
         if (!fifo_isfull(&uart0FIFO)) {
             char c = uart0_getchar_nowait();
-            fifo_push(&uart0FIFO, c);
+            fifo_push_locked(&uart0FIFO, c);
         } else {
             uart0_getchar_nowait();
         }
diff --git a/hardware/Serial.h b/hardware/Serial.h
index 417c47e..6ad858a 100755
--- a/hardware/Serial.h
+++ b/hardware/Serial.h
@@ -15,10 +15,10 @@ typedef struct Serial {
 } Serial;
 
 FIFOBuffer uart0FIFO;
-uint8_t uart0Buf[CONFIG_UART0_BUFFER_SIZE];
+uint8_t uart0Buf[CONFIG_UART0_BUFFER_SIZE+1];
 
 FIFOBuffer uart1FIFO;
-uint8_t uart1Buf[CONFIG_UART1_BUFFER_SIZE];
+uint8_t uart1Buf[CONFIG_UART1_BUFFER_SIZE+1];
 
 void serial_init(Serial *serial);
 bool serial_available(uint8_t index);
diff --git a/protocol/KISS.c b/protocol/KISS.c
index c8e2310..d6fa36e 100755
--- a/protocol/KISS.c
+++ b/protocol/KISS.c
@@ -460,24 +460,26 @@ void kiss_serialCallback(uint8_t sbyte) {
     if (IN_FRAME && sbyte == FEND && command == CMD_DATA) {
         IN_FRAME = false;
 
-        if (queue_height < CONFIG_QUEUE_MAX_LENGTH && queued_bytes < CONFIG_QUEUE_SIZE) {
-            size_t s = current_packet_start;
-            size_t e = queue_cursor-1; if (e == -1) e = CONFIG_QUEUE_SIZE-1;
-            size_t l;
+        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+            if (!fifo16_isfull(&packet_starts) && queued_bytes < CONFIG_QUEUE_SIZE) {
+                size_t s = current_packet_start;
+                size_t e = queue_cursor-1; if (e == -1) e = CONFIG_QUEUE_SIZE-1;
+                size_t l;
 
-            if (s != e) {
-                l = (s < e) ? e - s + 1 : CONFIG_QUEUE_SIZE - s + e + 1;
-            } else {
-                l = 1;
-            }
+                if (s != e) {
+                    l = (s < e) ? e - s + 1 : CONFIG_QUEUE_SIZE - s + e + 1;
+                } else {
+                    l = 1;
+                }
 
-            if (l >= AX25_MIN_PAYLOAD) {
-                queue_height++;
+                if (l >= AX25_MIN_PAYLOAD) {
+                    queue_height++;
 
-                fifo16_push_locked(&packet_starts, s);
-                fifo16_push_locked(&packet_lengths, l);
+                    fifo16_push(&packet_starts, s);
+                    fifo16_push(&packet_lengths, l);
 
-                current_packet_start = queue_cursor;
+                    current_packet_start = queue_cursor;
+                }
             }
         }