diff --git a/device.h b/device.h
index c93c530..a5ba0d0 100755
--- a/device.h
+++ b/device.h
@@ -20,11 +20,17 @@
 #define BAUD 115200
 #define SERIAL_DEBUG false
 #define TX_MAXWAIT 2UL
+#define CONFIG_QUEUE_SIZE 7500
+#define CONFIG_QUEUE_MAX_LENGTH 15
+#define CONFIG_SERIAL_BUFFER_SIZE 1532 // TODO: Tune this, what is actually required?
 
 // CSMA Settings
 #define CONFIG_FULL_DUPLEX false  // TODO: Actually implement fdx
 #define CONFIG_CSMA_P 255
 
+#define AX25_MIN_FRAME_LEN 1
+#define AX25_MAX_FRAME_LEN 1532
+
 // Packet settings
 #define CONFIG_PASSALL false
 
diff --git a/hardware/AFSK.c b/hardware/AFSK.c
index f24f049..7ae692b 100755
--- a/hardware/AFSK.c
+++ b/hardware/AFSK.c
@@ -1,6 +1,7 @@
 #include <string.h>
 #include "AFSK.h"
 #include "util/time.h"
+#include "protocol/KISS.h"
 
 // TODO: Remove testing vars ////
 #define SAMPLES_TO_CAPTURE 128
@@ -618,33 +619,10 @@ ISR(TIMER3_CAPT_vect) {
 
 ISR(ADC_vect) {
     TIFR1 = _BV(ICF1);
-
+    
     if (CONFIG_FULL_DUPLEX || !hw_afsk_dac_isr) {
         AFSK_adc_isr(AFSK_modem, (ADCH - 128));
     }
 
     ++_clock;
-
-    /*
-    // TODO: Remove these debug sample collection functions
-
-    //DAC_PORT ^= 0xFF;
-    //DAC_PORT = ADCH;
-
-    if (capturedsamples == SAMPLES_TO_CAPTURE) {
-        printf("--- Dumping samples ---");
-        for (ticks_t i = 0; i < SAMPLES_TO_CAPTURE; i++) {
-            uint8_t c = samplebuf[i];
-            printf("%d\r\n", c);
-        }
-        printf("-------- Done ---------");
-    }
-    DAC_PORT ^= 0xFF;
-    if (capturedsamples < SAMPLES_TO_CAPTURE) {
-        samplebuf[capturedsamples++] = ADCH;
-        // Clear Input Capture Flag from Timer1 Interrupt Flag Register
-        // to allow for next capture interrupt to occur
-        TIFR1 = _BV(ICF1);
-    }
-    */
 }
\ No newline at end of file
diff --git a/hardware/AFSK.h b/hardware/AFSK.h
index 68bfb71..cc93eba 100755
--- a/hardware/AFSK.h
+++ b/hardware/AFSK.h
@@ -39,7 +39,7 @@ inline static uint8_t sinSample(uint16_t i) {
 #define CPU_FREQ F_CPU
 
 
-#define BITRATE 1200
+#define BITRATE 2400
 
 #if BITRATE == 300
     #define CONFIG_ADC_SAMPLERATE 9600UL
@@ -53,8 +53,8 @@ inline static uint8_t sinSample(uint16_t i) {
 #endif
 
 
-#define CONFIG_AFSK_RX_BUFLEN CONFIG_ADC_SAMPLERATE/75
-#define CONFIG_AFSK_TX_BUFLEN CONFIG_ADC_SAMPLERATE/75
+#define CONFIG_AFSK_RX_BUFLEN AX25_MAX_FRAME_LEN
+#define CONFIG_AFSK_TX_BUFLEN AX25_MAX_FRAME_LEN
 #define CONFIG_AFSK_RXTIMEOUT 0
 
 #define CONFIG_AFSK_PREAMBLE_LEN 150UL
diff --git a/hardware/Serial.c b/hardware/Serial.c
index 769d8f1..bfc9d42 100755
--- a/hardware/Serial.c
+++ b/hardware/Serial.c
@@ -5,6 +5,8 @@
 
 void serial_init(Serial *serial) {
     memset(serial, 0, sizeof(*serial));
+    memset(serialBuf, 0, sizeof(serialBuf));
+
     UBRR0H = UBRRH_VALUE;
     UBRR0L = UBRRL_VALUE;
 
@@ -14,13 +16,15 @@ void serial_init(Serial *serial) {
         UCSR0A &= ~(_BV(U2X0));
     #endif
 
-    // Set to 8-bit data, enable RX and TX
+    // Set to 8-bit data, enable RX and TX, enable receive interrupt
     UCSR0C = _BV(UCSZ01) | _BV(UCSZ00);
-    UCSR0B = _BV(RXEN0) | _BV(TXEN0);
+    UCSR0B = _BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0);
 
     FILE uart0_fd = FDEV_SETUP_STREAM(uart0_putchar, uart0_getchar, _FDEV_SETUP_RW);
     
     serial->uart0 = uart0_fd;
+
+    fifo_init(&serialFIFO, serialBuf, sizeof(serialBuf));
 }
 
 bool serial_available(uint8_t index) {
@@ -45,4 +49,11 @@ int uart0_getchar(FILE *stream) {
 char uart0_getchar_nowait(void) {
     if (!(UCSR0A & _BV(RXC0))) return EOF;
     return UDR0;
+}
+
+ISR(USART0_RX_vect) {
+    if (serial_available(0)) {
+        char c = uart0_getchar_nowait();
+        fifo_push(&serialFIFO, c);
+    }
 }
\ No newline at end of file
diff --git a/hardware/Serial.h b/hardware/Serial.h
index 22f69b0..70e1201 100755
--- a/hardware/Serial.h
+++ b/hardware/Serial.h
@@ -6,11 +6,15 @@
 #include <stdio.h>
 #include <stdbool.h>
 #include <avr/io.h>
+#include "util/FIFO.h"
 
 typedef struct Serial {
     FILE uart0;
 } Serial;
 
+FIFOBuffer serialFIFO;
+uint8_t serialBuf[CONFIG_SERIAL_BUFFER_SIZE];
+
 void serial_init(Serial *serial);
 bool serial_available(uint8_t index);
 int uart0_putchar(char c, FILE *stream);
diff --git a/main.c b/main.c
index e2731c1..822ee6a 100755
--- a/main.c
+++ b/main.c
@@ -38,11 +38,8 @@ int main (void) {
 
     while (true) {
         ax25_poll(&AX25);
-        
-        if (serial_available(0)) {
-            char sbyte = uart0_getchar_nowait();
-            kiss_serialCallback(sbyte);
-        }
+        kiss_poll();
+        kiss_csma();
     }
 
     return(0);
diff --git a/protocol/AX25.c b/protocol/AX25.c
index 9fd25ac..0ed16c7 100755
--- a/protocol/AX25.c
+++ b/protocol/AX25.c
@@ -5,7 +5,8 @@
 #include "AX25.h"
 #include "protocol/HDLC.h"
 #include "util/CRC-CCIT.h"
-#include "../hardware/AFSK.h"
+#include "hardware/AFSK.h"
+#include "protocol/KISS.h"
 
 #define countof(a) sizeof(a)/sizeof(a[0])
 #define MIN(a,b) ({ typeof(a) _a = (a); typeof(b) _b = (b); ((typeof(_a))((_a < _b) ? _a : _b)); })
@@ -76,7 +77,10 @@ void ax25_sendRaw(AX25Ctx *ctx, void *_buf, size_t len) {
     ctx->crc_out = CRC_CCIT_INIT_VAL;
     fputc(HDLC_FLAG, ctx->ch);
     const uint8_t *buf = (const uint8_t *)_buf;
-    while (len--) ax25_putchar(ctx, *buf++);
+    while (len--) {
+        ax25_putchar(ctx, *buf++);
+        kiss_poll();
+    }
 
     uint8_t crcl = (ctx->crc_out & 0xff) ^ 0xff;
     uint8_t crch = (ctx->crc_out >> 8) ^ 0xff;
diff --git a/protocol/AX25.h b/protocol/AX25.h
index 40862cc..e91b3be 100755
--- a/protocol/AX25.h
+++ b/protocol/AX25.h
@@ -6,10 +6,6 @@
 #include "device.h"
 #include "hardware/AFSK.h"
 
-#define AX25_MIN_FRAME_LEN 1
-#define AX25_MAX_FRAME_LEN 1532
-
-
 #define AX25_CRC_CORRECT  0xF0B8
 
 #define AX25_CTRL_UI      0x03
diff --git a/protocol/KISS.c b/protocol/KISS.c
index 3e770ce..6ab354a 100755
--- a/protocol/KISS.c
+++ b/protocol/KISS.c
@@ -2,16 +2,29 @@
 #include <string.h>
 
 #include "device.h"
+#include "hardware/Serial.h"
+#include "util/FIFO16.h"
 #include "KISS.h"
 
-static uint8_t serialBuffer[AX25_MAX_FRAME_LEN]; // Buffer for holding incoming serial data
+uint8_t packet_queue[CONFIG_QUEUE_SIZE];
+uint8_t tx_buffer[AX25_MAX_FRAME_LEN];
+volatile uint8_t queue_height = 0;
+volatile size_t queued_bytes = 0;
+volatile size_t queue_cursor = 0;
+volatile size_t current_packet_start = 0;
+
+FIFOBuffer16 packet_starts;
+size_t packet_starts_buf[CONFIG_QUEUE_MAX_LENGTH+1];
+
+FIFOBuffer16 packet_lengths;
+size_t packet_lengths_buf[CONFIG_QUEUE_MAX_LENGTH+1];
+
 AX25Ctx *ax25ctx;
 Afsk *channel;
 Serial *serial;
 size_t frame_len;
 bool IN_FRAME;
 bool ESCAPE;
-bool FLOWCONTROL;
 
 uint8_t command = CMD_UNKNOWN;
 unsigned long custom_preamble = CONFIG_AFSK_PREAMBLE_LEN;
@@ -24,7 +37,20 @@ void kiss_init(AX25Ctx *ax25, Afsk *afsk, Serial *ser) {
     ax25ctx = ax25;
     serial = ser;
     channel = afsk;
-    FLOWCONTROL = false;
+
+    memset(packet_queue, 0, sizeof(packet_queue));
+    memset(packet_starts_buf, 0, sizeof(packet_starts));
+    memset(packet_lengths_buf, 0, sizeof(packet_lengths));
+
+    fifo16_init(&packet_starts, packet_starts_buf, sizeof(packet_starts_buf));
+    fifo16_init(&packet_lengths, packet_lengths_buf, sizeof(packet_lengths_buf));
+}
+
+void kiss_poll(void) {
+    while (!fifo_isempty_locked(&serialFIFO)) {
+        char sbyte = fifo_pop_locked(&serialFIFO);
+        kiss_serialCallback(sbyte);
+    }
 }
 
 // TODO: Remove debug functions
@@ -51,60 +77,95 @@ void kiss_messageCallback(AX25Ctx *ctx) {
     
 }
 
-void kiss_csma(AX25Ctx *ctx, uint8_t *buf, size_t len) {
-    bool sent = false;
-    // TODO: Determine if this is to be removed
-    // if (CONFIG_AFSK_TXWAIT > 0) {
-    //     ticks_t wait_start = timer_clock();
-    //     long wait_ticks = ms_to_ticks(CONFIG_AFSK_TXWAIT);
-    //     while (timer_clock() - wait_start < wait_ticks) {
-    //         cpu_relax();
-    //     }
-    // }
-    while (!sent) {
-        if(CONFIG_FULL_DUPLEX || !channel->hdlc.dcd) {
-            uint8_t tp = rand() & 0xFF;
-            if (tp < p) {
-                ax25_sendRaw(ctx, buf, len);
-                sent = true;
+void kiss_csma(void) {
+    if (queue_height > 0) {
+        if (!channel->hdlc.dcd) {
+            if (p == 255) {
+                kiss_flushQueue();
             } else {
-                ticks_t start = timer_clock();
-                long slot_ticks = ms_to_ticks(slotTime);
-                while (timer_clock() - start < slot_ticks) {
-                    cpu_relax();
-                }
-            }
-        } else {
-            while (!sent && channel->hdlc.receiving) {
-                // Continously poll the modem for data
-                // while waiting, so we don't overrun
-                // receive buffers
-                ax25_poll(ax25ctx);
-
-                if (channel->status != 0) {
-                    // If an overflow or other error
-                    // occurs, we'll back off and drop
-                    // this packet silently.
-                    channel->status = 0;
-                    sent = true;
-                }
+                // TODO: Implement real CSMA
             }
         }
     }
+}
 
-    if (FLOWCONTROL) {
-        while (!ctx->ready_for_data) { /* Wait */ }
-        fputc(FEND, &serial->uart0);
-        fputc(CMD_READY, &serial->uart0);
-        fputc(0x01, &serial->uart0);
-        fputc(FEND, &serial->uart0);
+// TODO: Remove this
+void kiss_flushQueueDebug(void) {
+    printf("Queue height %d\r\n", queue_height);
+    for (size_t n = 0; n < queue_height; n++) {
+        size_t start = fifo16_pop(&packet_starts);
+        size_t length = fifo16_pop(&packet_lengths);
+
+        printf("--- Packet %d, %d bytes ---\r\n", n+1, length);
+        for (size_t i = 0; i < length; i++) {
+            size_t pos = (start+i)%CONFIG_QUEUE_SIZE;
+            printf("%02x", packet_queue[pos]);
+        }
+        printf("\r\n\r\n");
     }
+    queue_height = 0;
+    queued_bytes = 0;
+}
+
+volatile bool queue_flushing = false;
+void kiss_flushQueue(void) {
+    if (!queue_flushing) {
+        queue_flushing = true;
+
+        size_t processed = 0;
+        for (size_t n = 0; n < queue_height; n++) {
+            size_t start = fifo16_pop_locked(&packet_starts);
+            size_t length = fifo16_pop_locked(&packet_lengths);
+
+            kiss_poll();
+            for (size_t i = 0; i < length; i++) {
+                size_t pos = (start+i)%CONFIG_QUEUE_SIZE;
+                tx_buffer[i] = packet_queue[pos];
+            }
+
+            ax25_sendRaw(ax25ctx, tx_buffer, length);
+            processed++;
+        }
+
+        if (processed < queue_height) {
+            while (true) {
+                LED_TX_ON();
+                LED_RX_ON();
+            }
+        }
+        printf("Processed %d\r\n", processed);
+
+        queue_height = 0;
+        queued_bytes = 0;
+        queue_flushing = false;
+    }
+}
+
+uint8_t kiss_queuedPackets(void) {
+    return 0;
+}
+
+bool kiss_queueIsFull(void) {
+    return false;
 }
 
 void kiss_serialCallback(uint8_t sbyte) {
     if (IN_FRAME && sbyte == FEND && command == CMD_DATA) {
         IN_FRAME = false;
-        kiss_csma(ax25ctx, serialBuffer, frame_len);
+
+        if (queue_height < CONFIG_QUEUE_MAX_LENGTH && queued_bytes < CONFIG_QUEUE_SIZE) {
+            queue_height++;
+            size_t s = current_packet_start;
+            size_t e = queue_cursor-1; if (e == -1) e = CONFIG_QUEUE_SIZE-1;
+            size_t l = (s < e) ? e - s + 1 : CONFIG_QUEUE_SIZE - s + e + 1;
+
+            fifo16_push_locked(&packet_starts, s);
+            fifo16_push_locked(&packet_lengths, l);
+
+            current_packet_start = queue_cursor;
+            printf("Queue height %d\r\n", queue_height);
+        }
+        
     } else if (sbyte == FEND) {
         IN_FRAME = true;
         command = CMD_UNKNOWN;
@@ -116,6 +177,7 @@ void kiss_serialCallback(uint8_t sbyte) {
             // strip off the port nibble of the command byte
             sbyte = sbyte & 0x0F;
             command = sbyte;
+            if (command == CMD_DATA) current_packet_start = queue_cursor;
         } else if (command == CMD_DATA) {
             if (sbyte == FESC) {
                 ESCAPE = true;
@@ -125,7 +187,11 @@ void kiss_serialCallback(uint8_t sbyte) {
                     if (sbyte == TFESC) sbyte = FESC;
                     ESCAPE = false;
                 }
-                serialBuffer[frame_len++] = sbyte;
+                if (queue_height < CONFIG_QUEUE_MAX_LENGTH && queued_bytes < CONFIG_QUEUE_SIZE) {
+                    queued_bytes++;
+                    packet_queue[queue_cursor++] = sbyte;
+                    if (queue_cursor == CONFIG_QUEUE_SIZE) queue_cursor = 0;
+                }
             }
         } else if (command == CMD_TXDELAY) {
             custom_preamble = sbyte * 10UL;
@@ -135,12 +201,11 @@ void kiss_serialCallback(uint8_t sbyte) {
             slotTime = sbyte * 10;
         } else if (command == CMD_P) {
             p = sbyte;
-        } else if (command == CMD_READY) {
-            if (sbyte == 0x00) {
-                FLOWCONTROL = false;
-            } else {
-                FLOWCONTROL = true;
-            }
+        } else if (command == CMD_FLUSHQUEUE) {
+            kiss_flushQueue();
+        // TODO: Remove this
+        } else if (command == CMD_FLUSHQUEUE_DEBUG) {
+            kiss_flushQueueDebug();
         }
         
     }
diff --git a/protocol/KISS.h b/protocol/KISS.h
index b762772..fe97011 100755
--- a/protocol/KISS.h
+++ b/protocol/KISS.h
@@ -19,12 +19,16 @@
 #define CMD_TXTAIL 0x04
 #define CMD_FULLDUPLEX 0x05
 #define CMD_SETHARDWARE 0x06
+#define CMD_FLUSHQUEUE 0x07
+#define CMD_FLUSHQUEUE_DEBUG 0x08
 #define CMD_READY 0x0F
 #define CMD_RETURN 0xFF
 
 void kiss_init(AX25Ctx *ax25, Afsk *afsk, Serial *ser);
-void kiss_csma(AX25Ctx *ctx, uint8_t *buf, size_t len);
 void kiss_messageCallback(AX25Ctx *ctx);
 void kiss_serialCallback(uint8_t sbyte);
+void kiss_flushQueue(void);
+void kiss_csma(void);
+void kiss_poll(void);
 
 #endif
\ No newline at end of file
diff --git a/util/FIFO16.h b/util/FIFO16.h
new file mode 100644
index 0000000..3c1c5f7
--- /dev/null
+++ b/util/FIFO16.h
@@ -0,0 +1,87 @@
+#ifndef UTIL_FIFO16_H
+#define UTIL_FIFO16_H
+
+#include <stdio.h>
+#include <stddef.h>
+#include <stdbool.h>
+#include <util/atomic.h>
+
+typedef struct FIFOBuffer16
+{
+  size_t *begin;
+  size_t *end;
+  size_t * volatile head;
+  size_t * volatile tail;
+} FIFOBuffer16;
+
+inline bool fifo16_isempty(const FIFOBuffer16 *f) {
+  return f->head == f->tail;
+}
+
+inline bool fifo16_isfull(const FIFOBuffer16 *f) {
+  return ((f->head == f->begin) && (f->tail == f->end)) || (f->tail == f->head - 1);
+}
+
+inline void fifo16_push(FIFOBuffer16 *f, size_t c) {
+  *(f->tail) = c;
+
+  if (f->tail == f->end) {
+    f->tail = f->begin;
+  } else {
+    f->tail++;
+  }
+}
+
+inline size_t fifo16_pop(FIFOBuffer16 *f) {
+  if(f->head == f->end) {
+    f->head = f->begin;
+    return *(f->end);
+  } else {
+    return *(f->head++);
+  }
+}
+
+inline void fifo16_flush(FIFOBuffer16 *f) {
+  f->head = f->tail;
+}
+
+static inline bool fifo16_isempty_locked(const FIFOBuffer16 *f) {
+  bool result;
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    result = fifo16_isempty(f);
+  }
+  return result;
+}
+
+static inline bool fifo16_isfull_locked(const FIFOBuffer16 *f) {
+  bool result;
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    result = fifo16_isfull(f);
+  }
+  return result;
+}
+
+static inline void fifo16_push_locked(FIFOBuffer16 *f, size_t c) {
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    fifo16_push(f, c);
+  }
+}
+
+static inline size_t fifo16_pop_locked(FIFOBuffer16 *f) {
+  size_t c;
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    c = fifo16_pop(f);
+  }
+  return c;
+}
+
+inline void fifo16_init(FIFOBuffer16 *f, size_t *buffer, size_t size) {
+  f->head = f->tail = f->begin = buffer;
+  f->end = buffer + (size/sizeof(size_t)) - 2;
+}
+
+inline size_t fifo16_len(FIFOBuffer16 *f) {
+  return ((f->end - f->begin))/sizeof(size_t);
+}
+
+#endif