From 799fe7a5ba8eb3277e7c972c5c9db7459ac8db64 Mon Sep 17 00:00:00 2001 From: Dirk Ziegelmeier Date: Tue, 23 Feb 2016 08:58:34 +0100 Subject: [PATCH] Fix shadowing warning in lowpan6.c - found by Erik Ekman's Travis CI https://travis-ci.org/yarrick/lwip-merged --- src/netif/lowpan6.c | 2716 +++++++++++++++++++++---------------------- 1 file changed, 1358 insertions(+), 1358 deletions(-) diff --git a/src/netif/lowpan6.c b/src/netif/lowpan6.c index 05c913f0..695f72bb 100644 --- a/src/netif/lowpan6.c +++ b/src/netif/lowpan6.c @@ -1,1358 +1,1358 @@ -/** - * @file - * - * 6LowPAN output for IPv6. Uses ND tables for link-layer addressing. Fragments packets to 6LowPAN units. - */ - -/* - * Copyright (c) 2015 Inico Technologies Ltd. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. The name of the author may not be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT - * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT - * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING - * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY - * OF SUCH DAMAGE. - * - * This file is part of the lwIP TCP/IP stack. - * - * Author: Ivan Delamer - * - * - * Please coordinate changes and requests with Ivan Delamer - * - */ - -#include "netif/lowpan6.h" - -#if LWIP_IPV6 && LWIP_6LOWPAN - -#include "lwip/ip.h" -#include "lwip/pbuf.h" -#include "lwip/ip_addr.h" -#include "lwip/netif.h" -#include "lwip/nd6.h" -#include "lwip/mem.h" -#include "lwip/udp.h" -#include "lwip/tcpip.h" - -struct ieee_802154_addr { - u8_t addr_len; - u8_t addr[8]; -}; - -/** This is a helper struct. - */ -struct lowpan6_reass_helper { - struct pbuf *pbuf; - struct lowpan6_reass_helper *next_packet; - u8_t timer; - struct ieee_802154_addr sender_addr; - u16_t datagram_size; - u16_t datagram_tag; -}; - -static struct lowpan6_reass_helper * reass_list; - -#if LWIP_6LOWPAN_NUM_CONTEXTS > 0 -static ip6_addr_t lowpan6_context[LWIP_6LOWPAN_NUM_CONTEXTS]; -#endif - -static u16_t ieee_802154_pan_id; - -static const struct ieee_802154_addr ieee_802154_broadcast = {2, {0xff, 0xff}}; - -#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS -static struct ieee_802154_addr short_mac_addr = {2, {0,0}}; -#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ - -static err_t dequeue_datagram(struct lowpan6_reass_helper *lrh); - -/** - * Periodic timer for 6LowPAN functions: - * - * - Remove incomplete/old packets - */ -void -lowpan6_tmr(void) -{ - struct lowpan6_reass_helper *lrh, *lrh_temp; - - lrh = reass_list; - while (lrh != NULL) - { - lrh_temp = lrh->next_packet; - if ((--lrh->timer) == 0) - { - dequeue_datagram(lrh); - pbuf_free(lrh->pbuf); - mem_free(lrh); - } - lrh = lrh_temp; - } -} - -/** - * Removes a datagram from the reassembly queue. - **/ -static err_t -dequeue_datagram(struct lowpan6_reass_helper *lrh) -{ - struct lowpan6_reass_helper *lrh_temp; - - if (reass_list == lrh) - { - reass_list = reass_list->next_packet; - } - else - { - lrh_temp = reass_list; - while (lrh_temp != NULL) - { - if (lrh_temp->next_packet == lrh) - { - lrh_temp->next_packet = lrh->next_packet; - break; - } - lrh_temp = lrh_temp->next_packet; - } - } - - return ERR_OK; -} - -static s8_t -lowpan6_context_lookup(const ip6_addr_t *ip6addr) -{ - s8_t i; - - for (i = 0; i < LWIP_6LOWPAN_NUM_CONTEXTS; i++) - { - if (ip6_addr_netcmp(&lowpan6_context[i], ip6addr)) - return i; - } - - return -1; -} - -/* Determine compression mode for unicast address. */ -static s8_t -lowpan6_get_address_mode(const ip6_addr_t *ip6addr, const struct ieee_802154_addr *mac_addr) -{ - if (mac_addr->addr_len == 2) - { - if ((ip6addr->addr[2] == (u32_t)PP_HTONL(0x000000ff)) && - ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000))) - { - if ((ip6addr->addr[3] & PP_HTONL(0x0000ffff)) == ntohl((mac_addr->addr[0] << 8) | mac_addr->addr[1])) - { - return 3; - } - } - } - else if (mac_addr->addr_len == 8) - { - if ((ip6addr->addr[2] == ntohl(((mac_addr->addr[0] ^ 2) << 24) | (mac_addr->addr[1] << 16) | mac_addr->addr[2] << 8 | mac_addr->addr[3])) && - (ip6addr->addr[3] == ntohl((mac_addr->addr[4] << 24) | (mac_addr->addr[5] << 16) | mac_addr->addr[6] << 8 | mac_addr->addr[7]))) - { - return 3; - } - } - - if ((ip6addr->addr[2] == PP_HTONL(0x000000ffUL)) && - ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000UL))) - { - return 2; - } - - return 1; -} - -/* Determine compression mode for multicast address. */ -static s8_t -lowpan6_get_address_mode_mc(const ip6_addr_t *ip6addr) -{ - if ((ip6addr->addr[0] == PP_HTONL(0xff020000)) && - (ip6addr->addr[1] == 0) && - (ip6addr->addr[2] == 0) && - ((ip6addr->addr[3] & PP_HTONL(0xffffff00)) == 0)) - { - return 3; - } - else if (((ip6addr->addr[0] & PP_HTONL(0xff00ffff)) == PP_HTONL(0xff000000)) && - (ip6addr->addr[1] == 0)) - { - if ((ip6addr->addr[2] == 0) && - ((ip6addr->addr[3] & PP_HTONL(0xff000000)) == 0)) - { - return 2; - } - else if ((ip6addr->addr[2] & PP_HTONL(0xffffff00)) == 0) - { - return 1; - } - } - - - return 0; -} - -/* - * Encapsulates data into IEEE 802.15.4 frames. - * Fragments an IPv6 datagram into 6LowPAN units, which fit into IEEE 802.15.4 frames. - * If configured, will compress IPv6 and or UDP headers. - * */ -static err_t -lowpan6_frag(struct netif *netif, struct pbuf *p, const struct ieee_802154_addr *src, const struct ieee_802154_addr *dst) -{ - struct pbuf * p_frag; - u16_t frag_len, remaining_len; - u8_t * buffer; - u8_t ieee_header_len; - u8_t lowpan6_header_len; - s8_t i; - static u8_t frame_seq_num; - static u16_t datagram_tag; - u16_t datagram_offset; - err_t err = ERR_IF; - - /* We'll use a dedicated pbuf for building 6LowPAN fragments. */ - p_frag = pbuf_alloc(PBUF_RAW, 127, PBUF_RAM); - if (p_frag == NULL) { - return ERR_MEM; - } - - /* Write IEEE 802.15.4 header. */ - buffer = (u8_t*)p_frag->payload; - ieee_header_len = 0; - if (dst == &ieee_802154_broadcast) - { - buffer[ieee_header_len++] = 0x01; /* data packet, no ack required. */ - } - else - { - buffer[ieee_header_len++] = 0x21; /* data packet, ack required. */ - } - buffer[ieee_header_len] = (0x00 << 4); /* 2003 frame version */ - buffer[ieee_header_len] |= (dst->addr_len == 2) ? (0x02 << 2) : (0x03 << 2); /* destination addressing mode */ - buffer[ieee_header_len] |= (src->addr_len == 2) ? (0x02 << 6) : (0x03 << 6); /* source addressing mode */ - ieee_header_len++; - buffer[ieee_header_len++] = frame_seq_num++; - - buffer[ieee_header_len++] = ieee_802154_pan_id & 0xff; /* pan id */ - buffer[ieee_header_len++] = (ieee_802154_pan_id >> 8) & 0xff; /* pan id */ - i = dst->addr_len; - while (i-- > 0) { - buffer[ieee_header_len++] = dst->addr[i]; - } - - buffer[ieee_header_len++] = ieee_802154_pan_id & 0xff; /* pan id */ - buffer[ieee_header_len++] = (ieee_802154_pan_id >> 8) & 0xff; /* pan id */ - i = src->addr_len; - while (i-- > 0) { - buffer[ieee_header_len++] = src->addr[i]; - } - -#if LWIP_6LOWPAN_IPHC - /* Perform 6LowPAN IPv6 header compression according to RFC 6282 */ - { - struct ip6_hdr *ip6hdr; - - /* Point to ip6 header and align copies of src/dest addresses. */ - ip6hdr = (struct ip6_hdr *)p->payload; - ip_addr_copy_from_ip6(ip_data.current_iphdr_dest, ip6hdr->dest); - ip_addr_copy_from_ip6(ip_data.current_iphdr_src, ip6hdr->src); - - /* Basic length of 6LowPAN header, set dispatch and clear fields. */ - lowpan6_header_len = 2; - buffer[ieee_header_len] = 0x60; - buffer[ieee_header_len + 1] = 0; - - /* Determine whether there will be a Context Identifier Extension byte or not. - * If so, set it already. */ -#if LWIP_6LOWPAN_NUM_CONTEXTS > 0 - buffer[ieee_header_len + 2] = 0; - - i = lowpan6_context_lookup(ip_2_ip6(&ip_data.current_iphdr_src)); - if (i >= 0) - { - /* Stateful source address compression. */ - buffer[ieee_header_len + 1] |= 0x40; - buffer[ieee_header_len + 2] |= (i & 0x0f) << 4; - } - - i = lowpan6_context_lookup(ip_2_ip6(&ip_data.current_iphdr_dest)); - if (i >= 0) - { - /* Stateful destination address compression. */ - buffer[ieee_header_len + 1] |= 0x04; - buffer[ieee_header_len + 2] |= i & 0x0f; - } - - if (buffer[ieee_header_len + 2] != 0x00) - { - /* Context identifier extension byte is appended. */ - buffer[ieee_header_len + 1] |= 0x80; - lowpan6_header_len++; - } -#endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */ - - /* Determine TF field: Traffic Class, Flow Label */ - if (IP6H_FL(ip6hdr) == 0) - { - /* Flow label is elided. */ - buffer[ieee_header_len] |= 0x10; - if (IP6H_TC(ip6hdr) == 0) - { - /* Traffic class (ECN+DSCP) elided too. */ - buffer[ieee_header_len] |= 0x08; - } - else - { - /* Traffic class (ECN+DSCP) appended. */ - buffer[ieee_header_len + lowpan6_header_len++] = IP6H_TC(ip6hdr); - } - } - else - { - if (((IP6H_TC(ip6hdr) & 0x3f) == 0)) - { - /* DSCP portion of Traffic Class is elided, ECN and FL are appended (3 bytes) */ - buffer[ieee_header_len] |= 0x08; - - buffer[ieee_header_len + lowpan6_header_len] = IP6H_TC(ip6hdr) & 0xc0; - buffer[ieee_header_len + lowpan6_header_len++] |= (IP6H_FL(ip6hdr) >> 16) & 0x0f; - buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff; - buffer[ieee_header_len + lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff; - } - else - { - /* Traffic class and flow label are appended (4 bytes) */ - buffer[ieee_header_len + lowpan6_header_len++] = IP6H_TC(ip6hdr); - buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 16) & 0x0f; - buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff; - buffer[ieee_header_len + lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff; - } - } - - /* Compress NH? - * Only if UDP for now. TODO support other NH compression. */ - if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) - { - buffer[ieee_header_len] |= 0x04; - } - else - { - /* append nexth. */ - buffer[ieee_header_len + lowpan6_header_len++] = IP6H_NEXTH(ip6hdr); - } - - /* Compress hop limit? */ - if (IP6H_HOPLIM(ip6hdr) == 255) - { - buffer[ieee_header_len] |= 0x03; - } - else if (IP6H_HOPLIM(ip6hdr) == 64) - { - buffer[ieee_header_len] |= 0x02; - } - else if (IP6H_HOPLIM(ip6hdr) == 1) - { - buffer[ieee_header_len] |= 0x01; - } - else - { - /* append hop limit */ - buffer[ieee_header_len + lowpan6_header_len++] = IP6H_HOPLIM(ip6hdr); - } - - /* Compress source address */ - if (((buffer[ieee_header_len + 1] & 0x40) != 0) || (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_src)))) - { - /* Context-based or link-local source address compression. */ - i = lowpan6_get_address_mode(ip_2_ip6(&ip_data.current_iphdr_src), src); - buffer[ieee_header_len + 1] |= (i & 0x03) << 4; - if (i == 1) - { - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 16, 8); - lowpan6_header_len += 8; - } - else if (i == 2) - { - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 22, 2); - lowpan6_header_len += 2; - } - } - else if (ip6_addr_isany(ip_2_ip6(&ip_data.current_iphdr_src))) - { - /* Special case: mark SAC and leave SAM=0 */ - buffer[ieee_header_len + 1] |= 0x40; - } - else - { - /* Append full address. */ - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 8, 16); - lowpan6_header_len += 16; - } - - /* Compress destination address */ - if (ip6_addr_ismulticast(ip_2_ip6(&ip_data.current_iphdr_dest))) - { - /* TODO support statefull multicast address compression */ - - buffer[ieee_header_len + 1] |= 0x08; - - i = lowpan6_get_address_mode_mc(ip_2_ip6(&ip_data.current_iphdr_dest)); - buffer[ieee_header_len + 1] |= i & 0x03; - if (i == 0) - { - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 24, 16); - lowpan6_header_len += 16; - } - else if (i == 1) - { - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[25]; - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 35, 5); - lowpan6_header_len += 5; - } - else if (i == 2) - { - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[25]; - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 37, 3); - lowpan6_header_len += 3; - } - else if (i == 3) - { - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[39]; - } - } - else if (((buffer[ieee_header_len + 1] & 0x04) != 0) || (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_dest)))) - { - /* Context-based or link-local destination address compression. */ - i = lowpan6_get_address_mode(ip_2_ip6(&ip_data.current_iphdr_dest), dst); - buffer[ieee_header_len + 1] |= i & 0x03; - if (i == 1) - { - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 32, 8); - lowpan6_header_len += 8; - } - else if (i == 2) - { - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 38, 2); - lowpan6_header_len += 2; - } - } - else - { - /* Append full address. */ - MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 24, 16); - lowpan6_header_len += 16; - } - - /* Move to payload. */ - pbuf_header(p, -IP6_HLEN); - - /* Compress UDP header? */ - if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) - { - /* TODO support optional checksum compression */ - - buffer[ieee_header_len + lowpan6_header_len] = 0xf0; - - /* determine port compression mode. */ - if ((((u8_t *)p->payload)[0] == 0xf0) && ((((u8_t *)p->payload)[1] & 0xf0) == 0xb0) && - (((u8_t *)p->payload)[2] == 0xf0) && ((((u8_t *)p->payload)[3] & 0xf0) == 0xb0)) - { - /* Compress source and dest ports. */ - buffer[ieee_header_len + lowpan6_header_len++] |= 0x03; - buffer[ieee_header_len + lowpan6_header_len++] = ((((u8_t *)p->payload)[1] & 0x0f) << 4) | (((u8_t *)p->payload)[3] & 0x0f); - } - else if (((u8_t *)p->payload)[0] == 0xf0) - { - /* Compress source port. */ - buffer[ieee_header_len + lowpan6_header_len++] |= 0x02; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2]; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; - } - else if (((u8_t *)p->payload)[2] == 0xf0) - { - /* Compress dest port. */ - buffer[ieee_header_len + lowpan6_header_len++] |= 0x01; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0]; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; - } - else - { - /* append full ports. */ - lowpan6_header_len++; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0]; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2]; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; - } - - /* elide length and copy checksum */ - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[6]; - buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[7]; - - pbuf_header(p, -UDP_HLEN); - } - } - -#else /* LWIP_6LOWPAN_HC */ - /* Send uncompressed IPv6 header with appropriate dispatch byte. */ - lowpan6_header_len = 1; - buffer[ieee_header_len] = 0x41; /* IPv6 dispatch */ -#endif /* LWIP_6LOWPAN_HC */ - - /* Calculate remaining packet length */ - remaining_len = p->tot_len; - - /* Fragment, or 1 packet? */ - if (remaining_len > (127 - ieee_header_len - lowpan6_header_len - 3)) /* 127 - header - 1 byte dispatch - 2 bytes CRC */ - { - /* We must move the 6LowPAN header to make room for the FRAG header. */ - i = lowpan6_header_len; - while (i-- != 0) - { - buffer[ieee_header_len + i + 4] = buffer[ieee_header_len + i]; - } - - /* Now we need to fragment the packet. FRAG1 header first */ - buffer[ieee_header_len] = 0xc0 | (((p->tot_len + lowpan6_header_len) >> 8) & 0x7); - buffer[ieee_header_len + 1] = (p->tot_len + lowpan6_header_len) & 0xff; - - datagram_tag++; - buffer[ieee_header_len + 2] = datagram_tag & 0xff; - buffer[ieee_header_len + 3] = (datagram_tag >> 8) & 0xff; - - /* Fragment follows. */ - frag_len = (127 - ieee_header_len - 4 - 2) & 0xf8; - - pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len + 4, frag_len - lowpan6_header_len, 0); - remaining_len -= frag_len - lowpan6_header_len; - datagram_offset = frag_len; - - /* 2 bytes CRC */ -#if LWIP_6LOWPAN_HW_CRC - /* Leave blank, will be filled by HW. */ -#else /* LWIP_6LOWPAN_HW_CRC */ - /* TODO calculate CRC */ -#endif /* LWIP_6LOWPAN_HW_CRC */ - - /* Calculate frame length */ - p_frag->len = p_frag->tot_len = ieee_header_len + 4 + frag_len + 2; /* add 2 dummy bytes for crc*/ - - /* send the packet */ - LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); - err = netif->linkoutput(netif, p_frag); - - while ((remaining_len > 0) && (err == ERR_OK)) - { - /* new frame, new seq num for ACK */ - buffer[2] = frame_seq_num++; - - buffer[ieee_header_len] |= 0x20; /* Change FRAG1 to FRAGN */ - - buffer[ieee_header_len + 4] = datagram_offset >> 3; /* datagram offset in FRAGN header */ - - frag_len = (127 - ieee_header_len - 5 - 2) & 0xf8; - if (frag_len > remaining_len) - frag_len = remaining_len; - - pbuf_copy_partial(p, buffer + ieee_header_len + 5, frag_len, p->tot_len - remaining_len); - remaining_len -= frag_len; - datagram_offset += frag_len; - - /* 2 bytes CRC */ -#if LWIP_6LOWPAN_HW_CRC - /* Leave blank, will be filled by HW. */ -#else /* LWIP_6LOWPAN_HW_CRC */ - /* TODO calculate CRC */ -#endif /* LWIP_6LOWPAN_HW_CRC */ - - /* Calculate frame length */ - p_frag->len = p_frag->tot_len = frag_len + 5 + ieee_header_len + 2; - - /* send the packet */ - LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); - err = netif->linkoutput(netif, p_frag); - } - } - else - { - /* It fits in one frame. */ - frag_len = remaining_len; - - /* Copy IPv6 packet */ - pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len, frag_len, 0); - remaining_len = 0; - - /* 2 bytes CRC */ -#if LWIP_6LOWPAN_HW_CRC - /* Leave blank, will be filled by HW. */ -#else /* LWIP_6LOWPAN_HW_CRC */ - /* TODO calculate CRC */ -#endif /* LWIP_6LOWPAN_HW_CRC */ - - /* Calculate frame length */ - p_frag->len = p_frag->tot_len = frag_len + lowpan6_header_len + ieee_header_len + 2; - - /* send the packet */ - LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); - err = netif->linkoutput(netif, p_frag); - } - - - pbuf_free(p_frag); - - return err; -} - -err_t -lowpan6_set_context(u8_t index, const ip6_addr_t * context) -{ - if (index > LWIP_6LOWPAN_NUM_CONTEXTS) - return ERR_ARG; - - ip6_addr_set(&lowpan6_context[index], context); - - return ERR_OK; -} - -#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS -err_t -lowpan6_set_short_addr(u8_t addr_high, u8_t addr_low) -{ - short_mac_addr.addr[0] = addr_high; - short_mac_addr.addr[1] = addr_low; - - return ERR_OK; -} -#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ - -#if LWIP_IPV4 -err_t -lowpan4_output(struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr) -{ - (void)netif; - (void)q; - (void)ipaddr; - - return ERR_IF; -} -#endif /* LWIP_IPV4 */ - -/** - * Resolve and fill-in IEEE 802.15.4 address header for outgoing IPv6 packet. - * - * Perform Header Compression and fragment if necessary. - * - * @param netif The lwIP network interface which the IP packet will be sent on. - * @param q The pbuf(s) containing the IP packet to be sent. - * @param ip6addr The IP address of the packet destination. - * - * @return - */ -err_t -lowpan6_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr) -{ - s8_t i; - struct ieee_802154_addr src, dest; -#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS - ip6_addr_t ip6_src; - struct ip6_hdr * ip6_hdr; -#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ - -#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS - /* Check if we can compress source address (use aligned copy) */ - ip6_hdr = (struct ip6_hdr *)q->payload; - ip6_addr_set(&ip6_src, &ip6_hdr->src); - if (lowpan6_get_address_mode(&ip6_src, &short_mac_addr) == 3) - { - src.addr_len = 2; - src.addr[0] = short_mac_addr.addr[0]; - src.addr[1] = short_mac_addr.addr[1]; - } - else -#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ - { - src.addr_len = netif->hwaddr_len; - SMEMCPY(src.addr, netif->hwaddr, netif->hwaddr_len); - } - - /* multicast destination IP address? */ - if (ip6_addr_ismulticast(ip6addr)) { - /* We need to send to the broadcast address.*/ - return lowpan6_frag(netif, q, &src, &ieee_802154_broadcast); - } - - /* We have a unicast destination IP address */ - /* TODO anycast? */ - -#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS - if (src.addr_len == 2) - { - /* If source address was compressable to short_mac_addr, and dest has same subnet and - * is also compressable to 2-bytes, assume we can infer dest as a short address too. */ - dest.addr_len = 2; - dest.addr[0] = ((u8_t *)q->payload)[38]; - dest.addr[1] = ((u8_t *)q->payload)[39]; - if ((src.addr_len == 2) && (ip6_addr_netcmp(&ip6_hdr->src, &ip6_hdr->dest)) && (lowpan6_get_address_mode(ip6addr, &dest) == 3)) - { - return lowpan6_frag(netif, q, &src, &dest); - } - } -#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ - - - /* Get next hop record. */ - i = nd6_get_next_hop_entry(ip6addr, netif); - if (i < 0) { - /* failed to get a next hop neighbor record. */ - return ERR_MEM; - } - - /* Now that we have a destination record, send or queue the packet. */ - if (neighbor_cache[i].state == ND6_STALE) { - /* Switch to delay state. */ - neighbor_cache[i].state = ND6_DELAY; - neighbor_cache[i].counter.delay_time = LWIP_ND6_DELAY_FIRST_PROBE_TIME; - } - /* TODO should we send or queue if PROBE? send for now, to let unicast NS pass. */ - if ((neighbor_cache[i].state == ND6_REACHABLE) || - (neighbor_cache[i].state == ND6_DELAY) || - (neighbor_cache[i].state == ND6_PROBE)) { - - /* Send out. */ - dest.addr_len = netif->hwaddr_len; - SMEMCPY(dest.addr, neighbor_cache[i].lladdr, netif->hwaddr_len); - return lowpan6_frag(netif, q, &src, &dest); - } - - /* We should queue packet on this interface. */ - return nd6_queue_packet(i, q); -} - -static struct pbuf * -lowpan6_decompress(struct pbuf * p, struct ieee_802154_addr * src, struct ieee_802154_addr * dest) -{ - struct pbuf * q; - u8_t * lowpan6_buffer; - s8_t lowpan6_offset; - struct ip6_hdr *ip6hdr; - s8_t i; - s8_t ip6_offset = IP6_HLEN; - - - q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN, PBUF_POOL); - if (q == NULL) - { - pbuf_free(p); - return NULL; - } - - lowpan6_buffer = (u8_t *)p->payload; - ip6hdr = (struct ip6_hdr *)q->payload; - - lowpan6_offset = 2; - if (lowpan6_buffer[1] & 0x80) - { - lowpan6_offset++; - } - - /* Set IPv6 version, traffic class and flow label. */ - if ((lowpan6_buffer[0] & 0x18) == 0x00) - { - IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset], ((lowpan6_buffer[lowpan6_offset+1] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset+3]); - lowpan6_offset += 4; - } - else if ((lowpan6_buffer[0] & 0x18) == 0x08) - { - IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset] & 0xc0, ((lowpan6_buffer[lowpan6_offset] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset+2]); - lowpan6_offset += 3; - } - else if ((lowpan6_buffer[0] & 0x18) == 0x10) - { - IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0); - lowpan6_offset += 1; - } - else if ((lowpan6_buffer[0] & 0x18) == 0x18) - { - IP6H_VTCFL_SET(ip6hdr, 6, 0, 0); - } - - /* Set Next Header */ - if ((lowpan6_buffer[0] & 0x04) == 0x00) - { - IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]); - } - else - { - /* We should fill this later with NHC decoding */ - IP6H_NEXTH_SET(ip6hdr, 0); - } - - /* Set Hop Limit */ - if ((lowpan6_buffer[0] & 0x03) == 0x00) - { - IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]); - } - else if ((lowpan6_buffer[0] & 0x03) == 0x01) - { - IP6H_HOPLIM_SET(ip6hdr, 1); - } - else if ((lowpan6_buffer[0] & 0x03) == 0x02) - { - IP6H_HOPLIM_SET(ip6hdr, 64); - } - else if ((lowpan6_buffer[0] & 0x03) == 0x03) - { - IP6H_HOPLIM_SET(ip6hdr, 255); - } - - /* Source address decoding. */ - if ((lowpan6_buffer[1] & 0x40) == 0x00) - { - /* Stateless compression */ - if ((lowpan6_buffer[1] & 0x30) == 0x00) - { - /* copy full address */ - MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16); - lowpan6_offset += 16; - } - else if ((lowpan6_buffer[1] & 0x30) == 0x10) - { - ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); - ip6hdr->src.addr[1] = 0; - MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8); - lowpan6_offset += 8; - } - else if ((lowpan6_buffer[1] & 0x30) == 0x20) - { - ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); - ip6hdr->src.addr[1] = 0; - ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); - ip6hdr->src.addr[3] = htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset+1]); - lowpan6_offset += 2; - } - else if ((lowpan6_buffer[1] & 0x30) == 0x30) - { - ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); - ip6hdr->src.addr[1] = 0; - if (src->addr_len == 2) - { - ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); - ip6hdr->src.addr[3] = htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]); - } - else - { - ip6hdr->src.addr[2] = htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | src->addr[2] << 8 | src->addr[3]); - ip6hdr->src.addr[3] = htonl((src->addr[4] << 24) | (src->addr[5] << 16) | src->addr[6] << 8 | src->addr[7]); - } - } - } - else - { - /* Stateful compression */ - if ((lowpan6_buffer[1] & 0x30) == 0x00) - { - /* ANY address */ - ip6hdr->src.addr[0] = 0; - ip6hdr->src.addr[1] = 0; - ip6hdr->src.addr[2] = 0; - ip6hdr->src.addr[3] = 0; - } - else - { - /* Set prefix from context info */ - if (lowpan6_buffer[1] & 0x80) - { - i = (lowpan6_buffer[2] >> 4) & 0x0f; - } - else - { - i = 0; - } - if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) - { - /* Error */ - pbuf_free(p); - pbuf_free(q); - return NULL; - } - - ip6hdr->src.addr[0] = lowpan6_context[i].addr[0]; - ip6hdr->src.addr[1] = lowpan6_context[i].addr[1]; - } - - if ((lowpan6_buffer[1] & 0x30) == 0x10) - { - MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8); - lowpan6_offset += 8; - } - else if ((lowpan6_buffer[1] & 0x30) == 0x20) - { - ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); - ip6hdr->src.addr[3] = htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset+1]); - lowpan6_offset += 2; - } - else if ((lowpan6_buffer[1] & 0x30) == 0x30) - { - if (src->addr_len == 2) - { - ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); - ip6hdr->src.addr[3] = htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]); - } - else - { - ip6hdr->src.addr[2] = htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | src->addr[2] << 8 | src->addr[3]); - ip6hdr->src.addr[3] = htonl((src->addr[4] << 24) | (src->addr[5] << 16) | src->addr[6] << 8 | src->addr[7]); - } - } - } - - /* Destination address decoding. */ - if (lowpan6_buffer[1] & 0x08) - { - /* Multicast destination */ - if (lowpan6_buffer[1] & 0x04) - { - /* TODO support stateful multicast addressing */ - pbuf_free(p); - pbuf_free(q); - return NULL; - } - - if ((lowpan6_buffer[1] & 0x03) == 0x00) - { - /* copy full address */ - MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16); - lowpan6_offset += 16; - } - else if ((lowpan6_buffer[1] & 0x03) == 0x01) - { - ip6hdr->dest.addr[0] = htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16)); - ip6hdr->dest.addr[1] = 0; - ip6hdr->dest.addr[2] = htonl(lowpan6_buffer[lowpan6_offset++]); - ip6hdr->dest.addr[3] = htonl((lowpan6_buffer[lowpan6_offset] << 24) | (lowpan6_buffer[lowpan6_offset + 1] << 16) | (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset + 3]); - lowpan6_offset += 4; - } - else if ((lowpan6_buffer[1] & 0x03) == 0x02) - { - ip6hdr->dest.addr[0] = htonl(0xff000000UL | lowpan6_buffer[lowpan6_offset++]); - ip6hdr->dest.addr[1] = 0; - ip6hdr->dest.addr[2] = 0; - ip6hdr->dest.addr[3] = htonl((lowpan6_buffer[lowpan6_offset] << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset + 2]); - lowpan6_offset += 3; - } - else if ((lowpan6_buffer[1] & 0x03) == 0x03) - { - ip6hdr->dest.addr[0] = PP_HTONL(0xff020000UL); - ip6hdr->dest.addr[1] = 0; - ip6hdr->dest.addr[2] = 0; - ip6hdr->dest.addr[3] = htonl(lowpan6_buffer[lowpan6_offset++]); - } - - } - else - { - if (lowpan6_buffer[1] & 0x04) - { - /* Stateful destination compression */ - /* Set prefix from context info */ - if (lowpan6_buffer[1] & 0x80) - { - i = lowpan6_buffer[2] & 0x0f; - } - else - { - i = 0; - } - if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) - { - /* Error */ - pbuf_free(p); - pbuf_free(q); - return NULL; - } - - ip6hdr->dest.addr[0] = lowpan6_context[i].addr[0]; - ip6hdr->dest.addr[1] = lowpan6_context[i].addr[1]; - } - else - { - /* Link local address compression */ - ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL); - ip6hdr->dest.addr[1] = 0; - } - - if ((lowpan6_buffer[1] & 0x03) == 0x00) - { - /* copy full address */ - MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16); - lowpan6_offset += 16; - } - else if ((lowpan6_buffer[1] & 0x03) == 0x01) - { - MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8); - lowpan6_offset += 8; - } - else if ((lowpan6_buffer[1] & 0x03) == 0x02) - { - ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL); - ip6hdr->dest.addr[3] = htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]); - lowpan6_offset += 2; - } - else if ((lowpan6_buffer[1] & 0x03) == 0x03) - { - if (dest->addr_len == 2) - { - ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL); - ip6hdr->dest.addr[3] = htonl(0xfe000000UL | (dest->addr[0] << 8) | dest->addr[1]); - } - else - { - ip6hdr->dest.addr[2] = htonl(((dest->addr[0] ^ 2) << 24) | (dest->addr[1] << 16) | dest->addr[2] << 8 | dest->addr[3]); - ip6hdr->dest.addr[3] = htonl((dest->addr[4] << 24) | (dest->addr[5] << 16) | dest->addr[6] << 8 | dest->addr[7]); - } - } - } - - - /* Next Header Compression (NHC) decoding? */ - if (lowpan6_buffer[0] & 0x04) - { - if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) - { - struct udp_hdr *udphdr; - - /* UDP compression */ - IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP); - udphdr = (struct udp_hdr *)((u8_t *)q->payload + ip6_offset); - - if (lowpan6_buffer[lowpan6_offset] & 0x04) - { - /* TODO support checksum decompress */ - pbuf_free(p); - pbuf_free(q); - return NULL; - } - - /* Decompress ports */ - i = lowpan6_buffer[lowpan6_offset++] & 0x03; - if (i == 0) - { - udphdr->src = htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); - udphdr->dest = htons(lowpan6_buffer[lowpan6_offset + 2] << 8 | lowpan6_buffer[lowpan6_offset + 3]); - lowpan6_offset += 4; - } - else if (i == 0x01) - { - udphdr->src = htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); - udphdr->dest = htons(0xf000 | lowpan6_buffer[lowpan6_offset + 2]); - lowpan6_offset += 3; - } - else if (i == 0x02) - { - udphdr->src = htons(0xf000 | lowpan6_buffer[lowpan6_offset]); - udphdr->dest = htons(lowpan6_buffer[lowpan6_offset + 1] << 8 | lowpan6_buffer[lowpan6_offset + 2]); - lowpan6_offset += 3; - } - else if (i == 0x03) - { - udphdr->src = htons(0xf0b0 | ((lowpan6_buffer[lowpan6_offset] >> 4) & 0x0f)); - udphdr->dest = htons(0xf0b0 | (lowpan6_buffer[lowpan6_offset] & 0x0f)); - lowpan6_offset += 1; - } - - udphdr->chksum = htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); - lowpan6_offset += 2; - udphdr->len = htons(p->tot_len - lowpan6_offset + UDP_HLEN); - - ip6_offset += UDP_HLEN; - } - else - { - /* TODO support NHC other than UDP */ - pbuf_free(p); - pbuf_free(q); - return NULL; - } - } - - /* Now we copy leftover contents from p to q, so we have all L2 and L3 headers (and L4?) in a single PBUF. - * Replace p with q, and free p */ - pbuf_header(p, -lowpan6_offset); - MEMCPY((u8_t*)q->payload + ip6_offset, p->payload, p->len); - q->len = q->tot_len = ip6_offset + p->len; - if (p->next != NULL) { - pbuf_cat(q, p->next); - } - p->next = NULL; - pbuf_free(p); - - /* Infer IPv6 payload length for header */ - IP6H_PLEN_SET(ip6hdr, q->tot_len - IP6_HLEN); - - /* all done */ - return q; -} - -err_t -lowpan6_input(struct pbuf * p, struct netif *netif) -{ - u8_t * puc; - s8_t i; - struct ieee_802154_addr src, dest; - u16_t datagram_size, datagram_offset, datagram_tag; - struct lowpan6_reass_helper *lrh, *lrh_temp; - - /* Analyze header. TODO validate. */ - puc = (u8_t*)p->payload; - datagram_offset = 5; - if ((puc[1] & 0x0c) == 0x0c) - { - dest.addr_len = 8; - for (i = 0; i < 8; i++) - { - dest.addr[i] = puc[datagram_offset + 7 - i]; - } - datagram_offset += 8; - } - else - { - dest.addr_len = 2; - dest.addr[0] = puc[datagram_offset + 1]; - dest.addr[1] = puc[datagram_offset]; - datagram_offset += 2; - } - - datagram_offset += 2; /* skip PAN ID. */ - - if ((puc[1] & 0xc0) == 0xc0) - { - src.addr_len = 8; - for (i = 0; i < 8; i++) - { - src.addr[i] = puc[datagram_offset + 7 - i]; - } - datagram_offset += 8; - } - else - { - src.addr_len = 2; - src.addr[0] = puc[datagram_offset + 1]; - src.addr[1] = puc[datagram_offset]; - datagram_offset += 2; - } - - pbuf_header(p, -datagram_offset); /* hide IEEE802.15.4 header. */ - - /* Check dispatch. */ - puc = (u8_t*)p->payload; - - if ((*puc & 0xf8) == 0xc0) - { - /* FRAG1 dispatch. add this packet to reassembly list. */ - datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1]; - datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3]; - - /* check for duplicate */ - lrh = reass_list; - while (lrh != NULL) - { - if ((lrh->sender_addr.addr_len == src.addr_len) && - (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0)) - { - /* address match with packet in reassembly. */ - if ((datagram_tag == lrh->datagram_tag) && (datagram_size == lrh->datagram_size)) { - /* duplicate fragment. */ - pbuf_free(p); - return ERR_OK; - } - else { - /* We are receiving the start of a new datagram. Discard old one (incomplete). */ - lrh_temp = lrh->next_packet; - dequeue_datagram(lrh); - pbuf_free(lrh->pbuf); - mem_free(lrh); - - /* Check next datagram in queue. */ - lrh = lrh_temp; - } - } - else { - /* Check next datagram in queue. */ - lrh = lrh->next_packet; - } - } - - pbuf_header(p, -4); /* hide frag1 dispatch */ - - lrh = (struct lowpan6_reass_helper *) mem_malloc(sizeof(struct lowpan6_reass_helper)); - if (lrh == NULL) { - pbuf_free(p); - return ERR_MEM; - } - - lrh->sender_addr.addr_len = src.addr_len; - for (i = 0; i < src.addr_len; i++) { - lrh->sender_addr.addr[i] = src.addr[i]; - } - lrh->datagram_size = datagram_size; - lrh->datagram_tag = datagram_tag; - lrh->pbuf = p; - lrh->next_packet = reass_list; - lrh->timer = 2; - reass_list = lrh; - - return ERR_OK; - } - else if ((*puc & 0xf8) == 0xe0) - { - /* FRAGN dispatch, find packet being reassembled. */ - datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1]; - datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3]; - datagram_offset = (u16_t)puc[4] << 3; - pbuf_header(p, -5); /* hide frag1 dispatch */ - - for (lrh = reass_list; lrh != NULL; lrh = lrh->next_packet) - { - if ( - (lrh->sender_addr.addr_len == src.addr_len) && - (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0) && - (datagram_tag == lrh->datagram_tag) && - (datagram_size == lrh->datagram_size) - ) - { - break; - } - } - if (lrh == NULL) - { - /* rogue fragment */ - pbuf_free(p); - return ERR_OK; - } - - if (lrh->pbuf->tot_len < datagram_offset) - { - /* duplicate, ignore. */ - pbuf_free(p); - return ERR_OK; - } - else if (lrh->pbuf->tot_len > datagram_offset) - { - /* We have missed a fragment. Delete whole reassembly. */ - dequeue_datagram(lrh); - pbuf_free(lrh->pbuf); - mem_free(lrh); - pbuf_free(p); - return ERR_OK; - } - pbuf_cat(lrh->pbuf, p); - p = NULL; - - /* is packet now complete?*/ - if (lrh->pbuf->tot_len >= lrh->datagram_size) - { - /* dequeue from reass list. */ - dequeue_datagram(lrh); - - /* get pbuf */ - p = lrh->pbuf; - - /* release helper */ - mem_free(lrh); - } - else - { - return ERR_OK; - } - } - - if (p == NULL) return ERR_OK; - - /* We have a complete packet, check dispatch for headers. */ - puc = (u8_t*)p->payload; - - if (*puc == 0x41) - { - /* This is a complete IPv6 packet, just skip dispatch byte. */ - pbuf_header(p, -1); /* hide dispatch byte. */ - } - else if ((*puc & 0xe0 )== 0x60) - { - /* IPv6 headers are compressed using IPHC. */ - p = lowpan6_decompress(p, &src, &dest); - if (p == NULL) return ERR_OK; - } - else - { - pbuf_free(p); - return ERR_OK; - } - - return ip6_input(p, netif); -} - -err_t -lowpan6_if_init(struct netif *netif) -{ - netif->name[0] = 'L'; - netif->name[1] = '6'; -#if LWIP_IPV4 - netif->output = lowpan4_output; -#endif /* LWIP_IPV4 */ - netif->output_ip6 = lowpan6_output; - - /* maximum transfer unit */ - netif->mtu = 1280; - - /* broadcast capability */ - netif->flags = NETIF_FLAG_BROADCAST /* | NETIF_FLAG_LOWPAN6 */; - - return ERR_OK; -} - -err_t -lowpan6_set_pan_id(u16_t pan_id) -{ - ieee_802154_pan_id = pan_id; - - return ERR_OK; -} - -/** - * Pass a received packet to tcpip_thread for input processing - * - * @param p the received packet, p->payload pointing to the - * IEEE 802.15.4 header. - * @param inp the network interface on which the packet was received - */ -err_t -tcpip_6lowpan_input(struct pbuf *p, struct netif *inp) -{ - return tcpip_inpkt(p, inp, lowpan6_input); -} - -#endif /* LWIP_IPV6 && LWIP_6LOWPAN */ +/** + * @file + * + * 6LowPAN output for IPv6. Uses ND tables for link-layer addressing. Fragments packets to 6LowPAN units. + */ + +/* + * Copyright (c) 2015 Inico Technologies Ltd. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. The name of the author may not be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT + * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + * + * This file is part of the lwIP TCP/IP stack. + * + * Author: Ivan Delamer + * + * + * Please coordinate changes and requests with Ivan Delamer + * + */ + +#include "netif/lowpan6.h" + +#if LWIP_IPV6 && LWIP_6LOWPAN + +#include "lwip/ip.h" +#include "lwip/pbuf.h" +#include "lwip/ip_addr.h" +#include "lwip/netif.h" +#include "lwip/nd6.h" +#include "lwip/mem.h" +#include "lwip/udp.h" +#include "lwip/tcpip.h" + +struct ieee_802154_addr { + u8_t addr_len; + u8_t addr[8]; +}; + +/** This is a helper struct. + */ +struct lowpan6_reass_helper { + struct pbuf *pbuf; + struct lowpan6_reass_helper *next_packet; + u8_t timer; + struct ieee_802154_addr sender_addr; + u16_t datagram_size; + u16_t datagram_tag; +}; + +static struct lowpan6_reass_helper * reass_list; + +#if LWIP_6LOWPAN_NUM_CONTEXTS > 0 +static ip6_addr_t lowpan6_context[LWIP_6LOWPAN_NUM_CONTEXTS]; +#endif + +static u16_t ieee_802154_pan_id; + +static const struct ieee_802154_addr ieee_802154_broadcast = {2, {0xff, 0xff}}; + +#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS +static struct ieee_802154_addr short_mac_addr = {2, {0,0}}; +#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ + +static err_t dequeue_datagram(struct lowpan6_reass_helper *lrh); + +/** + * Periodic timer for 6LowPAN functions: + * + * - Remove incomplete/old packets + */ +void +lowpan6_tmr(void) +{ + struct lowpan6_reass_helper *lrh, *lrh_temp; + + lrh = reass_list; + while (lrh != NULL) + { + lrh_temp = lrh->next_packet; + if ((--lrh->timer) == 0) + { + dequeue_datagram(lrh); + pbuf_free(lrh->pbuf); + mem_free(lrh); + } + lrh = lrh_temp; + } +} + +/** + * Removes a datagram from the reassembly queue. + **/ +static err_t +dequeue_datagram(struct lowpan6_reass_helper *lrh) +{ + struct lowpan6_reass_helper *lrh_temp; + + if (reass_list == lrh) + { + reass_list = reass_list->next_packet; + } + else + { + lrh_temp = reass_list; + while (lrh_temp != NULL) + { + if (lrh_temp->next_packet == lrh) + { + lrh_temp->next_packet = lrh->next_packet; + break; + } + lrh_temp = lrh_temp->next_packet; + } + } + + return ERR_OK; +} + +static s8_t +lowpan6_context_lookup(const ip6_addr_t *ip6addr) +{ + s8_t i; + + for (i = 0; i < LWIP_6LOWPAN_NUM_CONTEXTS; i++) + { + if (ip6_addr_netcmp(&lowpan6_context[i], ip6addr)) + return i; + } + + return -1; +} + +/* Determine compression mode for unicast address. */ +static s8_t +lowpan6_get_address_mode(const ip6_addr_t *ip6addr, const struct ieee_802154_addr *mac_addr) +{ + if (mac_addr->addr_len == 2) + { + if ((ip6addr->addr[2] == (u32_t)PP_HTONL(0x000000ff)) && + ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000))) + { + if ((ip6addr->addr[3] & PP_HTONL(0x0000ffff)) == ntohl((mac_addr->addr[0] << 8) | mac_addr->addr[1])) + { + return 3; + } + } + } + else if (mac_addr->addr_len == 8) + { + if ((ip6addr->addr[2] == ntohl(((mac_addr->addr[0] ^ 2) << 24) | (mac_addr->addr[1] << 16) | mac_addr->addr[2] << 8 | mac_addr->addr[3])) && + (ip6addr->addr[3] == ntohl((mac_addr->addr[4] << 24) | (mac_addr->addr[5] << 16) | mac_addr->addr[6] << 8 | mac_addr->addr[7]))) + { + return 3; + } + } + + if ((ip6addr->addr[2] == PP_HTONL(0x000000ffUL)) && + ((ip6addr->addr[3] & PP_HTONL(0xffff0000)) == PP_NTOHL(0xfe000000UL))) + { + return 2; + } + + return 1; +} + +/* Determine compression mode for multicast address. */ +static s8_t +lowpan6_get_address_mode_mc(const ip6_addr_t *ip6addr) +{ + if ((ip6addr->addr[0] == PP_HTONL(0xff020000)) && + (ip6addr->addr[1] == 0) && + (ip6addr->addr[2] == 0) && + ((ip6addr->addr[3] & PP_HTONL(0xffffff00)) == 0)) + { + return 3; + } + else if (((ip6addr->addr[0] & PP_HTONL(0xff00ffff)) == PP_HTONL(0xff000000)) && + (ip6addr->addr[1] == 0)) + { + if ((ip6addr->addr[2] == 0) && + ((ip6addr->addr[3] & PP_HTONL(0xff000000)) == 0)) + { + return 2; + } + else if ((ip6addr->addr[2] & PP_HTONL(0xffffff00)) == 0) + { + return 1; + } + } + + + return 0; +} + +/* + * Encapsulates data into IEEE 802.15.4 frames. + * Fragments an IPv6 datagram into 6LowPAN units, which fit into IEEE 802.15.4 frames. + * If configured, will compress IPv6 and or UDP headers. + * */ +static err_t +lowpan6_frag(struct netif *netif, struct pbuf *p, const struct ieee_802154_addr *src, const struct ieee_802154_addr *dst) +{ + struct pbuf * p_frag; + u16_t frag_len, remaining_len; + u8_t * buffer; + u8_t ieee_header_len; + u8_t lowpan6_header_len; + s8_t i; + static u8_t frame_seq_num; + static u16_t datagram_tag; + u16_t datagram_offset; + err_t err = ERR_IF; + + /* We'll use a dedicated pbuf for building 6LowPAN fragments. */ + p_frag = pbuf_alloc(PBUF_RAW, 127, PBUF_RAM); + if (p_frag == NULL) { + return ERR_MEM; + } + + /* Write IEEE 802.15.4 header. */ + buffer = (u8_t*)p_frag->payload; + ieee_header_len = 0; + if (dst == &ieee_802154_broadcast) + { + buffer[ieee_header_len++] = 0x01; /* data packet, no ack required. */ + } + else + { + buffer[ieee_header_len++] = 0x21; /* data packet, ack required. */ + } + buffer[ieee_header_len] = (0x00 << 4); /* 2003 frame version */ + buffer[ieee_header_len] |= (dst->addr_len == 2) ? (0x02 << 2) : (0x03 << 2); /* destination addressing mode */ + buffer[ieee_header_len] |= (src->addr_len == 2) ? (0x02 << 6) : (0x03 << 6); /* source addressing mode */ + ieee_header_len++; + buffer[ieee_header_len++] = frame_seq_num++; + + buffer[ieee_header_len++] = ieee_802154_pan_id & 0xff; /* pan id */ + buffer[ieee_header_len++] = (ieee_802154_pan_id >> 8) & 0xff; /* pan id */ + i = dst->addr_len; + while (i-- > 0) { + buffer[ieee_header_len++] = dst->addr[i]; + } + + buffer[ieee_header_len++] = ieee_802154_pan_id & 0xff; /* pan id */ + buffer[ieee_header_len++] = (ieee_802154_pan_id >> 8) & 0xff; /* pan id */ + i = src->addr_len; + while (i-- > 0) { + buffer[ieee_header_len++] = src->addr[i]; + } + +#if LWIP_6LOWPAN_IPHC + /* Perform 6LowPAN IPv6 header compression according to RFC 6282 */ + { + struct ip6_hdr *ip6hdr; + + /* Point to ip6 header and align copies of src/dest addresses. */ + ip6hdr = (struct ip6_hdr *)p->payload; + ip_addr_copy_from_ip6(ip_data.current_iphdr_dest, ip6hdr->dest); + ip_addr_copy_from_ip6(ip_data.current_iphdr_src, ip6hdr->src); + + /* Basic length of 6LowPAN header, set dispatch and clear fields. */ + lowpan6_header_len = 2; + buffer[ieee_header_len] = 0x60; + buffer[ieee_header_len + 1] = 0; + + /* Determine whether there will be a Context Identifier Extension byte or not. + * If so, set it already. */ +#if LWIP_6LOWPAN_NUM_CONTEXTS > 0 + buffer[ieee_header_len + 2] = 0; + + i = lowpan6_context_lookup(ip_2_ip6(&ip_data.current_iphdr_src)); + if (i >= 0) + { + /* Stateful source address compression. */ + buffer[ieee_header_len + 1] |= 0x40; + buffer[ieee_header_len + 2] |= (i & 0x0f) << 4; + } + + i = lowpan6_context_lookup(ip_2_ip6(&ip_data.current_iphdr_dest)); + if (i >= 0) + { + /* Stateful destination address compression. */ + buffer[ieee_header_len + 1] |= 0x04; + buffer[ieee_header_len + 2] |= i & 0x0f; + } + + if (buffer[ieee_header_len + 2] != 0x00) + { + /* Context identifier extension byte is appended. */ + buffer[ieee_header_len + 1] |= 0x80; + lowpan6_header_len++; + } +#endif /* LWIP_6LOWPAN_NUM_CONTEXTS > 0 */ + + /* Determine TF field: Traffic Class, Flow Label */ + if (IP6H_FL(ip6hdr) == 0) + { + /* Flow label is elided. */ + buffer[ieee_header_len] |= 0x10; + if (IP6H_TC(ip6hdr) == 0) + { + /* Traffic class (ECN+DSCP) elided too. */ + buffer[ieee_header_len] |= 0x08; + } + else + { + /* Traffic class (ECN+DSCP) appended. */ + buffer[ieee_header_len + lowpan6_header_len++] = IP6H_TC(ip6hdr); + } + } + else + { + if (((IP6H_TC(ip6hdr) & 0x3f) == 0)) + { + /* DSCP portion of Traffic Class is elided, ECN and FL are appended (3 bytes) */ + buffer[ieee_header_len] |= 0x08; + + buffer[ieee_header_len + lowpan6_header_len] = IP6H_TC(ip6hdr) & 0xc0; + buffer[ieee_header_len + lowpan6_header_len++] |= (IP6H_FL(ip6hdr) >> 16) & 0x0f; + buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff; + buffer[ieee_header_len + lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff; + } + else + { + /* Traffic class and flow label are appended (4 bytes) */ + buffer[ieee_header_len + lowpan6_header_len++] = IP6H_TC(ip6hdr); + buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 16) & 0x0f; + buffer[ieee_header_len + lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff; + buffer[ieee_header_len + lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff; + } + } + + /* Compress NH? + * Only if UDP for now. TODO support other NH compression. */ + if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) + { + buffer[ieee_header_len] |= 0x04; + } + else + { + /* append nexth. */ + buffer[ieee_header_len + lowpan6_header_len++] = IP6H_NEXTH(ip6hdr); + } + + /* Compress hop limit? */ + if (IP6H_HOPLIM(ip6hdr) == 255) + { + buffer[ieee_header_len] |= 0x03; + } + else if (IP6H_HOPLIM(ip6hdr) == 64) + { + buffer[ieee_header_len] |= 0x02; + } + else if (IP6H_HOPLIM(ip6hdr) == 1) + { + buffer[ieee_header_len] |= 0x01; + } + else + { + /* append hop limit */ + buffer[ieee_header_len + lowpan6_header_len++] = IP6H_HOPLIM(ip6hdr); + } + + /* Compress source address */ + if (((buffer[ieee_header_len + 1] & 0x40) != 0) || (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_src)))) + { + /* Context-based or link-local source address compression. */ + i = lowpan6_get_address_mode(ip_2_ip6(&ip_data.current_iphdr_src), src); + buffer[ieee_header_len + 1] |= (i & 0x03) << 4; + if (i == 1) + { + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 16, 8); + lowpan6_header_len += 8; + } + else if (i == 2) + { + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 22, 2); + lowpan6_header_len += 2; + } + } + else if (ip6_addr_isany(ip_2_ip6(&ip_data.current_iphdr_src))) + { + /* Special case: mark SAC and leave SAM=0 */ + buffer[ieee_header_len + 1] |= 0x40; + } + else + { + /* Append full address. */ + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 8, 16); + lowpan6_header_len += 16; + } + + /* Compress destination address */ + if (ip6_addr_ismulticast(ip_2_ip6(&ip_data.current_iphdr_dest))) + { + /* TODO support statefull multicast address compression */ + + buffer[ieee_header_len + 1] |= 0x08; + + i = lowpan6_get_address_mode_mc(ip_2_ip6(&ip_data.current_iphdr_dest)); + buffer[ieee_header_len + 1] |= i & 0x03; + if (i == 0) + { + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 24, 16); + lowpan6_header_len += 16; + } + else if (i == 1) + { + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[25]; + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 35, 5); + lowpan6_header_len += 5; + } + else if (i == 2) + { + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[25]; + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 37, 3); + lowpan6_header_len += 3; + } + else if (i == 3) + { + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[39]; + } + } + else if (((buffer[ieee_header_len + 1] & 0x04) != 0) || (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_dest)))) + { + /* Context-based or link-local destination address compression. */ + i = lowpan6_get_address_mode(ip_2_ip6(&ip_data.current_iphdr_dest), dst); + buffer[ieee_header_len + 1] |= i & 0x03; + if (i == 1) + { + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 32, 8); + lowpan6_header_len += 8; + } + else if (i == 2) + { + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 38, 2); + lowpan6_header_len += 2; + } + } + else + { + /* Append full address. */ + MEMCPY(buffer + ieee_header_len + lowpan6_header_len, (u8_t*)p->payload + 24, 16); + lowpan6_header_len += 16; + } + + /* Move to payload. */ + pbuf_header(p, -IP6_HLEN); + + /* Compress UDP header? */ + if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) + { + /* TODO support optional checksum compression */ + + buffer[ieee_header_len + lowpan6_header_len] = 0xf0; + + /* determine port compression mode. */ + if ((((u8_t *)p->payload)[0] == 0xf0) && ((((u8_t *)p->payload)[1] & 0xf0) == 0xb0) && + (((u8_t *)p->payload)[2] == 0xf0) && ((((u8_t *)p->payload)[3] & 0xf0) == 0xb0)) + { + /* Compress source and dest ports. */ + buffer[ieee_header_len + lowpan6_header_len++] |= 0x03; + buffer[ieee_header_len + lowpan6_header_len++] = ((((u8_t *)p->payload)[1] & 0x0f) << 4) | (((u8_t *)p->payload)[3] & 0x0f); + } + else if (((u8_t *)p->payload)[0] == 0xf0) + { + /* Compress source port. */ + buffer[ieee_header_len + lowpan6_header_len++] |= 0x02; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2]; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; + } + else if (((u8_t *)p->payload)[2] == 0xf0) + { + /* Compress dest port. */ + buffer[ieee_header_len + lowpan6_header_len++] |= 0x01; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0]; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; + } + else + { + /* append full ports. */ + lowpan6_header_len++; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[0]; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[1]; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[2]; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[3]; + } + + /* elide length and copy checksum */ + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[6]; + buffer[ieee_header_len + lowpan6_header_len++] = ((u8_t *)p->payload)[7]; + + pbuf_header(p, -UDP_HLEN); + } + } + +#else /* LWIP_6LOWPAN_HC */ + /* Send uncompressed IPv6 header with appropriate dispatch byte. */ + lowpan6_header_len = 1; + buffer[ieee_header_len] = 0x41; /* IPv6 dispatch */ +#endif /* LWIP_6LOWPAN_HC */ + + /* Calculate remaining packet length */ + remaining_len = p->tot_len; + + /* Fragment, or 1 packet? */ + if (remaining_len > (127 - ieee_header_len - lowpan6_header_len - 3)) /* 127 - header - 1 byte dispatch - 2 bytes CRC */ + { + /* We must move the 6LowPAN header to make room for the FRAG header. */ + i = lowpan6_header_len; + while (i-- != 0) + { + buffer[ieee_header_len + i + 4] = buffer[ieee_header_len + i]; + } + + /* Now we need to fragment the packet. FRAG1 header first */ + buffer[ieee_header_len] = 0xc0 | (((p->tot_len + lowpan6_header_len) >> 8) & 0x7); + buffer[ieee_header_len + 1] = (p->tot_len + lowpan6_header_len) & 0xff; + + datagram_tag++; + buffer[ieee_header_len + 2] = datagram_tag & 0xff; + buffer[ieee_header_len + 3] = (datagram_tag >> 8) & 0xff; + + /* Fragment follows. */ + frag_len = (127 - ieee_header_len - 4 - 2) & 0xf8; + + pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len + 4, frag_len - lowpan6_header_len, 0); + remaining_len -= frag_len - lowpan6_header_len; + datagram_offset = frag_len; + + /* 2 bytes CRC */ +#if LWIP_6LOWPAN_HW_CRC + /* Leave blank, will be filled by HW. */ +#else /* LWIP_6LOWPAN_HW_CRC */ + /* TODO calculate CRC */ +#endif /* LWIP_6LOWPAN_HW_CRC */ + + /* Calculate frame length */ + p_frag->len = p_frag->tot_len = ieee_header_len + 4 + frag_len + 2; /* add 2 dummy bytes for crc*/ + + /* send the packet */ + LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); + err = netif->linkoutput(netif, p_frag); + + while ((remaining_len > 0) && (err == ERR_OK)) + { + /* new frame, new seq num for ACK */ + buffer[2] = frame_seq_num++; + + buffer[ieee_header_len] |= 0x20; /* Change FRAG1 to FRAGN */ + + buffer[ieee_header_len + 4] = datagram_offset >> 3; /* datagram offset in FRAGN header */ + + frag_len = (127 - ieee_header_len - 5 - 2) & 0xf8; + if (frag_len > remaining_len) + frag_len = remaining_len; + + pbuf_copy_partial(p, buffer + ieee_header_len + 5, frag_len, p->tot_len - remaining_len); + remaining_len -= frag_len; + datagram_offset += frag_len; + + /* 2 bytes CRC */ +#if LWIP_6LOWPAN_HW_CRC + /* Leave blank, will be filled by HW. */ +#else /* LWIP_6LOWPAN_HW_CRC */ + /* TODO calculate CRC */ +#endif /* LWIP_6LOWPAN_HW_CRC */ + + /* Calculate frame length */ + p_frag->len = p_frag->tot_len = frag_len + 5 + ieee_header_len + 2; + + /* send the packet */ + LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); + err = netif->linkoutput(netif, p_frag); + } + } + else + { + /* It fits in one frame. */ + frag_len = remaining_len; + + /* Copy IPv6 packet */ + pbuf_copy_partial(p, buffer + ieee_header_len + lowpan6_header_len, frag_len, 0); + remaining_len = 0; + + /* 2 bytes CRC */ +#if LWIP_6LOWPAN_HW_CRC + /* Leave blank, will be filled by HW. */ +#else /* LWIP_6LOWPAN_HW_CRC */ + /* TODO calculate CRC */ +#endif /* LWIP_6LOWPAN_HW_CRC */ + + /* Calculate frame length */ + p_frag->len = p_frag->tot_len = frag_len + lowpan6_header_len + ieee_header_len + 2; + + /* send the packet */ + LWIP_DEBUGF(LOWPAN6_DEBUG | LWIP_DBG_TRACE, ("lowpan6_send: sending packet %p\n", (void *)p)); + err = netif->linkoutput(netif, p_frag); + } + + + pbuf_free(p_frag); + + return err; +} + +err_t +lowpan6_set_context(u8_t idx, const ip6_addr_t * context) +{ + if (idx > LWIP_6LOWPAN_NUM_CONTEXTS) + return ERR_ARG; + + ip6_addr_set(&lowpan6_context[idx], context); + + return ERR_OK; +} + +#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS +err_t +lowpan6_set_short_addr(u8_t addr_high, u8_t addr_low) +{ + short_mac_addr.addr[0] = addr_high; + short_mac_addr.addr[1] = addr_low; + + return ERR_OK; +} +#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ + +#if LWIP_IPV4 +err_t +lowpan4_output(struct netif *netif, struct pbuf *q, const ip4_addr_t *ipaddr) +{ + (void)netif; + (void)q; + (void)ipaddr; + + return ERR_IF; +} +#endif /* LWIP_IPV4 */ + +/** + * Resolve and fill-in IEEE 802.15.4 address header for outgoing IPv6 packet. + * + * Perform Header Compression and fragment if necessary. + * + * @param netif The lwIP network interface which the IP packet will be sent on. + * @param q The pbuf(s) containing the IP packet to be sent. + * @param ip6addr The IP address of the packet destination. + * + * @return + */ +err_t +lowpan6_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr) +{ + s8_t i; + struct ieee_802154_addr src, dest; +#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS + ip6_addr_t ip6_src; + struct ip6_hdr * ip6_hdr; +#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ + +#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS + /* Check if we can compress source address (use aligned copy) */ + ip6_hdr = (struct ip6_hdr *)q->payload; + ip6_addr_set(&ip6_src, &ip6_hdr->src); + if (lowpan6_get_address_mode(&ip6_src, &short_mac_addr) == 3) + { + src.addr_len = 2; + src.addr[0] = short_mac_addr.addr[0]; + src.addr[1] = short_mac_addr.addr[1]; + } + else +#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ + { + src.addr_len = netif->hwaddr_len; + SMEMCPY(src.addr, netif->hwaddr, netif->hwaddr_len); + } + + /* multicast destination IP address? */ + if (ip6_addr_ismulticast(ip6addr)) { + /* We need to send to the broadcast address.*/ + return lowpan6_frag(netif, q, &src, &ieee_802154_broadcast); + } + + /* We have a unicast destination IP address */ + /* TODO anycast? */ + +#if LWIP_6LOWPAN_INFER_SHORT_ADDRESS + if (src.addr_len == 2) + { + /* If source address was compressable to short_mac_addr, and dest has same subnet and + * is also compressable to 2-bytes, assume we can infer dest as a short address too. */ + dest.addr_len = 2; + dest.addr[0] = ((u8_t *)q->payload)[38]; + dest.addr[1] = ((u8_t *)q->payload)[39]; + if ((src.addr_len == 2) && (ip6_addr_netcmp(&ip6_hdr->src, &ip6_hdr->dest)) && (lowpan6_get_address_mode(ip6addr, &dest) == 3)) + { + return lowpan6_frag(netif, q, &src, &dest); + } + } +#endif /* LWIP_6LOWPAN_INFER_SHORT_ADDRESS */ + + + /* Get next hop record. */ + i = nd6_get_next_hop_entry(ip6addr, netif); + if (i < 0) { + /* failed to get a next hop neighbor record. */ + return ERR_MEM; + } + + /* Now that we have a destination record, send or queue the packet. */ + if (neighbor_cache[i].state == ND6_STALE) { + /* Switch to delay state. */ + neighbor_cache[i].state = ND6_DELAY; + neighbor_cache[i].counter.delay_time = LWIP_ND6_DELAY_FIRST_PROBE_TIME; + } + /* TODO should we send or queue if PROBE? send for now, to let unicast NS pass. */ + if ((neighbor_cache[i].state == ND6_REACHABLE) || + (neighbor_cache[i].state == ND6_DELAY) || + (neighbor_cache[i].state == ND6_PROBE)) { + + /* Send out. */ + dest.addr_len = netif->hwaddr_len; + SMEMCPY(dest.addr, neighbor_cache[i].lladdr, netif->hwaddr_len); + return lowpan6_frag(netif, q, &src, &dest); + } + + /* We should queue packet on this interface. */ + return nd6_queue_packet(i, q); +} + +static struct pbuf * +lowpan6_decompress(struct pbuf * p, struct ieee_802154_addr * src, struct ieee_802154_addr * dest) +{ + struct pbuf * q; + u8_t * lowpan6_buffer; + s8_t lowpan6_offset; + struct ip6_hdr *ip6hdr; + s8_t i; + s8_t ip6_offset = IP6_HLEN; + + + q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN, PBUF_POOL); + if (q == NULL) + { + pbuf_free(p); + return NULL; + } + + lowpan6_buffer = (u8_t *)p->payload; + ip6hdr = (struct ip6_hdr *)q->payload; + + lowpan6_offset = 2; + if (lowpan6_buffer[1] & 0x80) + { + lowpan6_offset++; + } + + /* Set IPv6 version, traffic class and flow label. */ + if ((lowpan6_buffer[0] & 0x18) == 0x00) + { + IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset], ((lowpan6_buffer[lowpan6_offset+1] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset+3]); + lowpan6_offset += 4; + } + else if ((lowpan6_buffer[0] & 0x18) == 0x08) + { + IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset] & 0xc0, ((lowpan6_buffer[lowpan6_offset] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset+2]); + lowpan6_offset += 3; + } + else if ((lowpan6_buffer[0] & 0x18) == 0x10) + { + IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0); + lowpan6_offset += 1; + } + else if ((lowpan6_buffer[0] & 0x18) == 0x18) + { + IP6H_VTCFL_SET(ip6hdr, 6, 0, 0); + } + + /* Set Next Header */ + if ((lowpan6_buffer[0] & 0x04) == 0x00) + { + IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]); + } + else + { + /* We should fill this later with NHC decoding */ + IP6H_NEXTH_SET(ip6hdr, 0); + } + + /* Set Hop Limit */ + if ((lowpan6_buffer[0] & 0x03) == 0x00) + { + IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]); + } + else if ((lowpan6_buffer[0] & 0x03) == 0x01) + { + IP6H_HOPLIM_SET(ip6hdr, 1); + } + else if ((lowpan6_buffer[0] & 0x03) == 0x02) + { + IP6H_HOPLIM_SET(ip6hdr, 64); + } + else if ((lowpan6_buffer[0] & 0x03) == 0x03) + { + IP6H_HOPLIM_SET(ip6hdr, 255); + } + + /* Source address decoding. */ + if ((lowpan6_buffer[1] & 0x40) == 0x00) + { + /* Stateless compression */ + if ((lowpan6_buffer[1] & 0x30) == 0x00) + { + /* copy full address */ + MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16); + lowpan6_offset += 16; + } + else if ((lowpan6_buffer[1] & 0x30) == 0x10) + { + ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); + ip6hdr->src.addr[1] = 0; + MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8); + lowpan6_offset += 8; + } + else if ((lowpan6_buffer[1] & 0x30) == 0x20) + { + ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); + ip6hdr->src.addr[1] = 0; + ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); + ip6hdr->src.addr[3] = htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset+1]); + lowpan6_offset += 2; + } + else if ((lowpan6_buffer[1] & 0x30) == 0x30) + { + ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); + ip6hdr->src.addr[1] = 0; + if (src->addr_len == 2) + { + ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); + ip6hdr->src.addr[3] = htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]); + } + else + { + ip6hdr->src.addr[2] = htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | src->addr[2] << 8 | src->addr[3]); + ip6hdr->src.addr[3] = htonl((src->addr[4] << 24) | (src->addr[5] << 16) | src->addr[6] << 8 | src->addr[7]); + } + } + } + else + { + /* Stateful compression */ + if ((lowpan6_buffer[1] & 0x30) == 0x00) + { + /* ANY address */ + ip6hdr->src.addr[0] = 0; + ip6hdr->src.addr[1] = 0; + ip6hdr->src.addr[2] = 0; + ip6hdr->src.addr[3] = 0; + } + else + { + /* Set prefix from context info */ + if (lowpan6_buffer[1] & 0x80) + { + i = (lowpan6_buffer[2] >> 4) & 0x0f; + } + else + { + i = 0; + } + if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) + { + /* Error */ + pbuf_free(p); + pbuf_free(q); + return NULL; + } + + ip6hdr->src.addr[0] = lowpan6_context[i].addr[0]; + ip6hdr->src.addr[1] = lowpan6_context[i].addr[1]; + } + + if ((lowpan6_buffer[1] & 0x30) == 0x10) + { + MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8); + lowpan6_offset += 8; + } + else if ((lowpan6_buffer[1] & 0x30) == 0x20) + { + ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); + ip6hdr->src.addr[3] = htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset+1]); + lowpan6_offset += 2; + } + else if ((lowpan6_buffer[1] & 0x30) == 0x30) + { + if (src->addr_len == 2) + { + ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); + ip6hdr->src.addr[3] = htonl(0xfe000000UL | (src->addr[0] << 8) | src->addr[1]); + } + else + { + ip6hdr->src.addr[2] = htonl(((src->addr[0] ^ 2) << 24) | (src->addr[1] << 16) | src->addr[2] << 8 | src->addr[3]); + ip6hdr->src.addr[3] = htonl((src->addr[4] << 24) | (src->addr[5] << 16) | src->addr[6] << 8 | src->addr[7]); + } + } + } + + /* Destination address decoding. */ + if (lowpan6_buffer[1] & 0x08) + { + /* Multicast destination */ + if (lowpan6_buffer[1] & 0x04) + { + /* TODO support stateful multicast addressing */ + pbuf_free(p); + pbuf_free(q); + return NULL; + } + + if ((lowpan6_buffer[1] & 0x03) == 0x00) + { + /* copy full address */ + MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16); + lowpan6_offset += 16; + } + else if ((lowpan6_buffer[1] & 0x03) == 0x01) + { + ip6hdr->dest.addr[0] = htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++] << 16)); + ip6hdr->dest.addr[1] = 0; + ip6hdr->dest.addr[2] = htonl(lowpan6_buffer[lowpan6_offset++]); + ip6hdr->dest.addr[3] = htonl((lowpan6_buffer[lowpan6_offset] << 24) | (lowpan6_buffer[lowpan6_offset + 1] << 16) | (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset + 3]); + lowpan6_offset += 4; + } + else if ((lowpan6_buffer[1] & 0x03) == 0x02) + { + ip6hdr->dest.addr[0] = htonl(0xff000000UL | lowpan6_buffer[lowpan6_offset++]); + ip6hdr->dest.addr[1] = 0; + ip6hdr->dest.addr[2] = 0; + ip6hdr->dest.addr[3] = htonl((lowpan6_buffer[lowpan6_offset] << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset + 2]); + lowpan6_offset += 3; + } + else if ((lowpan6_buffer[1] & 0x03) == 0x03) + { + ip6hdr->dest.addr[0] = PP_HTONL(0xff020000UL); + ip6hdr->dest.addr[1] = 0; + ip6hdr->dest.addr[2] = 0; + ip6hdr->dest.addr[3] = htonl(lowpan6_buffer[lowpan6_offset++]); + } + + } + else + { + if (lowpan6_buffer[1] & 0x04) + { + /* Stateful destination compression */ + /* Set prefix from context info */ + if (lowpan6_buffer[1] & 0x80) + { + i = lowpan6_buffer[2] & 0x0f; + } + else + { + i = 0; + } + if (i >= LWIP_6LOWPAN_NUM_CONTEXTS) + { + /* Error */ + pbuf_free(p); + pbuf_free(q); + return NULL; + } + + ip6hdr->dest.addr[0] = lowpan6_context[i].addr[0]; + ip6hdr->dest.addr[1] = lowpan6_context[i].addr[1]; + } + else + { + /* Link local address compression */ + ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL); + ip6hdr->dest.addr[1] = 0; + } + + if ((lowpan6_buffer[1] & 0x03) == 0x00) + { + /* copy full address */ + MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16); + lowpan6_offset += 16; + } + else if ((lowpan6_buffer[1] & 0x03) == 0x01) + { + MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8); + lowpan6_offset += 8; + } + else if ((lowpan6_buffer[1] & 0x03) == 0x02) + { + ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL); + ip6hdr->dest.addr[3] = htonl(0xfe000000UL | (lowpan6_buffer[lowpan6_offset] << 8) | lowpan6_buffer[lowpan6_offset + 1]); + lowpan6_offset += 2; + } + else if ((lowpan6_buffer[1] & 0x03) == 0x03) + { + if (dest->addr_len == 2) + { + ip6hdr->dest.addr[2] = PP_HTONL(0x000000ffUL); + ip6hdr->dest.addr[3] = htonl(0xfe000000UL | (dest->addr[0] << 8) | dest->addr[1]); + } + else + { + ip6hdr->dest.addr[2] = htonl(((dest->addr[0] ^ 2) << 24) | (dest->addr[1] << 16) | dest->addr[2] << 8 | dest->addr[3]); + ip6hdr->dest.addr[3] = htonl((dest->addr[4] << 24) | (dest->addr[5] << 16) | dest->addr[6] << 8 | dest->addr[7]); + } + } + } + + + /* Next Header Compression (NHC) decoding? */ + if (lowpan6_buffer[0] & 0x04) + { + if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) + { + struct udp_hdr *udphdr; + + /* UDP compression */ + IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP); + udphdr = (struct udp_hdr *)((u8_t *)q->payload + ip6_offset); + + if (lowpan6_buffer[lowpan6_offset] & 0x04) + { + /* TODO support checksum decompress */ + pbuf_free(p); + pbuf_free(q); + return NULL; + } + + /* Decompress ports */ + i = lowpan6_buffer[lowpan6_offset++] & 0x03; + if (i == 0) + { + udphdr->src = htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); + udphdr->dest = htons(lowpan6_buffer[lowpan6_offset + 2] << 8 | lowpan6_buffer[lowpan6_offset + 3]); + lowpan6_offset += 4; + } + else if (i == 0x01) + { + udphdr->src = htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); + udphdr->dest = htons(0xf000 | lowpan6_buffer[lowpan6_offset + 2]); + lowpan6_offset += 3; + } + else if (i == 0x02) + { + udphdr->src = htons(0xf000 | lowpan6_buffer[lowpan6_offset]); + udphdr->dest = htons(lowpan6_buffer[lowpan6_offset + 1] << 8 | lowpan6_buffer[lowpan6_offset + 2]); + lowpan6_offset += 3; + } + else if (i == 0x03) + { + udphdr->src = htons(0xf0b0 | ((lowpan6_buffer[lowpan6_offset] >> 4) & 0x0f)); + udphdr->dest = htons(0xf0b0 | (lowpan6_buffer[lowpan6_offset] & 0x0f)); + lowpan6_offset += 1; + } + + udphdr->chksum = htons(lowpan6_buffer[lowpan6_offset] << 8 | lowpan6_buffer[lowpan6_offset + 1]); + lowpan6_offset += 2; + udphdr->len = htons(p->tot_len - lowpan6_offset + UDP_HLEN); + + ip6_offset += UDP_HLEN; + } + else + { + /* TODO support NHC other than UDP */ + pbuf_free(p); + pbuf_free(q); + return NULL; + } + } + + /* Now we copy leftover contents from p to q, so we have all L2 and L3 headers (and L4?) in a single PBUF. + * Replace p with q, and free p */ + pbuf_header(p, -lowpan6_offset); + MEMCPY((u8_t*)q->payload + ip6_offset, p->payload, p->len); + q->len = q->tot_len = ip6_offset + p->len; + if (p->next != NULL) { + pbuf_cat(q, p->next); + } + p->next = NULL; + pbuf_free(p); + + /* Infer IPv6 payload length for header */ + IP6H_PLEN_SET(ip6hdr, q->tot_len - IP6_HLEN); + + /* all done */ + return q; +} + +err_t +lowpan6_input(struct pbuf * p, struct netif *netif) +{ + u8_t * puc; + s8_t i; + struct ieee_802154_addr src, dest; + u16_t datagram_size, datagram_offset, datagram_tag; + struct lowpan6_reass_helper *lrh, *lrh_temp; + + /* Analyze header. TODO validate. */ + puc = (u8_t*)p->payload; + datagram_offset = 5; + if ((puc[1] & 0x0c) == 0x0c) + { + dest.addr_len = 8; + for (i = 0; i < 8; i++) + { + dest.addr[i] = puc[datagram_offset + 7 - i]; + } + datagram_offset += 8; + } + else + { + dest.addr_len = 2; + dest.addr[0] = puc[datagram_offset + 1]; + dest.addr[1] = puc[datagram_offset]; + datagram_offset += 2; + } + + datagram_offset += 2; /* skip PAN ID. */ + + if ((puc[1] & 0xc0) == 0xc0) + { + src.addr_len = 8; + for (i = 0; i < 8; i++) + { + src.addr[i] = puc[datagram_offset + 7 - i]; + } + datagram_offset += 8; + } + else + { + src.addr_len = 2; + src.addr[0] = puc[datagram_offset + 1]; + src.addr[1] = puc[datagram_offset]; + datagram_offset += 2; + } + + pbuf_header(p, -datagram_offset); /* hide IEEE802.15.4 header. */ + + /* Check dispatch. */ + puc = (u8_t*)p->payload; + + if ((*puc & 0xf8) == 0xc0) + { + /* FRAG1 dispatch. add this packet to reassembly list. */ + datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1]; + datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3]; + + /* check for duplicate */ + lrh = reass_list; + while (lrh != NULL) + { + if ((lrh->sender_addr.addr_len == src.addr_len) && + (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0)) + { + /* address match with packet in reassembly. */ + if ((datagram_tag == lrh->datagram_tag) && (datagram_size == lrh->datagram_size)) { + /* duplicate fragment. */ + pbuf_free(p); + return ERR_OK; + } + else { + /* We are receiving the start of a new datagram. Discard old one (incomplete). */ + lrh_temp = lrh->next_packet; + dequeue_datagram(lrh); + pbuf_free(lrh->pbuf); + mem_free(lrh); + + /* Check next datagram in queue. */ + lrh = lrh_temp; + } + } + else { + /* Check next datagram in queue. */ + lrh = lrh->next_packet; + } + } + + pbuf_header(p, -4); /* hide frag1 dispatch */ + + lrh = (struct lowpan6_reass_helper *) mem_malloc(sizeof(struct lowpan6_reass_helper)); + if (lrh == NULL) { + pbuf_free(p); + return ERR_MEM; + } + + lrh->sender_addr.addr_len = src.addr_len; + for (i = 0; i < src.addr_len; i++) { + lrh->sender_addr.addr[i] = src.addr[i]; + } + lrh->datagram_size = datagram_size; + lrh->datagram_tag = datagram_tag; + lrh->pbuf = p; + lrh->next_packet = reass_list; + lrh->timer = 2; + reass_list = lrh; + + return ERR_OK; + } + else if ((*puc & 0xf8) == 0xe0) + { + /* FRAGN dispatch, find packet being reassembled. */ + datagram_size = ((u16_t)(puc[0] & 0x07) << 8) | (u16_t)puc[1]; + datagram_tag = ((u16_t)puc[2] << 8) | (u16_t)puc[3]; + datagram_offset = (u16_t)puc[4] << 3; + pbuf_header(p, -5); /* hide frag1 dispatch */ + + for (lrh = reass_list; lrh != NULL; lrh = lrh->next_packet) + { + if ( + (lrh->sender_addr.addr_len == src.addr_len) && + (memcmp(lrh->sender_addr.addr, src.addr, src.addr_len) == 0) && + (datagram_tag == lrh->datagram_tag) && + (datagram_size == lrh->datagram_size) + ) + { + break; + } + } + if (lrh == NULL) + { + /* rogue fragment */ + pbuf_free(p); + return ERR_OK; + } + + if (lrh->pbuf->tot_len < datagram_offset) + { + /* duplicate, ignore. */ + pbuf_free(p); + return ERR_OK; + } + else if (lrh->pbuf->tot_len > datagram_offset) + { + /* We have missed a fragment. Delete whole reassembly. */ + dequeue_datagram(lrh); + pbuf_free(lrh->pbuf); + mem_free(lrh); + pbuf_free(p); + return ERR_OK; + } + pbuf_cat(lrh->pbuf, p); + p = NULL; + + /* is packet now complete?*/ + if (lrh->pbuf->tot_len >= lrh->datagram_size) + { + /* dequeue from reass list. */ + dequeue_datagram(lrh); + + /* get pbuf */ + p = lrh->pbuf; + + /* release helper */ + mem_free(lrh); + } + else + { + return ERR_OK; + } + } + + if (p == NULL) return ERR_OK; + + /* We have a complete packet, check dispatch for headers. */ + puc = (u8_t*)p->payload; + + if (*puc == 0x41) + { + /* This is a complete IPv6 packet, just skip dispatch byte. */ + pbuf_header(p, -1); /* hide dispatch byte. */ + } + else if ((*puc & 0xe0 )== 0x60) + { + /* IPv6 headers are compressed using IPHC. */ + p = lowpan6_decompress(p, &src, &dest); + if (p == NULL) return ERR_OK; + } + else + { + pbuf_free(p); + return ERR_OK; + } + + return ip6_input(p, netif); +} + +err_t +lowpan6_if_init(struct netif *netif) +{ + netif->name[0] = 'L'; + netif->name[1] = '6'; +#if LWIP_IPV4 + netif->output = lowpan4_output; +#endif /* LWIP_IPV4 */ + netif->output_ip6 = lowpan6_output; + + /* maximum transfer unit */ + netif->mtu = 1280; + + /* broadcast capability */ + netif->flags = NETIF_FLAG_BROADCAST /* | NETIF_FLAG_LOWPAN6 */; + + return ERR_OK; +} + +err_t +lowpan6_set_pan_id(u16_t pan_id) +{ + ieee_802154_pan_id = pan_id; + + return ERR_OK; +} + +/** + * Pass a received packet to tcpip_thread for input processing + * + * @param p the received packet, p->payload pointing to the + * IEEE 802.15.4 header. + * @param inp the network interface on which the packet was received + */ +err_t +tcpip_6lowpan_input(struct pbuf *p, struct netif *inp) +{ + return tcpip_inpkt(p, inp, lowpan6_input); +} + +#endif /* LWIP_IPV6 && LWIP_6LOWPAN */