Merge pull request #4189 from jfischer-phytec-iot/wip@nhc-udp

gnrc_sixlowpan_iphc.c: add nhc udp encoding and decoding

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)))

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

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

sys/net/gnrc/network_layer/sixlowpan/iphc/gnrc_sixlowpan_iphc.c

 /*
  * 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
@@ -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>
@@ -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);

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");