Ensures MAX_PACKETS_INFLIGHT w/ semaphore

WIP, only effective w/ MAX_PACKETS_INFLIGHT = 1

src/config.h

@@ -5,7 +5,7 @@
 #ifndef HCI_UART_H5_CONFIG_H
 #define HCI_UART_H5_CONFIG_H
 
-#define MAX_PACKETS_IN_FLIGHT (7)
+#define MAX_PACKETS_IN_FLIGHT (1)
 #define MAX_PACKET_LEN (255)
 
 #endif //HCI_UART_H5_CONFIG_H

src/main.c

@@ -145,9 +145,11 @@ static struct h5 {
 
 	h5_linkstate_t link_state;
 	struct aq_entry ack_queue[MAX_PACKETS_IN_FLIGHT];
+	uint8_t peer_expects_seq;
 } h5;
 
 //static u8_t unack_queue_len;
+K_SEM_DEFINE(inflight_packet_sem, MAX_PACKETS_IN_FLIGHT, MAX_PACKETS_IN_FLIGHT);
 
 static const u8_t sync_req[] = {0x01, 0x7e};
 static const u8_t sync_rsp[] = {0x02, 0x7d};
@@ -166,7 +168,7 @@ NET_BUF_POOL_DEFINE(h5_pool, SIGNAL_COUNT, SIG_BUF_SIZE, 0, NULL);
 NET_BUF_POOL_DEFINE(h5_pack_pool, MAX_PACKETS_IN_FLIGHT + 10, PACKET_BUF_SIZE,
 	0, NULL);
 
-NET_BUF_POOL_DEFINE(uart_tx_pool, MAX_PACKETS_IN_FLIGHT + 10, PACKET_BUF_SIZE,
+NET_BUF_POOL_DEFINE(uart_tx_pool, MAX_PACKETS_IN_FLIGHT, PACKET_BUF_SIZE,
 	0, NULL);
 
 static inline void
@@ -285,7 +287,58 @@ h5_hdr_update_checksum(h5_hdr_t *hdr)
 static u8_t
 next_seq(u8_t i)
 {
-	return (i + 1) % 8;
+	return (i + 1) & 0x7;
+}
+
+static u8_t
+prev_seq(u8_t i)
+{
+	return (i - 1) & 0x07;
+}
+
+uint8_t mod8_forward_dist(uint8_t a, uint8_t b) {
+	size_t count = 0;
+	while (a != b) {
+		count += 1;
+		a = next_seq(a);
+	}
+	return count;
+}
+
+bool tx_win_closed(void) {
+	uint8_t dist = mod8_forward_dist(next_seq(h5.peer_expects_seq),
+			h5.local_seq);
+	return dist > h5.tx_win;
+}
+
+void
+h5_send_unreliable(const u8_t *payload, h5_pkt_t type, int len) {
+	if (len > MAX_PACKET_LEN) {
+		LOG_ERR("Tried to send too-large packet. Size = %d", len);
+		return;
+	}
+	h5_hdr_t hdr = {0};
+
+	/* Set ACK for outgoing packet and stop delayed work */
+	hdr.ack = h5.next_expected_seq_from_host;
+	k_delayed_work_cancel(&ack_work);
+
+	hdr.packet_type = type;
+	hdr.len = len;
+
+	h5_hdr_update_checksum(&hdr);
+
+	struct net_buf *buf = net_buf_alloc(&uart_tx_pool, K_MSEC(250));
+	if (!buf) {
+		LOG_WRN("Waited too long for a UART tx buffer, dropping");
+		return;
+	}
+	net_buf_add_mem(buf, &hdr, sizeof(h5_hdr_t));
+	net_buf_add_mem(buf, payload, len);
+	net_buf_put(&h5.uart_tx_queue, buf);
+	//memcpy(h5.ack.buf[hdr.seq], payload, len)
+	LOG_INF("Queued packet (rel: %d, seq: %d, len: %d, sum: %d)",
+			hdr.is_reliable, hdr.seq, len, hdr.checksum);
 }
 
 void
@@ -302,6 +355,7 @@ h5_send(const u8_t *payload, h5_pkt_t type, int len)
 	k_delayed_work_cancel(&ack_work);
 
 	if (reliable_packet(type)) {
+		k_sem_take(&inflight_packet_sem, K_FOREVER);
 		hdr.is_reliable = true;
 		hdr.seq = h5.local_seq;
 		h5.local_seq = next_seq(h5.local_seq);
@@ -363,7 +417,7 @@ ack_timeout(struct k_work *work)
 {
 	ARG_UNUSED(work);
 	LOG_INF("ACK Timeout, sending H5 ACK packet");
-	h5_send(NULL, HCI_3WIRE_ACK_PKT, 0);
+	h5_send_unreliable(NULL, HCI_3WIRE_ACK_PKT, 0);
 }
 
 bool
@@ -742,6 +796,19 @@ unproc_thread(void)
 			h5.next_expected_seq_from_host = next_seq(hdr.seq);
 		}
 
+		if (hdr.ack != h5.peer_expects_seq) {
+			LOG_INF("Peer expects seq %d", hdr.ack);
+			uint8_t num_back = mod8_forward_dist(
+					h5.peer_expects_seq, hdr.ack);
+			LOG_DBG("num_back: %d (%d %d)", num_back, h5
+					.peer_expects_seq,
+					hdr.ack);
+			for (int i = 0; i < num_back; i++) {
+				k_sem_give(&inflight_packet_sem);
+			}
+			h5.peer_expects_seq = hdr.ack;
+		}
+
 		if (h5.link_state < ACTIVE
 			&& hdr.packet_type != HCI_3WIRE_LINK_PKT) {
 			LOG_WRN("Dropping unexpected packet type (%d) in non-ACTIVE state (%d)",