diff --git a/src/CAN/can.c b/src/CAN/can.c
index 295b2e6..67ac00c 100644
--- a/src/CAN/can.c
+++ b/src/CAN/can.c
@@ -37,33 +37,43 @@ bool can_rx_buffer_empty(void) {
 }
 
 bool can_rx_buffer_full(void) {
-    return can_rx_head == ((can_rx_tail + 1) % CAN_RX_BUFFER_SIZE);
+    return (uint8_t)(can_rx_tail - can_rx_head) == CAN_RX_BUFFER_SIZE;
 }
 
-static volatile const CAN_Message* can_rx_buffer_peek(void) {
-    return &can_rx_buffer[can_rx_head];
+CAN_Message* can_rx_buffer_peek(void) {
+    if (!can_rx_buffer_empty()) {
+        return (CAN_Message*)(&can_rx_buffer[(can_rx_head % CAN_RX_BUFFER_SIZE)]);
+    } else {
+        return NULL;
+    }
 }
 
-static void can_rx_buffer_pop(void) {
-    can_rx_head = (can_rx_head + 1) % CAN_RX_BUFFER_SIZE;
+void can_rx_buffer_pop(void) {
+    can_rx_head++;
+
+    // Re-enable the ISR since we made space
+    nvic_enable_irq(CAN_NVIC_LINE);
 }
 
-static volatile CAN_Message* can_rx_buffer_tail(void) {
-    return &can_rx_buffer[can_rx_tail];
+static CAN_Message* can_rx_buffer_tail(void) {
+    return (CAN_Message*)&can_rx_buffer[(can_rx_tail % CAN_RX_BUFFER_SIZE)];
 }
 
 static void can_rx_buffer_extend(void) {
-    can_rx_tail = (can_rx_tail + 1) % CAN_RX_BUFFER_SIZE;
+    can_rx_tail++;
 }
 
 void can_rx_buffer_put(const CAN_Message* msg) {
-    memcpy((void*)&can_rx_buffer[can_rx_tail], (const void*)msg, sizeof(CAN_Message));
-    can_rx_tail = (can_rx_tail + 1) % CAN_RX_BUFFER_SIZE;
+    memcpy((void*)&can_rx_buffer[(can_rx_tail % CAN_RX_BUFFER_SIZE)], (const void*)msg, sizeof(CAN_Message));
+    can_rx_tail++;
 }
 
 void can_rx_buffer_get(CAN_Message* msg) {
-    memcpy((void*)msg, (const void*)&can_rx_buffer[can_rx_head], sizeof(CAN_Message));
-    can_rx_head = (can_rx_head + 1) % CAN_RX_BUFFER_SIZE;
+    memcpy((void*)msg, (const void*)&can_rx_buffer[(can_rx_head % CAN_RX_BUFFER_SIZE)], sizeof(CAN_Message));
+    can_rx_head++;
+
+    // Re-enable the ISR since we made space
+    nvic_enable_irq(CAN_NVIC_LINE);
 }
 
 bool can_reconfigure(uint32_t baudrate, CanMode mode) {
@@ -153,18 +163,20 @@ bool can_setup(uint32_t baudrate, CanMode mode) {
     return can_reconfigure(baudrate, mode);
 }
 
-
-
-bool can_read(CAN_Message* msg) {
-    bool success = false;
-
+static uint8_t can_fifo_depth(void) {
     uint8_t fifo_depth = (CAN_RF0R(CAN) & CAN_RF0R_FMP0_MASK);
     // Account for one fifo entry possibly going away
     if (CAN_RF0R(CAN) & CAN_RF0R_RFOM0) {
         fifo_depth = fifo_depth > 0 ? (fifo_depth - 1) : 0;
     }
 
-    if (fifo_depth > 0) {
+    return fifo_depth;
+}
+
+bool can_read(CAN_Message* msg) {
+    bool success = false;
+
+    if (can_fifo_depth() > 0) {
         // Wait for the previous message to be released
         while (CAN_RF0R(CAN) & CAN_RF0R_RFOM0);
 
@@ -195,16 +207,24 @@ bool can_write(CAN_Message* msg) {
     return (can_transmit(CAN, msg->id, ext, rtr, msg->len, msg->data) != -1);
 }
 
-
 void cec_can_isr(void) {
-    while (!can_rx_buffer_full()) {
-        CAN_Message msg;
-        if (can_read(&msg)) {
-            can_rx_buffer_put(&msg);
+    uint8_t messages_queued = 0;
+    uint8_t fifo_depth = can_fifo_depth();
+    while (!can_rx_buffer_full() && messages_queued < fifo_depth) {
+        CAN_Message* msg = can_rx_buffer_tail();
+        if (can_read(msg)) {
+            can_rx_buffer_extend();
+            messages_queued++;
         } else {
             break;
         }
     }
+
+    // If the software buffer is full, disable the ISR so that
+    // the main loop can drain the buffer over USB.
+    if (messages_queued == 0) {
+        nvic_disable_irq(CAN_NVIC_LINE);
+    }
 }
 
 
diff --git a/src/CAN/can.h b/src/CAN/can.h
index a58612f..ed9e6e7 100644
--- a/src/CAN/can.h
+++ b/src/CAN/can.h
@@ -34,5 +34,7 @@ extern bool can_rx_buffer_empty(void);
 extern bool can_rx_buffer_full(void);
 extern void can_rx_buffer_put(const CAN_Message* msg);
 extern void can_rx_buffer_get(CAN_Message* msg);
+extern CAN_Message* can_rx_buffer_peek(void);
+extern void can_rx_buffer_pop(void);
 
 #endif
diff --git a/src/CAN/slcan.c b/src/CAN/slcan.c
index 3a84bf1..7d97fdd 100644
--- a/src/CAN/slcan.c
+++ b/src/CAN/slcan.c
@@ -322,29 +322,55 @@ bool slcan_exec_command(const char* command, size_t len) {
     return success;
 }
 
+static size_t slcan_calc_message_length(const CAN_Message* msg) {
+    size_t len;
+    if (msg->format == CANStandard) {
+        len = 1 + 3 + 1 + (2 * msg->len) + 1;
+    } else {
+        len = 1 + 8 + 1 + (2 * msg->len) + 1;
+    }
+
+    return len;
+}
+
 bool slcan_output_messages(void) {
     if (slcan_mode == MODE_RESET) {
         return false;
     }
     bool read = false;
-    CAN_Message msg;
-    while (can_read_buffer(&msg)) {
+
+    size_t avail_buf_len = vcdc_send_buffer_space();
+    while (!can_rx_buffer_empty()) {
+        // Examine the current message without dequeuing it
+        CAN_Message* msg = can_rx_buffer_peek();
+
+        // Stop processing messages if there's no more room
+        size_t msg_len = slcan_calc_message_length(msg);
+        if (msg_len > avail_buf_len) {
+            break;
+        }
+        
         read = true;
-        if (msg.format == CANStandard) {
-            vcdc_print(msg.type == CANData ? "t" : "r");
-            vcdc_print_hex_nibble((uint8_t)(msg.id >> 8));
-            vcdc_print_hex_byte((uint8_t)(msg.id & 0xFF));
-            vcdc_putchar('0' + msg.len);
+        if (msg->format == CANStandard) {
+            vcdc_print(msg->type == CANData ? "t" : "r");
+            vcdc_print_hex_nibble((uint8_t)(msg->id >> 8));
+            vcdc_print_hex_byte((uint8_t)(msg->id & 0xFF));
+            vcdc_putchar('0' + msg->len);
         } else {
-            vcdc_print(msg.type == CANData ? "T" : "R");
-            vcdc_print_hex(msg.id);
-            vcdc_putchar('0' + msg.len);
+            vcdc_print(msg->type == CANData ? "T" : "R");
+            vcdc_print_hex(msg->id);
+            vcdc_putchar('0' + msg->len);
         }
         uint8_t i = 0;
-        for (i=0; i < msg.len; i++) {
-            vcdc_print_hex_byte(msg.data[i]);
+        for (i=0; i < msg->len; i++) {
+            vcdc_print_hex_byte(msg->data[i]);
         }
         vcdc_putchar('\r');
+
+        avail_buf_len -= msg_len;
+
+        // Release the message now that it's been processed
+        can_rx_buffer_pop();
     }
 
     return read;
diff --git a/src/USB/vcdc.c b/src/USB/vcdc.c
index 3a572dc..fa5a319 100644
--- a/src/USB/vcdc.c
+++ b/src/USB/vcdc.c
@@ -70,17 +70,17 @@ static bool vcdc_tx_buffer_empty(void) {
 }
 
 static bool vcdc_tx_buffer_full(void) {
-    return vcdc_tx_head == ((vcdc_tx_tail + 1) % VCDC_TX_BUFFER_SIZE);
+    return (uint16_t)(vcdc_tx_tail - vcdc_tx_head) == VCDC_TX_BUFFER_SIZE;
 }
 
 static void vcdc_tx_buffer_put(uint8_t data) {
-    vcdc_tx_buffer[vcdc_tx_tail] = data;
-    vcdc_tx_tail = (vcdc_tx_tail + 1) % VCDC_TX_BUFFER_SIZE;
+    vcdc_tx_buffer[vcdc_tx_tail % VCDC_TX_BUFFER_SIZE] = data;
+    vcdc_tx_tail++;
 }
 
 static uint8_t vcdc_tx_buffer_get(void) {
-    uint8_t data = vcdc_tx_buffer[vcdc_tx_head];
-    vcdc_tx_head = (vcdc_tx_head + 1) % VCDC_TX_BUFFER_SIZE;
+    uint8_t data = vcdc_tx_buffer[vcdc_tx_head % VCDC_TX_BUFFER_SIZE];
+    vcdc_tx_head++;
     return data;
 }
 
@@ -89,17 +89,17 @@ static bool vcdc_rx_buffer_empty(void) {
 }
 
 static bool vcdc_rx_buffer_full(void) {
-    return vcdc_rx_head == ((vcdc_rx_tail + 1) % VCDC_RX_BUFFER_SIZE);
+    return (uint16_t)(vcdc_rx_tail - vcdc_rx_head) == VCDC_RX_BUFFER_SIZE;
 }
 
 static void vcdc_rx_buffer_put(uint8_t data) {
-    vcdc_rx_buffer[vcdc_rx_tail] = data;
-    vcdc_rx_tail = (vcdc_rx_tail + 1) % VCDC_RX_BUFFER_SIZE;
+    vcdc_rx_buffer[vcdc_rx_tail % VCDC_RX_BUFFER_SIZE] = data;
+    vcdc_rx_tail++;
 }
 
 static uint8_t vcdc_rx_buffer_get(void) {
-    uint8_t data = vcdc_rx_buffer[vcdc_rx_head];
-    vcdc_rx_head = (vcdc_rx_head + 1) % VCDC_RX_BUFFER_SIZE;
+    uint8_t data = vcdc_rx_buffer[vcdc_rx_head % VCDC_RX_BUFFER_SIZE];
+    vcdc_rx_head++;
     return data;
 }
 
@@ -121,6 +121,10 @@ size_t vcdc_send_buffered(const uint8_t* data, size_t num_bytes) {
     return bytes_queued;
 }
 
+size_t vcdc_send_buffer_space(void) {
+    return VCDC_TX_BUFFER_SIZE - (uint16_t)(vcdc_tx_tail - vcdc_tx_head);
+}
+
 /* User callbacks */
 static GenericCallback vcdc_rx_callback = NULL;
 static GenericCallback vcdc_tx_callback = NULL;
diff --git a/src/USB/vcdc.h b/src/USB/vcdc.h
index a4622f4..451ab83 100644
--- a/src/USB/vcdc.h
+++ b/src/USB/vcdc.h
@@ -31,6 +31,8 @@ extern bool vcdc_app_update(void);
 extern size_t vcdc_recv_buffered(uint8_t* data, size_t max_bytes);
 extern size_t vcdc_send_buffered(const uint8_t* data, size_t num_bytes);
 
+extern size_t vcdc_send_buffer_space(void);
+
 extern void vcdc_putchar(const char c);
 extern void vcdc_print(const char* s);
 extern void vcdc_println(const char* s);