Embedded
/
zephyr-hci-uart-3wire
1
0
Fork 0
Code Releases Activity

packet oriented approach

This commit is contained in:
Shawn Nock 2018-01-02 07:56:33 -05:00
parent e2a6ca7035
commit fe70ee1c86
1 changed files with 263 additions and 279 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

532
src/main.c
View File

@ -53,6 +53,9 @@ NET_BUF_POOL_DEFINE(acl_tx_pool, TX_BUF_COUNT, BT_BUF_ACL_SIZE,
static BT_STACK_NOINIT(tx_stack, 256);
static struct k_thread tx_thread_data;
static BT_STACK_NOINIT(unproc_stack, 256);
static struct k_thread unproc_thread_data;
static struct k_delayed_work ack_work;
static struct k_delayed_work retx_work;
@ -103,11 +106,12 @@ static bool reliable_packet(u8_t type)
((hdr)[2] |= (len) >> 4))
static struct h5 {
struct net_buf *rx_buf;
//struct net_buf *rx_buf;
struct k_fifo tx_queue;
struct k_fifo rx_queue;
struct k_fifo controller_queue;
struct k_fifo host_queue;
struct k_fifo unack_queue;
struct k_fifo unprocessed_queue;
u8_t tx_win;
u8_t tx_ack;
@ -131,18 +135,25 @@ static struct h5 {
static u8_t unack_queue_len;
#define MAX_PACKETS_IN_FLIGHT (0x01)
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, 0x4 };
static const u8_t conf_rsp[] = { 0x04, 0x7b, MAX_PACKETS_IN_FLIGHT };
/* H5 signal buffers pool */
#define MAX_SIG_LEN 3
#define SIGNAL_COUNT 2
#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);
/* 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);
static inline void bt_uart_drain(struct device *dev)
{
u8_t c;
@ -152,46 +163,6 @@ static inline void bt_uart_drain(struct device *dev)
}
}
static void h5_reset_rx(void)
{
SYS_LOG_DBG("Reset");
if (h5.rx_buf) {
net_buf_unref(h5.rx_buf);
h5.rx_buf = NULL;
}
h5.rx_state = START;
}
static int h5_unslip_byte(u8_t *byte)
{
int count;
if (*byte != SLIP_ESC) {
return 0;
}
SYS_LOG_DBG("ESCAPE\n");
do {
count = uart_fifo_read(hci_uart_dev, byte, sizeof(*byte));
} while (!count);
switch (*byte) {
case SLIP_ESC_DELIM:
*byte = SLIP_DELIMITER;
break;
case SLIP_ESC_ESC:
*byte = SLIP_ESC;
break;
default:
SYS_LOG_ERR("Invalid escape byte %x\n", *byte);
return -EIO;
}
return 0;
}
static void process_unack(void)
{
u8_t next_seq = h5.tx_seq;
@ -235,7 +206,7 @@ static void process_unack(void)
/* TODO: print or do something with packet */
SYS_LOG_DBG("Remove buf from the unack_queue");
net_buf_unref(buf);
//net_buf_unref(buf);
unack_queue_len--;
number_removed--;
}
@ -308,7 +279,7 @@ void h5_send(const u8_t *payload, u8_t type, int len)
u8_t hdr[4];
int i;
hexdump("To Host <= ", payload, len);
//hexdump("To Host <= ", payload, len);
memset(hdr, 0, sizeof(hdr));
@ -328,7 +299,7 @@ void h5_send(const u8_t *payload, u8_t type, int len)
/* Calculate CRC */
hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
h5_print_header(hdr, "TX: <");
//h5_print_header(hdr, "TX: <");
uart_poll_out(hci_uart_dev, SLIP_DELIMITER);
@ -357,21 +328,21 @@ static void retx_timeout(struct k_work *work)
k_fifo_init(&tmp_queue);
/* Queue to temperary queue */
while ((buf = net_buf_get(&h5.tx_queue, K_NO_WAIT))) {
while ((buf = net_buf_get(&h5.host_queue, K_NO_WAIT))) {
net_buf_put(&tmp_queue, buf);
}
/* Queue unack packets to the beginning of the queue */
while ((buf = net_buf_get(&h5.unack_queue, K_NO_WAIT))) {
/* include also packet type */
net_buf_put(&h5.tx_queue, buf);
net_buf_put(&h5.host_queue, buf);
h5.tx_seq = (h5.tx_seq - 1) & 0x07;
unack_queue_len--;
}
/* Queue saved packets from temp queue */
while ((buf = net_buf_get(&tmp_queue, K_NO_WAIT))) {
net_buf_put(&h5.tx_queue, buf);
net_buf_put(&h5.host_queue, buf);
}
}
}
@ -389,13 +360,157 @@ static void ack_timeout(struct k_work *work)
//STACK_ANALYZE("rx_stack", rx_stack);
}
static void h5_process_complete_packet(u8_t *hdr)
int unslip_next_byte(struct net_buf *buf) {
if (!buf->len) {
return -1;
}
u8_t next = net_buf_pull_u8(buf);
if (next != SLIP_ESC) {
return next;
}
if (!buf->len) {
return -1;
}
next = net_buf_pull_u8(buf);
if (next == SLIP_ESC_ESC) {
return SLIP_ESC;
}
if (next == SLIP_ESC_DELIM) {
return SLIP_DELIMITER;
}
SYS_LOG_WRN("Bad Escape Seqence: %02X %02X", SLIP_ESC, next);
return -2;
}
void bt_uart_isr(struct device *unused)
{
static u8_t byte;
static struct net_buf *buf = NULL;
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);
if (!buf) {
bt_uart_drain(hci_uart_dev);
break;
}
SYS_LOG_DBG("ALLOC %p", buf);
}
if (!uart_fifo_read(hci_uart_dev, &byte, sizeof(byte))) {
continue;
}
if (byte == SLIP_DELIMITER) {
if (buf->len > 0) {
net_buf_put(&h5.unprocessed_queue, buf);
buf = NULL;
}
} else {
net_buf_add_u8(buf, byte);
}
}
}
int pull_header(struct net_buf *buf, u8_t *hdr) {
// Packet too short to contain an h5 header
if (buf->len < 4) {
return -1;
}
for (u8_t i = 0; i < 4; i++) {
int byte = unslip_next_byte(buf);
if (byte < 0) {
// Packet too short due to escaped bytes
return -1;
}
hdr[i] = byte;
}
// Checksum
if (((hdr[3] + hdr[0] + hdr[1] + hdr[2]) & 0xff) != 0xff) {
SYS_LOG_WRN("Invalid Header Checksum\n");
}
return 0;
}
static void unproc_thread(void) {
struct net_buf *buf;
//SYS_LOG_DBG("");
while (true) {
buf = net_buf_get(&h5.unprocessed_queue, K_FOREVER);
//hexdump("Packet: ", buf->data, buf->len);
u8_t hdr[4];
if (pull_header(buf, hdr) < 0) {
// Header is invalid
goto next;
}
struct net_buf *rx_buf = NULL;
switch (H5_HDR_PKT_TYPE(hdr)) {
case HCI_ACLDATA_PKT:
rx_buf = net_buf_alloc(&acl_tx_pool, K_NO_WAIT);
if (!rx_buf) {
SYS_LOG_WRN("No available data buffers");
return;
}
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);
if (!rx_buf) {
SYS_LOG_WRN("No available data buffers");
return;
}
bt_buf_set_type(rx_buf, BT_BUF_CMD);
break;
case HCI_3WIRE_LINK_PKT:
case HCI_3WIRE_ACK_PKT:
rx_buf = net_buf_alloc(&h5_pool, K_NO_WAIT);
if (!rx_buf) {
SYS_LOG_WRN("No available signal buffers");
return;
}
break;
default:
SYS_LOG_ERR("Wrong packet type from host: %u", H5_HDR_PKT_TYPE(hdr));
return;
}
int byte;
while ((byte = unslip_next_byte(buf)) >= 0) {
net_buf_add_u8(rx_buf, (u8_t) byte);
}
if (H5_HDR_LEN(hdr) != rx_buf->len) {
SYS_LOG_ERR("Payload too short\n");
goto next;
}
//h5_print_header(hdr, "From Host =>");
//hexdump("\tDecoded: ", rx_buf->data, rx_buf->len);
/* Check when full packet is received, it can be done
* when parsing packet header but we need to receive
* full packet anyway to clear UART.
*/
if (H5_HDR_RELIABLE(hdr) &&
H5_HDR_SEQ(hdr) != h5.tx_ack) {
SYS_LOG_ERR("Seq expected %u got %u. Drop packet", h5.tx_ack,
H5_HDR_SEQ(hdr));
goto next;
}
/* rx_ack should be in every packet */
h5.rx_ack = H5_HDR_ACK(hdr);
if (reliable_packet(H5_HDR_PKT_TYPE(hdr))) {
@ -405,202 +520,33 @@ static void h5_process_complete_packet(u8_t *hdr)
k_delayed_work_submit(&ack_work, H5_RX_ACK_TIMEOUT);
}
h5_print_header(hdr, "From Host =>");
process_unack();
buf = h5.rx_buf;
h5.rx_buf = NULL;
//process_unack();
switch (H5_HDR_PKT_TYPE(hdr)) {
case HCI_3WIRE_ACK_PKT:
net_buf_unref(buf);
// No further action required
break;
case HCI_3WIRE_LINK_PKT:
net_buf_put(&h5.rx_queue, buf);
net_buf_put(&h5.host_queue, rx_buf);
break;
case HCI_COMMAND_PKT:
case HCI_ACLDATA_PKT:
hexdump("HOST -> TX_QUEUE: ", buf->data, buf->len);
net_buf_put(&h5.tx_queue, buf);
//SYS_LOG_DBG("Adding to controller queue\n");
net_buf_put(&h5.controller_queue, rx_buf);
break;
default:
SYS_LOG_WRN("Unknown packet type %u\n", H5_HDR_PKT_TYPE(hdr));
break;
}
}
void bt_uart_isr(struct device *unused)
{
static int remaining;
u8_t byte;
int ret;
static u8_t hdr[4];
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)) {
if (uart_irq_tx_ready(hci_uart_dev)) {
SYS_LOG_DBG("transmit ready");
} else {
SYS_LOG_DBG("spurious interrupt");
}
/* Only the UART RX path is interrupt-enabled */
break;
}
ret = uart_fifo_read(hci_uart_dev, &byte, sizeof(byte));
SYS_LOG_DBG("BYTE: %02X, STATE: %02X", byte, h5.rx_state);
if (!ret) {
continue;
}
switch (h5.rx_state) {
case START:
if (byte == SLIP_DELIMITER) {
h5.rx_state = HEADER;
remaining = sizeof(hdr);
}
break;
case HEADER:
/* In a case we confuse ending slip delimeter
* with starting one.
*/
if (byte == SLIP_DELIMITER) {
remaining = sizeof(hdr);
continue;
}
if (h5_unslip_byte(&byte) < 0) {
h5_reset_rx();
continue;
}
memcpy(&hdr[sizeof(hdr) - remaining], &byte, 1);
remaining--;
if (remaining) {
break;
}
remaining = H5_HDR_LEN(hdr);
SYS_LOG_DBG("Payload len: %u", remaining);
switch (H5_HDR_PKT_TYPE(hdr)) {
case HCI_ACLDATA_PKT:
h5.rx_buf = net_buf_alloc(&acl_tx_pool, K_NO_WAIT);
if (!h5.rx_buf) {
SYS_LOG_WRN("No available data buffers");
h5_reset_rx();
continue;
}
bt_buf_set_type(h5.rx_buf, BT_BUF_ACL_OUT);
h5.rx_state = PAYLOAD;
break;
case HCI_COMMAND_PKT:
h5.rx_buf = net_buf_alloc(&cmd_tx_pool, K_NO_WAIT);
if (!h5.rx_buf) {
SYS_LOG_WRN("No available data buffers");
h5_reset_rx();
continue;
}
bt_buf_set_type(h5.rx_buf, BT_BUF_CMD);
h5.rx_state = PAYLOAD;
break;
case HCI_3WIRE_LINK_PKT:
case HCI_3WIRE_ACK_PKT:
h5.rx_buf = net_buf_alloc(&h5_pool, K_NO_WAIT);
if (!h5.rx_buf) {
SYS_LOG_WRN("No available signal buffers");
h5_reset_rx();
continue;
}
h5.rx_state = PAYLOAD;
break;
default:
SYS_LOG_ERR("Wrong packet type %u", H5_HDR_PKT_TYPE(hdr));
h5.rx_state = END;
break;
}
break;
case PAYLOAD:
if (h5_unslip_byte(&byte) < 0) {
SYS_LOG_WRN("Slip error\n");
h5_reset_rx();
continue;
}
net_buf_add_u8(h5.rx_buf, byte);
remaining--;
SYS_LOG_DBG("Payload remaining: %u", remaining);
if (!remaining) {
h5.rx_state = END;
}
break;
case END:
if (byte != SLIP_DELIMITER) {
SYS_LOG_ERR("Missing ending SLIP_DELIMITER");
h5_reset_rx();
break;
}
SYS_LOG_DBG("Received full packet: type %u",
H5_HDR_PKT_TYPE(hdr));
/* Check when full packet is received, it can be done
* when parsing packet header but we need to receive
* full packet anyway to clear UART.
*/
if (H5_HDR_RELIABLE(hdr) &&
H5_HDR_SEQ(hdr) != h5.tx_ack) {
SYS_LOG_ERR("Seq expected %u got %u. Drop packet",
h5.tx_ack, H5_HDR_SEQ(hdr));
h5_reset_rx();
break;
}
h5_process_complete_packet(hdr);
h5.rx_state = START;
break;
next:
SYS_LOG_DBG("UNREF: %p", buf);
net_buf_unref(buf);
}
}
}
u8_t h5_get_type(struct net_buf *buf)
{
return net_buf_pull_u8(buf) & 0xf;
}
/*static int h5_queue(struct net_buf *buf)
{
u8_t type;
BT_DBG("buf %p type %u len %u", buf, bt_buf_get_type(buf), buf->len);
switch (bt_buf_get_type(buf)) {
case BT_BUF_CMD:
type = HCI_COMMAND_PKT;
break;
case BT_BUF_ACL_OUT:
type = HCI_ACLDATA_PKT;
break;
default:
BT_ERR("Unknown packet type %u", bt_buf_get_type(buf));
return -1;
}
memcpy(net_buf_push(buf, sizeof(type)), &type, sizeof(type));
net_buf_put(&h5.tx_queue, buf);
return 0;
}*/
static void tx_thread(void)
{
SYS_LOG_DBG("");
SYS_LOG_DBG("TX Thread is alive.");
while (true) {
struct net_buf *buf;
@ -609,25 +555,26 @@ static void tx_thread(void)
switch (h5.link_state) {
case UNINIT:
/* FIXME: send sync */
k_sleep(100);
break;
case INIT:
/* FIXME: send conf */
k_sleep(100);
k_sleep(250);
break;
case ACTIVE:
buf = net_buf_get(&h5.tx_queue, K_FOREVER);
buf = net_buf_get(&h5.controller_queue, K_MSEC(250));
if (!buf) {
break;
}
hexdump("TX_QUEUE -> CTRL", buf->data, buf->len);
bt_send(buf);
/* buf is dequeued from tx_queue and queued to unack
* queue.
*/
net_buf_put(&h5.unack_queue, buf);
unack_queue_len++;
k_delayed_work_submit(&retx_work, H5_TX_ACK_TIMEOUT);
//net_buf_put(&h5.unack_queue, buf);
//unack_queue_len++;
//k_delayed_work_submit(&retx_work, H5_TX_ACK_TIMEOUT);
break;
}
@ -643,14 +590,14 @@ static void h5_init(void)
h5.tx_win = 4;
/* TX thread */
k_fifo_init(&h5.tx_queue);
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_COOP(CONFIG_BT_HCI_TX_PRIO),
0, K_NO_WAIT);
k_fifo_init(&h5.rx_queue);
k_fifo_init(&h5.host_queue);
/*k_thread_create(&rx_thread_data, rx_stack,
K_THREAD_STACK_SIZEOF(rx_stack),
(k_thread_entry_t)rx_thread, NULL, NULL, NULL,
@ -660,6 +607,14 @@ static void h5_init(void)
/* Unack queue */
k_fifo_init(&h5.unack_queue);
/* 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_COOP(CONFIG_BT_RX_PRIO),
0, K_NO_WAIT);
/* Init delayed work */
k_delayed_work_init(&ack_work, ack_timeout);
k_delayed_work_init(&retx_work, retx_timeout);
@ -739,71 +694,100 @@ void bt_ctlr_assert_handle(char *file, u32_t line)
DEVICE_INIT(hci_uart, "hci_uart", &h5_open, NULL, NULL,
APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEVICE);
bool _link_ctrl_memcmp(struct net_buf const * const buf, u8_t const * const ref) {
return !memcmp(buf->data, ref, 2);
}
bool packet_is_sync(struct net_buf *buf) {
return _link_ctrl_memcmp(buf, sync_req);
}
bool packet_is_sync_response(struct net_buf *buf) {
return _link_ctrl_memcmp(buf, sync_rsp);
}
bool packet_is_config(struct net_buf *buf) {
return _link_ctrl_memcmp(buf, conf_req);
}
static bool packet_is_config_response(struct net_buf *buf) {
return _link_ctrl_memcmp(buf, conf_rsp);
}
static void _send_link_control(u8_t const * const buf, u8_t len) {
h5_send(buf, HCI_3WIRE_LINK_PKT, len);
}
static void h5_send_sync(void) {
_send_link_control(sync_req, sizeof(sync_req));
}
static void h5_send_sync_response(void) {
_send_link_control(sync_rsp, sizeof(sync_rsp));
}
static void h5_send_config(void) {
_send_link_control(conf_req, sizeof(conf_req));
}
static void h5_send_config_response(void) {
_send_link_control(conf_rsp, sizeof(conf_rsp));
}
void main(void)
{
SYS_LOG_DBG("Start");
bt_enable_raw(&h5.rx_queue);
// Adds controller output to host output queue
bt_enable_raw(&h5.host_queue);
while (1) {
struct net_buf *buf;
buf = net_buf_get(&h5.rx_queue, K_FOREVER);
buf = net_buf_get(&h5.host_queue, K_FOREVER);
if (!memcmp(buf->data, sync_req, sizeof(sync_req))) {
SYS_LOG_DBG("Host sends SYNC\n");
if (packet_is_sync(buf)) {
h5.link_state = UNINIT;
h5_send(sync_rsp, HCI_3WIRE_LINK_PKT, sizeof(sync_rsp));
h5_send_sync_response();
goto next;
}
if (h5.link_state == UNINIT) {
if (!memcmp(buf->data, sync_rsp, sizeof(sync_rsp))) {
SYS_LOG_DBG("Host sends SYNC_RESP\n");
if (packet_is_sync_response(buf)) {
h5.link_state = INIT;
h5_send(conf_req, HCI_3WIRE_LINK_PKT, sizeof(conf_req));
h5_send_config();
} else {
/* SYNC is the answer to any non-SYNC_RESP packets in UNINIT
state */
h5_send(sync_req, HCI_3WIRE_LINK_PKT, sizeof(sync_req));
h5_send_sync();
}
}
if (h5.link_state == INIT) {
if (!memcmp(buf->data, conf_req, 2)) {
SYS_LOG_DBG("Host sends CONFIG\n");
h5_send(conf_rsp, HCI_3WIRE_LINK_PKT, sizeof(conf_rsp));
h5.tx_win = conf_rsp[2] & 0x7;
} else if (!memcmp(buf->data, conf_rsp, 2)) {
SYS_LOG_DBG("Host sends CONFIG_RESP\n");
} else if (h5.link_state == INIT) {
if (packet_is_config(buf)) {
h5_send_config_response();
} else if (packet_is_config_response(buf)) {
h5.link_state = ACTIVE;
h5.tx_win = conf_rsp[2] & 0x7;
h5.tx_seq = 0;
h5.tx_ack = 0;
SYS_LOG_DBG("Finished H5 configuration, tx_win %u", h5.tx_win);
}
}
if (h5.link_state == ACTIVE) {
if (!memcmp(buf->data, conf_req, 2)) {
SYS_LOG_DBG("Host sends CONFIG\n");
h5_send(conf_rsp, HCI_3WIRE_LINK_PKT, sizeof(conf_rsp));
h5.tx_win = conf_rsp[2] & 0x7;
} else if (h5.link_state == ACTIVE) {
if (packet_is_config(buf)) {
h5_send_config_response();
} else if (packet_is_config_response(buf)) {
goto next;
} else if (!memcmp(buf->data, conf_rsp, 2)) {
// Skip CONFIG_RESP in ACTIVE Mode
goto next;
}
} else if (packet_is_sync_response(buf) || packet_is_config(buf)) {
h5.link_state = UNINIT;
h5_send_sync();
} else {
// Presumably something from the controller
u8_t type = bt_buf_get_type(buf);
if (type == BT_BUF_EVT) {
SYS_LOG_DBG("EVENT from CTRL\n");
hexdump("CTRL => rx_queue", buf->data, buf->len);
//hexdump("From CTRL To HOST => ", buf->data, buf->len);
h5_send(buf->data, HCI_EVENT_PKT, buf->len);
} else {
SYS_LOG_DBG("WTF is this (%u): ", bt_buf_get_type(buf));
hexdump("CTRL WTF: ", buf->data, buf->len);
}
}
}
next:
net_buf_unref(buf);
}