ipv6: rpl: add source routing header for RPL

sys/include/net/gnrc/ipv6/ext.h

@@ -46,8 +46,8 @@ extern "C" {
  * @param[in] pkt   A packet.
  * @param[in] nh    A protocol number (see @ref net_protnum).
  *
- * @return  true, on success.
+ * @return  true, on success - continue packet processing.
- * @return  false, on failure.
+ * @return  false, on failure - stop packet processing.
  */
 bool gnrc_ipv6_ext_demux(kernel_pid_t iface, gnrc_pktsnip_t *pkt,
                          uint8_t nh);

sys/include/net/gnrc/rpl/srh.h

@@ -23,6 +23,7 @@
 #ifndef GNRC_RPL_SRH_H_
 #define GNRC_RPL_SRH_H_
 
+#include "net/ipv6/hdr.h"
 #include "net/ipv6/addr.h"
 
 #ifdef __cplusplus
@@ -48,17 +49,22 @@ typedef struct __attribute__((packed)) {
     uint8_t len;        /**< length in 8 octets without first octet */
     uint8_t type;       /**< identifier of a particular routing header type */
     uint8_t seg_left;   /**< number of route segments remaining */
+    uint8_t compr;      /**< number of prefix octets (comprI and comprE) */
+    uint8_t pad_resv;   /**< padding and reserved */
+    uint16_t resv;      /**< reserved */
 } gnrc_rpl_srh_t;
 
 /**
- * @brief   Extract next hop from the RPL source routing header.
+ * @brief   Process the RPL source routing header.
  *
+ * @param[in,out] ipv6  The IPv6 header of the incoming packet.
  * @param[in] rh        A RPL source routing header.
  *
- * @return  next hop, on success
+ * @return  EXT_RH_CODE_ERROR
- * @return  NULL, if not found.
+ * @return  EXT_RH_CODE_FORWARD
+ * @return  EXT_RH_CODE_OK
  */
-ipv6_addr_t *gnrc_rpl_srh_next_hop(gnrc_rpl_srh_t *rh);
+int gnrc_rpl_srh_process(ipv6_hdr_t *ipv6, gnrc_rpl_srh_t *rh);
 
 #ifdef __cplusplus
 }

sys/include/net/ipv6/ext/rh.h

@@ -23,12 +23,24 @@
 #include <stdint.h>
 
 #include "net/ipv6/addr.h"
+#include "net/ipv6/ext.h"
 #include "net/ipv6/hdr.h"
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+/**
+ * @name Return codes for routing header processing
+ * @{
+ */
+#define EXT_RH_CODE_ERROR       (-1)
+#define EXT_RH_CODE_FORWARD     (0)
+#define EXT_RH_CODE_OK          (1)
+/**
+ * @}
+ */
+
 /**
  * @brief   IPv6 routing extension header.
  *
@@ -46,14 +58,16 @@ typedef struct __attribute__((packed)) {
 } ipv6_ext_rh_t;
 
 /**
- * @brief   Extract next hop from the routing header of an IPv6 packet.
+ * @brief   Process the routing header of an IPv6 packet.
  *
- * @param[in] ipv6      An IPv6 packet.
+ * @param[in, out] ipv6     An IPv6 packet.
+ * @param[in] ext           A routing header of @ipv6.
  *
- * @return  next hop on success, on success
+ * @return  EXT_RH_CODE_ERROR
- * @return  NULL, if not found.
+ * @return  EXT_RH_CODE_FORWARD
+ * @return  EXT_RH_CODE_OK
  */
-ipv6_addr_t *ipv6_ext_rh_next_hop(ipv6_hdr_t *ipv6);
+int ipv6_ext_rh_process(ipv6_hdr_t *ipv6, ipv6_ext_rh_t *ext);
 
 #ifdef __cplusplus
 }

sys/net/gnrc/network_layer/ipv6/ext/gnrc_ipv6_ext.c

@@ -20,12 +20,17 @@
 
 #include "net/gnrc/ipv6/ext.h"
 
+#define ENABLE_DEBUG    (0)
+#include "debug.h"
+
 bool gnrc_ipv6_ext_demux(kernel_pid_t iface, gnrc_pktsnip_t *pkt,
                          uint8_t nh)
 {
-    gnrc_pktsnip_t *ext_snip;
+    gnrc_pktsnip_t *ext_snip, *tmp;
     ipv6_ext_t *ext;
     unsigned int offset = 0;
+    ipv6_hdr_t *hdr;
+    int res;
 
     ext = ((ipv6_ext_t *)(((uint8_t *)pkt->data) + sizeof(ipv6_hdr_t)));
 
@@ -36,6 +41,35 @@ bool gnrc_ipv6_ext_demux(kernel_pid_t iface, gnrc_pktsnip_t *pkt,
             case PROTNUM_IPV6_EXT_HOPOPT:
             case PROTNUM_IPV6_EXT_DST:
             case PROTNUM_IPV6_EXT_RH:
+                if ((tmp = gnrc_pktbuf_start_write(pkt)) == NULL) {
+                    DEBUG("ipv6: could not get a copy of pkt\n");
+                    gnrc_pktbuf_release(pkt);
+                    return false;
+                }
+                pkt = tmp;
+                hdr = pkt->data;
+                ext = (ipv6_ext_t *) (((uint8_t *) pkt->data) + sizeof(ipv6_hdr_t) + offset);
+                res = ipv6_ext_rh_process(hdr, (ipv6_ext_rh_t *)ext);
+                if (res == EXT_RH_CODE_ERROR) {
+                    /* TODO: send ICMPv6 error codes */
+                    gnrc_pktbuf_release(pkt);
+                    return false;
+                }
+                else if (res == EXT_RH_CODE_FORWARD) {
+                    /* forward packet */
+                    if (!gnrc_netapi_dispatch_receive(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL,
+                                                      pkt)) {
+                        DEBUG("ipv6: could not dispatch packet to the ipv6 thread\n");
+                        gnrc_pktbuf_release(pkt);
+                    }
+                    return false;
+                }
+                else if (res == EXT_RH_CODE_OK) {
+                    nh = ext->nh;
+                    offset += ((ext->len * IPV6_EXT_LEN_UNIT) + IPV6_EXT_LEN_UNIT);
+                    ext = ipv6_ext_get_next((ipv6_ext_t *)ext);
+                }
+                break;
             case PROTNUM_IPV6_EXT_FRAG:
             case PROTNUM_IPV6_EXT_AH:
             case PROTNUM_IPV6_EXT_ESP:
@@ -57,6 +91,7 @@ bool gnrc_ipv6_ext_demux(kernel_pid_t iface, gnrc_pktsnip_t *pkt,
     ext_snip = gnrc_pktbuf_mark(pkt, offset, GNRC_NETTYPE_IPV6);
 
     if (ext_snip == NULL) {
+        gnrc_pktbuf_release(pkt);
         return false;
     }
 

sys/net/gnrc/network_layer/ipv6/gnrc_ipv6.c

@@ -123,8 +123,7 @@ void gnrc_ipv6_demux(kernel_pid_t iface, gnrc_pktsnip_t *pkt, uint8_t nh)
         case PROTNUM_IPV6_EXT_MOB:
             DEBUG("ipv6: handle extension header (nh = %u)\n", nh);
             if (!gnrc_ipv6_ext_demux(iface, pkt, nh)) {
-                DEBUG("ipv6: unable to parse extension headers.\n");
+                DEBUG("ipv6: stop packet processing.\n");
-                gnrc_pktbuf_release(pkt);
                 return;
             }
 #endif

sys/net/gnrc/network_layer/ndp/node/gnrc_ndp_node.c

@@ -63,14 +63,6 @@ kernel_pid_t gnrc_ndp_node_next_hop_l2addr(uint8_t *l2addr, uint8_t *l2addr_len,
     ipv6_addr_t *next_hop_ip = NULL, *prefix = NULL;
     bool dst_link_local = ipv6_addr_is_link_local(dst);
 
-#ifdef MODULE_GNRC_IPV6_EXT_RH
-    ipv6_hdr_t *hdr;
-    gnrc_pktsnip_t *ipv6;
-    ipv6 = gnrc_pktsnip_search_type(pkt, GNRC_NETTYPE_IPV6);
-    assert(ipv6);
-    hdr = ipv6->data;
-    next_hop_ip = ipv6_ext_rh_next_hop(hdr);
-#endif
 #ifdef MODULE_FIB
     ipv6_addr_t next_hop_actual;    /* FIB copies address into this variable */
     /* don't look-up link local addresses in FIB */

sys/net/gnrc/network_layer/sixlowpan/nd/gnrc_sixlowpan_nd.c

@@ -124,14 +124,6 @@ kernel_pid_t gnrc_sixlowpan_nd_next_hop_l2addr(uint8_t *l2addr, uint8_t *l2addr_
     ipv6_addr_t *next_hop = NULL;
     gnrc_ipv6_nc_t *nc_entry = NULL;
 
-#ifdef MODULE_GNRC_IPV6_EXT_RH
-    ipv6_hdr_t *hdr;
-    gnrc_pktsnip_t *ipv6;
-    ipv6 = gnrc_pktsnip_search_type(pkt, GNRC_NETTYPE_IPV6);
-    assert(ipv6);
-    hdr = ipv6->data;
-    next_hop = ipv6_ext_rh_next_hop(hdr);
-#endif
 #ifdef MODULE_FIB
     kernel_pid_t fib_iface;
     ipv6_addr_t next_hop_actual;    /* FIB copies address into this variable */

sys/net/gnrc/routing/rpl/srh/Makefile

@@ -0,0 +1,3 @@
+MODULE = gnrc_rpl_srh
+
+include $(RIOTBASE)/Makefile.base

sys/net/gnrc/routing/rpl/srh/gnrc_rpl_srh.c

@@ -12,13 +12,86 @@
  * @file
  */
 
+#include <string.h>
+#include "net/gnrc/ipv6/netif.h"
 #include "net/gnrc/rpl/srh.h"
 
-ipv6_addr_t *gnrc_rpl_srh_next_hop(gnrc_rpl_srh_t *rh)
+#define ENABLE_DEBUG    (0)
-{
+#include "debug.h"
-    /* TODO */
 
-    return NULL;
+#if ENABLE_DEBUG
+static char addr_str[IPV6_ADDR_MAX_STR_LEN];
+#endif
+
+#define GNRC_RPL_SRH_PADDING(X)     ((X & 0xF0) >> 4)
+#define GNRC_RPL_SRH_COMPRE(X)      (X & 0x0F)
+#define GNRC_RPL_SRH_COMPRI(X)      ((X & 0xF0) >> 4)
+
+int gnrc_rpl_srh_process(ipv6_hdr_t *ipv6, gnrc_rpl_srh_t *rh)
+{
+    if (rh->seg_left == 0) {
+        return EXT_RH_CODE_OK;
+    }
+
+    uint8_t n = (((rh->len * 8) - GNRC_RPL_SRH_PADDING(rh->pad_resv) -
+                 (16 - GNRC_RPL_SRH_COMPRE(rh->compr))) /
+                 (16 - GNRC_RPL_SRH_COMPRI(rh->compr))) + 1;
+    ipv6_addr_t addr = ipv6->dst, tmp;
+    uint8_t i, pref_elided, tmp_pref_elided, addr_len, compri_addr_len, tmp_addr_len, found_pos = 0;
+    uint8_t *addr_vec = (uint8_t *) (rh + 1);
+    bool found = false;
+
+    DEBUG("RPL SRH: %" PRIu8 " addresses in the routing header\n", n);
+
+    if (rh->seg_left > n) {
+        DEBUG("RPL SRH: number of segments left > number of addresses - discard\n");
+        /* TODO ICMP Parameter Problem - Code 0 */
+        return EXT_RH_CODE_ERROR;
+    }
+
+    rh->seg_left--;
+    i = n - rh->seg_left;
+    pref_elided = rh->seg_left ? GNRC_RPL_SRH_COMPRI(rh->compr) : GNRC_RPL_SRH_COMPRE(rh->compr);
+    compri_addr_len = sizeof(ipv6_addr_t) - GNRC_RPL_SRH_COMPRI(rh->compr);
+    addr_len = sizeof(ipv6_addr_t) - pref_elided;
+    memcpy(&addr.u8[pref_elided], &addr_vec[(i - 1) * compri_addr_len], addr_len);
+
+    if (ipv6_addr_is_multicast(&ipv6->dst) || ipv6_addr_is_multicast(&addr)) {
+        DEBUG("RPL SRH: found a multicast address - discard\n");
+        /* TODO discard the packet */
+        return EXT_RH_CODE_ERROR;
+    }
+
+    /* check if multiple addresses of my interface exist */
+    tmp_pref_elided = GNRC_RPL_SRH_COMPRI(rh->compr);
+    tmp_addr_len = sizeof(ipv6_addr_t) - tmp_pref_elided;
+    tmp = ipv6->dst;
+    for (uint8_t k = 0; k < n; k++) {
+        if (k == n - 1) {
+            tmp_pref_elided = GNRC_RPL_SRH_COMPRE(rh->compr);
+            tmp_addr_len = sizeof(ipv6_addr_t) - tmp_pref_elided;
+            tmp = ipv6->dst;
+        }
+        memcpy(&tmp.u8[tmp_pref_elided], &addr_vec[k * compri_addr_len], tmp_addr_len);
+        if (gnrc_ipv6_netif_find_by_addr(NULL, &tmp) != KERNEL_PID_UNDEF) {
+            if (found && ((k - found_pos) > 1)) {
+                DEBUG("RPL SRH: found multiple addresses that belong to me - discard\n");
+                /* TODO send an ICMP Parameter Problem (Code 0) and discard the packet */
+                return EXT_RH_CODE_ERROR;
+            }
+            found_pos = k;
+            found = true;
+        }
+    }
+
+    memcpy(&addr_vec[(i - 1) * compri_addr_len], &ipv6->dst.u8[pref_elided], addr_len);
+
+    DEBUG("RPL SRH: Next hop: %s at position %d\n",
+          ipv6_addr_to_str(addr_str, &addr, sizeof(addr_str)), i);
+
+    ipv6->dst = addr;
+
+    return EXT_RH_CODE_FORWARD;
 }
 
 /** @} */

sys/net/network_layer/ipv6/ext/Makefile

@@ -0,0 +1,3 @@
+MODULE = ipv6_ext
+
+include $(RIOTBASE)/Makefile.base

sys/net/network_layer/ipv6/ext/rh/ipv6_ext_rh.c

@@ -15,48 +15,24 @@
 #include <stdbool.h>
 
 #include "net/protnum.h"
+#include "net/ipv6/ext.h"
 #include "net/ipv6/ext/rh.h"
 #include "net/gnrc/rpl/srh.h"
 
-ipv6_addr_t *ipv6_ext_rh_next_hop(ipv6_hdr_t *ipv6)
+int ipv6_ext_rh_process(ipv6_hdr_t *hdr, ipv6_ext_rh_t *ext)
 {
-    ipv6_ext_rh_t *ext = (ipv6_ext_rh_t *)(ipv6 + 1);
+    (void) hdr;
-    bool c = true;
 
-    while (c) {
+    switch (ext->type) {
-        switch (ext->type) {
-            case PROTNUM_IPV6_EXT_HOPOPT:
-            case PROTNUM_IPV6_EXT_DST:
-            case PROTNUM_IPV6_EXT_FRAG:
-            case PROTNUM_IPV6_EXT_AH:
-            case PROTNUM_IPV6_EXT_ESP:
-            case PROTNUM_IPV6_EXT_MOB:
-                ext = (ipv6_ext_rh_t *)ipv6_ext_get_next((ipv6_ext_t *)ext);
-                break;
-
-            case PROTNUM_IPV6_EXT_RH:
-                c = false;
-                break;
-
-            default:
-                c = false;
-                break;
-        }
-    }
-
-    if (ipv6->nh == PROTNUM_IPV6_EXT_RH) {
-        switch (ext->type) {
 #ifdef MODULE_GNRC_RPL_SRH
-            case GNRC_RPL_SRH_TYPE:
+        case GNRC_RPL_SRH_TYPE:
-                return gnrc_rpl_srh_next_hop((gnrc_rpl_srh_t *)ext);
+            return gnrc_rpl_srh_process(hdr, (gnrc_rpl_srh_t *)ext);
 #endif
 
-            default:
+        default:
-                break;
+            break;
-        }
     }
-
+    return EXT_RH_CODE_ERROR;
-    return NULL;
 }
 
 /** @} */