diff --git a/hardware/AFSK.c b/hardware/AFSK.c
index d39200b..d59d7fd 100755
--- a/hardware/AFSK.c
+++ b/hardware/AFSK.c
@@ -106,13 +106,13 @@ void AFSK_init(Afsk *afsk) {
     afsk->silentSamples = 0;
 
     // Initialise FIFO buffers
-    fifo_init(&afsk->delayFifo, (uint8_t *)afsk->delayBuf, sizeof(afsk->delayBuf));
-    fifo_init(&afsk->rxFifo, afsk->rxBuf, sizeof(afsk->rxBuf));
-    fifo_init(&afsk->txFifo, afsk->txBuf, sizeof(afsk->txBuf));
+    fifo_init(&afsk->delayFifo, (uint8_t *)afsk->delayBuf, sizeof(afsk->delayBuf)-1);
+    fifo_init(&afsk->rxFifo, afsk->rxBuf, CONFIG_AFSK_RX_BUFLEN);
+    fifo_init(&afsk->txFifo, afsk->txBuf, CONFIG_AFSK_TX_BUFLEN);
 
     // Fill delay FIFO with zeroes
-    for (int i = 0; i<ADC_SAMPLESPERBIT / 2; i++) {
-        fifo_push(&afsk->delayFifo, 0);
+    while (fifo_isfull(&afsk->delayFifo)) {
+        fifo_push_locked(&afsk->delayFifo, 0);
     }
 
     AFSK_hw_init();
diff --git a/hardware/AFSK.h b/hardware/AFSK.h
index 5c82e2a..d2b5fa6 100755
--- a/hardware/AFSK.h
+++ b/hardware/AFSK.h
@@ -182,18 +182,18 @@ typedef struct Afsk
     // Demodulation values
     FIFOBuffer delayFifo;                   // Delayed FIFO for frequency discrimination
     #if BITRATE == 1200
-        int8_t delayBuf[ADC_SAMPLESPERBIT / 2 + 1]; // Actual data storage for said FIFO
+        int8_t delayBuf[(ADC_SAMPLESPERBIT / 2 - 1) + 1]; // Actual data storage for said FIFO
     #elif BITRATE == 2400
-        int8_t delayBuf[7 + 1];
+        int8_t delayBuf[(7) + 1];
     #elif BITRATE == 300
-        int8_t delayBuf[9 + 1];
+        int8_t delayBuf[(9) + 1];
     #endif
 
     FIFOBuffer rxFifo;                      // FIFO for received data
-    uint8_t rxBuf[CONFIG_AFSK_RX_BUFLEN];   // Actual data storage for said FIFO
+    uint8_t rxBuf[CONFIG_AFSK_RX_BUFLEN+1]; // Actual data storage for said FIFO
 
     FIFOBuffer txFifo;                      // FIFO for transmit data
-    uint8_t txBuf[CONFIG_AFSK_TX_BUFLEN];   // Actual data storage for said FIFO
+    uint8_t txBuf[CONFIG_AFSK_TX_BUFLEN+1]; // Actual data storage for said FIFO
 
     int16_t iirX[2];                        // IIR Filter X cells
     int16_t iirY[2];                        // IIR Filter Y cells
diff --git a/hardware/Serial.c b/hardware/Serial.c
index ac201a5..dadbad0 100755
--- a/hardware/Serial.c
+++ b/hardware/Serial.c
@@ -84,7 +84,7 @@ ISR(USART0_RX_vect) {
             char c = uart0_getchar_nowait();
             fifo_push(&uart0FIFO, c);
         } else {
-            uart0_getchar_nowait();
+            //uart0_getchar_nowait();
         }
     }
 }
@@ -95,7 +95,7 @@ ISR(USART1_RX_vect) {
             char c = uart1_getchar_nowait();
             fifo_push(&uart1FIFO, c);
         } else {
-            uart1_getchar_nowait();
+            //uart1_getchar_nowait();
         }
     }
 }
diff --git a/protocol/KISS.c b/protocol/KISS.c
index d6fa36e..8ecbffe 100755
--- a/protocol/KISS.c
+++ b/protocol/KISS.c
@@ -53,10 +53,10 @@ void kiss_init(AX25Ctx *ax25, Afsk *afsk, Serial *ser) {
 
     memset(packet_queue, 0, sizeof(packet_queue));
 
-    memset(packet_starts_buf, 0, sizeof(packet_starts));
+    memset(packet_starts_buf, 0, sizeof(packet_starts_buf));
     fifo16_init(&packet_starts, packet_starts_buf, CONFIG_QUEUE_MAX_LENGTH);
     
-    memset(packet_lengths_buf, 0, sizeof(packet_lengths));
+    memset(packet_lengths_buf, 0, sizeof(packet_lengths_buf));
     fifo16_init(&packet_lengths, packet_lengths_buf, CONFIG_QUEUE_MAX_LENGTH);
 }
 
@@ -340,7 +340,7 @@ void kiss_flushQueue(void) {
         queue_flushing = true;
 
         size_t processed = 0;
-        for (size_t n = 0; n < queue_height; n++) {
+        while (!fifo16_isempty_locked(&packet_starts)) {
             size_t start = fifo16_pop_locked(&packet_starts);
             size_t length = fifo16_pop_locked(&packet_lengths);
 
@@ -460,26 +460,24 @@ void kiss_serialCallback(uint8_t sbyte) {
     if (IN_FRAME && sbyte == FEND && command == CMD_DATA) {
         IN_FRAME = false;
 
-        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 (!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(&packet_starts, s);
-                    fifo16_push(&packet_lengths, l);
+                fifo16_push(&packet_starts, s);
+                fifo16_push(&packet_lengths, l);
 
-                    current_packet_start = queue_cursor;
-                }
+                current_packet_start = queue_cursor;
             }
         }
         
@@ -501,7 +499,7 @@ void kiss_serialCallback(uint8_t sbyte) {
                     if (sbyte == TFESC) sbyte = FESC;
                     ESCAPE = false;
                 }
-                if (queue_height < CONFIG_QUEUE_MAX_LENGTH && queued_bytes < CONFIG_QUEUE_SIZE) {
+                if (!fifo16_isfull(&packet_starts) && queued_bytes < CONFIG_QUEUE_SIZE) {
                     queued_bytes++;
                     packet_queue[queue_cursor++] = sbyte;
                     if (queue_cursor == CONFIG_QUEUE_SIZE) queue_cursor = 0;