static bool net_pkt_is_compact(struct net_pkt *pkt)
{
struct net_buf *frag, *last;
size_t total = 0, calc;
int count = 0;
last = NULL;
frag = pkt->frags;
while (frag) {
total += frag->len;
count++;
last = frag;
frag = frag->frags;
}
NET_ASSERT(last);
if (!last) {
return false;
}
calc = count * (last->size) - net_buf_tailroom(last) -
count * (net_buf_headroom(last));
if (total == calc) {
return true;
}
NET_DBG("Not compacted total %zu real %zu", total, calc);
return false;
}
static inline void read_header(void)
{
switch (rx.type) {
case H4_NONE:
h4_get_type();
return;
case H4_EVT:
get_evt_hdr();
break;
case H4_ACL:
get_acl_hdr();
break;
default:
CODE_UNREACHABLE;
return;
}
if (rx.have_hdr && rx.buf) {
if (rx.remaining > net_buf_tailroom(rx.buf)) {
BT_ERR("Not enough space in buffer");
rx.discard = rx.remaining;
reset_rx();
} else {
copy_hdr(rx.buf);
}
}
}
static int unprovisioned_beacon_send(void)
{
#if defined(CONFIG_BT_MESH_PB_ADV)
const struct bt_mesh_prov *prov;
struct net_buf *buf;
u8_t uri_hash[16] = { 0 };
u16_t oob_info;
BT_DBG("");
buf = bt_mesh_adv_create(BT_MESH_ADV_BEACON, UNPROV_XMIT_COUNT,
UNPROV_XMIT_INT, K_NO_WAIT);
if (!buf) {
BT_ERR("Unable to allocate beacon buffer");
return -ENOBUFS;
}
prov = bt_mesh_prov_get();
net_buf_add_u8(buf, BEACON_TYPE_UNPROVISIONED);
net_buf_add_mem(buf, prov->uuid, 16);
/* for tmall ble profile, OOB info default is 0x0000
URI hash is optional */
if (prov->uri && bt_mesh_s1(prov->uri, uri_hash) == 0) {
oob_info = prov->oob_info | BT_MESH_PROV_OOB_URI;
} else {
oob_info = prov->oob_info;
}
net_buf_add_be16(buf, oob_info);
net_buf_add_mem(buf, uri_hash, 4);
bt_mesh_adv_send(buf, NULL, NULL);
net_buf_unref(buf);
if (prov->uri) {
size_t len;
buf = bt_mesh_adv_create(BT_MESH_ADV_URI, UNPROV_XMIT_COUNT,
UNPROV_XMIT_INT, K_NO_WAIT);
if (!buf) {
BT_ERR("Unable to allocate URI buffer");
return -ENOBUFS;
}
len = strlen(prov->uri);
if (net_buf_tailroom(buf) < len) {
BT_WARN("Too long URI to fit advertising data");
} else {
net_buf_add_mem(buf, prov->uri, len);
bt_mesh_adv_send(buf, NULL, NULL);
}
net_buf_unref(buf);
}
#endif /* CONFIG_BT_MESH_PB_ADV */
return 0;
}
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 struct net_buf *l2cap_chan_create_seg(struct bt_l2cap_le_chan *ch,
struct net_buf *buf,
size_t sdu_hdr_len)
{
struct net_buf *seg;
uint16_t headroom;
uint16_t len;
/* Segment if data (+ data headroom) is bigger than MPS */
if (buf->len + sdu_hdr_len > ch->tx.mps) {
goto segment;
}
/* Segment if there is no space in the user_data */
if (buf->pool->user_data_size < BT_BUF_USER_DATA_MIN) {
BT_WARN("Too small buffer user_data_size %u",
buf->pool->user_data_size);
goto segment;
}
headroom = sizeof(struct bt_hci_acl_hdr) +
sizeof(struct bt_l2cap_hdr) + sdu_hdr_len;
/* Check if original buffer has enough headroom and don't have any
* fragments.
*/
if (net_buf_headroom(buf) >= headroom && !buf->frags) {
if (sdu_hdr_len) {
/* Push SDU length if set */
net_buf_push_le16(buf, net_buf_frags_len(buf));
}
return net_buf_ref(buf);
}
segment:
seg = bt_l2cap_create_pdu(&le_data_pool, 0);
if (sdu_hdr_len) {
net_buf_add_le16(seg, net_buf_frags_len(buf));
}
/* Don't send more that TX MPS including SDU length */
len = min(net_buf_tailroom(seg), ch->tx.mps - sdu_hdr_len);
/* Limit if original buffer is smaller than the segment */
len = min(buf->len, len);
net_buf_add_mem(seg, buf->data, len);
net_buf_pull(buf, len);
BT_DBG("ch %p seg %p len %u", ch, seg, seg->len);
return seg;
}
static void rx_thread(void *p1, void *p2, void *p3)
{
struct net_buf *buf;
ARG_UNUSED(p1);
ARG_UNUSED(p2);
ARG_UNUSED(p3);
BT_DBG("started");
while (1) {
BT_DBG("rx.buf %p", rx.buf);
/* We can only do the allocation if we know the initial
* header, since Command Complete/Status events must use the
* original command buffer (if available).
*/
if (rx.have_hdr && !rx.buf) {
rx.buf = get_rx(K_FOREVER);
BT_DBG("Got rx.buf %p", rx.buf);
if (rx.remaining > net_buf_tailroom(rx.buf)) {
BT_ERR("Not enough space in buffer");
rx.discard = rx.remaining;
reset_rx();
} else {
copy_hdr(rx.buf);
}
}
/* Let the ISR continue receiving new packets */
uart_irq_rx_enable(h4_dev);
buf = net_buf_get(&rx.fifo, K_FOREVER);
do {
uart_irq_rx_enable(h4_dev);
BT_DBG("Calling bt_recv(%p)", buf);
bt_recv(buf);
/* Give other threads a chance to run if the ISR
* is receiving data so fast that rx.fifo never
* or very rarely goes empty.
*/
k_yield();
uart_irq_rx_disable(h4_dev);
buf = net_buf_get(&rx.fifo, K_NO_WAIT);
} while (buf);
}
}
static struct net_buf *l2cap_alloc_frag(struct bt_l2cap_le_chan *chan)
{
struct net_buf *frag = NULL;
frag = chan->chan.ops->alloc_buf(&chan->chan);
if (!frag) {
return NULL;
}
BT_DBG("frag %p tailroom %zu", frag, net_buf_tailroom(frag));
net_buf_frag_add(chan->_sdu, frag);
return frag;
}
static struct net_buf *create_frag(struct bt_conn *conn, struct net_buf *buf)
{
struct net_buf *frag;
uint16_t frag_len;
frag = bt_conn_create_pdu(&frag_buf, 0);
if (conn->state != BT_CONN_CONNECTED) {
net_buf_unref(frag);
return NULL;
}
frag_len = min(conn_mtu(conn), net_buf_tailroom(frag));
memcpy(net_buf_add(frag, frag_len), buf->data, frag_len);
net_buf_pull(buf, frag_len);
return frag;
}
static inline void read_payload(void)
{
struct net_buf *buf;
bool prio;
int read;
if (!rx.buf) {
rx.buf = get_rx(K_NO_WAIT);
if (!rx.buf) {
if (rx.discardable) {
BT_WARN("Discarding event 0x%02x", rx.evt.evt);
rx.discard = rx.remaining;
reset_rx();
return;
}
BT_WARN("Failed to allocate, deferring to rx_thread");
uart_irq_rx_disable(h4_dev);
return;
}
BT_DBG("Allocated rx.buf %p", rx.buf);
if (rx.remaining > net_buf_tailroom(rx.buf)) {
BT_ERR("Not enough space in buffer");
rx.discard = rx.remaining;
reset_rx();
return;
}
copy_hdr(rx.buf);
}
read = uart_fifo_read(h4_dev, net_buf_tail(rx.buf), rx.remaining);
net_buf_add(rx.buf, read);
rx.remaining -= read;
BT_DBG("got %d bytes, remaining %u", read, rx.remaining);
BT_DBG("Payload (len %u): %s", rx.buf->len,
bt_hex(rx.buf->data, rx.buf->len));
if (rx.remaining) {
return;
}
prio = (rx.type == H4_EVT && bt_hci_evt_is_prio(rx.evt.evt));
buf = rx.buf;
rx.buf = NULL;
if (rx.type == H4_EVT) {
bt_buf_set_type(buf, BT_BUF_EVT);
} else {
bt_buf_set_type(buf, BT_BUF_ACL_IN);
}
reset_rx();
if (prio) {
BT_DBG("Calling bt_recv_prio(%p)", buf);
bt_recv_prio(buf);
} else {
BT_DBG("Putting buf %p to rx fifo", buf);
net_buf_put(&rx.fifo, buf);
}
}
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);
}
/* Prepare initial DHCPv4 message and add options as per message type */
static struct net_buf *prepare_message(struct net_if *iface, uint8_t type)
{
struct net_buf *buf;
struct net_buf *frag;
struct dhcp_msg *msg;
buf = net_nbuf_get_reserve_tx(0);
if (!buf) {
return NULL;
}
frag = net_nbuf_get_reserve_data(net_if_get_ll_reserve(iface, NULL));
if (!frag) {
goto fail;
}
net_nbuf_set_ll_reserve(buf, net_buf_headroom(frag));
net_nbuf_set_iface(buf, iface);
net_nbuf_set_family(buf, AF_INET);
net_nbuf_set_ip_hdr_len(buf, sizeof(struct net_ipv4_hdr));
net_buf_frag_add(buf, frag);
/* Leave room for IPv4 + UDP headers */
net_buf_add(buf->frags, NET_IPV4UDPH_LEN);
if (net_buf_tailroom(frag) < sizeof(struct dhcp_msg)) {
goto fail;
}
msg = (struct dhcp_msg *)(frag->data + NET_IPV4UDPH_LEN);
memset(msg, 0, sizeof(struct dhcp_msg));
msg->op = DHCPV4_MSG_BOOT_REQUEST;
msg->htype = HARDWARE_ETHERNET_TYPE;
msg->hlen = HARDWARE_ETHERNET_LEN;
msg->xid = htonl(iface->dhcpv4.xid);
msg->flags = htons(DHCPV4_MSG_BROADCAST);
if (iface->dhcpv4.state == NET_DHCPV4_INIT) {
memset(msg->ciaddr, 0, sizeof(msg->ciaddr));
} else {
memcpy(msg->ciaddr, iface->dhcpv4.requested_ip.s4_addr, 4);
}
memcpy(msg->chaddr, iface->link_addr.addr, iface->link_addr.len);
net_buf_add(frag, sizeof(struct dhcp_msg));
if (!add_sname(buf) ||
!add_file(buf) ||
!add_cookie(buf) ||
!add_msg_type(buf, type)) {
goto fail;
}
return buf;
fail:
net_nbuf_unref(buf);
return NULL;
}
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");
}
}
static struct net_pkt *build_reply_pkt(const char *name,
struct net_context *context,
struct net_pkt *pkt)
{
struct net_pkt *reply_pkt;
struct net_buf *frag, *tmp;
int header_len, recv_len, reply_len;
NET_INFO("%s received %d bytes", name,
net_pkt_appdatalen(pkt));
if (net_pkt_appdatalen(pkt) == 0) {
return NULL;
}
reply_pkt = net_pkt_get_tx(context, K_FOREVER);
NET_ASSERT(reply_pkt);
recv_len = net_pkt_get_len(pkt);
tmp = pkt->frags;
/* First fragment will contain IP header so move the data
* down in order to get rid of it.
*/
header_len = net_pkt_appdata(pkt) - tmp->data;
NET_ASSERT(header_len < CONFIG_NET_BUF_DATA_SIZE);
/* After this pull, the tmp->data points directly to application
* data.
*/
net_buf_pull(tmp, header_len);
while (tmp) {
frag = net_pkt_get_data(context, K_FOREVER);
if (!net_buf_headroom(tmp)) {
/* If there is no link layer headers in the
* received fragment, then get rid of that also
* in the sending fragment. We end up here
* if MTU is larger than fragment size, this
* is typical for ethernet.
*/
net_buf_push(frag, net_buf_headroom(frag));
frag->len = 0; /* to make fragment empty */
/* Make sure to set the reserve so that
* in sending side we add the link layer
* header if needed.
*/
net_pkt_set_ll_reserve(reply_pkt, 0);
}
NET_ASSERT(net_buf_tailroom(frag) >= tmp->len);
memcpy(net_buf_add(frag, tmp->len), tmp->data, tmp->len);
net_pkt_frag_add(reply_pkt, frag);
tmp = net_pkt_frag_del(pkt, NULL, tmp);
}
reply_len = net_pkt_get_len(reply_pkt);
NET_ASSERT_INFO((recv_len - header_len) == reply_len,
"Received %d bytes, sending %d bytes",
recv_len - header_len, reply_len);
return reply_pkt;
}
void bt_conn_recv(struct bt_conn *conn, struct net_buf *buf, uint8_t flags)
{
struct bt_l2cap_hdr *hdr;
uint16_t len;
BT_DBG("handle %u len %u flags %02x", conn->handle, buf->len, flags);
/* Check packet boundary flags */
switch (flags) {
case BT_ACL_START:
hdr = (void *)buf->data;
len = sys_le16_to_cpu(hdr->len);
BT_DBG("First, len %u final %u", buf->len, len);
if (conn->rx_len) {
BT_ERR("Unexpected first L2CAP frame");
bt_conn_reset_rx_state(conn);
}
conn->rx_len = (sizeof(*hdr) + len) - buf->len;
BT_DBG("rx_len %u", conn->rx_len);
if (conn->rx_len) {
conn->rx = buf;
return;
}
break;
case BT_ACL_CONT:
if (!conn->rx_len) {
BT_ERR("Unexpected L2CAP continuation");
bt_conn_reset_rx_state(conn);
net_buf_unref(buf);
return;
}
if (buf->len > conn->rx_len) {
BT_ERR("L2CAP data overflow");
bt_conn_reset_rx_state(conn);
net_buf_unref(buf);
return;
}
BT_DBG("Cont, len %u rx_len %u", buf->len, conn->rx_len);
if (buf->len > net_buf_tailroom(conn->rx)) {
BT_ERR("Not enough buffer space for L2CAP data");
bt_conn_reset_rx_state(conn);
net_buf_unref(buf);
return;
}
memcpy(net_buf_add(conn->rx, buf->len), buf->data, buf->len);
conn->rx_len -= buf->len;
net_buf_unref(buf);
if (conn->rx_len) {
return;
}
buf = conn->rx;
conn->rx = NULL;
conn->rx_len = 0;
break;
default:
BT_ERR("Unexpected ACL flags (0x%02x)", flags);
bt_conn_reset_rx_state(conn);
net_buf_unref(buf);
return;
}
hdr = (void *)buf->data;
len = sys_le16_to_cpu(hdr->len);
if (sizeof(*hdr) + len != buf->len) {
BT_ERR("ACL len mismatch (%u != %u)", len, buf->len);
net_buf_unref(buf);
return;
}
BT_DBG("Successfully parsed %u byte L2CAP packet", buf->len);
bt_l2cap_recv(conn, buf);
}
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 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 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 bt_uart_isr(struct device *unused)
{
static struct net_buf *buf;
static int remaining;
ARG_UNUSED(unused);
while (uart_irq_update(h4_dev) && uart_irq_is_pending(h4_dev)) {
int read;
if (!uart_irq_rx_ready(h4_dev)) {
if (uart_irq_tx_ready(h4_dev)) {
BT_DBG("transmit ready");
} else {
BT_DBG("spurious interrupt");
}
continue;
}
/* Beginning of a new packet */
if (!remaining) {
uint8_t type;
/* Get packet type */
read = h4_read(h4_dev, &type, sizeof(type), 0);
if (read != sizeof(type)) {
BT_WARN("Unable to read H4 packet type");
continue;
}
switch (type) {
case H4_EVT:
buf = h4_evt_recv(&remaining);
break;
case H4_ACL:
buf = h4_acl_recv(&remaining);
break;
default:
BT_ERR("Unknown H4 type %u", type);
return;
}
BT_DBG("need to get %u bytes", remaining);
if (buf && remaining > net_buf_tailroom(buf)) {
BT_ERR("Not enough space in buffer");
net_buf_unref(buf);
buf = NULL;
}
}
if (!buf) {
read = h4_discard(h4_dev, remaining);
BT_WARN("Discarded %d bytes", read);
remaining -= read;
continue;
}
read = h4_read(h4_dev, net_buf_tail(buf), remaining, 0);
buf->len += read;
remaining -= read;
BT_DBG("received %d bytes", read);
if (!remaining) {
BT_DBG("full packet received");
/* Pass buffer to the stack */
bt_recv(buf);
buf = NULL;
}
}
}
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");
}