/* 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 inline void ieee802154_acknowledge(struct net_if *iface, struct ieee802154_mpdu *mpdu) { struct net_pkt *pkt; struct net_buf *frag; if (!mpdu->mhr.fs->fc.ar) { return; } pkt = net_pkt_get_reserve_tx(IEEE802154_ACK_PKT_LENGTH, K_FOREVER); if (!pkt) { return; } frag = net_pkt_get_frag(pkt, K_FOREVER); net_pkt_frag_insert(pkt, frag); if (ieee802154_create_ack_frame(iface, pkt, mpdu->mhr.fs->sequence)) { const struct ieee802154_radio_api *radio = iface->dev->driver_api; net_buf_add(frag, IEEE802154_ACK_PKT_LENGTH); radio->tx(iface->dev, pkt, frag); } net_pkt_unref(pkt); return; }
struct net_pkt *net_ipv4_create_raw(struct net_pkt *pkt, const struct in_addr *src, const struct in_addr *dst, struct net_if *iface, u8_t next_header) { struct net_buf *header; header = net_pkt_get_frag(pkt, K_FOREVER); net_pkt_frag_insert(pkt, header); NET_IPV4_HDR(pkt)->vhl = 0x45; NET_IPV4_HDR(pkt)->tos = 0x00; NET_IPV4_HDR(pkt)->proto = 0; NET_IPV4_HDR(pkt)->ttl = net_if_ipv4_get_ttl(iface); NET_IPV4_HDR(pkt)->offset[0] = NET_IPV4_HDR(pkt)->offset[1] = 0; NET_IPV4_HDR(pkt)->id[0] = NET_IPV4_HDR(pkt)->id[1] = 0; net_ipaddr_copy(&NET_IPV4_HDR(pkt)->dst, dst); net_ipaddr_copy(&NET_IPV4_HDR(pkt)->src, src); NET_IPV4_HDR(pkt)->proto = next_header; net_pkt_set_ip_hdr_len(pkt, sizeof(struct net_ipv4_hdr)); net_pkt_set_family(pkt, AF_INET); net_buf_add(header, sizeof(struct net_ipv4_hdr)); return pkt; }
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 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_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"); }