From 8adfc730ec2556a1f2c1221e0008b57d9e57baea Mon Sep 17 00:00:00 2001 From: goldsimon Date: Wed, 21 Mar 2018 22:57:45 +0100 Subject: [PATCH] lowpan6_ble.c: use common functions from lowpan6_common.c --- src/include/netif/lowpan6_ble.h | 25 +- src/include/netif/lowpan6_ble_opts.h | 87 ---- src/include/netif/lowpan6_opts.h | 16 + src/netif/lowpan6_ble.c | 736 ++++----------------------- 4 files changed, 126 insertions(+), 738 deletions(-) delete mode 100644 src/include/netif/lowpan6_ble_opts.h diff --git a/src/include/netif/lowpan6_ble.h b/src/include/netif/lowpan6_ble.h index ce567c91..68992231 100644 --- a/src/include/netif/lowpan6_ble.h +++ b/src/include/netif/lowpan6_ble.h @@ -36,13 +36,14 @@ * Based on the original 6lowpan implementation of lwIP ( @see 6lowpan.c) */ -#ifndef LWIP_HDR_RFC7668_H -#define LWIP_HDR_RFC7668_H +#ifndef LWIP_HDR_LOWPAN6_BLE_H +#define LWIP_HDR_LOWPAN6_BLE_H -#include "netif/lowpan6_ble_opts.h" +#include "netif/lowpan6_opts.h" -#if LWIP_IPV6 && LWIP_RFC7668 /* don't build if not configured for use in lwipopts.h */ +#if LWIP_IPV6 /* don't build if not configured for use in lwipopts.h */ +#include "netif/lowpan6_common.h" #include "lwip/pbuf.h" #include "lwip/ip.h" #include "lwip/ip_addr.h" @@ -53,17 +54,23 @@ extern "C" { #endif err_t rfc7668_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr); -err_t rfc7668_input(struct pbuf * p, struct netif *netif, const ip6_addr_t *src); +err_t rfc7668_input(struct pbuf * p, struct netif *netif); +err_t rfc7668_set_peer_addr_eui64(struct netif *netif, const u8_t *peer_addr, size_t peer_addr_len); +err_t rfc7668_set_peer_addr_mac48(struct netif *netif, const u8_t *peer_addr, size_t peer_addr_len, int is_public_addr); +err_t rfc7668_set_context(u8_t index, const ip6_addr_t * context); err_t rfc7668_if_init(struct netif *netif); +#if !NO_SYS +err_t tcpip_rfc7668_input(struct pbuf *p, struct netif *inp); +#endif -void ble_addr_to_eui64(uint8_t *dst, uint8_t *src, uint8_t public_addr); -void eui64_to_ble_addr(uint8_t *dst, uint8_t *src); +void ble_addr_to_eui64(uint8_t *dst, const uint8_t *src, int public_addr); +void eui64_to_ble_addr(uint8_t *dst, const uint8_t *src); #ifdef __cplusplus } #endif -#endif /* LWIP_IPV6 && LWIP_RFC7668 */ +#endif /* LWIP_IPV6 */ -#endif /* LWIP_HDR_RFC7668_H */ +#endif /* LWIP_HDR_LOWPAN6_BLE_H */ diff --git a/src/include/netif/lowpan6_ble_opts.h b/src/include/netif/lowpan6_ble_opts.h deleted file mode 100644 index ee76f6b4..00000000 --- a/src/include/netif/lowpan6_ble_opts.h +++ /dev/null @@ -1,87 +0,0 @@ -/** - * @file - * 6LowPAN over BLE for IPv6 (RFC7668). Config file - */ - -/* - * Copyright (c) 2017 Benjamin Aigner - * Copyright (c) 2015 Inico Technologies Ltd. , Author: Ivan Delamer - * - * 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. - * - * Author: Benjamin Aigner - * - * Based on the original 6lowpan implementation of lwIP ( @see 6lowpan.c) - */ - -#ifndef LWIP_HDR_RFC7668_OPTS_H -#define LWIP_HDR_RFC7668_OPTS_H - -#include "lwip/opt.h" - -/** LWIP_RFC7668==1: Enable the RFC7668 netif.*/ -#ifndef LWIP_RFC7668 -#define LWIP_RFC7668 1 -#endif - -/** LWIP_RFC7668_NUM_CONTEXTS: define the number of compression - * contexts. - * CURRENTLY NOT SUPPORTED. */ -#ifndef LWIP_RFC7668_NUM_CONTEXTS -#define LWIP_RFC7668_NUM_CONTEXTS 10 -#endif - -/** LWIP_RFC7668_DEBUG: Enable generic debugging in lowpan6_ble.c. */ -#ifndef LWIP_RFC7668_DEBUG -#define LWIP_RFC7668_DEBUG LWIP_DBG_OFF -#endif - -/** LWIP_RFC7668_IP_COMPRESSED_DEBUG: enable compressed IP frame - * output debugging */ -#ifndef LWIP_RFC7668_IP_COMPRESSED_DEBUG -#define LWIP_RFC7668_IP_COMPRESSED_DEBUG LWIP_DBG_OFF -#endif - -/** LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG: enable decompressed IP frame - * output debugging */ -#ifndef LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG -#define LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG LWIP_DBG_OFF -#endif - -/** LWIP_RFC7668_DECOMPRESSION_DEBUG: enable decompression debug output*/ -#ifndef LWIP_RFC7668_DECOMPRESSION_DEBUG -#define LWIP_RFC7668_DECOMPRESSION_DEBUG LWIP_DBG_OFF -#endif - -/** LWIP_RFC7668_LINUX_WORKAROUND_PUBLIC_ADDRESS: - * Currently, the linux kernel driver for 6lowpan sets/clears a bit in - * the address, depending on the BD address (either public or not). - * Might not be RFC7668 conform, so you may select to do that (=1) or - * not (=0) */ -#ifndef LWIP_RFC7668_LINUX_WORKAROUND_PUBLIC_ADDRESS -#define LWIP_RFC7668_LINUX_WORKAROUND_PUBLIC_ADDRESS 1 -#endif - -#endif /* LWIP_HDR_RFC7668_OPTS_H */ diff --git a/src/include/netif/lowpan6_opts.h b/src/include/netif/lowpan6_opts.h index c3a72648..17d46cdc 100644 --- a/src/include/netif/lowpan6_opts.h +++ b/src/include/netif/lowpan6_opts.h @@ -103,4 +103,20 @@ #define LWIP_LOWPAN6_DECOMPRESSION_DEBUG LWIP_DBG_OFF #endif +/** LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG: enable decompressed IP frame + * output debugging */ +#ifndef LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG +#define LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG LWIP_DBG_OFF +#endif + +/** LWIP_RFC7668_LINUX_WORKAROUND_PUBLIC_ADDRESS: + * Currently, the linux kernel driver for 6lowpan sets/clears a bit in + * the address, depending on the BD address (either public or not). + * Might not be RFC7668 conform, so you may select to do that (=1) or + * not (=0) */ +#ifndef LWIP_RFC7668_LINUX_WORKAROUND_PUBLIC_ADDRESS +#define LWIP_RFC7668_LINUX_WORKAROUND_PUBLIC_ADDRESS 1 +#endif + + #endif /* LWIP_HDR_LOWPAN6_OPTS_H */ diff --git a/src/netif/lowpan6_ble.c b/src/netif/lowpan6_ble.c index cf96a387..9e7b61db 100644 --- a/src/netif/lowpan6_ble.c +++ b/src/netif/lowpan6_ble.c @@ -68,7 +68,7 @@ #include "netif/lowpan6_ble.h" -#if LWIP_IPV6 && LWIP_RFC7668 +#if LWIP_IPV6 #include "lwip/ip.h" #include "lwip/pbuf.h" @@ -82,14 +82,14 @@ #include -#if LWIP_RFC7668_NUM_CONTEXTS > 0 - /** context memory, containing IPv6 addresses */ - static ip6_addr_t rfc7668_context[LWIP_RFC7668_NUM_CONTEXTS]; +#if LWIP_6LOWPAN_NUM_CONTEXTS > 0 +/** context memory, containing IPv6 addresses */ +static ip6_addr_t rfc7668_context[LWIP_6LOWPAN_NUM_CONTEXTS]; +#else +#define rfc7668_context NULL #endif -err_t tcpip_rfc7668_input(struct pbuf *p, struct netif *inp); -err_t rfc7668_set_context(u8_t index, const ip6_addr_t * context); - +static struct lowpan6_link_addr rfc7668_peer_addr; /** * @ingroup rfc7668if @@ -105,7 +105,7 @@ err_t rfc7668_set_context(u8_t index, const ip6_addr_t * context); * * @see LWIP_RFC7668_LINUX_WORKAROUND_PUBLIC_ADDRESS */ -void ble_addr_to_eui64(uint8_t *dst, uint8_t *src, uint8_t public_addr) +void ble_addr_to_eui64(uint8_t *dst, const uint8_t *src, int public_addr) { /* according to RFC7668 ch 3.2.2. */ memcpy(dst, src, 3); @@ -133,38 +133,46 @@ void ble_addr_to_eui64(uint8_t *dst, uint8_t *src, uint8_t public_addr) * @param dst BLE MAC address destination * */ -void eui64_to_ble_addr(uint8_t *dst, uint8_t *src) +void eui64_to_ble_addr(uint8_t *dst, const uint8_t *src) { /* according to RFC7668 ch 3.2.2. */ memcpy(dst,src,3); memcpy(&dst[3],&src[5],3); } -/** context lookup; find context ID for IPv6 address - * - * @param ip6addr Pointer to IPv6 address struct - * - * @return The context id, if one found; -1 if no context id found +/** Set the peer address used for stateful compression. + * This expects an address of 8 bytes. */ -static s8_t -rfc7668_context_lookup(const ip6_addr_t *ip6addr) +err_t +rfc7668_set_peer_addr_eui64(struct netif *netif, const u8_t *peer_addr, size_t peer_addr_len) { -#if LWIP_RFC7668_NUM_CONTEXTS > 0 - s8_t i; - /* iterate over all possible context addresses */ - for (i = 0; i < LWIP_RFC7668_NUM_CONTEXTS; i++) { - /* if a match is found, return id */ - if (ip6_addr_netcmp(&rfc7668_context[i], ip6addr)) { - return i; - } + /* netif not used for now, the address is stored globally... */ + LWIP_UNUSED_ARG(netif); + if ((peer_addr == NULL) || (peer_addr_len != 8)) { + return ERR_VAL; } -#else - LWIP_UNUSED_ARG(ip6addr); -#endif - /* no address found, return -1 */ - return -1; + rfc7668_peer_addr.addr_len = 8; + memcpy(rfc7668_peer_addr.addr, peer_addr, 8); + return ERR_OK; } +/** Set the peer address used for stateful compression. + * This expects an address of 6 bytes. + */ +err_t +rfc7668_set_peer_addr_mac48(struct netif *netif, const u8_t *peer_addr, size_t peer_addr_len, int is_public_addr) +{ + /* netif not used for now, the address is stored globally... */ + LWIP_UNUSED_ARG(netif); + if ((peer_addr == NULL) || (peer_addr_len != 6)) { + return ERR_VAL; + } + rfc7668_peer_addr.addr_len = 8; + ble_addr_to_eui64(rfc7668_peer_addr.addr, peer_addr, is_public_addr); + return ERR_OK; +} + + /** Determine unicast address compression mode * * NOT IMPLEMENTED. This method will determine if an address should @@ -213,249 +221,68 @@ rfc7668_get_address_mode_mc(const ip6_addr_t *ip6addr) * *) Fragmentation not necessary (done at L2CAP layer of BLE) * @note Currently the pbuf allocation uses 256 bytes. If longer packets are used (possible due to MTU=1480Bytes), increase it here! * - * @param dst Pointer to IPv6 address struct (destination) - * @param src Pointer to IPv6 address struct (source) * @param p Pbuf struct, containing the payload data * @param netif Output network interface. Should be of RFC7668 type * * @return Same as netif->output. */ static err_t -rfc7668_frag(struct netif *netif, struct pbuf *p, const ip6_addr_t * src, const ip6_addr_t *dst) +rfc7668_compress(struct netif *netif, struct pbuf *p) { - struct pbuf * p_frag; - u16_t frag_len, remaining_len; - u8_t * buffer; + struct pbuf *p_frag; + u16_t remaining_len; + u8_t *buffer; u8_t lowpan6_header_len; - s8_t i; - err_t err = ERR_IF; + u8_t hidden_header_len; + err_t err; - /* We'll use a dedicated pbuf for building BLE fragments. */ - p_frag = pbuf_alloc(PBUF_RAW, 256, PBUF_RAM); + LWIP_ASSERT("lowpan6_frag: netif->linkoutput not set", netif->linkoutput != NULL); + +#if LWIP_6LOWPAN_IPHC + + /* We'll use a dedicated pbuf for building BLE fragments. + * We'll over-allocate it by the bytes saved for header compression. + */ + p_frag = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM); if (p_frag == NULL) { MIB2_STATS_NETIF_INC(netif, ifoutdiscards); return ERR_MEM; } + LWIP_ASSERT("this needs a pbuf in one piece", p_frag->len == p_frag->tot_len); + /* Write IP6 header (with IPHC). */ - buffer = (u8_t*)p_frag->payload; + buffer = (u8_t*)p_frag->payload; - /* Perform IPv6 header compression according to RFC 6282 NECESSARY!*/ - { - 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_packed(ip_data.current_iphdr_dest, ip6hdr->dest); - ip_addr_copy_from_ip6_packed(ip_data.current_iphdr_src, ip6hdr->src); - - /* Basic length of 6LowPAN header, set dispatch and clear fields. */ - lowpan6_header_len = 2; - buffer[0] = 0x60; - buffer[1] = 0; - - /* Determine whether there will be a Context Identifier Extension byte or not. - * If so, set it already. */ -#if LWIP_RFC7668_NUM_CONTEXTS > 0 - buffer[2] = 0; - - i = rfc7668_context_lookup(ip_2_ip6(&ip_data.current_iphdr_src)); - if (i >= 0) { - /* Stateful source address compression. */ - buffer[1] |= 0x40; - buffer[2] |= (i & 0x0f) << 4; - } - - i = rfc7668_context_lookup(ip_2_ip6(&ip_data.current_iphdr_dest)); - if (i >= 0) { - /* Stateful destination address compression. */ - buffer[1] |= 0x04; - buffer[2] |= i & 0x0f; - } - - if (buffer[2] != 0x00) { - /* Context identifier extension byte is appended. */ - buffer[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[0] |= 0x10; - if (IP6H_TC(ip6hdr) == 0) { - /* Traffic class (ECN+DSCP) elided too. */ - buffer[0] |= 0x08; - } else { - /* Traffic class (ECN+DSCP) appended. */ - buffer[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[0] |= 0x08; - - buffer[lowpan6_header_len] = IP6H_TC(ip6hdr) & 0xc0; - buffer[lowpan6_header_len++] |= (IP6H_FL(ip6hdr) >> 16) & 0x0f; - buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff; - buffer[lowpan6_header_len++] = IP6H_FL(ip6hdr) & 0xff; - } else { - /* Traffic class and flow label are appended (4 bytes) */ - buffer[lowpan6_header_len++] = IP6H_TC(ip6hdr); - buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 16) & 0x0f; - buffer[lowpan6_header_len++] = (IP6H_FL(ip6hdr) >> 8) & 0xff; - buffer[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[0] |= 0x04; - } else { - /* append nexth. */ - buffer[lowpan6_header_len++] = IP6H_NEXTH(ip6hdr); - } - - /* Compress hop limit? */ - if (IP6H_HOPLIM(ip6hdr) == 255) { - buffer[0] |= 0x03; - } else if (IP6H_HOPLIM(ip6hdr) == 64) { - buffer[0] |= 0x02; - } else if (IP6H_HOPLIM(ip6hdr) == 1) { - buffer[0] |= 0x01; - } else { - /* append hop limit */ - buffer[lowpan6_header_len++] = IP6H_HOPLIM(ip6hdr); - } - - /* Compress source address */ - if (((buffer[1] & 0x40) != 0) || - (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_src)))) { - /* Context-based or link-local source address compression. */ - i = rfc7668_get_address_mode(src); - buffer[1] |= (i & 0x03) << 4; - if (i == 1) { - MEMCPY(buffer + lowpan6_header_len, (u8_t*)p->payload + 16, 8); - lowpan6_header_len += 8; - } else if (i == 2) { - MEMCPY(buffer + 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[1] |= 0x40; - } else { - /* Append full address. */ - MEMCPY(buffer + 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 stateful multicast address compression */ - - buffer[1] |= 0x08; - - i = rfc7668_get_address_mode_mc(ip_2_ip6(&ip_data.current_iphdr_dest)); - buffer[1] |= i & 0x03; - if (i == 0) { - MEMCPY(buffer + lowpan6_header_len, (u8_t*)p->payload + 24, 16); - lowpan6_header_len += 16; - } else if (i == 1) { - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[25]; - MEMCPY(buffer + lowpan6_header_len, (u8_t*)p->payload + 35, 5); - lowpan6_header_len += 5; - } else if (i == 2) { - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[25]; - MEMCPY(buffer + lowpan6_header_len, (u8_t*)p->payload + 37, 3); - lowpan6_header_len += 3; - } else if (i == 3) { - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[39]; - } - } else if (((buffer[1] & 0x04) != 0) || - (ip6_addr_islinklocal(ip_2_ip6(&ip_data.current_iphdr_dest)))) { - /* Context-based or link-local destination address compression. */ - i = rfc7668_get_address_mode(dst); - buffer[1] |= i & 0x03; - if (i == 1) { - MEMCPY(buffer + lowpan6_header_len, (u8_t*)p->payload + 32, 8); - lowpan6_header_len += 8; - } else if (i == 2) { - MEMCPY(buffer + lowpan6_header_len, (u8_t*)p->payload + 38, 2); - lowpan6_header_len += 2; - } - } else { - /* Append full address. */ - MEMCPY(buffer + lowpan6_header_len, (u8_t*)p->payload + 24, 16); - lowpan6_header_len += 16; - } - - /* Move to payload. */ - pbuf_remove_header(p, IP6_HLEN); - - /* Compress UDP header? */ - if (IP6H_NEXTH(ip6hdr) == IP6_NEXTH_UDP) { - /* @todo support optional checksum compression */ - - buffer[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[lowpan6_header_len++] |= 0x03; - buffer[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[lowpan6_header_len++] |= 0x02; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[1]; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[2]; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[3]; - } else if (((u8_t *)p->payload)[2] == 0xf0) { - /* Compress dest port. */ - buffer[lowpan6_header_len++] |= 0x01; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[0]; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[1]; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[3]; - } else { - /* append full ports. */ - lowpan6_header_len++; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[0]; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[1]; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[2]; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[3]; - } - - /* elide length and copy checksum */ - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[6]; - buffer[lowpan6_header_len++] = ((u8_t *)p->payload)[7]; - - pbuf_remove_header(p, UDP_HLEN); - } + err = lowpan6_compress_headers(netif, (u8_t *)p->payload, p->len, buffer, p_frag->len, + &lowpan6_header_len, &hidden_header_len, rfc7668_context, NULL, &rfc7668_peer_addr); + if (err != ERR_OK) { + MIB2_STATS_NETIF_INC(netif, ifoutdiscards); + pbuf_free(p_frag); + return err; } + pbuf_remove_header(p, hidden_header_len); /* Calculate remaining packet length */ remaining_len = p->tot_len; - /* It fits in one frame. */ - frag_len = remaining_len; - /* Copy IPv6 packet */ - pbuf_copy_partial(p, buffer + lowpan6_header_len, frag_len, 0); - remaining_len = 0; + pbuf_copy_partial(p, buffer + lowpan6_header_len, remaining_len, 0); /* Calculate frame length */ - p_frag->len = p_frag->tot_len = frag_len + lowpan6_header_len; + p_frag->len = p_frag->tot_len = remaining_len + lowpan6_header_len; /* send the packet */ MIB2_STATS_NETIF_ADD(netif, ifoutoctets, p_frag->tot_len); - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_DBG_TRACE, ("rfc7668_send: sending packet %p\n", (void *)p)); + LWIP_DEBUGF(LWIP_LOWPAN6_DEBUG|LWIP_DBG_TRACE, ("rfc7668_output: sending packet %p\n", (void *)p)); err = netif->linkoutput(netif, p_frag); pbuf_free(p_frag); return err; +#else /* LWIP_6LOWPAN_IPHC */ + /* 6LoWPAN over BLE requires IPHC! */ + return ERR_IF; +#endif/* LWIP_6LOWPAN_IPHC */ } /** @@ -472,13 +299,13 @@ rfc7668_frag(struct netif *netif, struct pbuf *p, const ip6_addr_t * src, const err_t rfc7668_set_context(u8_t idx, const ip6_addr_t *context) { -#if LWIP_RFC7668_NUM_CONTEXTS > 0 +#if LWIP_6LOWPAN_NUM_CONTEXTS > 0 /* check if the ID is possible */ - if (idx >= LWIP_RFC7668_NUM_CONTEXTS) { + if (idx >= LWIP_6LOWPAN_NUM_CONTEXTS) { return ERR_ARG; } /* copy IPv6 address to context storage */ - ip6_addr_set(&rfc7668_context[idx], context); + ip6_addr_set(&rfc7668_context[idx], context); return ERR_OK; #else LWIP_UNUSED_ARG(idx); @@ -489,397 +316,23 @@ rfc7668_set_context(u8_t idx, const ip6_addr_t *context) /** * @ingroup rfc7668if - * Resolve and fill-in IEEE 802.15.4 address header for outgoing IPv6 packet. - * - * Perform Header Compression and fragment if necessary. + * Compress outgoing IPv6 packet and pass it on to netif->linkoutput * * @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 See rfc7668_frag + * @return See rfc7668_compress */ err_t rfc7668_output(struct netif *netif, struct pbuf *q, const ip6_addr_t *ip6addr) { - return rfc7668_frag(netif, q, (ip6_addr_t *)netif->ip6_addr, ip6addr); + /* dst ip6addr is not used here, we only have one peer */ + LWIP_UNUSED_ARG(ip6addr); + + return rfc7668_compress(netif, q); } -/** - * Resolve the IPv6 address & metrics (NH, hops,...) from the compressed header - * - * Perform Header Deompression. - * - * @param p The pbuf containing the IP packet to be decompressed. - * @param dest The IP address of the packet destination. - * @param src The IP address of the packet source. - * - * @return pbuf pointer of the processed packet - */ -static struct pbuf * -rfc7668_decompress(struct pbuf * p, const ip6_addr_t * src, const ip6_addr_t * dest) -{ - struct pbuf * q; - u16_t j; - /* temp variable, ease up debug output/processing */ - u32_t header_temp; - u8_t * lowpan6_buffer; - s8_t lowpan6_offset; - struct ip6_hdr *ip6hdr; - s8_t ip6_offset = IP6_HLEN; - - LWIP_UNUSED_ARG(dest); - - /* allocate a new pbuf for the decompressed IPv6 packet */ - q = pbuf_alloc(PBUF_IP, p->len + IP6_HLEN + UDP_HLEN, PBUF_POOL); - if (q == NULL) { - LWIP_DEBUGF(LWIP_DBG_ON,("Out of memory, discarding!!!\n")); - pbuf_free(p); - return NULL; - } - - /* set buffer pointer to payload */ - lowpan6_buffer = (u8_t *)p->payload; - /* set pointer for new ip6 header */ - ip6hdr = (struct ip6_hdr *)q->payload; - - /* output the full compressed packet, if set in @see rfc7668_opt.h */ -#if LWIP_RFC7668_IP_COMPRESSED_DEBUG - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_IP_COMPRESSED_DEBUG,("IP6 payload(compressed): \n")); - for(j = 0; jlen;j++) - { - if((j%4)==0) LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_IP_COMPRESSED_DEBUG,("\n")); - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_IP_COMPRESSED_DEBUG,("%2X ",*((uint8_t *)p->payload+j))); - } - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_IP_COMPRESSED_DEBUG,("\np->len: %d",p->len)); - printf("\np->len: %d\n",p->len); -#endif - - /* offset for inline IP headers (RFC 6282 ch3)*/ - lowpan6_offset = 2; - - /* if CID is set (context identifier), the context byte - * follows immediately after the header, so other IPHC fields are @+3 */ - if (lowpan6_buffer[1] & 0x80) { - lowpan6_offset++; - } - - /* Set IPv6 version, traffic class and flow label. (RFC6282, ch 3.1.1.)*/ - if ((lowpan6_buffer[0] & 0x18) == 0x00) { - header_temp = ((lowpan6_buffer[lowpan6_offset+1] & 0x0f) << 16) | \ - (lowpan6_buffer[lowpan6_offset + 2] << 8) | lowpan6_buffer[lowpan6_offset+3]; - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("TF: 00, ECN: 0x%2x, Flowlabel+DSCP: 0x%8X\n", \ - lowpan6_buffer[lowpan6_offset],header_temp)); - IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset], header_temp); - /* increase offset, processed 4 bytes here: - * TF=00: ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)*/ - lowpan6_offset += 4; - } else if ((lowpan6_buffer[0] & 0x18) == 0x08) { - header_temp = ((lowpan6_buffer[lowpan6_offset] & 0x0f) << 16) | (lowpan6_buffer[lowpan6_offset + 1] << 8) | lowpan6_buffer[lowpan6_offset+2]; - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("TF: 01, ECN: 0x%2x, Flowlabel: 0x%2X, DSCP ignored\n", \ - lowpan6_buffer[lowpan6_offset] & 0xc0,header_temp)); - IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset] & 0xc0, header_temp); - /* increase offset, processed 3 bytes here: - * TF=01: ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided.*/ - lowpan6_offset += 3; - } else if ((lowpan6_buffer[0] & 0x18) == 0x10) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("TF: 10, DCSP+ECN: 0x%2x, Flowlabel ignored\n",lowpan6_buffer[lowpan6_offset])); - IP6H_VTCFL_SET(ip6hdr, 6, lowpan6_buffer[lowpan6_offset],0); - /* increase offset, processed 1 byte here: - * ECN + DSCP (1 byte), Flow Label is elided.*/ - lowpan6_offset += 1; - } else if ((lowpan6_buffer[0] & 0x18) == 0x18) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("TF: 11, DCSP/ECN & Flowlabel ignored\n")); - /* don't increase offset, no bytes processed here */ - IP6H_VTCFL_SET(ip6hdr, 6, 0, 0); - } - - /* Set Next Header (NH) - * 0: full next header byte carried inline (increase offset)*/ - if ((lowpan6_buffer[0] & 0x04) == 0x00) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("NH: 0x%2X\n",lowpan6_buffer[lowpan6_offset+1])); - IP6H_NEXTH_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]); - /* 1: NH compression, LOWPAN_NHC (RFC6282, ch 4.1) */ - } else { - /* We should fill this later with NHC decoding */ - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("NH: skipped, later done with NHC\n")); - IP6H_NEXTH_SET(ip6hdr, 0); - } - - /* Set Hop Limit, either carried inline or 3 different hops (1,64,255) */ - if ((lowpan6_buffer[0] & 0x03) == 0x00) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("Hops: full value: %d\n",lowpan6_buffer[lowpan6_offset+1])); - IP6H_HOPLIM_SET(ip6hdr, lowpan6_buffer[lowpan6_offset++]); - } else if ((lowpan6_buffer[0] & 0x03) == 0x01) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("Hops: compressed: 1\n")); - IP6H_HOPLIM_SET(ip6hdr, 1); - } else if ((lowpan6_buffer[0] & 0x03) == 0x02) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("Hops: compressed: 64\n")); - IP6H_HOPLIM_SET(ip6hdr, 64); - } else if ((lowpan6_buffer[0] & 0x03) == 0x03) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("Hops: compressed: 255\n")); - IP6H_HOPLIM_SET(ip6hdr, 255); - } - - /* Source address decoding. */ - /* Source address compression (SAC) = 0 -> stateless compression */ - if ((lowpan6_buffer[1] & 0x40) == 0x00) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAC == 0, no context byte\n")); - /* Stateless compression */ - if ((lowpan6_buffer[1] & 0x30) == 0x00) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == 00, no src compression, fetching 128bits inline\n")); - /* copy full address, increase offset by 16 Bytes */ - MEMCPY(&ip6hdr->src.addr[0], lowpan6_buffer + lowpan6_offset, 16); - lowpan6_offset += 16; - } else if ((lowpan6_buffer[1] & 0x30) == 0x10) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == 01, src compression, 64bits inline\n")); - /* set 64 bits to link local */ - ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); - ip6hdr->src.addr[1] = 0; - /* copy 8 Bytes, increase offset */ - MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8); - lowpan6_offset += 8; - } else if ((lowpan6_buffer[1] & 0x30) == 0x20) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == 10, src compression, 16bits inline\n")); - /* set 96 bits to link local */ - ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); - ip6hdr->src.addr[1] = 0; - ip6hdr->src.addr[2] = PP_HTONL(0x000000ffUL); - /* extract remaining 16bits from inline bytes, increase offset */ - 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) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == 11, src compression, 0bits inline, using other headers\n")); - /* no information avalaible, using other layers, see RFC6282 ch 3.2.2 */ - ip6hdr->src.addr[0] = PP_HTONL(0xfe800000UL); - ip6hdr->src.addr[1] = 0; - MEMCPY(&ip6hdr->src.addr[2], (const uint8_t *)src->addr, 8); - } - /* Source address compression (SAC) = 1 -> stateful/context-based compression */ - } else { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAC == 1, additional context byte\n")); - /* Stateful compression */ - /* SAM=00, address=> :: (ANY) */ - 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; - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == 00, context compression, ANY (::)\n")); - } else { - /* Set prefix from context info */ - if (lowpan6_buffer[1] & 0x80) { - j = (lowpan6_buffer[2] >> 4) & 0x0f; - } else { - j = 0; - } - if (j >= LWIP_RFC7668_NUM_CONTEXTS) { - /* Error, not possible (context id too high) */ - pbuf_free(p); - pbuf_free(q); - return NULL; - } -#if LWIP_RFC7668_NUM_CONTEXTS > 0 - /* load prefix from context storage */ - ip6hdr->src.addr[0] = rfc7668_context[j].addr[0]; - ip6hdr->src.addr[1] = rfc7668_context[j].addr[1]; - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == xx, context compression found @%d: %8X, %8X\n", j, ip6hdr->src.addr[0], ip6hdr->src.addr[1])); -#endif - } - - /* determine further address bits */ - /* SAM=01, load additional 64bits */ - if ((lowpan6_buffer[1] & 0x30) == 0x10) { - MEMCPY(&ip6hdr->src.addr[2], lowpan6_buffer + lowpan6_offset, 8); - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == 01, context compression, 64bits inline\n")); - lowpan6_offset += 8; - /* SAM=01, load additional 16bits */ - } 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]); - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == 10, context compression, 16bits inline\n")); - lowpan6_offset += 2; - /* SAM=11, address is fully elided, load from other layers */ - } else if ((lowpan6_buffer[1] & 0x30) == 0x30) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("SAM == 11, context compression, 0bits inline, using other headers\n")); - /* no information avalaible, using other layers, see RFC6282 ch 3.2.2 */ - } - } - - /* Destination address + Multicast decoding. */ - if (lowpan6_buffer[1] & 0x08) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("M=1: multicast\n")); - /* Multicast destination */ - if (lowpan6_buffer[1] & 0x04) { - LWIP_DEBUGF(LWIP_DBG_ON,("DAC == 1, context multicast: unsupported!!!\n")); - /* @todo support stateful multicast addressing */ - pbuf_free(p); - pbuf_free(q); - return NULL; - } else { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAC == 0, stateless multicast\n")); - - if ((lowpan6_buffer[1] & 0x03) == 0x00) { - /* DAM = 00, copy full address (128bits) */ - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAM == 00, no dst compression, fetching 128bits inline")); - MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16); - lowpan6_offset += 16; - } else if ((lowpan6_buffer[1] & 0x03) == 0x01) { - /* DAM = 01, copy 4 bytes (32bits) */ - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAM == 01, dst address form (48bits): ffXX::00XX:XXXX:XXXX\n")); - 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) { - /* DAM = 10, copy 3 bytes (24bits) */ - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAM == 10, dst address form (32bits): ffXX::00XX:XXXX\n")); - ip6hdr->dest.addr[0] = htonl(0xff000000UL | (lowpan6_buffer[lowpan6_offset++]<<16)); - 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) { - /* DAM = 11, copy 1 byte (8bits) */ - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAM == 11, dst address form (8bits): ff02::00XX\n")); - 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 { - /* no Multicast (M=0) */ - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("M=0: no multicast\n")); - - if (lowpan6_buffer[1] & 0x04) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAC == 1, stateful compression\n")); - /* Stateful destination compression */ - /* Set prefix from context info */ - if (lowpan6_buffer[1] & 0x80) { - j = lowpan6_buffer[2] & 0x0f; - } else { - j = 0; - } - if (j >= LWIP_RFC7668_NUM_CONTEXTS) { - /* Error, context id not found */ - pbuf_free(p); - pbuf_free(q); - return NULL; - } -#if LWIP_RFC7668_NUM_CONTEXTS > 0 - ip6hdr->dest.addr[0] = rfc7668_context[j].addr[0]; - ip6hdr->dest.addr[1] = rfc7668_context[j].addr[1]; -#endif - } else { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAC == 0, stateless compression, setting link local prefix\n")); - /* Link local address compression */ - ip6hdr->dest.addr[0] = PP_HTONL(0xfe800000UL); - ip6hdr->dest.addr[1] = 0; - } - - /* M=0, DAC=0, determining destination address length via DAM=xx */ - if ((lowpan6_buffer[1] & 0x03) == 0x00) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAM == 00, no dst compression, fetching 128bits inline")); - /* DAM=00, copy full address */ - MEMCPY(&ip6hdr->dest.addr[0], lowpan6_buffer + lowpan6_offset, 16); - lowpan6_offset += 16; - } else if ((lowpan6_buffer[1] & 0x03) == 0x01) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAM == 01, dst compression, 64bits inline\n")); - /* DAM=01, copy 64 inline bits, increase offset */ - MEMCPY(&ip6hdr->dest.addr[2], lowpan6_buffer + lowpan6_offset, 8); - lowpan6_offset += 8; - } else if ((lowpan6_buffer[1] & 0x03) == 0x02) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAM == 01, dst compression, 16bits inline\n")); - /* DAM=10, copy 16 inline bits, increase offset */ - 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) { - /* DAM=11, no bits available, use other headers (not done here) */ - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("DAM == 01, dst compression, 0bits inline, using other headers\n")); - } - } - - /* Next Header Compression (NHC) decoding? */ - if (lowpan6_buffer[0] & 0x04) { - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("NHC decoding\n")); - - /* NHC: UDP */ - if ((lowpan6_buffer[lowpan6_offset] & 0xf8) == 0xf0) { - struct udp_hdr *udphdr; - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("NHC: UDP\n")); - - /* UDP compression */ - IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_UDP); - udphdr = (struct udp_hdr *)((u8_t *)q->payload + ip6_offset); - - /* Checksum decompression */ - if (lowpan6_buffer[lowpan6_offset] & 0x04) { - /* @todo support checksum decompress */ - LWIP_DEBUGF(LWIP_DBG_ON,("NHC: UDP chechsum decompression UNSUPPORTED\n")); - pbuf_free(p); - pbuf_free(q); - return NULL; - } - - /* Decompress ports, according to RFC4944 */ - j = lowpan6_buffer[lowpan6_offset++] & 0x03; - if (j == 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 (j == 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 (j == 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 (j == 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 { - LWIP_DEBUGF(LWIP_DBG_ON,("NHC: unsupported protocol!\n")); - /* @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 */ - LWIP_DEBUGF(LWIP_RFC7668_DEBUG|LWIP_RFC7668_DECOMPRESSION_DEBUG,("IPHC decompression completed, copying remains (%d bytes)\n",p->len-lowpan6_offset)); - - MEMCPY((u8_t*)q->payload + ip6_offset, (u8_t *)p->payload + lowpan6_offset, p->len-lowpan6_offset); - q->len = q->tot_len = ip6_offset + p->len - lowpan6_offset; - 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; -} - - - /** * @ingroup rfc7668if * Process a received raw payload from an L2CAP channel @@ -893,10 +346,9 @@ rfc7668_decompress(struct pbuf * p, const ip6_addr_t * src, const ip6_addr_t * d * @return ERR_OK if everything was fine */ err_t -rfc7668_input(struct pbuf * p, struct netif *netif, const ip6_addr_t *src) +rfc7668_input(struct pbuf * p, struct netif *netif) { u8_t * puc; - ip6_addr_t dest; MIB2_STATS_NETIF_ADD(netif, ifinoctets, p->tot_len); @@ -905,22 +357,22 @@ rfc7668_input(struct pbuf * p, struct netif *netif, const ip6_addr_t *src) /* no IP header compression */ if (*puc == 0x41) { - LWIP_DEBUGF(LWIP_RFC7668_DECOMPRESSION_DEBUG | LWIP_RFC7668_DEBUG, ("Completed packet, removing dispatch: 0x%2x \n", *puc)); + LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Completed packet, removing dispatch: 0x%2x \n", *puc)); /* This is a complete IPv6 packet, just skip header byte. */ pbuf_remove_header(p, 1); /* IPHC header compression */ } else if ((*puc & 0xe0 )== 0x60) { - LWIP_DEBUGF(LWIP_RFC7668_DECOMPRESSION_DEBUG | LWIP_RFC7668_DEBUG, ("Completed packet, decompress dispatch: 0x%2x \n", *puc)); + LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Completed packet, decompress dispatch: 0x%2x \n", *puc)); /* IPv6 headers are compressed using IPHC. */ - p = rfc7668_decompress(p, src, &dest); + p = lowpan6_decompress(p, 0, rfc7668_context, &rfc7668_peer_addr, NULL); /* if no pbuf is returned, handle as discarded packet */ if (p == NULL) { MIB2_STATS_NETIF_INC(netif, ifindiscards); return ERR_OK; } - /* invalid header byte, discard */ + /* invalid header byte, discard */ } else { - LWIP_DEBUGF(LWIP_RFC7668_DECOMPRESSION_DEBUG | LWIP_RFC7668_DEBUG, ("Completed packet, discarding: 0x%2x \n", *puc)); + LWIP_DEBUGF(LWIP_LOWPAN6_DECOMPRESSION_DEBUG, ("Completed packet, discarding: 0x%2x \n", *puc)); MIB2_STATS_NETIF_INC(netif, ifindiscards); pbuf_free(p); return ERR_OK; @@ -928,16 +380,17 @@ rfc7668_input(struct pbuf * p, struct netif *netif, const ip6_addr_t *src) /* @todo: distinguish unicast/multicast */ MIB2_STATS_NETIF_INC(netif, ifinucastpkts); -#if LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG==LWIP_DBG_ON +#if LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG { u16_t i; - LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG | LWIP_RFC7668_DEBUG, ("IPv6 payload:\n")); - for(i = 0; ilen;i++) - { - if((i%4)==0) LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG | LWIP_RFC7668_DEBUG, ("\n")); - LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG | LWIP_RFC7668_DEBUG, ("%2X ", *((uint8_t *)p->payload+i))); + LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG, ("IPv6 payload:\n")); + for (i = 0; i < p->len; i++) { + if ((i%4)==0) { + LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG, ("\n")); + } + LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG, ("%2X ", *((uint8_t *)p->payload+i))); } - LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG | LWIP_RFC7668_DEBUG, ("\np->len: %d\n", p->len)); + LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG, ("\np->len: %d\n", p->len)); } #endif /* pass data to ip6_input */ @@ -975,8 +428,7 @@ rfc7668_if_init(struct netif *netif) return ERR_OK; } - -#if 0 /* TODO: tcpip_inpkt() can not take rfc7668_input as input callback */ +#if !NO_SYS /** * Pass a received packet to tcpip_thread for input processing * @@ -992,6 +444,6 @@ tcpip_rfc7668_input(struct pbuf *p, struct netif *inp) /* send data to upper layer, return the result */ return tcpip_inpkt(p, inp, rfc7668_input); } -#endif /* TODO */ +#endif /* !NO_SYS */ -#endif /* LWIP_IPV6 && LWIP_RFC7668 */ +#endif /* LWIP_IPV6 */