From 57bfd18ce46b9720d57841d97e8c3256a1e20d4f Mon Sep 17 00:00:00 2001 From: Shawn Nock Date: Sat, 2 May 2020 15:25:51 -0400 Subject: [PATCH] Ensures MAX_PACKETS_INFLIGHT w/ semaphore WIP, only effective w/ MAX_PACKETS_INFLIGHT = 1 --- src/config.h | 2 +- src/main.c | 73 +++++++++++++++++++++++++++++++++++++++++++++++++--- 2 files changed, 71 insertions(+), 4 deletions(-) diff --git a/src/config.h b/src/config.h index dad62f9..9fdbece 100644 --- a/src/config.h +++ b/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 diff --git a/src/main.c b/src/main.c index e88c58a..378a6ef 100644 --- a/src/main.c +++ b/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)",