/** @file * @brief 6lopan related functions */ /* * Copyright (c) 2016 Intel Corporation * Copyright (c) 2019 Alexander Wachter * * SPDX-License-Identifier: Apache-2.0 */ #include LOG_MODULE_REGISTER(net_6lo, CONFIG_NET_6LO_LOG_LEVEL); #include #include #include #include #include #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; }