static gnrc_pktsnip_t *_build_frag_pkt(gnrc_pktsnip_t *pkt, size_t payload_len,
size_t size)
{
gnrc_netif_hdr_t *hdr = pkt->data, *new_hdr;
gnrc_pktsnip_t *netif, *frag;
netif = gnrc_netif_hdr_build(gnrc_netif_hdr_get_src_addr(hdr), hdr->src_l2addr_len,
gnrc_netif_hdr_get_dst_addr(hdr), hdr->dst_l2addr_len);
if (netif == NULL) {
DEBUG("6lo frag: error allocating new link-layer header\n");
return NULL;
}
new_hdr = netif->data;
new_hdr->if_pid = hdr->if_pid;
new_hdr->flags = hdr->flags;
new_hdr->rssi = hdr->rssi;
new_hdr->lqi = hdr->lqi;
frag = gnrc_pktbuf_add(NULL, NULL, _min(size, payload_len),
GNRC_NETTYPE_SIXLOWPAN);
if (frag == NULL) {
DEBUG("6lo frag: error allocating first fragment\n");
gnrc_pktbuf_release(netif);
return NULL;
}
LL_PREPEND(frag, netif);
return frag;
}
/* functions for sending */
static void _send_unicast(kernel_pid_t iface, uint8_t *dst_l2addr,
uint16_t dst_l2addr_len, gnrc_pktsnip_t *pkt)
{
gnrc_pktsnip_t *netif;
if (pkt->type == GNRC_NETTYPE_NETIF) {
/* great: someone already added a netif_hdr_t we assume it's wrong
* to keep it simple
* XXX: alternative would be to check if gnrc_netif_hdr_t::dst_l2addr_len
* is long enough and only then to throw away the header. This causes
* to much overhead IMHO */
DEBUG("ipv6: removed old interface header\n");
pkt = gnrc_pktbuf_remove_snip(pkt, pkt);
}
DEBUG("ipv6: add interface header to packet\n");
netif = gnrc_netif_hdr_build(NULL, 0, dst_l2addr, dst_l2addr_len);
if (netif == NULL) {
DEBUG("ipv6: error on interface header allocation, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
/* add netif to front of the pkt list */
LL_PREPEND(pkt, netif);
DEBUG("ipv6: send unicast over interface %" PRIkernel_pid "\n", iface);
/* and send to interface */
#ifdef MODULE_NETSTATS_IPV6
gnrc_ipv6_netif_get_stats(iface)->tx_unicast_count++;
#endif
_send_to_iface(iface, pkt);
}
void gnrc_sixlowpan_frag_rbuf_dispatch_when_complete(gnrc_sixlowpan_rbuf_t *rbuf,
gnrc_netif_hdr_t *netif_hdr)
{
assert(rbuf);
assert(netif_hdr);
if (rbuf->current_size == rbuf->pkt->size) {
gnrc_pktsnip_t *netif = gnrc_netif_hdr_build(rbuf->src,
rbuf->src_len,
rbuf->dst,
rbuf->dst_len);
if (netif == NULL) {
DEBUG("6lo rbuf: error allocating netif header\n");
gnrc_pktbuf_release(rbuf->pkt);
gnrc_sixlowpan_frag_rbuf_remove(rbuf);
return;
}
/* copy the transmit information of the latest fragment into the newly
* created header to have some link_layer information. The link_layer
* info of the previous fragments is discarded.
*/
gnrc_netif_hdr_t *new_netif_hdr = netif->data;
new_netif_hdr->if_pid = netif_hdr->if_pid;
new_netif_hdr->flags = netif_hdr->flags;
new_netif_hdr->lqi = netif_hdr->lqi;
new_netif_hdr->rssi = netif_hdr->rssi;
LL_APPEND(rbuf->pkt, netif);
gnrc_sixlowpan_dispatch_recv(rbuf->pkt, NULL, 0);
gnrc_sixlowpan_frag_rbuf_remove(rbuf);
}
}
static gnrc_pktsnip_t *_make_netif_hdr(uint8_t *mhr)
{
gnrc_netif_hdr_t *hdr;
gnrc_pktsnip_t *snip;
uint8_t src[IEEE802154_LONG_ADDRESS_LEN], dst[IEEE802154_LONG_ADDRESS_LEN];
int src_len, dst_len;
le_uint16_t _pan_tmp; /* TODO: hand-up PAN IDs to GNRC? */
dst_len = ieee802154_get_dst(mhr, dst, &_pan_tmp);
src_len = ieee802154_get_src(mhr, src, &_pan_tmp);
if ((dst_len < 0) || (src_len < 0)) {
DEBUG("_make_netif_hdr: unable to get addresses\n");
return NULL;
}
/* allocate space for header */
snip = gnrc_netif_hdr_build(src, (size_t)src_len, dst, (size_t)dst_len);
if (snip == NULL) {
DEBUG("_make_netif_hdr: no space left in packet buffer\n");
return NULL;
}
hdr = snip->data;
/* set broadcast flag for broadcast destination */
if ((dst_len == 2) && (dst[0] == 0xff) && (dst[1] == 0xff)) {
hdr->flags |= GNRC_NETIF_HDR_FLAGS_BROADCAST;
}
/* set flags for pending frames */
if (mhr[0] & IEEE802154_FCF_FRAME_PEND) {
hdr->flags |= GNRC_NETIF_HDR_FLAGS_MORE_DATA;
}
return snip;
}
static gnrc_pktsnip_t *_build_ipv6_packet(const ipv6_addr_t *src,
const ipv6_addr_t *dst, uint8_t nh,
void *data, size_t data_len,
uint16_t netif)
{
gnrc_pktsnip_t *netif_hdr, *ipv6, *payload;
ipv6_hdr_t *ipv6_hdr;
if ((netif > INT16_MAX) || (data_len > UINT16_MAX)) {
return NULL;
}
payload = gnrc_pktbuf_add(NULL, data, data_len, GNRC_NETTYPE_UNDEF);
if (payload == NULL) {
return NULL;
}
ipv6 = gnrc_ipv6_hdr_build(NULL, src, dst);
if (ipv6 == NULL) {
return NULL;
}
ipv6_hdr = ipv6->data;
ipv6_hdr->len = byteorder_htons((uint16_t)payload->size);
ipv6_hdr->nh = nh;
ipv6_hdr->hl = 64;
LL_APPEND(payload, ipv6);
netif_hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif_hdr == NULL) {
return NULL;
}
((gnrc_netif_hdr_t *)netif_hdr->data)->if_pid = (kernel_pid_t)netif;
LL_APPEND(payload, netif_hdr);
return payload;
}
/* shell commands */
int _netif_send(int argc, char **argv)
{
kernel_pid_t dev;
uint8_t addr[MAX_ADDR_LEN];
size_t addr_len;
gnrc_pktsnip_t *pkt, *hdr;
gnrc_netif_hdr_t *nethdr;
uint8_t flags = 0x00;
if (argc < 4) {
printf("usage: %s <if> [<L2 addr>|bcast] <data>\n", argv[0]);
return 1;
}
/* parse interface */
dev = atoi(argv[1]);
if (!_is_iface(dev)) {
puts("error: invalid interface given");
return 1;
}
/* parse address */
addr_len = gnrc_netif_addr_from_str(addr, sizeof(addr), argv[2]);
if (addr_len == 0) {
if (strcmp(argv[2], "bcast") == 0) {
flags |= GNRC_NETIF_HDR_FLAGS_BROADCAST;
}
else {
puts("error: invalid address given");
return 1;
}
}
/* put packet together */
pkt = gnrc_pktbuf_add(NULL, argv[3], strlen(argv[3]), GNRC_NETTYPE_UNDEF);
if (pkt == NULL) {
puts("error: packet buffer full");
return 1;
}
hdr = gnrc_netif_hdr_build(NULL, 0, addr, addr_len);
if (hdr == NULL) {
puts("error: packet buffer full");
gnrc_pktbuf_release(pkt);
return 1;
}
LL_PREPEND(pkt, hdr);
nethdr = (gnrc_netif_hdr_t *)hdr->data;
nethdr->flags = flags;
/* and send it */
if (gnrc_netapi_send(dev, pkt) < 1) {
puts("error: unable to send");
gnrc_pktbuf_release(pkt);
return 1;
}
return 0;
}
void gnrc_icmpv6_echo_req_handle(gnrc_netif_t *netif, ipv6_hdr_t *ipv6_hdr,
icmpv6_echo_t *echo, uint16_t len)
{
uint8_t *payload = ((uint8_t *)echo) + sizeof(icmpv6_echo_t);
gnrc_pktsnip_t *hdr, *pkt;
if ((echo == NULL) || (len < sizeof(icmpv6_echo_t))) {
DEBUG("icmpv6_echo: echo was NULL or len (%" PRIu16
") was < sizeof(icmpv6_echo_t)\n", len);
return;
}
pkt = gnrc_icmpv6_echo_build(ICMPV6_ECHO_REP, byteorder_ntohs(echo->id),
byteorder_ntohs(echo->seq), payload,
len - sizeof(icmpv6_echo_t));
if (pkt == NULL) {
DEBUG("icmpv6_echo: no space left in packet buffer\n");
return;
}
if (ipv6_addr_is_multicast(&ipv6_hdr->dst)) {
hdr = gnrc_ipv6_hdr_build(pkt, NULL, &ipv6_hdr->src);
}
else {
hdr = gnrc_ipv6_hdr_build(pkt, &ipv6_hdr->dst, &ipv6_hdr->src);
}
if (hdr == NULL) {
DEBUG("icmpv6_echo: no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return;
}
pkt = hdr;
hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif != NULL) {
((gnrc_netif_hdr_t *)hdr->data)->if_pid = netif->pid;
}
else {
/* ipv6_hdr->dst is loopback address */
((gnrc_netif_hdr_t *)hdr->data)->if_pid = KERNEL_PID_UNDEF;
}
LL_PREPEND(pkt, hdr);
if (!gnrc_netapi_dispatch_send(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL,
pkt)) {
DEBUG("icmpv6_echo: no receivers for IPv6 packets\n");
gnrc_pktbuf_release(pkt);
}
}
void gnrc_rpl_send(gnrc_pktsnip_t *pkt, kernel_pid_t iface, ipv6_addr_t *src, ipv6_addr_t *dst,
ipv6_addr_t *dodag_id)
{
(void)dodag_id;
gnrc_pktsnip_t *hdr;
if (iface == KERNEL_PID_UNDEF) {
if ((iface = gnrc_ipv6_netif_find_by_addr(NULL, &ipv6_addr_all_rpl_nodes))
== KERNEL_PID_UNDEF) {
DEBUG("RPL: no suitable interface found for this destination address\n");
gnrc_pktbuf_release(pkt);
return;
}
}
if (src == NULL) {
src = gnrc_ipv6_netif_match_prefix(iface, &ipv6_addr_link_local_prefix);
if (src == NULL) {
DEBUG("RPL: no suitable src address found\n");
gnrc_pktbuf_release(pkt);
return;
}
}
if (dst == NULL) {
dst = (ipv6_addr_t *) &ipv6_addr_all_rpl_nodes;
}
hdr = gnrc_ipv6_hdr_build(pkt, src, dst);
if (hdr == NULL) {
DEBUG("RPL: Send - no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return;
}
pkt = hdr;
hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
((gnrc_netif_hdr_t *)hdr->data)->if_pid = iface;
LL_PREPEND(pkt, hdr);
if (!gnrc_netapi_dispatch_send(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL, pkt)) {
DEBUG("RPL: cannot send packet: no subscribers found.\n");
gnrc_pktbuf_release(pkt);
}
}
/* SLIP receive handler */
static void _slip_receive(gnrc_slip_dev_t *dev, size_t bytes)
{
gnrc_pktsnip_t *pkt, *hdr;
hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (hdr == NULL) {
DEBUG("slip: no space left in packet buffer\n");
return;
}
((gnrc_netif_hdr_t *)(hdr->data))->if_pid = thread_getpid();
pkt = gnrc_pktbuf_add(hdr, NULL, bytes, GNRC_NETTYPE_UNDEF);
if (pkt == NULL) {
DEBUG("slip: no space left in packet buffer\n");
gnrc_pktbuf_release(hdr);
return;
}
if (ringbuffer_get(&dev->in_buf, pkt->data, bytes) != bytes) {
DEBUG("slip: could not read %u bytes from ringbuffer\n", (unsigned)bytes);
gnrc_pktbuf_release(pkt);
return;
}
#if ENABLE_DEBUG && defined(MODULE_OD)
else {
DEBUG("slip: received data\n");
od_hex_dump(pkt->data, bytes, OD_WIDTH_DEFAULT);
}
#endif
#ifdef MODULE_GNRC_IPV6
if ((pkt->size >= sizeof(ipv6_hdr_t)) && ipv6_hdr_is(pkt->data)) {
pkt->type = GNRC_NETTYPE_IPV6;
}
#endif
if (gnrc_netapi_dispatch_receive(pkt->type, GNRC_NETREG_DEMUX_CTX_ALL, pkt) == 0) {
DEBUG("slip: unable to forward packet of type %i\n", pkt->type);
gnrc_pktbuf_release(pkt);
}
}
/* tests sending */
static int test_send(void)
{
ethernet_hdr_t *exp_mac = (ethernet_hdr_t *)_tmp;
uint8_t *exp_payload = _tmp + sizeof(ethernet_hdr_t);
gnrc_pktsnip_t *pkt, *hdr;
msg_t msg;
/* prepare packet for sending */
pkt = gnrc_pktbuf_add(NULL, _TEST_PAYLOAD1, sizeof(_TEST_PAYLOAD1) - 1,
GNRC_NETTYPE_UNDEF);
if (pkt == NULL) {
puts("Could not allocate send payload");
return 0;
}
hdr = gnrc_netif_hdr_build(NULL, 0, (uint8_t *)_test_dst, sizeof(_test_dst));
if (hdr == NULL) {
gnrc_pktbuf_release(pkt);
puts("Could not allocate send header");
return 0;
}
LL_PREPEND(pkt, hdr);
/* prepare expected data */
memcpy(exp_mac->dst, _test_dst, sizeof(_test_dst));
memcpy(exp_mac->src, _dev_addr, sizeof(_dev_addr));
exp_mac->type = byteorder_htons(ETHERTYPE_UNKNOWN);
memcpy(exp_payload, _TEST_PAYLOAD1, sizeof(_TEST_PAYLOAD1) - 1);
_tmp_len = sizeof(_TEST_PAYLOAD1) + sizeof(ethernet_hdr_t) - 1;
/* register for returned packet status */
if (gnrc_neterr_reg(pkt) != 0) {
puts("Can not register for error reporting");
return 0;
}
/* send packet to MAC layer */
gnrc_netapi_send(_mac_pid, pkt);
/* wait for packet status and check */
msg_receive(&msg);
if ((msg.type != GNRC_NETERR_MSG_TYPE) ||
(msg.content.value != GNRC_NETERR_SUCCESS)) {
puts("Error sending packet");
return 0;
}
return 1;
}
static gnrc_pktsnip_t *_build_headers(kernel_pid_t iface, gnrc_pktsnip_t *payload,
ipv6_addr_t *dst, ipv6_addr_t *src)
{
gnrc_pktsnip_t *l2hdr;
gnrc_pktsnip_t *iphdr = gnrc_ipv6_hdr_build(payload, (uint8_t *)src, sizeof(ipv6_addr_t),
(uint8_t *)dst, sizeof(ipv6_addr_t));
if (iphdr == NULL) {
DEBUG("ndp internal: error allocating IPv6 header.\n");
return NULL;
}
((ipv6_hdr_t *)iphdr->data)->hl = 255;
/* add netif header for send interface specification */
l2hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (l2hdr == NULL) {
DEBUG("ndp internal: error allocating netif header.\n");
gnrc_pktbuf_remove_snip(iphdr, iphdr);
return NULL;
}
((gnrc_netif_hdr_t *)l2hdr->data)->if_pid = iface;
LL_PREPEND(iphdr, l2hdr);
return l2hdr;
}
static int _send(int argc, char **argv)
{
(void) argc;
(void) argv;
gnrc_pktsnip_t *pkt, *hdr;
gnrc_netif_hdr_t *nethdr;
uint8_t addr[8];
size_t addr_len = 0;
uint8_t flags = 0x00;
kernel_pid_t ifs[GNRC_NETIF_NUMOF];
size_t numof = gnrc_netif_get(ifs);
for (size_t i = 0; i < numof && i < GNRC_NETIF_NUMOF; i++) {
flags |= GNRC_NETIF_HDR_FLAGS_BROADCAST;
/* put packet together */
conn_test_payload_t payload;
payload.id = conn_test_id;
// memcpy(payload.txt, CONN_TEST_PAYLOAD, sizeof(CONN_TEST_PAYLOAD));
pkt = gnrc_pktbuf_add(NULL, &payload, sizeof(payload), GNRC_NETTYPE_UNDEF);
hdr = gnrc_netif_hdr_build(NULL, 0, addr, addr_len);
LL_PREPEND(pkt, hdr);
nethdr = (gnrc_netif_hdr_t *)hdr->data;
nethdr->flags = flags;
/* and send it */
if (gnrc_netapi_send(ifs[i], pkt) < 1) {
puts("error: unable to send\n");
gnrc_pktbuf_release(pkt);
}
}
return 0;
}
static gnrc_pktsnip_t *_recv(gnrc_netif_t *netif)
{
netdev_t *dev = netif->dev;
netdev_ieee802154_rx_info_t rx_info;
gnrc_pktsnip_t *pkt = NULL;
int bytes_expected = dev->driver->recv(dev, NULL, 0, NULL);
if (bytes_expected >= (int)IEEE802154_MIN_FRAME_LEN) {
int nread;
pkt = gnrc_pktbuf_add(NULL, NULL, bytes_expected, GNRC_NETTYPE_UNDEF);
if (pkt == NULL) {
DEBUG("_recv_ieee802154: cannot allocate pktsnip.\n");
/* Discard packet on netdev device */
dev->driver->recv(dev, NULL, bytes_expected, NULL);
return NULL;
}
nread = dev->driver->recv(dev, pkt->data, bytes_expected, &rx_info);
if (nread <= 0) {
gnrc_pktbuf_release(pkt);
return NULL;
}
#ifdef MODULE_NETSTATS_L2
netif->stats.rx_count++;
netif->stats.rx_bytes += nread;
#endif
if (netif->flags & GNRC_NETIF_FLAGS_RAWMODE) {
/* Raw mode, skip packet processing, but provide rx_info via
* GNRC_NETTYPE_NETIF */
gnrc_pktsnip_t *netif_snip = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif_snip == NULL) {
DEBUG("_recv_ieee802154: no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return NULL;
}
gnrc_netif_hdr_t *hdr = netif_snip->data;
hdr->lqi = rx_info.lqi;
hdr->rssi = rx_info.rssi;
hdr->if_pid = netif->pid;
LL_APPEND(pkt, netif_snip);
}
else {
/* Normal mode, try to parse the frame according to IEEE 802.15.4 */
gnrc_pktsnip_t *ieee802154_hdr, *netif_hdr;
gnrc_netif_hdr_t *hdr;
#if ENABLE_DEBUG
char src_str[GNRC_NETIF_HDR_L2ADDR_PRINT_LEN];
#endif
size_t mhr_len = ieee802154_get_frame_hdr_len(pkt->data);
/* nread was checked for <= 0 before so we can safely cast it to
* unsigned */
if ((mhr_len == 0) || ((size_t)nread < mhr_len)) {
DEBUG("_recv_ieee802154: illegally formatted frame received\n");
gnrc_pktbuf_release(pkt);
return NULL;
}
nread -= mhr_len;
/* mark IEEE 802.15.4 header */
ieee802154_hdr = gnrc_pktbuf_mark(pkt, mhr_len, GNRC_NETTYPE_UNDEF);
if (ieee802154_hdr == NULL) {
DEBUG("_recv_ieee802154: no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return NULL;
}
netif_hdr = _make_netif_hdr(ieee802154_hdr->data);
if (netif_hdr == NULL) {
DEBUG("_recv_ieee802154: no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return NULL;
}
hdr = netif_hdr->data;
#ifdef MODULE_L2FILTER
if (!l2filter_pass(dev->filter, gnrc_netif_hdr_get_src_addr(hdr),
hdr->src_l2addr_len)) {
gnrc_pktbuf_release(pkt);
gnrc_pktbuf_release(netif_hdr);
DEBUG("_recv_ieee802154: packet dropped by l2filter\n");
return NULL;
}
#endif
#ifdef MODULE_GNRC_NETIF_DEDUP
if (_already_received(netif, hdr, ieee802154_hdr->data)) {
gnrc_pktbuf_release(pkt);
gnrc_pktbuf_release(netif_hdr);
DEBUG("_recv_ieee802154: packet dropped by deduplication\n");
return NULL;
}
memcpy(netif->last_pkt.src, gnrc_netif_hdr_get_src_addr(hdr),
hdr->src_l2addr_len);
netif->last_pkt.src_len = hdr->src_l2addr_len;
netif->last_pkt.seq = ieee802154_get_seq(ieee802154_hdr->data);
#endif /* MODULE_GNRC_NETIF_DEDUP */
hdr->lqi = rx_info.lqi;
hdr->rssi = rx_info.rssi;
hdr->if_pid = thread_getpid();
dev->driver->get(dev, NETOPT_PROTO, &pkt->type, sizeof(pkt->type));
#if ENABLE_DEBUG
DEBUG("_recv_ieee802154: received packet from %s of length %u\n",
gnrc_netif_addr_to_str(gnrc_netif_hdr_get_src_addr(hdr),
hdr->src_l2addr_len,
src_str),
nread);
#if defined(MODULE_OD)
od_hex_dump(pkt->data, nread, OD_WIDTH_DEFAULT);
#endif
#endif
gnrc_pktbuf_remove_snip(pkt, ieee802154_hdr);
LL_APPEND(pkt, netif_hdr);
}
DEBUG("_recv_ieee802154: reallocating.\n");
gnrc_pktbuf_realloc_data(pkt, nread);
} else if (bytes_expected > 0) {
DEBUG("_recv_ieee802154: received frame is too short\n");
dev->driver->recv(dev, NULL, bytes_expected, NULL);
}
return pkt;
}
void gnrc_icmpv6_echo_req_handle(kernel_pid_t iface, ipv6_hdr_t *ipv6_hdr,
icmpv6_echo_t *echo, uint16_t len)
{
uint8_t *payload = ((uint8_t *)echo) + sizeof(icmpv6_echo_t);
gnrc_pktsnip_t *hdr, *pkt;
gnrc_netreg_entry_t *sendto = NULL;
if ((echo == NULL) || (len < sizeof(icmpv6_echo_t))) {
DEBUG("icmpv6_echo: echo was NULL or len (%" PRIu16
") was < sizeof(icmpv6_echo_t)\n", len);
return;
}
pkt = gnrc_icmpv6_echo_build(ICMPV6_ECHO_REP, byteorder_ntohs(echo->id),
byteorder_ntohs(echo->seq), payload,
len - sizeof(icmpv6_echo_t));
if (pkt == NULL) {
DEBUG("icmpv6_echo: no space left in packet buffer\n");
return;
}
if (ipv6_addr_is_multicast(&ipv6_hdr->dst)) {
hdr = gnrc_ipv6_hdr_build(pkt, NULL, 0, (uint8_t *)&ipv6_hdr->src,
sizeof(ipv6_addr_t));
}
else {
hdr = gnrc_ipv6_hdr_build(pkt, (uint8_t *)&ipv6_hdr->dst,
sizeof(ipv6_addr_t), (uint8_t *)&ipv6_hdr->src,
sizeof(ipv6_addr_t));
}
if (hdr == NULL) {
DEBUG("icmpv6_echo: no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return;
}
pkt = hdr;
hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
((gnrc_netif_hdr_t *)hdr->data)->if_pid = iface;
LL_PREPEND(pkt, hdr);
sendto = gnrc_netreg_lookup(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL);
if (sendto == NULL) {
DEBUG("icmpv6_echo: no receivers for IPv6 packets\n");
gnrc_pktbuf_release(pkt);
return;
}
/* ICMPv6 is not interested anymore so `- 1` */
gnrc_pktbuf_hold(pkt, gnrc_netreg_num(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL) - 1);
while (sendto != NULL) {
if (gnrc_netapi_send(sendto->pid, pkt) < 1) {
DEBUG("icmpv6_echo: unable to send packet\n");
gnrc_pktbuf_release(pkt);
}
sendto = gnrc_netreg_getnext(sendto);
}
}
static void _send_multicast(kernel_pid_t iface, gnrc_pktsnip_t *pkt,
gnrc_pktsnip_t *ipv6, gnrc_pktsnip_t *payload,
bool prep_hdr)
{
kernel_pid_t ifs[GNRC_NETIF_NUMOF];
size_t ifnum = 0;
if (iface == KERNEL_PID_UNDEF) {
/* get list of interfaces */
ifnum = gnrc_netif_get(ifs);
/* throw away packet if no one is interested */
if (ifnum == 0) {
DEBUG("ipv6: no interfaces registered, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
}
#if GNRC_NETIF_NUMOF > 1
/* netif header not present: send over all interfaces */
if (iface == KERNEL_PID_UNDEF) {
assert(pkt == ipv6);
/* send packet to link layer */
gnrc_pktbuf_hold(pkt, ifnum - 1);
for (size_t i = 0; i < ifnum; i++) {
gnrc_pktsnip_t *netif;
if (prep_hdr) {
/* need to get second write access (duplication) to fill IPv6
* header interface-local */
gnrc_pktsnip_t *tmp = gnrc_pktbuf_start_write(pkt);
gnrc_pktsnip_t *ptr = tmp->next;
ipv6 = tmp;
if (ipv6 == NULL) {
DEBUG("ipv6: unable to get write access to IPv6 header, "
"for interface %" PRIkernel_pid "\n", ifs[i]);
gnrc_pktbuf_release(pkt);
return;
}
/* multiple interfaces => possibly different source addresses
* => different checksums => duplication of payload needed */
while (ptr != payload->next) {
/* duplicate everything including payload */
tmp->next = gnrc_pktbuf_start_write(ptr);
if (tmp->next == NULL) {
DEBUG("ipv6: unable to get write access to payload, drop it\n");
gnrc_pktbuf_release(ipv6);
return;
}
tmp = tmp->next;
ptr = ptr->next;
}
if (_fill_ipv6_hdr(ifs[i], ipv6, tmp) < 0) {
/* error on filling up header */
gnrc_pktbuf_release(ipv6);
return;
}
}
/* allocate interface header */
netif = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif == NULL) {
DEBUG("ipv6: error on interface header allocation, "
"dropping packet\n");
gnrc_pktbuf_release(ipv6);
return;
}
LL_PREPEND(ipv6, netif);
_send_multicast_over_iface(ifs[i], ipv6);
}
}
else {
/* iface != KERNEL_PID_UNDEF implies that netif header is present */
assert(pkt != ipv6);
if (prep_hdr) {
if (_fill_ipv6_hdr(iface, ipv6, payload) < 0) {
/* error on filling up header */
gnrc_pktbuf_release(pkt);
return;
}
}
_send_multicast_over_iface(iface, pkt);
}
#else /* GNRC_NETIF_NUMOF */
(void)ifnum; /* not used in this build branch */
if (iface == KERNEL_PID_UNDEF) {
gnrc_pktsnip_t *netif;
iface = ifs[0];
/* allocate interface header */
netif = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif == NULL) {
DEBUG("ipv6: error on interface header allocation, "
"dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
LL_PREPEND(pkt, netif);
}
if (prep_hdr) {
if (_fill_ipv6_hdr(iface, ipv6, payload) < 0) {
/* error on filling up header */
gnrc_pktbuf_release(pkt);
return;
}
}
_send_multicast_over_iface(iface, pkt);
#endif /* GNRC_NETIF_NUMOF */
}
static void _send_multicast(kernel_pid_t iface, gnrc_pktsnip_t *pkt,
gnrc_pktsnip_t *ipv6, gnrc_pktsnip_t *payload,
bool prep_hdr)
{
gnrc_pktsnip_t *netif;
kernel_pid_t ifs[GNRC_NETIF_NUMOF];
size_t ifnum = 0;
if (iface == KERNEL_PID_UNDEF) {
/* get list of interfaces */
ifnum = gnrc_netif_get(ifs);
/* throw away packet if no one is interested */
if (ifnum == 0) {
DEBUG("ipv6: no interfaces registered, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
}
#if GNRC_NETIF_NUMOF > 1
/* netif header not present: send over all interfaces */
if (iface == KERNEL_PID_UNDEF) {
/* send packet to link layer */
gnrc_pktbuf_hold(pkt, ifnum - 1);
for (size_t i = 0; i < ifnum; i++) {
if (prep_hdr) {
/* need to get second write access (duplication) to fill IPv6
* header interface-local */
ipv6 = gnrc_pktbuf_start_write(ipv6);
if (ipv6 == NULL) {
DEBUG("ipv6: unable to get write access to IPv6 header, "
"for interface %" PRIkernel_pid "\n", ifs[i]);
gnrc_pktbuf_release(pkt);
return;
}
if (_fill_ipv6_hdr(ifs[i], ipv6, payload) < 0) {
/* error on filling up header */
gnrc_pktbuf_release(pkt);
return;
}
}
/* allocate interface header */
netif = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif == NULL) {
DEBUG("ipv6: error on interface header allocation, "
"dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
LL_PREPEND(pkt, netif);
_send_multicast_over_iface(ifs[i], pkt, netif);
}
}
else {
/* iface != KERNEL_PID_UNDEF implies that netif header is present */
if (prep_hdr) {
if (_fill_ipv6_hdr(iface, ipv6, payload) < 0) {
/* error on filling up header */
gnrc_pktbuf_release(pkt);
return;
}
}
netif = pkt;
_send_multicast_over_iface(iface, pkt, netif);
}
#else /* GNRC_NETIF_NUMOF */
(void)ifnum; /* not used in this build branch */
if (iface == KERNEL_PID_UNDEF) {
iface = ifs[0];
/* allocate interface header */
netif = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif == NULL) {
DEBUG("ipv6: error on interface header allocation, "
"dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
LL_PREPEND(pkt, netif);
}
else {
netif = pkt;
}
if (prep_hdr) {
if (_fill_ipv6_hdr(iface, ipv6, payload) < 0) {
/* error on filling up header */
gnrc_pktbuf_release(pkt);
return;
}
}
_send_multicast_over_iface(iface, pkt, netif);
#endif /* GNRC_NETIF_NUMOF */
}
void rbuf_add(gnrc_netif_hdr_t *netif_hdr, gnrc_pktsnip_t *pkt,
size_t frag_size, size_t offset)
{
rbuf_t *entry;
/* cppcheck-suppress variableScope
* (reason: cppcheck is clearly wrong here) */
unsigned int data_offset = 0;
size_t original_size = frag_size;
sixlowpan_frag_t *frag = pkt->data;
rbuf_int_t *ptr;
uint8_t *data = ((uint8_t *)pkt->data) + sizeof(sixlowpan_frag_t);
_rbuf_gc();
entry = _rbuf_get(gnrc_netif_hdr_get_src_addr(netif_hdr), netif_hdr->src_l2addr_len,
gnrc_netif_hdr_get_dst_addr(netif_hdr), netif_hdr->dst_l2addr_len,
byteorder_ntohs(frag->disp_size) & SIXLOWPAN_FRAG_SIZE_MASK,
byteorder_ntohs(frag->tag));
if (entry == NULL) {
DEBUG("6lo rbuf: reassembly buffer full.\n");
return;
}
ptr = entry->ints;
/* dispatches in the first fragment are ignored */
if (offset == 0) {
if (data[0] == SIXLOWPAN_UNCOMP) {
data++; /* skip 6LoWPAN dispatch */
frag_size--;
}
#ifdef MODULE_GNRC_SIXLOWPAN_IPHC
else if (sixlowpan_iphc_is(data)) {
size_t iphc_len, nh_len = 0;
iphc_len = gnrc_sixlowpan_iphc_decode(&entry->pkt, pkt, entry->pkt->size,
sizeof(sixlowpan_frag_t), &nh_len);
if (iphc_len == 0) {
DEBUG("6lo rfrag: could not decode IPHC dispatch\n");
gnrc_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
data += iphc_len; /* take remaining data as data */
frag_size -= iphc_len; /* and reduce frag size by IPHC dispatch length */
/* but add IPv6 header + next header lengths */
frag_size += sizeof(ipv6_hdr_t) + nh_len;
/* start copying after IPv6 header and next headers */
data_offset += sizeof(ipv6_hdr_t) + nh_len;
}
#endif
}
else {
data++; /* FRAGN header is one byte longer (offset) */
}
if ((offset + frag_size) > entry->pkt->size) {
DEBUG("6lo rfrag: fragment too big for resulting datagram, discarding datagram\n");
gnrc_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
/* If the fragment overlaps another fragment and differs in either the size
* or the offset of the overlapped fragment, discards the datagram
* https://tools.ietf.org/html/rfc4944#section-5.3 */
while (ptr != NULL) {
if (_rbuf_int_overlap_partially(ptr, offset, offset + frag_size - 1)) {
DEBUG("6lo rfrag: overlapping intervals, discarding datagram\n");
gnrc_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
/* "A fresh reassembly may be commenced with the most recently
* received link fragment"
* https://tools.ietf.org/html/rfc4944#section-5.3 */
rbuf_add(netif_hdr, pkt, original_size, offset);
return;
}
ptr = ptr->next;
}
if (_rbuf_update_ints(entry, offset, frag_size)) {
DEBUG("6lo rbuf: add fragment data\n");
entry->cur_size += (uint16_t)frag_size;
memcpy(((uint8_t *)entry->pkt->data) + offset + data_offset, data,
frag_size - data_offset);
}
if (entry->cur_size == entry->pkt->size) {
gnrc_pktsnip_t *netif = gnrc_netif_hdr_build(entry->src, entry->src_len,
entry->dst, entry->dst_len);
if (netif == NULL) {
DEBUG("6lo rbuf: error allocating netif header\n");
gnrc_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
/* copy the transmit information of the latest fragment into the newly
* created header to have some link_layer information. The link_layer
* info of the previous fragments is discarded.
*/
gnrc_netif_hdr_t *new_netif_hdr = netif->data;
new_netif_hdr->if_pid = netif_hdr->if_pid;
new_netif_hdr->flags = netif_hdr->flags;
new_netif_hdr->lqi = netif_hdr->lqi;
new_netif_hdr->rssi = netif_hdr->rssi;
LL_APPEND(entry->pkt, netif);
if (!gnrc_netapi_dispatch_receive(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL,
entry->pkt)) {
DEBUG("6lo rbuf: No receivers for this packet found\n");
gnrc_pktbuf_release(entry->pkt);
}
_rbuf_rem(entry);
}
}
/* send a beacon */
void beaconing_send(void)
{
gnrc_pktsnip_t *pkt, *hdr;
gnrc_netif_hdr_t *nethdr;
/* put packet together */
beacon_t b = { .magic_key = BEACONING_MK, .id = dow_my_id };
pkt = gnrc_pktbuf_add(NULL, &b, sizeof(b), GNRC_NETTYPE_UNDEF);
if (pkt == NULL) {
puts("error: packet buffer full");
return;
}
hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (hdr == NULL) {
puts("error: packet buffer full");
gnrc_pktbuf_release(pkt);
return;
}
LL_PREPEND(pkt, hdr);
nethdr = (gnrc_netif_hdr_t *)hdr->data;
nethdr->flags |= GNRC_NETIF_HDR_FLAGS_BROADCAST;
/* and send it */
LOG_DEBUG("beaconing: send beacon\n");
if (gnrc_netapi_send(CCNLRIOT_NETIF, pkt) < 1) {
puts("error: unable to send");
gnrc_pktbuf_release(pkt);
return;
}
}
