diff --git a/sys/net/gnrc/network_layer/ipv6/ext/gnrc_ipv6_ext.c b/sys/net/gnrc/network_layer/ipv6/ext/gnrc_ipv6_ext.c index 2ac4c6b7cf..3aaaa90f23 100644 --- a/sys/net/gnrc/network_layer/ipv6/ext/gnrc_ipv6_ext.c +++ b/sys/net/gnrc/network_layer/ipv6/ext/gnrc_ipv6_ext.c @@ -25,6 +25,11 @@ #include "net/gnrc/ipv6.h" #include "net/gnrc/ipv6/ext/frag.h" #include "net/gnrc/ipv6/ext/rh.h" +#if defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && \ + defined(MODULE_GNRC_IPV6_EXT_FRAG) +#include "net/udp.h" +#endif /* defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && + * defined(MODULE_GNRC_IPV6_EXT_FRAG) */ #include "net/gnrc/ipv6/ext.h" @@ -101,7 +106,22 @@ gnrc_pktsnip_t *gnrc_ipv6_ext_process_all(gnrc_pktsnip_t *pkt, uint8_t *protnum) { bool is_ext = true; +#if defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && \ + defined(MODULE_GNRC_IPV6_EXT_FRAG) + bool is_frag = false; +#endif /* defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && + * defined(MODULE_GNRC_IPV6_EXT_FRAG) */ + while (is_ext) { +#if defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && \ + defined(MODULE_GNRC_IPV6_EXT_FRAG) + if (*protnum == PROTNUM_IPV6_EXT_FRAG) { + /* just assigning the if expression might override it if + * fragment header is not the last extension header ;-) */ + is_frag = true; + } +#endif /* defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && + * defined(MODULE_GNRC_IPV6_EXT_FRAG) */ switch (*protnum) { case PROTNUM_IPV6_EXT_DST: case PROTNUM_IPV6_EXT_RH: @@ -127,6 +147,34 @@ gnrc_pktsnip_t *gnrc_ipv6_ext_process_all(gnrc_pktsnip_t *pkt, } break; } +#if defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && \ + defined(MODULE_GNRC_IPV6_EXT_FRAG) + case PROTNUM_UDP: + if (is_frag) { + DEBUG("gnrc_ipv6_ext: adapting compressed header length\n"); + /* we can only really now determine the length of the UDP + * packet, after the IPv6 datagram is fully reassembled */ + udp_hdr_t *udp_hdr = pkt->data; + + udp_hdr->length = byteorder_htons((uint16_t)pkt->size); + } + is_ext = false; + break; + case PROTNUM_IPV6: + if (is_frag) { + DEBUG("gnrc_ipv6_ext: adapting compressed header length\n"); + /* we can only really now determine the length of the + * encapsulated IPv6 header, after the IPv6 datagram is + * fully reassembled */ + ipv6_hdr_t *ipv6_hdr = pkt->data; + + ipv6_hdr->len = byteorder_htons((uint16_t)pkt->size - + sizeof(ipv6_hdr_t)); + } + is_ext = false; + break; +#endif /* defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && + * defined(MODULE_GNRC_IPV6_EXT_FRAG) */ default: is_ext = false; break; diff --git a/sys/net/gnrc/network_layer/sixlowpan/iphc/gnrc_sixlowpan_iphc.c b/sys/net/gnrc/network_layer/sixlowpan/iphc/gnrc_sixlowpan_iphc.c index 0cfac91c34..33521af52b 100644 --- a/sys/net/gnrc/network_layer/sixlowpan/iphc/gnrc_sixlowpan_iphc.c +++ b/sys/net/gnrc/network_layer/sixlowpan/iphc/gnrc_sixlowpan_iphc.c @@ -20,6 +20,7 @@ #include "byteorder.h" #include "net/ipv6/hdr.h" +#include "net/ipv6/ext.h" #include "net/gnrc.h" #include "net/gnrc/netif/internal.h" #include "net/gnrc/sixlowpan.h" @@ -96,6 +97,18 @@ #define NHC_UDP_8BIT_PORT (0xF000) #define NHC_UDP_8BIT_MASK (0xFF00) +#define NHC_IPV6_EXT_ID (0xE0) +#define NHC_IPV6_EXT_ID_ALT (0xE8) /* first bit of EID 1 */ +#define NHC_IPV6_EXT_EID_MASK (0x0E) +#define NHC_IPV6_EXT_NH (0x01) + +#define NHC_IPV6_EXT_EID_HOPOPT (0x00 << 1) +#define NHC_IPV6_EXT_EID_RH (0x01 << 1) +#define NHC_IPV6_EXT_EID_FRAG (0x02 << 1) +#define NHC_IPV6_EXT_EID_DST (0x03 << 1) +#define NHC_IPV6_EXT_EID_MOB (0x04 << 1) +#define NHC_IPV6_EXT_EID_IPV6 (0x07 << 1) + /* currently only used with forwarding output, remove guard if more debug info * is added */ #ifdef MODULE_GNRC_SIXLOWPAN_FRAG_VRB @@ -134,148 +147,12 @@ static int _forward_frag(gnrc_pktsnip_t *pkt, gnrc_pktsnip_t *frag_hdr, gnrc_sixlowpan_frag_vrb_t *vrbe, unsigned page); #endif /* MODULE_GNRC_SIXLOWPAN_FRAG_VRB */ -#ifdef MODULE_GNRC_SIXLOWPAN_IPHC_NHC -/** - * @brief Decodes UDP NHC - * - * @param[in] sixlo The IPHC encoded packet - * @param[in] offset The offset of the NHC encoded header - * @param[in] rbuf Reassembly buffer entry if @p ipv6 is a - * fragmented datagram. May be NULL, if @p ipv6 - * is not fragmented - * @param[out] ipv6 The packet to write the decoded data to - * @param[in,out] uncomp_hdr_len Number of bytes already decoded into @p ipv6 - * by IPHC and other NHC. Adds size of @ref - * udp_hdr_t after successful UDP header - * decompression - * - * @return The offset after UDP NHC header on success. - * @return 0 on error. - */ -static size_t _iphc_nhc_udp_decode(gnrc_pktsnip_t *sixlo, size_t offset, - const gnrc_sixlowpan_frag_rb_t *rbuf, - gnrc_pktsnip_t *ipv6, size_t *uncomp_hdr_len) +static size_t _iphc_ipv6_decode(const uint8_t *iphc_hdr, + const gnrc_netif_hdr_t *netif_hdr, + gnrc_netif_t *iface, ipv6_hdr_t *ipv6_hdr) { - uint8_t *payload = sixlo->data; - ipv6_hdr_t *ipv6_hdr; - udp_hdr_t *udp_hdr; - uint16_t payload_len; - uint8_t udp_nhc = payload[offset++]; - uint8_t tmp; - - /* realloc size for uncompressed snip, if too small */ - if (ipv6->size < (*uncomp_hdr_len + sizeof(udp_hdr_t))) { - if (gnrc_pktbuf_realloc_data(ipv6, - *uncomp_hdr_len + sizeof(udp_hdr_t))) { - DEBUG("6lo: unable to decode UDP NHC (not enough buffer space)\n"); - return 0; - } - } - ipv6_hdr = ipv6->data; - udp_hdr = (udp_hdr_t *)((uint8_t *)ipv6->data + *uncomp_hdr_len); - network_uint16_t *src_port = &(udp_hdr->src_port); - network_uint16_t *dst_port = &(udp_hdr->dst_port); - - switch (udp_nhc & NHC_UDP_PP_MASK) { - - case NHC_UDP_SD_INLINE: - DEBUG("6lo iphc nhc: SD_INLINE\n"); - src_port->u8[0] = payload[offset++]; - src_port->u8[1] = payload[offset++]; - dst_port->u8[0] = payload[offset++]; - dst_port->u8[1] = payload[offset++]; - break; - - case NHC_UDP_S_INLINE: - DEBUG("6lo iphc nhc: S_INLINE\n"); - src_port->u8[0] = payload[offset++]; - src_port->u8[1] = payload[offset++]; - *dst_port = byteorder_htons(payload[offset++] + NHC_UDP_8BIT_PORT); - break; - - case NHC_UDP_D_INLINE: - DEBUG("6lo iphc nhc: D_INLINE\n"); - *src_port = byteorder_htons(payload[offset++] + NHC_UDP_8BIT_PORT); - dst_port->u8[0] = payload[offset++]; - dst_port->u8[1] = payload[offset++]; - break; - - case NHC_UDP_SD_ELIDED: - DEBUG("6lo iphc nhc: SD_ELIDED\n"); - tmp = payload[offset++]; - *src_port = byteorder_htons((tmp >> 4) + NHC_UDP_4BIT_PORT); - *dst_port = byteorder_htons((tmp & 0xf) + NHC_UDP_4BIT_PORT); - break; - - default: - break; - } - - if ((udp_nhc & NHC_UDP_C_ELIDED) != 0) { - DEBUG("6lo iphc nhc: unsupported elided checksum\n"); - return 0; - } - else { - udp_hdr->checksum.u8[0] = payload[offset++]; - udp_hdr->checksum.u8[1] = payload[offset++]; - } - - if (rbuf != NULL) { - payload_len = rbuf->super.datagram_size - *uncomp_hdr_len; - } - else { - payload_len = sixlo->size + sizeof(udp_hdr_t) - offset; - } - udp_hdr->length = byteorder_htons(payload_len); - *uncomp_hdr_len += sizeof(udp_hdr_t); - ipv6_hdr->nh = PROTNUM_UDP; - - return offset; -} -#endif - -static inline void _recv_error_release(gnrc_pktsnip_t *sixlo, - gnrc_pktsnip_t *ipv6, - gnrc_sixlowpan_frag_rb_t *rbuf) { - if (rbuf != NULL) { - gnrc_sixlowpan_frag_rb_remove(rbuf); - } - gnrc_pktbuf_release(ipv6); - gnrc_pktbuf_release(sixlo); -} - -void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, - unsigned page) -{ - assert(sixlo != NULL); - gnrc_pktsnip_t *ipv6, *netif; - gnrc_netif_hdr_t *netif_hdr; - gnrc_netif_t *iface; - ipv6_hdr_t *ipv6_hdr; - uint8_t *iphc_hdr = sixlo->data; - size_t payload_offset = SIXLOWPAN_IPHC_HDR_LEN; - size_t uncomp_hdr_len = sizeof(ipv6_hdr_t); gnrc_sixlowpan_ctx_t *ctx = NULL; - gnrc_sixlowpan_frag_rb_t *rbuf = rbuf_ptr; -#ifdef MODULE_GNRC_SIXLOWPAN_FRAG_VRB - gnrc_sixlowpan_frag_vrb_t *vrbe = NULL; -#endif /* MODULE_GNRC_SIXLOWPAN_FRAG_VRB */ - - if (rbuf != NULL) { - ipv6 = rbuf->pkt; - assert(ipv6 != NULL); - } - else { - ipv6 = gnrc_pktbuf_add(NULL, NULL, sizeof(ipv6_hdr_t), - GNRC_NETTYPE_IPV6); - if (ipv6 == NULL) { - gnrc_pktbuf_release(sixlo); - return; - } - } - - assert(ipv6->size >= sizeof(ipv6_hdr_t)); - ipv6_hdr = ipv6->data; + size_t payload_offset = SIXLOWPAN_IPHC_HDR_LEN; if (iphc_hdr[IPHC2_IDX] & SIXLOWPAN_IPHC2_CID_EXT) { payload_offset++; @@ -344,15 +221,11 @@ void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, if (ctx == NULL) { DEBUG("6lo iphc: could not find source context\n"); - _recv_error_release(sixlo, ipv6, rbuf); - return; + return 0; } } } - netif = gnrc_pktsnip_search_type(sixlo, GNRC_NETTYPE_NETIF); - assert(netif != NULL); - netif_hdr = netif->data; iface = gnrc_netif_hdr_get_netif(netif_hdr); switch (iphc_hdr[IPHC2_IDX] & (SIXLOWPAN_IPHC2_SAC | SIXLOWPAN_IPHC2_SAM)) { @@ -381,8 +254,7 @@ void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, iface, netif_hdr, (eui64_t *)(&ipv6_hdr->src.u64[1]) ) < 0) { DEBUG("6lo iphc: could not get source's IID\n"); - _recv_error_release(sixlo, ipv6, rbuf); - return; + return 0; } ipv6_addr_set_link_local_prefix(&ipv6_hdr->src); break; @@ -415,8 +287,7 @@ void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, iface, netif_hdr, (eui64_t *)(&ipv6_hdr->src.u64[1]) ) < 0) { DEBUG("6lo iphc: could not get source's IID\n"); - _recv_error_release(sixlo, ipv6, rbuf); - return; + return 0; } ipv6_addr_init_prefix(&ipv6_hdr->src, &ctx->prefix, ctx->prefix_len); @@ -435,8 +306,7 @@ void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, if (ctx == NULL) { DEBUG("6lo iphc: could not find destination context\n"); - _recv_error_release(sixlo, ipv6, rbuf); - return; + return 0; } } } @@ -468,8 +338,7 @@ void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, iface, netif_hdr, (eui64_t *)(&ipv6_hdr->dst.u64[1]) ) < 0) { DEBUG("6lo iphc: could not get destination's IID\n"); - _recv_error_release(sixlo, ipv6, rbuf); - return; + return 0; } ipv6_addr_set_link_local_prefix(&ipv6_hdr->dst); break; @@ -497,8 +366,7 @@ void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, iface, netif_hdr, (eui64_t *)(&ipv6_hdr->dst.u64[1]) ) < 0) { DEBUG("6lo iphc: could not get destination's IID\n"); - _recv_error_release(sixlo, ipv6, rbuf); - return; + return 0; } assert(ctx != NULL); ipv6_addr_init_prefix(&ipv6_hdr->dst, &ctx->prefix, @@ -559,23 +427,350 @@ void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, DEBUG("6lo iphc: unspecified or reserved M, DAC, DAM combination\n"); break; } - + return payload_offset; +} #ifdef MODULE_GNRC_SIXLOWPAN_IPHC_NHC - if (iphc_hdr[IPHC1_IDX] & SIXLOWPAN_IPHC1_NH) { - switch (iphc_hdr[payload_offset] & NHC_ID_MASK) { - case NHC_UDP_ID: { - payload_offset = _iphc_nhc_udp_decode(sixlo, payload_offset, - rbuf, ipv6, - &uncomp_hdr_len); - if (payload_offset == 0) { - _recv_error_release(sixlo, ipv6, rbuf); - return; - } - break; +static size_t _iphc_nhc_ipv6_ext_decode(gnrc_pktsnip_t *sixlo, size_t offset, + size_t *prev_nh_offset, + gnrc_pktsnip_t *ipv6, + size_t *uncomp_hdr_len) +{ + uint8_t *payload = sixlo->data; + ipv6_ext_t *ext_hdr; + uint8_t ipv6_ext_nhc = payload[offset++]; + uint8_t protnum; + uint8_t ext_len = (ipv6_ext_nhc & NHC_IPV6_EXT_NH) + ? payload[offset] + : payload[offset + 1]; + + /* realloc size for uncompressed snip, if too small */ + if (ipv6->size < (*uncomp_hdr_len + sizeof(ipv6_ext_t) + ext_len)) { + if (gnrc_pktbuf_realloc_data(ipv6, + *uncomp_hdr_len + sizeof(ipv6_ext_t) + + ext_len)) { + DEBUG("6lo iphc: unable to decode IPv6 Extension header NHC " + "(not enough buffer space)\n"); + return 0; + } + } + ext_hdr = (ipv6_ext_t *)((uint8_t *)ipv6->data + *uncomp_hdr_len); + switch (ipv6_ext_nhc & NHC_IPV6_EXT_EID_MASK) { + case NHC_IPV6_EXT_EID_HOPOPT: + protnum = PROTNUM_IPV6_EXT_HOPOPT; + break; + case NHC_IPV6_EXT_EID_RH: + protnum = PROTNUM_IPV6_EXT_RH; + break; + case NHC_IPV6_EXT_EID_FRAG: + protnum = PROTNUM_IPV6_EXT_FRAG; + break; + case NHC_IPV6_EXT_EID_DST: + protnum = PROTNUM_IPV6_EXT_DST; + break; + case NHC_IPV6_EXT_EID_MOB: + protnum = PROTNUM_IPV6_EXT_MOB; + break; + default: + DEBUG("6lo iphc: unexpected extension header EID %u\n", + (ipv6_ext_nhc & NHC_IPV6_EXT_EID_MASK) >> 1U); + return 0; + } + ((uint8_t *)ipv6->data)[*prev_nh_offset] = protnum; + if (!(ipv6_ext_nhc & NHC_IPV6_EXT_NH)) { + ext_hdr->nh = payload[offset++]; + /* signal end of next header compression to caller */ + *prev_nh_offset = 0; + } + else { + *prev_nh_offset = (&ext_hdr->nh) - ((uint8_t *)ipv6->data); + } + /* skip already fetched length field */ + offset++; + ext_hdr->len = ((sizeof(ipv6_ext_t) + ext_len) - IPV6_EXT_LEN_UNIT) / + IPV6_EXT_LEN_UNIT; + memcpy(ext_hdr + 1, &payload[offset], ext_len); + offset += ext_len; + *uncomp_hdr_len += sizeof(ipv6_ext_t) + ext_len; + return offset; +} + +static size_t _iphc_nhc_ipv6_decode(gnrc_pktsnip_t *sixlo, size_t offset, + const gnrc_sixlowpan_frag_rb_t *rbuf, + size_t *prev_nh_offset, + gnrc_pktsnip_t *ipv6, + size_t *uncomp_hdr_len) +{ + uint8_t *payload = sixlo->data; + uint8_t ipv6_nhc = payload[offset]; + + switch (ipv6_nhc & NHC_IPV6_EXT_EID_MASK) { + case NHC_IPV6_EXT_EID_HOPOPT: + case NHC_IPV6_EXT_EID_RH: + case NHC_IPV6_EXT_EID_FRAG: + case NHC_IPV6_EXT_EID_DST: + case NHC_IPV6_EXT_EID_MOB: { + size_t tmp; + tmp = _iphc_nhc_ipv6_ext_decode(sixlo, offset, prev_nh_offset, + ipv6, uncomp_hdr_len); + if (tmp == 0) { + /* unable to parse IPHC header */ + return 0; + } + offset = tmp; + break; + } + case NHC_IPV6_EXT_EID_IPV6: { + gnrc_pktsnip_t *netif = gnrc_pktsnip_search_type(sixlo, + GNRC_NETTYPE_NETIF); + ipv6_hdr_t *ipv6_hdr; + uint16_t payload_len; + size_t tmp; + + offset++; /* move over NHC header */ + /* realloc size for uncompressed snip, if too small */ + if (ipv6->size < (*uncomp_hdr_len + sizeof(ipv6_hdr_t))) { + if (gnrc_pktbuf_realloc_data(ipv6, + *uncomp_hdr_len + + sizeof(ipv6_hdr_t))) { + DEBUG("6lo iphc: unable to decode IPv6 encapsulated header " + "NHC (not enough buffer space)\n"); + return 0; + } + } + ipv6_hdr = (ipv6_hdr_t *)(((uint8_t *)ipv6->data) + *uncomp_hdr_len); + tmp = _iphc_ipv6_decode(&payload[offset], netif->data, + gnrc_netif_hdr_get_netif(netif->data), + ipv6_hdr); + if (tmp == 0) { + /* unable to parse IPHC header */ + return 0; + } + ((uint8_t *)ipv6->data)[*prev_nh_offset] = PROTNUM_IPV6; + if (payload[offset + IPHC1_IDX] & SIXLOWPAN_IPHC1_NH) { + *prev_nh_offset = (&ipv6_hdr->nh) - ((uint8_t *)ipv6->data); + } + else { + /* signal end of next header compression to caller */ + *prev_nh_offset = 0; + } + offset += tmp; + /* might be needed to be overwritten by IPv6 reassembly after the IPv6 + * packet was reassembled to get complete length */ + if (rbuf != NULL) { + payload_len = rbuf->super.datagram_size - *uncomp_hdr_len- + sizeof(ipv6_hdr_t); + } + else { + payload_len = (sixlo->size + *uncomp_hdr_len) - + sizeof(ipv6_hdr_t) - offset; + } + ipv6_hdr->len = byteorder_htons(payload_len); + *uncomp_hdr_len += sizeof(ipv6_hdr_t); + break; + } + default: + DEBUG("6lo iphc: unknown IPv6 extension header EID\n"); + break; + } + return offset; +} + +/** + * @brief Decodes UDP NHC + * + * @param[in] sixlo The IPHC encoded packet + * @param[in] offset The offset of the NHC encoded header + * @param[in] rbuf Reassembly buffer entry if @p ipv6 is a + * fragmented datagram. May be NULL, if @p ipv6 + * is not fragmented + * @param[out] prev_nh_offset Offset to previous nh field in + * gnrc_pktsnip_t::data of @p ipv6 + * @param[out] ipv6 The packet to write the decoded data to + * @param[in,out] uncomp_hdr_len Number of bytes already decoded into @p ipv6 + * by IPHC and other NHC. Adds size of @ref + * udp_hdr_t after successful UDP header + * decompression + * + * @return The offset after UDP NHC header on success. + * @return 0 on error. + */ +static size_t _iphc_nhc_udp_decode(gnrc_pktsnip_t *sixlo, size_t offset, + const gnrc_sixlowpan_frag_rb_t *rbuf, + size_t prev_nh_offset, gnrc_pktsnip_t *ipv6, + size_t *uncomp_hdr_len) +{ + uint8_t *payload = sixlo->data; + udp_hdr_t *udp_hdr; + uint16_t payload_len; + uint8_t udp_nhc = payload[offset++]; + uint8_t tmp; + + /* realloc size for uncompressed snip, if too small */ + if (ipv6->size < (*uncomp_hdr_len + sizeof(udp_hdr_t))) { + if (gnrc_pktbuf_realloc_data(ipv6, + *uncomp_hdr_len + sizeof(udp_hdr_t))) { + DEBUG("6lo: unable to decode UDP NHC (not enough buffer space)\n"); + return 0; + } + } + udp_hdr = (udp_hdr_t *)((uint8_t *)ipv6->data + *uncomp_hdr_len); + network_uint16_t *src_port = &(udp_hdr->src_port); + network_uint16_t *dst_port = &(udp_hdr->dst_port); + + switch (udp_nhc & NHC_UDP_PP_MASK) { + + case NHC_UDP_SD_INLINE: + DEBUG("6lo iphc nhc: SD_INLINE\n"); + src_port->u8[0] = payload[offset++]; + src_port->u8[1] = payload[offset++]; + dst_port->u8[0] = payload[offset++]; + dst_port->u8[1] = payload[offset++]; + break; + + case NHC_UDP_S_INLINE: + DEBUG("6lo iphc nhc: S_INLINE\n"); + src_port->u8[0] = payload[offset++]; + src_port->u8[1] = payload[offset++]; + *dst_port = byteorder_htons(payload[offset++] + NHC_UDP_8BIT_PORT); + break; + + case NHC_UDP_D_INLINE: + DEBUG("6lo iphc nhc: D_INLINE\n"); + *src_port = byteorder_htons(payload[offset++] + NHC_UDP_8BIT_PORT); + dst_port->u8[0] = payload[offset++]; + dst_port->u8[1] = payload[offset++]; + break; + + case NHC_UDP_SD_ELIDED: + DEBUG("6lo iphc nhc: SD_ELIDED\n"); + tmp = payload[offset++]; + *src_port = byteorder_htons((tmp >> 4) + NHC_UDP_4BIT_PORT); + *dst_port = byteorder_htons((tmp & 0xf) + NHC_UDP_4BIT_PORT); + break; + + default: + break; + } + + if ((udp_nhc & NHC_UDP_C_ELIDED) != 0) { + DEBUG("6lo iphc nhc: unsupported elided checksum\n"); + return 0; + } + else { + udp_hdr->checksum.u8[0] = payload[offset++]; + udp_hdr->checksum.u8[1] = payload[offset++]; + } + + /* might be needed to be overwritten by IPv6 reassembly after the IPv6 + * packet was reassembled to get complete length */ + if (rbuf != NULL) { + payload_len = rbuf->super.datagram_size - *uncomp_hdr_len; + } + else { + payload_len = sixlo->size + sizeof(udp_hdr_t) - offset; + } + udp_hdr->length = byteorder_htons(payload_len); + *uncomp_hdr_len += sizeof(udp_hdr_t); + ((uint8_t *)ipv6->data)[prev_nh_offset] = PROTNUM_UDP; + + return offset; +} +#endif + +static inline void _recv_error_release(gnrc_pktsnip_t *sixlo, + gnrc_pktsnip_t *ipv6, + gnrc_sixlowpan_frag_rb_t *rbuf) { + if (rbuf != NULL) { + gnrc_sixlowpan_frag_rb_remove(rbuf); + } + gnrc_pktbuf_release(ipv6); + gnrc_pktbuf_release(sixlo); +} + +void gnrc_sixlowpan_iphc_recv(gnrc_pktsnip_t *sixlo, void *rbuf_ptr, + unsigned page) +{ + assert(sixlo != NULL); + gnrc_pktsnip_t *ipv6, *netif; + gnrc_netif_t *iface; + ipv6_hdr_t *ipv6_hdr; + uint8_t *iphc_hdr = sixlo->data; + size_t payload_offset; + size_t uncomp_hdr_len = sizeof(ipv6_hdr_t); + gnrc_sixlowpan_frag_rb_t *rbuf = rbuf_ptr; +#ifdef MODULE_GNRC_SIXLOWPAN_FRAG_VRB + gnrc_sixlowpan_frag_vrb_t *vrbe = NULL; +#endif /* MODULE_GNRC_SIXLOWPAN_FRAG_VRB */ + + if (rbuf != NULL) { + ipv6 = rbuf->pkt; + assert(ipv6 != NULL); + } + else { + ipv6 = gnrc_pktbuf_add(NULL, NULL, sizeof(ipv6_hdr_t), + GNRC_NETTYPE_IPV6); + if (ipv6 == NULL) { + gnrc_pktbuf_release(sixlo); + return; + } + } + + assert(ipv6->size >= sizeof(ipv6_hdr_t)); + + netif = gnrc_pktsnip_search_type(sixlo, GNRC_NETTYPE_NETIF); + assert(netif != NULL); + iface = gnrc_netif_hdr_get_netif(netif->data); + payload_offset = _iphc_ipv6_decode(iphc_hdr, netif->data, iface, + ipv6->data); + if (payload_offset == 0) { + /* unable to parse IPHC header */ + _recv_error_release(sixlo, ipv6, rbuf); + return; + } +#ifdef MODULE_GNRC_SIXLOWPAN_IPHC_NHC + if (iphc_hdr[IPHC1_IDX] & SIXLOWPAN_IPHC1_NH) { + bool nhc_header = true; + ipv6_hdr = ipv6->data; + size_t prev_nh_offset = (&ipv6_hdr->nh) - ((uint8_t *)ipv6->data); + + while (nhc_header) { + switch (iphc_hdr[payload_offset] & NHC_ID_MASK) { + case NHC_IPV6_EXT_ID: + case NHC_IPV6_EXT_ID_ALT: + payload_offset = _iphc_nhc_ipv6_decode(sixlo, + payload_offset, + rbuf, + &prev_nh_offset, + ipv6, + &uncomp_hdr_len); + if (payload_offset == 0) { + _recv_error_release(sixlo, ipv6, rbuf); + return; + } + /* prev_nh_offset is set to 0 if next header is not + * compressed (== NH flag in compression header not set) */ + nhc_header = (prev_nh_offset > 0); + break; + case NHC_UDP_ID: { + payload_offset = _iphc_nhc_udp_decode(sixlo, + payload_offset, + rbuf, + prev_nh_offset, + ipv6, + &uncomp_hdr_len); + if (payload_offset == 0) { + _recv_error_release(sixlo, ipv6, rbuf); + return; + } + /* no NHC after UDP header */ + nhc_header = false; + break; + } + default: + nhc_header = false; + break; } - default: - break; } } #endif @@ -742,130 +937,35 @@ static int _forward_frag(gnrc_pktsnip_t *pkt, gnrc_pktsnip_t *frag_hdr, } #endif /* MODULE_GNRC_SIXLOWPAN_FRAG_VRB */ +static inline bool _compressible_nh(uint8_t nh) +{ + switch (nh) { #ifdef MODULE_GNRC_SIXLOWPAN_IPHC_NHC -static inline size_t iphc_nhc_udp_encode(uint8_t *nhc_data, - const gnrc_pktsnip_t *udp) -{ - const udp_hdr_t *udp_hdr = udp->data; - uint16_t src_port = byteorder_ntohs(udp_hdr->src_port); - uint16_t dst_port = byteorder_ntohs(udp_hdr->dst_port); - size_t nhc_len = 1; /* skip over NHC header */ - - /* Set UDP NHC header type - * (see https://tools.ietf.org/html/rfc6282#section-4.3). */ - nhc_data[0] = NHC_UDP_ID; - /* Compressing UDP ports, follow the same sequence as the linux kernel (nhc_udp module). */ - if (((src_port & NHC_UDP_4BIT_MASK) == NHC_UDP_4BIT_PORT) && - ((dst_port & NHC_UDP_4BIT_MASK) == NHC_UDP_4BIT_PORT)) { - DEBUG("6lo iphc nhc: elide src and dst\n"); - nhc_data[0] |= NHC_UDP_SD_ELIDED; - nhc_data[nhc_len++] = dst_port - NHC_UDP_4BIT_PORT + - ((src_port - NHC_UDP_4BIT_PORT) << 4); - } - else if ((dst_port & NHC_UDP_8BIT_MASK) == NHC_UDP_8BIT_PORT) { - DEBUG("6lo iphc nhc: elide dst\n"); - nhc_data[0] |= NHC_UDP_S_INLINE; - nhc_data[nhc_len++] = udp_hdr->src_port.u8[0]; - nhc_data[nhc_len++] = udp_hdr->src_port.u8[1]; - nhc_data[nhc_len++] = dst_port - NHC_UDP_8BIT_PORT; - } - else if ((src_port & NHC_UDP_8BIT_MASK) == NHC_UDP_8BIT_PORT) { - DEBUG("6lo iphc nhc: elide src\n"); - nhc_data[0] |= NHC_UDP_D_INLINE; - nhc_data[nhc_len++] = src_port - NHC_UDP_8BIT_PORT; - nhc_data[nhc_len++] = udp_hdr->dst_port.u8[0]; - nhc_data[nhc_len++] = udp_hdr->dst_port.u8[1]; - } - else { - DEBUG("6lo iphc nhc: src and dst inline\n"); - nhc_data[0] |= NHC_UDP_SD_INLINE; - nhc_data[nhc_len++] = udp_hdr->src_port.u8[0]; - nhc_data[nhc_len++] = udp_hdr->src_port.u8[1]; - nhc_data[nhc_len++] = udp_hdr->dst_port.u8[0]; - nhc_data[nhc_len++] = udp_hdr->dst_port.u8[1]; - } - - /* TODO: Add support for elided checksum. */ - nhc_data[nhc_len++] = udp_hdr->checksum.u8[0]; - nhc_data[nhc_len++] = udp_hdr->checksum.u8[1]; - - return nhc_len; -} -#endif - -static inline bool _compressible(gnrc_pktsnip_t *hdr) -{ - switch (hdr->type) { - case GNRC_NETTYPE_UNDEF: /* when forwarded */ - case GNRC_NETTYPE_IPV6: -#if defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && defined(MODULE_GNRC_UDP) - case GNRC_NETTYPE_UDP: -#endif + case PROTNUM_IPV6_EXT_HOPOPT: + case PROTNUM_UDP: + case PROTNUM_IPV6: + case PROTNUM_IPV6_EXT_RH: + case PROTNUM_IPV6_EXT_FRAG: + case PROTNUM_IPV6_EXT_DST: + case PROTNUM_IPV6_EXT_MOB: return true; +#endif default: return false; } } -static gnrc_pktsnip_t *_iphc_encode(gnrc_pktsnip_t *pkt, - const gnrc_netif_hdr_t *netif_hdr, - gnrc_netif_t *iface) +static size_t _iphc_ipv6_encode(gnrc_pktsnip_t *pkt, + const gnrc_netif_hdr_t *netif_hdr, + gnrc_netif_t *iface, + uint8_t *iphc_hdr) { - assert(pkt != NULL); - ipv6_hdr_t *ipv6_hdr; - uint8_t *iphc_hdr; gnrc_sixlowpan_ctx_t *src_ctx = NULL, *dst_ctx = NULL; - gnrc_pktsnip_t *dispatch, *ptr = pkt->next; + ipv6_hdr_t *ipv6_hdr = pkt->next->data; bool addr_comp = false; - size_t dispatch_size = 0; uint16_t inline_pos = SIXLOWPAN_IPHC_HDR_LEN; assert(iface != NULL); - dispatch = NULL; /* use dispatch as temporary pointer for prev */ - /* determine maximum dispatch size and write protect all headers until - * then because they will be removed */ - while ((ptr != NULL) && _compressible(ptr)) { - gnrc_pktsnip_t *tmp = gnrc_pktbuf_start_write(ptr); - - if (tmp == NULL) { - DEBUG("6lo iphc: unable to write protect compressible header\n"); - return NULL; - } - ptr = tmp; - if (dispatch == NULL) { - /* pkt was already write protected in gnrc_sixlowpan.c:_send so - * we shouldn't do it again */ - pkt->next = ptr; /* reset original packet */ - } - else { - dispatch->next = ptr; - } - if (ptr->type == GNRC_NETTYPE_UNDEF) { - /* most likely UDP for now so use that (XXX: extend if extension - * headers make problems) */ - dispatch_size += sizeof(udp_hdr_t); - break; /* nothing special after UDP so quit even if more UNDEF - * come */ - } - else { - dispatch_size += ptr->size; - } - dispatch = ptr; /* use dispatch as temporary point for prev */ - ptr = ptr->next; - } - /* there should be at least one compressible header in `pkt`, otherwise this - * function should not be called */ - assert(dispatch_size > 0); - ipv6_hdr = pkt->next->data; - dispatch = gnrc_pktbuf_add(NULL, NULL, dispatch_size, - GNRC_NETTYPE_SIXLOWPAN); - - if (dispatch == NULL) { - DEBUG("6lo iphc: error allocating dispatch space\n"); - return NULL; - } - - iphc_hdr = dispatch->data; /* set initial dispatch value*/ iphc_hdr[IPHC1_IDX] = SIXLOWPAN_IPHC1_DISP; @@ -936,16 +1036,11 @@ static gnrc_pktsnip_t *_iphc_encode(gnrc_pktsnip_t *pkt, } /* check for compressible next header */ - switch (ipv6_hdr->nh) { -#ifdef MODULE_GNRC_SIXLOWPAN_IPHC_NHC - case PROTNUM_UDP: - iphc_hdr[IPHC1_IDX] |= SIXLOWPAN_IPHC1_NH; - break; -#endif - - default: - iphc_hdr[inline_pos++] = ipv6_hdr->nh; - break; + if (_compressible_nh(ipv6_hdr->nh)) { + iphc_hdr[IPHC1_IDX] |= SIXLOWPAN_IPHC1_NH; + } + else { + iphc_hdr[inline_pos++] = ipv6_hdr->nh; } /* compress hop limit */ @@ -989,7 +1084,7 @@ static gnrc_pktsnip_t *_iphc_encode(gnrc_pktsnip_t *pkt, if (gnrc_netif_ipv6_get_iid(iface, &iid) < 0) { DEBUG("6lo iphc: could not get interface's IID\n"); gnrc_netif_release(iface); - return NULL; + return 0; } gnrc_netif_release(iface); @@ -1107,7 +1202,7 @@ static gnrc_pktsnip_t *_iphc_encode(gnrc_pktsnip_t *pkt, if (gnrc_netif_hdr_ipv6_iid_from_dst(iface, netif_hdr, &iid) < 0) { DEBUG("6lo iphc: could not get destination's IID\n"); - return NULL; + return 0; } if ((ipv6_hdr->dst.u64[1].u64 == iid.uint64.u64) || @@ -1140,29 +1235,354 @@ static gnrc_pktsnip_t *_iphc_encode(gnrc_pktsnip_t *pkt, inline_pos += 16; } + return inline_pos; +} + #ifdef MODULE_GNRC_SIXLOWPAN_IPHC_NHC - switch (ipv6_hdr->nh) { - case PROTNUM_UDP: { - gnrc_pktsnip_t *udp = pkt->next->next; +static ssize_t _iphc_nhc_ipv6_ext_encode(uint8_t *nhc_data, + const gnrc_pktsnip_t *ext, + uint16_t ext_len, + uint8_t *protnum) +{ + const ipv6_ext_t *ext_hdr = ext->data; + size_t nhc_len = 1; /* skip over NHC header */ + uint8_t nh = ext_hdr->nh; - assert(udp->size >= sizeof(udp_hdr_t)); - inline_pos += iphc_nhc_udp_encode(&iphc_hdr[inline_pos], udp); - /* remove UDP header */ - if (udp->size > sizeof(udp_hdr_t)) { - udp = gnrc_pktbuf_mark(udp, sizeof(udp_hdr_t), - GNRC_NETTYPE_UNDEF); - - if (udp == NULL) { - DEBUG("gnrc_sixlowpan_iphc_encode: unable to mark UDP header\n"); - gnrc_pktbuf_release(dispatch); - return NULL; - } - } - gnrc_pktbuf_remove_snip(pkt, udp); + /* From https://tools.ietf.org/html/rfc6282#section-4.1: + * > The Length field contained in a compressed IPv6 Extension Header + * > indicates the number of octets that pertain to the (compressed) + * > extension header following the Length field. + * + * ipv6_ext_t is nh + length field so subtract it + */ + ext_len -= sizeof(ipv6_ext_t); + if (ext_len > UINT8_MAX) { + /* From https://tools.ietf.org/html/rfc6282#section-4.1: + * > Note that specifying units in octets means that LOWPAN_NHC MUST NOT + * > be used to encode IPv6 Extension Headers that have more than 255 + * > octets following the Length field after compression. */ + return 0; + } + /* Set IPv6 extension compression header type + * (see https://tools.ietf.org/html/rfc6282#section-4.2). */ + nhc_data[0] = NHC_IPV6_EXT_ID; + switch (*protnum) { + case PROTNUM_IPV6_EXT_HOPOPT: + nhc_data[0] |= NHC_IPV6_EXT_EID_HOPOPT; + /* TODO: decrement ext_len by length of trailing Pad1/PadN option: + * > IPv6 Hop-by-Hop and Destination Options Headers may use a trailing + * > Pad1 or PadN to achieve 8-octet alignment. When there is a single + * > trailing Pad1 or PadN option of 7 octets or less and the containing + * > header is a multiple of 8 octets, the trailing Pad1 or PadN option + * > MAY be elided by the compressor. */ + break; + case PROTNUM_IPV6_EXT_RH: + nhc_data[0] |= NHC_IPV6_EXT_EID_RH; + break; + case PROTNUM_IPV6_EXT_FRAG: + nhc_data[0] |= NHC_IPV6_EXT_EID_FRAG; + break; + case PROTNUM_IPV6_EXT_DST: + nhc_data[0] |= NHC_IPV6_EXT_EID_DST; + /* TODO: decrement ext_len by length of trailing Pad1/PadN option: + * > IPv6 Hop-by-Hop and Destination Options Headers may use a trailing + * > Pad1 or PadN to achieve 8-octet alignment. When there is a single + * > trailing Pad1 or PadN option of 7 octets or less and the containing + * > header is a multiple of 8 octets, the trailing Pad1 or PadN option + * > MAY be elided by the compressor. */ + break; + case PROTNUM_IPV6_EXT_MOB: + nhc_data[0] |= NHC_IPV6_EXT_EID_MOB; break; - } default: - break; + return -1; + } + if (_compressible_nh(nh) && + /* carry next header inline when fragment header and offset is equal to + * 0 (which means the next header indicates the next header after the + * fragment header in the *first fragment*) */ + ((*protnum != PROTNUM_IPV6_EXT_FRAG) || + (ipv6_ext_frag_get_offset((ipv6_ext_frag_t *)ext_hdr) == 0))) { + nhc_data[0] |= NHC_IPV6_EXT_NH; + } + else { + nhc_data[nhc_len++] = ext_hdr->nh; + /* prevent next header from being encoded regardless (e.g. if not + * first IPv6 fragment) */ + nh = PROTNUM_RESERVED; + } + /* integer overflow prevented by `ext_len > UINT8_MAX` check above */ + nhc_data[nhc_len++] = (uint8_t)ext_len; + memcpy(&nhc_data[nhc_len], ext_hdr + 1, ext_len); + *protnum = nh; + return nhc_len + ext_len; +} + +static inline size_t iphc_nhc_udp_encode(uint8_t *nhc_data, + const gnrc_pktsnip_t *udp) +{ + const udp_hdr_t *udp_hdr = udp->data; + uint16_t src_port = byteorder_ntohs(udp_hdr->src_port); + uint16_t dst_port = byteorder_ntohs(udp_hdr->dst_port); + size_t nhc_len = 1; /* skip over NHC header */ + + /* Set UDP NHC header type + * (see https://tools.ietf.org/html/rfc6282#section-4.3). */ + nhc_data[0] = NHC_UDP_ID; + /* Compressing UDP ports, follow the same sequence as the linux kernel (nhc_udp module). */ + if (((src_port & NHC_UDP_4BIT_MASK) == NHC_UDP_4BIT_PORT) && + ((dst_port & NHC_UDP_4BIT_MASK) == NHC_UDP_4BIT_PORT)) { + DEBUG("6lo iphc nhc: elide src and dst\n"); + nhc_data[0] |= NHC_UDP_SD_ELIDED; + nhc_data[nhc_len++] = dst_port - NHC_UDP_4BIT_PORT + + ((src_port - NHC_UDP_4BIT_PORT) << 4); + } + else if ((dst_port & NHC_UDP_8BIT_MASK) == NHC_UDP_8BIT_PORT) { + DEBUG("6lo iphc nhc: elide dst\n"); + nhc_data[0] |= NHC_UDP_S_INLINE; + nhc_data[nhc_len++] = udp_hdr->src_port.u8[0]; + nhc_data[nhc_len++] = udp_hdr->src_port.u8[1]; + nhc_data[nhc_len++] = dst_port - NHC_UDP_8BIT_PORT; + } + else if ((src_port & NHC_UDP_8BIT_MASK) == NHC_UDP_8BIT_PORT) { + DEBUG("6lo iphc nhc: elide src\n"); + nhc_data[0] |= NHC_UDP_D_INLINE; + nhc_data[nhc_len++] = src_port - NHC_UDP_8BIT_PORT; + nhc_data[nhc_len++] = udp_hdr->dst_port.u8[0]; + nhc_data[nhc_len++] = udp_hdr->dst_port.u8[1]; + } + else { + DEBUG("6lo iphc nhc: src and dst inline\n"); + nhc_data[0] |= NHC_UDP_SD_INLINE; + nhc_data[nhc_len++] = udp_hdr->src_port.u8[0]; + nhc_data[nhc_len++] = udp_hdr->src_port.u8[1]; + nhc_data[nhc_len++] = udp_hdr->dst_port.u8[0]; + nhc_data[nhc_len++] = udp_hdr->dst_port.u8[1]; + } + + /* TODO: Add support for elided checksum. */ + nhc_data[nhc_len++] = udp_hdr->checksum.u8[0]; + nhc_data[nhc_len++] = udp_hdr->checksum.u8[1]; + + return nhc_len; +} + +static bool _remove_header(gnrc_pktsnip_t *pkt, gnrc_pktsnip_t *hdr, + size_t exp_hdr_size) +{ + if (hdr->size > exp_hdr_size) { + hdr = gnrc_pktbuf_mark(hdr, exp_hdr_size, + GNRC_NETTYPE_UNDEF); + + if (hdr == NULL) { + DEBUG("6lo iphc: unable to remove compressed header\n"); + return false; + } + } + gnrc_pktbuf_remove_snip(pkt, hdr); + return true; +} + +static ssize_t _nhc_ipv6_encode_snip(gnrc_pktsnip_t *pkt, + const gnrc_netif_hdr_t *netif_hdr, + gnrc_netif_t *iface, + uint8_t *nhc_data, + uint8_t *nh) +{ + gnrc_pktsnip_t *hdr = pkt->next->next; + ssize_t nhc_len = 1; /* skip over NHC header */ + size_t tmp; + uint8_t new_nh = ((ipv6_hdr_t *)hdr->data)->nh; + + assert(hdr->size >= sizeof(ipv6_hdr_t)); + /* Set IPv6 extension compression header type + * (see https://tools.ietf.org/html/rfc6282#section-4.2). */ + nhc_data[0] = NHC_IPV6_EXT_ID; + if (_compressible_nh(new_nh)) { + nhc_data[0] |= NHC_IPV6_EXT_NH; + } + else { + nhc_data[nhc_len++] = new_nh; + } + /* save to cast as result is max 40 */ + tmp = (ssize_t)_iphc_ipv6_encode(hdr, netif_hdr, iface, &nhc_data[nhc_len]); + if (tmp == 0) { + DEBUG("6lo iphc: error encoding IPv6 header\n"); + return -1; + } + nhc_len += tmp; + /* remove encapsulated IPv6 header */ + if (!_remove_header(pkt, hdr, sizeof(ipv6_hdr_t))) { + return -1; + } + *nh = new_nh; + return nhc_len; +} + +static ssize_t _nhc_ipv6_ext_encode_snip(gnrc_pktsnip_t *pkt, uint8_t *nhc_data, + uint8_t *nh) +{ + gnrc_pktsnip_t *hdr = pkt->next->next; + ipv6_ext_t *ext = hdr->data; + ssize_t nhc_len; + uint16_t ext_len = ((ext->len * IPV6_EXT_LEN_UNIT) + IPV6_EXT_LEN_UNIT); + uint8_t new_nh = *nh; + + assert((hdr->size >= sizeof(ipv6_ext_t)) && (hdr->size >= ext_len)); + /* _iphc_nhc_ipv6_ext_encode() manipulates nh, so use `new_nh` as temporary + * carrier in case of later errors */ + nhc_len = _iphc_nhc_ipv6_ext_encode(nhc_data, hdr, ext_len, &new_nh); + if (nhc_len == 0) { + /* extension header is not compressible, so don't compress it and + * just copy it after the preceding compression headers */ + return nhc_len; + } + /* remove IPv6 extension header */ + if (!_remove_header(pkt, hdr, ext_len)) { + return -1; + } + *nh = new_nh; + return nhc_len; +} + +static ssize_t _nhc_udp_encode_snip(gnrc_pktsnip_t *pkt, uint8_t *nhc_data) +{ + gnrc_pktsnip_t *hdr = pkt->next->next; + ssize_t nhc_len; + + assert(hdr->size >= sizeof(udp_hdr_t)); + /* save to cast, as result is max 8 */ + nhc_len = (ssize_t)iphc_nhc_udp_encode(nhc_data, hdr); + /* remove UDP header */ + if (!_remove_header(pkt, hdr, sizeof(udp_hdr_t))) { + return -1; + } + return nhc_len; +} +#endif + +static inline bool _compressible(gnrc_pktsnip_t *hdr) +{ + switch (hdr->type) { + case GNRC_NETTYPE_UNDEF: /* when forwarded */ + case GNRC_NETTYPE_IPV6: +#if defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) +# if defined(MODULE_GNRC_IPV6_EXT) + case GNRC_NETTYPE_IPV6_EXT: +# endif /* defined(MODULE_GNRC_IPV6_EXT) */ +# if defined(MODULE_GNRC_UDP) + case GNRC_NETTYPE_UDP: +# endif /* defined(MODULE_GNRC_UDP) */ +#endif + return true; + default: + return false; + } +} + +static gnrc_pktsnip_t *_iphc_encode(gnrc_pktsnip_t *pkt, + const gnrc_netif_hdr_t *netif_hdr, + gnrc_netif_t *iface) +{ + assert(pkt != NULL); + uint8_t *iphc_hdr; + gnrc_pktsnip_t *dispatch, *ptr = pkt->next; + size_t dispatch_size = 0; + uint16_t inline_pos = 0; + uint8_t nh; + + dispatch = NULL; /* use dispatch as temporary pointer for prev */ + /* determine maximum dispatch size and write protect all headers until + * then because they will be removed */ + while ((ptr != NULL) && _compressible(ptr)) { + gnrc_pktsnip_t *tmp = gnrc_pktbuf_start_write(ptr); + + if (tmp == NULL) { + DEBUG("6lo iphc: unable to write protect compressible header\n"); + return NULL; + } + ptr = tmp; + if (dispatch == NULL) { + /* pkt was already write protected in gnrc_sixlowpan.c:_send so + * we shouldn't do it again */ + pkt->next = ptr; /* reset original packet */ + } + else { + dispatch->next = ptr; + } + if (ptr->type == GNRC_NETTYPE_UNDEF) { + /* most likely UDP for now so use that (XXX: extend if extension + * headers make problems) */ + dispatch_size += sizeof(udp_hdr_t); + break; /* nothing special after UDP so quit even if more UNDEF + * come */ + } + else { + dispatch_size += ptr->size; + } + dispatch = ptr; /* use dispatch as temporary point for prev */ + ptr = ptr->next; + } + /* there should be at least one compressible header in `pkt`, otherwise this + * function should not be called */ + assert(dispatch_size > 0); + dispatch = gnrc_pktbuf_add(NULL, NULL, dispatch_size + 1, + GNRC_NETTYPE_SIXLOWPAN); + + if (dispatch == NULL) { + DEBUG("6lo iphc: error allocating dispatch space\n"); + return NULL; + } + + iphc_hdr = dispatch->data; + inline_pos = _iphc_ipv6_encode(pkt, netif_hdr, iface, iphc_hdr); + + if (inline_pos == 0) { + DEBUG("6lo iphc: error encoding IPv6 header\n"); + gnrc_pktbuf_release(dispatch); + return NULL; + } + + nh = ((ipv6_hdr_t *)pkt->next->data)->nh; +#ifdef MODULE_GNRC_SIXLOWPAN_IPHC_NHC + while (_compressible_nh(nh)) { + ssize_t local_pos = 0; + switch (nh) { + case PROTNUM_UDP: + local_pos = _nhc_udp_encode_snip(pkt, &iphc_hdr[inline_pos]); + /* abort loop on next iteration */ + nh = PROTNUM_RESERVED; + break; + case PROTNUM_IPV6: { /* encapsulated IPv6 header */ + local_pos = _nhc_ipv6_encode_snip(pkt, netif_hdr, iface, + &iphc_hdr[inline_pos], &nh); + break; + } + case PROTNUM_IPV6_EXT_HOPOPT: + case PROTNUM_IPV6_EXT_RH: + case PROTNUM_IPV6_EXT_FRAG: + case PROTNUM_IPV6_EXT_DST: + case PROTNUM_IPV6_EXT_MOB: + local_pos = _nhc_ipv6_ext_encode_snip(pkt, + &iphc_hdr[inline_pos], + &nh); + if (local_pos == 0) { + /* abort loop, extension header is not compressible as + * length field is too large value */ + nh = PROTNUM_RESERVED; + } + break; + default: + /* abort loop on next iteration */ + nh = PROTNUM_RESERVED; + break; + } + if (local_pos < 0) { + DEBUG("6lo iphc: error on compressing next header\n"); + gnrc_pktbuf_release(dispatch); + return NULL; + } + inline_pos += local_pos; } #endif diff --git a/tests/gnrc_sixlowpan_iphc_w_vrb/Makefile.ci b/tests/gnrc_sixlowpan_iphc_w_vrb/Makefile.ci index d792290668..01875ac753 100644 --- a/tests/gnrc_sixlowpan_iphc_w_vrb/Makefile.ci +++ b/tests/gnrc_sixlowpan_iphc_w_vrb/Makefile.ci @@ -19,4 +19,6 @@ BOARD_INSUFFICIENT_MEMORY := \ stm32l0538-disco \ telosb \ waspmote-pro \ + wsn430-v1_3b \ + wsn430-v1_4 \ #