diff --git a/src/main.c b/src/main.c index a453a96..61517ab 100644 --- a/src/main.c +++ b/src/main.c @@ -18,7 +18,6 @@ #include #include - #include #include @@ -43,7 +42,7 @@ static struct device *hci_uart_dev; /* HCI command buffers */ #define CMD_BUF_SIZE BT_BUF_RX_SIZE NET_BUF_POOL_DEFINE(cmd_tx_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE, - BT_BUF_USER_DATA_MIN, NULL); + BT_BUF_USER_DATA_MIN, NULL); #define BT_L2CAP_MTU (CONFIG_BT_CTLR_TX_BUFFER_SIZE - BT_L2CAP_HDR_SIZE) @@ -57,7 +56,7 @@ NET_BUF_POOL_DEFINE(cmd_tx_pool, CONFIG_BT_HCI_CMD_COUNT, CMD_BUF_SIZE, #endif NET_BUF_POOL_DEFINE(acl_tx_pool, TX_BUF_COUNT, BT_BUF_ACL_SIZE, - BT_BUF_USER_DATA_MIN, NULL); + BT_BUF_USER_DATA_MIN, NULL); static K_THREAD_STACK_DEFINE(tx_stack, 4096); static struct k_thread tx_thread_data; @@ -75,68 +74,69 @@ static struct k_delayed_work ack_work; //static struct k_delayed_work retx_work; typedef enum { - HCI_3WIRE_ACK_PKT = 0x00, - HCI_COMMAND_PKT, - HCI_ACLDATA_PKT, - HCI_SCODATA_PKT, - HCI_EVENT_PKT, - HCI_VENDOR_SPECIFIC_PKT = 0xd, - HCI_3WIRE_LINK_PKT = 0x0f, + HCI_3WIRE_ACK_PKT = 0x00, + HCI_COMMAND_PKT, + HCI_ACLDATA_PKT, + HCI_SCODATA_PKT, + HCI_EVENT_PKT, + HCI_VENDOR_SPECIFIC_PKT = 0xd, + HCI_3WIRE_LINK_PKT = 0x0f, } h5_pkt_t; -static bool reliable_packet(h5_pkt_t type) +static bool +reliable_packet(h5_pkt_t type) { switch (type) { case HCI_COMMAND_PKT: case HCI_ACLDATA_PKT: case HCI_EVENT_PKT: - return true; + return true; default: return false; } } /* FIXME: Correct timeout */ -#define H5_RX_ACK_TIMEOUT K_MSEC(250) -#define H5_TX_ACK_TIMEOUT K_MSEC(250) +#define H5_RX_ACK_TIMEOUT K_MSEC(250) +#define H5_TX_ACK_TIMEOUT K_MSEC(250) -#define SLIP_DELIMITER 0xc0 -#define SLIP_ESC 0xdb -#define SLIP_ESC_DELIM 0xdc -#define SLIP_ESC_ESC 0xdd +#define SLIP_DELIMITER 0xc0 +#define SLIP_ESC 0xdb +#define SLIP_ESC_DELIM 0xdc +#define SLIP_ESC_ESC 0xdd -#define H5_RX_ESC 1 -#define H5_TX_ACK_PEND 2 +#define H5_RX_ESC 1 +#define H5_TX_ACK_PEND 2 typedef struct __attribute__((packed)) { - uint8_t seq : 3; - uint8_t ack : 3; - bool crc_after_payload : 1; - bool is_reliable : 1; - uint8_t packet_type : 4; - uint16_t len : 12; - uint8_t checksum : 8; + uint8_t seq : 3; + uint8_t ack : 3; + bool crc_after_payload : 1; + bool is_reliable : 1; + uint8_t packet_type : 4; + uint16_t len : 12; + uint8_t checksum : 8; } h5_hdr_t; typedef enum { - UNINIT, - INIT, - ACTIVE, + UNINIT, + INIT, + ACTIVE, } h5_linkstate_t; static struct h5 { //struct net_buf *rx_buf; - struct k_fifo controller_queue; - struct k_fifo host_queue; - struct k_fifo unack_queue; - struct k_fifo unprocessed_queue; - struct k_fifo event_queue; - struct k_fifo uart_tx_queue; + struct k_fifo controller_queue; + struct k_fifo host_queue; + struct k_fifo unack_queue; + struct k_fifo unprocessed_queue; + struct k_fifo event_queue; + struct k_fifo uart_tx_queue; - u8_t tx_win; - u8_t next_expected_seq_from_host; - u8_t local_seq; + u8_t tx_win; + u8_t next_expected_seq_from_host; + u8_t local_seq; h5_linkstate_t link_state; } h5; @@ -145,14 +145,14 @@ static struct h5 { #define MAX_PACKETS_IN_FLIGHT (0x01) -static const u8_t sync_req[] = { 0x01, 0x7e }; -static const u8_t sync_rsp[] = { 0x02, 0x7d }; +static const u8_t sync_req[] = {0x01, 0x7e}; +static const u8_t sync_rsp[] = {0x02, 0x7d}; /* Third byte may change */ -static u8_t conf_req[] = { 0x03, 0xfc }; -static const u8_t conf_rsp[] = { 0x04, 0x7b, MAX_PACKETS_IN_FLIGHT }; +static u8_t conf_req[] = {0x03, 0xfc}; +static const u8_t conf_rsp[] = {0x04, 0x7b, MAX_PACKETS_IN_FLIGHT}; /* H5 signal buffers pool */ -#define MAX_SIG_LEN 3 +#define MAX_SIG_LEN 3 #define SIGNAL_COUNT 10 #define SIG_BUF_SIZE (CONFIG_BT_HCI_RESERVE + MAX_SIG_LEN) NET_BUF_POOL_DEFINE(h5_pool, SIGNAL_COUNT, SIG_BUF_SIZE, 0, NULL); @@ -160,11 +160,14 @@ NET_BUF_POOL_DEFINE(h5_pool, SIGNAL_COUNT, SIG_BUF_SIZE, 0, NULL); /* H5 Packet Buf */ #define MAX_PACKET_LEN 255 // CMD Header + 255 max payload #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(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); +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) +static inline void +bt_uart_drain(struct device *dev) { u8_t c; @@ -175,12 +178,14 @@ static inline void bt_uart_drain(struct device *dev) #define REBOOT_IF_NOT(buf) _reboot_if_not(buf, __FILE__, __LINE__) -void _reboot_if_not(struct net_buf *buf, char const *file, int line) { - if (!buf) { - log_panic(); - LOG_ERR("Failed alloc at %s:%d", file, line); - sys_reboot(SYS_REBOOT_COLD); - } +void +_reboot_if_not(struct net_buf *buf, char const *file, int line) +{ + if (!buf) { + log_panic(); + LOG_ERR("Failed alloc at %s:%d", file, line); + sys_reboot(SYS_REBOOT_COLD); + } } /* @@ -234,22 +239,23 @@ static void process_unack(void) } */ -static void h5_print_header(h5_hdr_t *hdr) +static void +h5_print_header(h5_hdr_t *hdr) { if (hdr->is_reliable) { LOG_DBG("REL: seq %d ack %d crc %d type %d len %d", - hdr->seq, hdr->ack, - hdr->checksum, hdr->packet_type, - hdr->len); + hdr->seq, hdr->ack, + hdr->checksum, hdr->packet_type, + hdr->len); } else { LOG_DBG("UNREL: ack %d crc %d type %d len %d", - hdr->ack, hdr->checksum, - hdr->packet_type, hdr->len); + hdr->ack, hdr->checksum, + hdr->packet_type, hdr->len); } } - -static u8_t h5_slip_byte(u8_t byte) +static u8_t +h5_slip_byte(u8_t byte) { switch (byte) { case SLIP_DELIMITER: @@ -266,26 +272,31 @@ 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_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); } -static u8_t next_seq(u8_t i) { - return (i + 1) % 8; +static u8_t +next_seq(u8_t i) +{ + return (i + 1) % 8; } -void h5_send(const u8_t *payload, h5_pkt_t type, int len) +void +h5_send(const u8_t *payload, h5_pkt_t type, int len) { 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); + k_delayed_work_cancel(&ack_work); if (reliable_packet(type)) { - hdr.is_reliable = true; - hdr.seq = h5.local_seq; + hdr.is_reliable = true; + hdr.seq = h5.local_seq; h5.local_seq = next_seq(h5.local_seq); } @@ -296,12 +307,12 @@ void h5_send(const u8_t *payload, h5_pkt_t type, int len) 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; + LOG_WRN("Waited to 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); + net_buf_add_mem(buf, payload, len); + net_buf_put(&h5.uart_tx_queue, buf); } /* Delayed work taking care about retransmitting packets */ @@ -337,46 +348,54 @@ void h5_send(const u8_t *payload, h5_pkt_t type, int len) } }*/ -static void ack_timeout(struct k_work *work) { +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); } -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; +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; } -bool unslip_header(uint8_t *buf, size_t len, h5_hdr_t *hdr) { - uint8_t hdr_idx = 0, *u8_hdr = (uint8_t *)&hdr; +bool +unslip_header(uint8_t *buf, size_t len, h5_hdr_t *hdr) +{ + uint8_t hdr_idx = 0, *u8_hdr = (uint8_t *) &hdr; - if (len < sizeof(h5_hdr_t)) { - return false; - } + if (len < sizeof(h5_hdr_t)) { + return false; + } - for (size_t i = 0; i < len; i++) { - if (buf[i] == SLIP_ESC) { - continue; - } - if (buf[i] == SLIP_ESC_ESC) { - u8_hdr[hdr_idx++] = SLIP_ESC; - } - if (buf[i] == SLIP_ESC_DELIM) { - u8_hdr[hdr_idx++] = SLIP_DELIMITER; - } - u8_hdr[hdr_idx++] = buf[i]; - } - if (hdr_idx < sizeof(h5_hdr_t)) { - // Buffer too small due to slip escape chars - return false; - } - return h5_hdr_checksum_good(hdr); + for (size_t i = 0; i < len; i++) { + if (buf[i] == SLIP_ESC) { + continue; + } + if (buf[i] == SLIP_ESC_ESC) { + u8_hdr[hdr_idx++] = SLIP_ESC; + } + if (buf[i] == SLIP_ESC_DELIM) { + u8_hdr[hdr_idx++] = SLIP_DELIMITER; + } + u8_hdr[hdr_idx++] = buf[i]; + } + if (hdr_idx < sizeof(h5_hdr_t)) { + // Buffer too small due to slip escape chars + return false; + } + return h5_hdr_checksum_good(hdr); } -int unslip_next_byte(struct net_buf *buf) { +int +unslip_next_byte(struct net_buf *buf) +{ if (!buf->len) { - // This happens at the end of every buffer, not work logging + // This happens at the end of every buffer, not work logging return -1; } @@ -386,7 +405,7 @@ int unslip_next_byte(struct net_buf *buf) { } if (!buf->len) { - LOG_WRN("Incomplete SLIP_ESC pair"); + LOG_WRN("Incomplete SLIP_ESC pair"); return -1; } @@ -401,79 +420,82 @@ int unslip_next_byte(struct net_buf *buf) { return -2; } -int pull_header1(struct net_buf *buf, h5_hdr_t *hdr) { - uint8_t *u8_hdr = (uint8_t *)hdr; - - if (buf->len < sizeof(h5_hdr_t)) { - return -1; - } - - for (u8_t i = 0; i < sizeof(h5_hdr_t); i++) { - int byte = unslip_next_byte(buf); - if (byte < 0) { - // Packet too short due to escaped bytes - return -1; - } - u8_hdr[i] = byte; - } - if (!h5_hdr_checksum_good(hdr)) { - return -2; - } - return 0; -} - -void bt_uart_isr(struct device *unused) +int +pull_header1(struct net_buf *buf, h5_hdr_t *hdr) { - static u8_t byte; - static struct net_buf *buf = NULL; - static enum slip_states { - SLIP_NOSYNC, - SLIP_SYNC, - } slip_state = SLIP_NOSYNC; + uint8_t *u8_hdr = (uint8_t *) hdr; - ARG_UNUSED(unused); + if (buf->len < sizeof(h5_hdr_t)) { + return -1; + } - while (uart_irq_update(hci_uart_dev) && - uart_irq_is_pending(hci_uart_dev)) { - - if (!uart_irq_rx_ready(hci_uart_dev)) { - /* Only the UART RX path is interrupt-enabled */ - break; - } - - if (!buf) { - buf = net_buf_alloc(&h5_pack_pool, K_NO_WAIT); - REBOOT_IF_NOT(buf); - } - - while (uart_fifo_read(hci_uart_dev, &byte, sizeof(byte))) { - if (buf->len == buf->size && byte != SLIP_DELIMITER) { - /* If our buffer is full, but the packet isn't done... - * unsync'ing SLIP will drop the rest of the packet and the net_buf_reset - * prepares the buffer for reuse without unref/alloc cycle. - */ - LOG_WRN("H5 packet exceeds buffer size, dropping"); - net_buf_reset(buf); - slip_state = SLIP_NOSYNC; - } - if (slip_state == SLIP_NOSYNC && byte != SLIP_DELIMITER) { - continue; - } - if (byte == SLIP_DELIMITER) { - if (buf->len > 0) { - net_buf_put(&h5.unprocessed_queue, buf); - buf = NULL; - slip_state = SLIP_NOSYNC; - } else { - slip_state = SLIP_SYNC; - } - } else { - net_buf_add_u8(buf, byte); - } - } - } + for (u8_t i = 0; i < sizeof(h5_hdr_t); i++) { + int byte = unslip_next_byte(buf); + if (byte < 0) { + // Packet too short due to escaped bytes + return -1; + } + u8_hdr[i] = byte; + } + if (!h5_hdr_checksum_good(hdr)) { + return -2; + } + return 0; } +void +bt_uart_isr(struct device *unused) +{ + static u8_t byte; + static struct net_buf *buf = NULL; + static enum slip_states { + SLIP_NOSYNC, + SLIP_SYNC, + } slip_state = SLIP_NOSYNC; + + ARG_UNUSED(unused); + + while (uart_irq_update(hci_uart_dev) && + uart_irq_is_pending(hci_uart_dev)) { + + if (!uart_irq_rx_ready(hci_uart_dev)) { + /* Only the UART RX path is interrupt-enabled */ + break; + } + + if (!buf) { + buf = net_buf_alloc(&h5_pack_pool, K_NO_WAIT); + REBOOT_IF_NOT(buf); + } + + while (uart_fifo_read(hci_uart_dev, &byte, sizeof(byte))) { + if (buf->len == buf->size && byte != SLIP_DELIMITER) { + /* If our buffer is full, but the packet isn't done... + * unsync'ing SLIP will drop the rest of the packet and the net_buf_reset + * prepares the buffer for reuse without unref/alloc cycle. + */ + LOG_WRN("H5 packet exceeds buffer size, dropping"); + net_buf_reset(buf); + slip_state = SLIP_NOSYNC; + } + if (slip_state == SLIP_NOSYNC + && byte != SLIP_DELIMITER) { + continue; + } + if (byte == SLIP_DELIMITER) { + if (buf->len > 0) { + net_buf_put(&h5.unprocessed_queue, buf); + buf = NULL; + slip_state = SLIP_NOSYNC; + } else { + slip_state = SLIP_SYNC; + } + } else { + net_buf_add_u8(buf, byte); + } + } + } +} /*void bt_uart_isr(struct device *unused) { static u8_t byte; @@ -526,9 +548,11 @@ void bt_uart_isr(struct device *unused) } }*/ -static void unproc_thread(void) { +static void +unproc_thread(void) +{ struct net_buf *buf; - k_thread_name_set(NULL, "unproc_thread"); + k_thread_name_set(NULL, "unproc_thread"); while (true) { buf = net_buf_get(&h5.unprocessed_queue, K_FOREVER); @@ -536,53 +560,58 @@ static void unproc_thread(void) { h5_hdr_t hdr = {0}; int r = pull_header1(buf, &hdr); if (r < 0) { - LOG_WRN("Bad h5 header: %d, dropping packet", r); - goto next; - } - - //LOG_HEXDUMP_DBG((uint8_t*)&hdr, sizeof(hdr), "Header"); - if (hdr.is_reliable) { - 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"); - if (hdr.seq != h5.next_expected_seq_from_host) { - LOG_ERR("Seq expected %u got %u. Drop packet", h5.next_expected_seq_from_host, hdr.seq); - goto next; - } - k_delayed_work_submit(&ack_work, H5_RX_ACK_TIMEOUT); - h5.next_expected_seq_from_host = next_seq(hdr.seq); - } - - if (h5.link_state < ACTIVE && hdr.packet_type != HCI_3WIRE_LINK_PKT) { - LOG_WRN("Dropping unexpected packet type (%d) in non-ACTIVE state (%d)", - hdr.packet_type, h5.link_state); - goto next; + LOG_WRN("Bad h5 header: %d, dropping packet", r); + goto next; } - struct net_buf *rx_buf = NULL; + //LOG_HEXDUMP_DBG((uint8_t*)&hdr, sizeof(hdr), "Header"); + if (hdr.is_reliable) { + 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"); + if (hdr.seq != h5.next_expected_seq_from_host) { + LOG_ERR("Seq expected %u got %u. Drop packet", + h5.next_expected_seq_from_host, + hdr.seq); + goto next; + } + k_delayed_work_submit(&ack_work, H5_RX_ACK_TIMEOUT); + h5.next_expected_seq_from_host = next_seq(hdr.seq); + } + + if (h5.link_state < ACTIVE + && hdr.packet_type != HCI_3WIRE_LINK_PKT) { + LOG_WRN("Dropping unexpected packet type (%d) in non-ACTIVE state (%d)", + hdr.packet_type, h5.link_state); + 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); - REBOOT_IF_NOT(rx_buf); - bt_buf_set_type(rx_buf, BT_BUF_ACL_OUT); - break; - case HCI_COMMAND_PKT: - rx_buf = net_buf_alloc(&cmd_tx_pool, K_NO_WAIT); - REBOOT_IF_NOT(rx_buf); - bt_buf_set_type(rx_buf, BT_BUF_CMD); - break; - case HCI_3WIRE_ACK_PKT: - // h5.rx_ack = hdr.ack; - // We currently don't retransmit, so drop ack packets - goto next; - case HCI_3WIRE_LINK_PKT: - rx_buf = net_buf_alloc(&h5_pool, K_NO_WAIT); - REBOOT_IF_NOT(rx_buf); - break; - default: - LOG_ERR("Wrong packet type from host: %u", hdr.packet_type); - goto next; - } + case HCI_ACLDATA_PKT: + rx_buf = net_buf_alloc(&acl_tx_pool, K_NO_WAIT); + REBOOT_IF_NOT(rx_buf); + bt_buf_set_type(rx_buf, BT_BUF_ACL_OUT); + break; + case HCI_COMMAND_PKT: + rx_buf = net_buf_alloc(&cmd_tx_pool, K_NO_WAIT); + REBOOT_IF_NOT(rx_buf); + bt_buf_set_type(rx_buf, BT_BUF_CMD); + break; + case HCI_3WIRE_ACK_PKT: + // h5.rx_ack = hdr.ack; + // We currently don't retransmit, so drop ack packets + goto next; + case HCI_3WIRE_LINK_PKT: + rx_buf = net_buf_alloc(&h5_pool, K_NO_WAIT); + REBOOT_IF_NOT(rx_buf); + break; + default: + LOG_ERR("Wrong packet type from host: %u", + hdr.packet_type); + goto next; + } int byte; while ((byte = unslip_next_byte(buf)) >= 0) { @@ -595,35 +624,36 @@ static void unproc_thread(void) { goto next; } - switch (hdr.packet_type) { - case HCI_3WIRE_ACK_PKT: - // No further action required - break; - case HCI_3WIRE_LINK_PKT: - net_buf_put(&h5.host_queue, rx_buf); - break; - case HCI_COMMAND_PKT: - case HCI_ACLDATA_PKT: - //LOG_DBG("Adding to controller queue\n"); - net_buf_put(&h5.controller_queue, rx_buf); - break; - default: - LOG_WRN("Unknown packet type %u\n", hdr.packet_type); - break; - } -next: + switch (hdr.packet_type) { + case HCI_3WIRE_ACK_PKT: + // No further action required + break; + case HCI_3WIRE_LINK_PKT: + net_buf_put(&h5.host_queue, rx_buf); + break; + case HCI_COMMAND_PKT: + case HCI_ACLDATA_PKT: + //LOG_DBG("Adding to controller queue\n"); + net_buf_put(&h5.controller_queue, rx_buf); + break; + default: + LOG_WRN("Unknown packet type %u\n", hdr.packet_type); + break; + } + next: net_buf_unref(buf); } } -static void h5_send_sync(void); -static void h5_send_config(void); +static void +h5_send_sync(void); +static void +h5_send_config(void); - - -static void tx_thread(void) +static void +tx_thread(void) { - k_thread_name_set(NULL, "tx_thread"); + k_thread_name_set(NULL, "tx_thread"); while (true) { struct net_buf *buf; @@ -631,19 +661,20 @@ static void tx_thread(void) switch (h5.link_state) { case UNINIT: h5_send_sync(); - LOG_DBG("Autonomous SYNC"); + LOG_DBG("Autonomous SYNC"); k_sleep(250); break; case INIT: h5_send_config(); - LOG_DBG("Autonomous CONFIG"); + LOG_DBG("Autonomous CONFIG"); k_sleep(250); break; case ACTIVE: - while ((buf = net_buf_get(&h5.controller_queue, K_FOREVER))) { - bt_send(buf); - } - //LOG_DBG("controller_queue empty"); + while ((buf = net_buf_get(&h5.controller_queue, + K_FOREVER))) { + bt_send(buf); + } + //LOG_DBG("controller_queue empty"); /* buf is dequeued from tx_queue and queued to unack * queue. @@ -659,31 +690,37 @@ static void tx_thread(void) } } -static void event_thread(void) { - static struct net_buf *buf = NULL; - while (true) { - buf = net_buf_get(&h5.event_queue, K_MSEC(1000)); +static void +event_thread(void) +{ + static struct net_buf *buf = NULL; + while (true) { + buf = net_buf_get(&h5.event_queue, K_MSEC(1000)); if (!buf) { - LOG_WRN("event_queue is empty"); - continue; + LOG_WRN("event_queue is empty"); + continue; } net_buf_put(&h5.host_queue, buf); - } + } } -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); +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); - } + 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); + net_buf_unref(buf); + } } -static void h5_init(void) +static void +h5_init(void) { LOG_DBG("h5_init"); @@ -693,10 +730,10 @@ static void h5_init(void) /* TX thread - handles period h5_link messages and forwards commands to controller */ k_fifo_init(&h5.controller_queue); k_thread_create(&tx_thread_data, tx_stack, - K_THREAD_STACK_SIZEOF(tx_stack), - (k_thread_entry_t)tx_thread, NULL, NULL, NULL, - K_PRIO_PREEMPT(1), - 0, K_NO_WAIT); + K_THREAD_STACK_SIZEOF(tx_stack), + (k_thread_entry_t) tx_thread, NULL, NULL, NULL, + K_PRIO_PREEMPT(1), + 0, K_NO_WAIT); k_fifo_init(&h5.host_queue); /*k_thread_create(&rx_thread_data, rx_stack, @@ -711,26 +748,26 @@ static void h5_init(void) /* Thread & queue to un-slip and un-h5 incoming packets */ k_fifo_init(&h5.unprocessed_queue); k_thread_create(&unproc_thread_data, unproc_stack, - K_THREAD_STACK_SIZEOF(unproc_stack), - (k_thread_entry_t)unproc_thread, NULL, NULL, NULL, - K_PRIO_PREEMPT(1), - 0, K_NO_WAIT); + K_THREAD_STACK_SIZEOF(unproc_stack), + (k_thread_entry_t) unproc_thread, NULL, NULL, NULL, + K_PRIO_PREEMPT(1), + 0, K_NO_WAIT); /* Thread handles incoming events from BT Controller */ k_fifo_init(&h5.event_queue); k_thread_create(&event_thread_data, event_stack, - K_THREAD_STACK_SIZEOF(event_stack), - (k_thread_entry_t)event_thread, NULL, NULL, NULL, - K_PRIO_PREEMPT(2), - 0, K_NO_WAIT); + K_THREAD_STACK_SIZEOF(event_stack), + (k_thread_entry_t) event_thread, NULL, NULL, NULL, + 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); + 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 */ @@ -738,7 +775,8 @@ static void h5_init(void) //k_delayed_work_init(&retx_work, retx_timeout); } -static int h5_open(struct device *unused) +static int +h5_open(struct device *unused) { LOG_DBG("h5_open"); @@ -751,7 +789,7 @@ static int h5_open(struct device *unused) uart_irq_tx_disable(hci_uart_dev); /* Enable HFXO so that UART clock is stable for 115200 bitrate */ - *(uint32_t *)0x40000000 = 0x1; + *(uint32_t *) 0x40000000 = 0x1; bt_uart_drain(hci_uart_dev); @@ -771,71 +809,95 @@ void bt_ctlr_assert_handle(char *file, u32_t line) #endif /* CONFIG_BT_CTLR_ASSERT_HANDLER */ DEVICE_INIT(hci_uart, "hci_uart", &h5_open, NULL, NULL, - APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEVICE); + APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEVICE); enum h5_pkt_types { - H5_UNKNOWN = -1, - H5_SYNC, - H5_SYNC_RESPONSE, - H5_CONFIG, - H5_CONFIG_RESPONSE, + H5_UNKNOWN = -1, + H5_SYNC, + H5_SYNC_RESPONSE, + H5_CONFIG, + H5_CONFIG_RESPONSE, }; -bool _link_ctrl_memcmp(struct net_buf const * buf, u8_t const * const ref) { +bool +_link_ctrl_memcmp(struct net_buf const *buf, u8_t const *const ref) +{ return !memcmp(buf->data, ref, 2); } -bool packet_is_sync(struct net_buf const *buf) { +bool +packet_is_sync(struct net_buf const *buf) +{ return _link_ctrl_memcmp(buf, sync_req); } -bool packet_is_sync_response(struct net_buf const *buf) { +bool +packet_is_sync_response(struct net_buf const *buf) +{ return _link_ctrl_memcmp(buf, sync_rsp); } -bool packet_is_config(struct net_buf const *buf) { +bool +packet_is_config(struct net_buf const *buf) +{ return _link_ctrl_memcmp(buf, conf_req); } -static bool packet_is_config_response(struct net_buf const *buf) { +static bool +packet_is_config_response(struct net_buf const *buf) +{ return _link_ctrl_memcmp(buf, conf_rsp); } -static void _send_link_control(u8_t const * const buf, u8_t len) { +static void +_send_link_control(u8_t const *const buf, u8_t len) +{ h5_send(buf, HCI_3WIRE_LINK_PKT, len); } -int packet_get_type(struct net_buf const * buf) { - if (packet_is_sync(buf)){ - return H5_SYNC; - } else if (packet_is_sync_response(buf)) { - return H5_SYNC_RESPONSE; - } else if (packet_is_config(buf)) { - return H5_CONFIG; - } else if (packet_is_config_response(buf)) { - return H5_CONFIG_RESPONSE; - } - return H5_UNKNOWN; +int +packet_get_type(struct net_buf const *buf) +{ + if (packet_is_sync(buf)) { + return H5_SYNC; + } else if (packet_is_sync_response(buf)) { + return H5_SYNC_RESPONSE; + } else if (packet_is_config(buf)) { + return H5_CONFIG; + } else if (packet_is_config_response(buf)) { + return H5_CONFIG_RESPONSE; + } + return H5_UNKNOWN; } -static void h5_send_sync(void) { +static void +h5_send_sync(void) +{ _send_link_control(sync_req, sizeof(sync_req)); } -static void h5_send_sync_response(void) { +static void +h5_send_sync_response(void) +{ _send_link_control(sync_rsp, sizeof(sync_rsp)); } -static void h5_send_config(void) { +static void +h5_send_config(void) +{ _send_link_control(conf_req, sizeof(conf_req)); } -static void h5_send_config_response(void) { +static void +h5_send_config_response(void) +{ _send_link_control(conf_rsp, sizeof(conf_rsp)); } static struct device *gpio_dev; static struct gpio_callback gpio_cb; -void gpio_callback(struct device *port, - struct gpio_callback *cb, u32_t pins) { +void +gpio_callback(struct device *port, + struct gpio_callback *cb, u32_t pins) +{ LOG_ERR("Host wants the bootloader, rebooting\n"); k_sleep(250); sys_reboot(SYS_REBOOT_COLD); @@ -843,18 +905,23 @@ void gpio_callback(struct device *port, #define BOOTLOADER_REQ_GPIO_PIN 8 #define BOOTLOADER_STATUS_GPIO_PIN 7 -void gpio_init() { - int ret; - gpio_dev = device_get_binding("GPIO_0"); - if (!gpio_dev) { - LOG_ERR("Cannot find %s!\n", "GPIO_0"); - } - ret = gpio_pin_configure(gpio_dev, BOOTLOADER_REQ_GPIO_PIN, - (GPIO_DIR_IN | GPIO_INT | GPIO_INT_EDGE | GPIO_INT_ACTIVE_LOW | GPIO_PUD_PULL_UP)); - if (ret) { - LOG_ERR("Error configuring GPIO_%d!\n", BOOTLOADER_REQ_GPIO_PIN); - } - gpio_init_callback(&gpio_cb, gpio_callback, BIT(BOOTLOADER_REQ_GPIO_PIN)); +void +gpio_init() +{ + int ret; + gpio_dev = device_get_binding("GPIO_0"); + if (!gpio_dev) { + LOG_ERR("Cannot find %s!\n", "GPIO_0"); + } + ret = gpio_pin_configure(gpio_dev, BOOTLOADER_REQ_GPIO_PIN, + (GPIO_DIR_IN | GPIO_INT | GPIO_INT_EDGE | GPIO_INT_ACTIVE_LOW + | GPIO_PUD_PULL_UP)); + if (ret) { + LOG_ERR("Error configuring GPIO_%d!\n", + BOOTLOADER_REQ_GPIO_PIN); + } + gpio_init_callback(&gpio_cb, gpio_callback, + BIT(BOOTLOADER_REQ_GPIO_PIN)); ret = gpio_add_callback(gpio_dev, &gpio_cb); if (ret) { @@ -868,87 +935,98 @@ void gpio_init() { } -static h5_linkstate_t do_uninit(struct net_buf const *buf) { - switch (packet_get_type(buf)) { - case H5_SYNC: - LOG_DBG("RX'd SYNC: UNINIT -> UNINIT"); - h5_send_sync_response(); - return UNINIT; - case H5_SYNC_RESPONSE: - //h5_send_config(); - LOG_DBG("RX'd SYNC RESPONSE: UNINIT -> INIT"); - return INIT; - default: - LOG_DBG("RX'd Incorrect Type: UNINIT -> UNINIT"); - /* SYNC is the answer to any non-SYNC_RESP packets in UNINIT state */ - //h5_send_sync(); - return UNINIT; - } +static h5_linkstate_t +do_uninit(struct net_buf const *buf) +{ + switch (packet_get_type(buf)) { + case H5_SYNC: + LOG_DBG("RX'd SYNC: UNINIT -> UNINIT"); + h5_send_sync_response(); + return UNINIT; + case H5_SYNC_RESPONSE: + //h5_send_config(); + LOG_DBG("RX'd SYNC RESPONSE: UNINIT -> INIT"); + return INIT; + default: + LOG_DBG("RX'd Incorrect Type: UNINIT -> UNINIT"); + /* SYNC is the answer to any non-SYNC_RESP packets in UNINIT state */ + //h5_send_sync(); + return UNINIT; + } } -static h5_linkstate_t do_init(struct net_buf const *buf) { - switch (packet_get_type(buf)) { - case H5_SYNC: - LOG_DBG("RX'd SYNC: INIT -> INIT"); - h5_send_sync_response(); - return INIT; - case H5_CONFIG: - LOG_DBG("RX'd CONFIG: INIT -> INIT"); - h5_send_config_response(); - return INIT; - case H5_CONFIG_RESPONSE: - h5.tx_win = conf_rsp[2] & 0x7; - h5.next_expected_seq_from_host = 0; - h5.local_seq = 0; - LOG_DBG("Finished H5 configuration, tx_win %u: INIT -> ACTIVE", h5.tx_win); - return ACTIVE; - default: - LOG_DBG("RX'd Other: INIT -> INIT"); - return INIT; - } +static h5_linkstate_t +do_init(struct net_buf const *buf) +{ + switch (packet_get_type(buf)) { + case H5_SYNC: + LOG_DBG("RX'd SYNC: INIT -> INIT"); + h5_send_sync_response(); + return INIT; + case H5_CONFIG: + LOG_DBG("RX'd CONFIG: INIT -> INIT"); + h5_send_config_response(); + return INIT; + case H5_CONFIG_RESPONSE: + h5.tx_win = conf_rsp[2] & 0x7; + h5.next_expected_seq_from_host = 0; + h5.local_seq = 0; + LOG_DBG("Finished H5 configuration, tx_win %u: INIT -> ACTIVE", + h5.tx_win); + return ACTIVE; + default: + LOG_DBG("RX'd Other: INIT -> INIT"); + return INIT; + } } -static h5_linkstate_t do_active(struct net_buf *buf) { - switch (packet_get_type(buf)) { - case H5_SYNC: - LOG_WRN("SYNC in ACTIVE state, peer reset: ACTIVE -> UNINIT"); - //h5_send_sync(); - return UNINIT; - case H5_SYNC_RESPONSE: - LOG_DBG("SYNC_RESPONSE in ACTIVE state: ACTIVE -> UNINIT"); - //h5_send_sync(); - return UNINIT; - case H5_CONFIG: - h5_send_config_response(); - LOG_DBG("RX'd CONFIG: ACTIVE -> ACTIVE"); - return ACTIVE; - case H5_CONFIG_RESPONSE: - LOG_DBG("RX'd CONFIG RESPONSE: ACTIVE -> ACTIVE"); - return ACTIVE; - default: - // Presumably something from the controller - if (bt_buf_get_type(buf) == BT_BUF_EVT) { - h5_send(buf->data, HCI_EVENT_PKT, buf->len); - } else { - LOG_HEXDUMP_ERR(buf->data, buf->len, "Unexpected buffer in host_queue"); - } - return ACTIVE; - } +static h5_linkstate_t +do_active(struct net_buf *buf) +{ + switch (packet_get_type(buf)) { + case H5_SYNC: + LOG_WRN("SYNC in ACTIVE state, peer reset: ACTIVE -> UNINIT"); + //h5_send_sync(); + return UNINIT; + case H5_SYNC_RESPONSE: + LOG_DBG("SYNC_RESPONSE in ACTIVE state: ACTIVE -> UNINIT"); + //h5_send_sync(); + return UNINIT; + case H5_CONFIG: + h5_send_config_response(); + LOG_DBG("RX'd CONFIG: ACTIVE -> ACTIVE"); + return ACTIVE; + case H5_CONFIG_RESPONSE: + LOG_DBG("RX'd CONFIG RESPONSE: ACTIVE -> ACTIVE"); + return ACTIVE; + default: + // Presumably something from the controller + if (bt_buf_get_type(buf) == BT_BUF_EVT) { + h5_send(buf->data, HCI_EVENT_PKT, buf->len); + } else { + LOG_HEXDUMP_ERR(buf->data, buf->len, + "Unexpected buffer in host_queue"); + } + return ACTIVE; + } } -void state_change(int old, int new) { - if (old == new) { - return; - } - h5.link_state = new; - if (old != ACTIVE) { - k_wakeup(&tx_thread_data); - } else { - k_fifo_cancel_wait(&h5.controller_queue); - } +void +state_change(int old, int new) +{ + if (old == new) { + return; + } + h5.link_state = new; + if (old != ACTIVE) { + k_wakeup(&tx_thread_data); + } else { + k_fifo_cancel_wait(&h5.controller_queue); + } } -void main(void) +void +main(void) { LOG_DBG("Start"); gpio_init(); @@ -956,25 +1034,25 @@ void main(void) bt_enable_raw(&h5.event_queue); struct net_buf *buf = NULL; - h5_init(); + h5_init(); while (true) { buf = net_buf_get(&h5.host_queue, K_MSEC(1000)); if (!buf) { - LOG_WRN("host_queue is empty"); - continue; + LOG_WRN("host_queue is empty"); + continue; } switch (h5.link_state) { case UNINIT: - state_change(h5.link_state, do_uninit(buf)); + state_change(h5.link_state, do_uninit(buf)); break; case INIT: - state_change(h5.link_state, do_init(buf)); - break; + state_change(h5.link_state, do_init(buf)); + break; case ACTIVE: - state_change(h5.link_state, do_active(buf)); - break; + state_change(h5.link_state, do_active(buf)); + break; } net_buf_unref(buf); }