Merge pull request #3613 from authmillenon/ng_sixlowpan/fix/issue-3588

ng_sixlowpan: fix #3588

sys/include/net/ng_sixlowpan/frag.h

@@ -105,16 +105,12 @@ static inline bool ng_sixlowpan_frag_is(ng_sixlowpan_frag_t *hdr)
  *
  * @param[in] pid           The interface to send the packet over.
  * @param[in] pkt           The packet to send.
- * @param[in] payload_len   The length of the payload to send (IPv6 packet size
- *                          + inner 6LoWPAN dispatches).
- *                          This value is purely given to not calculate the
- *                          payload length using @ref ng_pkt_len() repeatedly.
  * @param[in] datagram_size The length of just the IPv6 packet. It is the value
- *                          set that the ng_sixlowpan_frag_t::disp_size will be
+ *                          that the ng_sixlowpan_frag_t::disp_size field will be
  *                          set to.
  */
 void ng_sixlowpan_frag_send(kernel_pid_t pid, ng_pktsnip_t *pkt,
-                            size_t payload_len, size_t datagram_size);
+                            size_t datagram_size);
 
 /**
  * @brief   Handles a packet containing a fragment header.

sys/include/net/ng_sixlowpan/iphc.h

@@ -144,13 +144,18 @@ static inline bool ng_sixlowpan_iphc_is(uint8_t *data)
 /**
  * @brief   Decompresses a received 6LoWPAN IPHC frame.
  *
- * @param[in,out] pkt   A received 6LoWPAN IPHC frame. Will be translated
+ * @pre (ipv6 != NULL) && (ipv6->size >= sizeof(ng_ipv6_hdr_t))
- *                      into an IPv6 packet.
  *
- * @return  true, on success
+ * @param[out] ipv6     A pre-allocated IPv6 header. Will not be inserted into
- * @return  false, on error.
+ *                      @p pkt
+ * @param[in,out] pkt   A received 6LoWPAN IPHC frame. IPHC dispatch will not
+ *                      be marked.
+ * @param[in] size      Offset of the IPHC dispatch in 6LoWPaN frame.
+ *
+ * @return  length of the HC dispatches + inline values on success.
+ * @return  0 on error.
  */
-bool ng_sixlowpan_iphc_decode(ng_pktsnip_t *pkt);
+size_t ng_sixlowpan_iphc_decode(ng_pktsnip_t *ipv6, ng_pktsnip_t *pkt, size_t offset);
 
 /**
  * @brief   Compresses a 6LoWPAN for IPHC.

sys/net/network_layer/ng_sixlowpan/frag/ng_sixlowpan_frag.c

@@ -80,18 +80,19 @@ static uint16_t _send_1st_fragment(ng_sixlowpan_netif_t *iface, ng_pktsnip_t *pk
                                    size_t payload_len, size_t datagram_size)
 {
     ng_pktsnip_t *frag;
-    uint16_t max_frag_size = _floor8(iface->max_frag_size -
-                                     (payload_len - datagram_size) -
-                                     sizeof(ng_sixlowpan_frag_t));
     uint16_t local_offset = 0;
+    /* payload_len: actual size of the packet vs
+     * datagram_size: size of the uncompressed IPv6 packet */
+    int payload_diff = (datagram_size - payload_len);
+    /* virtually add payload_diff to flooring to account for offset (must be divisable by 8)
+     * in uncompressed datagram */
+    uint16_t max_frag_size = _floor8(iface->max_frag_size + payload_diff -
+                                     sizeof(ng_sixlowpan_frag_t)) - payload_diff;
     ng_sixlowpan_frag_t *hdr;
     uint8_t *data;
 
     DEBUG("6lo frag: determined max_frag_size = %" PRIu16 "\n", max_frag_size);
 
-    /* 6LoWPAN dispatches don't count into that */
-    max_frag_size += (payload_len - datagram_size);
-
     frag = _build_frag_pkt(pkt, payload_len,
                            max_frag_size + sizeof(ng_sixlowpan_frag_t));
 
@@ -134,6 +135,8 @@ static uint16_t _send_nth_fragment(ng_sixlowpan_netif_t *iface, ng_pktsnip_t *pk
                                    uint16_t offset)
 {
     ng_pktsnip_t *frag;
+    /* since dispatches aren't supposed to go into subsequent fragments, we need not account
+     * for payload difference as for the first fragment */
     uint16_t max_frag_size = _floor8(iface->max_frag_size - sizeof(ng_sixlowpan_frag_n_t));
     uint16_t local_offset = 0, offset_count = 0;
     ng_sixlowpan_frag_n_t *hdr;
@@ -156,7 +159,8 @@ static uint16_t _send_nth_fragment(ng_sixlowpan_netif_t *iface, ng_pktsnip_t *pk
     hdr->disp_size = byteorder_htons((uint16_t)datagram_size);
     hdr->disp_size.u8[0] |= NG_SIXLOWPAN_FRAG_N_DISP;
     hdr->tag = byteorder_htons(_tag);
-    hdr->offset = (uint8_t)(offset >> 3);
+    /* don't mention payload diff in offset */
+    hdr->offset = (uint8_t)((offset + (datagram_size - payload_len)) >> 3);
     pkt = pkt->next;    /* don't copy netif header */
 
     while ((pkt != NULL) && (offset_count != offset)) {   /* go to offset */
@@ -202,10 +206,13 @@ static uint16_t _send_nth_fragment(ng_sixlowpan_netif_t *iface, ng_pktsnip_t *pk
 }
 
 void ng_sixlowpan_frag_send(kernel_pid_t pid, ng_pktsnip_t *pkt,
-                            size_t payload_len, size_t datagram_size)
+                            size_t datagram_size)
 {
     ng_sixlowpan_netif_t *iface = ng_sixlowpan_netif_get(pid);
     uint16_t offset = 0, res;
+    /* payload_len: actual size of the packet vs
+     * datagram_size: size of the uncompressed IPv6 packet */
+    size_t payload_len = ng_pkt_len(pkt->next);
 
 #if defined(DEVELHELP) && defined(ENABLE_DEBUG)
     if (iface == NULL) {
@@ -225,7 +232,8 @@ void ng_sixlowpan_frag_send(kernel_pid_t pid, ng_pktsnip_t *pkt,
     offset += res;
     thread_yield();
 
-    while (offset < datagram_size) {
+    /* (offset + (datagram_size - payload_len) < datagram_size) simplified */
+    while (offset < payload_len) {
         if ((res = _send_nth_fragment(iface, pkt, payload_len, datagram_size,
                                       offset)) == 0) {
             /* error sending subsequent fragment */
@@ -268,7 +276,7 @@ void ng_sixlowpan_frag_handle_pkt(ng_pktsnip_t *pkt)
             return;
     }
 
-    rbuf_add(hdr, frag, frag_size, offset);
+    rbuf_add(hdr, pkt, frag_size, offset);
 
     ng_pktbuf_release(pkt);
 }

sys/net/network_layer/ng_sixlowpan/frag/rbuf.c

@@ -17,6 +17,7 @@
 
 #include "rbuf.h"
 #include "net/ng_netbase.h"
+#include "net/ng_ipv6/hdr.h"
 #include "net/ng_ipv6/netif.h"
 #include "net/ng_sixlowpan.h"
 #include "net/ng_sixlowpan/frag.h"
@@ -58,12 +59,16 @@ static rbuf_t *_rbuf_get(const void *src, size_t src_len,
                          const void *dst, size_t dst_len,
                          size_t size, uint16_t tag);
 
-void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_sixlowpan_frag_t *frag,
+void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_pktsnip_t *pkt,
               size_t frag_size, size_t offset)
 {
     rbuf_t *entry;
+    /* cppcheck is clearly wrong here */
+    /* cppcheck-suppress variableScope */
+    unsigned int data_offset = 0;
+    ng_sixlowpan_frag_t *frag = pkt->data;
     rbuf_int_t *ptr;
-    uint8_t *data = ((uint8_t *)frag) + sizeof(ng_sixlowpan_frag_t);
+    uint8_t *data = ((uint8_t *)pkt->data) + sizeof(ng_sixlowpan_frag_t);
 
     _rbuf_gc();
     entry = _rbuf_get(ng_netif_hdr_get_src_addr(netif_hdr), netif_hdr->src_l2addr_len,
@@ -80,18 +85,27 @@ void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_sixlowpan_frag_t *frag,
 
     /* dispatches in the first fragment are ignored */
     if (offset == 0) {
-        switch (data[0]) {
+        if (data[0] == NG_SIXLOWPAN_UNCOMPRESSED) {
-            case NG_SIXLOWPAN_UNCOMPRESSED:
+            data++;             /* skip 6LoWPAN dispatch */
-                data++;             /* skip 6LoWPAN dispatch */
+            frag_size--;
-                frag_size--;
-                entry->compressed = 0;  /* datagram is not compressed */
-
-                break;
-
-            default:
-                break;
         }
-
+#ifdef MODULE_NG_SIXLOWPAN_IPHC
+        else if (ng_sixlowpan_iphc_is(data)) {
+            size_t iphc_len;
+            iphc_len = ng_sixlowpan_iphc_decode(entry->pkt, pkt,
+                                                sizeof(ng_sixlowpan_frag_t));
+            if (iphc_len == 0) {
+                DEBUG("6lo rfrag: could not decode IPHC dispatch\n");
+                ng_pktbuf_release(entry->pkt);
+                _rbuf_rem(entry);
+                return;
+            }
+            data += iphc_len;       /* take remaining data as data */
+            frag_size -= iphc_len;  /* and reduce frag size by IPHC dispatch length */
+            frag_size += sizeof(ipv6_hdr_t);    /* but add IPv6 header length */
+            data_offset += sizeof(ipv6_hdr_t);  /* start copying after IPv6 header */
+        }
+#endif
     }
     else {
         data++; /* FRAGN header is one byte longer (offset) */
@@ -118,7 +132,8 @@ void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_sixlowpan_frag_t *frag,
     if (_rbuf_update_ints(entry, offset, frag_size)) {
         DEBUG("6lo rbuf: add fragment data\n");
         entry->cur_size += (uint16_t)frag_size;
-        memcpy(((uint8_t *)entry->pkt->data) + offset, data, frag_size);
+        memcpy(((uint8_t *)entry->pkt->data) + offset + data_offset, data,
+               frag_size - data_offset);
     }
 
     if (entry->cur_size == entry->pkt->size) {
@@ -137,18 +152,12 @@ void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_sixlowpan_frag_t *frag,
         netif_hdr->if_pid = iface;
         LL_APPEND(entry->pkt, netif);
 
-        if (entry->compressed) {
+        if (!ng_netapi_dispatch_receive(NG_NETTYPE_IPV6, NG_NETREG_DEMUX_CTX_ALL,
-            DEBUG("6lo rbuf: datagram complete, send to self for decompression\n");
+                                        entry->pkt)) {
-            ng_netapi_receive(thread_getpid(), entry->pkt);
+            DEBUG("6lo rbuf: No receivers for this packet found\n");
-        }
+            ng_pktbuf_release(entry->pkt);
-        else {
-            DEBUG("6lo rbuf: datagram complete, send to IPv6 listeners\n");
-            if (!ng_netapi_dispatch_receive(NG_NETTYPE_IPV6, NG_NETREG_DEMUX_CTX_ALL,
-                                            entry->pkt)) {
-                DEBUG("6lo rbuf: No receivers for this packet found\n");
-                ng_pktbuf_release(entry->pkt);
-            }
         }
+
         _rbuf_rem(entry);
     }
 }
@@ -204,7 +213,7 @@ static bool _rbuf_update_ints(rbuf_t *entry, uint16_t offset, size_t frag_size)
           new->start, new->end, ng_netif_addr_to_str(l2addr_str,
                   sizeof(l2addr_str), entry->src, entry->src_len));
     DEBUG("%s, %u, %u)\n", ng_netif_addr_to_str(l2addr_str,
-          sizeof(l2addr_str), entry->dst, entry->dst_len),
+            sizeof(l2addr_str), entry->dst, entry->dst_len),
           (unsigned)entry->pkt->size, entry->tag);
 
     LL_PREPEND(entry->ints, new);
@@ -227,7 +236,7 @@ static void _rbuf_gc(void)
         else if ((rbuf[i].pkt != NULL) &&
                  ((now.seconds - rbuf[i].arrival) > RBUF_TIMEOUT)) {
             DEBUG("6lo rfrag: entry (%s, ", ng_netif_addr_to_str(l2addr_str,
-                  sizeof(l2addr_str), rbuf[i].src, rbuf[i].src_len));
+                    sizeof(l2addr_str), rbuf[i].src, rbuf[i].src_len));
             DEBUG("%s, %u, %u) timed out\n",
                   ng_netif_addr_to_str(l2addr_str, sizeof(l2addr_str), rbuf[i].dst,
                                        rbuf[i].dst_len),
@@ -297,7 +306,6 @@ static rbuf_t *_rbuf_get(const void *src, size_t src_len,
         res->dst_len = dst_len;
         res->tag = tag;
         res->cur_size = 0;
-        res->compressed = 1;
 
         DEBUG("6lo rfrag: entry %p (%s, ", (void *)res,
               ng_netif_addr_to_str(l2addr_str, sizeof(l2addr_str), res->src,
@@ -305,7 +313,7 @@ static rbuf_t *_rbuf_get(const void *src, size_t src_len,
         DEBUG("%s, %u, %u) created\n",
               ng_netif_addr_to_str(l2addr_str, sizeof(l2addr_str), res->dst,
                                    res->dst_len), (unsigned)res->pkt->size,
-                                   res->tag);
+              res->tag);
     }
 
     return res;

sys/net/network_layer/ng_sixlowpan/frag/rbuf.h

@@ -78,7 +78,6 @@ typedef struct {
     uint8_t dst_len;                    /**< length of destination address */
     uint16_t tag;                       /**< the datagram's tag */
     uint16_t cur_size;                  /**< the datagram's current size */
-    uint16_t compressed;                /**< the datagram has a compressed header */
 } rbuf_t;
 
 /**
@@ -91,7 +90,7 @@ typedef struct {
  * @param[in] frag_size     The fragment's size.
  * @param[in] offset        The fragment's offset.
  */
-void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_sixlowpan_frag_t *frag,
+void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_pktsnip_t *frag,
               size_t frag_size, size_t offset);
 
 #ifdef __cplusplus

sys/net/network_layer/ng_sixlowpan/iphc/ng_sixlowpan_iphc.c

@@ -89,23 +89,19 @@ static inline bool _context_overlaps_iid(ng_sixlowpan_ctx_t *ctx,
              (iid->uint8[(ctx->prefix_len / 8) - 8] & byte_mask[ctx->prefix_len % 8])));
 }
 
-bool ng_sixlowpan_iphc_decode(ng_pktsnip_t *pkt)
+size_t ng_sixlowpan_iphc_decode(ng_pktsnip_t *ipv6, ng_pktsnip_t *pkt, size_t offset)
 {
     ng_netif_hdr_t *netif_hdr = pkt->next->data;
     ipv6_hdr_t *ipv6_hdr;
     uint8_t *iphc_hdr = pkt->data;
-    uint16_t payload_offset = NG_SIXLOWPAN_IPHC_HDR_LEN;
+    size_t payload_offset = NG_SIXLOWPAN_IPHC_HDR_LEN;
     ng_sixlowpan_ctx_t *ctx = NULL;
-    ng_pktsnip_t *payload;
-    ng_pktsnip_t *ipv6 = ng_pktbuf_add(NULL, NULL, sizeof(ipv6_hdr_t),
-                                       NG_NETTYPE_IPV6);
 
-    if (ipv6 == NULL) {
+    assert(ipv6 != NULL);
-        DEBUG("6lo iphc: error allocating ipv6 header space\n");
+    assert(ipv6->size >= sizeof(ipv6_hdr_t));
-        return false;
-    }
 
     ipv6_hdr = ipv6->data;
+    iphc_hdr += offset;
 
     if (iphc_hdr[IPHC2_IDX] & NG_SIXLOWPAN_IPHC2_CID_EXT) {
         payload_offset++;
@@ -174,7 +170,7 @@ bool ng_sixlowpan_iphc_decode(ng_pktsnip_t *pkt)
 
             if (ctx == NULL) {
                 DEBUG("6lo iphc: could not find source context\n");
-                return false;
+                return 0;
             }
         }
     }
@@ -248,7 +244,7 @@ bool ng_sixlowpan_iphc_decode(ng_pktsnip_t *pkt)
 
             if (ctx == NULL) {
                 DEBUG("6lo iphc: could not find destination context\n");
-                return false;
+                return 0;
             }
         }
     }
@@ -362,25 +358,18 @@ bool ng_sixlowpan_iphc_decode(ng_pktsnip_t *pkt)
 
         default:
             DEBUG("6lo iphc: unspecified or reserved M, DAC, DAM combination\n");
-            return false;
+            return 0;
 
     }
 
     /* TODO: add next header decoding */
 
-    /* remove 6LoWPAN dispatch */
-    payload = ng_pktbuf_mark(pkt, payload_offset, NG_NETTYPE_SIXLOWPAN);
-    pkt = ng_pktbuf_remove_snip(pkt, payload);
-
     /* set IPv6 header payload length field to the length of whatever is left
      * after removing the 6LoWPAN header */
-    ipv6_hdr->len = byteorder_htons(pkt->size);
+    ipv6_hdr->len = byteorder_htons((uint16_t)(pkt->size - payload_offset));
 
-    /* insert IPv6 header */
-    ipv6->next = pkt->next;
-    pkt->next = ipv6;
 
-    return true;
+    return payload_offset;
 }
 
 bool ng_sixlowpan_iphc_encode(ng_pktsnip_t *pkt)

sys/net/network_layer/ng_sixlowpan/ng_sixlowpan.c

@@ -17,6 +17,7 @@
 #include "thread.h"
 #include "utlist.h"
 
+#include "net/ng_ipv6/hdr.h"
 #include "net/ng_sixlowpan.h"
 #include "net/ng_sixlowpan/frag.h"
 #include "net/ng_sixlowpan/iphc.h"
@@ -122,12 +123,32 @@ static void _receive(ng_pktsnip_t *pkt)
 #endif
 #ifdef MODULE_NG_SIXLOWPAN_IPHC
     else if (ng_sixlowpan_iphc_is(dispatch)) {
-        if (!ng_sixlowpan_iphc_decode(pkt)) {
+        size_t dispatch_size;
+        ng_pktsnip_t *sixlowpan;
+        ng_pktsnip_t *ipv6 = ng_pktbuf_add(NULL, NULL, sizeof(ipv6_hdr_t),
+                                           NG_NETTYPE_IPV6);
+        if ((ipv6 == NULL) ||
+            (dispatch_size = ng_sixlowpan_iphc_decode(ipv6, pkt, 0)) == 0) {
             DEBUG("6lo: error on IPHC decoding\n");
+            if (ipv6 != NULL) {
+                ng_pktbuf_release(ipv6);
+            }
             ng_pktbuf_release(pkt);
             return;
         }
-        LL_SEARCH_SCALAR(pkt, payload, type, NG_NETTYPE_IPV6);
+        sixlowpan = ng_pktbuf_mark(pkt, dispatch_size, NG_NETTYPE_SIXLOWPAN);
+        if (sixlowpan == NULL) {
+            DEBUG("6lo: error on marking IPHC dispatch\n");
+            ng_pktbuf_release(ipv6);
+            ng_pktbuf_release(pkt);
+            return;
+        }
+
+        /* Remove IPHC dispatch */
+        ng_pktbuf_remove_snip(pkt, sixlowpan);
+        /* Insert IPv6 header instead */
+        ipv6->next = pkt->next;
+        pkt->next = ipv6;
     }
 #endif
     else {
@@ -169,7 +190,8 @@ static void _send(ng_pktsnip_t *pkt)
     ng_netif_hdr_t *hdr;
     ng_pktsnip_t *pkt2;
     ng_sixlowpan_netif_t *iface;
-    size_t datagram_size, dispatch_len = 0;
+    /* datagram_size: pure IPv6 packet without 6LoWPAN dispatches or compression */
+    size_t datagram_size;
 
     if ((pkt == NULL) || (pkt->size < sizeof(ng_netif_hdr_t))) {
         DEBUG("6lo: Sending packet has no netif header\n");
@@ -193,6 +215,7 @@ static void _send(ng_pktsnip_t *pkt)
 
     hdr = pkt2->data;
     iface = ng_sixlowpan_netif_get(hdr->if_pid);
+    datagram_size = ng_pkt_len(pkt2->next);
 
     if (iface == NULL) {
         DEBUG("6lo: Can not get 6LoWPAN specific interface information.\n");
@@ -217,7 +240,6 @@ static void _send(ng_pktsnip_t *pkt)
             ng_pktbuf_release(pkt2);
             return;
         }
-        dispatch_len++;
     }
 #else
     /* suppress clang-analyzer report about iface being not read */
@@ -228,10 +250,7 @@ static void _send(ng_pktsnip_t *pkt)
         ng_pktbuf_release(pkt2);
         return;
     }
-    dispatch_len++;
 #endif
-    datagram_size = ng_pkt_len(pkt2->next);
-
     DEBUG("6lo: iface->max_frag_size = %" PRIu16 " for interface %"
           PRIkernel_pid "\n", iface->max_frag_size, hdr->if_pid);
 
@@ -248,8 +267,7 @@ static void _send(ng_pktsnip_t *pkt)
     else {
         DEBUG("6lo: Send fragmented (%u > %" PRIu16 ")\n",
               (unsigned int)datagram_size, iface->max_frag_size);
-        ng_sixlowpan_frag_send(hdr->if_pid, pkt2, datagram_size,
+        ng_sixlowpan_frag_send(hdr->if_pid, pkt2, datagram_size);
-                               datagram_size - dispatch_len);
     }
 #else
     (void)datagram_size;