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 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 bool send_ipv4_udp_msg(struct net_if *iface,
struct in_addr *src,
struct in_addr *dst,
u16_t src_port,
u16_t dst_port,
struct ud *ud,
bool expect_failure)
{
struct net_pkt *pkt;
struct net_buf *frag;
int ret;
pkt = net_pkt_get_reserve_tx(0, K_FOREVER);
frag = net_pkt_get_frag(pkt, K_FOREVER);
net_pkt_frag_add(pkt, frag);
net_pkt_set_iface(pkt, iface);
net_pkt_set_ll_reserve(pkt, net_buf_headroom(frag));
setup_ipv4_udp(pkt, src, dst, src_port, dst_port);
ret = net_recv_data(iface, pkt);
if (ret < 0) {
printk("Cannot recv pkt %p, ret %d\n", pkt, ret);
zassert_true(0, "exiting");
}
if (k_sem_take(&recv_lock, TIMEOUT)) {
/**TESTPOINT: Check for failure*/
zassert_true(expect_failure, "Timeout, packet not received");
return true;
}
/* Check that the returned user data is the same as what was given
* as a parameter.
*/
if (ud != returned_ud && !expect_failure) {
printk("IPv4 wrong user data %p returned, expected %p\n",
returned_ud, ud);
zassert_true(0, "exiting");
}
return !fail;
}
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;
}
headroom = sizeof(struct bt_hci_acl_hdr) +
sizeof(struct bt_l2cap_hdr) + sdu_hdr_len;
/* Check if original buffer has enough headroom */
if (net_buf_headroom(buf) >= headroom) {
if (sdu_hdr_len) {
/* Push SDU length if set */
net_buf_push_le16(buf, buf->len);
}
return net_buf_ref(buf);
}
segment:
seg = bt_l2cap_create_pdu(&le_data);
if (!seg) {
return NULL;
}
if (sdu_hdr_len) {
net_buf_add_le16(seg, buf->len);
}
len = min(min(buf->len, L2CAP_LE_MIN_MTU - sdu_hdr_len), ch->tx.mps);
memcpy(net_buf_add(seg, len), buf->data, len);
net_buf_pull(buf, len);
BT_DBG("ch %p seg %p len %u", ch, seg, seg->len);
return seg;
}
/* 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 int slip_send(struct net_if *iface, struct net_pkt *pkt)
{
struct net_buf *frag;
#if defined(CONFIG_SLIP_TAP)
u16_t ll_reserve = net_pkt_ll_reserve(pkt);
bool send_header_once = false;
#endif
u8_t *ptr;
u16_t i;
u8_t c;
if (!pkt->frags) {
/* No data? */
return -ENODATA;
}
slip_writeb(SLIP_END);
for (frag = pkt->frags; frag; frag = frag->frags) {
#if SYS_LOG_LEVEL >= SYS_LOG_LEVEL_DEBUG
int frag_count = 0;
#endif
#if defined(CONFIG_SLIP_TAP)
ptr = frag->data - ll_reserve;
/* This writes ethernet header */
if (!send_header_once && ll_reserve) {
for (i = 0; i < ll_reserve; i++) {
slip_writeb_esc(*ptr++);
}
}
if (net_if_get_mtu(iface) > net_buf_headroom(frag)) {
/* Do not add link layer header if the mtu is bigger
* than fragment size. The first packet needs the
* link layer header always.
*/
send_header_once = true;
ll_reserve = 0;
ptr = frag->data;
}
#else
/* There is no ll header in tun device */
ptr = frag->data;
#endif
for (i = 0; i < frag->len; ++i) {
c = *ptr++;
slip_writeb_esc(c);
}
#if SYS_LOG_LEVEL >= SYS_LOG_LEVEL_DEBUG
SYS_LOG_DBG("sent data %d bytes",
frag->len + net_pkt_ll_reserve(pkt));
if (frag->len + ll_reserve) {
char msg[8 + 1];
snprintf(msg, sizeof(msg), "<slip %2d", frag_count++);
hexdump(msg, net_pkt_ll(pkt),
frag->len + net_pkt_ll_reserve(pkt),
net_pkt_ll_reserve(pkt));
}
#endif
}
net_pkt_unref(pkt);
slip_writeb(SLIP_END);
return 0;
}
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;
}
