diff --git a/Makefile.dep b/Makefile.dep index f05f8d4191..c42434832b 100644 --- a/Makefile.dep +++ b/Makefile.dep @@ -110,6 +110,7 @@ endif ifneq (,$(filter gnrc_sixlowpan_iphc,$(USEMODULE))) USEMODULE += gnrc_sixlowpan USEMODULE += gnrc_sixlowpan_ctx + USEMODULE += gnrc_sixlowpan_iphc_nhc endif ifneq (,$(filter gnrc_sixlowpan,$(USEMODULE))) diff --git a/Makefile.pseudomodules b/Makefile.pseudomodules index ee9c6d12d8..038de122c2 100644 --- a/Makefile.pseudomodules +++ b/Makefile.pseudomodules @@ -11,6 +11,7 @@ PSEUDOMODULES += gnrc_sixlowpan_border_router_default PSEUDOMODULES += gnrc_sixlowpan_nd_border_router PSEUDOMODULES += gnrc_sixlowpan_router PSEUDOMODULES += gnrc_sixlowpan_router_default +PSEUDOMODULES += gnrc_sixlowpan_iphc_nhc PSEUDOMODULES += gnrc_pktbuf PSEUDOMODULES += ieee802154 PSEUDOMODULES += log diff --git a/sys/net/gnrc/network_layer/sixlowpan/gnrc_sixlowpan.c b/sys/net/gnrc/network_layer/sixlowpan/gnrc_sixlowpan.c index 32af618ce5..d9794b527e 100644 --- a/sys/net/gnrc/network_layer/sixlowpan/gnrc_sixlowpan.c +++ b/sys/net/gnrc/network_layer/sixlowpan/gnrc_sixlowpan.c @@ -152,7 +152,12 @@ static void _receive(gnrc_pktsnip_t *pkt) /* Remove IPHC dispatch */ gnrc_pktbuf_remove_snip(pkt, sixlowpan); /* Insert IPv6 header instead */ - ipv6->next = pkt->next; + if (ipv6->next != NULL) { + ipv6->next->next = pkt->next; + } + else { + ipv6->next = pkt->next; + } pkt->next = ipv6; } #endif 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 d1453aa0d3..267f0da57f 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 @@ -1,5 +1,6 @@ /* * Copyright (C) 2015 Martine Lenders + * Copyright (C) 2015 PHYTEC Messtechnik GmbH * * This file is subject to the terms and conditions of the GNU Lesser * General Public License v2.1. See the file LICENSE in the top level @@ -10,6 +11,9 @@ * @{ * * @file + * + * @author Martine Lenders + * @author Johann Fischer (nhc udp encoding) */ #include @@ -21,6 +25,8 @@ #include "net/gnrc/sixlowpan/ctx.h" #include "net/sixlowpan.h" #include "utlist.h" +#include "net/gnrc/nettype.h" +#include "net/gnrc/udp.h" #include "net/gnrc/sixlowpan/iphc.h" @@ -69,6 +75,21 @@ #define IPHC_M_DAC_DAM_M_8 (0x0b) #define IPHC_M_DAC_DAM_M_UC_PREFIX (0x0c) +#define NHC_ID_MASK (0xF8) +#define NHC_UDP_ID (0xF0) +#define NHC_UDP_PP_MASK (0x03) +#define NHC_UDP_SD_INLINE (0x00) +#define NHC_UDP_S_INLINE (0x01) +#define NHC_UDP_D_INLINE (0x02) +#define NHC_UDP_SD_ELIDED (0x03) +#define NHC_UDP_C_MASK (0xF4) +#define NHC_UDP_C_ELIDED (0x03) + +#define NHC_UDP_4BIT_PORT (0xF0B0) +#define NHC_UDP_4BIT_MASK (0xFFF0) +#define NHC_UDP_8BIT_PORT (0xF000) +#define NHC_UDP_8BIT_MASK (0xFF00) + static inline bool _context_overlaps_iid(gnrc_sixlowpan_ctx_t *ctx, ipv6_addr_t *addr, eui64_t *iid) @@ -90,6 +111,79 @@ static inline bool _context_overlaps_iid(gnrc_sixlowpan_ctx_t *ctx, (iid->uint8[(ctx->prefix_len / 8) - 8] & byte_mask[ctx->prefix_len % 8]))); } +#ifdef MODULE_GNRC_UDP +inline static size_t iphc_nhc_udp_decode(gnrc_pktsnip_t *pkt, gnrc_pktsnip_t *ipv6, size_t offset) +{ + uint8_t *payload = pkt->data; + uint8_t udp_nhc = payload[offset++]; + ipv6_hdr_t *ipv6_hdr = ipv6->data; + uint8_t tmp; + + gnrc_pktsnip_t *udp = gnrc_pktbuf_add(NULL, NULL, sizeof(udp_hdr_t), + GNRC_NETTYPE_UDP); + if (udp == NULL) { + DEBUG("6lo: error on IPHC NHC UDP decoding\n"); + return 0; + } + udp_hdr_t *udp_hdr = udp->data; + 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_MASK) == NHC_UDP_C_ELIDED) { + DEBUG("6lo iphc nhc: unsupported elided checksum\n"); + gnrc_pktbuf_release(udp); + return 0; + } + else { + udp_hdr->checksum.u8[0] = payload[offset++]; + udp_hdr->checksum.u8[1] = payload[offset++]; + } + + udp_hdr->length = byteorder_htons(pkt->size - offset + sizeof(udp_hdr_t)); + ipv6_hdr->nh = PROTNUM_UDP; + ipv6_hdr->len = udp_hdr->length; + + ipv6->next = udp; + + return offset; +} +#endif + size_t gnrc_sixlowpan_iphc_decode(gnrc_pktsnip_t *ipv6, gnrc_pktsnip_t *pkt, size_t datagram_size, size_t offset) { @@ -364,8 +458,6 @@ size_t gnrc_sixlowpan_iphc_decode(gnrc_pktsnip_t *ipv6, gnrc_pktsnip_t *pkt, siz } - /* TODO: add next header decoding */ - /* set IPv6 header payload length field to the length of whatever is left * after removing the 6LoWPAN header */ if (datagram_size == 0) { @@ -375,17 +467,86 @@ size_t gnrc_sixlowpan_iphc_decode(gnrc_pktsnip_t *ipv6, gnrc_pktsnip_t *pkt, siz ipv6_hdr->len = byteorder_htons((uint16_t)(datagram_size - sizeof(ipv6_hdr_t))); } +#ifdef MODULE_GNRC_SIXLOWPAN_IPHC_NHC + if (iphc_hdr[IPHC1_IDX] & SIXLOWPAN_IPHC1_NH) { + switch (iphc_hdr[payload_offset] & NHC_ID_MASK) { +#ifdef MODULE_GNRC_UDP + case NHC_UDP_ID: + payload_offset = iphc_nhc_udp_decode(pkt, ipv6, payload_offset); + break; +#endif + + default: + break; + } + } +#endif return payload_offset; } +#ifdef MODULE_GNRC_UDP +inline static size_t iphc_nhc_udp_encode(gnrc_pktsnip_t *udp, ipv6_hdr_t *ipv6_hdr) +{ + udp_hdr_t *udp_hdr = udp->data; + network_uint16_t *src_port = &(udp_hdr->src_port); + network_uint16_t *dst_port = &(udp_hdr->dst_port); + uint8_t *udp_data = udp->data; + size_t nhc_len = 0; + + /* TODO: Add support for elided checksum. */ + + /* Compressing UDP ports, follow the same sequence as the linux kernel (nhc_udp module). */ + if (((byteorder_ntohs(*src_port) & NHC_UDP_4BIT_MASK) == NHC_UDP_4BIT_PORT) && + ((byteorder_ntohs(*dst_port) & NHC_UDP_4BIT_MASK) == NHC_UDP_4BIT_PORT)) { + DEBUG("6lo iphc nhc: elide src and dst\n"); + ipv6_hdr->nh = NHC_UDP_SD_ELIDED; + udp_data[nhc_len++] = byteorder_ntohs(*dst_port) - NHC_UDP_4BIT_PORT + + ((byteorder_ntohs(*src_port) - NHC_UDP_4BIT_PORT) << 4); + udp_data[nhc_len++] = udp_hdr->checksum.u8[0]; + udp_data[nhc_len++] = udp_hdr->checksum.u8[1]; + } + else if ((byteorder_ntohs(*dst_port) & NHC_UDP_8BIT_MASK) == NHC_UDP_8BIT_PORT) { + DEBUG("6lo iphc nhc: elide dst\n"); + ipv6_hdr->nh = NHC_UDP_S_INLINE; + nhc_len += 2; /* keep src_port */ + udp_data[nhc_len++] = byteorder_ntohs(*dst_port) - NHC_UDP_8BIT_PORT; + udp_data[nhc_len++] = udp_hdr->checksum.u8[0]; + udp_data[nhc_len++] = udp_hdr->checksum.u8[1]; + } + else if ((byteorder_ntohs(*src_port) & NHC_UDP_8BIT_MASK) == NHC_UDP_8BIT_PORT) { + DEBUG("6lo iphc nhc: elide src\n"); + ipv6_hdr->nh = NHC_UDP_D_INLINE; + udp_data[nhc_len++] = byteorder_ntohs(*src_port) - NHC_UDP_8BIT_PORT; + udp_data[nhc_len++] = udp_hdr->dst_port.u8[0]; + udp_data[nhc_len++] = udp_hdr->dst_port.u8[1]; + udp_data[nhc_len++] = udp_hdr->checksum.u8[0]; + udp_data[nhc_len++] = udp_hdr->checksum.u8[1]; + } + else { + DEBUG("6lo iphc nhc: src and dst inline\n"); + ipv6_hdr->nh = NHC_UDP_SD_INLINE; + nhc_len = sizeof(udp_hdr_t) - 4; /* skip src + dst and elide length */ + udp_data[nhc_len++] = udp_hdr->checksum.u8[0]; + udp_data[nhc_len++] = udp_hdr->checksum.u8[1]; + } + + /* Set UDP header ID (rfc6282#section-5). */ + ipv6_hdr->nh |= NHC_UDP_ID; + /* shrink udp allocation to final size */ + gnrc_pktbuf_realloc_data(udp, nhc_len); + + return nhc_len; +} +#endif + bool gnrc_sixlowpan_iphc_encode(gnrc_pktsnip_t *pkt) { gnrc_netif_hdr_t *netif_hdr = pkt->data; ipv6_hdr_t *ipv6_hdr = pkt->next->data; uint8_t *iphc_hdr; uint16_t inline_pos = SIXLOWPAN_IPHC_HDR_LEN; - bool addr_comp = false; + bool addr_comp = false, nhc_comp = false; gnrc_sixlowpan_ctx_t *src_ctx = NULL, *dst_ctx = NULL; gnrc_pktsnip_t *dispatch = gnrc_pktbuf_add(NULL, NULL, pkt->next->size, GNRC_NETTYPE_SIXLOWPAN); @@ -459,7 +620,14 @@ bool gnrc_sixlowpan_iphc_encode(gnrc_pktsnip_t *pkt) /* compress next header */ switch (ipv6_hdr->nh) { - /* TODO: add next header compression and set NH bit */ +#if defined(MODULE_GNRC_SIXLOWPAN_IPHC_NHC) && defined(MODULE_GNRC_UDP) + case PROTNUM_UDP: + iphc_nhc_udp_encode(pkt->next->next, ipv6_hdr); + iphc_hdr[IPHC1_IDX] |= SIXLOWPAN_IPHC1_NH; + nhc_comp = true; + break; +#endif + default: iphc_hdr[inline_pos++] = ipv6_hdr->nh; break; @@ -660,6 +828,10 @@ bool gnrc_sixlowpan_iphc_encode(gnrc_pktsnip_t *pkt) inline_pos += 16; } + if (nhc_comp) { + iphc_hdr[inline_pos++] = ipv6_hdr->nh; + } + /* shrink dispatch allocation to final size */ /* NOTE: Since this only shrinks the data nothing bad SHOULD happen ;-) */ gnrc_pktbuf_realloc_data(dispatch, (size_t)inline_pos); diff --git a/sys/net/gnrc/transport_layer/udp/gnrc_udp.c b/sys/net/gnrc/transport_layer/udp/gnrc_udp.c index f6d33bf154..6c4a02de76 100644 --- a/sys/net/gnrc/transport_layer/udp/gnrc_udp.c +++ b/sys/net/gnrc/transport_layer/udp/gnrc_udp.c @@ -68,7 +68,7 @@ static uint16_t _calc_csum(gnrc_pktsnip_t *hdr, gnrc_pktsnip_t *pseudo_hdr, uint16_t len = (uint16_t)hdr->size; /* process the payload */ - while (payload && payload != hdr) { + while (payload && payload != hdr && payload != pseudo_hdr) { csum = inet_csum_slice(csum, (uint8_t *)(payload->data), payload->size, len); len += (uint16_t)payload->size; payload = payload->next; @@ -104,21 +104,29 @@ static void _receive(gnrc_pktsnip_t *pkt) return; } pkt = udp; - udp = gnrc_pktbuf_mark(pkt, sizeof(udp_hdr_t), GNRC_NETTYPE_UDP); - if (udp == NULL) { - DEBUG("udp: error marking UDP header, dropping packet\n"); - gnrc_pktbuf_release(pkt); - return; + + LL_SEARCH_SCALAR(pkt, ipv6, type, GNRC_NETTYPE_IPV6); + + assert(ipv6 != NULL); + + if ((ipv6->next != NULL) && (ipv6->next->type == GNRC_NETTYPE_UDP) && + (ipv6->next->size == sizeof(udp_hdr_t))) { + /* UDP header was already marked. Take it. */ + udp = ipv6->next; + } + else { + udp = gnrc_pktbuf_mark(pkt, sizeof(udp_hdr_t), GNRC_NETTYPE_UDP); + if (udp == NULL) { + DEBUG("udp: error marking UDP header, dropping packet\n"); + gnrc_pktbuf_release(pkt); + return; + } } /* mark payload as Type: UNDEF */ pkt->type = GNRC_NETTYPE_UNDEF; /* get explicit pointer to UDP header */ hdr = (udp_hdr_t *)udp->data; - LL_SEARCH_SCALAR(pkt, ipv6, type, GNRC_NETTYPE_IPV6); - - assert(ipv6 != NULL); - /* validate checksum */ if (_calc_csum(udp, ipv6, pkt)) { DEBUG("udp: received packet with invalid checksum, dropping it\n");