static void ipsp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
{
struct bt_context *ctxt = CHAN_CTXT(chan);
struct net_buf *nbuf;
NET_DBG("Incoming data channel %p len %u", chan,
net_buf_frags_len(buf));
/* Get buffer for bearer / protocol related data */
nbuf = net_nbuf_get_reserve_rx(0);
/* Set destination address */
net_nbuf_ll_dst(nbuf)->addr = ctxt->src.val;
net_nbuf_ll_dst(nbuf)->len = sizeof(ctxt->src);
/* Set source address */
net_nbuf_ll_src(nbuf)->addr = ctxt->dst.val;
net_nbuf_ll_src(nbuf)->len = sizeof(ctxt->dst);
/* Add data buffer as fragment of RX buffer, take a reference while
* doing so since L2CAP will unref the buffer after return.
*/
net_buf_frag_add(nbuf, net_buf_ref(buf));
if (net_recv_data(ctxt->iface, nbuf) < 0) {
NET_DBG("Packet dropped by NET stack");
net_nbuf_unref(nbuf);
}
}
struct net_buf *
ieee802154_create_mac_cmd_frame(struct net_if *iface,
enum ieee802154_cfi type,
struct ieee802154_frame_params *params)
{
struct ieee802154_context *ctx = net_if_l2_data(iface);
struct ieee802154_fcf_seq *fs;
struct net_buf *buf, *frag;
uint8_t *p_buf;
buf = net_nbuf_get_reserve_tx(0);
if (!buf) {
return NULL;
}
frag = net_nbuf_get_reserve_data(0);
if (!frag) {
goto error;
}
net_buf_frag_add(buf, frag);
p_buf = net_nbuf_ll(buf);
fs = generate_fcf_grounds(&p_buf, false);
fs->fc.frame_type = IEEE802154_FRAME_TYPE_MAC_COMMAND;
fs->sequence = ctx->sequence;
if (!cfi_to_fs_settings(type, fs, params)) {
goto error;
}
p_buf = generate_addressing_fields(iface, fs, params, p_buf);
/* Let's insert the cfi */
((struct ieee802154_command *)p_buf)->cfi = type;
/* In MAC command, we consider ll header being the mhr.
* Rest will be the MAC command itself. This will proove
* to be easy to handle afterwards to point directly to MAC
* command space, in order to fill-in its content.
*/
net_nbuf_set_ll_reserve(buf, p_buf - net_nbuf_ll(buf));
net_buf_pull(frag, net_nbuf_ll_reserve(buf));
/* Thus setting the right MAC command length
* Now up to the caller to fill-in this space relevantly.
* See ieee802154_mac_command() helper.
*/
net_nbuf_set_len(frag, mac_command_length(type));
dbg_print_fs(fs);
return buf;
error:
net_nbuf_unref(buf);
return NULL;
}
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;
}
/* 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_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_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");
}
}
