diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..8ec1ba3
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "Core/Lib/FT_CAN_AL"]
+	path = Core/Lib/FT_CAN_AL
+	url = ssh://git@git.fasttube.de:313/FaSTTUBe/FT_CAN_AL.git
diff --git a/Core/Inc/can.h b/Core/Inc/can.h
index d2106e2..516f577 100644
--- a/Core/Inc/can.h
+++ b/Core/Inc/can.h
@@ -20,9 +20,8 @@
 #define CAN_ID_SHUNT_ENERGY_COUNTER 0x528
 
 void can_init(CAN_HandleTypeDef *handle);
-HAL_StatusTypeDef can_transmit(uint8_t id, uint8_t *data, size_t datalen);
 HAL_StatusTypeDef can_send_status();
 
-void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *handle);
+void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data);
 
 #endif // INC_CAN_H
diff --git a/Core/Inc/shunt_monitoring.h b/Core/Inc/shunt_monitoring.h
index a28ef0c..0a5aa6b 100644
--- a/Core/Inc/shunt_monitoring.h
+++ b/Core/Inc/shunt_monitoring.h
@@ -24,6 +24,6 @@ extern ShuntData shunt_data;
 void shunt_init();
 void shunt_check();
 
-void shunt_handle_can_msg(CAN_RxHeaderTypeDef *header, uint8_t *data);
+void shunt_handle_can_msg(uint16_t id, const uint8_t *data);
 
 #endif // INC_SHUNT_MONITORING_H
diff --git a/Core/Inc/slave_monitoring.h b/Core/Inc/slave_monitoring.h
index 1fb91ec..29f5abf 100644
--- a/Core/Inc/slave_monitoring.h
+++ b/Core/Inc/slave_monitoring.h
@@ -43,7 +43,7 @@ extern int16_t max_temp;
 void slaves_init();
 void slaves_check();
 
-void slaves_handle_panic(uint8_t *data);
-void slaves_handle_log(uint8_t *data);
+void slaves_handle_panic(const uint8_t *data);
+void slaves_handle_log(const uint8_t *data);
 
 #endif // INC_SLAVE_MONITORING_H
diff --git a/Core/Inc/ts_state_machine.h b/Core/Inc/ts_state_machine.h
index edd7eb6..01eefc6 100644
--- a/Core/Inc/ts_state_machine.h
+++ b/Core/Inc/ts_state_machine.h
@@ -53,7 +53,7 @@ void ts_sm_set_relay_positions(TSState state);
 void ts_sm_set_relay_position(Relay relay, int closed);
 void ts_sm_check_close_wait(int *is_closed, int should_close);
 
-void ts_sm_handle_ams_in(uint8_t *data);
+void ts_sm_handle_ams_in(const uint8_t *data);
 
 void ts_sm_set_error_source(uint32_t flag, int state);
 
diff --git a/Core/Inc/util.h b/Core/Inc/util.h
index 8990828..8060c62 100644
--- a/Core/Inc/util.h
+++ b/Core/Inc/util.h
@@ -5,8 +5,4 @@
 
 void set_error_led();
 
-uint64_t ntohll(uint64_t netlonglong);
-uint32_t ntohl(uint32_t netlong);
-uint16_t ntohs(uint16_t netshort);
-
 #endif // INC_UTIL_H
diff --git a/Core/Lib/FT_CAN_AL b/Core/Lib/FT_CAN_AL
new file mode 160000
index 0000000..1589839
--- /dev/null
+++ b/Core/Lib/FT_CAN_AL
@@ -0,0 +1 @@
+Subproject commit 1589839b447df113d48c052897583de4ab3c8dfc
diff --git a/Core/Src/can.c b/Core/Src/can.c
index 9e4c7fe..7ad3387 100644
--- a/Core/Src/can.c
+++ b/Core/Src/can.c
@@ -4,57 +4,19 @@
 #include "shunt_monitoring.h"
 #include "slave_monitoring.h"
 #include "soc_estimation.h"
-#include "stm32f3xx_hal_can.h"
-#include "stm32f3xx_hal_gpio.h"
 #include "ts_state_machine.h"
-#include "util.h"
+
+#include "FT_CAN_AL.h"
+
 #include <stdint.h>
 
-static CAN_HandleTypeDef *hcan;
-
 void can_init(CAN_HandleTypeDef *handle) {
-  hcan = handle;
-
-  CAN_FilterTypeDef filter;
-  filter.FilterIdHigh = CAN_ID_SHUNT_BASE & 0xFF0;
-  filter.FilterMaskIdHigh = 0xFF0;
-  filter.FilterIdLow = CAN_ID_AMS_IN;
-  filter.FilterMaskIdLow = 0xFFF;
-  filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
-  filter.FilterBank = 0;
-  filter.FilterMode = CAN_FILTERMODE_IDMASK;
-  filter.FilterScale = CAN_FILTERSCALE_16BIT;
-  if (HAL_CAN_ConfigFilter(hcan, &filter) != HAL_OK) {
-    Error_Handler();
-  }
+  ftcan_init(handle);
+  ftcan_add_filter(CAN_ID_SHUNT_BASE, 0xFF0);
+  ftcan_add_filter(CAN_ID_AMS_IN, 0xFFF);
+  ftcan_add_filter(CAN_ID_SLAVE_PANIC, 0xFFF);
+  ftcan_add_filter(CAN_ID_SLAVE_LOG, 0xFFF);
   // TODO: Slave status?
-  filter.FilterIdHigh = CAN_ID_SLAVE_PANIC;
-  filter.FilterMaskIdHigh = 0xFFF;
-  filter.FilterIdLow = CAN_ID_SLAVE_LOG;
-  filter.FilterMaskIdLow = 0xFFF;
-  filter.FilterBank = 1;
-  if (HAL_CAN_ConfigFilter(hcan, &filter) != HAL_OK) {
-    Error_Handler();
-  }
-
-  if (HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING) !=
-      HAL_OK) {
-    Error_Handler();
-  }
-  if (HAL_CAN_Start(hcan) != HAL_OK) {
-    Error_Handler();
-  }
-}
-
-HAL_StatusTypeDef can_transmit(uint8_t id, uint8_t *data, size_t datalen) {
-  static CAN_TxHeaderTypeDef header;
-  header.StdId = id;
-  header.IDE = CAN_ID_STD;
-  header.RTR = CAN_RTR_DATA;
-  header.DLC = datalen;
-
-  uint32_t mailbox;
-  return HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox);
 }
 
 HAL_StatusTypeDef can_send_status() {
@@ -63,30 +25,15 @@ HAL_StatusTypeDef can_send_status() {
   data[1] = current_soc;
   data[2] = min_voltage >> 8;
   data[3] = max_temp >> 8;
-  return can_transmit(CAN_ID_AMS_STATUS, data, 4);
+  return ftcan_transmit(CAN_ID_AMS_STATUS, data, 4);
 }
 
-void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *handle) {
-  if (handle != hcan) {
+void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
+  if ((id & 0xFF0) == CAN_ID_SHUNT_BASE) {
+    shunt_handle_can_msg(id, data);
     return;
   }
-
-  CAN_RxHeaderTypeDef header;
-  uint8_t data[8];
-  if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &header, data) != HAL_OK) {
-    set_error_led();
-    return;
-  }
-
-  if (header.IDE != CAN_ID_STD) {
-    return;
-  }
-
-  if ((header.StdId & 0xFF0) == CAN_ID_SHUNT_BASE) {
-    shunt_handle_can_msg(&header, data);
-    return;
-  }
-  switch (header.StdId) {
+  switch (id) {
   case CAN_ID_SLAVE_PANIC:
     slaves_handle_panic(data);
     break;
diff --git a/Core/Src/shunt_monitoring.c b/Core/Src/shunt_monitoring.c
index 972a7cb..4cd3e72 100644
--- a/Core/Src/shunt_monitoring.c
+++ b/Core/Src/shunt_monitoring.c
@@ -6,6 +6,8 @@
 #include "ts_state_machine.h"
 #include "util.h"
 
+#include "FT_CAN_AL.h"
+
 ShuntData shunt_data;
 
 void shunt_init() {
@@ -25,20 +27,11 @@ void shunt_check() {
   ts_sm_set_error_source(TS_ERROR_SOURCE_SHUNT, is_error);
 }
 
-void shunt_handle_can_msg(CAN_RxHeaderTypeDef *header, uint8_t *data) {
+void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
   // All result messages contain a big-endian 6-byte integer
-  data[7] = 0;
-  data[8] = 0;
-  uint64_t result;
-  memcpy(&result, data, 8);
-  result = ntohll(result);
-  // Top two bytes should always be zero in our usecase (or 0xFF for a signed
-  // number)
-  if ((result >> 4) != 0 && (result >> 4) != 0xFFFF) {
-    set_error_led();
-  }
+  uint64_t result = ftcan_unmarshal_unsigned(&data, 6);
 
-  switch (header->StdId) {
+  switch (id) {
   case CAN_ID_SHUNT_CURRENT:
     shunt_data.current = result;
     break;
diff --git a/Core/Src/slave_monitoring.c b/Core/Src/slave_monitoring.c
index c59012b..d710ceb 100644
--- a/Core/Src/slave_monitoring.c
+++ b/Core/Src/slave_monitoring.c
@@ -3,6 +3,8 @@
 #include "main.h"
 #include "ts_state_machine.h"
 
+#include "FT_CAN_AL.h"
+
 #include "stm32f3xx_hal.h"
 #include "stm32f3xx_hal_gpio.h"
 
@@ -70,9 +72,10 @@ void slaves_check() {
   }
 }
 
-void slaves_handle_panic(uint8_t *data) {
-  uint8_t slave_id = data[0];
-  switch (data[1]) {
+void slaves_handle_panic(const uint8_t *data) {
+  uint8_t slave_id = ftcan_unmarshal_unsigned(&data, 1);
+  uint8_t error_kind = ftcan_unmarshal_unsigned(&data, 1);
+  switch (error_kind) {
   case SLAVE_PANIC_OT:
     slaves[slave_id].error.kind = SLAVE_ERR_OT;
     break;
@@ -86,10 +89,10 @@ void slaves_handle_panic(uint8_t *data) {
     slaves[slave_id].error.kind = SLAVE_ERR_UV;
     break;
   }
-  memcpy(&slaves[slave_id].error.data, &data[2], 4);
+  slaves[slave_id].error.data = ftcan_unmarshal_unsigned(&data, 4);
   slaves[slave_id].last_message = HAL_GetTick();
 }
 
-void slaves_handle_log(uint8_t *data) {
+void slaves_handle_log(const uint8_t *data) {
   // TODO
 }
diff --git a/Core/Src/ts_state_machine.c b/Core/Src/ts_state_machine.c
index 66fbc3b..8c8d429 100644
--- a/Core/Src/ts_state_machine.c
+++ b/Core/Src/ts_state_machine.c
@@ -194,7 +194,7 @@ void ts_sm_check_close_wait(int *is_closed, int should_close) {
   }
 }
 
-void ts_sm_handle_ams_in(uint8_t *data) {
+void ts_sm_handle_ams_in(const uint8_t *data) {
   if (data[0] & 0x01) {
     ts_state.target_state = TS_ACTIVE;
   } else {
diff --git a/Core/Src/util.c b/Core/Src/util.c
index fb17a72..6a47d64 100644
--- a/Core/Src/util.c
+++ b/Core/Src/util.c
@@ -5,22 +5,3 @@
 void set_error_led() {
   HAL_GPIO_WritePin(STATUS2_GPIO_Port, STATUS2_Pin, GPIO_PIN_SET);
 }
-
-uint64_t ntohll(uint64_t netlonglong) {
-  uint8_t *p = (uint8_t *)&netlonglong;
-  return ((uint64_t)p[0] << 56) | ((uint64_t)p[1] << 48) |
-         ((uint64_t)p[2] << 40) | ((uint64_t)p[3] << 32) |
-         ((uint64_t)p[4] << 24) | ((uint64_t)p[5] << 16) |
-         ((uint64_t)p[6] << 8) | ((uint64_t)p[7] << 0);
-}
-
-uint32_t ntohl(uint32_t netlong) {
-  uint8_t *p = (uint8_t *)&netlong;
-  return ((uint32_t)p[3] << 24) | ((uint32_t)p[2] << 16) |
-         ((uint32_t)p[1] << 8) | ((uint32_t)p[0] << 0);
-}
-
-uint16_t ntohs(uint16_t netshort) {
-  uint8_t *p = (uint8_t *)&netshort;
-  return ((uint16_t)p[1] << 8) | ((uint16_t)p[0] << 0);
-}
diff --git a/STM32-for-VSCode.config.yaml b/STM32-for-VSCode.config.yaml
index dbff8e2..c5ae07a 100644
--- a/STM32-for-VSCode.config.yaml
+++ b/STM32-for-VSCode.config.yaml
@@ -23,6 +23,7 @@ ldscript: STM32F302CBTx_FLASH.ld # linker script
 cDefinitions: 
   - USE_HAL_DRIVER
   - STM32F302xC
+  - STM32F3
 
 cxxDefinitions: 
   - USE_HAL_DRIVER
diff --git a/STM32Make.make b/STM32Make.make
index a8b5f24..8b8fd01 100644
--- a/STM32Make.make
+++ b/STM32Make.make
@@ -36,6 +36,7 @@ BUILD_DIR = build
 ######################################
 # C sources
 C_SOURCES =  \
+Core/Lib/FT_CAN_AL/FT_CAN_AL.c \
 Core/Src/can.c \
 Core/Src/main.c \
 Core/Src/shunt_monitoring.c \
@@ -122,6 +123,7 @@ AS_DEFS =
 
 # C defines
 C_DEFS =  \
+-DSTM32F3 \
 -DSTM32F302xC \
 -DUSE_HAL_DRIVER
 
@@ -138,6 +140,7 @@ AS_INCLUDES = \
 # C includes
 C_INCLUDES =  \
 -ICore/Inc \
+-ICore/Lib/FT_CAN_AL \
 -IDrivers/CMSIS/Device/ST/STM32F3xx/Include \
 -IDrivers/CMSIS/Include \
 -IDrivers/STM32F3xx_HAL_Driver/Inc \