9 files changed, 310 insertions, 303 deletions

diff --git a/src/vnet/ip/ip6_format.c b/src/vnet/ip/ip6_format.c
index 1b8ff1e0ab0..1a1bef26aa6 100644
--- a/src/vnet/ip/ip6_format.c
+++ b/src/vnet/ip/ip6_format.c
@@ -288,7 +288,7 @@ format_ip6_header (u8 * s, va_list * args)
 	    "\n%Utos 0x%02x, flow label 0x%x, hop limit %d, payload length %d",
 	    format_white_space, indent, traffic_class, flow_label,
 	    ip->hop_limit, clib_net_to_host_u16 (ip->payload_length));
-
+#if 0
   /* Recurse into next protocol layer. */
   if (max_header_bytes != 0 && sizeof (ip[0]) < max_header_bytes)
     {
@@ -301,7 +301,7 @@ format_ip6_header (u8 * s, va_list * args)
 		    /* next protocol header */ (void *) (ip + 1),
 		    max_header_bytes - sizeof (ip[0]));
     }
-
+#endif
   return s;
 }
 
diff --git a/src/vnet/ip/ip6_forward.c b/src/vnet/ip/ip6_forward.c
index 9ee3d11cef2..5b7704e460e 100644
--- a/src/vnet/ip/ip6_forward.c
+++ b/src/vnet/ip/ip6_forward.c
@@ -1227,14 +1227,11 @@ always_inline u8
 ip6_next_proto_is_tcp_udp (vlib_buffer_t * p0, ip6_header_t * ip0,
 			   u32 * udp_offset0)
 {
-  u32 proto0;
-  proto0 = ip6_locate_header (p0, ip0, IP_PROTOCOL_UDP, udp_offset0);
-  if (proto0 != IP_PROTOCOL_UDP)
-    {
-      proto0 = ip6_locate_header (p0, ip0, IP_PROTOCOL_TCP, udp_offset0);
-      proto0 = (proto0 == IP_PROTOCOL_TCP) ? proto0 : 0;
-    }
-  return proto0;
+  int nh = ip6_locate_header (p0, ip0, -1, udp_offset0);
+  if (nh > 0)
+    if (nh == IP_PROTOCOL_UDP || nh == IP_PROTOCOL_TCP)
+      return nh;
+  return 0;
 }
 
 /* *INDENT-OFF* */
diff --git a/src/vnet/ip/ip6_inlines.h b/src/vnet/ip/ip6_inlines.h
index 2a4bb70573b..9c2be60b267 100644
--- a/src/vnet/ip/ip6_inlines.h
+++ b/src/vnet/ip/ip6_inlines.h
@@ -134,65 +134,17 @@ ip6_compute_flow_hash (const ip6_header_t * ip,
  *      it is a non-first fragment -1 is returned.
  */
 always_inline int
-ip6_locate_header (vlib_buffer_t * p0,
-		   ip6_header_t * ip0, int find_hdr_type, u32 * offset)
+ip6_locate_header (vlib_buffer_t *b, ip6_header_t *ip, int find_hdr_type,
+		   u32 *offset)
 {
-  u8 next_proto = ip0->protocol;
-  u8 *next_header;
-  u8 done = 0;
-  u32 cur_offset;
-  u8 *temp_nxthdr = 0;
-  u32 exthdr_len = 0;
-
-  next_header = ip6_next_header (ip0);
-  cur_offset = sizeof (ip6_header_t);
-  while (1)
+  ip6_ext_hdr_chain_t hdr_chain;
+  int res = ip6_ext_header_walk (b, ip, find_hdr_type, &hdr_chain);
+  if (res >= 0)
     {
-      done = (next_proto == find_hdr_type);
-      if (PREDICT_FALSE
-	  (next_header >=
-	   (u8 *) vlib_buffer_get_current (p0) + p0->current_length))
-	{
-	  //A malicious packet could set an extension header with a too big size
-	  return (-1);
-	}
-      if (done)
-	break;
-      if ((!ip6_ext_hdr (next_proto)) || next_proto == IP_PROTOCOL_IP6_NONXT)
-	{
-	  if (find_hdr_type < 0)
-	    break;
-	  return -1;
-	}
-      if (next_proto == IP_PROTOCOL_IPV6_FRAGMENTATION)
-	{
-	  ip6_frag_hdr_t *frag_hdr = (ip6_frag_hdr_t *) next_header;
-	  u16 frag_off = ip6_frag_hdr_offset (frag_hdr);
-	  /* Non first fragment return -1 */
-	  if (frag_off)
-	    return (-1);
-	  exthdr_len = sizeof (ip6_frag_hdr_t);
-	  temp_nxthdr = next_header + exthdr_len;
-	}
-      else if (next_proto == IP_PROTOCOL_IPSEC_AH)
-	{
-	  exthdr_len =
-	    ip6_ext_authhdr_len (((ip6_ext_header_t *) next_header));
-	  temp_nxthdr = next_header + exthdr_len;
-	}
-      else
-	{
-	  exthdr_len =
-	    ip6_ext_header_len (((ip6_ext_header_t *) next_header));
-	  temp_nxthdr = next_header + exthdr_len;
-	}
-      next_proto = ((ip6_ext_header_t *) next_header)->next_hdr;
-      next_header = temp_nxthdr;
-      cur_offset += exthdr_len;
+      *offset = hdr_chain.eh[res].offset;
+      return hdr_chain.eh[res].protocol;
     }
-
-  *offset = cur_offset;
-  return (next_proto);
+  return -1;
 }
 
 
diff --git a/src/vnet/ip/ip6_packet.h b/src/vnet/ip/ip6_packet.h
index 7a8c31cee48..7f337a61be6 100644
--- a/src/vnet/ip/ip6_packet.h
+++ b/src/vnet/ip/ip6_packet.h
@@ -42,6 +42,7 @@
 
 #include <vnet/tcp/tcp_packet.h>
 #include <vnet/ip/ip4_packet.h>
+#include <stdbool.h>
 
 typedef union
 {
@@ -62,13 +63,11 @@ typedef struct
 } ip6_address_and_mask_t;
 
 /* Packed so that the mhash key doesn't include uninitialized pad bytes */
-/* *INDENT-OFF* */
 typedef CLIB_PACKED (struct {
   /* IP address must be first for ip_interface_address_get_address() to work */
   ip6_address_t ip6_addr;
   u32 fib_index;
 }) ip6_address_fib_t;
-/* *INDENT-ON* */
 
 always_inline void
 ip6_addr_fib_init (ip6_address_fib_t * addr_fib,
@@ -477,44 +476,32 @@ ip6_tcp_reply_x2 (ip6_header_t * ip0, ip6_header_t * ip1,
   }
 }
 
-
-/* *INDENT-OFF* */
 typedef CLIB_PACKED (struct {
   u8 data;
 }) ip6_pad1_option_t;
-/* *INDENT-ON* */
 
-/* *INDENT-OFF* */
 typedef CLIB_PACKED (struct {
   u8 type;
   u8 len;
   u8 data[0];
 }) ip6_padN_option_t;
-/* *INDENT-ON* */
 
-/* *INDENT-OFF* */
 typedef CLIB_PACKED (struct {
 #define IP6_MLDP_ALERT_TYPE  0x5
   u8 type;
   u8 len;
   u16 value;
 }) ip6_router_alert_option_t;
-/* *INDENT-ON* */
 
-/* *INDENT-OFF* */
 typedef CLIB_PACKED (struct {
   u8 next_hdr;
   /* Length of this header plus option data in 8 byte units. */
   u8 n_data_u64s;
 }) ip6_ext_header_t;
-/* *INDENT-ON* */
 
 #define foreach_ext_hdr_type \
   _(IP6_HOP_BY_HOP_OPTIONS) \
   _(IPV6_ROUTE) \
-  _(IPV6_FRAGMENTATION) \
-  _(IPSEC_ESP) \
-  _(IPSEC_AH) \
   _(IP6_DESTINATION_OPTIONS) \
   _(MOBILITY) \
   _(HIP) \
@@ -542,15 +529,70 @@ ip6_ext_hdr (u8 nexthdr)
 #endif
 }
 
+typedef CLIB_PACKED (struct {
+  u8 next_hdr;
+  /* Length of this header plus option data in 8 byte units. */
+  u8 n_data_u64s;
+  u8 data[0];
+}) ip6_hop_by_hop_ext_t;
+
+typedef CLIB_PACKED (struct {
+  u8 next_hdr;
+  u8 rsv;
+  u16 fragment_offset_and_more;
+  u32 identification;
+}) ip6_frag_hdr_t;
+
+#define ip6_frag_hdr_offset(hdr)                                              \
+  (clib_net_to_host_u16 ((hdr)->fragment_offset_and_more) >> 3)
+
+#define ip6_frag_hdr_offset_bytes(hdr) (8 * ip6_frag_hdr_offset (hdr))
+
+#define ip6_frag_hdr_more(hdr)                                                \
+  (clib_net_to_host_u16 ((hdr)->fragment_offset_and_more) & 0x1)
+
+#define ip6_frag_hdr_offset_and_more(offset, more)                            \
+  clib_host_to_net_u16 (((offset) << 3) + !!(more))
+
 #define ip6_ext_header_len(p)  ((((ip6_ext_header_t *)(p))->n_data_u64s+1) << 3)
 #define ip6_ext_authhdr_len(p) ((((ip6_ext_header_t *)(p))->n_data_u64s+2) << 2)
 
+static inline int
+ip6_ext_header_len_s (ip_protocol_t nh, void *p)
+{
+  if (ip6_ext_hdr (nh))
+    return ip6_ext_header_len (p);
+  switch (nh)
+    {
+    case IP_PROTOCOL_IPSEC_AH:
+      return ip6_ext_authhdr_len (p);
+    case IP_PROTOCOL_IPV6_FRAGMENTATION:
+      return sizeof (ip6_frag_hdr_t);
+    case IP_PROTOCOL_ICMP6:
+      return 4;
+    case IP_PROTOCOL_UDP:
+      return 8;
+    case IP_PROTOCOL_TCP:
+      return 20;
+    default: /* Caller is responsible for validating the length of terminating
+	     protocols */
+	     ;
+    }
+  return 0;
+}
+
 always_inline void *
 ip6_ext_next_header (ip6_ext_header_t * ext_hdr)
 {
   return (void *) ((u8 *) ext_hdr + ip6_ext_header_len (ext_hdr));
 }
 
+always_inline void *
+ip6_ext_next_header_offset (void *hdr, u16 offset)
+{
+  return (hdr + offset);
+}
+
 always_inline int
 vlib_object_within_buffer_data (vlib_main_t * vm, vlib_buffer_t * b,
 				void *obj, size_t len)
@@ -562,153 +604,151 @@ vlib_object_within_buffer_data (vlib_main_t * vm, vlib_buffer_t * b,
   return 1;
 }
 
-/*
- * find ipv6 extension header within ipv6 header within buffer b
- *
- * @param vm
- * @param b buffer to limit search to
- * @param ip6_header ipv6 header
- * @param header_type extension header type to search for
- * @param[out] prev_ext_header address of header preceding found header
- */
+/* Returns the number of bytes left in buffer from p. */
+static inline u32
+vlib_bytes_left_in_buffer (vlib_buffer_t *b, void *obj)
+{
+  return b->current_length - (((u8 *) obj - b->data) - b->current_data);
+}
+
 always_inline void *
-ip6_ext_header_find (vlib_main_t * vm, vlib_buffer_t * b,
-		     ip6_header_t * ip6_header, u8 header_type,
-		     ip6_ext_header_t ** prev_ext_header)
+ip6_ext_next_header_s (ip_protocol_t cur_nh, void *hdr, u32 max_offset,
+		       u32 *offset, int *res_nh, bool *last)
 {
-  ip6_ext_header_t *prev = NULL;
-  ip6_ext_header_t *result = NULL;
-  if ((ip6_header)->protocol == header_type)
+  u16 hdrlen = 0;
+  int new_nh = -1;
+  void *res = 0;
+  if (ip6_ext_hdr (cur_nh))
     {
-      result = (void *) (ip6_header + 1);
-      if (!vlib_object_within_buffer_data (vm, b, result,
-					   ip6_ext_header_len (result)))
-	{
-	  result = NULL;
-	}
+      hdrlen = ip6_ext_header_len (hdr);
+      new_nh = ((ip6_ext_header_t *) hdr)->next_hdr;
+      res = hdr + hdrlen;
+    }
+  else if (cur_nh == IP_PROTOCOL_IPV6_FRAGMENTATION)
+    {
+      ip6_frag_hdr_t *frag_hdr = (ip6_frag_hdr_t *) hdr;
+      if (ip6_frag_hdr_offset (frag_hdr) > 0)
+	*last = true;
+      new_nh = frag_hdr->next_hdr;
+      hdrlen = sizeof (ip6_frag_hdr_t);
+      res = hdr + hdrlen;
+    }
+  else if (cur_nh == IP_PROTOCOL_IPSEC_AH)
+    {
+      new_nh = ((ip6_ext_header_t *) hdr)->next_hdr;
+      hdrlen = ip6_ext_authhdr_len (hdr);
+      res = hdr + hdrlen;
     }
   else
     {
-      result = NULL;
-      prev = (void *) (ip6_header + 1);
-      while (ip6_ext_hdr (prev->next_hdr) && prev->next_hdr != header_type)
-	{
-	  prev = ip6_ext_next_header (prev);
-	  if (!vlib_object_within_buffer_data (vm, b, prev,
-					       ip6_ext_header_len (prev)))
-	    {
-	      prev = NULL;
-	      break;
-	    }
-	}
-      if (prev && (prev->next_hdr == header_type))
-	{
-	  result = ip6_ext_next_header (prev);
-	  if (!vlib_object_within_buffer_data (vm, b, result,
-					       ip6_ext_header_len (result)))
-	    {
-	      result = NULL;
-	    }
-	}
+      ;
     }
-  if (prev_ext_header)
+
+  if (res && (*offset + hdrlen) >= max_offset)
     {
-      *prev_ext_header = prev;
+      return 0;
     }
-  return result;
+  *res_nh = new_nh;
+  *offset += hdrlen;
+  return res;
 }
 
+#define IP6_EXT_HDR_MAX	      (4)   /* Maximum number of headers */
+#define IP6_EXT_HDR_MAX_DEPTH (256) /* Maximum header depth */
+typedef struct
+{
+  int length;
+  struct
+  {
+    u16 protocol;
+    u16 offset;
+  } eh[IP6_EXT_HDR_MAX];
+} ip6_ext_hdr_chain_t;
+
 /*
- * walk extension headers, looking for a specific extension header and last
- * extension header, calculating length of all extension headers
+ * find ipv6 extension header within ipv6 header within
+ * whichever is smallest of buffer or IP6_EXT_HDR_MAX_DEPTH.
+ * The complete header chain must be in first buffer.
  *
- * @param vm
- * @param b buffer to limit search to
- * @param ip6_header ipv6 header
- * @param find_hdr extension header to look for (ignored if ext_hdr is NULL)
- * @param length[out] length of all extension headers
- * @param ext_hdr[out] extension header of type find_hdr (may be NULL)
- * @param last_ext_hdr[out] last extension header (may be NULL)
- *
- * @return 0 on success, -1 on failure (ext headers crossing buffer boundary)
+ * The complete header chain (up to the terminating header) is
+ * returned in res.
+ * Returns the index of the find_hdr_type if > 0. Otherwise
+ * it returns the index of the last header.
  */
 always_inline int
-ip6_walk_ext_hdr (vlib_main_t * vm, vlib_buffer_t * b,
-		  const ip6_header_t * ip6_header, u8 find_hdr, u32 * length,
-		  ip6_ext_header_t ** ext_hdr,
-		  ip6_ext_header_t ** last_ext_hdr)
-{
-  if (!ip6_ext_hdr (ip6_header->protocol))
+ip6_ext_header_walk (vlib_buffer_t *b, ip6_header_t *ip, int find_hdr_type,
+		     ip6_ext_hdr_chain_t *res)
+{
+  int i = 0;
+  int found = -1;
+  void *next_header = ip6_next_header (ip);
+  int next_proto = ip->protocol;
+  res->length = 0;
+  u32 n_bytes_this_buffer =
+    clib_min (vlib_bytes_left_in_buffer (b, ip), IP6_EXT_HDR_MAX_DEPTH);
+  u32 max_offset = clib_min (n_bytes_this_buffer,
+			     sizeof (ip6_header_t) +
+			       clib_net_to_host_u16 (ip->payload_length));
+  u32 offset = sizeof (ip6_header_t);
+  if ((ip6_ext_header_len_s (ip->protocol, next_header) + offset) > max_offset)
     {
-      *length = 0;
-      *ext_hdr = NULL;
-      *last_ext_hdr = NULL;
-      return 0;
+      return -1;
     }
-  *length = 0;
-  ip6_ext_header_t *h = (void *) (ip6_header + 1);
-  if (!vlib_object_within_buffer_data (vm, b, h, ip6_ext_header_len (h)))
+  bool last = false;
+  while (next_header)
     {
-      return -1;
+      /* Move on to next header */
+      res->eh[i].offset = offset;
+      res->eh[i].protocol = next_proto;
+      if (next_proto == find_hdr_type)
+	found = i;
+      i++;
+      if (last)
+	break;
+      if (i > IP6_EXT_HDR_MAX)
+	break;
+      next_header = ip6_ext_next_header_s (next_proto, next_header, max_offset,
+					   &offset, &next_proto, &last);
     }
-  *length += ip6_ext_header_len (h);
-  *last_ext_hdr = h;
-  *ext_hdr = NULL;
-  if (ip6_header->protocol == find_hdr)
+  if (ip6_ext_hdr (res->eh[i].protocol))
     {
-      *ext_hdr = h;
+      /* Header chain is not terminated */
+      ;
     }
-  while (ip6_ext_hdr (h->next_hdr))
+  res->length = i;
+  if (find_hdr_type < 0)
     {
-      if (h->next_hdr == find_hdr)
+      return i - 1;
+    }
+  return found != -1 ? found : i - 1;
+}
+
+always_inline void *
+ip6_ext_header_find (vlib_main_t *vm, vlib_buffer_t *b, ip6_header_t *ip,
+		     int find_hdr_type, ip6_ext_header_t **prev_ext_header)
+{
+  ip6_ext_hdr_chain_t hdr_chain;
+  int res = ip6_ext_header_walk (b, ip, find_hdr_type, &hdr_chain);
+  if (res < 0)
+    return 0;
+
+  if (prev_ext_header)
+    {
+      if (res > 0)
 	{
-	  h = ip6_ext_next_header (h);
-	  *ext_hdr = h;
+	  *prev_ext_header =
+	    ip6_ext_next_header_offset (ip, hdr_chain.eh[res - 1].offset);
 	}
       else
 	{
-	  h = ip6_ext_next_header (h);
-	}
-      if (!vlib_object_within_buffer_data (vm, b, h, ip6_ext_header_len (h)))
-	{
-	  return -1;
+	  *prev_ext_header = 0;
 	}
-      *length += ip6_ext_header_len (h);
-      *last_ext_hdr = h;
     }
+  if (find_hdr_type == hdr_chain.eh[res].protocol)
+    return ip6_ext_next_header_offset (ip, hdr_chain.eh[res].offset);
   return 0;
 }
 
-/* *INDENT-OFF* */
-typedef CLIB_PACKED (struct {
-  u8 next_hdr;
-  /* Length of this header plus option data in 8 byte units. */
-  u8 n_data_u64s;
-  u8 data[0];
-}) ip6_hop_by_hop_ext_t;
-/* *INDENT-ON* */
-
-/* *INDENT-OFF* */
-typedef CLIB_PACKED (struct {
-  u8 next_hdr;
-  u8 rsv;
-  u16 fragment_offset_and_more;
-  u32 identification;
-}) ip6_frag_hdr_t;
-/* *INDENT-ON* */
-
-#define ip6_frag_hdr_offset(hdr) \
-  (clib_net_to_host_u16((hdr)->fragment_offset_and_more) >> 3)
-
-#define ip6_frag_hdr_offset_bytes(hdr) \
-  (8 * ip6_frag_hdr_offset(hdr))
-
-#define ip6_frag_hdr_more(hdr) \
-  (clib_net_to_host_u16((hdr)->fragment_offset_and_more) & 0x1)
-
-#define ip6_frag_hdr_offset_and_more(offset, more) \
-  clib_host_to_net_u16(((offset) << 3) + !!(more))
-
 #endif /* included_ip6_packet_h */
 
 /*
diff --git a/src/vnet/ip/ip6_to_ip4.h b/src/vnet/ip/ip6_to_ip4.h
index 6a533e3b54e..b1b5bdb2d11 100644
--- a/src/vnet/ip/ip6_to_ip4.h
+++ b/src/vnet/ip/ip6_to_ip4.h
@@ -62,41 +62,25 @@ static u8 icmp6_to_icmp_updater_pointer_table[] =
  * @returns 0 on success, non-zero value otherwise.
  */
 static_always_inline int
-ip6_parse (vlib_main_t * vm, vlib_buffer_t * b, const ip6_header_t * ip6,
-	   u32 buff_len, u8 * l4_protocol, u16 * l4_offset,
-	   u16 * frag_hdr_offset)
+ip6_parse (vlib_main_t *vm, vlib_buffer_t *b, ip6_header_t *ip6, u32 buff_len,
+	   u8 *l4_protocol, u16 *l4_offset, u16 *frag_hdr_offset)
 {
-  ip6_ext_header_t *last_hdr, *frag_hdr;
-  u32 length;
-  if (ip6_walk_ext_hdr
-      (vm, b, ip6, IP_PROTOCOL_IPV6_FRAGMENTATION, &length, &frag_hdr,
-       &last_hdr))
+  ip6_ext_hdr_chain_t hdr_chain;
+  int res =
+    ip6_ext_header_walk (b, ip6, IP_PROTOCOL_IPV6_FRAGMENTATION, &hdr_chain);
+  if (res < 0)
     {
       return -1;
     }
-
-  if (length > 0)
-    {
-      if (frag_hdr)
-	{
-	  *frag_hdr_offset = (u8 *) frag_hdr - (u8 *) ip6;
-	}
-      else
-	{
-	  *frag_hdr_offset = 0;
-	}
-      *l4_protocol = last_hdr->next_hdr;
-    }
+  if (hdr_chain.eh[res].protocol == IP_PROTOCOL_IPV6_FRAGMENTATION)
+    *frag_hdr_offset = hdr_chain.eh[res].offset;
   else
-    {
-      *frag_hdr_offset = 0;
-      *l4_protocol = ip6->protocol;
-    }
-  *l4_offset = sizeof (*ip6) + length;
+    *frag_hdr_offset = 0;
 
-  return (buff_len < (*l4_offset + 4)) ||
-    (clib_net_to_host_u16 (ip6->payload_length) <
-     (*l4_offset + 4 - sizeof (*ip6)));
+  *l4_protocol = hdr_chain.eh[hdr_chain.length - 1].protocol;
+  *l4_offset = hdr_chain.eh[hdr_chain.length - 1].offset;
+
+  return 0;
 }
 
 /**
@@ -124,13 +108,13 @@ ip6_get_port (vlib_main_t * vm, vlib_buffer_t * b, ip6_header_t * ip6,
   u16 frag_offset;
   u8 *l4;
 
-  if (ip6_parse
-      (vm, b, ip6, buffer_len, &l4_protocol, &l4_offset, &frag_offset))
-    return 0;
-
+  if (ip6_parse (vm, b, ip6, buffer_len, &l4_protocol, &l4_offset,
+		 &frag_offset))
+    {
+      return 0;
+    }
   if (frag_offset &&
-      ip6_frag_hdr_offset (((ip6_frag_hdr_t *)
-			    u8_ptr_add (ip6, frag_offset))))
+      ip6_frag_hdr_offset (((ip6_frag_hdr_t *) u8_ptr_add (ip6, frag_offset))))
     return 0;			//Can't deal with non-first fragment for now
 
   if (ip_protocol)
diff --git a/src/vnet/ip/reass/ip6_full_reass.c b/src/vnet/ip/reass/ip6_full_reass.c
index 8486aea6753..901da99c85d 100644
--- a/src/vnet/ip/reass/ip6_full_reass.c
+++ b/src/vnet/ip/reass/ip6_full_reass.c
@@ -25,6 +25,7 @@
 #include <vnet/ip/ip.h>
 #include <vppinfra/bihash_48_8.h>
 #include <vnet/ip/reass/ip6_full_reass.h>
+#include <vnet/ip/ip6_inlines.h>
 
 #define MSEC_PER_SEC 1000
 #define IP6_FULL_REASS_TIMEOUT_DEFAULT_MS 100
@@ -729,19 +730,27 @@ ip6_full_reass_finalize (vlib_main_t * vm, vlib_node_runtime_t * node,
   vnet_buffer_opaque_t *first_b_vnb = vnet_buffer (first_b);
   ip6_header_t *ip = vlib_buffer_get_current (first_b);
   u16 ip6_frag_hdr_offset = first_b_vnb->ip.reass.ip6_frag_hdr_offset;
-  ip6_ext_header_t *prev_hdr;
-  frag_hdr =
-    ip6_ext_header_find (vm, first_b, ip, IP_PROTOCOL_IPV6_FRAGMENTATION,
-			 &prev_hdr);
-  if (prev_hdr)
+  ip6_ext_hdr_chain_t hdr_chain;
+  ip6_ext_header_t *prev_hdr = 0;
+  int res = ip6_ext_header_walk (first_b, ip, IP_PROTOCOL_IPV6_FRAGMENTATION,
+				 &hdr_chain);
+  if (res < 0 ||
+      (hdr_chain.eh[res].protocol != IP_PROTOCOL_IPV6_FRAGMENTATION))
     {
+      rv = IP6_FULL_REASS_RC_INTERNAL_ERROR;
+      goto free_buffers_and_return;
+    }
+  frag_hdr = ip6_ext_next_header_offset (ip, hdr_chain.eh[res].offset);
+  if (res > 0)
+    {
+      prev_hdr = ip6_ext_next_header_offset (ip, hdr_chain.eh[res - 1].offset);
       prev_hdr->next_hdr = frag_hdr->next_hdr;
     }
   else
     {
       ip->protocol = frag_hdr->next_hdr;
     }
-  if (!((u8 *) frag_hdr - (u8 *) ip == ip6_frag_hdr_offset))
+  if (hdr_chain.eh[res].offset != ip6_frag_hdr_offset)
     {
       rv = IP6_FULL_REASS_RC_INTERNAL_ERROR;
       goto free_buffers_and_return;
@@ -982,22 +991,18 @@ check_if_done_maybe:
 }
 
 always_inline bool
-ip6_full_reass_verify_upper_layer_present (vlib_node_runtime_t * node,
-					   vlib_buffer_t * b,
-					   ip6_frag_hdr_t * frag_hdr)
+ip6_full_reass_verify_upper_layer_present (vlib_node_runtime_t *node,
+					   vlib_buffer_t *b,
+					   ip6_ext_hdr_chain_t *hc)
 {
-  ip6_ext_header_t *tmp = (ip6_ext_header_t *) frag_hdr;
-  while (ip6_ext_hdr (tmp->next_hdr))
+  int nh = hc->eh[hc->length - 1].protocol;
+  /* Checking to see if it's a terminating header */
+  if (ip6_ext_hdr (nh))
     {
-      tmp = ip6_ext_next_header (tmp);
-    }
-  if (IP_PROTOCOL_IP6_NONXT == tmp->next_hdr)
-    {
-      icmp6_error_set_vnet_buffer (b, ICMP6_parameter_problem,
-				   ICMP6_parameter_problem_first_fragment_has_incomplete_header_chain,
-				   0);
+      icmp6_error_set_vnet_buffer (
+	b, ICMP6_parameter_problem,
+	ICMP6_parameter_problem_first_fragment_has_incomplete_header_chain, 0);
       b->error = node->errors[IP6_ERROR_REASS_MISSING_UPPER];
-
       return false;
     }
   return true;
@@ -1076,29 +1081,27 @@ ip6_full_reassembly_inline (vlib_main_t * vm,
 	  b0 = vlib_get_buffer (vm, bi0);
 
 	  ip6_header_t *ip0 = vlib_buffer_get_current (b0);
-	  ip6_frag_hdr_t *frag_hdr = NULL;
-	  ip6_ext_header_t *prev_hdr;
-	  if (ip6_ext_hdr (ip0->protocol))
-	    {
-	      frag_hdr =
-		ip6_ext_header_find (vm, b0, ip0,
-				     IP_PROTOCOL_IPV6_FRAGMENTATION,
-				     &prev_hdr);
-	    }
-	  if (!frag_hdr)
+	  ip6_frag_hdr_t *frag_hdr;
+	  ip6_ext_hdr_chain_t hdr_chain;
+	  int res = ip6_ext_header_walk (
+	    b0, ip0, IP_PROTOCOL_IPV6_FRAGMENTATION, &hdr_chain);
+	  if (res < 0 ||
+	      hdr_chain.eh[res].protocol != IP_PROTOCOL_IPV6_FRAGMENTATION)
 	    {
-	      // this is a regular packet - no fragmentation
-	      next0 = IP6_FULL_REASSEMBLY_NEXT_INPUT;
+	      // this is a mangled packet - no fragmentation
+	      next0 = IP6_FULL_REASSEMBLY_NEXT_DROP;
 	      goto skip_reass;
 	    }
+	  frag_hdr =
+	    ip6_ext_next_header_offset (ip0, hdr_chain.eh[res].offset);
 	  vnet_buffer (b0)->ip.reass.ip6_frag_hdr_offset =
-	    (u8 *) frag_hdr - (u8 *) ip0;
+	    hdr_chain.eh[res].offset;
 
 	  if (0 == ip6_frag_hdr_offset (frag_hdr))
 	    {
 	      // first fragment - verify upper-layer is present
-	      if (!ip6_full_reass_verify_upper_layer_present
-		  (node, b0, frag_hdr))
+	      if (!ip6_full_reass_verify_upper_layer_present (node, b0,
+							      &hdr_chain))
 		{
 		  next0 = IP6_FULL_REASSEMBLY_NEXT_ICMP_ERROR;
 		  goto skip_reass;
diff --git a/src/vnet/ip/reass/ip6_sv_reass.c b/src/vnet/ip/reass/ip6_sv_reass.c
index f18bbe0d564..fb435ba9630 100644
--- a/src/vnet/ip/reass/ip6_sv_reass.c
+++ b/src/vnet/ip/reass/ip6_sv_reass.c
@@ -26,6 +26,7 @@
 #include <vnet/ip/ip6_to_ip4.h>
 #include <vppinfra/bihash_48_8.h>
 #include <vnet/ip/reass/ip6_sv_reass.h>
+#include <vnet/ip/ip6_inlines.h>
 
 #define MSEC_PER_SEC 1000
 #define IP6_SV_REASS_TIMEOUT_DEFAULT_MS 100
@@ -440,22 +441,18 @@ ip6_sv_reass_update (vlib_main_t *vm, vlib_node_runtime_t *node,
 }
 
 always_inline bool
-ip6_sv_reass_verify_upper_layer_present (vlib_node_runtime_t * node,
-					 vlib_buffer_t * b,
-					 ip6_frag_hdr_t * frag_hdr)
+ip6_sv_reass_verify_upper_layer_present (vlib_node_runtime_t *node,
+					 vlib_buffer_t *b,
+					 ip6_ext_hdr_chain_t *hc)
 {
-  ip6_ext_header_t *tmp = (ip6_ext_header_t *) frag_hdr;
-  while (ip6_ext_hdr (tmp->next_hdr))
+  int nh = hc->eh[hc->length - 1].protocol;
+  /* Checking to see if it's a terminating header */
+  if (ip6_ext_hdr (nh))
     {
-      tmp = ip6_ext_next_header (tmp);
-    }
-  if (IP_PROTOCOL_IP6_NONXT == tmp->next_hdr)
-    {
-      icmp6_error_set_vnet_buffer (b, ICMP6_parameter_problem,
-				   ICMP6_parameter_problem_first_fragment_has_incomplete_header_chain,
-				   0);
+      icmp6_error_set_vnet_buffer (
+	b, ICMP6_parameter_problem,
+	ICMP6_parameter_problem_first_fragment_has_incomplete_header_chain, 0);
       b->error = node->errors[IP6_ERROR_REASS_MISSING_UPPER];
-
       return false;
     }
   return true;
@@ -533,16 +530,13 @@ ip6_sv_reassembly_inline (vlib_main_t * vm,
 	  b0 = vlib_get_buffer (vm, bi0);
 
 	  ip6_header_t *ip0 = vlib_buffer_get_current (b0);
-	  ip6_frag_hdr_t *frag_hdr = NULL;
-	  ip6_ext_header_t *prev_hdr;
-	  if (ip6_ext_hdr (ip0->protocol))
-	    {
-	      frag_hdr =
-		ip6_ext_header_find (vm, b0, ip0,
-				     IP_PROTOCOL_IPV6_FRAGMENTATION,
-				     &prev_hdr);
-	    }
-	  if (!frag_hdr)
+	  ip6_frag_hdr_t *frag_hdr;
+	  ip6_ext_hdr_chain_t hdr_chain;
+
+	  int res = ip6_ext_header_walk (
+	    b0, ip0, IP_PROTOCOL_IPV6_FRAGMENTATION, &hdr_chain);
+	  if (res < 0 ||
+	      hdr_chain.eh[res].protocol != IP_PROTOCOL_IPV6_FRAGMENTATION)
 	    {
 	      // this is a regular packet - no fragmentation
 	      if (!ip6_get_port
@@ -571,13 +565,15 @@ ip6_sv_reassembly_inline (vlib_main_t * vm,
 		}
 	      goto packet_enqueue;
 	    }
+	  frag_hdr =
+	    ip6_ext_next_header_offset (ip0, hdr_chain.eh[res].offset);
 	  vnet_buffer (b0)->ip.reass.ip6_frag_hdr_offset =
-	    (u8 *) frag_hdr - (u8 *) ip0;
+	    hdr_chain.eh[res].offset;
 	  if (0 == ip6_frag_hdr_offset (frag_hdr))
 	    {
 	      // first fragment - verify upper-layer is present
-	      if (!ip6_sv_reass_verify_upper_layer_present
-		  (node, b0, frag_hdr))
+	      if (!ip6_sv_reass_verify_upper_layer_present (node, b0,
+							    &hdr_chain))
 		{
 		  next0 = IP6_SV_REASSEMBLY_NEXT_ICMP_ERROR;
 		  goto packet_enqueue;
diff --git a/src/vnet/ipsec/esp_encrypt.c b/src/vnet/ipsec/esp_encrypt.c
index 0d29604e61f..8ac0e1a2be7 100644
--- a/src/vnet/ipsec/esp_encrypt.c
+++ b/src/vnet/ipsec/esp_encrypt.c
@@ -225,9 +225,8 @@ esp_get_ip6_hdr_len (ip6_header_t * ip6, ip6_ext_header_t ** ext_hdr)
       return len;
     }
 
-  p = (void *) (ip6 + 1);
+  p = ip6_next_header (ip6);
   len += ip6_ext_header_len (p);
-
   while (ext_hdr_is_pre_esp (p->next_hdr))
     {
       len += ip6_ext_header_len (p);
@@ -842,16 +841,28 @@ esp_encrypt_inline (vlib_main_t *vm, vlib_node_runtime_t *node,
 	}
       else			/* transport mode */
 	{
-	  u8 *l2_hdr, l2_len, *ip_hdr, ip_len;
+	  u8 *l2_hdr, l2_len, *ip_hdr;
+	  u16 ip_len;
 	  ip6_ext_header_t *ext_hdr;
 	  udp_header_t *udp = 0;
 	  u16 udp_len = 0;
 	  u8 *old_ip_hdr = vlib_buffer_get_current (b[0]);
 
+	  /*
+	   * Get extension header chain length. It might be longer than the
+	   * buffer's pre_data area.
+	   */
 	  ip_len =
 	    (VNET_LINK_IP6 == lt ?
 	       esp_get_ip6_hdr_len ((ip6_header_t *) old_ip_hdr, &ext_hdr) :
 	       ip4_header_bytes ((ip4_header_t *) old_ip_hdr));
+	  if ((old_ip_hdr - ip_len) < &b[0]->pre_data[0])
+	    {
+	      err = ESP_ENCRYPT_ERROR_NO_BUFFERS;
+	      esp_set_next_index (b[0], node, err, n_noop, noop_nexts,
+				  drop_next);
+	      goto trace;
+	    }
 
 	  vlib_buffer_advance (b[0], ip_len);
 	  payload = vlib_buffer_get_current (b[0]);
diff --git a/test/test_reassembly.py b/test/test_reassembly.py
index b99930f0202..254b2ae3833 100644
--- a/test/test_reassembly.py
+++ b/test/test_reassembly.py
@@ -10,7 +10,8 @@ from scapy.packet import Raw
 from scapy.layers.l2 import Ether, GRE
 from scapy.layers.inet import IP, UDP, ICMP
 from scapy.layers.inet6 import HBHOptUnknown, ICMPv6ParamProblem,\
-    ICMPv6TimeExceeded, IPv6, IPv6ExtHdrFragment, IPv6ExtHdrHopByHop
+    ICMPv6TimeExceeded, IPv6, IPv6ExtHdrFragment,\
+    IPv6ExtHdrHopByHop, IPv6ExtHdrDestOpt, PadN, ICMPv6EchoRequest
 from framework import VppTestCase, VppTestRunner
 from util import ppp, ppc, fragment_rfc791, fragment_rfc8200
 from vpp_gre_interface import VppGreInterface
@@ -1375,18 +1376,16 @@ class TestIPv6Reassembly(VppTestCase):
 
     def test_missing_upper(self):
         """ missing upper layer """
+        optdata = '\x00' * 100
         p = (Ether(dst=self.src_if.local_mac, src=self.src_if.remote_mac) /
              IPv6(src=self.src_if.remote_ip6,
                   dst=self.src_if.local_ip6) /
-             UDP(sport=1234, dport=5678) /
-             Raw())
-        self.extend_packet(p, 1000, self.padding)
-        fragments = fragment_rfc8200(p, 1, 500)
-        bad_fragment = p.__class__(scapy.compat.raw(fragments[1]))
-        bad_fragment[IPv6ExtHdrFragment].nh = 59
-        bad_fragment[IPv6ExtHdrFragment].offset = 0
+             IPv6ExtHdrFragment(m=1) /
+             IPv6ExtHdrDestOpt(nh=17, options=PadN(optdata='\101' * 255) /
+             PadN(optdata='\102'*255)))
+
         self.pg_enable_capture()
-        self.src_if.add_stream([bad_fragment])
+        self.src_if.add_stream([p])
         self.pg_start()
         pkts = self.src_if.get_capture(expected_count=1)
         icmp = pkts[0]
@@ -1431,6 +1430,31 @@ class TestIPv6Reassembly(VppTestCase):
         self.assertIn(ICMPv6ParamProblem, icmp)
         self.assert_equal(icmp[ICMPv6ParamProblem].code, 0, "ICMP code")
 
+    def test_atomic_fragment(self):
+        """ IPv6 atomic fragment """
+        pkt = (Ether(src=self.pg0.local_mac, dst=self.pg0.remote_mac) /
+               IPv6(src=self.pg0.remote_ip6, dst=self.pg0.local_ip6,
+                    nh=44, plen=65535) /
+               IPv6ExtHdrFragment(offset=8191, m=1, res1=0xFF, res2=0xFF,
+                                  nh=255, id=0xffff)/('X'*1452))
+
+        rx = self.send_and_expect(self.pg0, [pkt], self.pg0)
+        self.assertIn(ICMPv6ParamProblem, rx[0])
+
+    def test_truncated_fragment(self):
+        """ IPv6 truncated fragment header """
+        pkt = (Ether(src=self.pg0.local_mac, dst=self.pg0.remote_mac) /
+               IPv6(src=self.pg0.remote_ip6, dst=self.pg0.local_ip6,
+                    nh=44, plen=2) /
+               IPv6ExtHdrFragment(nh=6))
+
+        self.send_and_assert_no_replies(self.pg0, [pkt], self.pg0)
+
+        pkt = (Ether(src=self.pg0.local_mac, dst=self.pg0.remote_mac) /
+               IPv6(src=self.pg0.remote_ip6, dst=self.pg0.remote_ip6) /
+               ICMPv6EchoRequest())
+        rx = self.send_and_expect(self.pg0, [pkt], self.pg0)
+
 
 class TestIPv6MWReassembly(VppTestCase):
     """ IPv6 Reassembly (multiple workers) """