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

Simplifies logic with bitfield struct hdr operations

This commit is contained in:
Shawn Nock 2019-07-17 15:11:28 -04:00
parent b1d071da0d
commit 249b882fae
1 changed files with 48 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/main.c
View File

@ -99,6 +99,16 @@ static bool reliable_packet(u8_t type)
#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;
} h5_hdr_t;
#define H5_HDR_SEQ(hdr) ((hdr)[0] & 0x07)
#define H5_HDR_ACK(hdr) (((hdr)[0] >> 3) & 0x07)
#define H5_HDR_CRC(hdr) (((hdr)[0] >> 6) & 0x01)
@ -352,7 +362,7 @@ static void ack_timeout(struct k_work *work)
int unslip_next_byte(struct net_buf *buf) {
if (!buf->len) {
LOG_WRN("Tried to unslip empty buffer");
// This happens at the end of every buffer, not work logging
return -1;
}
@ -436,21 +446,45 @@ int pull_header(struct net_buf *buf, u8_t *hdr) {
return 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;
}
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;
}
return 0;
}
static void unproc_thread(void) {
struct net_buf *buf;
while (true) {
buf = net_buf_get(&h5.unprocessed_queue, K_FOREVER);
u8_t hdr[4];
if (pull_header(buf, hdr) < 0) {
//u8_t hdr[4];
h5_hdr_t hdr = {0};
if (pull_header1(buf, &hdr) < 0) {
// Header is invalid
goto next;
}
struct net_buf *rx_buf = NULL;
switch (H5_HDR_PKT_TYPE(hdr)) {
switch (hdr.packet_type) {
case HCI_ACLDATA_PKT:
rx_buf = net_buf_alloc(&acl_tx_pool, K_NO_WAIT);
REBOOT_IF_NOT(rx_buf);
@ -462,15 +496,14 @@ static void unproc_thread(void) {
bt_buf_set_type(rx_buf, BT_BUF_CMD);
break;
case HCI_3WIRE_ACK_PKT:
h5.rx_ack = H5_HDR_ACK(hdr);
h5.rx_ack = hdr.ack;
goto next;
break;
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", H5_HDR_PKT_TYPE(hdr));
LOG_ERR("Wrong packet type from host: %u", hdr.packet_type);
goto next;
}
@ -479,27 +512,26 @@ static void unproc_thread(void) {
net_buf_add_u8(rx_buf, (u8_t) byte);
}
if (H5_HDR_LEN(hdr) != rx_buf->len) {
LOG_ERR("Payload too short\n");
if (hdr.len != rx_buf->len) {
LOG_WRN("Truncated payload, skipping\n");
goto next;
}
if (H5_HDR_RELIABLE(hdr) &&
H5_HDR_SEQ(hdr) != h5.tx_ack) {
LOG_ERR("Seq expected %u got %u. Drop packet", h5.tx_ack, H5_HDR_SEQ(hdr));
if (hdr.is_reliable && (hdr.seq != h5.tx_ack)) {
LOG_ERR("Seq expected %u got %u. Drop packet", h5.tx_ack, hdr.seq);
goto next;
}
h5.rx_ack = H5_HDR_ACK(hdr);
h5.rx_ack = hdr.ack;
if (reliable_packet(H5_HDR_PKT_TYPE(hdr))) {
if (hdr.is_reliable) {
/* For reliable packet increment next transmit ack number */
h5.tx_ack = (h5.tx_ack + 1) % 8;
/* Submit delayed work to ack the packet */
k_delayed_work_submit(&ack_work, H5_RX_ACK_TIMEOUT);
}
switch (H5_HDR_PKT_TYPE(hdr)) {
switch (hdr.packet_type) {
case HCI_3WIRE_ACK_PKT:
// No further action required
break;
@ -512,7 +544,7 @@ static void unproc_thread(void) {
net_buf_put(&h5.controller_queue, rx_buf);
break;
default:
LOG_WRN("Unknown packet type %u\n", H5_HDR_PKT_TYPE(hdr));
LOG_WRN("Unknown packet type %u\n", hdr.packet_type);
break;
}
next: