/* Allocate and send data to USB Host */
static void send_data(u8_t *cfg, u8_t *data, size_t len)
{
struct net_pkt *pkt;
struct net_buf *buf;
pkt = net_pkt_get_reserve_rx(0, K_NO_WAIT);
if (!pkt) {
SYS_LOG_DBG("No pkt available");
return;
}
buf = net_pkt_get_frag(pkt, K_NO_WAIT);
if (!buf) {
SYS_LOG_DBG("No fragment available");
net_pkt_unref(pkt);
return;
}
net_pkt_frag_insert(pkt, buf);
SYS_LOG_DBG("queue pkt %p buf %p len %u", pkt, buf, len);
/* Add configuration id */
memcpy(net_buf_add(buf, 2), cfg, 2);
memcpy(net_buf_add(buf, len), data, len);
/* simulate LQI */
net_buf_add(buf, 1);
/* simulate FCS */
net_buf_add(buf, 2);
k_fifo_put(&tx_queue, pkt);
}
static void ipsp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
{
struct bt_context *ctxt = CHAN_CTXT(chan);
struct net_pkt *pkt;
NET_DBG("Incoming data channel %p len %zu", chan,
net_buf_frags_len(buf));
/* Get packet for bearer / protocol related data */
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
/* Set destination address */
net_pkt_ll_dst(pkt)->addr = ctxt->src.val;
net_pkt_ll_dst(pkt)->len = sizeof(ctxt->src);
net_pkt_ll_dst(pkt)->type = NET_LINK_BLUETOOTH;
/* Set source address */
net_pkt_ll_src(pkt)->addr = ctxt->dst.val;
net_pkt_ll_src(pkt)->len = sizeof(ctxt->dst);
net_pkt_ll_src(pkt)->type = NET_LINK_BLUETOOTH;
/* Add data buffer as fragment of RX buffer, take a reference while
* doing so since L2CAP will unref the buffer after return.
*/
net_pkt_frag_add(pkt, net_buf_ref(buf));
if (net_recv_data(ctxt->iface, pkt) < 0) {
NET_DBG("Packet dropped by NET stack");
net_pkt_unref(pkt);
}
}
static inline struct net_pkt *prepare_arp_request(struct net_if *iface,
struct net_pkt *req,
struct net_eth_addr *addr)
{
struct net_pkt *pkt;
struct net_buf *frag;
struct net_arp_hdr *hdr, *req_hdr;
struct net_eth_hdr *eth, *eth_req;
pkt = net_pkt_get_reserve_rx(sizeof(struct net_eth_hdr),
K_FOREVER);
if (!pkt) {
goto fail;
}
frag = net_pkt_get_frag(pkt, K_FOREVER);
if (!frag) {
goto fail;
}
net_pkt_frag_add(pkt, frag);
net_pkt_set_iface(pkt, iface);
hdr = NET_ARP_HDR(pkt);
eth = NET_ETH_HDR(pkt);
req_hdr = NET_ARP_HDR(req);
eth_req = NET_ETH_HDR(req);
eth->type = htons(NET_ETH_PTYPE_ARP);
memset(ð->dst.addr, 0xff, sizeof(struct net_eth_addr));
memcpy(ð->src.addr, addr, sizeof(struct net_eth_addr));
hdr->hwtype = htons(NET_ARP_HTYPE_ETH);
hdr->protocol = htons(NET_ETH_PTYPE_IP);
hdr->hwlen = sizeof(struct net_eth_addr);
hdr->protolen = sizeof(struct in_addr);
hdr->opcode = htons(NET_ARP_REQUEST);
memset(&hdr->dst_hwaddr.addr, 0x00, sizeof(struct net_eth_addr));
memcpy(&hdr->src_hwaddr.addr, addr, sizeof(struct net_eth_addr));
net_ipaddr_copy(&hdr->src_ipaddr, &req_hdr->src_ipaddr);
net_ipaddr_copy(&hdr->dst_ipaddr, &req_hdr->dst_ipaddr);
net_buf_add(frag, sizeof(struct net_arp_hdr));
return pkt;
fail:
net_pkt_unref(pkt);
return NULL;
}
static struct net_pkt *frame_get(struct gmac_queue *queue)
{
struct gmac_desc_list *rx_desc_list = &queue->rx_desc_list;
struct gmac_desc *rx_desc;
struct ring_buf *rx_frag_list = &queue->rx_frag_list;
struct net_pkt *rx_frame;
bool frame_is_complete;
struct net_buf *frag;
struct net_buf *new_frag;
struct net_buf *last_frag = NULL;
u8_t *frag_data;
u32_t frag_len;
u32_t frame_len = 0;
u16_t tail;
/* Check if there exists a complete frame in RX descriptor list */
tail = rx_desc_list->tail;
rx_desc = &rx_desc_list->buf[tail];
frame_is_complete = false;
while ((rx_desc->w0 & GMAC_RXW0_OWNERSHIP) && !frame_is_complete) {
frame_is_complete = (bool)(rx_desc->w1 & GMAC_RXW1_EOF);
MODULO_INC(tail, rx_desc_list->len);
rx_desc = &rx_desc_list->buf[tail];
}
/* Frame which is not complete can be dropped by GMAC. Do not process
* it, even partially.
*/
if (!frame_is_complete) {
return NULL;
}
rx_frame = net_pkt_get_reserve_rx(0, K_NO_WAIT);
/* Process a frame */
tail = rx_desc_list->tail;
rx_desc = &rx_desc_list->buf[tail];
frame_is_complete = false;
/* TODO: Don't assume first RX fragment will have SOF (Start of frame)
* bit set. If SOF bit is missing recover gracefully by dropping
* invalid frame.
*/
__ASSERT(rx_desc->w1 & GMAC_RXW1_SOF,
"First RX fragment is missing SOF bit");
/* TODO: We know already tail and head indexes of fragments containing
* complete frame. Loop over those indexes, don't search for them
* again.
*/
while ((rx_desc->w0 & GMAC_RXW0_OWNERSHIP) && !frame_is_complete) {
frag = (struct net_buf *)rx_frag_list->buf[tail];
frag_data = (u8_t *)(rx_desc->w0 & GMAC_RXW0_ADDR);
__ASSERT(frag->data == frag_data,
"RX descriptor and buffer list desynchronized");
frame_is_complete = (bool)(rx_desc->w1 & GMAC_RXW1_EOF);
if (frame_is_complete) {
frag_len = (rx_desc->w1 & GMAC_TXW1_LEN) - frame_len;
} else {
frag_len = CONFIG_NET_BUF_DATA_SIZE;
}
frame_len += frag_len;
/* Link frame fragments only if RX net buffer is valid */
if (rx_frame != NULL) {
/* Assure cache coherency after DMA write operation */
DCACHE_INVALIDATE(frag_data, frag_len);
/* Get a new data net buffer from the buffer pool */
new_frag = net_pkt_get_frag(rx_frame, K_NO_WAIT);
if (new_frag == NULL) {
queue->err_rx_frames_dropped++;
net_pkt_unref(rx_frame);
rx_frame = NULL;
} else {
net_buf_add(frag, frag_len);
if (!last_frag) {
net_pkt_frag_insert(rx_frame, frag);
} else {
net_buf_frag_insert(last_frag, frag);
}
last_frag = frag;
frag = new_frag;
rx_frag_list->buf[tail] = (u32_t)frag;
}
}
/* Update buffer descriptor status word */
rx_desc->w1 = 0;
/* Guarantee that status word is written before the address
* word to avoid race condition.
*/
__DMB(); /* data memory barrier */
/* Update buffer descriptor address word */
rx_desc->w0 =
((u32_t)frag->data & GMAC_RXW0_ADDR)
| (tail == rx_desc_list->len-1 ? GMAC_RXW0_WRAP : 0);
MODULO_INC(tail, rx_desc_list->len);
rx_desc = &rx_desc_list->buf[tail];
}
rx_desc_list->tail = tail;
SYS_LOG_DBG("Frame complete: rx=%p, tail=%d", rx_frame, tail);
__ASSERT_NO_MSG(frame_is_complete);
return rx_frame;
}
static void nrf5_rx_thread(void *arg1, void *arg2, void *arg3)
{
struct device *dev = (struct device *)arg1;
struct nrf5_802154_data *nrf5_radio = NRF5_802154_DATA(dev);
struct net_buf *frag = NULL;
enum net_verdict ack_result;
struct net_pkt *pkt;
u8_t pkt_len;
ARG_UNUSED(arg2);
ARG_UNUSED(arg3);
while (1) {
pkt = NULL;
SYS_LOG_DBG("Waiting for frame");
k_sem_take(&nrf5_radio->rx_wait, K_FOREVER);
SYS_LOG_DBG("Frame received");
pkt = net_pkt_get_reserve_rx(0, K_NO_WAIT);
if (!pkt) {
SYS_LOG_ERR("No pkt available");
goto out;
}
#if defined(CONFIG_IEEE802154_NRF5_RAW)
/**
* Reserve 1 byte for length
*/
net_pkt_set_ll_reserve(pkt, 1);
#endif
frag = net_pkt_get_frag(pkt, K_NO_WAIT);
if (!frag) {
SYS_LOG_ERR("No frag available");
goto out;
}
net_pkt_frag_insert(pkt, frag);
/* rx_mpdu contains length, psdu, fcs|lqi
* The last 2 bytes contain LQI or FCS, depending if
* automatic CRC handling is enabled or not, respectively.
*/
#if defined(CONFIG_IEEE802154_NRF5_RAW)
pkt_len = nrf5_radio->rx_psdu[0];
#else
pkt_len = nrf5_radio->rx_psdu[0] - NRF5_FCS_LENGTH;
#endif
/* Skip length (first byte) and copy the payload */
memcpy(frag->data, nrf5_radio->rx_psdu + 1, pkt_len);
net_buf_add(frag, pkt_len);
nrf_drv_radio802154_buffer_free(nrf5_radio->rx_psdu);
ack_result = ieee802154_radio_handle_ack(nrf5_radio->iface,
pkt);
if (ack_result == NET_OK) {
SYS_LOG_DBG("ACK packet handled");
goto out;
}
SYS_LOG_DBG("Caught a packet (%u) (LQI: %u)",
pkt_len, nrf5_radio->lqi);
if (net_recv_data(nrf5_radio->iface, pkt) < 0) {
SYS_LOG_DBG("Packet dropped by NET stack");
goto out;
}
net_analyze_stack("nRF5 rx stack",
(unsigned char *)nrf5_radio->rx_stack,
CONFIG_IEEE802154_NRF5_RX_STACK_SIZE);
continue;
out:
if (pkt) {
net_pkt_unref(pkt);
}
}
}
static bool run_tests(void)
{
struct net_pkt *pkt, *pkt2;
struct net_buf *frag;
struct net_if *iface;
struct net_if_addr *ifaddr;
struct net_arp_hdr *arp_hdr;
struct net_ipv4_hdr *ipv4;
struct net_eth_hdr *eth_hdr;
int len;
struct in_addr dst = { { { 192, 168, 0, 2 } } };
struct in_addr dst_far = { { { 10, 11, 12, 13 } } };
struct in_addr dst_far2 = { { { 172, 16, 14, 186 } } };
struct in_addr src = { { { 192, 168, 0, 1 } } };
struct in_addr netmask = { { { 255, 255, 255, 0 } } };
struct in_addr gw = { { { 192, 168, 0, 42 } } };
net_arp_init();
iface = net_if_get_default();
net_if_ipv4_set_gw(iface, &gw);
net_if_ipv4_set_netmask(iface, &netmask);
/* Unicast test */
ifaddr = net_if_ipv4_addr_add(iface,
&src,
NET_ADDR_MANUAL,
0);
ifaddr->addr_state = NET_ADDR_PREFERRED;
/* Application data for testing */
pkt = net_pkt_get_reserve_tx(sizeof(struct net_eth_hdr), K_FOREVER);
if (!pkt) {
printk("Out of mem TX\n");
return false;
}
frag = net_pkt_get_frag(pkt, K_FOREVER);
if (!frag) {
printk("Out of mem DATA\n");
return false;
}
net_pkt_frag_add(pkt, frag);
net_pkt_set_iface(pkt, iface);
setup_eth_header(iface, pkt, &hwaddr, NET_ETH_PTYPE_IP);
len = strlen(app_data);
if (net_pkt_ll_reserve(pkt) != sizeof(struct net_eth_hdr)) {
printk("LL reserve invalid, should be %zd was %d\n",
sizeof(struct net_eth_hdr),
net_pkt_ll_reserve(pkt));
return false;
}
ipv4 = (struct net_ipv4_hdr *)net_buf_add(frag,
sizeof(struct net_ipv4_hdr));
net_ipaddr_copy(&ipv4->src, &src);
net_ipaddr_copy(&ipv4->dst, &dst);
memcpy(net_buf_add(frag, len), app_data, len);
pkt2 = net_arp_prepare(pkt);
/* pkt2 is the ARP packet and pkt is the IPv4 packet and it was
* stored in ARP table.
*/
if (pkt2 == pkt) {
/* The packets cannot be the same as the ARP cache has
* still room for the pkt.
*/
printk("ARP cache should still have free space\n");
return false;
}
if (!pkt2) {
printk("ARP pkt is empty\n");
return false;
}
/* The ARP cache should now have a link to pending net_pkt
* that is to be sent after we have got an ARP reply.
*/
if (!pkt->frags) {
printk("Pending pkt fragment is NULL\n");
return false;
}
pending_pkt = pkt;
/* pkt2 should contain the arp header, verify it */
if (memcmp(net_pkt_ll(pkt2), net_eth_broadcast_addr(),
sizeof(struct net_eth_addr))) {
printk("ARP ETH dest address invalid\n");
net_hexdump("ETH dest wrong ", net_pkt_ll(pkt2),
sizeof(struct net_eth_addr));
net_hexdump("ETH dest correct",
(u8_t *)net_eth_broadcast_addr(),
sizeof(struct net_eth_addr));
return false;
}
if (memcmp(net_pkt_ll(pkt2) + sizeof(struct net_eth_addr),
iface->link_addr.addr,
sizeof(struct net_eth_addr))) {
printk("ARP ETH source address invalid\n");
net_hexdump("ETH src correct",
iface->link_addr.addr,
sizeof(struct net_eth_addr));
net_hexdump("ETH src wrong ",
net_pkt_ll(pkt2) + sizeof(struct net_eth_addr),
sizeof(struct net_eth_addr));
return false;
}
arp_hdr = NET_ARP_HDR(pkt2);
eth_hdr = NET_ETH_HDR(pkt2);
if (eth_hdr->type != htons(NET_ETH_PTYPE_ARP)) {
printk("ETH type 0x%x, should be 0x%x\n",
eth_hdr->type, htons(NET_ETH_PTYPE_ARP));
return false;
}
if (arp_hdr->hwtype != htons(NET_ARP_HTYPE_ETH)) {
printk("ARP hwtype 0x%x, should be 0x%x\n",
arp_hdr->hwtype, htons(NET_ARP_HTYPE_ETH));
return false;
}
if (arp_hdr->protocol != htons(NET_ETH_PTYPE_IP)) {
printk("ARP protocol 0x%x, should be 0x%x\n",
arp_hdr->protocol, htons(NET_ETH_PTYPE_IP));
return false;
}
if (arp_hdr->hwlen != sizeof(struct net_eth_addr)) {
printk("ARP hwlen 0x%x, should be 0x%zx\n",
arp_hdr->hwlen, sizeof(struct net_eth_addr));
return false;
}
if (arp_hdr->protolen != sizeof(struct in_addr)) {
printk("ARP IP addr len 0x%x, should be 0x%zx\n",
arp_hdr->protolen, sizeof(struct in_addr));
return false;
}
if (arp_hdr->opcode != htons(NET_ARP_REQUEST)) {
printk("ARP opcode 0x%x, should be 0x%x\n",
arp_hdr->opcode, htons(NET_ARP_REQUEST));
return false;
}
if (!net_ipv4_addr_cmp(&arp_hdr->dst_ipaddr,
&NET_IPV4_HDR(pkt)->dst)) {
char out[sizeof("xxx.xxx.xxx.xxx")];
snprintk(out, sizeof(out), "%s",
net_sprint_ipv4_addr(&arp_hdr->dst_ipaddr));
printk("ARP IP dest invalid %s, should be %s", out,
net_sprint_ipv4_addr(&NET_IPV4_HDR(pkt)->dst));
return false;
}
if (!net_ipv4_addr_cmp(&arp_hdr->src_ipaddr,
&NET_IPV4_HDR(pkt)->src)) {
char out[sizeof("xxx.xxx.xxx.xxx")];
snprintk(out, sizeof(out), "%s",
net_sprint_ipv4_addr(&arp_hdr->src_ipaddr));
printk("ARP IP src invalid %s, should be %s", out,
net_sprint_ipv4_addr(&NET_IPV4_HDR(pkt)->src));
return false;
}
/* We could have send the new ARP request but for this test we
* just free it.
*/
net_pkt_unref(pkt2);
if (pkt->ref != 2) {
printk("ARP cache should own the original packet\n");
return false;
}
/* Then a case where target is not in the same subnet */
net_ipaddr_copy(&ipv4->dst, &dst_far);
pkt2 = net_arp_prepare(pkt);
if (pkt2 == pkt) {
printk("ARP cache should not find anything\n");
return false;
}
if (!pkt2) {
printk("ARP pkt2 is empty\n");
return false;
}
arp_hdr = NET_ARP_HDR(pkt2);
if (!net_ipv4_addr_cmp(&arp_hdr->dst_ipaddr, &iface->ipv4.gw)) {
char out[sizeof("xxx.xxx.xxx.xxx")];
snprintk(out, sizeof(out), "%s",
net_sprint_ipv4_addr(&arp_hdr->dst_ipaddr));
printk("ARP IP dst invalid %s, should be %s\n", out,
net_sprint_ipv4_addr(&iface->ipv4.gw));
return false;
}
net_pkt_unref(pkt2);
/* Try to find the same destination again, this should fail as there
* is a pending request in ARP cache.
*/
net_ipaddr_copy(&ipv4->dst, &dst_far);
/* Make sure prepare will not free the pkt because it will be
* needed in the later test case.
*/
net_pkt_ref(pkt);
pkt2 = net_arp_prepare(pkt);
if (!pkt2) {
printk("ARP cache is not sending the request again\n");
return false;
}
net_pkt_unref(pkt2);
/* Try to find the different destination, this should fail too
* as the cache table should be full.
*/
net_ipaddr_copy(&ipv4->dst, &dst_far2);
/* Make sure prepare will not free the pkt because it will be
* needed in the next test case.
*/
net_pkt_ref(pkt);
pkt2 = net_arp_prepare(pkt);
if (!pkt2) {
printk("ARP cache did not send a req\n");
return false;
}
/* Restore the original address so that following test case can
* work properly.
*/
net_ipaddr_copy(&ipv4->dst, &dst);
/* The arp request packet is now verified, create an arp reply.
* The previous value of pkt is stored in arp table and is not lost.
*/
pkt = net_pkt_get_reserve_rx(sizeof(struct net_eth_hdr), K_FOREVER);
if (!pkt) {
printk("Out of mem RX reply\n");
return false;
}
printk("%d pkt %p\n", __LINE__, pkt);
frag = net_pkt_get_frag(pkt, K_FOREVER);
if (!frag) {
printk("Out of mem DATA reply\n");
return false;
}
printk("%d frag %p\n", __LINE__, frag);
net_pkt_frag_add(pkt, frag);
net_pkt_set_iface(pkt, iface);
arp_hdr = NET_ARP_HDR(pkt);
net_buf_add(frag, sizeof(struct net_arp_hdr));
net_ipaddr_copy(&arp_hdr->dst_ipaddr, &dst);
net_ipaddr_copy(&arp_hdr->src_ipaddr, &src);
pkt2 = prepare_arp_reply(iface, pkt, &hwaddr);
if (!pkt2) {
printk("ARP reply generation failed.");
return false;
}
/* The pending packet should now be sent */
switch (net_arp_input(pkt2)) {
case NET_OK:
case NET_CONTINUE:
break;
case NET_DROP:
break;
}
/* Yielding so that network interface TX thread can proceed. */
k_yield();
if (send_status < 0) {
printk("ARP reply was not sent\n");
return false;
}
if (pkt->ref != 1) {
printk("ARP cache should no longer own the original packet\n");
return false;
}
net_pkt_unref(pkt);
/* Then feed in ARP request */
pkt = net_pkt_get_reserve_rx(sizeof(struct net_eth_hdr), K_FOREVER);
if (!pkt) {
printk("Out of mem RX request\n");
return false;
}
frag = net_pkt_get_frag(pkt, K_FOREVER);
if (!frag) {
printk("Out of mem DATA request\n");
return false;
}
net_pkt_frag_add(pkt, frag);
net_pkt_set_iface(pkt, iface);
send_status = -EINVAL;
arp_hdr = NET_ARP_HDR(pkt);
net_buf_add(frag, sizeof(struct net_arp_hdr));
net_ipaddr_copy(&arp_hdr->dst_ipaddr, &src);
net_ipaddr_copy(&arp_hdr->src_ipaddr, &dst);
setup_eth_header(iface, pkt, &hwaddr, NET_ETH_PTYPE_ARP);
pkt2 = prepare_arp_request(iface, pkt, &hwaddr);
if (!pkt2) {
printk("ARP request generation failed.");
return false;
}
req_test = true;
switch (net_arp_input(pkt2)) {
case NET_OK:
case NET_CONTINUE:
break;
case NET_DROP:
break;
}
/* Yielding so that network interface TX thread can proceed. */
k_yield();
if (send_status < 0) {
printk("ARP req was not sent\n");
return false;
}
net_pkt_unref(pkt);
printk("Network ARP checks passed\n");
return true;
}
static inline int slip_input_byte(struct slip_context *slip,
unsigned char c)
{
switch (slip->state) {
case STATE_GARBAGE:
if (c == SLIP_END) {
slip->state = STATE_OK;
}
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;
SLIP_STATS(slip->garbage++);
return 0;
}
slip->state = STATE_OK;
break;
case STATE_OK:
if (c == SLIP_ESC) {
slip->state = STATE_ESC;
return 0;
}
if (c == SLIP_END) {
slip->state = STATE_OK;
slip->first = false;
if (slip->rx) {
return 1;
}
return 0;
}
if (slip->first && !slip->rx) {
/* Must have missed buffer allocation on first byte. */
return 0;
}
if (!slip->first) {
slip->first = true;
slip->rx = net_pkt_get_reserve_rx(0, K_NO_WAIT);
if (!slip->rx) {
SYS_LOG_ERR("[%p] cannot allocate pkt",
slip);
return 0;
}
slip->last = net_pkt_get_frag(slip->rx, K_NO_WAIT);
if (!slip->last) {
SYS_LOG_ERR("[%p] cannot allocate 1st data frag",
slip);
net_pkt_unref(slip->rx);
slip->rx = NULL;
return 0;
}
net_pkt_frag_add(slip->rx, slip->last);
slip->ptr = net_pkt_ip_data(slip->rx);
}
break;
}
/* It is possible that slip->last is not set during the startup
* of the device. If this happens do not continue and overwrite
* some random memory.
*/
if (!slip->last) {
return 0;
}
if (!net_buf_tailroom(slip->last)) {
/* We need to allocate a new fragment */
struct net_buf *frag;
frag = net_pkt_get_reserve_rx_data(0, K_NO_WAIT);
if (!frag) {
SYS_LOG_ERR("[%p] cannot allocate next data frag",
slip);
net_pkt_unref(slip->rx);
slip->rx = NULL;
slip->last = NULL;
return 0;
}
net_buf_frag_insert(slip->last, frag);
slip->last = frag;
slip->ptr = slip->last->data;
}
/* The net_buf_add_u8() cannot add data to ll header so we need
* a way to do it.
*/
if (slip->ptr < slip->last->data) {
*slip->ptr = c;
} else {
slip->ptr = net_buf_add_u8(slip->last, c);
}
slip->ptr++;
return 0;
}
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 u8_t *upipe_rx(u8_t *buf, size_t *off)
{
struct net_pkt *pkt = NULL;
struct upipe_context *upipe;
if (!upipe_dev) {
goto done;
}
upipe = upipe_dev->driver_data;
if (!upipe->rx && *buf == UART_PIPE_RADIO_15_4_FRAME_TYPE) {
upipe->rx = true;
goto done;
}
if (!upipe->rx_len) {
if (*buf > 127) {
goto flush;
}
upipe->rx_len = *buf;
goto done;
}
upipe->rx_buf[upipe->rx_off++] = *buf;
if (upipe->rx_len == upipe->rx_off) {
struct net_buf *frag;
pkt = net_pkt_get_reserve_rx(K_NO_WAIT);
if (!pkt) {
LOG_DBG("No pkt available");
goto flush;
}
frag = net_pkt_get_frag(pkt, K_NO_WAIT);
if (!frag) {
LOG_DBG("No fragment available");
goto out;
}
net_pkt_frag_insert(pkt, frag);
memcpy(frag->data, upipe->rx_buf, upipe->rx_len);
net_buf_add(frag, upipe->rx_len);
#if defined(CONFIG_IEEE802154_UPIPE_HW_FILTER)
if (received_dest_addr_matched(frag->data) == false) {
LOG_DBG("Packet received is not addressed to me");
goto out;
}
#endif
if (ieee802154_radio_handle_ack(upipe->iface, pkt) == NET_OK) {
LOG_DBG("ACK packet handled");
goto out;
}
LOG_DBG("Caught a packet (%u)", upipe->rx_len);
if (net_recv_data(upipe->iface, pkt) < 0) {
LOG_DBG("Packet dropped by NET stack");
goto out;
}
goto flush;
out:
net_pkt_unref(pkt);
flush:
upipe->rx = false;
upipe->rx_len = 0U;
upipe->rx_off = 0U;
}
done:
*off = 0;
return buf;
}
static void test_fragment_split(void)
{
#define TEST_FRAG_COUNT (FRAG_COUNT - 2)
#define FRAGA (FRAG_COUNT - 2)
#define FRAGB (FRAG_COUNT - 1)
struct net_pkt *pkt;
struct net_buf *frags[FRAG_COUNT], *frag, *frag_a, *frag_b;
int i, total, split_a, split_b;
int ret, frag_size;
memset(frags, 0, FRAG_COUNT * sizeof(void *));
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
frag = NULL;
for (i = 0, total = 0; i < TEST_FRAG_COUNT; i++) {
frags[i] = net_pkt_get_reserve_rx_data(12, K_FOREVER);
if (frag) {
net_buf_frag_add(frag, frags[i]);
}
frag = frags[i];
/* Copy some test data in front of the fragment */
memcpy(net_buf_add(frags[i], sizeof(frag_data)),
frag_data, sizeof(frag_data));
total++;
}
if (total != TEST_FRAG_COUNT) {
printk("There should be %d fragments but was %d\n",
TEST_FRAG_COUNT, total);
zassert_true(false, "Frags missing");
}
frag_size = frags[0]->size;
zassert_true(frag_size > 0, "Invalid frag size");
net_pkt_frag_add(pkt, frags[0]);
frag_a = frags[FRAGA];
frag_b = frags[FRAGB];
zassert_is_null(frag_a, "frag_a is not NULL");
zassert_is_null(frag_b, "frag_b is not NULL");
split_a = frag_size * 2 / 3;
split_b = frag_size - split_a;
zassert_true(split_a > 0, "A size is 0");
zassert_true(split_a > split_b, "A is smaller than B");
/* Test some error cases first */
ret = net_pkt_split(NULL, NULL, 1024, &frag_a, &frag_b, K_NO_WAIT);
zassert_equal(ret, -EINVAL, "Invalid buf pointers");
ret = net_pkt_split(pkt, pkt->frags, CONFIG_NET_BUF_DATA_SIZE + 1,
&frag_a, &frag_b, K_NO_WAIT);
zassert_equal(ret, 0, "Split failed");
ret = net_pkt_split(pkt, pkt->frags, split_a,
&frag_a, &frag_b, K_NO_WAIT);
zassert_equal(ret, 0, "Cannot split frag");
if (frag_a->len != split_a) {
printk("Frag_a len %d not %d\n", frag_a->len, split_a);
zassert_equal(frag_a->len, split_a, "Frag_a len wrong");
}
if (frag_b->len != split_b) {
printk("Frag_b len %d not %d\n", frag_b->len, split_b);
zassert_true(false, "Frag_b len wrong");
}
zassert_false(memcmp(pkt->frags->data, frag_a->data, split_a),
"Frag_a data mismatch");
zassert_false(memcmp(pkt->frags->data + split_a, frag_b->data, split_b),
"Frag_b data mismatch");
}
static void test_fragment_compact(void)
{
struct net_pkt *pkt;
struct net_buf *frags[FRAG_COUNT], *frag;
int i, bytes, total, count;
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
frag = NULL;
for (i = 0, total = 0; i < FRAG_COUNT; i++) {
frags[i] = net_pkt_get_reserve_rx_data(12, K_FOREVER);
if (frag) {
net_buf_frag_add(frag, frags[i]);
}
frag = frags[i];
/* Copy character test data in front of the fragment */
memcpy(net_buf_add(frags[i], sizeof(test_data)),
test_data, sizeof(test_data));
/* Followed by bytes of zeroes */
memset(net_buf_add(frags[i], sizeof(test_data)), 0,
sizeof(test_data));
total++;
}
if (total != FRAG_COUNT) {
printk("There should be %d fragments but was %d\n",
FRAG_COUNT, total);
zassert_true(false, "Invalid fragment count");
}
DBG("step 1\n");
pkt->frags = net_buf_frag_add(pkt->frags, frags[0]);
bytes = net_pkt_get_len(pkt);
if (bytes != FRAG_COUNT * sizeof(test_data) * 2) {
printk("Compact test failed, fragments had %d bytes but "
"should have had %zd\n", bytes,
FRAG_COUNT * sizeof(test_data) * 2);
zassert_true(false, "Invalid fragment bytes");
}
zassert_false(net_pkt_is_compact(pkt),
"The pkt is definitely not compact");
DBG("step 2\n");
net_pkt_compact(pkt);
zassert_true(net_pkt_is_compact(pkt),
"The pkt should be in compact form");
DBG("step 3\n");
/* Try compacting again, nothing should happen */
net_pkt_compact(pkt);
zassert_true(net_pkt_is_compact(pkt),
"The pkt should be compacted now");
total = calc_fragments(pkt);
/* Add empty fragment at the end and compact, the last fragment
* should be removed.
*/
frag = net_pkt_get_reserve_rx_data(0, K_FOREVER);
net_pkt_frag_add(pkt, frag);
count = calc_fragments(pkt);
DBG("step 4\n");
net_pkt_compact(pkt);
i = calc_fragments(pkt);
if (count != (i + 1)) {
printk("Last fragment removal failed, chain should have %d "
"fragments but had %d\n", i-1, i);
zassert_true(false, "Last frag rm fails");
}
if (i != total) {
printk("Fragments missing, expecting %d but got %d\n",
total, i);
zassert_true(false, "Frags missing");
}
/* Add two empty fragments at the end and compact, the last two
* fragment should be removed.
*/
frag = net_pkt_get_reserve_rx_data(0, K_FOREVER);
net_pkt_frag_add(pkt, frag);
frag = net_pkt_get_reserve_rx_data(0, K_FOREVER);
net_pkt_frag_add(pkt, frag);
count = calc_fragments(pkt);
DBG("step 5\n");
net_pkt_compact(pkt);
i = calc_fragments(pkt);
if (count != (i + 2)) {
printk("Last two fragment removal failed, chain should have "
"%d fragments but had %d\n", i-2, i);
zassert_true(false, "Last two frag rm fails");
}
if (i != total) {
printk("Fragments missing, expecting %d but got %d\n",
total, i);
zassert_true(false, "Frags missing");
}
/* Add empty fragment at the beginning and at the end, and then
* compact, the two fragment should be removed.
*/
frag = net_pkt_get_reserve_rx_data(0, K_FOREVER);
net_pkt_frag_insert(pkt, frag);
frag = net_pkt_get_reserve_rx_data(0, K_FOREVER);
net_pkt_frag_add(pkt, frag);
count = calc_fragments(pkt);
DBG("step 6\n");
net_pkt_compact(pkt);
i = calc_fragments(pkt);
if (count != (i + 2)) {
printk("Two fragment removal failed, chain should have "
"%d fragments but had %d\n", i-2, i);
zassert_true(false, "Two frag rm fails");
}
if (i != total) {
printk("Fragments missing, expecting %d but got %d\n",
total, i);
zassert_true(false, "Frags missing");
}
DBG("test_fragment_compact passed\n");
}
static void test_ipv6_multi_frags(void)
{
struct net_pkt *pkt;
struct net_buf *frag;
struct ipv6_hdr *ipv6;
struct udp_hdr *udp;
int bytes, remaining = strlen(example_data), pos = 0;
/* Example of multi fragment scenario with IPv6 */
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 byte count");
}
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(example_data));
zassert_true(false, "Still space");
}
while (remaining > 0) {
int copy;
bytes = net_buf_tailroom(frag);
copy = remaining > bytes ? bytes : remaining;
memcpy(net_buf_add(frag, copy), &example_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, "There is 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(example_data)) {
printk("Invalid number of bytes in message, %zd vs %d\n",
strlen(example_data), bytes);
zassert_true(false, "Invalid number of bytes");
}
/* Normally one should not unref the fragment list like this
* because it will leave the pkt->frags pointing to already
* freed fragment.
*/
net_pkt_frag_unref(pkt->frags);
zassert_not_null(pkt->frags, "Frag list empty");
pkt->frags = NULL; /* to prevent double free */
net_pkt_unref(pkt);
}
static void test_pkt_read_write_insert(void)
{
struct net_buf *read_frag;
struct net_buf *temp_frag;
struct net_pkt *pkt;
struct net_buf *frag;
u8_t read_data[100];
u16_t read_pos;
u16_t len;
u16_t pos;
/* Example of multi fragment read, append and skip APS's */
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
net_pkt_set_ll_reserve(pkt, LL_RESERVE);
frag = net_pkt_get_reserve_rx_data(net_pkt_ll_reserve(pkt),
K_FOREVER);
net_pkt_frag_add(pkt, frag);
/* 1) Offset is with in input fragment.
* Write app data after IPv6 and UDP header. (If the offset is after
* IPv6 + UDP header size, api will create empty space till offset
* and write data).
*/
frag = net_pkt_write(pkt, frag, NET_IPV6UDPH_LEN, &pos, 10,
(u8_t *)sample_data, K_FOREVER);
zassert_false(!frag || pos != 58, "Usecase 1: Write failed");
read_frag = net_frag_read(frag, NET_IPV6UDPH_LEN, &read_pos, 10,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 1: Read failed");
zassert_false(memcmp(read_data, sample_data, 10),
"Usecase 1: Read data mismatch");
/* 2) Write IPv6 and UDP header at offset 0. (Empty space is created
* already in Usecase 1, just need to fill the header, at this point
* there shouldn't be any length change).
*/
frag = net_pkt_write(pkt, frag, 0, &pos, NET_IPV6UDPH_LEN,
(u8_t *)sample_data, K_FOREVER);
zassert_false(!frag || pos != 48, "Usecase 2: Write failed");
read_frag = net_frag_read(frag, 0, &read_pos, NET_IPV6UDPH_LEN,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 2: Read failed");
zassert_false(memcmp(read_data, sample_data, NET_IPV6UDPH_LEN),
"Usecase 2: Read data mismatch");
net_pkt_unref(pkt);
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
net_pkt_set_ll_reserve(pkt, LL_RESERVE);
/* 3) Offset is in next to next fragment.
* Write app data after 2 fragments. (If the offset far away, api will
* create empty fragments(space) till offset and write data).
*/
frag = net_pkt_write(pkt, pkt->frags, 200, &pos, 10,
(u8_t *)sample_data + 10, K_FOREVER);
zassert_not_null(frag, "Usecase 3: Write failed");
read_frag = net_frag_read(frag, pos - 10, &read_pos, 10,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 3: Read failed");
zassert_false(memcmp(read_data, sample_data + 10, 10),
"Usecase 3: Read data mismatch");
/* 4) Offset is in next to next fragment (overwrite).
* Write app data after 2 fragments. (Space is already available from
* Usecase 3, this scenatio doesn't create any space, it just overwrites
* the existing data.
*/
frag = net_pkt_write(pkt, pkt->frags, 190, &pos, 10,
(u8_t *)sample_data, K_FOREVER);
zassert_not_null(frag, "Usecase 4: Write failed");
read_frag = net_frag_read(frag, pos - 10, &read_pos, 20,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 4: Read failed");
zassert_false(memcmp(read_data, sample_data, 20),
"Usecase 4: Read data mismatch");
net_pkt_unref(pkt);
/* 5) Write 20 bytes in fragment which has only 10 bytes space.
* API should overwrite on first 10 bytes and create extra 10 bytes
* and write there.
*/
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
net_pkt_set_ll_reserve(pkt, LL_RESERVE);
frag = net_pkt_get_reserve_rx_data(net_pkt_ll_reserve(pkt),
K_FOREVER);
net_pkt_frag_add(pkt, frag);
/* Create 10 bytes space. */
net_buf_add(frag, 10);
frag = net_pkt_write(pkt, frag, 0, &pos, 20, (u8_t *)sample_data,
K_FOREVER);
zassert_false(!frag && pos != 20, "Usecase 5: Write failed");
read_frag = net_frag_read(frag, 0, &read_pos, 20, read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 5: Read failed");
zassert_false(memcmp(read_data, sample_data, 20),
"USecase 5: Read data mismatch");
net_pkt_unref(pkt);
/* 6) First fragment is full, second fragment has 10 bytes tail room,
* third fragment has 5 bytes.
* Write data (30 bytes) in second fragment where offset is 10 bytes
* before the tailroom.
* So it should overwrite 10 bytes and create space for another 10
* bytes and write data. Third fragment 5 bytes overwritten and space
* for 5 bytes created.
*/
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
net_pkt_set_ll_reserve(pkt, LL_RESERVE);
/* First fragment make it fully occupied. */
frag = net_pkt_get_reserve_rx_data(net_pkt_ll_reserve(pkt),
K_FOREVER);
net_pkt_frag_add(pkt, frag);
len = net_buf_tailroom(frag);
net_buf_add(frag, len);
/* 2nd fragment last 10 bytes tailroom, rest occupied */
frag = net_pkt_get_reserve_rx_data(net_pkt_ll_reserve(pkt),
K_FOREVER);
net_pkt_frag_add(pkt, frag);
len = net_buf_tailroom(frag);
net_buf_add(frag, len - 10);
read_frag = temp_frag = frag;
read_pos = frag->len - 10;
/* 3rd fragment, only 5 bytes occupied */
frag = net_pkt_get_reserve_rx_data(net_pkt_ll_reserve(pkt),
K_FOREVER);
net_pkt_frag_add(pkt, frag);
net_buf_add(frag, 5);
temp_frag = net_pkt_write(pkt, temp_frag, temp_frag->len - 10, &pos,
30, (u8_t *) sample_data, K_FOREVER);
zassert_not_null(temp_frag, "Use case 6: Write failed");
read_frag = net_frag_read(read_frag, read_pos, &read_pos, 30,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 6: Read failed");
zassert_false(memcmp(read_data, sample_data, 30),
"Usecase 6: Read data mismatch");
net_pkt_unref(pkt);
/* 7) Offset is with in input fragment.
* Write app data after IPv6 and UDP header. (If the offset is after
* IPv6 + UDP header size, api will create empty space till offset
* and write data). Insert some app data after IPv6 + UDP header
* before first set of app data.
*/
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
net_pkt_set_ll_reserve(pkt, LL_RESERVE);
/* First fragment make it fully occupied. */
frag = net_pkt_get_reserve_rx_data(net_pkt_ll_reserve(pkt),
K_FOREVER);
net_pkt_frag_add(pkt, frag);
frag = net_pkt_write(pkt, frag, NET_IPV6UDPH_LEN, &pos, 10,
(u8_t *)sample_data + 10, K_FOREVER);
zassert_false(!frag || pos != 58, "Usecase 7: Write failed");
read_frag = net_frag_read(frag, NET_IPV6UDPH_LEN, &read_pos, 10,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 7: Read failed");
zassert_false(memcmp(read_data, sample_data + 10, 10),
"Usecase 7: Read data mismatch");
zassert_true(net_pkt_insert(pkt, frag, NET_IPV6UDPH_LEN, 10,
(u8_t *)sample_data, K_FOREVER),
"Usecase 7: Insert failed");
read_frag = net_frag_read(frag, NET_IPV6UDPH_LEN, &read_pos, 20,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 7: Read after failed");
zassert_false(memcmp(read_data, sample_data, 20),
"Usecase 7: Read data mismatch after insertion");
/* Insert data outside input fragment length, error case. */
zassert_false(net_pkt_insert(pkt, frag, 70, 10, (u8_t *)sample_data,
K_FOREVER),
"Usecase 7: False insert failed");
net_pkt_unref(pkt);
/* 8) Offset is with in input fragment.
* Write app data after IPv6 and UDP header. (If the offset is after
* IPv6 + UDP header size, api will create empty space till offset
* and write data). Insert some app data after IPv6 + UDP header
* before first set of app data. Insertion data is long which will
* take two fragments.
*/
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
net_pkt_set_ll_reserve(pkt, LL_RESERVE);
/* First fragment make it fully occupied. */
frag = net_pkt_get_reserve_rx_data(net_pkt_ll_reserve(pkt),
K_FOREVER);
net_pkt_frag_add(pkt, frag);
frag = net_pkt_write(pkt, frag, NET_IPV6UDPH_LEN, &pos, 10,
(u8_t *)sample_data + 60, K_FOREVER);
zassert_false(!frag || pos != 58, "Usecase 8: Write failed");
read_frag = net_frag_read(frag, NET_IPV6UDPH_LEN, &read_pos, 10,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 8: Read failed");
zassert_false(memcmp(read_data, sample_data + 60, 10),
"Usecase 8: Read data mismatch");
zassert_true(net_pkt_insert(pkt, frag, NET_IPV6UDPH_LEN, 60,
(u8_t *)sample_data, K_FOREVER),
"Usecase 8: Insert failed");
read_frag = net_frag_read(frag, NET_IPV6UDPH_LEN, &read_pos, 70,
read_data);
zassert_false(!read_frag && read_pos == 0xffff,
"Usecase 8: Read after failed");
zassert_false(memcmp(read_data, sample_data, 70),
"Usecase 8: Read data mismatch after insertion");
net_pkt_unref(pkt);
DBG("test_pkt_read_write_insert passed\n");
}
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");
}
static void test_fragment_copy(void)
{
struct net_pkt *pkt, *new_pkt;
struct net_buf *frag, *new_frag;
struct ipv6_hdr *ipv6;
struct udp_hdr *udp;
size_t orig_len, reserve;
int pos;
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_buf_add(frag, sizeof(*udp));
memcpy(net_buf_add(frag, 15), example_data, 15);
net_pkt_set_appdata(pkt, (void *)udp + sizeof(*udp) + 15);
net_pkt_set_appdatalen(pkt, 0);
}
net_pkt_frag_add(pkt, frag);
orig_len = net_pkt_get_len(pkt);
DBG("Total copy data len %zd\n", orig_len);
linearize(pkt, buf_orig, orig_len);
/* Then copy a fragment list to a new fragment list.
* Reserve some space in front of the buffers.
*/
reserve = sizeof(struct ipv6_hdr) + sizeof(struct icmp_hdr);
new_frag = net_pkt_copy_all(pkt, reserve, K_FOREVER);
zassert_not_null(new_frag, "Cannot copy fragment list");
new_pkt = net_pkt_get_reserve_tx(0, K_FOREVER);
new_pkt->frags = net_buf_frag_add(new_pkt->frags, new_frag);
DBG("Total new data len %zd\n", net_pkt_get_len(new_pkt));
if ((net_pkt_get_len(pkt) + reserve) != net_pkt_get_len(new_pkt)) {
int diff;
diff = net_pkt_get_len(new_pkt) - reserve -
net_pkt_get_len(pkt);
printk("Fragment list missing data, %d bytes not copied "
"(%zd vs %zd)\n", diff,
net_pkt_get_len(pkt) + reserve,
net_pkt_get_len(new_pkt));
zassert_true(false, "Frag list missing");
}
if (net_pkt_get_len(new_pkt) != (orig_len + sizeof(struct ipv6_hdr) +
sizeof(struct icmp_hdr))) {
printk("Fragment list missing data, new pkt len %zd "
"should be %zd\n", net_pkt_get_len(new_pkt),
orig_len + sizeof(struct ipv6_hdr) +
sizeof(struct icmp_hdr));
zassert_true(false, "Frag list missing data");
}
linearize(new_pkt, buf_copy, sizeof(buf_copy));
zassert_true(memcmp(buf_orig, buf_copy, sizeof(buf_orig)),
"Buffer copy failed, buffers are same");
pos = memcmp(buf_orig, buf_copy + sizeof(struct ipv6_hdr) +
sizeof(struct icmp_hdr), orig_len);
if (pos) {
printk("Buffer copy failed at pos %d\n", pos);
zassert_true(false, "Buf copy failed");
}
}