diff --git a/src/main.c b/src/main.c index 5750ce9..a453a96 100644 --- a/src/main.c +++ b/src/main.c @@ -68,6 +68,9 @@ static struct k_thread unproc_thread_data; static K_THREAD_STACK_DEFINE(event_stack, 4096); static struct k_thread event_thread_data; +static K_THREAD_STACK_DEFINE(uart_tx_stack, 4096); +static struct k_thread uart_tx_thread_data; + static struct k_delayed_work ack_work; //static struct k_delayed_work retx_work; @@ -128,7 +131,8 @@ static struct h5 { struct k_fifo host_queue; struct k_fifo unack_queue; struct k_fifo unprocessed_queue; - struct k_fifo event_queue; + struct k_fifo event_queue; + struct k_fifo uart_tx_queue; u8_t tx_win; u8_t next_expected_seq_from_host; @@ -158,6 +162,8 @@ NET_BUF_POOL_DEFINE(h5_pool, SIGNAL_COUNT, SIG_BUF_SIZE, 0, NULL); #define PACKET_BUF_SIZE (CONFIG_BT_HCI_RESERVE + MAX_PACKET_LEN) 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, 0, NULL); + static inline void bt_uart_drain(struct device *dev) { u8_t c; @@ -288,18 +294,14 @@ void h5_send(const u8_t *payload, h5_pkt_t type, int len) h5_hdr_update_checksum(&hdr); - uart_poll_out(hci_uart_dev, SLIP_DELIMITER); - - uint8_t *u8_hdr = (uint8_t *)&hdr; - for (int i = 0; i < 4; i++) { - h5_slip_byte(u8_hdr[i]); + struct net_buf *buf = net_buf_alloc(&uart_tx_pool, K_MSEC(250)); + if (!buf) { + LOG_WRN("Waited to long for a UART tx buffer, dropping"); + return; } - - for (int i = 0; i < len; i++) { - h5_slip_byte(payload[i]); - } - - uart_poll_out(hci_uart_dev, SLIP_DELIMITER); + 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); } /* Delayed work taking care about retransmitting packets */ @@ -337,6 +339,7 @@ void h5_send(const u8_t *payload, h5_pkt_t type, int len) static void 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); } @@ -668,6 +671,18 @@ static void event_thread(void) { } } +static void uart_tx_thread() { + while (true) { + struct net_buf *buf = net_buf_get(&h5.uart_tx_queue, K_FOREVER); + uart_poll_out(hci_uart_dev, SLIP_DELIMITER); + + for (size_t i=0; i < buf->len; i++) { + h5_slip_byte(net_buf_pull_u8(buf)); + } + uart_poll_out(hci_uart_dev, SLIP_DELIMITER); + } +} + static void h5_init(void) { LOG_DBG("h5_init"); @@ -709,6 +724,14 @@ static void h5_init(void) K_PRIO_PREEMPT(2), 0, K_NO_WAIT); + /* Thread handles incoming events from BT Controller */ + k_fifo_init(&h5.uart_tx_queue); + k_thread_create(&uart_tx_thread_data, uart_tx_stack, + K_THREAD_STACK_SIZEOF(uart_tx_stack), + (k_thread_entry_t)uart_tx_thread, NULL, NULL, NULL, + K_PRIO_PREEMPT(0), + 0, K_NO_WAIT); + /* Init delayed work */ k_delayed_work_init(&ack_work, ack_timeout);