diff --git a/src/main.c b/src/main.c
index 22e7980..db014ab 100644
--- a/src/main.c
+++ b/src/main.c
@@ -109,20 +109,6 @@ typedef struct __attribute__((packed)) {
   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)
-#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
-#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
-#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
-
-#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
-#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
-#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
-#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
-#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
-				 ((hdr)[2] |= (len) >> 4))
-
 typedef enum {
   UNINIT,
   INIT,
@@ -274,40 +260,38 @@ static u8_t h5_slip_byte(u8_t byte)
 	}
 }
 
+void h5_hdr_update_checksum(h5_hdr_t *hdr) {
+    uint8_t *u8_hdr = (uint8_t *)hdr;
+    u8_hdr[3] = ~((u8_hdr[0] + u8_hdr[1] + u8_hdr[2]) & 0xff);
+}
+
 void h5_send(const u8_t *payload, u8_t type, int len)
 {
-	u8_t hdr[4];
-	int i;
-
-	//hexdump("To Host <= ", payload, len);
-
-	memset(hdr, 0, sizeof(hdr));
+	h5_hdr_t hdr = {0};
 
 	/* Set ACK for outgoing packet and stop delayed work */
-	H5_SET_ACK(hdr, h5.tx_ack);
-	k_delayed_work_cancel(&ack_work);
+	hdr.ack = h5.tx_ack;
+    k_delayed_work_cancel(&ack_work);
 
 	if (reliable_packet(type)) {
-		H5_SET_RELIABLE(hdr);
-		H5_SET_SEQ(hdr, h5.tx_seq);
+	    hdr.is_reliable = true;
+	    hdr.seq = h5.tx_seq;
 		h5.tx_seq = (h5.tx_seq + 1) % 8;
 	}
 
-	H5_SET_TYPE(hdr, type);
-	H5_SET_LEN(hdr, len);
+	hdr.packet_type = type;
+	hdr.len = len;
 
-	/* Calculate CRC */
-	hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
+	h5_hdr_update_checksum(&hdr);
 
-	//h5_print_header(hdr, "TX: <");
+    uart_poll_out(hci_uart_dev, SLIP_DELIMITER);
 
-	uart_poll_out(hci_uart_dev, SLIP_DELIMITER);
-
-	for (i = 0; i < 4; i++) {
-		h5_slip_byte(hdr[i]);
+    uint8_t *u8_hdr = (uint8_t *)&hdr;
+	for (int i = 0; i < 4; i++) {
+		h5_slip_byte(u8_hdr[i]);
 	}
 
-	for (i = 0; i < len; i++) {
+	for (int i = 0; i < len; i++) {
 		h5_slip_byte(payload[i]);
 	}
 
@@ -423,29 +407,6 @@ void bt_uart_isr(struct device *unused)
 	}
 }
 
-int pull_header(struct net_buf *buf, u8_t *hdr) {
-	// Packet too short to contain an h5 header
-	if (buf->len < 4) {
-		return -1;
-	}
-
-	for (u8_t i = 0; i < 4; i++) {
-		int byte = unslip_next_byte(buf);
-		if (byte < 0) {
-			// Packet too short due to escaped bytes
-			return -1;
-		}
-		hdr[i] = byte;
-	}
-
-	// Checksum
-	if (((hdr[3] + hdr[0] + hdr[1] + hdr[2]) & 0xff) != 0xff) {
-		LOG_WRN("Invalid Header Checksum\n");
-	}
-
-	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;
@@ -466,6 +427,9 @@ int pull_header1(struct net_buf *buf, h5_hdr_t *hdr) {
 		}
 		u8_hdr[i] = byte;
 	}
+    if (!h5_hdr_checksum_good(hdr)) {
+        return -2;
+    }
     return 0;
 }
 
@@ -475,12 +439,11 @@ static void unproc_thread(void) {
 	while (true) {
 		buf = net_buf_get(&h5.unprocessed_queue, K_FOREVER);
 
-		//u8_t hdr[4];
 		h5_hdr_t hdr = {0};
 		if (pull_header1(buf, &hdr) < 0) {
-			// Header is invalid
-			goto next;
-		}
+            // Header is invalid
+            goto next;
+        }
 
 		struct net_buf *rx_buf = NULL;