diff --git a/os/net/ipv6/multicast/esmrf.c b/os/net/ipv6/multicast/esmrf.c
index d72d99fad..746f2f03b 100644
--- a/os/net/ipv6/multicast/esmrf.c
+++ b/os/net/ipv6/multicast/esmrf.c
@@ -108,7 +108,6 @@ static uip_ipaddr_t des_ip;
 static void icmp_input(void);
 static void icmp_output(void);
 static void mcast_fwd(void *p);
-int remove_ext_hdr(void);
 /*---------------------------------------------------------------------------*/
 /* Internal Data Structures */
 /*---------------------------------------------------------------------------*/
@@ -192,7 +191,7 @@ icmp_input()
   }
 #endif
 
-  remove_ext_hdr();
+  uip_remove_ext_hdr();
 
   PRINTF("ESMRF: ICMPv6 In from ");
   PRINT6ADDR(&UIP_IP_BUF->srcipaddr);
diff --git a/os/net/ipv6/multicast/roll-tm.c b/os/net/ipv6/multicast/roll-tm.c
index e9214f8d4..c942dfd46 100644
--- a/os/net/ipv6/multicast/roll-tm.c
+++ b/os/net/ipv6/multicast/roll-tm.c
@@ -1349,8 +1349,7 @@ out()
   HBH_SET_M(lochbhmptr);
 #endif
 
-  uip_ext_len += HBHO_TOTAL_LEN;
-  uip_len += HBHO_TOTAL_LEN;
+  uipbuf_add_ext_hdr(HBHO_TOTAL_LEN);
 
   /* Update the proto and length field in the v6 header */
   UIP_IP_BUF->proto = UIP_PROTO_HBHO;
diff --git a/os/net/ipv6/tcpip.c b/os/net/ipv6/tcpip.c
index b7a10de9a..d78cae510 100644
--- a/os/net/ipv6/tcpip.c
+++ b/os/net/ipv6/tcpip.c
@@ -451,15 +451,13 @@ tcpip_input(void)
   uip_clear_buf();
 }
 /*---------------------------------------------------------------------------*/
-extern void remove_ext_hdr(void);
-/*---------------------------------------------------------------------------*/
 static void
 output_fallback(void)
 {
 #ifdef UIP_FALLBACK_INTERFACE
   LOG_INFO("fallback: removing ext hdrs & setting proto %d %d\n",
          uip_ext_len, *((uint8_t *)UIP_IP_BUF + 40));
-  remove_ext_hdr();
+  uip_remove_ext_hdr();
   /* Inform the other end that the destination is not reachable. If it's
    * not informed routes might get lost unexpectedly until there's a need
    * to send a new packet to the peer */
diff --git a/os/net/ipv6/uip-icmp6.c b/os/net/ipv6/uip-icmp6.c
index ca543d68b..c4003352f 100644
--- a/os/net/ipv6/uip-icmp6.c
+++ b/os/net/ipv6/uip-icmp6.c
@@ -130,22 +130,7 @@ echo_request_input(void)
     uip_ipaddr_copy(&UIP_IP_BUF->destipaddr, &tmp_ipaddr);
   }
 
-  if(uip_ext_len > 0) {
-    /* Remove extension headers if any */
-    UIP_IP_BUF->proto = UIP_PROTO_ICMP6;
-    uip_len -= uip_ext_len;
-    UIP_IP_BUF->len[0] = ((uip_len - UIP_IPH_LEN) >> 8);
-    UIP_IP_BUF->len[1] = ((uip_len - UIP_IPH_LEN) & 0xff);
-    /* move the echo request payload (starting after the icmp header)
-     * to the new location in the reply.
-     * The shift is equal to the length of the extension headers present
-     * Note: UIP_ICMP_BUF still points to the echo request at this stage
-     */
-    memmove((uint8_t *)UIP_ICMP_BUF + UIP_ICMPH_LEN - uip_ext_len,
-        (uint8_t *)UIP_ICMP_BUF + UIP_ICMPH_LEN,
-        (uip_len - UIP_IPH_LEN - UIP_ICMPH_LEN));
-    uip_ext_len = 0;
-  }
+  uip_remove_ext_hdr();
 
   /* Below is important for the correctness of UIP_ICMP_BUF and the
    * checksum
@@ -169,41 +154,32 @@ echo_request_input(void)
 void
 uip_icmp6_error_output(uint8_t type, uint8_t code, uint32_t param) {
   /* check if originating packet is not an ICMP error */
-  if(uip_ext_len) {
-    if(UIP_EXT_BUF(uip_ext_len)->next == UIP_PROTO_ICMP6 && UIP_ICMP_BUF->type < 128) {
-      uip_clear_buf();
-      return;
-    }
-  } else {
-    if(UIP_IP_BUF->proto == UIP_PROTO_ICMP6 && UIP_ICMP_BUF->type < 128) {
-      uip_clear_buf();
-      return;
-    }
+  uint16_t shift;
+  if(uip_last_proto == UIP_PROTO_ICMP6 && UIP_ICMP_BUF->type < 128) {
+    uip_clear_buf();
+    return;
   }
 
-  /* Remove all extension headers related to the routing protocol in place */
+  /* Remove all extension headers related to the routing protocol in place.
+   * Keep all other extension headers, so as to match original packet. */
   NETSTACK_ROUTING.ext_header_remove();
 
   /* remember data of original packet before shifting */
   uip_ipaddr_copy(&tmp_ipaddr, &UIP_IP_BUF->destipaddr);
 
-  uip_len += UIP_IPICMPH_LEN + UIP_ICMP6_ERROR_LEN;
-
-  if(uip_len > UIP_LINK_MTU) {
-    uip_len = UIP_LINK_MTU;
-  }
-
-  memmove(UIP_ICMP_PAYLOAD + uip_ext_len + UIP_ICMP6_ERROR_LEN,
-          (void *)UIP_IP_BUF, uip_len - UIP_IPICMPH_LEN - uip_ext_len - UIP_ICMP6_ERROR_LEN);
+  /* The ICMPv6 error message contains as much of possible of the invoking packet
+   * (see RFC 4443 section 3). Make space for the additional IPv6 and
+   * ICMPv6 headers here and move payload to the "right". What we move includes
+    * extension headers */
+  shift = UIP_IPH_LEN + UIP_ICMPH_LEN + UIP_ICMP6_ERROR_LEN;
+  uip_len += shift;
+  uip_len = MIN(uip_len, UIP_LINK_MTU);
+  memmove(UIP_IP_BUF_CHAR + shift, (void *)UIP_IP_BUF, uip_len - shift);
 
   UIP_IP_BUF->vtc = 0x60;
   UIP_IP_BUF->tcflow = 0;
   UIP_IP_BUF->flow = 0;
-  if (uip_ext_len) {
-    UIP_EXT_BUF(0)->next = UIP_PROTO_ICMP6;
-  } else {
-    UIP_IP_BUF->proto = UIP_PROTO_ICMP6;
-  }
+  UIP_IP_BUF->proto = UIP_PROTO_ICMP6;
   UIP_IP_BUF->ttl = uip_ds6_if.cur_hop_limit;
 
   /* the source should not be unspecified nor multicast, the check for
@@ -293,22 +269,7 @@ echo_reply_input(void)
   uip_ipaddr_copy(&sender, &UIP_IP_BUF->srcipaddr);
   ttl = UIP_IP_BUF->ttl;
 
-  if(uip_ext_len > 0) {
-    /* Remove extension headers if any */
-    UIP_IP_BUF->proto = UIP_PROTO_ICMP6;
-    uip_len -= uip_ext_len;
-    UIP_IP_BUF->len[0] = ((uip_len - UIP_IPH_LEN) >> 8);
-    UIP_IP_BUF->len[1] = ((uip_len - UIP_IPH_LEN) & 0xff);
-    /* move the echo reply payload (starting after the icmp header)
-     * to the new location in the reply.  The shift is equal to the
-     * length of the extension headers present Note: UIP_ICMP_BUF
-     * still points to the echo request at this stage
-     */
-    memmove((uint8_t *)UIP_ICMP_BUF + UIP_ICMPH_LEN - uip_ext_len,
-        (uint8_t *)UIP_ICMP_BUF + UIP_ICMPH_LEN,
-        (uip_len - UIP_IPH_LEN - UIP_ICMPH_LEN));
-    uip_ext_len = 0;
-  }
+  uip_remove_ext_hdr();
 
   /* Call all registered applications to let them know an echo reply
      has been received. */
diff --git a/os/net/ipv6/uip-nd6.c b/os/net/ipv6/uip-nd6.c
index 2dbf95b00..9fc7429fe 100644
--- a/os/net/ipv6/uip-nd6.c
+++ b/os/net/ipv6/uip-nd6.c
@@ -322,7 +322,7 @@ create_na:
 #if UIP_CONF_ROUTER
     flags = flags | UIP_ND6_NA_FLAG_ROUTER;
 #endif
-  uip_ext_len = 0;
+  uip_clear_buf();
   UIP_IP_BUF->vtc = 0x60;
   UIP_IP_BUF->tcflow = 0;
   UIP_IP_BUF->flow = 0;
@@ -343,8 +343,7 @@ create_na:
   UIP_ICMP_BUF->icmpchksum = 0;
   UIP_ICMP_BUF->icmpchksum = ~uip_icmp6chksum();
 
-  uip_len =
-    UIP_IPH_LEN + UIP_ICMPH_LEN + UIP_ND6_NA_LEN + UIP_ND6_OPT_LLAO_LEN;
+  uipbuf_set_len(UIP_IPH_LEN + UIP_ICMPH_LEN + UIP_ND6_NA_LEN + UIP_ND6_OPT_LLAO_LEN);
 
   UIP_STAT(++uip_stat.nd6.sent);
   LOG_INFO("Sending NA to ");
@@ -368,7 +367,7 @@ discard:
 void
 uip_nd6_ns_output(uip_ipaddr_t * src, uip_ipaddr_t * dest, uip_ipaddr_t * tgt)
 {
-  uip_ext_len = 0;
+  uip_clear_buf();
   UIP_IP_BUF->vtc = 0x60;
   UIP_IP_BUF->tcflow = 0;
   UIP_IP_BUF->flow = 0;
diff --git a/os/net/ipv6/uip.h b/os/net/ipv6/uip.h
index 39714c135..49a7a0b21 100755
--- a/os/net/ipv6/uip.h
+++ b/os/net/ipv6/uip.h
@@ -1330,6 +1330,10 @@ extern uint16_t uip_len;
  * The length of the extension headers
  */
 extern uint8_t uip_ext_len;
+
+/** The final protocol after IPv6 extension headers:
+  * UIP_PROTO_TCP, UIP_PROTO_UDP or UIP_PROTO_ICMP6 */
+extern uint8_t uip_last_proto;
 /** @} */
 
 #if UIP_URGDATA > 0
@@ -1345,6 +1349,7 @@ extern uint16_t uip_urglen, uip_surglen;
 #define uip_clear_buf() { \
   uip_len = 0; \
   uip_ext_len = 0; \
+  uip_last_proto = 0; \
   uipbuf_clear_attr();\
 }
 
@@ -1804,7 +1809,7 @@ struct uip_udp_hdr {
 #define UIP_PROTO_NONE        59
 /** @} */
 
-#define uip_is_proto_ext_hdr(proto) (proto == UIP_PROTO_HBHO || proto == UIP_PROTO_DESTO || proto == UIP_PROTO_ROUTING || proto == UIP_PROTO_FRAG || proto == UIP_PROTO_NONE)
+#define uip_is_proto_ext_hdr(proto) ((proto) != UIP_PROTO_TCP && (proto) != UIP_PROTO_UDP && (proto) != UIP_PROTO_ICMP6)
 
 /** @{ */
 /** \brief  Destination and Hop By Hop extension headers option types */
@@ -2175,6 +2180,11 @@ uint16_t uip_udpchksum(void);
  */
 uint16_t uip_icmp6chksum(void);
 
+/**
+ * Removes all IPv6 extension headers from uip_buf, updates length fields
+ * (uip_len and uip_ext_len)
+ */
+void uip_remove_ext_hdr(void);
 
 #endif /* UIP_H_ */
 
diff --git a/os/net/ipv6/uip6.c b/os/net/ipv6/uip6.c
index d9aec0089..4ded21bdc 100644
--- a/os/net/ipv6/uip6.c
+++ b/os/net/ipv6/uip6.c
@@ -132,6 +132,9 @@ uint8_t uip_ext_bitmap = 0;
 uint8_t uip_ext_len = 0;
 /** \brief length of the header options read */
 uint8_t uip_ext_opt_offset = 0;
+/** \brief The final protocol after IPv6 extension headers:
+  * UIP_PROTO_TCP, UIP_PROTO_UDP or UIP_PROTO_ICMP6 */
+uint8_t uip_last_proto = 0;
 /** @} */
 
 /*---------------------------------------------------------------------------*/
@@ -499,15 +502,15 @@ uip_connect(const uip_ipaddr_t *ripaddr, uint16_t rport)
 #endif /* UIP_TCP && UIP_ACTIVE_OPEN */
 /*---------------------------------------------------------------------------*/
 void
-remove_ext_hdr(void)
+uip_remove_ext_hdr(void)
 {
   int last_uip_ext_len;
   /* Remove ext header before TCP/UDP processing. */
   if(uip_ext_len > 0) {
-    LOG_DBG("Cutting ext-header before processing (extlen: %d, uiplen: %d)\n",
+    LOG_DBG("Removing IPv6 extension headers (extlen: %d, uiplen: %d)\n",
            uip_ext_len, uip_len);
     if(uip_len < UIP_IPH_LEN + uip_ext_len) {
-      LOG_ERR("ERROR: uip_len too short compared to ext len\n");
+      LOG_ERR("uip_len too short compared to ext len\n");
       uip_clear_buf();
       return;
     }
@@ -861,7 +864,7 @@ ext_hdr_options_process(void)
        * Using this fix, the header is ignored, and the next header (if
        * present) is processed.
        */
-      if(!NETSTACK_ROUTING.ext_header_hbh_update(uip_ext_opt_offset)) {
+      if(!NETSTACK_ROUTING.ext_header_hbh_update(uip_ext_len, uip_ext_opt_offset)) {
         LOG_ERR("RPL Option Error: Dropping Packet\n");
         return 1;
       }
@@ -909,6 +912,7 @@ ext_hdr_options_process(void)
 void
 uip_process(uint8_t flag)
 {
+  uint8_t *last_header;
 #if UIP_TCP
   int c;
   uint16_t tmp16;
@@ -1104,10 +1108,21 @@ uip_process(uint8_t flag)
      * header (40 bytes).
      */
   } else {
-    LOG_ERR("packet shorter than reported in IP header.");
+    LOG_ERR("packet shorter than reported in IP header");
     goto drop;
   }
 
+  /* Check sanity of extension headers, and compute the total extension header
+   * length (uip_ext_len) as well as the final protocol (uip_last_proto) */
+  uip_last_proto = 0;
+  last_header = uipbuf_get_last_header(UIP_IP_BUF_CHAR, uip_len, &uip_last_proto);
+  if(last_header == NULL) {
+    LOG_ERR("invalid extension header chain");
+    goto drop;
+  }
+  /* Set uip_ext_len */
+  uip_ext_len = last_header - UIP_IP_BUF_CHAR;
+
   LOG_INFO("packet received from ");
   LOG_INFO_6ADDR(&UIP_IP_BUF->srcipaddr);
   LOG_INFO_(" to ");
@@ -1136,6 +1151,7 @@ uip_process(uint8_t flag)
   uip_next_hdr = &UIP_IP_BUF->proto;
   uip_ext_len = 0;
   uip_ext_bitmap = 0;
+
   if(*uip_next_hdr == UIP_PROTO_HBHO) {
 #if UIP_CONF_IPV6_CHECKS
     uip_ext_bitmap |= UIP_EXT_HDR_BITMAP_HBHO;
@@ -1254,15 +1270,18 @@ uip_process(uint8_t flag)
 #if UIP_TCP
     case UIP_PROTO_TCP:
       /* TCP, for both IPv4 and IPv6 */
+      uip_last_proto = *uip_next_hdr;
       goto tcp_input;
 #endif /* UIP_TCP */
 #if UIP_UDP
     case UIP_PROTO_UDP:
       /* UDP, for both IPv4 and IPv6 */
+      uip_last_proto = *uip_next_hdr;
       goto udp_input;
 #endif /* UIP_UDP */
     case UIP_PROTO_ICMP6:
       /* ICMPv6 */
+      uip_last_proto = *uip_next_hdr;
       goto icmp6_input;
     case UIP_PROTO_HBHO:
       LOG_DBG("Processing hbh header\n");
@@ -1469,7 +1488,7 @@ uip_process(uint8_t flag)
   /* UDP input processing. */
   udp_input:
 
-  remove_ext_hdr();
+  uip_remove_ext_hdr();
 
   LOG_INFO("Receiving UDP packet\n");
 
@@ -1581,7 +1600,7 @@ uip_process(uint8_t flag)
   /* TCP input processing. */
   tcp_input:
 
-  remove_ext_hdr();
+  uip_remove_ext_hdr();
 
   UIP_STAT(++uip_stat.tcp.recv);
   LOG_INFO("Receiving TCP packet\n");
diff --git a/os/net/ipv6/uipbuf.c b/os/net/ipv6/uipbuf.c
index 6ccbb99aa..efa2413e9 100644
--- a/os/net/ipv6/uipbuf.c
+++ b/os/net/ipv6/uipbuf.c
@@ -38,45 +38,119 @@
 
 static uint16_t uipbuf_attrs[UIPBUF_ATTR_MAX];
 
+/*---------------------------------------------------------------------------*/
+bool
+uipbuf_add_ext_hdr(int16_t len)
+{
+  if(len + uip_len <= UIP_LINK_MTU && len + uip_len >= 0) {
+    uip_ext_len += len;
+    uip_len += len;
+    return true;
+  } else {
+    return false;
+  }
+}
+/*---------------------------------------------------------------------------*/
+bool
+uipbuf_set_len(uint16_t len)
+{
+  if(len <= UIP_LINK_MTU) {
+    uip_len = len;
+    return true;
+  } else {
+    return false;
+  }
+}
 /*---------------------------------------------------------------------------*/
 /* Get the next header given the buffer - start indicates that this is
    start of the IPv6 header - needs to be set to 0 when in an ext hdr */
-uint8_t*
-uipbuf_get_next_header(uint8_t *buffer, uint16_t size, uint8_t *protocol, uint8_t start)
+uint8_t *
+uipbuf_get_next_header(uint8_t *buffer, uint16_t size, uint8_t *protocol, bool start)
 {
-  int ext_len = 0;
-  struct uip_ip_hdr *ipbuf = (struct uip_ip_hdr *) buffer;
-  struct uip_ext_hdr *ext = NULL;
+  int curr_hdr_len = 0;
+  int next_hdr_len = 0;
+  uint8_t *next_header = NULL;
+  struct uip_ip_hdr *ipbuf = NULL;
+  struct uip_ext_hdr *curr_ext = NULL;
+  struct uip_ext_hdr *next_ext = NULL;
 
   if(start) {
     /* protocol in the IP buffer */
+    ipbuf = (struct uip_ip_hdr *)buffer;
     *protocol = ipbuf->proto;
-    return buffer + UIP_IPH_LEN;
+    curr_hdr_len = UIP_IPH_LEN;
   } else {
     /* protocol in the Ext hdr */
-    ext = (struct uip_ext_hdr *) buffer;
-    *protocol = ext->next;
+    curr_ext = (struct uip_ext_hdr *)buffer;
+    *protocol = curr_ext->next;
     /* This is just an ext header */
-    ext_len = (ext->len << 3) + 8;
-    return buffer + ext_len;
+    curr_hdr_len = (curr_ext->len << 3) + 8;
   }
+
+  /* Check first if enough space for current header */
+  if(curr_hdr_len > size) {
+    return NULL;
+  }
+  next_header = buffer + curr_hdr_len;
+
+  /* Check if the buffer is large enough for the next header */
+  if(uip_is_proto_ext_hdr(*protocol)) {
+    next_ext = (struct uip_ext_hdr *)next_header;
+    next_hdr_len = (next_ext->len << 3) + 8;
+  } else {
+    if(*protocol == UIP_PROTO_TCP) {
+      next_hdr_len = UIP_TCPH_LEN;
+    } else if(*protocol == UIP_PROTO_UDP) {
+      next_hdr_len = UIP_UDPH_LEN;
+    } else if(*protocol == UIP_PROTO_ICMP6) {
+      next_hdr_len = UIP_ICMPH_LEN;
+    }
+  }
+
+  /* Size must be enough to hold both the current and next header */
+  if(next_hdr_len == 0 || curr_hdr_len + next_hdr_len > size) {
+    return NULL;
+  }
+
+  return next_header;
 }
 /*---------------------------------------------------------------------------*/
 /* Get the final header given the buffer - that is assumed to be at start
    of an IPv6 header */
-uint8_t*
+uint8_t *
 uipbuf_get_last_header(uint8_t *buffer, uint16_t size, uint8_t *protocol)
 {
   uint8_t *nbuf;
 
-  nbuf = uipbuf_get_next_header(buffer, size, protocol, 1);
-  while(uip_is_proto_ext_hdr(*protocol)) {
-    /* send in and move beyond the ext hdr */
-    nbuf = uipbuf_get_next_header(nbuf, size - (nbuf - buffer), protocol, 0);
+  nbuf = uipbuf_get_next_header(buffer, size, protocol, true);
+  while(nbuf != NULL && uip_is_proto_ext_hdr(*protocol)) {
+    /* move to the ext hdr */
+    nbuf = uipbuf_get_next_header(nbuf, size - (nbuf - buffer), protocol, false);
   }
+
+  /* In case the buffer wasn't large enough for all headers, return NULL */
   return nbuf;
 }
 /*---------------------------------------------------------------------------*/
+uint8_t *
+uipbuf_search_header(uint8_t *buffer, uint16_t size, uint8_t protocol)
+{
+  uint8_t *nbuf;
+  uint8_t next_proto;
+
+  nbuf = uipbuf_get_next_header(buffer, size, &next_proto, true);
+  while(nbuf != NULL && next_proto != protocol && uip_is_proto_ext_hdr(next_proto)) {
+    /* move to the ext hdr */
+    nbuf = uipbuf_get_next_header(nbuf, size - (nbuf - buffer), &next_proto, false);
+  }
+
+  if(next_proto == protocol) {
+    return nbuf;
+  } else {
+    return NULL;
+  }
+}
+/*---------------------------------------------------------------------------*/
 /**
  * Common functions for uipbuf (attributes, etc).
  *
diff --git a/os/net/ipv6/uipbuf.h b/os/net/ipv6/uipbuf.h
index 337ffe17f..a2e091015 100644
--- a/os/net/ipv6/uipbuf.h
+++ b/os/net/ipv6/uipbuf.h
@@ -35,17 +35,31 @@
 
 #include "contiki.h"
 
+/**
+ * \brief          Update uip buffer length for addition of an extension header
+ * \param len      The length of the new extension header
+ * \retval         true if the length fields were successfully set, false otherwise
+ */
+bool uipbuf_add_ext_hdr(int16_t len);
+
+/**
+ * \brief          Set the length of the uIP buffer
+ * \param len      The new length
+ * \retval         true if the len was successfully set, false otherwise
+ */
+bool uipbuf_set_len(uint16_t len);
+
 /**
  * \brief          Get the next IPv6 header.
  * \param buffer   A pointer to the buffer holding the IPv6 packet
  * \param size     The size of the data in the buffer
  * \param protocol A pointer to a variable where the protocol of the header will be stored
  * \param start    A flag that indicates if this is expected to be the IPv6 packet header or a later header (Extension header)
- * \retval         returns address of the starting position of the next header
+ * \retval         returns address of the next header, or NULL in case of insufficient buffer space
  *
  *                 This function moves to the next header in a IPv6 packet.
  */
-uint8_t* uipbuf_get_next_header(uint8_t *buffer, uint16_t size, uint8_t *protocol, uint8_t start);
+uint8_t *uipbuf_get_next_header(uint8_t *buffer, uint16_t size, uint8_t *protocol, bool start);
 
 
 /**
@@ -53,12 +67,22 @@ uint8_t* uipbuf_get_next_header(uint8_t *buffer, uint16_t size, uint8_t *protoco
  * \param buffer   A pointer to the buffer holding the IPv6 packet
  * \param size     The size of the data in the buffer
  * \param protocol A pointer to a variable where the protocol of the header will be stored
- * \retval         returns address of the starting position of the next header
+ * \retval         returns address of the last header, or NULL in case of insufficient buffer space
  *
  *                 This function moves to the last header of the IPv6 packet.
  */
-uint8_t* uipbuf_get_last_header(uint8_t *buffer, uint16_t size, uint8_t *protocol);
+uint8_t *uipbuf_get_last_header(uint8_t *buffer, uint16_t size, uint8_t *protocol);
 
+/**
+ * \brief          Get an IPv6 header with a given protocol field.
+ * \param buffer   A pointer to the buffer holding the IPv6 packet
+ * \param size     The size of the data in the buffer
+ * \param protocol The protocol we are looking for
+ * \retval         returns address of the header if found, else NULL
+ *
+ *                 This function moves to the last header of the IPv6 packet.
+ */
+uint8_t *uipbuf_search_header(uint8_t *buffer, uint16_t size, uint8_t protocol);
 
 /**
  * \brief          Get the value of the attribute
diff --git a/os/net/routing/nullrouting/nullrouting.c b/os/net/routing/nullrouting/nullrouting.c
index bb1399fbb..3443dbb13 100644
--- a/os/net/routing/nullrouting/nullrouting.c
+++ b/os/net/routing/nullrouting/nullrouting.c
@@ -107,7 +107,7 @@ static void
 ext_header_remove(void)
 {
 #if NETSTACK_CONF_WITH_IPV6
-  uip_ext_len = 0;
+  uip_remove_ext_hdr();
 #endif /* NETSTACK_CONF_WITH_IPV6 */
 }
 /*---------------------------------------------------------------------------*/
@@ -118,7 +118,7 @@ ext_header_update(void)
 }
 /*---------------------------------------------------------------------------*/
 static int
-ext_header_hbh_update(int uip_ext_opt_offset)
+ext_header_hbh_update(int ext_offset, int opt_offset)
 {
   return 1;
 }
diff --git a/os/net/routing/routing.h b/os/net/routing/routing.h
index 7fe6e7ca8..be1fe0173 100644
--- a/os/net/routing/routing.h
+++ b/os/net/routing/routing.h
@@ -131,12 +131,14 @@ struct routing_driver {
   * Process and update the routing protocol hob-by-hop
   * extention headers of the current uIP packet.
   *
-  * \param uip_ext_opt_offset The offset within the uIP packet where
+  * \param ext_offset The offset within the uIP packet where
   * extension headers start
+  * \param opt_offset The offset within the extension header where
+  * the option starts
   * \return 1 in case the packet is valid and to be processed further,
   * 0 in case the packet must be dropped.
   */
-  int (* ext_header_hbh_update)(int uip_ext_opt_offset);
+  int (* ext_header_hbh_update)(int ext_offset, int opt_offset);
   /**
   * Process and update SRH in-place,
   * i.e. internal address swapping as per RFC6554
diff --git a/os/net/routing/rpl-classic/rpl-ext-header.c b/os/net/routing/rpl-classic/rpl-ext-header.c
index 6a99b573a..b175edd38 100644
--- a/os/net/routing/rpl-classic/rpl-ext-header.c
+++ b/os/net/routing/rpl-classic/rpl-ext-header.c
@@ -62,7 +62,7 @@
 
 /*---------------------------------------------------------------------------*/
 int
-rpl_ext_header_hbh_update(int uip_ext_opt_offset)
+rpl_ext_header_hbh_update(int ext_offset, int opt_offset)
 {
   rpl_instance_t *instance;
   int down;
@@ -71,25 +71,25 @@ rpl_ext_header_hbh_update(int uip_ext_opt_offset)
   uip_ds6_route_t *route;
   rpl_parent_t *sender = NULL;
 
-  if(UIP_HBHO_BUF(uip_ext_len)->len != ((RPL_HOP_BY_HOP_LEN - 8) / 8)
-      || UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type != UIP_EXT_HDR_OPT_RPL
-      || UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len != RPL_HDR_OPT_LEN) {
+  if(UIP_HBHO_BUF(ext_offset)->len != ((RPL_HOP_BY_HOP_LEN - 8) / 8)
+      || UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->opt_type != UIP_EXT_HDR_OPT_RPL
+      || UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->opt_len != RPL_HDR_OPT_LEN) {
 
     LOG_ERR("Hop-by-hop extension header has wrong size or type (%u %u %u)\n",
-        UIP_HBHO_BUF(uip_ext_len)->len,
-        UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type,
-        UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len);
+        UIP_HBHO_BUF(ext_offset)->len,
+        UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->opt_type,
+        UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->opt_len);
     return 0; /* Drop */
   }
 
-  instance = rpl_get_instance(UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance);
+  instance = rpl_get_instance(UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->instance);
   if(instance == NULL) {
     LOG_ERR("Unknown instance: %u\n",
-           UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance);
+           UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->instance);
     return 0;
   }
 
-  if(UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags & RPL_HDR_OPT_FWD_ERR) {
+  if(UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags & RPL_HDR_OPT_FWD_ERR) {
     LOG_ERR("Forward error!\n");
     /* We should try to repair it by removing the neighbor that caused
          the packet to be forwareded in the first place. We drop any
@@ -113,14 +113,14 @@ rpl_ext_header_hbh_update(int uip_ext_opt_offset)
     return 0;
   }
   down = 0;
-  if(UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags & RPL_HDR_OPT_DOWN) {
+  if(UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags & RPL_HDR_OPT_DOWN) {
     down = 1;
   }
 
-  sender_rank = UIP_HTONS(UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->senderrank);
+  sender_rank = UIP_HTONS(UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->senderrank);
   sender = nbr_table_get_from_lladdr(rpl_parents, packetbuf_addr(PACKETBUF_ADDR_SENDER));
 
-  if(sender != NULL && (UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags & RPL_HDR_OPT_RANK_ERR)) {
+  if(sender != NULL && (UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags & RPL_HDR_OPT_RANK_ERR)) {
     /* A rank error was signalled, attempt to repair it by updating
      * the sender's rank from ext header */
     sender->rank = sender_rank;
@@ -150,7 +150,7 @@ rpl_ext_header_hbh_update(int uip_ext_opt_offset)
       instance->unicast_dio_target = sender;
       rpl_schedule_unicast_dio_immediately(instance);
     }
-    if(UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags & RPL_HDR_OPT_RANK_ERR) {
+    if(UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags & RPL_HDR_OPT_RANK_ERR) {
       RPL_STAT(rpl_stats.loop_errors++);
       LOG_ERR(" Rank error signalled in RPL option!\n");
       /* Packet must be dropped and dio trickle timer reset, see RFC6550 - 11.2.2.2 */
@@ -159,7 +159,7 @@ rpl_ext_header_hbh_update(int uip_ext_opt_offset)
     }
     LOG_WARN("Single error tolerated\n");
     RPL_STAT(rpl_stats.loop_warnings++);
-    UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags |= RPL_HDR_OPT_RANK_ERR;
+    UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags |= RPL_HDR_OPT_RANK_ERR;
     return 1;
   }
 
@@ -171,41 +171,19 @@ int
 rpl_ext_header_srh_get_next_hop(uip_ipaddr_t *ipaddr)
 {
 #if RPL_WITH_NON_STORING
-  uint8_t *uip_next_hdr;
-  int last_uip_ext_len = uip_ext_len;
+  struct uip_routing_hdr *rh_header;
   rpl_dag_t *dag;
   uip_sr_node_t *dest_node;
   uip_sr_node_t *root_node;
 
-  uip_ext_len = 0;
-  uip_next_hdr = &UIP_IP_BUF->proto;
-
-  /* Look for routing header */
-  while(uip_next_hdr != NULL && *uip_next_hdr != UIP_PROTO_ROUTING) {
-    switch(*uip_next_hdr) {
-      case UIP_PROTO_HBHO:
-      case UIP_PROTO_DESTO:
-        /*
-         * As per RFC 2460, only the Hop-by-Hop Options header and
-         * Destination Options header can appear before the Routing
-         * header.
-         */
-        /* Move to next header */
-        uip_next_hdr = &UIP_EXT_BUF(uip_ext_len)->next;
-        uip_ext_len += (UIP_EXT_BUF(uip_ext_len)->len << 3) + 8;
-        break;
-      default:
-        uip_next_hdr = NULL;
-        break;
-    }
-  }
+  /* Look for routing ext header */
+  rh_header = (struct uip_routing_hdr *)uipbuf_search_header(UIP_IP_BUF_CHAR, uip_len, UIP_PROTO_ROUTING);
 
   dag = rpl_get_dag(&UIP_IP_BUF->destipaddr);
   root_node = uip_sr_get_node(dag, &dag->dag_id);
   dest_node = uip_sr_get_node(dag, &UIP_IP_BUF->destipaddr);
 
-  if((uip_next_hdr != NULL && *uip_next_hdr == UIP_PROTO_ROUTING
-      && UIP_RH_BUF(uip_ext_len)->routing_type == RPL_RH_TYPE_SRH) ||
+  if((rh_header != NULL && rh_header->routing_type == RPL_RH_TYPE_SRH) ||
      (dest_node != NULL && root_node != NULL &&
       dest_node->parent == root_node)) {
     /* Routing header found or the packet destined for a direct child of the root.
@@ -214,11 +192,9 @@ rpl_ext_header_srh_get_next_hop(uip_ipaddr_t *ipaddr)
      * forwarding to next hop */
     uip_ipaddr_copy(ipaddr, &UIP_IP_BUF->destipaddr);
     uip_create_linklocal_prefix(ipaddr);
-    uip_ext_len = last_uip_ext_len;
     return 1;
   }
 
-  uip_ext_len = last_uip_ext_len;
   return 0;
 #else /* RPL_WITH_NON_STORING */
   return 0; /* SRH not found */
@@ -229,34 +205,13 @@ int
 rpl_ext_header_srh_update(void)
 {
 #if RPL_WITH_NON_STORING
-  uint8_t *uip_next_hdr;
-  int last_uip_ext_len = uip_ext_len;
+  struct uip_routing_hdr *rh_header;
+  struct uip_rpl_srh_hdr *srh_header;
 
-  uip_ext_len = 0;
-  uip_next_hdr = &UIP_IP_BUF->proto;
+  /* Look for routing ext header */
+  rh_header = (struct uip_routing_hdr *)uipbuf_search_header(UIP_IP_BUF_CHAR, uip_len, UIP_PROTO_ROUTING);
 
-  /* Look for routing header */
-  while(uip_next_hdr != NULL && *uip_next_hdr != UIP_PROTO_ROUTING) {
-    switch(*uip_next_hdr) {
-      case UIP_PROTO_HBHO:
-      case UIP_PROTO_DESTO:
-        /*
-         * As per RFC 2460, only the Hop-by-Hop Options header and
-         * Destination Options header can appear before the Routing
-         * header.
-         */
-        /* Move to next header */
-        uip_next_hdr = &UIP_EXT_BUF(uip_ext_len)->next;
-        uip_ext_len += (UIP_EXT_BUF(uip_ext_len)->len << 3) + 8;
-        break;
-      default:
-        uip_next_hdr = NULL;
-        break;
-    }
-  }
-
-  if(uip_next_hdr != NULL && *uip_next_hdr == UIP_PROTO_ROUTING
-      && UIP_RH_BUF(uip_ext_len)->routing_type == RPL_RH_TYPE_SRH) {
+  if(rh_header != NULL && rh_header->routing_type == RPL_RH_TYPE_SRH) {
     /* SRH found, now look for next hop */
     uint8_t cmpri, cmpre;
     uint8_t ext_len;
@@ -265,11 +220,12 @@ rpl_ext_header_srh_update(void)
     uint8_t segments_left;
     uip_ipaddr_t current_dest_addr;
 
-    segments_left = UIP_RH_BUF(uip_ext_len)->seg_left;
-    ext_len = (UIP_RH_BUF(uip_ext_len)->len * 8) + 8;
-    cmpri = UIP_RPL_SRH_BUF(uip_ext_len)->cmpr >> 4;
-    cmpre = UIP_RPL_SRH_BUF(uip_ext_len)->cmpr & 0x0f;
-    padding = UIP_RPL_SRH_BUF(uip_ext_len)->pad >> 4;
+    srh_header = (struct uip_rpl_srh_hdr *)(((uint8_t *)rh_header) + RPL_RH_LEN);
+    segments_left = rh_header->seg_left;
+    ext_len = rh_header->len * 8 + 8;
+    cmpri = srh_header->cmpr >> 4;
+    cmpre = srh_header->cmpr & 0x0f;
+    padding = srh_header->pad >> 4;
     path_len = ((ext_len - padding - RPL_RH_LEN - RPL_SRH_LEN - (16 - cmpre)) / (16 - cmpri)) + 1;
     (void)path_len;
 
@@ -280,7 +236,7 @@ rpl_ext_header_srh_update(void)
       /* We are the final destination, do nothing */
     } else {
       uint8_t i = path_len - segments_left; /* The index of the next address to be visited */
-      uint8_t *addr_ptr = ((uint8_t *)UIP_RH_BUF(uip_ext_len)) + RPL_RH_LEN + RPL_SRH_LEN + (i * (16 - cmpri));
+      uint8_t *addr_ptr = ((uint8_t *)rh_header) + RPL_RH_LEN + RPL_SRH_LEN + (i * (16 - cmpri));
       uint8_t cmpr = segments_left == 1 ? cmpre : cmpri;
 
       /* As per RFC6554: swap the IPv6 destination address and address[i] */
@@ -293,17 +249,15 @@ rpl_ext_header_srh_update(void)
       memcpy(addr_ptr, ((uint8_t *)&current_dest_addr) + cmpr, 16 - cmpr);
 
       /* Update segments left field */
-      UIP_RH_BUF(uip_ext_len)->seg_left--;
+      rh_header->seg_left--;
 
       LOG_INFO("SRH next hop ");
       LOG_INFO_6ADDR(&UIP_IP_BUF->destipaddr);
       LOG_INFO_("\n");
     }
-    uip_ext_len = last_uip_ext_len;
     return 1;
   }
 
-  uip_ext_len = last_uip_ext_len;
   return 0;
 #else /* RPL_WITH_NON_STORING */
   return 0; /* SRH not found */
@@ -408,7 +362,7 @@ insert_srh_header(void)
       path_len, cmpri, cmpre, ext_len, padding);
 
   /* Check if there is enough space to store the extension header */
-  if(uip_len + ext_len > UIP_BUFSIZE - UIP_LLH_LEN) {
+  if(uip_len + ext_len > UIP_LINK_MTU) {
     LOG_ERR("Packet too long: impossible to add source routing header (%u bytes)\n", ext_len);
     return 0;
   }
@@ -418,23 +372,23 @@ insert_srh_header(void)
       uip_buf + uip_l2_l3_hdr_len, uip_len - UIP_IPH_LEN);
   memset(uip_buf + uip_l2_l3_hdr_len, 0, ext_len);
 
-  /* Insert source routing header */
-  UIP_RH_BUF(uip_ext_len)->next = UIP_IP_BUF->proto;
+  /* Insert source routing header (as first ext header) */
+  UIP_RH_BUF(0)->next = UIP_IP_BUF->proto;
   UIP_IP_BUF->proto = UIP_PROTO_ROUTING;
 
   /* Initialize IPv6 Routing Header */
-  UIP_RH_BUF(uip_ext_len)->len = (ext_len - 8) / 8;
-  UIP_RH_BUF(uip_ext_len)->routing_type = RPL_RH_TYPE_SRH;
-  UIP_RH_BUF(uip_ext_len)->seg_left = path_len;
+  UIP_RH_BUF(0)->len = (ext_len - 8) / 8;
+  UIP_RH_BUF(0)->routing_type = RPL_RH_TYPE_SRH;
+  UIP_RH_BUF(0)->seg_left = path_len;
 
   /* Initialize RPL Source Routing Header */
-  UIP_RPL_SRH_BUF(uip_ext_len)->cmpr = (cmpri << 4) + cmpre;
-  UIP_RPL_SRH_BUF(uip_ext_len)->pad = padding << 4;
+  UIP_RPL_SRH_BUF(0)->cmpr = (cmpri << 4) + cmpre;
+  UIP_RPL_SRH_BUF(0)->pad = padding << 4;
 
   /* Initialize addresses field (the actual source route).
    * From last to first. */
   node = dest_node;
-  hop_ptr = ((uint8_t *)UIP_RH_BUF(uip_ext_len)) + ext_len - padding; /* Pointer where to write the next hop compressed address */
+  hop_ptr = ((uint8_t *)UIP_RH_BUF(0)) + ext_len - padding; /* Pointer where to write the next hop compressed address */
 
   while(node != NULL && node->parent != root_node) {
     NETSTACK_ROUTING.get_sr_node_ipaddr(&node_addr, node);
@@ -456,8 +410,7 @@ insert_srh_header(void)
     UIP_IP_BUF->len[0]++;
   }
 
-  uip_ext_len += ext_len;
-  uip_len += ext_len;
+  uipbuf_add_ext_hdr(ext_len);
 
   return 1;
 }
@@ -466,44 +419,37 @@ static int
 update_hbh_header(void)
 {
   rpl_instance_t *instance;
-  int uip_ext_opt_offset;
-  int last_uip_ext_len;
   rpl_parent_t *parent;
+  int opt_offset = 2;
 
-  last_uip_ext_len = uip_ext_len;
-  uip_ext_len = 0;
-  uip_ext_opt_offset = 2;
+  if(UIP_IP_BUF->proto == UIP_PROTO_HBHO && UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_type == UIP_EXT_HDR_OPT_RPL) {
+    if(UIP_HBHO_BUF(0)->len != ((RPL_HOP_BY_HOP_LEN - 8) / 8)
+        || UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_len != RPL_HDR_OPT_LEN) {
 
-  if(UIP_IP_BUF->proto == UIP_PROTO_HBHO && UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type == UIP_EXT_HDR_OPT_RPL) {
-    if(UIP_HBHO_BUF(uip_ext_len)->len != ((RPL_HOP_BY_HOP_LEN - 8) / 8)
-        || UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len != RPL_HDR_OPT_LEN) {
-
-      LOG_ERR("Hop-by-hop extension header has wrong size (%u %u)\n",
-          UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len,
-          uip_ext_len);
+      LOG_ERR("Hop-by-hop extension header has wrong size (%u)\n",
+          UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_len);
       return 0; /* Drop */
     }
 
-    instance = rpl_get_instance(UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance);
+    instance = rpl_get_instance(UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->instance);
     if(instance == NULL || !instance->used || !instance->current_dag->joined) {
       LOG_ERR("Unable to add/update hop-by-hop extension header: incorrect instance\n");
-      uip_ext_len = last_uip_ext_len;
       return 0; /* Drop */
     }
 
     LOG_INFO("Updating RPL option\n");
     /* Update sender rank and instance, will update flags next */
-    UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->senderrank = UIP_HTONS(instance->current_dag->rank);
-    UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance = instance->instance_id;
+    UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->senderrank = UIP_HTONS(instance->current_dag->rank);
+    UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->instance = instance->instance_id;
 
     if(RPL_IS_STORING(instance)) { /* In non-storing mode, downwards traffic does not have the HBH option */
       /* Check the direction of the down flag, as per Section 11.2.2.3,
             which states that if a packet is going down it should in
             general not go back up again. If this happens, a
             RPL_HDR_OPT_FWD_ERR should be flagged. */
-      if((UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags & RPL_HDR_OPT_DOWN)) {
+      if((UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->flags & RPL_HDR_OPT_DOWN)) {
         if(uip_ds6_route_lookup(&UIP_IP_BUF->destipaddr) == NULL) {
-          UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags |= RPL_HDR_OPT_FWD_ERR;
+          UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->flags |= RPL_HDR_OPT_FWD_ERR;
           LOG_WARN("RPL forwarding error\n");
           /* We should send back the packet to the originating parent,
                 but it is not feasible yet, so we send a No-Path DAO instead */
@@ -522,37 +468,30 @@ update_hbh_header(void)
         if(uip_ds6_route_lookup(&UIP_IP_BUF->destipaddr) == NULL) {
           /* No route was found, so this packet will go towards the RPL
                 root. If so, we should not set the down flag. */
-          UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags &= ~RPL_HDR_OPT_DOWN;
+          UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->flags &= ~RPL_HDR_OPT_DOWN;
           LOG_DBG("RPL option going up\n");
         } else {
           /* A DAO route was found so we set the down flag. */
-          UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags |= RPL_HDR_OPT_DOWN;
+          UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->flags |= RPL_HDR_OPT_DOWN;
           LOG_DBG("RPL option going down\n");
         }
       }
     }
   }
 
-  uip_ext_len = last_uip_ext_len;
   return 1;
 }
 /*---------------------------------------------------------------------------*/
 static int
 insert_hbh_header(const rpl_instance_t *instance)
 {
-  int uip_ext_opt_offset;
-  int last_uip_ext_len;
   uint8_t temp_len;
-
-  last_uip_ext_len = uip_ext_len;
-  uip_ext_len = 0;
-  uip_ext_opt_offset = 2;
+  int opt_offset = 2;
 
   /* Insert hop-by-hop header */
   LOG_DBG("Creating hop-by-hop option\n");
-  if(uip_len + RPL_HOP_BY_HOP_LEN > UIP_BUFSIZE - UIP_LLH_LEN) {
+  if(uip_len + RPL_HOP_BY_HOP_LEN > UIP_LINK_MTU) {
     LOG_ERR("Packet too long: impossible to add hop-by-hop option\n");
-    uip_ext_len = last_uip_ext_len;
     return 0;
   }
 
@@ -560,25 +499,25 @@ insert_hbh_header(const rpl_instance_t *instance)
   memmove(UIP_IP_PAYLOAD(RPL_HOP_BY_HOP_LEN), UIP_IP_PAYLOAD(0), uip_len - UIP_IPH_LEN);
   memset(UIP_IP_PAYLOAD(0), 0, RPL_HOP_BY_HOP_LEN);
 
-  /* Update IP and HBH protocol and fields */
-  UIP_HBHO_BUF(uip_ext_len)->next = UIP_IP_BUF->proto;
+  /* Insert HBH header (as first ext header) */
+  UIP_HBHO_BUF(0)->next = UIP_IP_BUF->proto;
   UIP_IP_BUF->proto = UIP_PROTO_HBHO;
 
   /* Initialize HBH option */
-  UIP_HBHO_BUF(uip_ext_len)->len = (RPL_HOP_BY_HOP_LEN - 8) / 8;
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type = UIP_EXT_HDR_OPT_RPL;
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len = RPL_HDR_OPT_LEN;
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags = 0;
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->senderrank = UIP_HTONS(instance->current_dag->rank);
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance = instance->instance_id;
-  uip_len += RPL_HOP_BY_HOP_LEN;
+  UIP_HBHO_BUF(0)->len = (RPL_HOP_BY_HOP_LEN - 8) / 8;
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_type = UIP_EXT_HDR_OPT_RPL;
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_len = RPL_HDR_OPT_LEN;
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->flags = 0;
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->senderrank = UIP_HTONS(instance->current_dag->rank);
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->instance = instance->instance_id;
+
   temp_len = UIP_IP_BUF->len[1];
   UIP_IP_BUF->len[1] += RPL_HOP_BY_HOP_LEN;
   if(UIP_IP_BUF->len[1] < temp_len) {
     UIP_IP_BUF->len[0]++;
   }
 
-  uip_ext_len = last_uip_ext_len + RPL_HOP_BY_HOP_LEN;
+  uipbuf_add_ext_hdr(RPL_HOP_BY_HOP_LEN);
 
   /* Update header before returning */
   return update_hbh_header();
@@ -587,52 +526,34 @@ insert_hbh_header(const rpl_instance_t *instance)
 void
 rpl_ext_header_remove(void)
 {
-  uint8_t temp_len;
-  uint8_t rpl_ext_hdr_len;
-  int uip_ext_opt_offset;
-  uint8_t *uip_next_hdr;
+  uint8_t *prev_proto_ptr;
+  uint8_t protocol;
+  uint8_t ext_len;
+  uint8_t *next_header;
+  struct uip_ext_hdr *ext_ptr;
+  struct uip_ext_hdr_opt *opt_ptr;
 
-  uip_ext_len = 0;
-  uip_ext_opt_offset = 2;
-  uip_next_hdr = &UIP_IP_BUF->proto;
-
-  /* Look for hop-by-hop and routing headers */
-  while(uip_next_hdr != NULL) {
-    switch(*uip_next_hdr) {
-      case UIP_PROTO_HBHO:
-      case UIP_PROTO_ROUTING:
-        if((*uip_next_hdr != UIP_PROTO_HBHO || UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type == UIP_EXT_HDR_OPT_RPL)) {
-          /* Remove hop-by-hop and routing headers */
-          *uip_next_hdr = UIP_EXT_BUF(uip_ext_len)->next;
-          rpl_ext_hdr_len = (UIP_EXT_BUF(uip_ext_len)->len * 8) + 8;
-          temp_len = UIP_IP_BUF->len[1];
-          uip_len -= rpl_ext_hdr_len;
-          UIP_IP_BUF->len[1] -= rpl_ext_hdr_len;
-          if(UIP_IP_BUF->len[1] > temp_len) {
-            UIP_IP_BUF->len[0]--;
-          }
-          LOG_DBG("Removing RPL extension header (type %u, len %u)\n", *uip_next_hdr, rpl_ext_hdr_len);
-          memmove(UIP_EXT_BUF(uip_ext_len), ((uint8_t *)UIP_EXT_BUF(uip_ext_len)) + rpl_ext_hdr_len, uip_len - UIP_IPH_LEN);
-        } else {
-          uip_next_hdr = &UIP_EXT_BUF(uip_ext_len)->next;
-          uip_ext_len += (UIP_EXT_BUF(uip_ext_len)->len << 3) + 8;
-        }
-        break;
-      case UIP_PROTO_DESTO:
-        /*
-         * As per RFC 2460, any header other than the Destination
-         * Options header does not appear between the Hop-by-Hop
-         * Options header and the Routing header.
-         *
-         * We're moving to the next header only if uip_next_hdr has
-         * UIP_PROTO_DESTO. Otherwise, we'll return.
-         */
-        /* Move to next header */
-        uip_next_hdr = &UIP_EXT_BUF(uip_ext_len)->next;
-        uip_ext_len += (UIP_EXT_BUF(uip_ext_len)->len << 3) + 8;
-        break;
-    default:
-      return;
+  next_header = uipbuf_get_next_header(UIP_IP_BUF_CHAR, uip_len, &protocol, true);
+  ext_ptr = (struct uip_ext_hdr *)next_header;
+  prev_proto_ptr = &UIP_IP_BUF->proto;
+  while(next_header != NULL && uip_is_proto_ext_hdr(protocol)) {
+    opt_ptr = (struct uip_ext_hdr_opt *)(next_header + 2);
+    if(protocol == UIP_PROTO_ROUTING || (protocol == UIP_PROTO_HBHO && opt_ptr->type == UIP_EXT_HDR_OPT_RPL)) {
+      /* Remove ext header */
+      *prev_proto_ptr = ext_ptr->next;
+      ext_len = ext_ptr->len * 8 + 8;
+      uipbuf_add_ext_hdr(-ext_len);
+      /* Update length field and rest of packer to the "left" */
+      UIP_IP_BUF->len[0] = ((uip_len - UIP_IPH_LEN) >> 8);
+      UIP_IP_BUF->len[1] = ((uip_len - UIP_IPH_LEN) & 0xff);
+      memmove(next_header, next_header + ext_len, uip_len - (next_header - UIP_IP_BUF_CHAR));
+      /* Update loop variables */
+      protocol = *prev_proto_ptr;
+    } else {
+      /* move to the ext hdr */
+      next_header = uipbuf_get_next_header(next_header, uip_len - (next_header - UIP_IP_BUF_CHAR), &protocol, false);
+      ext_ptr = (struct uip_ext_hdr *)next_header;
+      prev_proto_ptr = &ext_ptr->next;
     }
   }
 }
diff --git a/os/net/routing/rpl-classic/rpl.h b/os/net/routing/rpl-classic/rpl.h
index db87cc824..170c97e58 100644
--- a/os/net/routing/rpl-classic/rpl.h
+++ b/os/net/routing/rpl-classic/rpl.h
@@ -275,7 +275,7 @@ rpl_dag_t *rpl_get_dag(const uip_ipaddr_t *addr);
 rpl_dag_t *rpl_get_any_dag(void);
 rpl_instance_t *rpl_get_instance(uint8_t instance_id);
 int rpl_ext_header_update(void);
-int rpl_ext_header_hbh_update(int);
+int rpl_ext_header_hbh_update(int, int);
 void rpl_insert_header(void);
 void rpl_ext_header_remove(void);
 const struct link_stats *rpl_get_parent_link_stats(rpl_parent_t *p);
diff --git a/os/net/routing/rpl-lite/rpl-ext-header.c b/os/net/routing/rpl-lite/rpl-ext-header.c
index 02a681476..26d13c8ec 100644
--- a/os/net/routing/rpl-lite/rpl-ext-header.c
+++ b/os/net/routing/rpl-lite/rpl-ext-header.c
@@ -57,31 +57,12 @@
 int
 rpl_ext_header_srh_get_next_hop(uip_ipaddr_t *ipaddr)
 {
-  uint8_t *uip_next_hdr;
-  int last_uip_ext_len = uip_ext_len;
+  struct uip_routing_hdr *rh_header;
   uip_sr_node_t *dest_node;
   uip_sr_node_t *root_node;
 
-  uip_ext_len = 0;
-  uip_next_hdr = &UIP_IP_BUF->proto;
-
-  /* Look for routing header */
-  while(uip_next_hdr != NULL && *uip_next_hdr != UIP_PROTO_ROUTING) {
-    switch(*uip_next_hdr) {
-      case UIP_PROTO_HBHO:
-      case UIP_PROTO_DESTO:
-        /*
-         * As per RFC 2460, only the Hop-by-Hop Options header and
-         * Destination Options header can appear before the Routing header.
-         */
-        uip_next_hdr = &UIP_EXT_BUF(uip_ext_len)->next;
-        uip_ext_len += (UIP_EXT_BUF(uip_ext_len)->len << 3) + 8;
-        break;
-      default:
-        uip_next_hdr = NULL;
-        break;
-    }
-  }
+  /* Look for routing ext header */
+  rh_header = (struct uip_routing_hdr *)uipbuf_search_header(UIP_IP_BUF_CHAR, uip_len, UIP_PROTO_ROUTING);
 
   if(!rpl_is_addr_in_our_dag(&UIP_IP_BUF->destipaddr)) {
     return 0;
@@ -90,8 +71,7 @@ rpl_ext_header_srh_get_next_hop(uip_ipaddr_t *ipaddr)
   root_node = uip_sr_get_node(NULL, &curr_instance.dag.dag_id);
   dest_node = uip_sr_get_node(NULL, &UIP_IP_BUF->destipaddr);
 
-  if((uip_next_hdr != NULL && *uip_next_hdr == UIP_PROTO_ROUTING
-      && UIP_RH_BUF(uip_ext_len)->routing_type == RPL_RH_TYPE_SRH) ||
+  if((rh_header != NULL && rh_header->routing_type == RPL_RH_TYPE_SRH) ||
      (dest_node != NULL && root_node != NULL &&
       dest_node->parent == root_node)) {
     /* Routing header found or the packet destined for a direct child of the root.
@@ -100,20 +80,18 @@ rpl_ext_header_srh_get_next_hop(uip_ipaddr_t *ipaddr)
      * forwarding to next hop */
     uip_ipaddr_copy(ipaddr, &UIP_IP_BUF->destipaddr);
     uip_create_linklocal_prefix(ipaddr);
-    uip_ext_len = last_uip_ext_len;
     return 1;
   }
 
   LOG_DBG("no SRH found\n");
-  uip_ext_len = last_uip_ext_len;
   return 0;
 }
 /*---------------------------------------------------------------------------*/
 int
 rpl_ext_header_srh_update(void)
 {
-  uint8_t *uip_next_hdr;
-  int last_uip_ext_len = uip_ext_len;
+  struct uip_routing_hdr *rh_header;
+  struct uip_rpl_srh_hdr *srh_header;
   uint8_t cmpri, cmpre;
   uint8_t ext_len;
   uint8_t padding;
@@ -121,40 +99,21 @@ rpl_ext_header_srh_update(void)
   uint8_t segments_left;
   uip_ipaddr_t current_dest_addr;
 
-  uip_ext_len = 0;
-  uip_next_hdr = &UIP_IP_BUF->proto;
+  /* Look for routing ext header */
+  rh_header = (struct uip_routing_hdr *)uipbuf_search_header(UIP_IP_BUF_CHAR, uip_len, UIP_PROTO_ROUTING);
 
-  /* Look for routing header */
-  while(uip_next_hdr != NULL && *uip_next_hdr != UIP_PROTO_ROUTING) {
-    switch(*uip_next_hdr) {
-      case UIP_PROTO_HBHO:
-      case UIP_PROTO_DESTO:
-        /*
-         * As per RFC 2460, only the Hop-by-Hop Options header and
-         * Destination Options header can appear before the Routing header.
-         */
-        uip_next_hdr = &UIP_EXT_BUF(uip_ext_len)->next;
-        uip_ext_len += (UIP_EXT_BUF(uip_ext_len)->len << 3) + 8;
-        break;
-      default:
-        uip_next_hdr = NULL;
-        break;
-    }
-  }
-
-  if(uip_next_hdr == NULL || *uip_next_hdr != UIP_PROTO_ROUTING
-      || UIP_RH_BUF(uip_ext_len)->routing_type != RPL_RH_TYPE_SRH) {
+  if(rh_header == NULL || rh_header->routing_type != RPL_RH_TYPE_SRH) {
     LOG_INFO("SRH not found\n");
-    uip_ext_len = last_uip_ext_len;
     return 0;
   }
 
   /* Parse SRH */
-  segments_left = UIP_RH_BUF(uip_ext_len)->seg_left;
-  ext_len = (UIP_RH_BUF(uip_ext_len)->len * 8) + 8;
-  cmpri = UIP_RPL_SRH_BUF(uip_ext_len)->cmpr >> 4;
-  cmpre = UIP_RPL_SRH_BUF(uip_ext_len)->cmpr & 0x0f;
-  padding = UIP_RPL_SRH_BUF(uip_ext_len)->pad >> 4;
+  srh_header = (struct uip_rpl_srh_hdr *)(((uint8_t *)rh_header) + RPL_RH_LEN);
+  segments_left = rh_header->seg_left;
+  ext_len = rh_header->len * 8 + 8;
+  cmpri = srh_header->cmpr >> 4;
+  cmpre = srh_header->cmpr & 0x0f;
+  padding = srh_header->pad >> 4;
   path_len = ((ext_len - padding - RPL_RH_LEN - RPL_SRH_LEN - (16 - cmpre)) / (16 - cmpri)) + 1;
   (void)path_len;
 
@@ -166,7 +125,7 @@ rpl_ext_header_srh_update(void)
     /* We are the final destination, do nothing */
   } else {
     uint8_t i = path_len - segments_left; /* The index of the next address to be visited */
-    uint8_t *addr_ptr = ((uint8_t *)UIP_RH_BUF(uip_ext_len)) + RPL_RH_LEN + RPL_SRH_LEN + (i * (16 - cmpri));
+    uint8_t *addr_ptr = ((uint8_t *)rh_header) + RPL_RH_LEN + RPL_SRH_LEN + (i * (16 - cmpri));
     uint8_t cmpr = segments_left == 1 ? cmpre : cmpri;
 
     /* As per RFC6554: swap the IPv6 destination address with address[i] */
@@ -179,14 +138,13 @@ rpl_ext_header_srh_update(void)
     memcpy(addr_ptr, ((uint8_t *)&current_dest_addr) + cmpr, 16 - cmpr);
 
     /* Update segments left field */
-    UIP_RH_BUF(uip_ext_len)->seg_left--;
+    rh_header->seg_left--;
 
     LOG_INFO("SRH next hop ");
     LOG_INFO_6ADDR(&UIP_IP_BUF->destipaddr);
     LOG_INFO_("\n");
   }
 
-  uip_ext_len = last_uip_ext_len;
   return 1;
 }
 /*---------------------------------------------------------------------------*/
@@ -293,7 +251,7 @@ insert_srh_header(void)
       path_len, cmpri, cmpre, ext_len, padding);
 
   /* Check if there is enough space to store the extension header */
-  if(uip_len + ext_len > UIP_BUFSIZE - UIP_LLH_LEN) {
+  if(uip_len + ext_len > UIP_LINK_MTU) {
     LOG_ERR("packet too long: impossible to add source routing header (%u bytes)\n", ext_len);
     return 0;
   }
@@ -303,23 +261,23 @@ insert_srh_header(void)
       uip_buf + uip_l2_l3_hdr_len, uip_len - UIP_IPH_LEN);
   memset(uip_buf + uip_l2_l3_hdr_len, 0, ext_len);
 
-  /* Insert source routing header */
-  UIP_RH_BUF(uip_ext_len)->next = UIP_IP_BUF->proto;
+  /* Insert source routing header (as first ext header) */
+  UIP_RH_BUF(0)->next = UIP_IP_BUF->proto;
   UIP_IP_BUF->proto = UIP_PROTO_ROUTING;
 
   /* Initialize IPv6 Routing Header */
-  UIP_RH_BUF(uip_ext_len)->len = (ext_len - 8) / 8;
-  UIP_RH_BUF(uip_ext_len)->routing_type = RPL_RH_TYPE_SRH;
-  UIP_RH_BUF(uip_ext_len)->seg_left = path_len;
+  UIP_RH_BUF(0)->len = (ext_len - 8) / 8;
+  UIP_RH_BUF(0)->routing_type = RPL_RH_TYPE_SRH;
+  UIP_RH_BUF(0)->seg_left = path_len;
 
   /* Initialize RPL Source Routing Header */
-  UIP_RPL_SRH_BUF(uip_ext_len)->cmpr = (cmpri << 4) + cmpre;
-  UIP_RPL_SRH_BUF(uip_ext_len)->pad = padding << 4;
+  UIP_RPL_SRH_BUF(0)->cmpr = (cmpri << 4) + cmpre;
+  UIP_RPL_SRH_BUF(0)->pad = padding << 4;
 
   /* Initialize addresses field (the actual source route).
    * From last to first. */
   node = dest_node;
-  hop_ptr = ((uint8_t *)UIP_RH_BUF(uip_ext_len)) + ext_len - padding; /* Pointer where to write the next hop compressed address */
+  hop_ptr = ((uint8_t *)UIP_RH_BUF(0)) + ext_len - padding; /* Pointer where to write the next hop compressed address */
 
   while(node != NULL && node->parent != root_node) {
     NETSTACK_ROUTING.get_sr_node_ipaddr(&node_addr, node);
@@ -341,14 +299,13 @@ insert_srh_header(void)
     UIP_IP_BUF->len[0]++;
   }
 
-  uip_ext_len += ext_len;
-  uip_len += ext_len;
+  uipbuf_add_ext_hdr(ext_len);
 
   return 1;
 }
 /*---------------------------------------------------------------------------*/
 int
-rpl_ext_header_hbh_update(int uip_ext_opt_offset)
+rpl_ext_header_hbh_update(int ext_offset, int opt_offset)
 {
   int down;
   int rank_error_signaled;
@@ -356,32 +313,32 @@ rpl_ext_header_hbh_update(int uip_ext_opt_offset)
   uint16_t sender_rank;
   uint8_t sender_closer;
   rpl_nbr_t *sender;
-  uint8_t opt_type = UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type;
-  uint8_t opt_len = UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len;
+  uint8_t opt_type = UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->opt_type;
+  uint8_t opt_len = UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->opt_len;
 
-  if(UIP_HBHO_BUF(uip_ext_len)->len != ((RPL_HOP_BY_HOP_LEN - 8) / 8)
+  if(UIP_HBHO_BUF(ext_offset)->len != ((RPL_HOP_BY_HOP_LEN - 8) / 8)
       || opt_type != UIP_EXT_HDR_OPT_RPL
       || opt_len != RPL_HDR_OPT_LEN) {
     LOG_ERR("hop-by-hop extension header has wrong size or type (%u %u %u)\n",
-        UIP_HBHO_BUF(uip_ext_len)->len, opt_type, opt_len);
+        UIP_HBHO_BUF(ext_offset)->len, opt_type, opt_len);
     return 0; /* Drop */
   }
 
-  if(!curr_instance.used || curr_instance.instance_id != UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance) {
+  if(!curr_instance.used || curr_instance.instance_id != UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->instance) {
     LOG_ERR("unknown instance: %u\n",
-           UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance);
+           UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->instance);
     return 0; /* Drop */
   }
 
-  if(UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags & RPL_HDR_OPT_FWD_ERR) {
+  if(UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags & RPL_HDR_OPT_FWD_ERR) {
     LOG_ERR("forward error!\n");
     return 0; /* Drop */
   }
 
-  down = (UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags & RPL_HDR_OPT_DOWN) ? 1 : 0;
-  sender_rank = UIP_HTONS(UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->senderrank);
+  down = (UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags & RPL_HDR_OPT_DOWN) ? 1 : 0;
+  sender_rank = UIP_HTONS(UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->senderrank);
   sender = nbr_table_get_from_lladdr(rpl_neighbors, packetbuf_addr(PACKETBUF_ADDR_SENDER));
-  rank_error_signaled = (UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags & RPL_HDR_OPT_RANK_ERR) ? 1 : 0;
+  rank_error_signaled = (UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags & RPL_HDR_OPT_RANK_ERR) ? 1 : 0;
   sender_closer = sender_rank < curr_instance.dag.rank;
   loop_detected = (down && !sender_closer) || (!down && sender_closer);
 
@@ -395,7 +352,7 @@ rpl_ext_header_hbh_update(int uip_ext_opt_offset)
 
   if(loop_detected) {
     /* Set forward error flag */
-    UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags |= RPL_HDR_OPT_RANK_ERR;
+    UIP_EXT_HDR_OPT_RPL_BUF(ext_offset, opt_offset)->flags |= RPL_HDR_OPT_RANK_ERR;
   }
 
   return rpl_process_hbh(sender, sender_rank, loop_detected, rank_error_signaled);
@@ -407,34 +364,27 @@ rpl_ext_header_hbh_update(int uip_ext_opt_offset)
 static int
 update_hbh_header(void)
 {
-  int uip_ext_opt_offset;
-  int last_uip_ext_len;
+  int opt_offset = 2;
 
-  last_uip_ext_len = uip_ext_len;
-  uip_ext_len = 0;
-  uip_ext_opt_offset = 2;
+  if(UIP_IP_BUF->proto == UIP_PROTO_HBHO && UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_type == UIP_EXT_HDR_OPT_RPL) {
+    if(UIP_HBHO_BUF(0)->len != ((RPL_HOP_BY_HOP_LEN - 8) / 8)
+        || UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_len != RPL_HDR_OPT_LEN) {
 
-  if(UIP_IP_BUF->proto == UIP_PROTO_HBHO && UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type == UIP_EXT_HDR_OPT_RPL) {
-    if(UIP_HBHO_BUF(uip_ext_len)->len != ((RPL_HOP_BY_HOP_LEN - 8) / 8)
-        || UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len != RPL_HDR_OPT_LEN) {
-
-      LOG_ERR("hop-by-hop extension header has wrong size (%u %u)\n",
-          UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len, uip_ext_len);
+      LOG_ERR("hop-by-hop extension header has wrong size (%u)\n",
+          UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_len);
       return 0; /* Drop */
     }
 
-    if(!curr_instance.used || curr_instance.instance_id != UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance) {
+    if(!curr_instance.used || curr_instance.instance_id != UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->instance) {
       LOG_ERR("unable to add/update hop-by-hop extension header: incorrect instance\n");
-      uip_ext_len = last_uip_ext_len;
       return 0; /* Drop */
     }
 
     /* Update sender rank and instance, will update flags next */
-    UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->senderrank = UIP_HTONS(curr_instance.dag.rank);
-    UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance = curr_instance.instance_id;
+    UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->senderrank = UIP_HTONS(curr_instance.dag.rank);
+    UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->instance = curr_instance.instance_id;
   }
 
-  uip_ext_len = last_uip_ext_len;
   return 1;
 }
 /*---------------------------------------------------------------------------*/
@@ -445,19 +395,13 @@ update_hbh_header(void)
 static int
 insert_hbh_header(void)
 {
-  int uip_ext_opt_offset;
-  int last_uip_ext_len;
   uint8_t temp_len;
-
-  last_uip_ext_len = uip_ext_len;
-  uip_ext_len = 0;
-  uip_ext_opt_offset = 2;
+  int opt_offset = 2;
 
   /* Insert hop-by-hop header */
   LOG_INFO("creating hop-by-hop option\n");
-  if(uip_len + RPL_HOP_BY_HOP_LEN > UIP_BUFSIZE - UIP_LLH_LEN) {
+  if(uip_len + RPL_HOP_BY_HOP_LEN > UIP_LINK_MTU) {
     LOG_ERR("packet too long: impossible to add hop-by-hop option\n");
-    uip_ext_len = last_uip_ext_len;
     return 0;
   }
 
@@ -465,25 +409,25 @@ insert_hbh_header(void)
   memmove(UIP_IP_PAYLOAD(RPL_HOP_BY_HOP_LEN), UIP_IP_PAYLOAD(0), uip_len - UIP_IPH_LEN);
   memset(UIP_IP_PAYLOAD(0), 0, RPL_HOP_BY_HOP_LEN);
 
-  /* Update IP and HBH protocol and fields */
-  UIP_HBHO_BUF(uip_ext_len)->next = UIP_IP_BUF->proto;
+  /* Insert HBH header (as first ext header) */
+  UIP_HBHO_BUF(0)->next = UIP_IP_BUF->proto;
   UIP_IP_BUF->proto = UIP_PROTO_HBHO;
 
   /* Initialize HBH option */
-  UIP_HBHO_BUF(uip_ext_len)->len = (RPL_HOP_BY_HOP_LEN - 8) / 8;
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type = UIP_EXT_HDR_OPT_RPL;
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_len = RPL_HDR_OPT_LEN;
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->flags = 0;
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->senderrank = UIP_HTONS(curr_instance.dag.rank);
-  UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->instance = curr_instance.instance_id;
-  uip_len += RPL_HOP_BY_HOP_LEN;
+  UIP_HBHO_BUF(0)->len = (RPL_HOP_BY_HOP_LEN - 8) / 8;
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_type = UIP_EXT_HDR_OPT_RPL;
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->opt_len = RPL_HDR_OPT_LEN;
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->flags = 0;
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->senderrank = UIP_HTONS(curr_instance.dag.rank);
+  UIP_EXT_HDR_OPT_RPL_BUF(0, opt_offset)->instance = curr_instance.instance_id;
+
   temp_len = UIP_IP_BUF->len[1];
   UIP_IP_BUF->len[1] += RPL_HOP_BY_HOP_LEN;
   if(UIP_IP_BUF->len[1] < temp_len) {
     UIP_IP_BUF->len[0]++;
   }
 
-  uip_ext_len = last_uip_ext_len + RPL_HOP_BY_HOP_LEN;
+  uipbuf_add_ext_hdr(RPL_HOP_BY_HOP_LEN);
 
   /* Update header before returning */
   return update_hbh_header();
@@ -520,54 +464,35 @@ rpl_ext_header_update(void)
 void
 rpl_ext_header_remove(void)
 {
-  uint8_t temp_len;
-  uint8_t rpl_ext_hdr_len;
-  int uip_ext_opt_offset;
-  uint8_t *uip_next_hdr;
+  uint8_t *prev_proto_ptr;
+  uint8_t protocol;
+  uint8_t ext_len;
+  uint8_t *next_header;
+  struct uip_ext_hdr *ext_ptr;
+  struct uip_ext_hdr_opt *opt_ptr;
 
-  uip_ext_len = 0;
-  uip_ext_opt_offset = 2;
-  uip_next_hdr = &UIP_IP_BUF->proto;
-
-  /* Look for hop-by-hop and routing headers */
-  while(uip_next_hdr != NULL) {
-    switch(*uip_next_hdr) {
-      case UIP_PROTO_HBHO:
-      case UIP_PROTO_ROUTING:
-        if((*uip_next_hdr != UIP_PROTO_HBHO || UIP_EXT_HDR_OPT_RPL_BUF(uip_ext_len, uip_ext_opt_offset)->opt_type == UIP_EXT_HDR_OPT_RPL)) {
-          /* Remove hop-by-hop and routing headers */
-          *uip_next_hdr = UIP_EXT_BUF(uip_ext_len)->next;
-          rpl_ext_hdr_len = (UIP_EXT_BUF(uip_ext_len)->len * 8) + 8;
-          temp_len = UIP_IP_BUF->len[1];
-          uip_len -= rpl_ext_hdr_len;
-          UIP_IP_BUF->len[1] -= rpl_ext_hdr_len;
-          if(UIP_IP_BUF->len[1] > temp_len) {
-            UIP_IP_BUF->len[0]--;
-          }
-          LOG_INFO("removing RPL extension header (type %u, len %u)\n", *uip_next_hdr, rpl_ext_hdr_len);
-          memmove(UIP_EXT_BUF(uip_ext_len), ((uint8_t *)UIP_EXT_BUF(uip_ext_len)) + rpl_ext_hdr_len, uip_len - UIP_IPH_LEN);
-        } else {
-          uip_next_hdr = &UIP_EXT_BUF(uip_ext_len)->next;
-          uip_ext_len += (UIP_EXT_BUF(uip_ext_len)->len << 3) + 8;
-        }
-        break;
-      case UIP_PROTO_DESTO:
-        /*
-         * As per RFC 2460, any header other than the Destination
-         * Options header does not appear between the Hop-by-Hop
-         * Options header and the Routing header.
-         *
-         * We're moving to the next header only if uip_next_hdr has
-         * UIP_PROTO_DESTO. Otherwise, we'll return.
-         */
-        /* Move to next header */
-        uip_next_hdr = &UIP_EXT_BUF(uip_ext_len)->next;
-        uip_ext_len += (UIP_EXT_BUF(uip_ext_len)->len << 3) + 8;
-        break;
-    default:
-      return;
+  next_header = uipbuf_get_next_header(UIP_IP_BUF_CHAR, uip_len, &protocol, true);
+  ext_ptr = (struct uip_ext_hdr *)next_header;
+  prev_proto_ptr = &UIP_IP_BUF->proto;
+  while(next_header != NULL && uip_is_proto_ext_hdr(protocol)) {
+    opt_ptr = (struct uip_ext_hdr_opt *)(next_header + 2);
+    if(protocol == UIP_PROTO_ROUTING || (protocol == UIP_PROTO_HBHO && opt_ptr->type == UIP_EXT_HDR_OPT_RPL)) {
+      /* Remove ext header */
+      *prev_proto_ptr = ext_ptr->next;
+      ext_len = ext_ptr->len * 8 + 8;
+      uipbuf_add_ext_hdr(-ext_len);
+      /* Update length field and rest of packer to the "left" */
+      UIP_IP_BUF->len[0] = ((uip_len - UIP_IPH_LEN) >> 8);
+      UIP_IP_BUF->len[1] = ((uip_len - UIP_IPH_LEN) & 0xff);
+      memmove(next_header, next_header + ext_len, uip_len - (next_header - UIP_IP_BUF_CHAR));
+      /* Update loop variables */
+      protocol = *prev_proto_ptr;
+    } else {
+      /* move to the ext hdr */
+      next_header = uipbuf_get_next_header(next_header, uip_len - (next_header - UIP_IP_BUF_CHAR), &protocol, false);
+      ext_ptr = (struct uip_ext_hdr *)next_header;
+      prev_proto_ptr = &ext_ptr->next;
     }
   }
 }
-
 /** @}*/
diff --git a/os/net/routing/rpl-lite/rpl-ext-header.h b/os/net/routing/rpl-lite/rpl-ext-header.h
index df61bc325..7576c6a57 100644
--- a/os/net/routing/rpl-lite/rpl-ext-header.h
+++ b/os/net/routing/rpl-lite/rpl-ext-header.h
@@ -63,12 +63,14 @@ int rpl_ext_header_srh_update(void);
 * Process and update the RPL hop-by-hop extension headers of
 * the current uIP packet.
 *
-* \param uip_ext_opt_offset The offset within the uIP packet where
+* \param ext_offset The offset within the uIP packet where
 * extension headers start
+* \param opt_offset The offset within the extension header where
+* the option starts
 * \return 1 in case the packet is valid and to be processed further,
 * 0 in case the packet must be dropped.
 */
-int rpl_ext_header_hbh_update(int uip_ext_opt_offset);
+int rpl_ext_header_hbh_update(int ext_offset, int opt_offset);
 
 /**
  * Adds/updates all RPL extension headers to current uIP packet.