static void process_data(struct net_pkt *pkt)
{
struct net_buf *buf = net_buf_frag_last(pkt->frags);
u8_t seq, num_attr;
int ret, i;
seq = net_buf_pull_u8(buf);
num_attr = net_buf_pull_u8(buf);
SYS_LOG_DBG("seq %u num_attr %u", seq, num_attr);
/**
* There are some attributes sent over this protocol
* discard them and return packet data report.
*/
for (i = 0; i < num_attr; i++) {
/* attr */
net_buf_pull_u8(buf);
/* value */
net_buf_pull_be16(buf);
}
/* Transmit data through radio */
ret = radio_api->tx(ieee802154_dev, pkt, buf);
if (ret) {
SYS_LOG_ERR("Error transmit data");
}
/* TODO: Return correct status codes */
/* TODO: Implement re-transmissions if needed */
/* Send packet data report */
send_pkt_report(seq, ret, 1);
}
static void rx_thread(void)
{
SYS_LOG_INF("RX thread started");
while (1) {
struct net_pkt *pkt;
struct net_buf *buf;
u8_t specifier;
pkt = k_fifo_get(&rx_queue, K_FOREVER);
buf = net_buf_frag_last(pkt->frags);
SYS_LOG_DBG("Got pkt %p buf %p", pkt, buf);
hexdump("SLIP >", buf->data, buf->len);
/* TODO: process */
specifier = net_buf_pull_u8(buf);
switch (specifier) {
case '?':
process_request(buf);
break;
case '!':
process_config(pkt);
break;
default:
SYS_LOG_ERR("Unknown message specifier %c", specifier);
break;
}
net_pkt_unref(pkt);
k_yield();
}
}
static void tx_thread(void)
{
SYS_LOG_DBG("Tx thread started");
while (1) {
uint8_t cmd;
struct net_buf *pkt, *buf;
pkt = net_buf_get(&tx_queue, K_FOREVER);
buf = net_buf_frag_last(pkt);
cmd = net_buf_pull_u8(buf);
hexdump(">", buf->data, buf->len);
switch (cmd) {
case RESET:
SYS_LOG_DBG("Reset device");
break;
case TX:
tx(pkt);
break;
case START:
start();
break;
case STOP:
stop();
break;
case SET_CHANNEL:
set_channel(buf->data, buf->len);
break;
case SET_IEEE_ADDR:
set_ieee_addr(buf->data, buf->len);
break;
case SET_SHORT_ADDR:
set_short_addr(buf->data, buf->len);
break;
case SET_PAN_ID:
set_pan_id(buf->data, buf->len);
break;
default:
SYS_LOG_ERR("%x: Not handled for now", cmd);
break;
}
net_nbuf_unref(pkt);
k_yield();
}
}
static void l2cap_chan_le_recv_sdu(struct bt_l2cap_le_chan *chan,
struct net_buf *buf)
{
struct net_buf *frag;
uint16_t len;
BT_DBG("chan %p len %u sdu %zu", chan, buf->len,
net_buf_frags_len(chan->_sdu));
if (net_buf_frags_len(chan->_sdu) + buf->len > chan->_sdu_len) {
BT_ERR("SDU length mismatch");
bt_l2cap_chan_disconnect(&chan->chan);
return;
}
/* Jump to last fragment */
frag = net_buf_frag_last(chan->_sdu);
while (buf->len) {
/* Check if there is any space left in the current fragment */
if (!net_buf_tailroom(frag)) {
frag = l2cap_alloc_frag(chan);
if (!frag) {
BT_ERR("Unable to store SDU");
bt_l2cap_chan_disconnect(&chan->chan);
return;
}
}
len = min(net_buf_tailroom(frag), buf->len);
net_buf_add_mem(frag, buf->data, len);
net_buf_pull(buf, len);
BT_DBG("frag %p len %u", frag, frag->len);
}
if (net_buf_frags_len(chan->_sdu) == chan->_sdu_len) {
/* Receiving complete SDU, notify channel and reset SDU buf */
chan->chan.ops->recv(&chan->chan, chan->_sdu);
net_buf_unref(chan->_sdu);
chan->_sdu = NULL;
chan->_sdu_len = 0;
}
l2cap_chan_update_credits(chan);
}
static void process_config(struct net_pkt *pkt)
{
struct net_buf *buf = net_buf_frag_last(pkt->frags);
u8_t cmd = net_buf_pull_u8(buf);
SYS_LOG_DBG("Process config %c", cmd);
switch (cmd) {
case 'S':
process_data(pkt);
break;
case 'C':
set_channel(net_buf_pull_u8(buf));
break;
default:
SYS_LOG_ERR("Unhandled cmd %u", cmd);
}
}
/**
* TX - transmit to SLIP interface
*/
static void tx_thread(void)
{
SYS_LOG_DBG("TX thread started");
/* Allow to send one TX */
k_sem_give(&tx_sem);
while (1) {
struct net_pkt *pkt;
struct net_buf *buf;
size_t len;
k_sem_take(&tx_sem, K_FOREVER);
pkt = k_fifo_get(&tx_queue, K_FOREVER);
buf = net_buf_frag_last(pkt->frags);
len = net_pkt_get_len(pkt);
SYS_LOG_DBG("Send pkt %p buf %p len %d", pkt, buf, len);
hexdump("SLIP <", buf->data, buf->len);
/* Remove LQI */
/* TODO: Reuse get_lqi() */
buf->len -= 1;
/* remove FCS 2 bytes */
buf->len -= 2;
/* SLIP encode and send */
len = slip_buffer(slip_buf, buf);
uart_fifo_fill(uart_dev, slip_buf, len);
net_pkt_unref(pkt);
#if 0
k_yield();
#endif
}
}
int net_recv_data(struct net_if *iface, struct net_buf *buf)
{
struct net_buf *frag;
SYS_LOG_DBG("Got data, buf %p, len %d frags->len %d",
buf, buf->len, net_buf_frags_len(buf));
frag = net_buf_frag_last(buf);
/**
* Add length 1 byte, do not forget to reserve it
*/
net_buf_push_u8(frag, net_buf_frags_len(buf) - 1);
hexdump("<", frag->data, net_buf_frags_len(buf));
try_write(WPANUSB_ENDP_BULK_IN, frag->data, net_buf_frags_len(buf));
net_nbuf_unref(buf);
return 0;
}
static int tx(struct net_buf *pkt)
{
struct net_buf *buf = net_buf_frag_last(pkt);
uint8_t seq = net_buf_pull_u8(buf);
int retries = 3;
int ret;
SYS_LOG_DBG("len %d seq %u", buf->len, seq);
do {
ret = radio_api->tx(ieee802154_dev, pkt);
} while (ret && retries--);
if (ret) {
SYS_LOG_ERR("Error sending data, seq %u", seq);
/* Send seq = 0 for unsuccessful send */
seq = 0;
}
try_write(WPANUSB_ENDP_BULK_IN, &seq, sizeof(seq));
return ret;
}
static int slip_process_byte(unsigned char c)
{
struct net_buf *buf;
#ifdef VERBOSE_DEBUG
SYS_LOG_DBG("recv: state %u byte %x", slip_state, c);
#endif
switch (slip_state) {
case STATE_GARBAGE:
if (c == SLIP_END) {
slip_state = STATE_OK;
}
SYS_LOG_DBG("garbage: discard byte %x", c);
return 0;
case STATE_ESC:
if (c == SLIP_ESC_END) {
c = SLIP_END;
} else if (c == SLIP_ESC_ESC) {
c = SLIP_ESC;
} else {
slip_state = STATE_GARBAGE;
return 0;
}
slip_state = STATE_OK;
break;
case STATE_OK:
if (c == SLIP_ESC) {
slip_state = STATE_ESC;
return 0;
} else if (c == SLIP_END) {
return 1;
}
break;
}
#ifdef VERBOSE_DEBUG
SYS_LOG_DBG("processed: state %u byte %x", slip_state, c);
#endif
if (!pkt_curr) {
pkt_curr = net_pkt_get_reserve_rx(0, K_NO_WAIT);
if (!pkt_curr) {
SYS_LOG_ERR("No more buffers");
return 0;
}
buf = net_pkt_get_frag(pkt_curr, K_NO_WAIT);
if (!buf) {
SYS_LOG_ERR("No more buffers");
net_pkt_unref(pkt_curr);
return 0;
}
net_pkt_frag_insert(pkt_curr, buf);
} else {
buf = net_buf_frag_last(pkt_curr->frags);
}
if (!net_buf_tailroom(buf)) {
SYS_LOG_ERR("No more buf space: buf %p len %u", buf, buf->len);
net_pkt_unref(pkt_curr);
pkt_curr = NULL;
return 0;
}
net_buf_add_u8(buf, c);
return 0;
}
static void test_pkt_read_append(void)
{
int remaining = strlen(sample_data);
u8_t verify_rw_short[sizeof(test_rw_short)];
u8_t verify_rw_long[sizeof(test_rw_long)];
struct net_pkt *pkt;
struct net_buf *frag;
struct net_buf *tfrag;
struct ipv6_hdr *ipv6;
struct udp_hdr *udp;
u8_t data[10];
int pos = 0;
int bytes;
u16_t off;
u16_t tpos;
u16_t fail_pos;
/* Example of multi fragment read, append and skip APS's */
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
frag = net_pkt_get_reserve_rx_data(LL_RESERVE, K_FOREVER);
/* Place the IP + UDP header in the first fragment */
if (!net_buf_tailroom(frag)) {
ipv6 = (struct ipv6_hdr *)(frag->data);
udp = (struct udp_hdr *)((void *)ipv6 + sizeof(*ipv6));
if (net_buf_tailroom(frag) < sizeof(ipv6)) {
printk("Not enough space for IPv6 header, "
"needed %zd bytes, has %zd bytes\n",
sizeof(ipv6), net_buf_tailroom(frag));
zassert_true(false, "No space for IPv6 header");
}
net_buf_add(frag, sizeof(ipv6));
if (net_buf_tailroom(frag) < sizeof(udp)) {
printk("Not enough space for UDP header, "
"needed %zd bytes, has %zd bytes\n",
sizeof(udp), net_buf_tailroom(frag));
zassert_true(false, "No space for UDP header");
}
net_pkt_set_appdata(pkt, (void *)udp + sizeof(*udp));
net_pkt_set_appdatalen(pkt, 0);
}
net_pkt_frag_add(pkt, frag);
/* Put some data to rest of the fragments */
frag = net_pkt_get_reserve_rx_data(LL_RESERVE, K_FOREVER);
if (net_buf_tailroom(frag) -
(CONFIG_NET_BUF_DATA_SIZE - LL_RESERVE)) {
printk("Invalid number of bytes available in the buf, "
"should be 0 but was %zd - %d\n",
net_buf_tailroom(frag),
CONFIG_NET_BUF_DATA_SIZE - LL_RESERVE);
zassert_true(false, "Invalid number of bytes avail");
}
if (((int)net_buf_tailroom(frag) - remaining) > 0) {
printk("We should have been out of space now, "
"tailroom %zd user data len %zd\n",
net_buf_tailroom(frag),
strlen(sample_data));
zassert_true(false, "Not out of space");
}
while (remaining > 0) {
int copy;
bytes = net_buf_tailroom(frag);
copy = remaining > bytes ? bytes : remaining;
memcpy(net_buf_add(frag, copy), &sample_data[pos], copy);
DBG("Remaining %d left %d copy %d\n", remaining, bytes, copy);
pos += bytes;
remaining -= bytes;
if (net_buf_tailroom(frag) - (bytes - copy)) {
printk("There should have not been any tailroom left, "
"tailroom %zd\n",
net_buf_tailroom(frag) - (bytes - copy));
zassert_true(false, "Still tailroom left");
}
net_pkt_frag_add(pkt, frag);
if (remaining > 0) {
frag = net_pkt_get_reserve_rx_data(LL_RESERVE,
K_FOREVER);
}
}
bytes = net_pkt_get_len(pkt);
if (bytes != strlen(sample_data)) {
printk("Invalid number of bytes in message, %zd vs %d\n",
strlen(sample_data), bytes);
zassert_true(false, "Message size wrong");
}
/* Failure cases */
/* Invalid buffer */
tfrag = net_frag_skip(NULL, 10, &fail_pos, 10);
zassert_true(!tfrag && fail_pos == 0xffff, "Invalid case NULL buffer");
/* Invalid: Skip more than a buffer length.*/
tfrag = net_buf_frag_last(pkt->frags);
tfrag = net_frag_skip(tfrag, tfrag->len - 1, &fail_pos, tfrag->len + 2);
if (!(!tfrag && fail_pos == 0xffff)) {
printk("Invalid case offset %d length to skip %d,"
"frag length %d\n",
tfrag->len - 1, tfrag->len + 2, tfrag->len);
zassert_true(false, "Invalid offset");
}
/* Invalid offset */
tfrag = net_buf_frag_last(pkt->frags);
tfrag = net_frag_skip(tfrag, tfrag->len + 10, &fail_pos, 10);
if (!(!tfrag && fail_pos == 0xffff)) {
printk("Invalid case offset %d length to skip %d,"
"frag length %d\n",
tfrag->len + 10, 10, tfrag->len);
zassert_true(false, "Invalid offset");
}
/* Valid cases */
/* Offset is more than single fragment length */
/* Get the first data fragment */
tfrag = pkt->frags;
tfrag = tfrag->frags;
off = tfrag->len;
tfrag = net_frag_read(tfrag, off + 10, &tpos, 10, data);
if (!tfrag ||
memcmp(sample_data + off + 10, data, 10)) {
printk("Failed to read from offset %d, frag length %d "
"read length %d\n",
tfrag->len + 10, tfrag->len, 10);
zassert_true(false, "Fail offset read");
}
/* Skip till end of all fragments */
/* Get the first data fragment */
tfrag = pkt->frags;
tfrag = tfrag->frags;
tfrag = net_frag_skip(tfrag, 0, &tpos, strlen(sample_data));
zassert_true(!tfrag && tpos == 0,
"Invalid skip till end of all fragments");
/* Short data test case */
/* Test case scenario:
* 1) Cache the current fragment and offset
* 2) Append short data
* 3) Append short data again
* 4) Skip first short data from cached frag or offset
* 5) Read short data and compare
*/
tfrag = net_buf_frag_last(pkt->frags);
off = tfrag->len;
zassert_true(net_pkt_append_all(pkt, (u16_t)sizeof(test_rw_short),
test_rw_short, K_FOREVER),
"net_pkt_append failed");
zassert_true(net_pkt_append_all(pkt, (u16_t)sizeof(test_rw_short),
test_rw_short, K_FOREVER),
"net_pkt_append failed");
tfrag = net_frag_skip(tfrag, off, &tpos,
(u16_t)sizeof(test_rw_short));
zassert_not_null(tfrag, "net_frag_skip failed");
tfrag = net_frag_read(tfrag, tpos, &tpos,
(u16_t)sizeof(test_rw_short),
verify_rw_short);
zassert_true(!tfrag && tpos == 0, "net_frag_read failed");
zassert_false(memcmp(test_rw_short, verify_rw_short,
sizeof(test_rw_short)),
"net_frag_read failed with mismatch data");
/* Long data test case */
/* Test case scenario:
* 1) Cache the current fragment and offset
* 2) Append long data
* 3) Append long data again
* 4) Skip first long data from cached frag or offset
* 5) Read long data and compare
*/
tfrag = net_buf_frag_last(pkt->frags);
off = tfrag->len;
zassert_true(net_pkt_append_all(pkt, (u16_t)sizeof(test_rw_long),
test_rw_long, K_FOREVER),
"net_pkt_append failed");
zassert_true(net_pkt_append_all(pkt, (u16_t)sizeof(test_rw_long),
test_rw_long, K_FOREVER),
"net_pkt_append failed");
tfrag = net_frag_skip(tfrag, off, &tpos,
(u16_t)sizeof(test_rw_long));
zassert_not_null(tfrag, "net_frag_skip failed");
tfrag = net_frag_read(tfrag, tpos, &tpos,
(u16_t)sizeof(test_rw_long),
verify_rw_long);
zassert_true(!tfrag && tpos == 0, "net_frag_read failed");
zassert_false(memcmp(test_rw_long, verify_rw_long,
sizeof(test_rw_long)),
"net_frag_read failed with mismatch data");
net_pkt_unref(pkt);
DBG("test_pkt_read_append passed\n");
}
