diff --git a/src/generic/canbus.c b/src/generic/canbus.c
new file mode 100644
index 00000000..b3e1bada
--- /dev/null
+++ b/src/generic/canbus.c
@@ -0,0 +1,227 @@
+// Generic handling of serial over CAN support
+//
+// Copyright (C) 2019 Eug Krashtan <eug.krashtan@gmail.com>
+// Copyright (C) 2020 Pontus Borg <glpontus@gmail.com>
+// Copyright (C) 2021  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <string.h> // memcpy
+#include "canbus.h" // canbus_set_uuid
+#include "command.h" // DECL_CONSTANT
+#include "sched.h" // sched_wake_task
+
+static uint32_t canbus_assigned_id;
+static uint8_t canbus_uuid[CANBUS_UUID_LEN];
+
+
+/****************************************************************
+ * Data transmission over CAN
+ ****************************************************************/
+
+static struct task_wake canbus_tx_wake;
+static uint8_t transmit_buf[96], transmit_pos, transmit_max;
+
+void
+canbus_notify_tx(void)
+{
+    sched_wake_task(&canbus_tx_wake);
+}
+
+void
+canbus_tx_task(void)
+{
+    if (!sched_check_wake(&canbus_tx_wake))
+        return;
+    uint32_t id = canbus_assigned_id;
+    if (!id) {
+        transmit_pos = transmit_max = 0;
+        return;
+    }
+    uint32_t tpos = transmit_pos, tmax = transmit_max;
+    for (;;) {
+        int avail = tmax - tpos, now = avail > 8 ? 8 : avail;
+        if (avail <= 0)
+            break;
+        int ret = canbus_send(id + 1, now, &transmit_buf[tpos]);
+        if (ret <= 0)
+            break;
+        tpos += now;
+    }
+    transmit_pos = tpos;
+}
+DECL_TASK(canbus_tx_task);
+
+// Encode and transmit a "response" message
+void
+console_sendf(const struct command_encoder *ce, va_list args)
+{
+    // Verify space for message
+    uint32_t tpos = transmit_pos, tmax = transmit_max;
+    if (tpos >= tmax)
+        transmit_pos = transmit_max = tpos = tmax = 0;
+    uint32_t max_size = ce->max_size;
+    if (tmax + max_size > sizeof(transmit_buf)) {
+        if (tmax + max_size - tpos > sizeof(transmit_buf))
+            // Not enough space for message
+            return;
+        // Move buffer
+        tmax -= tpos;
+        memmove(&transmit_buf[0], &transmit_buf[tpos], tmax);
+        transmit_pos = tpos = 0;
+        transmit_max = tmax;
+    }
+
+    // Generate message
+    uint32_t msglen = command_encode_and_frame(&transmit_buf[tmax], ce, args);
+
+    // Start message transmit
+    transmit_max = tmax + msglen;
+    canbus_notify_tx();
+}
+
+
+/****************************************************************
+ * CAN command handling
+ ****************************************************************/
+
+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)
+{
+    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;
+}
+
+// Helper to retry sending until successful
+static void
+canbus_send_blocking(uint32_t id, uint32_t len, uint8_t *data)
+{
+    for (;;) {
+        int ret = canbus_send(id, len, data);
+        if (ret >= 0)
+            return;
+    }
+}
+
+static void
+can_process_ping(uint32_t id, uint32_t len, uint8_t *data)
+{
+    canbus_send_blocking(canbus_assigned_id + 1, 0, NULL);
+}
+
+static void
+can_process_reset(uint32_t id, uint32_t len, uint8_t *data)
+{
+    uint32_t reset_id = data[0] | (data[1] << 8);
+    if (reset_id == canbus_assigned_id)
+        canbus_reboot();
+}
+
+static void
+can_process_uuid(uint32_t id, uint32_t len, uint8_t *data)
+{
+    if (canbus_assigned_id)
+        return;
+    canbus_send_blocking(CANBUS_ID_UUID_RESP, sizeof(canbus_uuid), canbus_uuid);
+}
+
+static void
+can_process_set_id(uint32_t id, uint32_t len, uint8_t *data)
+{
+    // compare my UUID with packet to check if this packet mine
+    if (memcmp(&data[2], canbus_uuid, sizeof(canbus_uuid)) == 0) {
+        canbus_assigned_id = data[0] | (data[1] << 8);
+        canbus_set_dataport(canbus_assigned_id);
+    }
+}
+
+static void
+can_process(uint32_t id, uint32_t len, uint8_t *data)
+{
+    if (id == canbus_assigned_id) {
+        if (len)
+            can_process_data(id, len, data);
+        else
+            can_process_ping(id, len, data);
+    } else if (id == CANBUS_ID_UUID) {
+        if (len)
+            can_process_reset(id, len, data);
+        else
+            can_process_uuid(id, len, data);
+    } else if (id==CANBUS_ID_SET) {
+        can_process_set_id(id, len, data);
+    }
+}
+
+
+/****************************************************************
+ * CAN packet reading
+ ****************************************************************/
+
+static struct task_wake canbus_rx_wake;
+
+void
+canbus_notify_rx(void)
+{
+    sched_wake_task(&canbus_rx_wake);
+}
+
+void
+canbus_rx_task(void)
+{
+    if (!sched_check_wake(&canbus_rx_wake))
+        return;
+
+    // Read any pending CAN packets
+    for (;;) {
+        uint8_t data[8];
+        uint32_t id;
+        int ret = canbus_read(&id, data);
+        if (ret < 0)
+            break;
+        can_process(id, ret, data);
+    }
+
+    // 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);
+    if (ret) {
+        // Move buffer
+        int needcopy = rpos - pop_count;
+        if (needcopy) {
+            memmove(receive_buf, &receive_buf[pop_count], needcopy);
+            canbus_notify_rx();
+        }
+        rpos = needcopy;
+    }
+    receive_pos = rpos;
+}
+DECL_TASK(canbus_rx_task);
+
+
+/****************************************************************
+ * Setup and shutdown
+ ****************************************************************/
+
+void
+canbus_set_uuid(void *uuid)
+{
+    memcpy(canbus_uuid, uuid, sizeof(canbus_uuid));
+
+    // Send initial message
+    can_process_uuid(0, 0, NULL);
+}
+
+void
+canbus_shutdown(void)
+{
+    canbus_notify_tx();
+    canbus_notify_rx();
+}
+DECL_SHUTDOWN(canbus_shutdown);
diff --git a/src/generic/canbus.h b/src/generic/canbus.h
new file mode 100644
index 00000000..d797dc53
--- /dev/null
+++ b/src/generic/canbus.h
@@ -0,0 +1,22 @@
+#ifndef __CANBUS_H__
+#define __CANBUS_H__
+
+#include <stdint.h> // uint32_t
+
+#define CANBUS_ID_UUID 0x321
+#define CANBUS_ID_SET 0x322
+#define CANBUS_ID_UUID_RESP 0x323
+#define CANBUS_UUID_LEN 6
+
+// callbacks provided by board specific code
+int canbus_read(uint32_t *id, uint8_t *data);
+int canbus_send(uint32_t id, uint32_t len, uint8_t *data);
+void canbus_set_dataport(uint32_t id);
+void canbus_reboot(void);
+
+// canbus.c
+void canbus_notify_tx(void);
+void canbus_notify_rx(void);
+void canbus_set_uuid(void *data);
+
+#endif // canbus.h
diff --git a/src/stm32/Kconfig b/src/stm32/Kconfig
index fd6276cd..19da0e22 100644
--- a/src/stm32/Kconfig
+++ b/src/stm32/Kconfig
@@ -176,9 +176,9 @@ config CANSERIAL
     bool "Use CAN for communication (instead of serial)"
     depends on !USBSERIAL
     default n
-config SERIAL_BAUD
+config CANBUS_FREQUENCY
     int "CAN bus speed" if LOW_LEVEL_OPTIONS && CANSERIAL
-    default 500000 if CANSERIAL
+    default 500000
 choice
     depends on CANSERIAL
     prompt "CAN pins"
diff --git a/src/stm32/Makefile b/src/stm32/Makefile
index bd5017f2..1587d937 100644
--- a/src/stm32/Makefile
+++ b/src/stm32/Makefile
@@ -46,8 +46,8 @@ src-$(CONFIG_USBSERIAL) += $(usb-src-y) stm32/chipid.c generic/usb_cdc.c
 serial-src-y := stm32/serial.c
 serial-src-$(CONFIG_MACH_STM32F0) := stm32/stm32f0_serial.c
 src-$(CONFIG_SERIAL) += $(serial-src-y) generic/serial_irq.c
-can-src-$(CONFIG_CANSERIAL) += stm32/can.c ../lib/fast-hash/fasthash.c
-src-$(CONFIG_CANSERIAL) += $(can-src-y) generic/serial_irq.c
+src-$(CONFIG_CANSERIAL) += stm32/can.c ../lib/fast-hash/fasthash.c
+src-$(CONFIG_CANSERIAL) += generic/canbus.c
 
 dirs-$(CONFIG_CANSERIAL) += lib/fast-hash
 
diff --git a/src/stm32/can.c b/src/stm32/can.c
index 20162f14..987a8190 100644
--- a/src/stm32/can.c
+++ b/src/stm32/can.c
@@ -9,10 +9,10 @@
 #include <string.h> // memcpy
 #include "autoconf.h" // CONFIG_MACH_STM32F1
 #include "board/irq.h" // irq_disable
-#include "can.h" // SHORT_UUID_LEN
 #include "command.h" // DECL_CONSTANT_STR
 #include "fasthash.h" // fasthash64
 #include "generic/armcm_boot.h" // armcm_enable_irq
+#include "generic/canbus.h" // canbus_notify_tx
 #include "generic/serial_irq.h" // serial_rx_byte
 #include "internal.h" // enable_pclock
 #include "sched.h" // DECL_INIT
@@ -87,64 +87,76 @@
  #error No known CAN device for configured MCU
 #endif
 
-static uint16_t MyCanId = 0;
-
-static int
-can_find_empty_tx_mbox(void)
+// Read the next CAN packet
+int
+canbus_read(uint32_t *id, uint8_t *data)
 {
-    uint32_t tsr = SOC_CAN->TSR;
-    if (tsr & (CAN_TSR_TME0|CAN_TSR_TME1|CAN_TSR_TME2))
-        return (tsr & CAN_TSR_CODE) >> CAN_TSR_CODE_Pos;
-    return -1;
+    if (!(SOC_CAN->RF0R & CAN_RF0R_FMP0)) {
+        // All rx mboxes empty, enable wake on rx IRQ
+        irq_disable();
+        SOC_CAN->IER |= CAN_IER_FMPIE0;
+        irq_enable();
+        return -1;
+    }
+
+    // Read and ack packet
+    CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[0];
+    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->RF0R = CAN_RF0R_RFOM0;
+
+    // Return packet
+    *id = rir_id;
+    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;
+    return dlc;
 }
 
-static void
-can_transmit_mbox(uint32_t id, int mbox, uint32_t dlc, uint8_t *pkt)
+// Transmit a packet
+int
+canbus_send(uint32_t id, uint32_t len, uint8_t *data)
 {
+    uint32_t tsr = SOC_CAN->TSR;
+    if (!(tsr & (CAN_TSR_TME0|CAN_TSR_TME1|CAN_TSR_TME2))) {
+        // No space in transmit fifo - enable tx irq
+        irq_disable();
+        SOC_CAN->IER |= CAN_IER_TMEIE;
+        irq_enable();
+        return -1;
+    }
+    int mbox = (tsr & CAN_TSR_CODE) >> CAN_TSR_CODE_Pos;
     CAN_TxMailBox_TypeDef *mb = &SOC_CAN->sTxMailBox[mbox];
 
     /* Set up the DLC */
-    mb->TDTR = (mb->TDTR & 0xFFFFFFF0) | (dlc & 0x0F);
+    mb->TDTR = (mb->TDTR & 0xFFFFFFF0) | (len & 0x0F);
 
     /* Set up the data field */
-    if (pkt) {
-        mb->TDLR = (((uint32_t)pkt[3] << 24)
-                    | ((uint32_t)pkt[2] << 16)
-                    | ((uint32_t)pkt[1] << 8)
-                    | ((uint32_t)pkt[0] << 0));
-        mb->TDHR = (((uint32_t)pkt[7] << 24)
-                    | ((uint32_t)pkt[6] << 16)
-                    | ((uint32_t)pkt[5] << 8)
-                    | ((uint32_t)pkt[4] << 0));
+    if (len) {
+        mb->TDLR = (((uint32_t)data[3] << 24)
+                    | ((uint32_t)data[2] << 16)
+                    | ((uint32_t)data[1] << 8)
+                    | ((uint32_t)data[0] << 0));
+        mb->TDHR = (((uint32_t)data[7] << 24)
+                    | ((uint32_t)data[6] << 16)
+                    | ((uint32_t)data[5] << 8)
+                    | ((uint32_t)data[4] << 0));
     }
 
     /* Request transmission */
     mb->TIR = (id << CAN_TI0R_STID_Pos) | CAN_TI0R_TXRQ;
-}
-
-// Blocking transmit function
-static void
-can_transmit(uint32_t id, uint32_t dlc, uint8_t *pkt)
-{
-    int mbox = -1;
-
-    do {
-        mbox = can_find_empty_tx_mbox();
-    } while (mbox < 0);
-
-    can_transmit_mbox(id, mbox, dlc, pkt);
-}
-
-// Convert Unique 96-bit value into 48 bit representation
-static void
-pack_uuid(uint8_t *u)
-{
-    uint64_t hash = fasthash64((uint8_t*)UID_BASE, 12, 0xA16231A7);
-    memcpy(u, &hash, SHORT_UUID_LEN);
+    return len;
 }
 
 #define CAN_FILTER_NUMBER 0
 
+// Setup the receive packet filter
 static void
 can_set_filter(uint32_t id1, uint32_t id2)
 {
@@ -172,145 +184,33 @@ can_set_filter(uint32_t id1, uint32_t id2)
     SOC_CAN->FMR &= ~CAN_FMR_FINIT;
 }
 
-static void
-can_process_data(uint32_t id, uint32_t dlc, uint8_t *data)
+void
+canbus_set_dataport(uint32_t id)
 {
-    int i;
-    for (i=0; i < dlc; i++)
-        serial_rx_byte(data[i]);
+    can_set_filter(CANBUS_ID_UUID, id);
 }
 
-static void
-can_process_ping(uint32_t id, uint32_t dlc, uint8_t *data)
-{
-    can_transmit(MyCanId+1, 0, NULL);
-}
-
-static void
-can_process_reset(uint32_t id, uint32_t dlc, uint8_t *data)
-{
-    uint32_t reset_id = data[0] | (data[1] << 8);
-    if (reset_id == MyCanId)
-        NVIC_SystemReset();
-}
-
-static void
-can_process_uuid(uint32_t id, uint32_t dlc, uint8_t *data)
-{
-    if (MyCanId)
-        return;
-    uint8_t short_uuid[SHORT_UUID_LEN];
-    pack_uuid(short_uuid);
-    can_transmit(PKT_ID_UUID_RESP, SHORT_UUID_LEN, short_uuid);
-}
-
-static void
-can_process_set_id(uint32_t id, uint32_t dlc, uint8_t *data)
-{
-    uint8_t short_uuid[SHORT_UUID_LEN];
-    pack_uuid(short_uuid);
-
-    // compare my UUID with packet to check if this packet mine
-    if (memcmp(&data[2], short_uuid, SHORT_UUID_LEN) == 0) {
-        MyCanId = data[0] | (data[1] << 8);
-        can_set_filter(MyCanId, PKT_ID_UUID);
-    }
-}
-
-static void
-can_process(uint32_t id, uint32_t dlc, uint8_t *data)
-{
-    if (id == MyCanId) {
-        if (dlc)
-            can_process_data(id, dlc, data);
-        else
-            can_process_ping(id, dlc, data);
-    } else if (id == PKT_ID_UUID) {
-        if (dlc)
-            can_process_reset(id, dlc, data);
-        else
-            can_process_uuid(id, dlc, data);
-    } else if (id==PKT_ID_SET) {
-        can_process_set_id(id, dlc, data);
-    }
-}
-
-static struct task_wake canbus_wake;
-
 void
-can_dispatch_task(void)
+canbus_reboot(void)
 {
-    if (!sched_check_wake(&canbus_wake))
-        return;
-
-    // Check for rx
-    for (;;) {
-        if (!(SOC_CAN->RF0R & CAN_RF0R_FMP0)) {
-            // All rx mboxes empty, enable wake on rx IRQ
-            irq_disable();
-            SOC_CAN->IER |= CAN_IER_FMPIE0;
-            irq_enable();
-            break;
-        }
-
-        // Read and ack packet
-        CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[0];
-        uint32_t 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->RF0R = CAN_RF0R_RFOM0;
-
-        // 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;
-        can_process(id, dlc, data);
-    }
-
-    // Check for tx data
-    for (;;) {
-        int mbox = can_find_empty_tx_mbox();
-        if (mbox < 0) {
-            // All tx mboxes full, enable wake on tx IRQ
-            irq_disable();
-            SOC_CAN->IER |= CAN_IER_TMEIE;
-            irq_enable();
-            break;
-        }
-        int i;
-        uint8_t databuf[8];
-        for (i=0; i<8; i++) {
-            if (serial_get_tx_byte(&(databuf[i])) == -1)
-                break;
-        }
-        if (!i)
-            break;
-        can_transmit_mbox(MyCanId+1, mbox, i, databuf);
-    }
+    NVIC_SystemReset();
 }
-DECL_TASK(can_dispatch_task);
 
 // This function handles CAN global interrupts
 void
 CAN_IRQHandler(void)
 {
-    if (SOC_CAN->RF0R & CAN_RF0R_FMP0) {
-        // Rx
-        SOC_CAN->IER &= ~CAN_IER_FMPIE0;
-        sched_wake_task(&canbus_wake);
-    }
     uint32_t ier = SOC_CAN->IER;
+    if (ier & CAN_IER_FMPIE0 && SOC_CAN->RF0R & CAN_RF0R_FMP0) {
+        // Rx
+        SOC_CAN->IER = ier = ier & ~CAN_IER_FMPIE0;
+        canbus_notify_rx();
+    }
     if (ier & CAN_IER_TMEIE
         && SOC_CAN->TSR & (CAN_TSR_RQCP0|CAN_TSR_RQCP1|CAN_TSR_RQCP2)) {
         // Tx
-        SOC_CAN->IER &= ~CAN_IER_TMEIE;
-        sched_wake_task(&canbus_wake);
+        SOC_CAN->IER = ier & ~CAN_IER_TMEIE;
+        canbus_notify_tx();
     }
 }
 
@@ -372,7 +272,7 @@ can_init(void)
 
     uint32_t pclock = get_pclock_frequency((uint32_t)SOC_CAN);
 
-    uint32_t btr = compute_btr(pclock, CONFIG_SERIAL_BAUD);
+    uint32_t btr = compute_btr(pclock, CONFIG_CANBUS_FREQUENCY);
 
     /*##-1- Configure the CAN #######################################*/
 
@@ -392,7 +292,7 @@ can_init(void)
         ;
 
     /*##-2- Configure the CAN Filter #######################################*/
-    can_set_filter(PKT_ID_UUID, PKT_ID_SET);
+    can_set_filter(CANBUS_ID_UUID, CANBUS_ID_SET);
 
     /*##-3- Configure Interrupts #################################*/
 
@@ -404,13 +304,8 @@ can_init(void)
     if (CAN_RX0_IRQn != CAN_TX_IRQn)
         armcm_enable_irq(CAN_IRQHandler, CAN_TX_IRQn, 0);
 
-    /*##-4- Say Hello #################################*/
-    can_process_uuid(0, 0, NULL);
+    // Convert unique 96-bit chip id into 48 bit representation
+    uint64_t hash = fasthash64((uint8_t*)UID_BASE, 12, 0xA16231A7);
+    canbus_set_uuid(&hash);
 }
 DECL_INIT(can_init);
-
-void
-serial_enable_tx_irq(void)
-{
-    sched_wake_task(&canbus_wake);
-}
diff --git a/src/stm32/can.h b/src/stm32/can.h
deleted file mode 100644
index 92429a26..00000000
--- a/src/stm32/can.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef __STM32_CAN_H__
-#define __STM32_CAN_H__
-
-// Read UUID  (6bytes)
-#define PKT_ID_UUID (0x321)
-// Set address (2bytes) to UUID (6b)
-#define PKT_ID_SET (0x322)
-// UUID response from slave  (6bytes)
-#define PKT_ID_UUID_RESP (0x323)
-
-#define SHORT_UUID_LEN (6)
-
-#endif /* __STM32_CAN_H__*/