OrgZephyr/zephyr
OrgZephyr
/
zephyr
mirror of https://github.com/zephyrproject-rtos/zephyr.git
1
0
Fork 0
Code Issues Releases Wiki Activity
zephyr/subsys/net/ip/6lo.c

1595 lines
38 KiB
C
Raw Blame History

/** @file
* @brief 6lopan related functions
*/
/*
* Copyright (c) 2016 Intel Corporation
* Copyright (c) 2019 Alexander Wachter
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <logging/log.h>
LOG_MODULE_REGISTER(net_6lo, CONFIG_NET_6LO_LOG_LEVEL);
#include <errno.h>
#include <net/net_core.h>
#include <net/net_if.h>
#include <net/net_stats.h>
#include <net/udp.h>
#include "net_private.h"
#include "6lo.h"
#include "6lo_private.h"
#if defined(CONFIG_NET_6LO_CONTEXT)
struct net_6lo_context {
struct in6_addr prefix;
struct net_if *iface;
u16_t lifetime;
u8_t is_used : 1;
u8_t compress : 1;
u8_t cid : 4;
u8_t unused : 2;
} __packed;
static inline u8_t get_6co_compress(struct net_icmpv6_nd_opt_6co *opt)
{
return (opt->flag & 0x10) >> 4;
}
static inline u8_t get_6co_cid(struct net_icmpv6_nd_opt_6co *opt)
{
return opt->flag & 0x0F;
}
static struct net_6lo_context ctx_6co[CONFIG_NET_MAX_6LO_CONTEXTS];
#endif
static const u8_t udp_nhc_inline_size_table[] = {4, 3, 3, 1};
static const u8_t tf_inline_size_table[] = {4, 3, 1, 0};
/* The first bit of the index is SAC | SAC=0 | SAC=1 |*/
static const u8_t sa_inline_size_table[] = {16, 8, 2, 0, 0, 8, 2, 0};
/* The first bit is M, the second DAC
* | M=0 DAC=0 | M=0 DAC=1 | M=1 DAC=0 | M=1 DAC=1 (DAM always 00)
*/
static const u8_t da_inline_size_table[] = {
16, 8, 2, 0, 0, 8, 2, 0, 16, 6, 4, 1, 6
};
static int get_udp_nhc_inlined_size(u8_t nhc)
{
int size = 0;
if ((nhc & 0xF8) != NET_6LO_NHC_UDP_BARE) {
NET_DBG("UDP NHC dispatch doesn't match");
return 0;
}
if (!(nhc & NET_6LO_NHC_UDP_CHECKSUM)) {
size += 2U;
}
size += udp_nhc_inline_size_table[(nhc & NET_6LO_NHC_UDP_PORT_MASK)];
NET_DBG("Size of inlined UDP HDR data: %d", size);
return size;
}
static int get_ihpc_inlined_size(u16_t iphc)
{
int size = 0;
if (((iphc >> 8) & NET_6LO_DISPATCH_IPHC_MASK) !=
NET_6LO_DISPATCH_IPHC) {
NET_DBG("IPHC dispatch doesn't match");
return -1;
}
size += tf_inline_size_table[(iphc & NET_6LO_IPHC_TF_MASK) >>
NET_6LO_IPHC_TF_POS];
if (!(iphc & NET_6LO_IPHC_NH_MASK)) {
size += 1U;
}
if (!(iphc & NET_6LO_IPHC_HLIM_MASK)) {
size += 1U;
}
if (iphc & NET_6LO_IPHC_CID_MASK) {
size += 1U;
}
size += sa_inline_size_table[(iphc & NET_6LO_IPHC_SA_MASK) >>
NET_6LO_IPHC_SAM_POS];
size += da_inline_size_table[(iphc & NET_6LO_IPHC_DA_MASK) >>
NET_6LO_IPHC_DAM_POS];
NET_DBG("Size of inlined IP HDR data: %d", size);
return size;
}
/* TODO: Unicast-Prefix based IPv6 Multicast(dst) address compression
* Mesh header compression
*/
static inline bool net_6lo_ll_prefix_padded_with_zeros(struct in6_addr *addr)
{
return (net_ipv6_is_ll_addr(addr) &&
(UNALIGNED_GET(&addr->s6_addr16[1]) == 0x00) &&
(UNALIGNED_GET(&addr->s6_addr32[1]) == 0x00));
}
static inline bool net_6lo_addr_16_bit_compressible(struct in6_addr *addr)
{
return ((UNALIGNED_GET(&addr->s6_addr32[2]) == htonl(0xFF)) &&
(UNALIGNED_GET(&addr->s6_addr16[6]) == htons(0xFE00)));
}
static inline bool net_6lo_maddr_8_bit_compressible(struct in6_addr *addr)
{
return ((addr->s6_addr[1] == 0x02) &&
(UNALIGNED_GET(&addr->s6_addr16[1]) == 0x00) &&
(UNALIGNED_GET(&addr->s6_addr32[1]) == 0x00) &&
(UNALIGNED_GET(&addr->s6_addr32[2]) == 0x00) &&
(addr->s6_addr[14] == 0x00));
}
static inline bool net_6lo_maddr_32_bit_compressible(struct in6_addr *addr)
{
return ((UNALIGNED_GET(&addr->s6_addr32[1]) == 0x00) &&
(UNALIGNED_GET(&addr->s6_addr32[2]) == 0x00) &&
(addr->s6_addr[12] == 0x00));
}
static inline bool net_6lo_maddr_48_bit_compressible(struct in6_addr *addr)
{
return ((UNALIGNED_GET(&addr->s6_addr32[1]) == 0x00) &&
(UNALIGNED_GET(&addr->s6_addr16[4]) == 0x00) &&
(addr->s6_addr[10] == 0x00));
}
#if defined(CONFIG_NET_6LO_CONTEXT)
/* RFC 6775, 4.2, 5.4.2, 5.4.3 and 7.2*/
static inline void set_6lo_context(struct net_if *iface, u8_t index,
struct net_icmpv6_nd_opt_6co *context)
{
ctx_6co[index].is_used = true;
ctx_6co[index].iface = iface;
/*TODO: Start timer */
ctx_6co[index].lifetime = context->lifetime;
ctx_6co[index].compress = get_6co_compress(context);
ctx_6co[index].cid = get_6co_cid(context);
net_ipaddr_copy(&ctx_6co[index].prefix, &context->prefix);
}
void net_6lo_set_context(struct net_if *iface,
struct net_icmpv6_nd_opt_6co *context)
{
int unused = -1;
u8_t i;
/* If the context information already exists, update or remove
* as per data.
*/
for (i = 0U; i < CONFIG_NET_MAX_6LO_CONTEXTS; i++) {
if (!ctx_6co[i].is_used) {
unused = i;
continue;
}
if (ctx_6co[i].iface == iface &&
ctx_6co[i].cid == get_6co_cid(context)) {
/* Remove if lifetime is zero */
if (!context->lifetime) {
ctx_6co[i].is_used = false;
return;
}
/* Update the context */
set_6lo_context(iface, i, context);
return;
}
}
/* Cache the context information. */
if (unused != -1) {
set_6lo_context(iface, unused, context);
return;
}
NET_DBG("Either no free slots in the table or exceeds limit");
}
/* Get the context by matching cid */
static inline struct net_6lo_context *
get_6lo_context_by_cid(struct net_if *iface, u8_t cid)
{
u8_t i;
for (i = 0U; i < CONFIG_NET_MAX_6LO_CONTEXTS; i++) {
if (!ctx_6co[i].is_used) {
continue;
}
if (ctx_6co[i].iface == iface && ctx_6co[i].cid == cid) {
return &ctx_6co[i];
}
}
return NULL;
}
/* Get the context by addr */
static inline struct net_6lo_context *
get_6lo_context_by_addr(struct net_if *iface, struct in6_addr *addr)
{
u8_t i;
for (i = 0U; i < CONFIG_NET_MAX_6LO_CONTEXTS; i++) {
if (!ctx_6co[i].is_used) {
continue;
}
if (ctx_6co[i].iface == iface &&
!memcmp(ctx_6co[i].prefix.s6_addr, addr->s6_addr, 8)) {
return &ctx_6co[i];
}
}
return NULL;
}
#endif
/* Helper routine to compress Traffic class and Flow label */
/* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* |Version| Traffic Class | Flow Label |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* version: 4 bits, Traffic Class: 8 bits, Flow label: 20 bits
* The Traffic Class field in the IPv6 header comprises 6 bits of
* Diffserv extension [RFC2474] and 2 bits of Explicit Congestion
* Notification (ECN) [RFC3168]
*/
/* IPHC (compressed) format of traffic class is ECN, DSCP but original
* IPv6 traffic class format is DSCP, ECN.
* DSCP(6), ECN(2).
*/
static u8_t *compress_tfl(struct net_ipv6_hdr *ipv6, u8_t *inline_ptr,
u16_t *iphc)
{
u8_t tcl;
tcl = ((ipv6->vtc & 0x0F) << 4) | ((ipv6->tcflow & 0xF0) >> 4);
tcl = (tcl << 6) | (tcl >> 2); /* ECN(2), DSCP(6) */
if (((ipv6->tcflow & 0x0F) == 0U) && (ipv6->flow == 0U)) {
if (((ipv6->vtc & 0x0F) == 0U) && ((ipv6->tcflow & 0xF0) == 0U)) {
NET_DBG("Traffic class and Flow label elided");
/* Traffic class and Flow label elided */
*iphc |= NET_6LO_IPHC_TF_11;
} else {
NET_DBG("Flow label elided");
/* Flow label elided */
*iphc |= NET_6LO_IPHC_TF_10;
inline_ptr -= sizeof(tcl);
*inline_ptr = tcl;
}
} else {
if (((ipv6->vtc & 0x0F) == 0U) && (ipv6->tcflow & 0x30)) {
NET_DBG("ECN + 2-bit Pad + Flow Label, DSCP is elided");
/* ECN + 2-bit Pad + Flow Label, DSCP is elided.*/
*iphc |= NET_6LO_IPHC_TF_01;
inline_ptr -= sizeof(ipv6->flow);
memmove(inline_ptr, &ipv6->flow, sizeof(ipv6->flow));
inline_ptr -= sizeof(u8_t);
*inline_ptr = (tcl & 0xC0) | (ipv6->tcflow & 0x0F);
} else {
NET_DBG("ECN + DSCP + 4-bit Pad + Flow Label");
/* ECN + DSCP + 4-bit Pad + Flow Label */
*iphc |= NET_6LO_IPHC_TF_00;
inline_ptr -= sizeof(ipv6->flow);
memmove(inline_ptr, &ipv6->flow, sizeof(ipv6->flow));
inline_ptr -= sizeof(u8_t);
*inline_ptr = ipv6->tcflow & 0x0F;
inline_ptr -= sizeof(tcl);
*inline_ptr = tcl;
}
}
return inline_ptr;
}
/* Helper to compress Hop limit */
static u8_t *compress_hoplimit(struct net_ipv6_hdr *ipv6, u8_t *inline_ptr,
u16_t *iphc)
{
/* Hop Limit */
switch (ipv6->hop_limit) {
case 1:
NET_DBG("HLIM compressed (1)");
*iphc |= NET_6LO_IPHC_HLIM1;
break;
case 64:
NET_DBG("HLIM compressed (64)");
*iphc |= NET_6LO_IPHC_HLIM64;
break;
case 255:
NET_DBG("HLIM compressed (255)");
*iphc |= NET_6LO_IPHC_HLIM255;
break;
default:
inline_ptr -= sizeof(ipv6->hop_limit);
*inline_ptr = ipv6->hop_limit;
break;
}
return inline_ptr;
}
/* Helper to compress Next header */
static u8_t *compress_nh(struct net_ipv6_hdr *ipv6, u8_t *inline_ptr,
u16_t *iphc)
{
/* Next header */
if (ipv6->nexthdr == IPPROTO_UDP) {
*iphc |= NET_6LO_IPHC_NH_1;
} else {
inline_ptr -= sizeof(ipv6->nexthdr);
*inline_ptr = ipv6->nexthdr;
}
return inline_ptr;
}
/* Helpers to compress Source Address */
static u8_t *compress_sa(struct net_ipv6_hdr *ipv6, struct net_pkt *pkt,
u8_t *inline_ptr, u16_t *iphc)
{
NET_ASSERT(net_pkt_lladdr_src(pkt)->addr);
/* Address is fully elided */
if (net_ipv6_addr_based_on_ll(&ipv6->src, net_pkt_lladdr_src(pkt))) {
NET_DBG("SAM_11 src address is fully elided");
*iphc |= NET_6LO_IPHC_SAM_11;
return inline_ptr;
}
/* Following 64 bits are 0000:00ff:fe00:XXXX */
if (net_6lo_addr_16_bit_compressible(&ipv6->src)) {
NET_DBG("SAM_10 src addr 16 bit compressible");
*iphc |= NET_6LO_IPHC_SAM_10;
inline_ptr -= sizeof(u16_t);
memmove(inline_ptr, &ipv6->src.s6_addr[14], sizeof(u16_t));
return inline_ptr;
}
NET_DBG("SAM_01 src 64 bits are inlined");
/* Remaining 64 bits are in-line */
*iphc |= NET_6LO_IPHC_SAM_01;
inline_ptr -= 8U;
memmove(inline_ptr, &ipv6->src.s6_addr[8], 8U);
return inline_ptr;
}
static u8_t *set_sa_inline(struct net_ipv6_hdr *ipv6, u8_t *inline_ptr,
u16_t *iphc)
{
*iphc |= NET_6LO_IPHC_SAM_00;
inline_ptr -= 16U;
memmove(inline_ptr, &ipv6->src.s6_addr[0], 16U);
return inline_ptr;
}
#if defined(CONFIG_NET_6LO_CONTEXT)
static u8_t *compress_sa_ctx(struct net_ipv6_hdr *ipv6, u8_t *inline_ptr,
struct net_pkt *pkt, u16_t *iphc,
struct net_6lo_context *src)
{
NET_ASSERT(net_pkt_lladdr_src(pkt)->addr);
NET_DBG("SAC_1 src address context based");
*iphc |= NET_6LO_IPHC_SAC_1;
if (net_ipv6_addr_based_on_ll(&ipv6->src, net_pkt_lladdr_src(pkt))) {
NET_DBG("SAM_11 src address is fully elided");
/* Address is fully elided */
*iphc |= NET_6LO_IPHC_SAM_11;
return inline_ptr;
}
/* Following 64 bits are 0000:00ff:fe00:XXXX */
if (net_6lo_addr_16_bit_compressible(&ipv6->src)) {
NET_DBG("SAM_10 src addr 16 bit compressible");
*iphc |= NET_6LO_IPHC_SAM_10;
inline_ptr -= sizeof(u16_t);
memmove(inline_ptr, &ipv6->src.s6_addr[14], sizeof(u16_t));
return inline_ptr;
}
NET_DBG("SAM_01 src remaining 64 bits are inlined");
/* Remaining 64 bits are in-line */
*iphc |= NET_6LO_IPHC_SAM_01;
inline_ptr -= 8U;
memmove(inline_ptr, &ipv6->src.s6_addr[8], 8U);
return inline_ptr;
}
#endif
/* Helpers to compress Destination Address */
static u8_t *compress_da_mcast(struct net_ipv6_hdr *ipv6, u8_t *inline_ptr,
u16_t *iphc)
{
*iphc |= NET_6LO_IPHC_M_1;
NET_DBG("M_1 dst is mcast");
if (net_6lo_maddr_8_bit_compressible(&ipv6->dst)) {
NET_DBG("DAM_11 dst maddr 8 bit compressible");
/* last byte */
*iphc |= NET_6LO_IPHC_DAM_11;
inline_ptr -= sizeof(u8_t);
memmove(inline_ptr, &ipv6->dst.s6_addr[15], sizeof(u8_t));
return inline_ptr;
}
if (net_6lo_maddr_32_bit_compressible(&ipv6->dst)) {
NET_DBG("DAM_10 4 bytes: 2nd byte + last three bytes");
/* 4 bytes: 2nd byte + last three bytes */
*iphc |= NET_6LO_IPHC_DAM_10;
inline_ptr -= 3U;
memmove(inline_ptr, &ipv6->dst.s6_addr[13], 3U);
inline_ptr -= sizeof(u8_t);
memmove(inline_ptr, &ipv6->dst.s6_addr[1], sizeof(u8_t));
return inline_ptr;
}
if (net_6lo_maddr_48_bit_compressible(&ipv6->dst)) {
NET_DBG("DAM_01 6 bytes: 2nd byte + last five bytes");
/* 6 bytes: 2nd byte + last five bytes */
*iphc |= NET_6LO_IPHC_DAM_01;
inline_ptr -= 5U;
memmove(inline_ptr, &ipv6->dst.s6_addr[11], 5U);
inline_ptr -= sizeof(u8_t);
memmove(inline_ptr, &ipv6->dst.s6_addr[1], sizeof(u8_t));
return inline_ptr;
}
NET_DBG("DAM_00 dst complete addr inlined");
/* complete address NET_6LO_IPHC_DAM_00 */
inline_ptr -= 16U;
memmove(inline_ptr, &ipv6->dst.s6_addr[0], 16U);
return inline_ptr;
}
static u8_t *compress_da(struct net_ipv6_hdr *ipv6, struct net_pkt *pkt,
u8_t *inline_ptr, u16_t *iphc)
{
NET_ASSERT(net_pkt_lladdr_dst(pkt)->addr);
/* Address is fully elided */
if (net_ipv6_addr_based_on_ll(&ipv6->dst, net_pkt_lladdr_dst(pkt))) {
NET_DBG("DAM_11 dst addr fully elided");
*iphc |= NET_6LO_IPHC_DAM_11;
return inline_ptr;
}
/* Following 64 bits are 0000:00ff:fe00:XXXX */
if (net_6lo_addr_16_bit_compressible(&ipv6->dst)) {
NET_DBG("DAM_10 dst addr 16 bit compressible");
*iphc |= NET_6LO_IPHC_DAM_10;
inline_ptr -= sizeof(u16_t);
memmove(inline_ptr, &ipv6->dst.s6_addr[14], sizeof(u16_t));
return inline_ptr;
}
NET_DBG("DAM_01 remaining 64 bits are inlined");
/* Remaining 64 bits are in-line */
*iphc |= NET_6LO_IPHC_DAM_01;
inline_ptr -= 8U;
memmove(inline_ptr, &ipv6->dst.s6_addr[8], 8U);
return inline_ptr;
}
static u8_t *set_da_inline(struct net_ipv6_hdr *ipv6, u8_t *inline_ptr,
u16_t *iphc)
{
*iphc |= NET_6LO_IPHC_DAM_00;
inline_ptr -= 16U;
memmove(inline_ptr, &ipv6->dst.s6_addr[0], 16U);
return inline_ptr;
}
#if defined(CONFIG_NET_6LO_CONTEXT)
static u8_t *compress_da_ctx(struct net_ipv6_hdr *ipv6, u8_t *inline_ptr,
struct net_pkt *pkt, u16_t *iphc,
struct net_6lo_context *dst)
{
*iphc |= NET_6LO_IPHC_DAC_1;
if (net_ipv6_addr_based_on_ll(&ipv6->dst, net_pkt_lladdr_dst(pkt))) {
NET_DBG("DAM_11 dst addr fully elided");
*iphc |= NET_6LO_IPHC_DAM_11;
return inline_ptr;
}
/* Following 64 bits are 0000:00ff:fe00:XXXX */
if (net_6lo_addr_16_bit_compressible(&ipv6->dst)) {
NET_DBG("DAM_10 dst addr 16 bit compressible");
*iphc |= NET_6LO_IPHC_DAM_10;
inline_ptr -= sizeof(u16_t);
memmove(inline_ptr, &ipv6->dst.s6_addr[14], sizeof(u16_t));
return inline_ptr;
}
NET_DBG("DAM_01 remaining 64 bits are inlined");
/* Remaining 64 bits are in-line */
*iphc |= NET_6LO_IPHC_DAM_01;
inline_ptr -= 8U;
memmove(inline_ptr, &ipv6->dst.s6_addr[8], 8U);
return inline_ptr;
}
#endif
/* Helper to compress Next header UDP */
static inline u8_t *compress_nh_udp(struct net_udp_hdr *udp, u8_t *inline_ptr,
bool compress_checksum)
{
u8_t nhc = NET_6LO_NHC_UDP_BARE;
u8_t *inline_ptr_udp = inline_ptr;
u8_t tmp;
/* 4.3.3 UDP LOWPAN_NHC Format
* 0 1 2 3 4 5 6 7
* +---+---+---+---+---+---+---+---+
* | 1 | 1 | 1 | 1 | 0 | C | P |
* +---+---+---+---+---+---+---+---+
*/
/* Port compression
* 00: All 16 bits for src and dst are inlined.
* 01: All 16 bits for src port inlined. First 8 bits of dst port is
* 0xf0 and elided. The remaining 8 bits of dst port inlined.
* 10: First 8 bits of src port 0xf0 and elided. The remaining 8 bits
* of src port inlined. All 16 bits of dst port inlined.
* 11: First 12 bits of both src and dst are 0xf0b and elided. The
* remaining 4 bits for each are inlined.
*/
if (compress_checksum) {
nhc |= NET_6LO_NHC_UDP_CHECKSUM;
} else {
inline_ptr_udp -= sizeof(udp->chksum);
memmove(inline_ptr_udp, &udp->chksum, sizeof(udp->chksum));
}
if ((((htons(udp->src_port) >> 4) & 0xFFF) ==
NET_6LO_NHC_UDP_4_BIT_PORT) &&
(((htons(udp->dst_port) >> 4) & 0xFFF) ==
NET_6LO_NHC_UDP_4_BIT_PORT)) {
NET_DBG("UDP ports src and dst 4 bits inlined");
/** src: first 16 bits elided, next 4 bits inlined
* dst: first 16 bits elided, next 4 bits inlined
*/
nhc |= NET_6LO_NHC_UDP_PORT_11;
tmp = (u8_t)(htons(udp->src_port));
tmp = tmp << 4;
tmp |= (((u8_t)(htons(udp->dst_port))) & 0x0F);
inline_ptr_udp -= sizeof(tmp);
*inline_ptr_udp = tmp;
} else if (((htons(udp->dst_port) >> 8) & 0xFF) ==
NET_6LO_NHC_UDP_8_BIT_PORT) {
NET_DBG("UDP ports src full, dst 8 bits inlined");
/* dst: first 8 bits elided, next 8 bits inlined
* src: fully carried inline
*/
nhc |= NET_6LO_NHC_UDP_PORT_01;
inline_ptr_udp -= sizeof(u8_t);
*inline_ptr_udp = (u8_t)(htons(udp->dst_port));
inline_ptr_udp -= sizeof(udp->src_port);
memmove(inline_ptr_udp, &udp->src_port, sizeof(udp->src_port));
} else if (((htons(udp->src_port) >> 8) & 0xFF) ==
NET_6LO_NHC_UDP_8_BIT_PORT) {
NET_DBG("UDP ports src 8bits, dst full inlined");
/* src: first 8 bits elided, next 8 bits inlined
* dst: fully carried inline
*/
nhc |= NET_6LO_NHC_UDP_PORT_10;
inline_ptr_udp -= sizeof(udp->dst_port);
memmove(inline_ptr_udp, &udp->dst_port, sizeof(udp->dst_port));
inline_ptr_udp -= sizeof(u8_t);
*inline_ptr_udp = (u8_t)(htons(udp->src_port));
} else {
NET_DBG("Can not compress ports, ports are inlined");
/* can not compress ports, ports are inlined */
inline_ptr_udp -= sizeof(udp->dst_port) + sizeof(udp->src_port);
memmove(inline_ptr_udp, &udp->src_port,
sizeof(udp->dst_port) + sizeof(udp->src_port));
}
inline_ptr_udp -= sizeof(nhc);
*inline_ptr_udp = nhc;
return inline_ptr_udp;
}
#if defined(CONFIG_NET_6LO_CONTEXT)
static struct net_6lo_context *get_src_addr_ctx(struct net_pkt *pkt,
struct net_ipv6_hdr *ipv6)
{
/* If compress flag is unset means use only in uncompression. */
struct net_6lo_context *src;
src = get_6lo_context_by_addr(net_pkt_iface(pkt), &ipv6->src);
if (!src || !src->compress) {
return NULL;
}
return src;
}
static struct net_6lo_context *get_dst_addr_ctx(struct net_pkt *pkt,
struct net_ipv6_hdr *ipv6)
{
/* If compress flag is unset means use only in uncompression. */
struct net_6lo_context *dst;
dst = get_6lo_context_by_addr(net_pkt_iface(pkt), &ipv6->dst);
if (!dst || !dst->compress) {
return NULL;
}
return dst;
}
#endif /* CONFIG_NET_6LO_CONTEXT */
/* RFC 6282 LOWPAN IPHC Encoding format (3.1)
* Base Format
* 0 1
* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
* +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
* | 0 | 1 | 1 | TF |NH | HLIM |CID|SAC| SAM | M |DAC| DAM |
* +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
*/
static inline int compress_IPHC_header(struct net_pkt *pkt)
{
#if defined(CONFIG_NET_6LO_CONTEXT)
struct net_6lo_context *src_ctx = NULL;
struct net_6lo_context *dst_ctx = NULL;
#endif
u8_t compressed = 0;
u16_t iphc = (NET_6LO_DISPATCH_IPHC << 8);
struct net_ipv6_hdr *ipv6 = NET_IPV6_HDR(pkt);
struct net_udp_hdr *udp;
u8_t *inline_pos;
if (pkt->frags->len < NET_IPV6H_LEN) {
NET_ERR("Invalid length %d, min %d",
pkt->frags->len, NET_IPV6H_LEN);
return -EINVAL;
}
if (ipv6->nexthdr == IPPROTO_UDP &&
pkt->frags->len < NET_IPV6UDPH_LEN) {
NET_ERR("Invalid length %d, min %d",
pkt->frags->len, NET_IPV6UDPH_LEN);
return -EINVAL;
}
inline_pos = pkt->buffer->data + NET_IPV6H_LEN;
if (ipv6->nexthdr == IPPROTO_UDP) {
udp = (struct net_udp_hdr *)inline_pos;
inline_pos += NET_UDPH_LEN;
inline_pos = compress_nh_udp(udp, inline_pos, false);
}
if (net_6lo_ll_prefix_padded_with_zeros(&ipv6->dst)) {
inline_pos = compress_da(ipv6, pkt, inline_pos, &iphc);
goto da_end;
}
if (net_ipv6_is_addr_mcast(&ipv6->dst)) {
inline_pos = compress_da_mcast(ipv6, inline_pos, &iphc);
goto da_end;
}
#if defined(CONFIG_NET_6LO_CONTEXT)
dst_ctx = get_dst_addr_ctx(pkt, ipv6);
if (dst_ctx) {
iphc |= NET_6LO_IPHC_CID_1;
inline_pos = compress_da_ctx(ipv6, inline_pos, pkt, &iphc,
dst_ctx);
goto da_end;
}
#endif
inline_pos = set_da_inline(ipv6, inline_pos, &iphc);
da_end:
if (net_6lo_ll_prefix_padded_with_zeros(&ipv6->src)) {
inline_pos = compress_sa(ipv6, pkt, inline_pos, &iphc);
goto sa_end;
}
if (net_ipv6_is_addr_unspecified(&ipv6->src)) {
NET_DBG("SAM_00, SAC_1 unspecified src address");
/* Unspecified IPv6 src address */
iphc |= NET_6LO_IPHC_SAC_1;
iphc |= NET_6LO_IPHC_SAM_00;
goto sa_end;
}
#if defined(CONFIG_NET_6LO_CONTEXT)
src_ctx = get_src_addr_ctx(pkt, ipv6);
if (src_ctx) {
inline_pos = compress_sa_ctx(ipv6, inline_pos, pkt, &iphc,
src_ctx);
iphc |= NET_6LO_IPHC_CID_1;
goto sa_end;
}
#endif
inline_pos = set_sa_inline(ipv6, inline_pos, &iphc);
sa_end:
inline_pos = compress_hoplimit(ipv6, inline_pos, &iphc);
inline_pos = compress_nh(ipv6, inline_pos, &iphc);
inline_pos = compress_tfl(ipv6, inline_pos, &iphc);
#if defined(CONFIG_NET_6LO_CONTEXT)
if (iphc & NET_6LO_IPHC_CID_1) {
inline_pos -= sizeof(u8_t);
*inline_pos = 0;
if (src_ctx) {
*inline_pos = src_ctx->cid << 4;
}
if (dst_ctx) {
*inline_pos |= dst_ctx->cid & 0x0F;
}
}
#endif
inline_pos -= sizeof(iphc);
iphc = htons(iphc);
memmove(inline_pos, &iphc, sizeof(iphc));
compressed = inline_pos - pkt->buffer->data;
net_buf_pull(pkt->buffer, compressed);
return compressed;
}
/* Helper to uncompress Traffic class and Flow label */
static inline u8_t *uncompress_tfl(u16_t iphc, u8_t *cursor,
struct net_ipv6_hdr *ipv6)
{
u8_t tcl;
/* Uncompress tcl and flow label */
switch (iphc & NET_6LO_IPHC_TF_11) {
case NET_6LO_IPHC_TF_00:
NET_DBG("ECN + DSCP + 4-bit Pad + Flow Label");
tcl = *cursor;
cursor++;
tcl = (tcl >> 6) | (tcl << 2);
ipv6->vtc |= ((tcl & 0xF0) >> 4);
ipv6->tcflow = ((tcl & 0x0F) << 4) | (*cursor & 0x0F);
cursor++;
memmove(&ipv6->flow, cursor, sizeof(ipv6->flow));
cursor += sizeof(ipv6->flow);
break;
case NET_6LO_IPHC_TF_01:
NET_DBG("ECN + 2-bit Pad + Flow Label, DSCP is elided");
tcl = ((*cursor & 0xF0) >> 6);
ipv6->tcflow = ((tcl & 0x0F) << 4) | (*cursor & 0x0F);
cursor++;
memmove(&ipv6->flow, cursor, sizeof(ipv6->flow));
cursor += sizeof(ipv6->flow);
break;
case NET_6LO_IPHC_TF_10:
NET_DBG("Flow label elided");
tcl = *cursor;
cursor++;
tcl = (tcl >> 6) | (tcl << 2);
ipv6->vtc |= ((tcl & 0xF0) >> 4);
ipv6->tcflow = (tcl & 0x0F) << 4;
ipv6->flow = 0U;
break;
case NET_6LO_IPHC_TF_11:
NET_DBG("Tcl and Flow label elided");
ipv6->tcflow = 0U;
ipv6->flow = 0U;
break;
}
return cursor;
}
/* Helper to uncompress Hoplimit */
static inline u8_t *uncompress_hoplimit(u16_t iphc, u8_t *cursor,
struct net_ipv6_hdr *ipv6)
{
switch (iphc & NET_6LO_IPHC_HLIM_MASK) {
case NET_6LO_IPHC_HLIM:
ipv6->hop_limit = *cursor;
cursor++;
break;
case NET_6LO_IPHC_HLIM1:
ipv6->hop_limit = 1U;
break;
case NET_6LO_IPHC_HLIM64:
ipv6->hop_limit = 64U;
break;
case NET_6LO_IPHC_HLIM255:
ipv6->hop_limit = 255U;
break;
}
return cursor;
}
/* Helper to uncompress Source Address */
static inline u8_t *uncompress_sa(u16_t iphc, u8_t *cursor,
struct net_ipv6_hdr *ipv6,
struct net_pkt *pkt)
{
NET_DBG("SAC_0");
switch (iphc & NET_6LO_IPHC_SAM_MASK) {
case NET_6LO_IPHC_SAM_00:
NET_DBG("SAM_00 full src addr inlined");
memmove(ipv6->src.s6_addr, cursor, sizeof(ipv6->src.s6_addr));
cursor += sizeof(ipv6->src.s6_addr);
break;
case NET_6LO_IPHC_SAM_01:
NET_DBG("SAM_01 last 64 bits are inlined");
memmove(&ipv6->src.s6_addr[8], cursor, 8);
cursor += 8U;
ipv6->src.s6_addr32[0] = 0x00;
ipv6->src.s6_addr32[1] = 0x00;
ipv6->src.s6_addr[0] = 0xFE;
ipv6->src.s6_addr[1] = 0x80;
break;
case NET_6LO_IPHC_SAM_10:
NET_DBG("SAM_10 src addr 16 bit compressed");
memmove(&ipv6->src.s6_addr[14], cursor, 2);
cursor += 2U;
ipv6->src.s6_addr16[6] = 0x00;
ipv6->src.s6_addr32[0] = 0x00;
ipv6->src.s6_addr32[1] = 0x00;
ipv6->src.s6_addr32[2] = 0x00;
ipv6->src.s6_addr[0] = 0xFE;
ipv6->src.s6_addr[1] = 0x80;
ipv6->src.s6_addr[11] = 0xFF;
ipv6->src.s6_addr[12] = 0xFE;
break;
case NET_6LO_IPHC_SAM_11:
NET_DBG("SAM_11 generate src addr from ll");
net_ipv6_addr_create_iid(&ipv6->src, net_pkt_lladdr_src(pkt));
break;
}
return cursor;
}
#if defined(CONFIG_NET_6LO_CONTEXT)
static inline u8_t *uncompress_sa_ctx(u16_t iphc, u8_t *cursor,
struct net_ipv6_hdr *ipv6,
struct net_6lo_context *ctx,
struct net_pkt *pkt)
{
switch (iphc & NET_6LO_IPHC_SAM_MASK) {
case NET_6LO_IPHC_SAM_01:
NET_DBG("SAM_01 last 64 bits are inlined");
/* First 8 bytes are from context */
memmove(&ipv6->src.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
/* And the rest are carried in-line*/
memmove(&ipv6->src.s6_addr[8], cursor, 8);
cursor += 8U;
break;
case NET_6LO_IPHC_SAM_10:
NET_DBG("SAM_10 src addr 16 bit compressed");
/* 16 bit carried in-line */
memmove(&ipv6->src.s6_addr[14], cursor, 2);
cursor += 2U;
/* First 8 bytes are from context */
memmove(&ipv6->src.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
ipv6->src.s6_addr32[2] = 0x00;
ipv6->src.s6_addr16[6] = 0x00;
ipv6->src.s6_addr[11] = 0xFF;
ipv6->src.s6_addr[12] = 0xFE;
break;
case NET_6LO_IPHC_SAM_11:
NET_DBG("SAM_11 generate src addr from ll");
/* RFC 6282, 3.1.1. If SAC = 1 and SAM = 11
* Derive addr using context information and
* the encapsulating header.
* (e.g., 802.15.4 or IPv6 source address).
*/
net_ipv6_addr_create_iid(&ipv6->src, net_pkt_lladdr_src(pkt));
/* net_ipv6_addr_create_iid will copy first 8 bytes
* as link local prefix.
* Overwrite first 8 bytes from context prefix here.
*/
memmove(&ipv6->src.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
break;
}
return cursor;
}
#endif
/* Helpers to uncompress Destination Address */
static inline u8_t *uncompress_da_mcast(u16_t iphc, u8_t *cursor,
struct net_ipv6_hdr *ipv6)
{
NET_DBG("Dst is multicast");
if (iphc & NET_6LO_IPHC_DAC_1) {
NET_WARN("Unsupported DAM options");
return 0;
}
/* If M=1 and DAC=0:
* 00: 128 bits, The full address is carried in-line.
* 01: 48 bits, The address takes the form ffXX::00XX:XXXX:XXXX.
* 10: 32 bits, The address takes the form ffXX::00XX:XXXX.
* 11: 8 bits, The address takes the form ff02::00XX.
*/
switch (iphc & NET_6LO_IPHC_DAM_MASK) {
case NET_6LO_IPHC_DAM_00:
NET_DBG("DAM_00 full dst addr inlined");
memmove(&ipv6->dst.s6_addr[0], cursor,
sizeof(ipv6->dst.s6_addr));
cursor += sizeof(ipv6->dst.s6_addr);
break;
case NET_6LO_IPHC_DAM_01:
NET_DBG("DAM_01 2nd byte and last five bytes");
ipv6->dst.s6_addr[1] = *cursor;
cursor++;
memmove(&ipv6->dst.s6_addr[11], cursor, 5);
cursor += 5U;
ipv6->dst.s6_addr[0] = 0xFF;
ipv6->dst.s6_addr16[1] = 0x00;
ipv6->dst.s6_addr32[1] = 0x00;
ipv6->dst.s6_addr[10] = 0x00;
ipv6->dst.s6_addr16[4] = 0x00;
break;
case NET_6LO_IPHC_DAM_10:
NET_DBG("DAM_10 2nd byte and last three bytes");
ipv6->dst.s6_addr[1] = *cursor;
cursor++;
memmove(&ipv6->dst.s6_addr[13], cursor, 3);
cursor += 3U;
ipv6->dst.s6_addr[0] = 0xFF;
ipv6->dst.s6_addr16[1] = 0x00;
ipv6->dst.s6_addr32[1] = 0x00;
ipv6->dst.s6_addr32[2] = 0x00;
ipv6->dst.s6_addr[12] = 0x00;
break;
case NET_6LO_IPHC_DAM_11:
NET_DBG("DAM_11 8 bit compressed");
ipv6->dst.s6_addr[15] = *cursor;
cursor++;
ipv6->dst.s6_addr[14] = 0x00;
ipv6->dst.s6_addr32[0] = 0x00;
ipv6->dst.s6_addr32[1] = 0x00;
ipv6->dst.s6_addr32[2] = 0x00;
ipv6->dst.s6_addr16[6] = 0x00;
ipv6->dst.s6_addr[0] = 0xFF;
ipv6->dst.s6_addr[1] = 0x02;
break;
}
return cursor;
}
/* Helper to uncompress Destination Address */
static inline u8_t *uncompress_da(u16_t iphc, u8_t *cursor,
struct net_ipv6_hdr *ipv6,
struct net_pkt *pkt)
{
NET_DBG("DAC_0");
switch (iphc & NET_6LO_IPHC_DAM_MASK) {
case NET_6LO_IPHC_DAM_00:
NET_DBG("DAM_00 full dst addr inlined");
memmove(&ipv6->dst.s6_addr[0], cursor,
sizeof(ipv6->dst.s6_addr));
cursor += sizeof(ipv6->dst.s6_addr);
break;
case NET_6LO_IPHC_DAM_01:
NET_DBG("DAM_01 last 64 bits are inlined");
memmove(&ipv6->dst.s6_addr[8], cursor, 8);
cursor += 8U;
ipv6->dst.s6_addr32[0] = 0x00;
ipv6->dst.s6_addr32[1] = 0x00;
ipv6->dst.s6_addr[0] = 0xFE;
ipv6->dst.s6_addr[1] = 0x80;
break;
case NET_6LO_IPHC_DAM_10:
NET_DBG("DAM_10 dst addr 16 bit compressed");
memmove(&ipv6->dst.s6_addr[14], cursor, 2);
cursor += 2U;
ipv6->dst.s6_addr32[0] = 0x00;
ipv6->dst.s6_addr32[1] = 0x00;
ipv6->dst.s6_addr32[2] = 0x00;
ipv6->dst.s6_addr16[6] = 0x00;
ipv6->dst.s6_addr[0] = 0xFE;
ipv6->dst.s6_addr[1] = 0x80;
ipv6->dst.s6_addr[11] = 0xFF;
ipv6->dst.s6_addr[12] = 0xFE;
break;
case NET_6LO_IPHC_DAM_11:
NET_DBG("DAM_11 generate dst addr from ll");
net_ipv6_addr_create_iid(&ipv6->dst, net_pkt_lladdr_dst(pkt));
break;
}
return cursor;
}
#if defined(CONFIG_NET_6LO_CONTEXT)
static inline u8_t *uncompress_da_ctx(u16_t iphc, u8_t *cursor,
struct net_ipv6_hdr *ipv6,
struct net_6lo_context *ctx,
struct net_pkt *pkt)
{
NET_DBG("DAC_1");
switch (iphc & NET_6LO_IPHC_DAM_MASK) {
case NET_6LO_IPHC_DAM_01:
NET_DBG("DAM_01 last 64 bits are inlined");
/* Last 8 bytes carried in-line */
memmove(&ipv6->dst.s6_addr[8], cursor, 8);
cursor += 8U;
/* First 8 bytes are from context */
memmove(&ipv6->dst.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
break;
case NET_6LO_IPHC_DAM_10:
NET_DBG("DAM_10 src addr 16 bit compressed");
/* 16 bit carried in-line */
memmove(&ipv6->dst.s6_addr[14], cursor, 2);
cursor += 2U;
/* First 8 bytes are from context */
memmove(&ipv6->dst.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
ipv6->dst.s6_addr32[2] = 0x00;
ipv6->dst.s6_addr16[6] = 0x00;
ipv6->dst.s6_addr[11] = 0xFF;
ipv6->dst.s6_addr[12] = 0xFE;
break;
case NET_6LO_IPHC_DAM_11:
NET_DBG("DAM_11 generate src addr from ll");
/* RFC 6282, 3.1.1. If SAC = 1 and SAM = 11
* Derive addr using context information and
* the encapsulating header.
* (e.g., 802.15.4 or IPv6 source address).
*/
net_ipv6_addr_create_iid(&ipv6->dst, net_pkt_lladdr_dst(pkt));
/* net_ipv6_addr_create_iid will copy first 8 bytes
* as link local prefix.
* Overwrite first 8 bytes from context prefix here.
*/
memmove(&ipv6->dst.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
break;
}
return cursor;
}
#endif
/* Helper to uncompress NH UDP */
static u8_t *uncompress_nh_udp(u8_t nhc, u8_t *cursor,
struct net_udp_hdr *udp)
{
/* Port uncompression
* 00: All 16 bits for src and dst are inlined
* 01: src, 16 bits are lined, dst(0xf0) 8 bits are inlined
* 10: dst, 16 bits are lined, src(0xf0) 8 bits are inlined
* 11: src, dst (0xf0b) 4 bits are inlined
*/
/* UDP header uncompression */
switch (nhc & NET_6LO_NHC_UDP_PORT_11) {
case NET_6LO_NHC_UDP_PORT_00:
NET_DBG("src and dst ports are inlined");
memmove(&udp->src_port, cursor, sizeof(udp->src_port));
cursor += sizeof(udp->src_port);
memmove(&udp->dst_port, cursor, sizeof(udp->dst_port));
cursor += sizeof(udp->dst_port);
break;
case NET_6LO_NHC_UDP_PORT_01:
NET_DBG("src full, dst 8 bits inlined");
memmove(&udp->src_port, cursor, sizeof(udp->src_port));
cursor += sizeof(udp->src_port);
udp->dst_port = htons(((u16_t)NET_6LO_NHC_UDP_8_BIT_PORT << 8) |
*cursor);
cursor++;
break;
case NET_6LO_NHC_UDP_PORT_10:
NET_DBG("src 8 bits, dst full inlined");
udp->src_port = htons(((u16_t)NET_6LO_NHC_UDP_8_BIT_PORT << 8) |
*cursor);
cursor++;
memmove(&udp->dst_port, cursor, sizeof(udp->dst_port));
cursor += sizeof(udp->dst_port);
break;
case NET_6LO_NHC_UDP_PORT_11:
NET_DBG("src and dst 4 bits inlined");
udp->src_port = htons((NET_6LO_NHC_UDP_4_BIT_PORT << 4) |
(*cursor >> 4));
udp->dst_port = htons((NET_6LO_NHC_UDP_4_BIT_PORT << 4) |
(*cursor & 0x0F));
cursor++;
break;
}
if (!(nhc & NET_6LO_NHC_UDP_CHECKSUM)) {
memmove(&udp->chksum, cursor, sizeof(udp->chksum));
cursor += sizeof(udp->chksum);
}
return cursor;
}
#if defined(CONFIG_NET_6LO_CONTEXT)
/* Helper function to uncompress src and dst contexts */
static inline void uncompress_cid(struct net_pkt *pkt, u8_t cid,
struct net_6lo_context **src,
struct net_6lo_context **dst)
{
u8_t cid_tmp;
/* Extract source and destination Context Index,
* Either src or dest address is context based or both.
*/
cid_tmp = (cid >> 4) & 0x0F;
*src = get_6lo_context_by_cid(net_pkt_iface(pkt), cid_tmp);
if (!(*src)) {
NET_DBG("Unknown src cid %d", cid_tmp);
}
cid_tmp = cid & 0x0F;
*dst = get_6lo_context_by_cid(net_pkt_iface(pkt), cid_tmp);
if (!(*dst)) {
NET_DBG("Unknown dst cid %d", cid_tmp);
}
}
#endif
static bool uncompress_IPHC_header(struct net_pkt *pkt)
{
struct net_udp_hdr *udp = NULL;
struct net_buf *frag = NULL;
u8_t nhc = 0;
int nhc_inline_size = 0;
struct net_ipv6_hdr *ipv6;
u16_t len;
u16_t iphc;
int inline_size, compressed_hdr_size;
size_t diff;
u8_t *cursor;
#if defined(CONFIG_NET_6LO_CONTEXT)
struct net_6lo_context *src = NULL;
struct net_6lo_context *dst = NULL;
#endif
iphc = ntohs(UNALIGNED_GET((u16_t *)pkt->buffer->data));
inline_size = get_ihpc_inlined_size(iphc);
if (inline_size < 0) {
return false;
}
compressed_hdr_size = sizeof(iphc) + inline_size;
diff = sizeof(struct net_ipv6_hdr) - compressed_hdr_size;
if (iphc & NET_6LO_IPHC_NH_MASK) {
nhc = *(pkt->buffer->data + sizeof(iphc) + inline_size);
if ((nhc & 0xF8) != NET_6LO_NHC_UDP_BARE) {
NET_ERR("Unsupported next header");
return false;
}
nhc_inline_size = get_udp_nhc_inlined_size(nhc);
compressed_hdr_size += sizeof(u8_t) + nhc_inline_size;
diff += sizeof(struct net_udp_hdr) - sizeof(u8_t) -
nhc_inline_size;
}
if (net_buf_tailroom(pkt->buffer) >= diff) {
NET_DBG("Enough tailroom. Uncompress inplace");
frag = pkt->buffer;
net_buf_add(frag, diff);
cursor = frag->data + diff;
memmove(cursor, frag->data, frag->len - diff);
} else {
NET_DBG("Not enough tailroom. Get new fragment");
cursor = pkt->buffer->data;
frag = net_pkt_get_frag(pkt, NET_6LO_RX_PKT_TIMEOUT);
if (!frag) {
NET_ERR("Can't get frag for uncompression");
return false;
}
net_buf_pull(pkt->buffer, compressed_hdr_size);
net_buf_add(frag, nhc ? NET_IPV6UDPH_LEN : NET_IPV6H_LEN);
}
ipv6 = (struct net_ipv6_hdr *)(frag->data);
cursor += sizeof(iphc);
if (iphc & NET_6LO_IPHC_CID_1) {
#if defined(CONFIG_NET_6LO_CONTEXT)
uncompress_cid(pkt, *cursor, &src, &dst);
cursor++;
#else
NET_ERR("Context based uncompression not enabled");
return false;
#endif
}
/* Version is always 6 */
ipv6->vtc = 0x60;
net_pkt_set_ip_hdr_len(pkt, NET_IPV6H_LEN);
/* Uncompress Traffic class and Flow label */
cursor = uncompress_tfl(iphc, cursor, ipv6);
if (!(iphc & NET_6LO_IPHC_NH_MASK)) {
ipv6->nexthdr = *cursor;
cursor++;
}
/* Uncompress Hoplimit */
cursor = uncompress_hoplimit(iphc, cursor, ipv6);
/* Uncompress Source Address */
if (iphc & NET_6LO_IPHC_SAC_1) {
NET_DBG("SAC_1");
if ((iphc & NET_6LO_IPHC_SAM_MASK) == NET_6LO_IPHC_SAM_00) {
NET_DBG("SAM_00 unspecified address");
memset(&ipv6->src.s6_addr[0], 0,
sizeof(ipv6->src.s6_addr));
} else if (IS_ENABLED(CONFIG_NET_6LO_CONTEXT)) {
#if defined(CONFIG_NET_6LO_CONTEXT)
if (!src) {
NET_ERR("Src context doesn't exists");
goto fail;
}
cursor = uncompress_sa_ctx(iphc, cursor, ipv6, src, pkt);
#endif
} else {
NET_ERR("Context based uncompression not enabled");
goto fail;
}
} else {
cursor = uncompress_sa(iphc, cursor, ipv6, pkt);
}
/* Uncompress Destination Address */
if (iphc & NET_6LO_IPHC_M_1) {
if (iphc & NET_6LO_IPHC_DAC_1) {
/* TODO: DAM00 Unicast-Prefix-based IPv6 Multicast
* Addresses. DAM_01, DAM_10 and DAM_11 are reserved.
*/
NET_ERR("DAC_1 and M_1 is not supported");
goto fail;
} else {
cursor = uncompress_da_mcast(iphc, cursor, ipv6);
}
} else {
if (iphc & NET_6LO_IPHC_DAC_1) {
#if defined(CONFIG_NET_6LO_CONTEXT)
if (!dst) {
NET_ERR("Dst context doesn't exists");
goto fail;
}
cursor = uncompress_da_ctx(iphc, cursor, ipv6, dst, pkt);
#else
NET_ERR("Context based uncompression not enabled");
goto fail;
#endif
} else {
cursor = uncompress_da(iphc, cursor, ipv6, pkt);
}
}
if (iphc & NET_6LO_IPHC_NH_MASK) {
ipv6->nexthdr = IPPROTO_UDP;
udp = (struct net_udp_hdr *)(frag->data + NET_IPV6H_LEN);
/* skip nhc */
cursor++;
cursor = uncompress_nh_udp(nhc, cursor, udp);
}
if (frag != pkt->buffer) {
/* Insert the fragment (this one holds uncompressed headers) */
net_pkt_frag_insert(pkt, frag);
}
/* Set IPv6 header and UDP (if next header is) length */
len = net_pkt_get_len(pkt) - NET_IPV6H_LEN;
ipv6->len = htons(len);
if (ipv6->nexthdr == IPPROTO_UDP && udp) {
udp->len = htons(len);
if (nhc & NET_6LO_NHC_UDP_CHECKSUM) {
udp->chksum = net_calc_chksum_udp(pkt);
}
}
net_pkt_cursor_init(pkt);
return true;
fail:
if (frag != pkt->buffer) {
net_pkt_frag_unref(frag);
}
return false;
}
/* Adds IPv6 dispatch as first byte and adjust fragments */
static inline int compress_ipv6_header(struct net_pkt *pkt)
{
struct net_buf *buffer = pkt->buffer;
if (net_buf_tailroom(buffer) >= 1U) {
memmove(buffer->data + 1U, buffer->data, buffer->len);
net_buf_add(buffer, 1U);
buffer->data[0] = NET_6LO_DISPATCH_IPV6;
return 0;
}
buffer = net_pkt_get_frag(pkt, K_FOREVER);
if (!buffer) {
return -ENOBUFS;
}
buffer->data[0] = NET_6LO_DISPATCH_IPV6;
net_buf_add(buffer, 1);
net_pkt_frag_insert(pkt, buffer);
/* Compact the fragments, so that gaps will be filled */
net_pkt_compact(pkt);
return 0;
}
static inline bool uncompress_ipv6_header(struct net_pkt *pkt)
{
/* Pull off IPv6 dispatch header and adjust data and length */
net_buf_pull(pkt->buffer, 1U);
net_pkt_cursor_init(pkt);
return true;
}
int net_6lo_compress(struct net_pkt *pkt, bool iphc)
{
if (iphc) {
return compress_IPHC_header(pkt);
} else {
return compress_ipv6_header(pkt);
}
}
bool net_6lo_uncompress(struct net_pkt *pkt)
{
NET_ASSERT(pkt && pkt->frags);
if ((pkt->frags->data[0] & NET_6LO_DISPATCH_IPHC_MASK) ==
NET_6LO_DISPATCH_IPHC) {
/* Uncompress IPHC header */
return uncompress_IPHC_header(pkt);
} else if (pkt->frags->data[0] == NET_6LO_DISPATCH_IPV6) {
/* Uncompress IPv6 header, it has only IPv6 dispatch in the
* beginning */
return uncompress_ipv6_header(pkt);
}
NET_DBG("pkt %p is not compressed", pkt);
return true;
}
int net_6lo_uncompress_hdr_diff(struct net_pkt *pkt)
{
int inline_size, compressed_hdr_size, nhc_inline_size, diff;
u16_t iphc;
u8_t nhc;
if (pkt->frags->data[0] == NET_6LO_DISPATCH_IPV6) {
return -1;
}
if ((pkt->frags->data[0] & NET_6LO_DISPATCH_IPHC_MASK) !=
NET_6LO_DISPATCH_IPHC) {
return 0;
}
iphc = ntohs(UNALIGNED_GET((u16_t *)pkt->buffer->data));
inline_size = get_ihpc_inlined_size(iphc);
if (inline_size < 0) {
return INT_MAX;
}
compressed_hdr_size = sizeof(iphc) + inline_size;
diff = sizeof(struct net_ipv6_hdr) - compressed_hdr_size;
if (iphc & NET_6LO_IPHC_NH_MASK) {
nhc = *(pkt->buffer->data + sizeof(iphc) + inline_size);
if ((nhc & 0xF8) != NET_6LO_NHC_UDP_BARE) {
NET_ERR("Unsupported next header");
return INT_MAX;
}
nhc_inline_size = get_udp_nhc_inlined_size(nhc);
compressed_hdr_size += sizeof(u8_t) + nhc_inline_size;
diff += sizeof(struct net_udp_hdr) - sizeof(u8_t) -
nhc_inline_size;
}
return diff;
}
Reference in New Issue View Git Blame Copy Permalink