diff --git a/src/main.c b/src/main.c index 19b7077..e9647c3 100644 --- a/src/main.c +++ b/src/main.c @@ -282,6 +282,8 @@ void h5_send(const u8_t *payload, u8_t type, int len) hdr.ack = h5.last_seen_seq_from_host; k_delayed_work_cancel(&ack_work); + LOG_INF("ACK'd %d", hdr.ack); + if (reliable_packet(type)) { hdr.is_reliable = true; hdr.seq = h5.local_seq; @@ -545,8 +547,14 @@ static void unproc_thread(void) { LOG_DBG("Header: seq %d, ack %d, rel: %s, has_crc16: %s", hdr.seq, hdr.ack, hdr.is_reliable ? "true" : "false", hdr.crc_after_payload ? "true" : "false"); + u8_t expected_seq = next_seq(h5.last_seen_seq_from_host); + if (hdr.seq != expected_seq) { + LOG_ERR("Seq expected %u got %u. Drop packet", expected_seq, hdr.seq); + goto next; + } + k_delayed_work_submit(&ack_work, H5_RX_ACK_TIMEOUT); + h5.last_seen_seq_from_host = hdr.seq; } - struct net_buf *rx_buf = NULL; if (h5.link_state < ACTIVE && hdr.packet_type != HCI_3WIRE_LINK_PKT) { LOG_WRN("Dropping unexpected packet type (%d) in non-ACTIVE state (%d)", @@ -554,6 +562,7 @@ static void unproc_thread(void) { goto next; } + struct net_buf *rx_buf = NULL; switch (hdr.packet_type) { case HCI_ACLDATA_PKT: rx_buf = net_buf_alloc(&acl_tx_pool, K_NO_WAIT); @@ -585,21 +594,10 @@ static void unproc_thread(void) { if (hdr.len != rx_buf->len) { LOG_WRN("Truncated payload, skipping\n"); + net_buf_unref(rx_buf); goto next; } - if (hdr.is_reliable) { - u8_t expected_seq = next_seq(h5.last_seen_seq_from_host); - if (hdr.seq != expected_seq) { - LOG_ERR("Seq expected %u got %u. Drop packet", expected_seq, hdr.seq); - net_buf_unref(rx_buf); - goto next; - } - h5.last_seen_seq_from_host = hdr.seq; - /* Submit delayed work to ack the packet */ - k_delayed_work_submit(&ack_work, H5_RX_ACK_TIMEOUT); - } - switch (hdr.packet_type) { case HCI_3WIRE_ACK_PKT: // No further action required