diff --git a/src/generic/canbus.c b/src/generic/canbus.c
index 9b8e0e54..6c7f2ccc 100644
--- a/src/generic/canbus.c
+++ b/src/generic/canbus.c
@@ -9,6 +9,8 @@
 #include <string.h> // memcpy
 #include "canbus.h" // canbus_set_uuid
 #include "command.h" // DECL_CONSTANT
+#include "generic/io.h" // readb
+#include "generic/irq.h" // irq_disable
 #include "sched.h" // sched_wake_task
 
 static uint32_t canbus_assigned_id;
@@ -183,16 +185,47 @@ canbus_notify_rx(void)
 static uint8_t receive_buf[192], receive_pos;
 DECL_CONSTANT("RECEIVE_WINDOW", ARRAY_SIZE(receive_buf));
 
-static void
-can_process_data(uint32_t id, uint32_t len, uint8_t *data)
+// Handle incoming data (called from IRQ handler)
+void
+canbus_process_data(uint32_t id, uint32_t len, uint8_t *data)
 {
+    if (!id || id != canbus_assigned_id)
+        return;
     int rpos = receive_pos;
     if (len > sizeof(receive_buf) - rpos)
         len = sizeof(receive_buf) - rpos;
     memcpy(&receive_buf[rpos], data, len);
     receive_pos = rpos + len;
+    canbus_notify_rx();
 }
 
+// Remove from the receive buffer the given number of bytes
+static void
+console_pop_input(int len)
+{
+    int copied = 0;
+    for (;;) {
+        int rpos = readb(&receive_pos);
+        int needcopy = rpos - len;
+        if (needcopy) {
+            memmove(&receive_buf[copied], &receive_buf[copied + len]
+                    , needcopy - copied);
+            copied = needcopy;
+            canbus_notify_rx();
+        }
+        irqstatus_t flag = irq_save();
+        if (rpos != readb(&receive_pos)) {
+            // Raced with irq handler - retry
+            irq_restore(flag);
+            continue;
+        }
+        receive_pos = needcopy;
+        irq_restore(flag);
+        break;
+    }
+}
+
+// Task to process incoming commands and admin messages
 void
 canbus_rx_task(void)
 {
@@ -206,8 +239,6 @@ canbus_rx_task(void)
         int ret = canbus_read(&id, data);
         if (ret < 0)
             break;
-        if (id && id == canbus_assigned_id)
-            can_process_data(id, ret, data);
         if (id && id == canbus_assigned_id + 1)
             can_id_conflict();
         else if (id == CANBUS_ID_ADMIN)
@@ -215,18 +246,15 @@ canbus_rx_task(void)
     }
 
     // Check for a complete message block and process it
-    uint_fast8_t rpos = receive_pos, pop_count;
-    int ret = command_find_and_dispatch(receive_buf, rpos, &pop_count);
+    uint_fast8_t rpos = readb(&receive_pos), pop_count;
+    int ret = command_find_block(receive_buf, rpos, &pop_count);
+    if (ret > 0)
+        command_dispatch(receive_buf, pop_count);
     if (ret) {
-        // Move buffer
-        int needcopy = rpos - pop_count;
-        if (needcopy) {
-            memmove(receive_buf, &receive_buf[pop_count], needcopy);
-            canbus_notify_rx();
-        }
-        rpos = needcopy;
+        console_pop_input(pop_count);
+        if (ret > 0)
+            command_send_ack();
     }
-    receive_pos = rpos;
 }
 DECL_TASK(canbus_rx_task);
 
diff --git a/src/generic/canbus.h b/src/generic/canbus.h
index d9c330b3..a7df077d 100644
--- a/src/generic/canbus.h
+++ b/src/generic/canbus.h
@@ -15,6 +15,7 @@ void canbus_set_filter(uint32_t id);
 // canbus.c
 void canbus_notify_tx(void);
 void canbus_notify_rx(void);
+void canbus_process_data(uint32_t id, uint32_t len, uint8_t *data);
 void canbus_set_uuid(void *data);
 
 #endif // canbus.h
diff --git a/src/stm32/can.c b/src/stm32/can.c
index 360ea1ff..323c1d54 100644
--- a/src/stm32/can.c
+++ b/src/stm32/can.c
@@ -179,8 +179,8 @@ canbus_set_filter(uint32_t id)
     /* 32-bit scale for the filter */
     SOC_CAN->FS1R = (1<<0) | (1<<1) | (1<<2);
 
-    /* FIFO 0 assigned for the filter */
-    SOC_CAN->FFA1R = 0;
+    /* FIFO 1 assigned to 'id' */
+    SOC_CAN->FFA1R = (1<<2);
 
     /* Filter activation */
     SOC_CAN->FA1R = (1<<0) | (id ? (1<<1) | (1<<2) : 0);
@@ -192,9 +192,29 @@ canbus_set_filter(uint32_t id)
 void
 CAN_IRQHandler(void)
 {
+    if (SOC_CAN->RF1R & CAN_RF1R_FMP1) {
+        // Read and ack data packet
+        CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[1];
+        uint32_t rir_id = (mb->RIR >> CAN_RI0R_STID_Pos) & 0x7FF;
+        uint32_t dlc = mb->RDTR & CAN_RDT0R_DLC;
+        uint32_t rdlr = mb->RDLR, rdhr = mb->RDHR;
+        SOC_CAN->RF1R = CAN_RF1R_RFOM1;
+
+        // Process packet
+        uint8_t data[8];
+        data[0] = (rdlr >>  0) & 0xff;
+        data[1] = (rdlr >>  8) & 0xff;
+        data[2] = (rdlr >> 16) & 0xff;
+        data[3] = (rdlr >> 24) & 0xff;
+        data[4] = (rdhr >>  0) & 0xff;
+        data[5] = (rdhr >>  8) & 0xff;
+        data[6] = (rdhr >> 16) & 0xff;
+        data[7] = (rdhr >> 24) & 0xff;
+        canbus_process_data(rir_id, dlc, data);
+    }
     uint32_t ier = SOC_CAN->IER;
     if (ier & CAN_IER_FMPIE0 && SOC_CAN->RF0R & CAN_RF0R_FMP0) {
-        // Rx
+        // Admin Rx
         SOC_CAN->IER = ier = ier & ~CAN_IER_FMPIE0;
         canbus_notify_rx();
     }
@@ -292,6 +312,7 @@ can_init(void)
         armcm_enable_irq(CAN_IRQHandler, CAN_RX1_IRQn, 0);
     if (CAN_RX0_IRQn != CAN_TX_IRQn)
         armcm_enable_irq(CAN_IRQHandler, CAN_TX_IRQn, 0);
+    SOC_CAN->IER = CAN_IER_FMPIE1;
 
     // Convert unique 96-bit chip id into 48 bit representation
     uint64_t hash = fasthash64((uint8_t*)UID_BASE, 12, 0xA16231A7);