Skip to content

Commit

Permalink
Merge pull request #4189 from jfischer-phytec-iot/wip@nhc-udp
Browse files Browse the repository at this point in the history 
gnrc_sixlowpan_iphc.c: add nhc udp encoding and decoding
  • Loading branch information
@OlegHahm
OlegHahm committed Dec 8, 2015
2 parents e8ed9fb + 994dd19 commit 693d438
Show file tree
Hide file tree
Showing 5 changed files with 202 additions and 15 deletions.
1 change: 1 addition & 0 deletions Makefile.dep
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)))
Expand Down
1 change: 1 addition & 0 deletions Makefile.pseudomodules
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 6 additions & 1 deletion sys/net/gnrc/network_layer/sixlowpan/gnrc_sixlowpan.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
180 changes: 176 additions & 4 deletions sys/net/gnrc/network_layer/sixlowpan/iphc/gnrc_sixlowpan_iphc.c
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2015 Martine Lenders <mlenders@inf.fu-berlin.de>
* 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
Expand All @@ -10,6 +11,9 @@
* @{
*
* @file
*
* @author Martine Lenders <mlenders@inf.fu-berlin.de>
* @author Johann Fischer <j.fischer@phytec.de> (nhc udp encoding)
*/

#include <stdbool.h>
Expand All @@ -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"

Expand Down Expand Up @@ -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)
Expand All @@ -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)
{
Expand Down Expand Up @@ -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) {
Expand All @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
28 changes: 18 additions & 10 deletions sys/net/gnrc/transport_layer/udp/gnrc_udp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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");
Expand Down

0 comments on commit 693d438

Please sign in to comment.