/* 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);
}
static void _decapsulate(gnrc_pktsnip_t *pkt)
{
gnrc_pktsnip_t *ptr = pkt;
pkt->type = GNRC_NETTYPE_UNDEF; /* prevent payload (the encapsulated packet)
* from being removed */
/* Remove encapsulating IPv6 header */
while ((ptr->next != NULL) && (ptr->next->type == GNRC_NETTYPE_IPV6)) {
gnrc_pktbuf_remove_snip(pkt, pkt->next);
}
pkt->type = GNRC_NETTYPE_IPV6;
_receive(pkt);
}
static void test_pktbuf_mark__success_aligned(void)
{
uint8_t *data = (uint8_t *)(TEST_STRING16);
gnrc_pktsnip_t *pkt1 = gnrc_pktbuf_add(NULL, data, sizeof(TEST_STRING16),
GNRC_NETTYPE_TEST);
gnrc_pktsnip_t *pkt2;
uint8_t exp_data1[sizeof(TEST_STRING16) - 8];
uint8_t exp_data2[8];
memcpy(exp_data1, data + sizeof(exp_data2), sizeof(exp_data1));
memcpy(exp_data2, data, sizeof(exp_data2));
TEST_ASSERT_NOT_NULL(pkt1);
TEST_ASSERT_NOT_NULL((pkt2 = gnrc_pktbuf_mark(pkt1, 8, GNRC_NETTYPE_UNDEF)));
TEST_ASSERT(gnrc_pktbuf_is_sane());
TEST_ASSERT(pkt1->next == pkt2);
TEST_ASSERT_NOT_NULL(pkt1->data);
TEST_ASSERT_EQUAL_INT(0, memcmp(exp_data1, pkt1->data, pkt1->size));
TEST_ASSERT_EQUAL_INT(sizeof(TEST_STRING16) - 8,
pkt1->size);
TEST_ASSERT_EQUAL_INT(GNRC_NETTYPE_TEST, pkt1->type);
TEST_ASSERT_EQUAL_INT(1, pkt1->users);
TEST_ASSERT_NULL(pkt2->next);
TEST_ASSERT_NOT_NULL(pkt2->data);
TEST_ASSERT_EQUAL_INT(0, memcmp(exp_data2, pkt2->data, pkt2->size));
TEST_ASSERT_EQUAL_INT(8, pkt2->size);
TEST_ASSERT_EQUAL_INT(GNRC_NETTYPE_UNDEF, pkt2->type);
TEST_ASSERT_EQUAL_INT(1, pkt2->users);
/* check if slightly larger packet would override data */
gnrc_pktbuf_remove_snip(pkt1, pkt2);
pkt2 = gnrc_pktbuf_add(NULL, TEST_STRING12, 12, GNRC_NETTYPE_TEST);
TEST_ASSERT(gnrc_pktbuf_is_sane());
TEST_ASSERT_NOT_NULL(pkt1->data);
TEST_ASSERT_EQUAL_INT(0, memcmp(exp_data1, pkt1->data, pkt1->size));
TEST_ASSERT_EQUAL_INT(sizeof(TEST_STRING16) - 8,
pkt1->size);
TEST_ASSERT_EQUAL_INT(GNRC_NETTYPE_TEST, pkt1->type);
TEST_ASSERT_EQUAL_INT(1, pkt1->users);
/* check if everything can be cleaned up */
gnrc_pktbuf_release(pkt1);
gnrc_pktbuf_release(pkt2);
TEST_ASSERT(gnrc_pktbuf_is_empty());
}
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 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;
}
static gnrc_pktsnip_t *_recv(gnrc_netdev2_t *gnrc_netdev2)
{
netdev2_t *netdev = gnrc_netdev2->dev;
netdev2_ieee802154_rx_info_t rx_info;
netdev2_ieee802154_t *state = (netdev2_ieee802154_t *)gnrc_netdev2->dev;
gnrc_pktsnip_t *pkt = NULL;
int bytes_expected = netdev->driver->recv(netdev, NULL, 0, NULL);
if (bytes_expected > 0) {
int nread;
pkt = gnrc_pktbuf_add(NULL, NULL, bytes_expected, GNRC_NETTYPE_UNDEF);
if (pkt == NULL) {
DEBUG("_recv_ieee802154: cannot allocate pktsnip.\n");
return NULL;
}
nread = netdev->driver->recv(netdev, pkt->data, bytes_expected, &rx_info);
if (nread <= 0) {
gnrc_pktbuf_release(pkt);
return NULL;
}
if (!(state->flags & NETDEV2_IEEE802154_RAW)) {
gnrc_pktsnip_t *ieee802154_hdr, *netif_hdr;
gnrc_netif_hdr_t *hdr;
#if ENABLE_DEBUG
char src_str[GNRC_NETIF_HDR_L2ADDR_MAX_LEN];
#endif
size_t mhr_len = ieee802154_get_frame_hdr_len(pkt->data);
if (mhr_len == 0) {
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;
hdr->lqi = rx_info.lqi;
hdr->rssi = rx_info.rssi;
hdr->if_pid = thread_getpid();
pkt->type = state->proto;
#if ENABLE_DEBUG
DEBUG("_recv_ieee802154: received packet from %s of length %u\n",
gnrc_netif_addr_to_str(src_str, sizeof(src_str),
gnrc_netif_hdr_get_src_addr(hdr),
hdr->src_l2addr_len),
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);
}
return pkt;
}
static void _receive(gnrc_pktsnip_t *pkt)
{
kernel_pid_t iface = KERNEL_PID_UNDEF;
gnrc_pktsnip_t *ipv6, *netif, *first_ext;
ipv6_hdr_t *hdr;
assert(pkt != NULL);
netif = gnrc_pktsnip_search_type(pkt, GNRC_NETTYPE_NETIF);
if (netif != NULL) {
iface = ((gnrc_netif_hdr_t *)netif->data)->if_pid;
#ifdef MODULE_NETSTATS_IPV6
assert(iface);
netstats_t *stats = gnrc_ipv6_netif_get_stats(iface);
stats->rx_count++;
stats->rx_bytes += (gnrc_pkt_len(pkt) - netif->size);
#endif
}
first_ext = pkt;
for (ipv6 = pkt; ipv6 != NULL; ipv6 = ipv6->next) { /* find IPv6 header if already marked */
if ((ipv6->type == GNRC_NETTYPE_IPV6) && (ipv6->size == sizeof(ipv6_hdr_t)) &&
(ipv6_hdr_is(ipv6->data))) {
break;
}
first_ext = ipv6;
}
if (ipv6 == NULL) {
if (!ipv6_hdr_is(pkt->data)) {
DEBUG("ipv6: Received packet was not IPv6, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
#ifdef MODULE_GNRC_IPV6_WHITELIST
if (!gnrc_ipv6_whitelisted(&((ipv6_hdr_t *)(pkt->data))->src)) {
DEBUG("ipv6: Source address not whitelisted, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
#endif
#ifdef MODULE_GNRC_IPV6_BLACKLIST
if (gnrc_ipv6_blacklisted(&((ipv6_hdr_t *)(pkt->data))->src)) {
DEBUG("ipv6: Source address blacklisted, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
#endif
/* seize ipv6 as a temporary variable */
ipv6 = gnrc_pktbuf_start_write(pkt);
if (ipv6 == NULL) {
DEBUG("ipv6: unable to get write access to packet, drop it\n");
gnrc_pktbuf_release(pkt);
return;
}
pkt = ipv6; /* reset pkt from temporary variable */
ipv6 = gnrc_pktbuf_mark(pkt, sizeof(ipv6_hdr_t), GNRC_NETTYPE_IPV6);
first_ext = pkt;
pkt->type = GNRC_NETTYPE_UNDEF; /* snip is no longer IPv6 */
if (ipv6 == NULL) {
DEBUG("ipv6: error marking IPv6 header, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
}
#ifdef MODULE_GNRC_IPV6_WHITELIST
else if (!gnrc_ipv6_whitelisted(&((ipv6_hdr_t *)(ipv6->data))->src)) {
/* if ipv6 header already marked*/
DEBUG("ipv6: Source address not whitelisted, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
#endif
#ifdef MODULE_GNRC_IPV6_BLACKLIST
else if (gnrc_ipv6_blacklisted(&((ipv6_hdr_t *)(ipv6->data))->src)) {
/* if ipv6 header already marked*/
DEBUG("ipv6: Source address blacklisted, dropping packet\n");
gnrc_pktbuf_release(pkt);
return;
}
#endif
/* extract header */
hdr = (ipv6_hdr_t *)ipv6->data;
/* if available, remove any padding that was added by lower layers
* to fulfill their minimum size requirements (e.g. ethernet) */
if (byteorder_ntohs(hdr->len) < pkt->size) {
gnrc_pktbuf_realloc_data(pkt, byteorder_ntohs(hdr->len));
}
else if (byteorder_ntohs(hdr->len) >
(gnrc_pkt_len_upto(pkt, GNRC_NETTYPE_IPV6) - sizeof(ipv6_hdr_t))) {
DEBUG("ipv6: invalid payload length: %d, actual: %d, dropping packet\n",
(int) byteorder_ntohs(hdr->len),
(int) (gnrc_pkt_len_upto(pkt, GNRC_NETTYPE_IPV6) - sizeof(ipv6_hdr_t)));
gnrc_pktbuf_release(pkt);
return;
}
DEBUG("ipv6: Received (src = %s, ",
ipv6_addr_to_str(addr_str, &(hdr->src), sizeof(addr_str)));
DEBUG("dst = %s, next header = %u, length = %" PRIu16 ")\n",
ipv6_addr_to_str(addr_str, &(hdr->dst), sizeof(addr_str)),
hdr->nh, byteorder_ntohs(hdr->len));
if (_pkt_not_for_me(&iface, hdr)) { /* if packet is not for me */
DEBUG("ipv6: packet destination not this host\n");
#ifdef MODULE_GNRC_IPV6_ROUTER /* only routers redirect */
/* redirect to next hop */
DEBUG("ipv6: decrement hop limit to %u\n", (uint8_t) (hdr->hl - 1));
/* RFC 4291, section 2.5.6 states: "Routers must not forward any
* packets with Link-Local source or destination addresses to other
* links."
*/
if ((ipv6_addr_is_link_local(&(hdr->src))) || (ipv6_addr_is_link_local(&(hdr->dst)))) {
DEBUG("ipv6: do not forward packets with link-local source or"
" destination address\n");
gnrc_pktbuf_release(pkt);
return;
}
/* TODO: check if receiving interface is router */
else if (--(hdr->hl) > 0) { /* drop packets that *reach* Hop Limit 0 */
gnrc_pktsnip_t *reversed_pkt = NULL, *ptr = pkt;
DEBUG("ipv6: forward packet to next hop\n");
/* pkt might not be writable yet, if header was given above */
ipv6 = gnrc_pktbuf_start_write(ipv6);
if (ipv6 == NULL) {
DEBUG("ipv6: unable to get write access to packet: dropping it\n");
gnrc_pktbuf_release(pkt);
return;
}
/* remove L2 headers around IPV6 */
netif = gnrc_pktsnip_search_type(pkt, GNRC_NETTYPE_NETIF);
if (netif != NULL) {
gnrc_pktbuf_remove_snip(pkt, netif);
}
/* reverse packet snip list order */
while (ptr != NULL) {
gnrc_pktsnip_t *next;
ptr = gnrc_pktbuf_start_write(ptr); /* duplicate if not already done */
if (ptr == NULL) {
DEBUG("ipv6: unable to get write access to packet: dropping it\n");
gnrc_pktbuf_release(reversed_pkt);
gnrc_pktbuf_release(pkt);
return;
}
next = ptr->next;
ptr->next = reversed_pkt;
reversed_pkt = ptr;
ptr = next;
}
_send(reversed_pkt, false);
return;
}
else {
DEBUG("ipv6: hop limit reached 0: drop packet\n");
gnrc_pktbuf_release(pkt);
return;
}
#else /* MODULE_GNRC_IPV6_ROUTER */
DEBUG("ipv6: dropping packet\n");
/* non rounting hosts just drop the packet */
gnrc_pktbuf_release(pkt);
return;
#endif /* MODULE_GNRC_IPV6_ROUTER */
}
/* IPv6 internal demuxing (ICMPv6, Extension headers etc.) */
gnrc_ipv6_demux(iface, first_ext, pkt, hdr->nh);
}
static gnrc_pktsnip_t *_recv(gnrc_netdev2_t *gnrc_netdev2)
{
netdev2_t *dev = gnrc_netdev2->dev;
int bytes_expected = dev->driver->recv(dev, NULL, 0);
gnrc_pktsnip_t *pkt = NULL;
if (bytes_expected) {
pkt = gnrc_pktbuf_add(NULL, NULL,
bytes_expected,
GNRC_NETTYPE_UNDEF);
if(!pkt) {
DEBUG("_recv_ethernet_packet: cannot allocate pktsnip.\n");
goto out;
}
int nread = dev->driver->recv(dev, pkt->data, bytes_expected);
if(nread <= 0) {
DEBUG("_recv_ethernet_packet: read error.\n");
goto safe_out;
}
if (nread < bytes_expected) {
/* we've got less then the expected packet size,
* so free the unused space.*/
DEBUG("_recv_ethernet_packet: reallocating.\n");
gnrc_pktbuf_realloc_data(pkt, nread);
}
/* mark ethernet header */
gnrc_pktsnip_t *eth_hdr = gnrc_pktbuf_mark(pkt, sizeof(ethernet_hdr_t), GNRC_NETTYPE_UNDEF);
if (!eth_hdr) {
DEBUG("gnrc_netdev2_eth: no space left in packet buffer\n");
goto safe_out;
}
ethernet_hdr_t *hdr = (ethernet_hdr_t *)eth_hdr->data;
/* set payload type from ethertype */
pkt->type = gnrc_nettype_from_ethertype(byteorder_ntohs(hdr->type));
/* create netif header */
gnrc_pktsnip_t *netif_hdr;
netif_hdr = gnrc_pktbuf_add(NULL, NULL,
sizeof(gnrc_netif_hdr_t) + (2 * ETHERNET_ADDR_LEN),
GNRC_NETTYPE_NETIF);
if (netif_hdr == NULL) {
DEBUG("gnrc_netdev2_eth: no space left in packet buffer\n");
pkt = eth_hdr;
goto safe_out;
}
gnrc_netif_hdr_init(netif_hdr->data, ETHERNET_ADDR_LEN, ETHERNET_ADDR_LEN);
gnrc_netif_hdr_set_src_addr(netif_hdr->data, hdr->src, ETHERNET_ADDR_LEN);
gnrc_netif_hdr_set_dst_addr(netif_hdr->data, hdr->dst, ETHERNET_ADDR_LEN);
((gnrc_netif_hdr_t *)netif_hdr->data)->if_pid = thread_getpid();
DEBUG("gnrc_netdev2_eth: received packet from %02x:%02x:%02x:%02x:%02x:%02x "
"of length %zu\n",
hdr->src[0], hdr->src[1], hdr->src[2], hdr->src[3], hdr->src[4],
hdr->src[5], nread);
#if defined(MODULE_OD) && ENABLE_DEBUG
od_hex_dump(hdr, nread, OD_WIDTH_DEFAULT);
#endif
gnrc_pktbuf_remove_snip(pkt, eth_hdr);
LL_APPEND(pkt, netif_hdr);
}
out:
return pkt;
safe_out:
gnrc_pktbuf_release(pkt);
return NULL;
}
static void _receive(gnrc_pktsnip_t *pkt)
{
gnrc_pktsnip_t *payload;
uint8_t *dispatch;
/* seize payload as a temporary variable */
payload = gnrc_pktbuf_start_write(pkt); /* need to duplicate since pkt->next
* might get replaced */
if (payload == NULL) {
DEBUG("6lo: can not get write access on received packet\n");
#if defined(DEVELHELP) && ENABLE_DEBUG
gnrc_pktbuf_stats();
#endif
gnrc_pktbuf_release(pkt);
return;
}
pkt = payload; /* reset pkt from temporary variable */
payload = gnrc_pktsnip_search_type(pkt, GNRC_NETTYPE_SIXLOWPAN);
if ((payload == NULL) || (payload->size < 1)) {
DEBUG("6lo: Received packet has no 6LoWPAN payload\n");
gnrc_pktbuf_release(pkt);
return;
}
dispatch = payload->data;
if (dispatch[0] == SIXLOWPAN_UNCOMP) {
gnrc_pktsnip_t *sixlowpan;
DEBUG("6lo: received uncompressed IPv6 packet\n");
payload = gnrc_pktbuf_start_write(payload);
if (payload == NULL) {
DEBUG("6lo: can not get write access on received packet\n");
#if defined(DEVELHELP) && ENABLE_DEBUG
gnrc_pktbuf_stats();
#endif
gnrc_pktbuf_release(pkt);
return;
}
/* packet is uncompressed: just mark and remove the dispatch */
sixlowpan = gnrc_pktbuf_mark(payload, sizeof(uint8_t), GNRC_NETTYPE_SIXLOWPAN);
if (sixlowpan == NULL) {
DEBUG("6lo: can not mark 6LoWPAN dispatch\n");
gnrc_pktbuf_release(pkt);
return;
}
pkt = gnrc_pktbuf_remove_snip(pkt, sixlowpan);
payload->type = GNRC_NETTYPE_IPV6;
}
#ifdef MODULE_GNRC_SIXLOWPAN_FRAG
else if (sixlowpan_frag_is((sixlowpan_frag_t *)dispatch)) {
DEBUG("6lo: received 6LoWPAN fragment\n");
gnrc_sixlowpan_frag_handle_pkt(pkt);
return;
}
#endif
#ifdef MODULE_GNRC_SIXLOWPAN_IPHC
else if (sixlowpan_iphc_is(dispatch)) {
size_t dispatch_size, nh_len;
gnrc_pktsnip_t *sixlowpan;
gnrc_pktsnip_t *dec_hdr = gnrc_pktbuf_add(NULL, NULL, sizeof(ipv6_hdr_t),
GNRC_NETTYPE_IPV6);
if ((dec_hdr == NULL) ||
(dispatch_size = gnrc_sixlowpan_iphc_decode(&dec_hdr, pkt, 0, 0,
&nh_len)) == 0) {
DEBUG("6lo: error on IPHC decoding\n");
if (dec_hdr != NULL) {
gnrc_pktbuf_release(dec_hdr);
}
gnrc_pktbuf_release(pkt);
return;
}
sixlowpan = gnrc_pktbuf_mark(pkt, dispatch_size, GNRC_NETTYPE_SIXLOWPAN);
if (sixlowpan == NULL) {
DEBUG("6lo: error on marking IPHC dispatch\n");
gnrc_pktbuf_release(dec_hdr);
gnrc_pktbuf_release(pkt);
return;
}
/* Remove IPHC dispatches */
/* Insert decoded header instead */
pkt = gnrc_pktbuf_replace_snip(pkt, sixlowpan, dec_hdr);
payload->type = GNRC_NETTYPE_UNDEF;
}
#endif
else {
DEBUG("6lo: dispatch %02" PRIx8 " ... is not supported\n",
dispatch[0]);
gnrc_pktbuf_release(pkt);
return;
}
if (!gnrc_netapi_dispatch_receive(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL, pkt)) {
DEBUG("6lo: No receivers for this packet found\n");
gnrc_pktbuf_release(pkt);
}
}
static int _parse_packet(gnrc_netif_t *netif, gnrc_pktsnip_t *pkt,
gnrc_gomach_packet_info_t *info)
{
assert(info != NULL);
assert(pkt != NULL);
netdev_ieee802154_t *state = (netdev_ieee802154_t *)netif->dev;
/* Get the packet sequence number */
info->seq = ieee802154_get_seq(pkt->next->data);
gnrc_pktsnip_t *netif_snip = _make_netif_hdr(pkt->next->data);
if (netif_snip == NULL) {
DEBUG("gomach: no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return -ENODATA;
}
gnrc_netif_hdr_t *netif_hdr = netif_snip->data;
netif_hdr->lqi = netif->mac.prot.gomach.rx_pkt_lqi;
netif_hdr->rssi = netif->mac.prot.gomach.rx_pkt_rssi;
netif_hdr->if_pid = netif->pid;
pkt->type = state->proto;
gnrc_pktbuf_remove_snip(pkt, pkt->next);
LL_APPEND(pkt, netif_snip);
gnrc_pktsnip_t *gomach_snip = NULL;
gnrc_gomach_hdr_t *gomach_hdr = NULL;
netif_snip = gnrc_pktsnip_search_type(pkt, GNRC_NETTYPE_NETIF);
if (netif_snip == NULL) {
return -ENODATA;
}
else {
netif_hdr = netif_snip->data;
}
if (netif_hdr->dst_l2addr_len > sizeof(info->dst_addr)) {
return -ENODATA;
}
if (netif_hdr->src_l2addr_len > sizeof(info->src_addr)) {
return -ENODATA;
}
/* Dissect GoMacH header, Every frame has header as first member */
gomach_hdr = (gnrc_gomach_hdr_t *) pkt->data;
switch (gomach_hdr->type) {
case GNRC_GOMACH_FRAME_BEACON: {
gomach_snip = gnrc_pktbuf_mark(pkt, sizeof(gnrc_gomach_frame_beacon_t),
GNRC_NETTYPE_GOMACH);
break;
}
case GNRC_GOMACH_FRAME_PREAMBLE: {
gomach_snip = gnrc_pktbuf_mark(pkt, sizeof(gnrc_gomach_frame_preamble_t),
GNRC_NETTYPE_GOMACH);
break;
}
case GNRC_GOMACH_FRAME_PREAMBLE_ACK: {
gomach_snip = gnrc_pktbuf_mark(pkt, sizeof(gnrc_gomach_frame_preamble_ack_t),
GNRC_NETTYPE_GOMACH);
break;
}
case GNRC_GOMACH_FRAME_DATA: {
gomach_snip = gnrc_pktbuf_mark(pkt, sizeof(gnrc_gomach_frame_data_t),
GNRC_NETTYPE_GOMACH);
break;
}
case GNRC_GOMACH_FRAME_ANNOUNCE: {
gomach_snip = gnrc_pktbuf_mark(pkt, sizeof(gnrc_gomach_frame_announce_t),
GNRC_NETTYPE_GOMACH);
break;
}
case GNRC_GOMACH_FRAME_BROADCAST: {
gomach_snip = gnrc_pktbuf_mark(pkt, sizeof(gnrc_gomach_frame_broadcast_t),
GNRC_NETTYPE_GOMACH);
break;
}
default: {
return -ENODATA;
}
}
/* Memory location may have changed while marking. */
gomach_hdr = gomach_snip->data;
/* Get the destination address. */
switch (gomach_hdr->type) {
case GNRC_GOMACH_FRAME_PREAMBLE: {
info->dst_addr = ((gnrc_gomach_frame_preamble_t *)gomach_hdr)->dst_addr;
break;
}
case GNRC_GOMACH_FRAME_PREAMBLE_ACK: {
info->dst_addr = ((gnrc_gomach_frame_preamble_ack_t *)gomach_hdr)->dst_addr;
break;
}
case GNRC_GOMACH_FRAME_DATA: {
if (netif_hdr->dst_l2addr_len) {
info->dst_addr.len = netif_hdr->dst_l2addr_len;
memcpy(info->dst_addr.addr,
gnrc_netif_hdr_get_dst_addr(netif_hdr),
netif_hdr->dst_l2addr_len);
}
break;
}
default: {
break;
}
}
/* Get the source address. */
if (netif_hdr->src_l2addr_len) {
info->src_addr.len = netif_hdr->src_l2addr_len;
memcpy(info->src_addr.addr,
gnrc_netif_hdr_get_src_addr(netif_hdr),
netif_hdr->src_l2addr_len);
}
info->header = gomach_hdr;
return 0;
}
