mirror of
https://github.com/lwip-tcpip/lwip.git
synced 2024-10-05 22:29:49 +00:00
lowpan6_ble.c: use common functions from lowpan6_common.c
This commit is contained in:
goldsimon
parent
ac03107036
commit
8adfc730ec
@ -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 */
|
||||
|
||||
@ -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 <delamer@inicotech.com>
|
||||
*
|
||||
* 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 <aignerb@technikum-wien.at>
|
||||
*
|
||||
* 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 */
|
||||
@ -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 */
|
||||
|
||||
@ -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 <string.h>
|
||||
|
||||
#if LWIP_RFC7668_NUM_CONTEXTS > 0
|
||||
#if LWIP_6LOWPAN_NUM_CONTEXTS > 0
|
||||
/** context memory, containing IPv6 addresses */
|
||||
static ip6_addr_t rfc7668_context[LWIP_RFC7668_NUM_CONTEXTS];
|
||||
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,37 +133,45 @@ 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;
|
||||
}
|
||||
rfc7668_peer_addr.addr_len = 8;
|
||||
memcpy(rfc7668_peer_addr.addr, peer_addr, 8);
|
||||
return ERR_OK;
|
||||
}
|
||||
#else
|
||||
LWIP_UNUSED_ARG(ip6addr);
|
||||
#endif
|
||||
/* no address found, return -1 */
|
||||
return -1;
|
||||
|
||||
/** 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
|
||||
*
|
||||
@ -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;
|
||||
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;
|
||||
|
||||
/* 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,9 +299,9 @@ 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 */
|
||||
@ -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; j<p->len;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,14 +357,14 @@ 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);
|
||||
@ -920,7 +372,7 @@ rfc7668_input(struct pbuf * p, struct netif *netif, const ip6_addr_t *src)
|
||||
}
|
||||
/* 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; i<p->len;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 | LWIP_RFC7668_DEBUG, ("\np->len: %d\n", p->len));
|
||||
LWIP_DEBUGF(LWIP_RFC7668_IP_UNCOMPRESSED_DEBUG, ("%2X ", *((uint8_t *)p->payload+i)));
|
||||
}
|
||||
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 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user