diff --git a/src/main.c b/src/main.c
index 60cbc56..22e7980 100644
--- a/src/main.c
+++ b/src/main.c
@@ -99,6 +99,16 @@ static bool reliable_packet(u8_t type)
 #define H5_RX_ESC	1
 #define H5_TX_ACK_PEND	2
 
+typedef struct __attribute__((packed)) {
+  uint8_t seq : 3;
+  uint8_t ack : 3;
+  bool crc_after_payload : 1;
+  bool is_reliable : 1;
+  uint8_t packet_type : 4;
+  uint16_t len : 12;
+  uint8_t checksum : 8;
+} h5_hdr_t;
+
 #define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
 #define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
 #define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
@@ -352,7 +362,7 @@ static void ack_timeout(struct k_work *work)
 
 int unslip_next_byte(struct net_buf *buf) {
 	if (!buf->len) {
-	    LOG_WRN("Tried to unslip empty buffer");
+	    // This happens at the end of every buffer, not work logging
 		return -1;
 	}
 
@@ -436,21 +446,45 @@ int pull_header(struct net_buf *buf, u8_t *hdr) {
 	return 0;
 }
 
+bool h5_hdr_checksum_good(h5_hdr_t *hdr) {
+    uint8_t *u8_hdr = (uint8_t *)hdr;
+    return ((u8_hdr[3] + u8_hdr[0] + u8_hdr[1] + u8_hdr[2]) & 0xff) == 0xff;
+}
+
+int pull_header1(struct net_buf *buf, h5_hdr_t *hdr) {
+    uint8_t *u8_hdr = (uint8_t *)hdr;
+
+    if (buf->len < sizeof(h5_hdr_t)) {
+		return -1;
+	}
+
+    for (u8_t i = 0; i < sizeof(h5_hdr_t); i++) {
+		int byte = unslip_next_byte(buf);
+		if (byte < 0) {
+			// Packet too short due to escaped bytes
+			return -1;
+		}
+		u8_hdr[i] = byte;
+	}
+    return 0;
+}
+
 static void unproc_thread(void) {
 	struct net_buf *buf;
 
 	while (true) {
 		buf = net_buf_get(&h5.unprocessed_queue, K_FOREVER);
 
-		u8_t hdr[4];
-		if (pull_header(buf, hdr) < 0) {
+		//u8_t hdr[4];
+		h5_hdr_t hdr = {0};
+		if (pull_header1(buf, &hdr) < 0) {
 			// Header is invalid
 			goto next;
 		}
 
 		struct net_buf *rx_buf = NULL;
 
-		switch (H5_HDR_PKT_TYPE(hdr)) {
+		switch (hdr.packet_type) {
         case HCI_ACLDATA_PKT:
             rx_buf = net_buf_alloc(&acl_tx_pool, K_NO_WAIT);
             REBOOT_IF_NOT(rx_buf);
@@ -462,15 +496,14 @@ static void unproc_thread(void) {
             bt_buf_set_type(rx_buf, BT_BUF_CMD);
             break;
         case HCI_3WIRE_ACK_PKT:
-            h5.rx_ack = H5_HDR_ACK(hdr);
+            h5.rx_ack = hdr.ack;
             goto next;
-            break;
         case HCI_3WIRE_LINK_PKT:
             rx_buf = net_buf_alloc(&h5_pool, K_NO_WAIT);
             REBOOT_IF_NOT(rx_buf);
             break;
         default:
-            LOG_ERR("Wrong packet type from host: %u", H5_HDR_PKT_TYPE(hdr));
+            LOG_ERR("Wrong packet type from host: %u", hdr.packet_type);
             goto next;
         }
 
@@ -479,27 +512,26 @@ static void unproc_thread(void) {
 			net_buf_add_u8(rx_buf, (u8_t) byte);
 		}
 
-		if (H5_HDR_LEN(hdr) != rx_buf->len) {
-			LOG_ERR("Payload too short\n");
+		if (hdr.len != rx_buf->len) {
+			LOG_WRN("Truncated payload, skipping\n");
 			goto next;
 		}
 
-        if (H5_HDR_RELIABLE(hdr) &&
-	    	H5_HDR_SEQ(hdr) != h5.tx_ack) {
-			LOG_ERR("Seq expected %u got %u. Drop packet", h5.tx_ack, H5_HDR_SEQ(hdr));
+        if (hdr.is_reliable && (hdr.seq != h5.tx_ack)) {
+			LOG_ERR("Seq expected %u got %u. Drop packet", h5.tx_ack, hdr.seq);
             goto next;
         }
 
-		h5.rx_ack = H5_HDR_ACK(hdr);
+		h5.rx_ack = hdr.ack;
 
-		if (reliable_packet(H5_HDR_PKT_TYPE(hdr))) {
+		if (hdr.is_reliable) {
 			/* For reliable packet increment next transmit ack number */
             h5.tx_ack = (h5.tx_ack + 1) % 8;
             /* Submit delayed work to ack the packet */
             k_delayed_work_submit(&ack_work, H5_RX_ACK_TIMEOUT);
 		}
 
-        switch (H5_HDR_PKT_TYPE(hdr)) {
+        switch (hdr.packet_type) {
         case HCI_3WIRE_ACK_PKT:
             // No further action required
             break;
@@ -512,7 +544,7 @@ static void unproc_thread(void) {
             net_buf_put(&h5.controller_queue, rx_buf);
             break;
         default:
-            LOG_WRN("Unknown packet type %u\n", H5_HDR_PKT_TYPE(hdr));
+            LOG_WRN("Unknown packet type %u\n", hdr.packet_type);
             break;
         }
 next: