diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml
index e96c5f5c..442616cb 100644
--- a/.github/workflows/test.yaml
+++ b/.github/workflows/test.yaml
@@ -41,6 +41,18 @@ jobs:
       - name: Build pedal STM image and bootstub
         run: $RUN "cd /tmp/openpilot/panda && PEDAL=1 scons"
 
+  unit_tests:
+    name: unit tests
+    runs-on: ubuntu-20.04
+    timeout-minutes: 45
+    steps:
+      - uses: actions/checkout@v2
+      - name: Build Docker image
+        run: eval "$BUILD"
+      - name: Test pack/unpack for USB protocol
+        run: $RUN "cd /tmp/openpilot/panda/tests/usbprotocol &&
+                   python -m unittest discover ."
+
   safety:
     name: safety
     runs-on: ubuntu-20.04
diff --git a/CANPacket_structure.png b/CANPacket_structure.png
new file mode 100644
index 00000000..7dd13431
Binary files /dev/null and b/CANPacket_structure.png differ
diff --git a/USB_packet_structure.png b/USB_packet_structure.png
new file mode 100644
index 00000000..30e848c3
Binary files /dev/null and b/USB_packet_structure.png differ
diff --git a/__init__.py b/__init__.py
index 81ee2f01..a9bf38f4 100644
--- a/__init__.py
+++ b/__init__.py
@@ -1,8 +1,8 @@
 # flake8: noqa
 # pylint: skip-file
 from .python import Panda, PandaWifiStreaming, PandaDFU, flash_release, \
-                    BASEDIR, ensure_st_up_to_date, PandaSerial, \
-                    DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, MCU_TYPE_F4
+                    BASEDIR, ensure_st_up_to_date, PandaSerial, pack_can_buffer, unpack_can_buffer, \
+                    DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, MCU_TYPE_F4, DLC_TO_LEN, LEN_TO_DLC
 
 from .python.config import BOOTSTUB_ADDRESS, BLOCK_SIZE_FX, APP_ADDRESS_FX, \
                            BLOCK_SIZE_H7, APP_ADDRESS_H7, DEVICE_SERIAL_NUMBER_ADDR_H7, \
diff --git a/board/can_definitions.h b/board/can_definitions.h
new file mode 100644
index 00000000..552cf3ff
--- /dev/null
+++ b/board/can_definitions.h
@@ -0,0 +1,29 @@
+#include "dlc_to_len.h"
+
+#define CAN_PACKET_VERSION 2
+typedef struct {
+  unsigned char reserved : 1;
+  unsigned char bus : 3;
+  unsigned char data_len_code : 4;
+  unsigned char rejected : 1;
+  unsigned char returned : 1;
+  unsigned char extended : 1;
+  unsigned int addr : 29;
+  unsigned char data[CANPACKET_DATA_SIZE_MAX];
+} __attribute__((packed, aligned(4))) CANPacket_t;
+
+#define GET_BUS(msg) ((msg)->bus)
+#define GET_LEN(msg) (dlc_to_len[(msg)->data_len_code])
+#define GET_ADDR(msg) ((msg)->addr)
+
+// Flasher and pedal use raw mailbox access
+#define GET_MAILBOX_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
+#define GET_MAILBOX_BYTES_04(msg) ((msg)->RDLR)
+#define GET_MAILBOX_BYTES_48(msg) ((msg)->RDHR)
+
+#define CAN_INIT_TIMEOUT_MS 500U
+
+#define CANPACKET_HEAD_SIZE 5U
+
+#define WORD_TO_BYTE_ARRAY(dst8, src32) 0[dst8] = ((src32) & 0xFF); 1[dst8] = (((src32) >> 8) & 0xFF); 2[dst8] = (((src32) >> 16) & 0xFF); 3[dst8] = (((src32) >> 24) & 0xFF)
+#define BYTE_ARRAY_TO_WORD(dst32, src8) ((dst32) = 0[src8] | (1[src8] << 8) | (2[src8] << 16) | (3[src8] << 24))
diff --git a/board/config.h b/board/config.h
index c7a50e70..311a0a2e 100644
--- a/board/config.h
+++ b/board/config.h
@@ -34,18 +34,6 @@
 
 #define MAX_RESP_LEN 0x40U
 
-#define CAN_PACKET_VERSION 1
-
-#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
-#define GET_LEN(msg) ((msg)->RDTR & 0xF)
-#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
-#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
-#define GET_BYTES_04(msg) ((msg)->RDLR)
-#define GET_BYTES_48(msg) ((msg)->RDHR)
-#define GET_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask))
-
-#define CAN_INIT_TIMEOUT_MS 500U
-
 #include <stdbool.h>
 #ifdef STM32H7
   #include "stm32h7/stm32h7_config.h"
diff --git a/board/dlc_to_len.h b/board/dlc_to_len.h
new file mode 100644
index 00000000..82d743ff
--- /dev/null
+++ b/board/dlc_to_len.h
@@ -0,0 +1 @@
+unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U};
diff --git a/board/drivers/bxcan.h b/board/drivers/bxcan.h
index e704b2f4..10cee05f 100644
--- a/board/drivers/bxcan.h
+++ b/board/drivers/bxcan.h
@@ -101,18 +101,23 @@ void process_can(uint8_t can_number) {
     uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
 
     // check for empty mailbox
-    CAN_FIFOMailBox_TypeDef to_send;
+    CANPacket_t to_send;
     if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
       // add successfully transmitted message to my fifo
       if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
         can_txd_cnt += 1;
 
         if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
-          CAN_FIFOMailBox_TypeDef to_push;
-          to_push.RIR = CAN->sTxMailBox[0].TIR;
-          to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000FU) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
-          to_push.RDLR = CAN->sTxMailBox[0].TDLR;
-          to_push.RDHR = CAN->sTxMailBox[0].TDHR;
+          CANPacket_t to_push;
+          to_push.returned = 1U;
+          to_push.rejected = 0U;
+          to_push.extended = (CAN->sTxMailBox[0].TIR >> 2) & 0x1U;
+          to_push.addr = (to_push.extended != 0) ? (CAN->sTxMailBox[0].TIR >> 3) : (CAN->sTxMailBox[0].TIR >> 21);
+          to_push.data_len_code = CAN->sTxMailBox[0].TDTR & 0xFU;
+          to_push.bus = bus_number;
+          WORD_TO_BYTE_ARRAY(&to_push.data[0], CAN->sTxMailBox[0].TDLR);
+          WORD_TO_BYTE_ARRAY(&to_push.data[4], CAN->sTxMailBox[0].TDHR);
+
           can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
         }
 
@@ -136,10 +141,12 @@ void process_can(uint8_t can_number) {
       if (can_pop(can_queues[bus_number], &to_send)) {
         can_tx_cnt += 1;
         // only send if we have received a packet
-        CAN->sTxMailBox[0].TDLR = to_send.RDLR;
-        CAN->sTxMailBox[0].TDHR = to_send.RDHR;
-        CAN->sTxMailBox[0].TDTR = to_send.RDTR;
-        CAN->sTxMailBox[0].TIR = to_send.RIR;
+        CAN->sTxMailBox[0].TIR = ((to_send.extended != 0) ? (to_send.addr << 3) : (to_send.addr << 21)) | (to_send.extended << 2);
+        CAN->sTxMailBox[0].TDTR = to_send.data_len_code;
+        BYTE_ARRAY_TO_WORD(CAN->sTxMailBox[0].TDLR, &to_send.data[0]);
+        BYTE_ARRAY_TO_WORD(CAN->sTxMailBox[0].TDHR, &to_send.data[4]);
+        // Send request TXRQ
+        CAN->sTxMailBox[0].TIR |= 0x1U;
 
         usb_cb_ep3_out_complete();
       }
@@ -161,23 +168,29 @@ void can_rx(uint8_t can_number) {
     pending_can_live = 1;
 
     // add to my fifo
-    CAN_FIFOMailBox_TypeDef to_push;
-    to_push.RIR = CAN->sFIFOMailBox[0].RIR;
-    to_push.RDTR = CAN->sFIFOMailBox[0].RDTR;
-    to_push.RDLR = CAN->sFIFOMailBox[0].RDLR;
-    to_push.RDHR = CAN->sFIFOMailBox[0].RDHR;
+    CANPacket_t to_push;
 
-    // modify RDTR for our API
-    to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
+    to_push.returned = 0U;
+    to_push.rejected = 0U;
+    to_push.extended = (CAN->sFIFOMailBox[0].RIR >> 2) & 0x1U;
+    to_push.addr = (to_push.extended != 0) ? (CAN->sFIFOMailBox[0].RIR >> 3) : (CAN->sFIFOMailBox[0].RIR >> 21);
+    to_push.data_len_code = CAN->sFIFOMailBox[0].RDTR & 0xFU;
+    to_push.bus = bus_number;
+    WORD_TO_BYTE_ARRAY(&to_push.data[0], CAN->sFIFOMailBox[0].RDLR);
+    WORD_TO_BYTE_ARRAY(&to_push.data[4], CAN->sFIFOMailBox[0].RDHR);
 
     // forwarding (panda only)
     int bus_fwd_num = safety_fwd_hook(bus_number, &to_push);
     if (bus_fwd_num != -1) {
-      CAN_FIFOMailBox_TypeDef to_send;
-      to_send.RIR = to_push.RIR | 1; // TXRQ
-      to_send.RDTR = to_push.RDTR;
-      to_send.RDLR = to_push.RDLR;
-      to_send.RDHR = to_push.RDHR;
+      CANPacket_t to_send;
+
+      to_send.returned = 0U;
+      to_push.rejected = 0U;
+      to_send.extended = to_push.extended; // TXRQ
+      to_send.addr = to_push.addr;
+      to_send.bus = to_push.bus;
+      to_send.data_len_code = to_push.data_len_code;
+      (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]);
       can_send(&to_send, bus_fwd_num, true);
     }
 
diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h
index 1d915fd1..fac20287 100644
--- a/board/drivers/can_common.h
+++ b/board/drivers/can_common.h
@@ -2,7 +2,7 @@ typedef struct {
   volatile uint32_t w_ptr;
   volatile uint32_t r_ptr;
   uint32_t fifo_size;
-  CAN_FIFOMailBox_TypeDef *elems;
+  CANPacket_t *elems;
 } can_ring;
 
 typedef struct {
@@ -12,8 +12,6 @@ typedef struct {
   uint32_t can_data_speed;
 } bus_config_t;
 
-#define CAN_BUS_RET_FLAG 0x80U
-#define CAN_BUS_BLK_FLAG 0x40U
 #define CAN_BUS_NUM_MASK 0x3FU
 
 #define BUS_MAX 4U
@@ -49,19 +47,19 @@ void process_can(uint8_t can_number);
 
 // ********************* instantiate queues *********************
 #define can_buffer(x, size) \
-  CAN_FIFOMailBox_TypeDef elems_##x[size]; \
-  can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = (CAN_FIFOMailBox_TypeDef *)&(elems_##x) };
+  CANPacket_t elems_##x[size]; \
+  can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = (CANPacket_t *)&(elems_##x) };
 
 #ifdef STM32H7
 __attribute__((section(".ram_d1"))) can_buffer(rx_q, 0x1000)
-__attribute__((section(".ram_d1"))) can_buffer(txgmlan_q, 0x100)
+__attribute__((section(".ram_d1"))) can_buffer(txgmlan_q, 0x1A0)
 #else
 can_buffer(rx_q, 0x1000)
-can_buffer(txgmlan_q, 0x100)
+can_buffer(txgmlan_q, 0x1A0)
 #endif
-can_buffer(tx1_q, 0x100)
-can_buffer(tx2_q, 0x100)
-can_buffer(tx3_q, 0x100)
+can_buffer(tx1_q, 0x1A0)
+can_buffer(tx2_q, 0x1A0)
+can_buffer(tx3_q, 0x1A0)
 // FIXME:
 // cppcheck-suppress misra-c2012-9.3
 can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q};
@@ -74,7 +72,7 @@ int can_err_cnt = 0;
 int can_overflow_cnt = 0;
 
 // ********************* interrupt safe queue *********************
-bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
+bool can_pop(can_ring *q, CANPacket_t *elem) {
   bool ret = 0;
 
   ENTER_CRITICAL();
@@ -92,7 +90,7 @@ bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
   return ret;
 }
 
-bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
+bool can_push(can_ring *q, CANPacket_t *elem) {
   bool ret = false;
   uint32_t next_w_ptr;
 
@@ -193,7 +191,7 @@ void can_flip_buses(uint8_t bus1, uint8_t bus2){
   bus_config[bus2].can_num_lookup = bus1;
 }
 
-void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+void ignition_can_hook(CANPacket_t *to_push) {
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
   int len = GET_LEN(to_push);
@@ -236,12 +234,10 @@ bool can_tx_check_min_slots_free(uint32_t min) {
     (can_slots_empty(&can_txgmlan_q) >= min);
 }
 
-void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) {
+void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook) {
   if (skip_tx_hook || safety_tx_hook(to_push) != 0) {
     if (bus_number < BUS_MAX) {
       // add CAN packet to send queue
-      // bus number isn't passed through
-      to_push->RDTR &= 0xF;
       if ((bus_number == 3U) && (bus_config[3].can_num_lookup == 0xFFU)) {
         gmlan_send_errs += bitbang_gmlan(to_push) ? 0U : 1U;
       } else {
@@ -250,7 +246,7 @@ void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx
       }
     }
   } else {
-    to_push->RDTR = (to_push->RDTR & 0xFFFF000FU) | ((CAN_BUS_RET_FLAG | CAN_BUS_BLK_FLAG | bus_number) << 4);
+    to_push->rejected = 1U;
     can_send_errs += can_push(&can_rx_q, to_push) ? 0U : 1U;
   }
 }
diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h
index a01a908d..3d7d376e 100644
--- a/board/drivers/fdcan.h
+++ b/board/drivers/fdcan.h
@@ -5,6 +5,11 @@
 #define BUS_OFF_FAIL_LIMIT 2U
 uint8_t bus_off_err[] = {0U, 0U, 0U};
 
+typedef struct {
+  volatile uint32_t header[2];
+  volatile uint32_t data_word[CANPACKET_DATA_SIZE_MAX/4U];
+} canfd_fifo;
+
 FDCAN_GlobalTypeDef *cans[] = {FDCAN1, FDCAN2, FDCAN3};
 
 bool can_set_speed(uint8_t can_number) {
@@ -46,32 +51,37 @@ void process_can(uint8_t can_number) {
     CANx->IR |= FDCAN_IR_TFE; // Clear Tx FIFO Empty flag
 
     if ((CANx->TXFQS & FDCAN_TXFQS_TFQF) == 0) {
-      CAN_FIFOMailBox_TypeDef to_send;
+      CANPacket_t to_send;
       if (can_pop(can_queues[bus_number], &to_send)) {
         can_tx_cnt += 1;
         uint32_t TxFIFOSA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET) + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE);
         uint8_t tx_index = (CANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1F;
         // only send if we have received a packet
-        CAN_FIFOMailBox_TypeDef *fifo;
-        fifo = (CAN_FIFOMailBox_TypeDef *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE));
+        canfd_fifo *fifo;
+        fifo = (canfd_fifo *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE));
 
-        // Convert from "mailbox type"
-        fifo->RIR = ((to_send.RIR & 0x6) << 28) | (to_send.RIR >> 3);  // identifier format and frame type | identifier
-        //REDEBUG: enable CAN FD and BRS for test purposes
-        //fifo->RDTR = ((to_send.RDTR & 0xF) << 16) | ((to_send.RDTR) >> 16) | (1U << 21) | (1U << 20); // DLC (length) | timestamp | enable CAN FD | enable BRS
-        fifo->RDTR = ((to_send.RDTR & 0xF) << 16) | ((to_send.RDTR) >> 16); // DLC (length) | timestamp
-        fifo->RDLR = to_send.RDLR;
-        fifo->RDHR = to_send.RDHR;
+        fifo->header[0] = (to_send.extended << 30) | ((to_send.extended != 0) ? (to_send.addr) : (to_send.addr << 18));
+        fifo->header[1] = (to_send.data_len_code << 16); // | (1U << 21) | (1U << 20) to enable CAN FD , enable BRS
+
+        uint8_t data_len_w = (dlc_to_len[to_send.data_len_code] / 4U);
+        data_len_w += ((dlc_to_len[to_send.data_len_code] % 4) > 0) ? 1U : 0U;
+        for (unsigned int i = 0; i < data_len_w; i++) {
+          BYTE_ARRAY_TO_WORD(fifo->data_word[i], &to_send.data[i*4]);
+        }
 
         CANx->TXBAR = (1UL << tx_index);
 
         // Send back to USB
         can_txd_cnt += 1;
-        CAN_FIFOMailBox_TypeDef to_push;
-        to_push.RIR = to_send.RIR;
-        to_push.RDTR = (to_send.RDTR & 0xFFFF000FU) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
-        to_push.RDLR = to_send.RDLR;
-        to_push.RDHR = to_send.RDHR;
+        CANPacket_t to_push;
+
+        to_push.returned = 1U;
+        to_push.rejected = 0U;
+        to_push.extended = to_send.extended;
+        to_push.addr = to_send.addr;
+        to_push.bus = to_send.bus;
+        to_push.data_len_code = to_send.data_len_code;
+        (void)memcpy(to_push.data, to_send.data, dlc_to_len[to_push.data_len_code]);
         can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
 
         usb_cb_ep3_out_complete();
@@ -123,29 +133,37 @@ void can_rx(uint8_t can_number) {
       rx_fifo_idx = (uint8_t)((CANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F);
 
       uint32_t RxFIFO0SA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET);
-      CAN_FIFOMailBox_TypeDef to_push;
-      CAN_FIFOMailBox_TypeDef *fifo;
+      CANPacket_t to_push;
+      canfd_fifo *fifo;
 
       // getting address
-      fifo = (CAN_FIFOMailBox_TypeDef *)(RxFIFO0SA + (rx_fifo_idx * FDCAN_RX_FIFO_0_EL_SIZE));
+      fifo = (canfd_fifo *)(RxFIFO0SA + (rx_fifo_idx * FDCAN_RX_FIFO_0_EL_SIZE));
 
-      // Need to convert real CAN frame format to mailbox "type"
-      to_push.RIR = ((fifo->RIR >> 28) & 0x6) | (fifo->RIR << 3); // identifier format and frame type | identifier
-      to_push.RDTR = ((fifo->RDTR >> 16) & 0xF) | (fifo->RDTR << 16); // DLC (length) | timestamp
-      to_push.RDLR = fifo->RDLR;
-      to_push.RDHR = fifo->RDHR;
+      to_push.returned = 0U;
+      to_push.rejected = 0U;
+      to_push.extended = (fifo->header[0] >> 30) & 0x1U;
+      to_push.addr = ((to_push.extended != 0) ? (fifo->header[0] & 0x1FFFFFFFU) : ((fifo->header[0] >> 18) & 0x7FFU));
+      to_push.bus = bus_number;
+      to_push.data_len_code = ((fifo->header[1] >> 16) & 0xFU);
 
-      // modify RDTR for our API
-      to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
+      uint8_t data_len_w = (dlc_to_len[to_push.data_len_code] / 4U);
+      data_len_w += ((dlc_to_len[to_push.data_len_code] % 4) > 0) ? 1U : 0U;
+      for (unsigned int i = 0; i < data_len_w; i++) {
+        WORD_TO_BYTE_ARRAY(&to_push.data[i*4], fifo->data_word[i]);
+      }
 
       // forwarding (panda only)
       int bus_fwd_num = safety_fwd_hook(bus_number, &to_push);
       if (bus_fwd_num != -1) {
-        CAN_FIFOMailBox_TypeDef to_send;
-        to_send.RIR = to_push.RIR;
-        to_send.RDTR = to_push.RDTR;
-        to_send.RDLR = to_push.RDLR;
-        to_send.RDHR = to_push.RDHR;
+        CANPacket_t to_send;
+
+        to_send.returned = 0U;
+        to_push.rejected = 0U;
+        to_send.extended = to_push.extended;
+        to_send.addr = to_push.addr;
+        to_send.bus = to_push.bus;
+        to_send.data_len_code = to_push.data_len_code;
+        (void)memcpy(to_send.data, to_push.data, dlc_to_len[to_push.data_len_code]);
         can_send(&to_send, bus_fwd_num, true);
       }
 
diff --git a/board/drivers/gmlan_alt.h b/board/drivers/gmlan_alt.h
index 063ac850..9b95e0dd 100644
--- a/board/drivers/gmlan_alt.h
+++ b/board/drivers/gmlan_alt.h
@@ -75,7 +75,7 @@ int append_int(char *in, int in_len, int val, int val_len) {
   return in_len_copy;
 }
 
-int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
+int get_bit_message(char *out, CANPacket_t *to_bang) {
   char pkt[MAX_BITS_CAN_PACKET];
   char footer[] = {
     1,  // CRC delimiter
@@ -88,18 +88,18 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
   int len = 0;
 
   // test packet
-  int dlc_len = to_bang->RDTR & 0xF;
+  int dlc_len = GET_LEN(to_bang);
   len = append_int(pkt, len, 0, 1);    // Start-of-frame
 
-  if ((to_bang->RIR & 4) != 0) {
+  if (to_bang->extended != 0) {
     // extended identifier
-    len = append_int(pkt, len, to_bang->RIR >> 21, 11);  // Identifier
+    len = append_int(pkt, len, GET_ADDR(to_bang) >> 18, 11);  // Identifier
     len = append_int(pkt, len, 3, 2);    // SRR+IDE
-    len = append_int(pkt, len, (to_bang->RIR >> 3) & ((1U << 18) - 1U), 18);  // Identifier
+    len = append_int(pkt, len, (GET_ADDR(to_bang)) & ((1U << 18) - 1U), 18);  // Identifier
     len = append_int(pkt, len, 0, 3);    // RTR+r1+r0
   } else {
     // standard identifier
-    len = append_int(pkt, len, to_bang->RIR >> 21, 11);  // Identifier
+    len = append_int(pkt, len, GET_ADDR(to_bang), 11);  // Identifier
     len = append_int(pkt, len, 0, 3);    // RTR+IDE+reserved
   }
 
@@ -107,8 +107,7 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
 
   // append data
   for (int i = 0; i < dlc_len; i++) {
-    unsigned char dat = ((unsigned char *)(&(to_bang->RDLR)))[i];
-    len = append_int(pkt, len, dat, 8);
+    len = append_int(pkt, len, to_bang->data[i], 8);
   }
 
   // append crc
@@ -269,7 +268,7 @@ void TIM12_IRQ_Handler(void) {
   TIM12->SR = 0;
 }
 
-bool bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
+bool bitbang_gmlan(CANPacket_t *to_bang) {
   gmlan_send_ok = true;
   gmlan_alt_mode = BITBANG;
 
diff --git a/board/drivers/usb.h b/board/drivers/usb.h
index 2316f722..6b743bd1 100644
--- a/board/drivers/usb.h
+++ b/board/drivers/usb.h
@@ -23,7 +23,8 @@ typedef union _USB_Setup {
 }
 USB_Setup_TypeDef;
 
-#define MAX_CAN_MSGS_PER_BULK_TRANSFER 4U
+#define MAX_CAN_MSGS_PER_BULK_TRANSFER 51U
+#define MAX_EP1_CHUNK_PER_BULK_TRANSFER 16256 // max data stream chunk in bytes, shouldn't be higher than 16320 or counter will overflow
 
 bool usb_eopf_detected = false;
 
@@ -493,7 +494,7 @@ void usb_setup(void) {
                                USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP;
       USBx_OUTEP(2)->DOEPINT = 0xFF;
 
-      USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
+      USBx_OUTEP(3)->DOEPTSIZ = (32U << 19) | 0x800U;
       USBx_OUTEP(3)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2U << 18) |
                                USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP;
       USBx_OUTEP(3)->DOEPINT = 0xFF;
@@ -934,7 +935,7 @@ void usb_irqhandler(void) {
 void usb_outep3_resume_if_paused(void) {
   ENTER_CRITICAL();
   if (!outep3_processing && (USBx_OUTEP(3)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0) {
-    USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
+    USBx_OUTEP(3)->DOEPTSIZ = (32U << 19) | 0x800U;
     USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
   }
   EXIT_CRITICAL();
diff --git a/board/flasher.h b/board/flasher.h
index 764eddb9..534b65e7 100644
--- a/board/flasher.h
+++ b/board/flasher.h
@@ -183,7 +183,7 @@ void CAN1_RX0_IRQ_Handler(void) {
     if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
       uint8_t dat[8];
       for (int i = 0; i < 8; i++) {
-        dat[i] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
+        dat[i] = GET_MAILBOX_BYTE(&CAN->sFIFOMailBox[0], i);
       }
       uint8_t odat[8];
       uint8_t type = dat[0] & 0xF0;
diff --git a/board/main.c b/board/main.c
index c3b27c58..c0fa83e9 100644
--- a/board/main.c
+++ b/board/main.c
@@ -20,6 +20,8 @@
   #include "drivers/bxcan.h"
 #endif
 
+#include "usb_protocol.h"
+
 #include "obj/gitversion.h"
 
 extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used
@@ -193,15 +195,7 @@ int get_rtc_pkt(void *dat) {
   return sizeof(t);
 }
 
-int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
-  UNUSED(hardwired);
-  CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata;
-  int ilen = 0;
-  while (ilen < MIN(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) {
-    ilen++;
-  }
-  return ilen*0x10;
-}
+
 
 // send on serial, first byte to select the ring
 void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
@@ -219,26 +213,8 @@ void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
   }
 }
 
-// send on CAN
-void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
-  UNUSED(hardwired);
-  int dpkt = 0;
-  uint32_t *d32 = (uint32_t *)usbdata;
-  for (dpkt = 0; dpkt < (len / 4); dpkt += 4) {
-    CAN_FIFOMailBox_TypeDef to_push;
-    to_push.RDHR = d32[dpkt + 3];
-    to_push.RDLR = d32[dpkt + 2];
-    to_push.RDTR = d32[dpkt + 1];
-    to_push.RIR = d32[dpkt];
-
-    uint8_t bus_number = (to_push.RDTR >> 4) & CAN_BUS_NUM_MASK;
-    can_send(&to_push, bus_number, false);
-  }
-}
-
 void usb_cb_ep3_out_complete(void) {
-  // TODO: how does a second USB packet sneek in? (why multiply by 2)
-  if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER * 2U)) {
+  if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
     usb_outep3_resume_if_paused();
   }
 }
diff --git a/board/pedal/main.c b/board/pedal/main.c
index 32d85aab..7553b3e5 100644
--- a/board/pedal/main.c
+++ b/board/pedal/main.c
@@ -132,11 +132,11 @@ void CAN1_RX0_IRQ_Handler(void) {
     int address = CAN->sFIFOMailBox[0].RIR >> 21;
     if (address == CAN_GAS_INPUT) {
       // softloader entry
-      if (GET_BYTES_04(&CAN->sFIFOMailBox[0]) == 0xdeadface) {
-        if (GET_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x0ab00b1e) {
+      if (GET_MAILBOX_BYTES_04(&CAN->sFIFOMailBox[0]) == 0xdeadface) {
+        if (GET_MAILBOX_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x0ab00b1e) {
           enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
           NVIC_SystemReset();
-        } else if (GET_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x02b00b1e) {
+        } else if (GET_MAILBOX_BYTES_48(&CAN->sFIFOMailBox[0]) == 0x02b00b1e) {
           enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
           NVIC_SystemReset();
         } else {
@@ -147,7 +147,7 @@ void CAN1_RX0_IRQ_Handler(void) {
       // normal packet
       uint8_t dat[8];
       for (int i=0; i<8; i++) {
-        dat[i] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
+        dat[i] = GET_MAILBOX_BYTE(&CAN->sFIFOMailBox[0], i);
       }
       uint16_t value_0 = (dat[0] << 8) | dat[1];
       uint16_t value_1 = (dat[2] << 8) | dat[3];
diff --git a/board/safety.h b/board/safety.h
index 951eede7..fd151627 100644
--- a/board/safety.h
+++ b/board/safety.h
@@ -44,11 +44,11 @@ int16_t current_safety_param = 0;
 const safety_hooks *current_hooks = &nooutput_hooks;
 const addr_checks *current_rx_checks = &default_rx_checks;
 
-int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+int safety_rx_hook(CANPacket_t *to_push) {
   return current_hooks->rx(to_push);
 }
 
-int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+int safety_tx_hook(CANPacket_t *to_send) {
   return (relay_malfunction ? -1 : current_hooks->tx(to_send));
 }
 
@@ -56,7 +56,7 @@ int safety_tx_lin_hook(int lin_num, uint8_t *data, int len) {
   return current_hooks->tx_lin(lin_num, data, len);
 }
 
-int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+int safety_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   return (relay_malfunction ? -1 : current_hooks->fwd(bus_num, to_fwd));
 }
 
@@ -75,7 +75,7 @@ void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) {
   }
 }
 
-bool msg_allowed(CAN_FIFOMailBox_TypeDef *to_send, const CanMsg msg_list[], int len) {
+bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len) {
   int addr = GET_ADDR(to_send);
   int bus = GET_BUS(to_send);
   int length = GET_LEN(to_send);
@@ -96,7 +96,7 @@ uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
   return ts - ts_last;
 }
 
-int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len) {
+int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], const int len) {
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
   int length = GET_LEN(to_push);
@@ -170,11 +170,11 @@ void update_addr_timestamp(AddrCheckStruct addr_list[], int index) {
   }
 }
 
-bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push,
+bool addr_safety_check(CANPacket_t *to_push,
                        const addr_checks *rx_checks,
-                       uint8_t (*get_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
-                       uint8_t (*compute_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
-                       uint8_t (*get_counter)(CAN_FIFOMailBox_TypeDef *to_push)) {
+                       uint8_t (*get_checksum)(CANPacket_t *to_push),
+                       uint8_t (*compute_checksum)(CANPacket_t *to_push),
+                       uint8_t (*get_counter)(CANPacket_t *to_push)) {
 
   int index = get_addr_check_index(to_push, rx_checks->check, rx_checks->len);
   update_addr_timestamp(rx_checks->check, index);
diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h
index ad30f4ca..35474f34 100644
--- a/board/safety/safety_chrysler.h
+++ b/board/safety/safety_chrysler.h
@@ -18,12 +18,12 @@ AddrCheckStruct chrysler_addr_checks[] = {
 #define CHRYSLER_ADDR_CHECK_LEN (sizeof(chrysler_addr_checks) / sizeof(chrysler_addr_checks[0]))
 addr_checks chrysler_rx_checks = {chrysler_addr_checks, CHRYSLER_ADDR_CHECK_LEN};
 
-static uint8_t chrysler_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t chrysler_get_checksum(CANPacket_t *to_push) {
   int checksum_byte = GET_LEN(to_push) - 1;
   return (uint8_t)(GET_BYTE(to_push, checksum_byte));
 }
 
-static uint8_t chrysler_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t chrysler_compute_checksum(CANPacket_t *to_push) {
   /* This function does not want the checksum byte in the input data.
   jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf */
   uint8_t checksum = 0xFFU;
@@ -56,12 +56,12 @@ static uint8_t chrysler_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
   return ~checksum;
 }
 
-static uint8_t chrysler_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t chrysler_get_counter(CANPacket_t *to_push) {
   // Well defined counter only for 8 bytes messages
   return (uint8_t)(GET_BYTE(to_push, 6) >> 4);
 }
 
-static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int chrysler_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &chrysler_rx_checks,
                                  chrysler_get_checksum, chrysler_compute_checksum,
@@ -117,7 +117,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   return valid;
 }
 
-static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int chrysler_tx_hook(CANPacket_t *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
@@ -182,7 +182,7 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int chrysler_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
 
   int bus_fwd = -1;
   int addr = GET_ADDR(to_fwd);
diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h
index cdbb656e..409ab0c8 100644
--- a/board/safety/safety_defaults.h
+++ b/board/safety/safety_defaults.h
@@ -3,7 +3,7 @@ const addr_checks default_rx_checks = {
   .len = 0,
 };
 
-int default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+int default_rx_hook(CANPacket_t *to_push) {
   UNUSED(to_push);
   return true;
 }
@@ -17,7 +17,7 @@ static const addr_checks* nooutput_init(int16_t param) {
   return &default_rx_checks;
 }
 
-static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int nooutput_tx_hook(CANPacket_t *to_send) {
   UNUSED(to_send);
   return false;
 }
@@ -29,7 +29,7 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
   return false;
 }
 
-static int default_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int default_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   UNUSED(bus_num);
   UNUSED(to_fwd);
   return -1;
@@ -52,7 +52,7 @@ static const addr_checks* alloutput_init(int16_t param) {
   return &default_rx_checks;
 }
 
-static int alloutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int alloutput_tx_hook(CANPacket_t *to_send) {
   UNUSED(to_send);
   return true;
 }
diff --git a/board/safety/safety_elm327.h b/board/safety/safety_elm327.h
index e6655c3d..d4f562a8 100644
--- a/board/safety/safety_elm327.h
+++ b/board/safety/safety_elm327.h
@@ -1,4 +1,4 @@
-static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int elm327_tx_hook(CANPacket_t *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h
index cc771c44..6a8f0583 100644
--- a/board/safety/safety_ford.h
+++ b/board/safety/safety_ford.h
@@ -8,7 +8,7 @@
 //      brake > 0mph
 
 
-static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int ford_rx_hook(CANPacket_t *to_push) {
 
   int addr = GET_ADDR(to_push);
   int bus = GET_BUS(to_push);
@@ -65,7 +65,7 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 // else
 //     block all commands that produce actuation
 
-static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int ford_tx_hook(CANPacket_t *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h
index efe749a3..6ef30f27 100644
--- a/board/safety/safety_gm.h
+++ b/board/safety/safety_gm.h
@@ -34,7 +34,7 @@ AddrCheckStruct gm_addr_checks[] = {
 #define GM_RX_CHECK_LEN (sizeof(gm_addr_checks) / sizeof(gm_addr_checks[0]))
 addr_checks gm_rx_checks = {gm_addr_checks, GM_RX_CHECK_LEN};
 
-static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int gm_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &gm_rx_checks, NULL, NULL, NULL);
 
@@ -104,7 +104,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 // else
 //     block all commands that produce actuation
 
-static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int gm_tx_hook(CANPacket_t *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h
index 89e2b5a0..8b797e67 100644
--- a/board/safety/safety_honda.h
+++ b/board/safety/safety_honda.h
@@ -53,12 +53,12 @@ enum {HONDA_N_HW, HONDA_BG_HW, HONDA_BH_HW} honda_hw = HONDA_N_HW;
 addr_checks honda_rx_checks = {honda_addr_checks, HONDA_ADDR_CHECKS_LEN};
 
 
-static uint8_t honda_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t honda_get_checksum(CANPacket_t *to_push) {
   int checksum_byte = GET_LEN(to_push) - 1;
   return (uint8_t)(GET_BYTE(to_push, checksum_byte)) & 0xFU;
 }
 
-static uint8_t honda_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t honda_compute_checksum(CANPacket_t *to_push) {
   int len = GET_LEN(to_push);
   uint8_t checksum = 0U;
   unsigned int addr = GET_ADDR(to_push);
@@ -75,12 +75,12 @@ static uint8_t honda_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
   return (8U - checksum) & 0xFU;
 }
 
-static uint8_t honda_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t honda_get_counter(CANPacket_t *to_push) {
   int counter_byte = GET_LEN(to_push) - 1;
   return ((uint8_t)(GET_BYTE(to_push, counter_byte)) >> 4U) & 0x3U;
 }
 
-static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int honda_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &honda_rx_checks,
                                  honda_get_checksum, honda_compute_checksum, honda_get_counter);
@@ -186,7 +186,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 // else
 //     block all commands that produce actuation
 
-static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int honda_tx_hook(CANPacket_t *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
@@ -345,7 +345,7 @@ static const addr_checks* honda_bosch_harness_init(int16_t param) {
   return &honda_rx_checks;
 }
 
-static int honda_nidec_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int honda_nidec_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   // fwd from car to camera. also fwd certain msgs from camera to car
   // 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX,
   // 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud,
@@ -370,7 +370,7 @@ static int honda_nidec_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   return bus_fwd;
 }
 
-static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int honda_bosch_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   int bus_fwd = -1;
   int bus_rdr_cam = (honda_hw == HONDA_BH_HW) ? 2 : 1;  // radar bus, camera side
   int bus_rdr_car = (honda_hw == HONDA_BH_HW) ? 0 : 2;  // radar bus, car side
diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h
index 59b56fa1..4e4f0e65 100644
--- a/board/safety/safety_hyundai.h
+++ b/board/safety/safety_hyundai.h
@@ -69,7 +69,7 @@ bool hyundai_longitudinal = false;
 
 addr_checks hyundai_rx_checks = {hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
 
-static uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t hyundai_get_counter(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
   uint8_t cnt;
@@ -89,7 +89,7 @@ static uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
   return cnt;
 }
 
-static uint8_t hyundai_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t hyundai_get_checksum(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
   uint8_t chksum;
@@ -107,7 +107,7 @@ static uint8_t hyundai_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
   return chksum;
 }
 
-static uint8_t hyundai_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t hyundai_compute_checksum(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
   uint8_t chksum = 0;
@@ -143,7 +143,7 @@ static uint8_t hyundai_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
   return chksum;
 }
 
-static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int hyundai_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &hyundai_rx_checks,
                                  hyundai_get_checksum, hyundai_compute_checksum,
@@ -223,7 +223,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   return valid;
 }
 
-static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int hyundai_tx_hook(CANPacket_t *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
@@ -338,7 +338,7 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int hyundai_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
 
   int bus_fwd = -1;
   int addr = GET_ADDR(to_fwd);
diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h
index 07a8ef09..01cc29f1 100644
--- a/board/safety/safety_mazda.h
+++ b/board/safety/safety_mazda.h
@@ -37,7 +37,7 @@ AddrCheckStruct mazda_addr_checks[] = {
 addr_checks mazda_rx_checks = {mazda_addr_checks, MAZDA_ADDR_CHECKS_LEN};
 
 // track msgs coming from OP so that we know what CAM msgs to drop and what to forward
-static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int mazda_rx_hook(CANPacket_t *to_push) {
   bool valid = addr_safety_check(to_push, &mazda_rx_checks, NULL, NULL, NULL);
   if (valid && (GET_BUS(to_push) == MAZDA_MAIN)) {
     int addr = GET_ADDR(to_push);
@@ -80,7 +80,7 @@ static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   return valid;
 }
 
-static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int mazda_tx_hook(CANPacket_t *to_send) {
   int tx = 1;
   int addr = GET_ADDR(to_send);
   int bus = GET_BUS(to_send);
@@ -153,7 +153,7 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static int mazda_fwd_hook(int bus, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int mazda_fwd_hook(int bus, CANPacket_t *to_fwd) {
   int bus_fwd = -1;
   int addr = GET_ADDR(to_fwd);
 
diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h
index af170770..2c2c6f68 100644
--- a/board/safety/safety_nissan.h
+++ b/board/safety/safety_nissan.h
@@ -41,7 +41,7 @@ addr_checks nissan_rx_checks = {nissan_addr_checks, NISSAN_ADDR_CHECK_LEN};
 // EPS Location. false = V-CAN, true = C-CAN
 bool nissan_alt_eps = false;
 
-static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int nissan_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &nissan_rx_checks, NULL, NULL, NULL);
 
@@ -106,7 +106,7 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 }
 
 
-static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int nissan_tx_hook(CANPacket_t *to_send) {
   int tx = 1;
   int addr = GET_ADDR(to_send);
   bool violation = 0;
@@ -166,7 +166,7 @@ static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 }
 
 
-static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int nissan_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   int bus_fwd = -1;
   int addr = GET_ADDR(to_fwd);
 
diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index 09f7f9b9..36eecb51 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -37,15 +37,15 @@ AddrCheckStruct subaru_l_addr_checks[] = {
 #define SUBARU_L_ADDR_CHECK_LEN (sizeof(subaru_l_addr_checks) / sizeof(subaru_l_addr_checks[0]))
 addr_checks subaru_l_rx_checks = {subaru_l_addr_checks, SUBARU_L_ADDR_CHECK_LEN};
 
-static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t subaru_get_checksum(CANPacket_t *to_push) {
   return (uint8_t)GET_BYTE(to_push, 0);
 }
 
-static uint8_t subaru_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t subaru_get_counter(CANPacket_t *to_push) {
   return (uint8_t)(GET_BYTE(to_push, 1) & 0xF);
 }
 
-static uint8_t subaru_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t subaru_compute_checksum(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
   int len = GET_LEN(to_push);
   uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U);
@@ -55,7 +55,7 @@ static uint8_t subaru_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
   return checksum;
 }
 
-static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int subaru_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &subaru_rx_checks,
                             subaru_get_checksum, subaru_compute_checksum, subaru_get_counter);
@@ -102,7 +102,7 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   return valid;
 }
 
-static int subaru_legacy_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int subaru_legacy_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &subaru_l_rx_checks, NULL, NULL, NULL);
 
@@ -148,7 +148,7 @@ static int subaru_legacy_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   return valid;
 }
 
-static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int subaru_tx_hook(CANPacket_t *to_send) {
   int tx = 1;
   int addr = GET_ADDR(to_send);
 
@@ -208,7 +208,7 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static int subaru_legacy_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int subaru_legacy_tx_hook(CANPacket_t *to_send) {
   int tx = 1;
   int addr = GET_ADDR(to_send);
 
@@ -268,7 +268,7 @@ static int subaru_legacy_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int subaru_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   int bus_fwd = -1;
 
   if (bus_num == 0) {
@@ -290,7 +290,7 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   return bus_fwd;
 }
 
-static int subaru_legacy_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int subaru_legacy_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   int bus_fwd = -1;
 
   if (bus_num == 0) {
diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h
index 9cca63ae..aa771d35 100644
--- a/board/safety/safety_tesla.h
+++ b/board/safety/safety_tesla.h
@@ -28,7 +28,7 @@ addr_checks tesla_rx_checks = {tesla_addr_checks, TESLA_ADDR_CHECK_LEN};
 
 bool autopilot_enabled = false;
 
-static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int tesla_rx_hook(CANPacket_t *to_push) {
   bool valid = addr_safety_check(to_push, &tesla_rx_checks,
                                  NULL, NULL, NULL);
 
@@ -101,7 +101,7 @@ static int tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 }
 
 
-static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int tesla_tx_hook(CANPacket_t *to_send) {
   int tx = 1;
   int addr = GET_ADDR(to_send);
   bool violation = false;
@@ -162,7 +162,7 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int tesla_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   int bus_fwd = -1;
   int addr = GET_ADDR(to_fwd);
 
diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
index 401f9559..843acab0 100644
--- a/board/safety/safety_toyota.h
+++ b/board/safety/safety_toyota.h
@@ -44,7 +44,7 @@ addr_checks toyota_rx_checks = {toyota_addr_checks, TOYOTA_ADDR_CHECKS_LEN};
 // global actuation limit states
 int toyota_dbc_eps_torque_factor = 100;   // conversion factor for STEER_TORQUE_EPS in %: see dbc file
 
-static uint8_t toyota_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t toyota_compute_checksum(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
   int len = GET_LEN(to_push);
   uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U) + (uint8_t)(len);
@@ -54,12 +54,12 @@ static uint8_t toyota_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
   return checksum;
 }
 
-static uint8_t toyota_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t toyota_get_checksum(CANPacket_t *to_push) {
   int checksum_byte = GET_LEN(to_push) - 1;
   return (uint8_t)(GET_BYTE(to_push, checksum_byte));
 }
 
-static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int toyota_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &toyota_rx_checks,
                                  toyota_get_checksum, toyota_compute_checksum, NULL);
@@ -134,7 +134,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   return valid;
 }
 
-static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int toyota_tx_hook(CANPacket_t *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
@@ -247,7 +247,8 @@ static const addr_checks* toyota_init(int16_t param) {
   return &toyota_rx_checks;
 }
 
-static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int toyota_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
+
   int bus_fwd = -1;
 
   if (bus_num == 0) {
diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h
index d7c06a36..42c6cac2 100644
--- a/board/safety/safety_volkswagen.h
+++ b/board/safety/safety_volkswagen.h
@@ -58,22 +58,22 @@ int volkswagen_lane_msg = 0;
 uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
 
 
-static uint8_t volkswagen_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_get_checksum(CANPacket_t *to_push) {
   return (uint8_t)GET_BYTE(to_push, 0);
 }
 
-static uint8_t volkswagen_mqb_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_mqb_get_counter(CANPacket_t *to_push) {
   // MQB message counters are consistently found at LSB 8.
   return (uint8_t)GET_BYTE(to_push, 1) & 0xFU;
 }
 
-static uint8_t volkswagen_pq_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_pq_get_counter(CANPacket_t *to_push) {
   // Few PQ messages have counters, and their offsets are inconsistent. This
   // function works only for Lenkhilfe_3 at this time.
   return (uint8_t)(GET_BYTE(to_push, 1) & 0xF0U) >> 4;
 }
 
-static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_mqb_compute_crc(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
   int len = GET_LEN(to_push);
 
@@ -108,7 +108,7 @@ static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) {
   return crc ^ 0xFFU;
 }
 
-static uint8_t volkswagen_pq_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
+static uint8_t volkswagen_pq_compute_checksum(CANPacket_t *to_push) {
   int len = GET_LEN(to_push);
   uint8_t checksum = 0U;
 
@@ -140,7 +140,7 @@ static const addr_checks* volkswagen_pq_init(int16_t param) {
   return &volkswagen_pq_rx_checks;
 }
 
-static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int volkswagen_mqb_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &volkswagen_mqb_rx_checks,
                                  volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_mqb_get_counter);
@@ -200,7 +200,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   return valid;
 }
 
-static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+static int volkswagen_pq_rx_hook(CANPacket_t *to_push) {
 
   bool valid = addr_safety_check(to_push, &volkswagen_pq_rx_checks,
                                 volkswagen_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_get_counter);
@@ -297,7 +297,7 @@ static bool volkswagen_steering_check(int desired_torque) {
   return violation;
 }
 
-static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int volkswagen_mqb_tx_hook(CANPacket_t *to_send) {
   int addr = GET_ADDR(to_send);
   int tx = 1;
 
@@ -333,7 +333,7 @@ static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static int volkswagen_pq_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+static int volkswagen_pq_tx_hook(CANPacket_t *to_send) {
   int addr = GET_ADDR(to_send);
   int tx = 1;
 
@@ -370,7 +370,7 @@ static int volkswagen_pq_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+static int volkswagen_fwd_hook(int bus_num, CANPacket_t *to_fwd) {
   int addr = GET_ADDR(to_fwd);
   int bus_fwd = -1;
 
diff --git a/board/safety_declarations.h b/board/safety_declarations.h
index a502a320..4754738f 100644
--- a/board/safety_declarations.h
+++ b/board/safety_declarations.h
@@ -1,3 +1,8 @@
+#define GET_BYTE(msg, b) ((msg)->data[(b)])
+#define GET_BYTES_04(msg) ((msg)->data[0] | ((msg)->data[1] << 8) | ((msg)->data[2] << 16) | ((msg)->data[3] << 24))
+#define GET_BYTES_48(msg) ((msg)->data[4] | ((msg)->data[5] << 8) | ((msg)->data[6] << 16) | ((msg)->data[7] << 24))
+#define GET_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask))
+
 const int MAX_WRONG_COUNTERS = 5;
 const uint8_t MAX_MISSED_MSGS = 10U;
 
@@ -48,8 +53,8 @@ typedef struct {
   int len;
 } addr_checks;
 
-int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
-int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
+int safety_rx_hook(CANPacket_t *to_push);
+int safety_tx_hook(CANPacket_t *to_send);
 int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
 uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
 int to_signed(int d, int bits);
@@ -63,25 +68,25 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
 bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
 float interpolate(struct lookup_t xy, float x);
 void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]);
-bool msg_allowed(CAN_FIFOMailBox_TypeDef *to_send, const CanMsg msg_list[], int len);
-int get_addr_check_index(CAN_FIFOMailBox_TypeDef *to_push, AddrCheckStruct addr_list[], const int len);
+bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len);
+int get_addr_check_index(CANPacket_t *to_push, AddrCheckStruct addr_list[], const int len);
 void update_counter(AddrCheckStruct addr_list[], int index, uint8_t counter);
 void update_addr_timestamp(AddrCheckStruct addr_list[], int index);
 bool is_msg_valid(AddrCheckStruct addr_list[], int index);
-bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push,
+bool addr_safety_check(CANPacket_t *to_push,
                        const addr_checks *rx_checks,
-                       uint8_t (*get_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
-                       uint8_t (*compute_checksum)(CAN_FIFOMailBox_TypeDef *to_push),
-                       uint8_t (*get_counter)(CAN_FIFOMailBox_TypeDef *to_push));
+                       uint8_t (*get_checksum)(CANPacket_t *to_push),
+                       uint8_t (*compute_checksum)(CANPacket_t *to_push),
+                       uint8_t (*get_counter)(CANPacket_t *to_push));
 void generic_rx_checks(bool stock_ecu_detected);
 void relay_malfunction_set(void);
 void relay_malfunction_reset(void);
 
 typedef const addr_checks* (*safety_hook_init)(int16_t param);
-typedef int (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
-typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send);
+typedef int (*rx_hook)(CANPacket_t *to_push);
+typedef int (*tx_hook)(CANPacket_t *to_send);
 typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len);
-typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
+typedef int (*fwd_hook)(int bus_num, CANPacket_t *to_fwd);
 
 typedef struct {
   safety_hook_init init;
diff --git a/board/stm32fx/stm32fx_config.h b/board/stm32fx/stm32fx_config.h
index 0777d2eb..8c5ca1c7 100644
--- a/board/stm32fx/stm32fx_config.h
+++ b/board/stm32fx/stm32fx_config.h
@@ -38,6 +38,10 @@
 #define PROVISION_CHUNK_ADDRESS 0x1FFF79E0U
 #define DEVICE_SERIAL_NUMBER_ADDRESS 0x1FFF79C0U
 
+#define CANPACKET_DATA_SIZE_MAX 8U
+
+#include "can_definitions.h"
+
 #ifndef BOOTSTUB
   #ifdef PANDA
     #include "main_declarations.h"
diff --git a/board/stm32h7/llfdcan.h b/board/stm32h7/llfdcan.h
index 2750f642..c3afadae 100644
--- a/board/stm32h7/llfdcan.h
+++ b/board/stm32h7/llfdcan.h
@@ -33,13 +33,6 @@
 #define CAN_NAME_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? "FDCAN1" : (((CAN_DEV) == FDCAN2) ? "FDCAN2" : "FDCAN3"))
 #define CAN_NUM_FROM_CANIF(CAN_DEV) (((CAN_DEV)==FDCAN1) ? 0UL : (((CAN_DEV) == FDCAN2) ? 1UL : 2UL))
 
-// For backwards compatibility with safety code
-typedef struct {
-  __IO uint32_t RIR;  /*!< CAN receive FIFO mailbox identifier register */
-  __IO uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */
-  __IO uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */
-  __IO uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */
-} CAN_FIFOMailBox_TypeDef;
 
 void puts(const char *a);
 
diff --git a/board/stm32h7/stm32h7_config.h b/board/stm32h7/stm32h7_config.h
index a295d18a..cdc2a9dc 100644
--- a/board/stm32h7/stm32h7_config.h
+++ b/board/stm32h7/stm32h7_config.h
@@ -34,6 +34,10 @@
 #define PROVISION_CHUNK_ADDRESS 0x080FFFE0U
 #define DEVICE_SERIAL_NUMBER_ADDRESS 0x080FFFC0U
 
+#define CANPACKET_DATA_SIZE_MAX 64U
+
+#include "can_definitions.h"
+
 #ifndef BOOTSTUB
   #include "main_declarations.h"
 #else
diff --git a/board/usb_protocol.h b/board/usb_protocol.h
new file mode 100644
index 00000000..0e50aa7a
--- /dev/null
+++ b/board/usb_protocol.h
@@ -0,0 +1,108 @@
+typedef struct {
+  uint8_t ptr;
+  uint8_t tail_size;
+  uint8_t data[72];
+  uint8_t counter;
+} usb_asm_buffer;
+
+usb_asm_buffer ep1_buffer = {.ptr = 0, .tail_size = 0, .counter = 0};
+uint32_t total_rx_size = 0;
+
+int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
+  UNUSED(hardwired);
+  uint8_t pos = 1;
+  uint8_t *usbdata8 = (uint8_t *)usbdata;
+  usbdata8[0] = ep1_buffer.counter;
+  // Send tail of previous message if it is in buffer
+  if (ep1_buffer.ptr > 0) {
+    if (ep1_buffer.ptr <= 63U) {
+      (void)memcpy(&usbdata8[pos], ep1_buffer.data, ep1_buffer.ptr);
+      pos += ep1_buffer.ptr;
+      ep1_buffer.ptr = 0;
+    } else {
+      (void)memcpy(&usbdata8[pos], ep1_buffer.data, 63U);
+      ep1_buffer.ptr = ep1_buffer.ptr - 63U;
+      (void)memcpy(ep1_buffer.data, &ep1_buffer.data[63U], ep1_buffer.ptr);
+      pos += 63U;
+    }
+  }
+
+  if (total_rx_size > MAX_EP1_CHUNK_PER_BULK_TRANSFER) {
+    total_rx_size = 0;
+    ep1_buffer.counter = 0;
+  } else {
+    CANPacket_t can_packet;
+    while ((pos < len) && can_pop(&can_rx_q, &can_packet)) {
+      uint8_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[can_packet.data_len_code];
+      if ((pos + pckt_len) <= len) {
+        (void)memcpy(&usbdata8[pos], &can_packet, pckt_len);
+        pos += pckt_len;
+      } else {
+        (void)memcpy(&usbdata8[pos], &can_packet, len - pos);
+        ep1_buffer.ptr = pckt_len - (len - pos);
+        //(void)memcpy(ep1_buffer.data, ((uint8_t*)&can_packet + (len - pos)), ep1_buffer.ptr);
+        // cppcheck-suppress objectIndex
+        (void)memcpy(ep1_buffer.data, &((uint8_t*)&can_packet)[(len - pos)], ep1_buffer.ptr);
+        pos = len;
+      }
+    }
+    ep1_buffer.counter++;
+    total_rx_size += pos;
+  }
+  if (pos != len) {
+    ep1_buffer.counter = 0;
+    total_rx_size = 0;
+  }
+  if (pos <= 1) { pos = 0; }
+  return pos;
+}
+
+usb_asm_buffer ep3_buffer = {.ptr = 0, .tail_size = 0, .counter = 0};
+
+// send on CAN
+void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
+  UNUSED(hardwired);
+  uint8_t *usbdata8 = (uint8_t *)usbdata;
+  // Got first packet from a stream, resetting buffer and counter
+  if (usbdata8[0] == 0) {
+    ep3_buffer.counter = 0;
+    ep3_buffer.ptr = 0;
+    ep3_buffer.tail_size = 0;
+  }
+  // Assembling can message with data from buffer
+  if (usbdata8[0] == ep3_buffer.counter) {
+    uint8_t pos = 1;
+    ep3_buffer.counter++;
+    if (ep3_buffer.ptr != 0) {
+      if (ep3_buffer.tail_size <= 63U) {
+        CANPacket_t to_push;
+        (void)memcpy(&ep3_buffer.data[ep3_buffer.ptr], &usbdata8[pos], ep3_buffer.tail_size);
+        (void)memcpy(&to_push, ep3_buffer.data, ep3_buffer.ptr + ep3_buffer.tail_size);
+        can_send(&to_push, to_push.bus, false);
+        pos += ep3_buffer.tail_size;
+        ep3_buffer.ptr = 0;
+        ep3_buffer.tail_size = 0;
+      } else {
+        (void)memcpy(&ep3_buffer.data[ep3_buffer.ptr], &usbdata8[pos], len - pos);
+        ep3_buffer.tail_size -= 63U;
+        ep3_buffer.ptr += 63U;
+        pos += 63U;
+      }
+    }
+
+    while (pos < len) {
+      uint8_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(usbdata8[pos] >> 4U)];
+      if ((pos + pckt_len) <= (uint8_t)len) {
+        CANPacket_t to_push;
+        (void)memcpy(&to_push, &usbdata8[pos], pckt_len);
+        can_send(&to_push, to_push.bus, false);
+        pos += pckt_len;
+      } else {
+        (void)memcpy(ep3_buffer.data, &usbdata8[pos], len - pos);
+        ep3_buffer.ptr = len - pos;
+        ep3_buffer.tail_size = pckt_len - ep3_buffer.ptr;
+        pos += ep3_buffer.ptr;
+      }
+    }
+  }
+}
diff --git a/python/__init__.py b/python/__init__.py
index 597f6e9f..fb5dc481 100644
--- a/python/__init__.py
+++ b/python/__init__.py
@@ -16,26 +16,83 @@ from .serial import PandaSerial  # noqa pylint: disable=import-error
 from .isotp import isotp_send, isotp_recv  # pylint: disable=import-error
 from .config import DEFAULT_FW_FN, DEFAULT_H7_FW_FN  # noqa pylint: disable=import-error
 
-__version__ = '0.0.9'
+__version__ = '0.0.10'
 
 BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
 
 DEBUG = os.getenv("PANDADEBUG") is not None
 
-def parse_can_buffer(dat):
-  ret = []
-  for j in range(0, len(dat), 0x10):
-    ddat = dat[j:j + 0x10]
-    f1, f2 = struct.unpack("II", ddat[0:8])
-    extended = 4
-    if f1 & extended:
-      address = f1 >> 3
-    else:
-      address = f1 >> 21
-    dddat = ddat[8:8 + (f2 & 0xF)]
+CANPACKET_HEAD_SIZE = 0x5
+DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64]
+LEN_TO_DLC = {length: dlc for (dlc, length) in enumerate(DLC_TO_LEN)}
+
+def pack_can_buffer(arr):
+  snds = [b'']
+  idx = 0
+  for address, _, dat, bus in arr:
+    assert len(dat) in LEN_TO_DLC
     if DEBUG:
-      print(f"  R 0x{address:x}: 0x{dddat.hex()}")
-    ret.append((address, f2 >> 16, dddat, (f2 >> 4) & 0xFF))
+      print(f"  W 0x{address:x}: 0x{dat.hex()}")
+    extended = 1 if address >= 0x800 else 0
+    data_len_code = LEN_TO_DLC[len(dat)]
+    header = bytearray(5)
+    word_4b = address << 3 | extended << 2
+    header[0] = (data_len_code << 4) | (bus << 1)
+    header[1] = word_4b & 0xFF
+    header[2] = (word_4b >> 8) & 0xFF
+    header[3] = (word_4b >> 16) & 0xFF
+    header[4] = (word_4b >> 24) & 0xFF
+    snds[idx] += header + dat
+    if len(snds[idx]) > 256: # Limit chunks to 256 bytes
+      snds.append(b'')
+      idx += 1
+
+  #Apply counter to each 64 byte packet
+  for idx in range(len(snds)):
+    tx = b''
+    counter = 0
+    for i in range (0, len(snds[idx]), 63):
+      tx += bytes([counter]) + snds[idx][i:i+63]
+      counter += 1
+    snds[idx] = tx
+  return snds
+
+def unpack_can_buffer(dat):
+  ret = []
+  counter = 0
+  tail = bytearray()
+  for i in range(0, len(dat), 64):
+    if counter != dat[i]:
+      print("CAN: LOST RECV PACKET COUNTER")
+      break
+    counter+=1
+    chunk = tail + dat[i+1:i+64]
+    tail = bytearray()
+    pos = 0
+    while pos<len(chunk):
+      data_len = DLC_TO_LEN[(chunk[pos]>>4)]
+      pckt_len = CANPACKET_HEAD_SIZE + data_len
+      if pckt_len <= len(chunk[pos:]):
+        header = chunk[pos:pos+CANPACKET_HEAD_SIZE]
+        if len(header) < 5:
+          print("CAN: MALFORMED USB RECV PACKET")
+          break
+        bus = (header[0] >> 1) & 0x7
+        address = (header[4] << 24 | header[3] << 16 | header[2] << 8 | header[1]) >> 3
+        returned = (header[1] >> 1) & 0x1
+        rejected = header[1] & 0x1
+        data = chunk[pos + CANPACKET_HEAD_SIZE:pos + CANPACKET_HEAD_SIZE + data_len]
+        if returned:
+          bus += 128
+        if rejected:
+          bus += 192
+        if DEBUG:
+          print(f"  R 0x{address:x}: 0x{data.hex()}")
+        ret.append((address, 0, data, bus))
+        pos += pckt_len
+      else:
+        tail = chunk[pos:]
+        break
   return ret
 
 def ensure_health_packet_version(fn):
@@ -76,7 +133,7 @@ class PandaWifiStreaming(object):
       try:
         dat, addr = self.sock.recvfrom(0x200 * 0x10)
         if addr == (self.ip, self.port):
-          ret += parse_can_buffer(dat)
+          ret += unpack_can_buffer(dat)
       except socket.error as e:
         if e.errno != 35 and e.errno != 11:
           traceback.print_exc()
@@ -161,7 +218,7 @@ class Panda(object):
   HW_TYPE_DOS = b'\x06'
   HW_TYPE_RED_PANDA = b'\x07'
 
-  CAN_PACKET_VERSION = 1
+  CAN_PACKET_VERSION = 2
   HEALTH_PACKET_VERSION = 1
 
   F2_DEVICES = [HW_TYPE_PEDAL]
@@ -546,34 +603,20 @@ class Panda(object):
 
   @ensure_can_packet_version
   def can_send_many(self, arr, timeout=CAN_SEND_TIMEOUT_MS):
-    snds = []
-    transmit = 1
-    extended = 4
-    for addr, _, dat, bus in arr:
-      assert len(dat) <= 8
-      if DEBUG:
-        print(f"  W 0x{addr:x}: 0x{dat.hex()}")
-      if addr >= 0x800:
-        rir = (addr << 3) | transmit | extended
-      else:
-        rir = (addr << 21) | transmit
-      snd = struct.pack("II", rir, len(dat) | (bus << 4)) + dat
-      snd = snd.ljust(0x10, b'\x00')
-      snds.append(snd)
-
+    snds = pack_can_buffer(arr)
     while True:
       try:
         if self.wifi:
           for s in snds:
             self._handle.bulkWrite(3, s)
         else:
-          dat = b''.join(snds)
-          while True:
-            bs = self._handle.bulkWrite(3, dat, timeout=timeout)
-            dat = dat[bs:]
-            if len(dat) == 0:
-              break
-            print("CAN: PARTIAL SEND MANY, RETRYING")
+          for tx in snds:
+            while True:
+              bs = self._handle.bulkWrite(3, tx, timeout=timeout)
+              tx = tx[bs:]
+              if len(tx) == 0:
+                break
+              print("CAN: PARTIAL SEND MANY, RETRYING")
         break
       except (usb1.USBErrorIO, usb1.USBErrorOverflow):
         print("CAN: BAD SEND MANY, RETRYING")
@@ -586,12 +629,12 @@ class Panda(object):
     dat = bytearray()
     while True:
       try:
-        dat = self._handle.bulkRead(1, 0x10 * 256)
+        dat = self._handle.bulkRead(1, 16384) # Max receive batch size + 2 extra reserve frames
         break
       except (usb1.USBErrorIO, usb1.USBErrorOverflow):
         print("CAN: BAD RECV, RETRYING")
         time.sleep(0.1)
-    return parse_can_buffer(dat)
+    return unpack_can_buffer(dat)
 
   def can_clear(self, bus):
     """Clears all messages from the specified internal CAN ringbuffer as
diff --git a/tests/gmbitbang/test_packer.c b/tests/gmbitbang/test_packer.c
index f056dd48..63c01310 100644
--- a/tests/gmbitbang/test_packer.c
+++ b/tests/gmbitbang/test_packer.c
@@ -1,21 +1,17 @@
 #include <stdio.h>
 #include <stdint.h>
 
-typedef struct {
-  uint32_t RIR;  /*!< CAN receive FIFO mailbox identifier register */
-  uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */
-  uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */
-  uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */
-} CAN_FIFOMailBox_TypeDef;
+#define CANPACKET_DATA_SIZE_MAX 8
+#include "../../board/can_definitions.h"
 
 #include "../../board/drivers/canbitbang.h"
 
 int main() {
   char out[300];
-  CAN_FIFOMailBox_TypeDef to_bang = {0};
-  to_bang.RIR = 20 << 21;
-  to_bang.RDTR = 1;
-  to_bang.RDLR = 1;
+  CANPacket_t to_bang = {0};
+  to_bang.addr = 20 << 18;
+  to_bang.data_len_code = 1;
+  to_bang.data[0] = 1;
 
   int len = get_bit_message(out, &to_bang);
   printf("T:");
diff --git a/tests/misra/suppressions.txt b/tests/misra/suppressions.txt
index 44e59ace..f34a35ae 100644
--- a/tests/misra/suppressions.txt
+++ b/tests/misra/suppressions.txt
@@ -9,4 +9,8 @@ misra-c2012-19.2
 # Advisory: The # and ## preprocessor operators should not be used
 misra-c2012-20.10
 # Required: it's ok re-defining potentially reserved Macro names. Not likely to cause confusion
-misra-c2012-21.1
\ No newline at end of file
+misra-c2012-21.1
+# MUST BE FIXED, mostly in the safety code
+misra-c2012-10.4
+# MUST BE FIXED, mostly in the safety code
+misra-c2012-10.1
diff --git a/tests/safety/common.py b/tests/safety/common.py
index 6bda0d38..7335f1e1 100644
--- a/tests/safety/common.py
+++ b/tests/safety/common.py
@@ -1,11 +1,11 @@
 import os
 import abc
-import struct
 import unittest
 import importlib
 import numpy as np
 from typing import Optional, List, Dict
 from opendbc.can.packer import CANPacker  # pylint: disable=import-error
+from panda import LEN_TO_DLC
 from panda.tests.safety import libpandasafety_py
 
 MAX_WRONG_COUNTERS = 5
@@ -18,16 +18,12 @@ class UNSAFE_MODE:
 
 def package_can_msg(msg):
   addr, _, dat, bus = msg
-  rdlr, rdhr = struct.unpack('II', dat.ljust(8, b'\x00'))
-
-  ret = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-  if addr >= 0x800:
-    ret[0].RIR = (addr << 3) | 5
-  else:
-    ret[0].RIR = (addr << 21) | 1
-  ret[0].RDTR = len(dat) | ((bus & 0xF) << 4)
-  ret[0].RDHR = rdhr
-  ret[0].RDLR = rdlr
+  ret = libpandasafety_py.ffi.new('CANPacket_t *')
+  ret[0].extended = 1 if addr >= 0x800 else 0
+  ret[0].addr = addr
+  ret[0].data_len_code = LEN_TO_DLC[len(dat)]
+  ret[0].bus = bus
+  ret[0].data = bytes(dat)
 
   return ret
 
diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py
index c0099818..12d2c6cb 100644
--- a/tests/safety/libpandasafety_py.py
+++ b/tests/safety/libpandasafety_py.py
@@ -7,22 +7,19 @@ libpandasafety_fn = os.path.join(can_dir, "libpandasafety.so")
 
 ffi = FFI()
 ffi.cdef("""
-typedef struct
-{
-  uint32_t TIR;  /*!< CAN TX mailbox identifier register */
-  uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */
-  uint32_t TDLR; /*!< CAN mailbox data low register */
-  uint32_t TDHR; /*!< CAN mailbox data high register */
-} CAN_TxMailBox_TypeDef;
-
-typedef struct
-{
-  uint32_t RIR;  /*!< CAN receive FIFO mailbox identifier register */
-  uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */
-  uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */
-  uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */
-} CAN_FIFOMailBox_TypeDef;
+typedef struct {
+  unsigned char reserved : 1;
+  unsigned char bus : 3;
+  unsigned char data_len_code : 4;
+  unsigned char rejected : 1;
+  unsigned char returned : 1;
+  unsigned char extended : 1;
+  unsigned int addr : 29;
+  unsigned char data[8];
+} CANPacket_t;
+""", packed=True)
 
+ffi.cdef("""
 typedef struct
 {
   uint32_t CNT;
@@ -55,9 +52,9 @@ bool get_vehicle_moving(void);
 int get_hw_type(void);
 void set_timer(uint32_t t);
 
-int safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send);
-int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push);
-int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
+int safety_rx_hook(CANPacket_t *to_send);
+int safety_tx_hook(CANPacket_t *to_push);
+int safety_fwd_hook(int bus_num, CANPacket_t *to_fwd);
 int set_safety_hooks(uint16_t  mode, int16_t param);
 
 void init_tests(void);
diff --git a/tests/safety/test.c b/tests/safety/test.c
index 5cf1b7f5..015d179f 100644
--- a/tests/safety/test.c
+++ b/tests/safety/test.c
@@ -2,21 +2,8 @@
 #include <stdbool.h>
 #include <stdlib.h>
 
-typedef struct
-{
-  uint32_t TIR;  /*!< CAN TX mailbox identifier register */
-  uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */
-  uint32_t TDLR; /*!< CAN mailbox data low register */
-  uint32_t TDHR; /*!< CAN mailbox data high register */
-} CAN_TxMailBox_TypeDef;
-
-typedef struct
-{
-  uint32_t RIR;  /*!< CAN receive FIFO mailbox identifier register */
-  uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */
-  uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */
-  uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */
-} CAN_FIFOMailBox_TypeDef;
+#define CANPACKET_DATA_SIZE_MAX 8
+#include "can_definitions.h"
 
 typedef struct
 {
@@ -66,15 +53,6 @@ void fault_occurred(uint32_t fault) {
 void fault_recovered(uint32_t fault) {
 }
 
-// from llcan.h
-#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
-#define GET_LEN(msg) ((msg)->RDTR & 0xf)
-#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
-#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0XFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU))
-#define GET_BYTES_04(msg) ((msg)->RDLR)
-#define GET_BYTES_48(msg) ((msg)->RDHR)
-#define GET_FLAG(value, mask) (((__typeof__(mask))param & mask) == mask)
-
 #define UNUSED(x) (void)(x)
 
 #ifndef PANDA
diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py
index 276021ca..1f8152e3 100755
--- a/tests/safety/test_honda.py
+++ b/tests/safety/test_honda.py
@@ -116,7 +116,10 @@ class TestHondaSafety(common.PandaSafetyTest):
         to_push = self._speed_msg(0)
       self.assertTrue(self._rx(to_push))
       if msg != "btn":
-        to_push[0].RDHR = 0  # invalidate checksum
+        to_push[0].data[4] = 0  # invalidate checksum
+        to_push[0].data[5] = 0
+        to_push[0].data[6] = 0
+        to_push[0].data[7] = 0
         self.assertFalse(self._rx(to_push))
         self.assertFalse(self.safety.get_controls_allowed())
 
@@ -203,8 +206,10 @@ class TestHondaNidecSafety(TestHondaSafety, common.InterceptorSafetyTest):
   def _interceptor_msg(self, gas, addr):
     to_send = make_msg(0, addr, 6)
     gas2 = gas * 2
-    to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
-                      ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
+    to_send[0].data[0] = (gas & 0xFF00) >> 8 
+    to_send[0].data[1] = gas & 0xFF
+    to_send[0].data[2] = (gas2 & 0xFF00) >> 8
+    to_send[0].data[3] = gas2 & 0xFF
     return to_send
 
   def _send_brake_msg(self, brake):
diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py
index ee1d1dea..11c0a5fb 100755
--- a/tests/safety/test_toyota.py
+++ b/tests/safety/test_toyota.py
@@ -74,8 +74,10 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
   # Toyota gas gains are the same
   def _interceptor_msg(self, gas, addr):
     to_send = make_msg(0, addr, 6)
-    to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
-                      ((gas & 0xff) << 24) | ((gas & 0xff00) << 8)
+    to_send[0].data[0] = (gas & 0xFF00) >> 8 
+    to_send[0].data[1] = gas & 0xFF
+    to_send[0].data[2] = (gas & 0xFF00) >> 8
+    to_send[0].data[3] = gas & 0xFF
     return to_send
 
   def test_accel_actuation_limits(self):
@@ -122,7 +124,10 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
       if msg == "pcm":
         to_push = self._pcm_status_msg(True)
       self.assertTrue(self._rx(to_push))
-      to_push[0].RDHR = 0
+      to_push[0].data[4] = 0
+      to_push[0].data[5] = 0
+      to_push[0].data[6] = 0
+      to_push[0].data[7] = 0
       self.assertFalse(self._rx(to_push))
       self.assertFalse(self.safety.get_controls_allowed())
 
diff --git a/tests/safety/test_volkswagen_mqb.py b/tests/safety/test_volkswagen_mqb.py
index 68760ff4..8b1c604f 100644
--- a/tests/safety/test_volkswagen_mqb.py
+++ b/tests/safety/test_volkswagen_mqb.py
@@ -225,7 +225,7 @@ class TestVolkswagenMqbSafety(common.PandaSafetyTest):
       if msg == MSG_MOTOR_20:
         to_push = self._gas_msg(0)
       self.assertTrue(self._rx(to_push))
-      to_push[0].RDHR ^= 0xFF
+      to_push[0].data[4] ^= 0xFF
       self.assertFalse(self._rx(to_push))
       self.assertFalse(self.safety.get_controls_allowed())
 
diff --git a/tests/safety/test_volkswagen_pq.py b/tests/safety/test_volkswagen_pq.py
index 9d9f7e06..87f00f67 100644
--- a/tests/safety/test_volkswagen_pq.py
+++ b/tests/safety/test_volkswagen_pq.py
@@ -25,7 +25,7 @@ MSG_LDW_1 = 0x5BE        # TX by OP, Lane line recognition and text alerts
 
 
 def volkswagen_pq_checksum(msg, addr, len_msg):
-  msg_bytes = msg.RDLR.to_bytes(4, 'little') + msg.RDHR.to_bytes(4, 'little')
+  msg_bytes = bytes(msg.data)
   msg_bytes = msg_bytes[1:len_msg]
 
   checksum = 0
@@ -234,7 +234,7 @@ class TestVolkswagenPqSafety(common.PandaSafetyTest):
     self.safety.set_controls_allowed(1)
     to_push = self._lenkhilfe_3_msg(0)
     self.assertTrue(self._rx(to_push))
-    to_push[0].RDHR ^= 0xFF
+    to_push[0].data[4] ^= 0xFF
     self.assertFalse(self._rx(to_push))
     self.assertFalse(self.safety.get_controls_allowed())
 
diff --git a/tests/safety_replay/helpers.py b/tests/safety_replay/helpers.py
index 572ab7d3..e3ed215a 100644
--- a/tests/safety_replay/helpers.py
+++ b/tests/safety_replay/helpers.py
@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
-import struct
 import panda.tests.safety.libpandasafety_py as libpandasafety_py
-from panda import Panda
+from panda import Panda, LEN_TO_DLC
 
 def to_signed(d, bits):
   ret = d
@@ -59,16 +58,12 @@ def set_desired_torque_last(safety, mode, torque):
     safety.set_subaru_desired_torque_last(torque)
 
 def package_can_msg(msg):
-  rdlr, rdhr = struct.unpack('II', msg.dat.ljust(8, b'\x00'))
-
-  ret = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-  if msg.address >= 0x800:
-    ret[0].RIR = (msg.address << 3) | 5
-  else:
-    ret[0].RIR = (msg.address << 21) | 1
-  ret[0].RDTR = len(msg.dat) | ((msg.src & 0xF) << 4)
-  ret[0].RDHR = rdhr
-  ret[0].RDLR = rdlr
+  ret = libpandasafety_py.ffi.new('CANPacket_t *')
+  ret[0].extended = 1 if msg.address >= 0x800 else 0
+  ret[0].addr = msg.address
+  ret[0].data_len_code = LEN_TO_DLC[len(msg.dat)]
+  ret[0].bus = msg.src
+  ret[0].data = msg.dat
 
   return ret
 
diff --git a/tests/usbprotocol/test_pandalib.py b/tests/usbprotocol/test_pandalib.py
new file mode 100644
index 00000000..7f2a3b38
--- /dev/null
+++ b/tests/usbprotocol/test_pandalib.py
@@ -0,0 +1,23 @@
+#!/usr/bin/env python3
+import random
+import unittest
+from panda import pack_can_buffer, unpack_can_buffer
+
+
+class PandaTestPackUnpack(unittest.TestCase):
+  def test_panda_lib_pack_unpack(self):
+    to_pack = []
+    for _ in range(10000):
+      address = random.randint(1, 0x1FFFFFFF)
+      data = bytes([random.getrandbits(8) for _ in range(random.randrange(1, 9))])
+      to_pack.append((address, 0, data, 0))
+
+    packed = pack_can_buffer(to_pack)
+    unpacked = []
+    for dat in packed:
+      unpacked.extend(unpack_can_buffer(dat))
+
+    assert unpacked == to_pack
+
+if __name__ == "__main__":
+  unittest.main()