srv6-mobile: Update GTP4/6.D function

GTP4/6.D behavior is updated as shown below.
1. When receiving GTP-U message or IPv6 linklocal destination in inner IP, GTP packet is tnralated to SRv6.
2. When receiving T-PDU packet, OuterIP/UDP/GTP headers are stripped off and Inner IP is encapsulated into SRv6 based on L3VPN SRv6 manner.

Type: feature

Signed-off-by: Tetsuya Murakami <tetsuya.mrk@gmail.com>
Change-Id: I6092c98ea80236d54017f84c5b35cca0b645f034
Signed-off-by: Tetsuya Murakami <tetsuya.mrk@gmail.com>

1 files changed, 1265 insertions, 1107 deletions

diff --git a/src/plugins/srv6-mobile/node.c b/src/plugins/srv6-mobile/node.c
index 448d6332b15..36445a683c0 100644
--- a/src/plugins/srv6-mobile/node.c
+++ b/src/plugins/srv6-mobile/node.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020 Arrcus Inc and/or its affiliates.
+ * Copyright (c) 2019 Arrcus Inc and/or its affiliates.
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at:
@@ -102,11 +102,11 @@ format_srv6_end_rewrite_trace6 (u8 * s, va_list * args)
   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
   srv6_end_rewrite_trace_t *t = va_arg (*args, srv6_end_rewrite_trace_t *);
 
-  return format (s,
-		 "SRv6-END-rewrite: src %U dst %U\n\tTEID: 0x%x\n\tsr_prefix: %U/%d",
-		 format_ip6_address, &t->src, format_ip6_address, &t->dst,
-		 clib_net_to_host_u32 (t->teid), format_ip6_address,
-		 &t->sr_prefix, t->sr_prefixlen);
+  return format (
+    s, "SRv6-END-rewrite: src %U dst %U\n\tTEID: 0x%x\n\tsr_prefix: %U/%d",
+    format_ip6_address, &t->src, format_ip6_address, &t->dst,
+    clib_net_to_host_u32 (t->teid), format_ip6_address, &t->sr_prefix,
+    t->sr_prefixlen);
 }
 
 #define foreach_srv6_end_v4_error \
@@ -245,7 +245,8 @@ typedef enum
 typedef enum
 {
   SRV6_T_M_GTP4_D_NEXT_DROP,
-  SRV6_T_M_GTP4_D_NEXT_LOOKUP,
+  SRV6_T_M_GTP4_D_NEXT_LOOKUP4,
+  SRV6_T_M_GTP4_D_NEXT_LOOKUP6,
   SRV6_T_M_GTP4_D_N_NEXT,
 } srv6_T_m_gtp4_d_next_t;
 
@@ -259,7 +260,8 @@ typedef enum
 typedef enum
 {
   SRV6_END_M_GTP6_D_NEXT_DROP,
-  SRV6_END_M_GTP6_D_NEXT_LOOKUP,
+  SRV6_END_M_GTP6_D_NEXT_LOOKUP4,
+  SRV6_END_M_GTP6_D_NEXT_LOOKUP6,
   SRV6_END_M_GTP6_D_N_NEXT,
 } srv6_end_m_gtp6_d_next_t;
 
@@ -317,9 +319,8 @@ gtpu_type_get (u16 tag)
 }
 
 // Function for SRv6 GTP4.E function.
-VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
-				  vlib_node_runtime_t * node,
-				  vlib_frame_t * frame)
+VLIB_NODE_FN (srv6_end_m_gtp4_e)
+(vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
 {
   srv6_end_main_v4_t *sm = &srv6_end_main_v4;
   ip6_sr_main_t *sm2 = &sr_main;
@@ -343,7 +344,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 	  u32 bi0;
 	  vlib_buffer_t *b0;
 	  ip6_sr_localsid_t *ls0;
-	  srv6_end_gtp4_param_t *ls_param;
+	  srv6_end_gtp4_e_param_t *ls_param;
 
 	  ip6srv_combo_header_t *ip6srv0;
 	  ip6_address_t src0, dst0;
@@ -362,11 +363,10 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 	  n_left_to_next -= 1;
 
 	  b0 = vlib_get_buffer (vm, bi0);
-	  ls0 =
-	    pool_elt_at_index (sm2->localsids,
-			       vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+	  ls0 = pool_elt_at_index (sm2->localsids,
+				   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 
-	  ls_param = (srv6_end_gtp4_param_t *) ls0->plugin_mem;
+	  ls_param = (srv6_end_gtp4_e_param_t *) ls0->plugin_mem;
 
 	  ip6srv0 = vlib_buffer_get_current (b0);
 	  src0 = ip6srv0->ip.src_address;
@@ -374,10 +374,10 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 
 	  len0 = vlib_buffer_length_in_chain (vm, b0);
 
-	  if ((ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE
-	       && len0 <
-	       sizeof (ip6srv_combo_header_t) + ip6srv0->sr.length * 8)
-	      || (len0 < sizeof (ip6_header_t)))
+	  if ((ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE &&
+	       len0 <
+		 sizeof (ip6srv_combo_header_t) + ip6srv0->sr.length * 8) ||
+	      (len0 < sizeof (ip6_header_t)))
 	    {
 	      next0 = SRV6_END_M_GTP4_E_NEXT_DROP;
 
@@ -388,7 +388,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 	      u8 gtpu_type = 0;
 	      u16 tag = 0;
 	      u32 teid = 0;
-	      u8 *teid8p = (u8 *) & teid;
+	      u8 *teid8p = (u8 *) &teid;
 	      u8 qfi = 0;
 	      u16 seq = 0;
 	      u32 index;
@@ -418,9 +418,9 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 
 		  qfi = dst0.as_u8[offset + 4];
 
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		    {
 		      clib_memcpy_fast (&seq, &dst0.as_u8[offset + 5], 2);
 		    }
@@ -443,11 +443,11 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 		  qfi |= dst0.as_u8[offset + 4] << shift;
 		  qfi |= dst0.as_u8[offset + 5] >> (8 - shift);
 
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		    {
-		      sp = (u8 *) & seq;
+		      sp = (u8 *) &seq;
 		      for (index = 0; index < 2; index++)
 			{
 			  sp[index] = dst0.as_u8[offset + 5 + index] << shift;
@@ -472,9 +472,9 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 		  hdrlen =
 		    sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
 		}
-	      else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		       || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+	      else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		       gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		       gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		{
 		  hdrlen = sizeof (gtpu_exthdr_t);
 		}
@@ -494,11 +494,10 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 		  if (ext_len >
 		      sizeof (ip6_address_t) * (ip6srv0->sr.last_entry + 1))
 		    {
-		      tlv =
-			(ip6_sr_tlv_t *) ((u8 *) & ip6srv0->sr +
-					  sizeof (ip6_sr_header_t) +
-					  sizeof (ip6_address_t) *
-					  (ip6srv0->sr.last_entry + 1));
+		      tlv = (ip6_sr_tlv_t *) ((u8 *) &ip6srv0->sr +
+					      sizeof (ip6_sr_header_t) +
+					      sizeof (ip6_address_t) *
+						(ip6srv0->sr.last_entry + 1));
 
 		      if (tlv->type == SRH_TLV_USER_PLANE_CONTAINER)
 			{
@@ -518,7 +517,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 		{
 		  vlib_buffer_advance (b0,
 				       (word) sizeof (ip6srv_combo_header_t) +
-				       ip6srv0->sr.length * 8);
+					 ip6srv0->sr.length * 8);
 		}
 	      else
 		{
@@ -549,38 +548,9 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 
 	      hdr0->gtpu.type = gtpu_type;
 
-	      if (qfi)
-		{
-		  u8 type = 0;
-		  gtpu_pdu_session_t *sess;
-
-		  hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
-
-		  hdr0->gtpu.ext->seq = 0;
-
-		  hdr0->gtpu.ext->npdu_num = 0;
-		  hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
-
-		  type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
-
-		  qfi =
-		    ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
-		    ((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
-
-		  sess =
-		    (gtpu_pdu_session_t *) (((char *) hdr0) +
-					    sizeof (ip4_gtpu_header_t) +
-					    sizeof (gtpu_exthdr_t));
-		  sess->exthdrlen = 1;
-		  sess->type = type;
-		  sess->spare = 0;
-		  sess->u.val = qfi;
-		  sess->nextexthdr = 0;
-		}
-
-	      if (gtpu_type == GTPU_TYPE_ECHO_REPLY
-		  || gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		  || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+	      if (gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		  gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		  gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		{
 		  hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG;
 		  hdr0->gtpu.ext->seq = seq;
@@ -609,41 +579,80 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 			}
 		    }
 		}
+	      else
+		{
+		  if (qfi)
+		    {
+		      hdr0->gtpu.ext->seq = 0;
+		      hdr0->gtpu.ext->npdu_num = 0;
+		    }
+		}
 
-	      offset = ls_param->v4src_position / 8;
-	      shift = ls_param->v4src_position % 8;
+	      if (qfi)
+		{
+		  u8 type = 0;
+		  gtpu_pdu_session_t *sess;
 
-	      if (PREDICT_TRUE (shift == 0))
+		  hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
+
+		  hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
+
+		  type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
+
+		  qfi = ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
+			((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
+
+		  sess = (gtpu_pdu_session_t *) (((char *) hdr0) +
+						 sizeof (ip4_gtpu_header_t) +
+						 sizeof (gtpu_exthdr_t));
+		  sess->exthdrlen = 1;
+		  sess->type = type;
+		  sess->spare = 0;
+		  sess->u.val = qfi;
+		  sess->nextexthdr = 0;
+		}
+
+	      vnet_buffer (b0)->sw_if_index[VLIB_TX] = ls_param->fib4_index;
+
+	      if (ls_param->v4src_position)
 		{
-		  for (index = 0; index < 4; index++)
+		  offset = ls_param->v4src_position / 8;
+		  shift = ls_param->v4src_position % 8;
+
+		  if (PREDICT_TRUE (shift == 0))
+		    {
+		      for (index = 0; index < 4; index++)
+			{
+			  hdr0->ip4.src_address.as_u8[index] =
+			    src0.as_u8[offset + index];
+			}
+		    }
+		  else
 		    {
-		      hdr0->ip4.src_address.as_u8[index] =
-			src0.as_u8[offset + index];
+		      for (index = 0; index < 4; index++)
+			{
+			  hdr0->ip4.src_address.as_u8[index] =
+			    src0.as_u8[offset + index] << shift;
+			  hdr0->ip4.src_address.as_u8[index] |=
+			    src0.as_u8[offset + index + 1] >> (8 - shift);
+			}
 		    }
 		}
 	      else
 		{
-		  for (index = 0; index < 4; index++)
-		    {
-		      hdr0->ip4.src_address.as_u8[index] =
-			src0.as_u8[offset + index] << shift;
-		      hdr0->ip4.src_address.as_u8[index] |=
-			src0.as_u8[offset + index + 1] >> (8 - shift);
-		    }
+		  clib_memcpy_fast (&hdr0->ip4.src_address,
+				    &ls_param->v4src_addr, 4);
 		}
 
 	      key = hash_memory (p, plen < 40 ? plen : 40, 0);
 	      port = hash_uword_to_u16 (&key);
 	      hdr0->udp.src_port = port;
 
-	      hdr0->udp.length = clib_host_to_net_u16 (len0 +
-						       sizeof (udp_header_t) +
-						       sizeof
-						       (gtpu_header_t));
+	      hdr0->udp.length = clib_host_to_net_u16 (
+		len0 + sizeof (udp_header_t) + sizeof (gtpu_header_t));
 
-	      hdr0->ip4.length = clib_host_to_net_u16 (len0 +
-						       sizeof
-						       (ip4_gtpu_header_t));
+	      hdr0->ip4.length =
+		clib_host_to_net_u16 (len0 + sizeof (ip4_gtpu_header_t));
 
 	      hdr0->ip4.checksum = ip4_header_checksum (&hdr0->ip4);
 
@@ -655,18 +664,19 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 		  srv6_end_rewrite_trace_t *tr =
 		    vlib_add_trace (vm, node, b0, sizeof (*tr));
 		  clib_memcpy (tr->src.as_u8, hdr0->ip4.src_address.as_u8,
-			       sizeof (hdr0->ip4.src_address.as_u8));
+			       sizeof (tr->src.as_u8));
 		  clib_memcpy (tr->dst.as_u8, hdr0->ip4.dst_address.as_u8,
-			       sizeof (hdr0->ip4.dst_address.as_u8));
+			       sizeof (tr->dst.as_u8));
 		  tr->teid = hdr0->gtpu.teid;
 		}
 	    }
 
-	  vlib_increment_combined_counter
-	    (((next0 ==
-	       SRV6_END_M_GTP4_E_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
-	      &(sm2->sr_ls_valid_counters)), thread_index,
-	     ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
+	  vlib_increment_combined_counter (
+	    ((next0 == SRV6_END_M_GTP4_E_NEXT_DROP) ?
+	       &(sm2->sr_ls_invalid_counters) :
+	       &(sm2->sr_ls_valid_counters)),
+	    thread_index, ls0 - sm2->localsids, 1,
+	    vlib_buffer_length_in_chain (vm, b0));
 
 	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 					   n_left_to_next, bi0, next0);
@@ -685,515 +695,590 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 }
 
 // Function for SRv6 GTP4.D function.
-VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
-				vlib_node_runtime_t * node,
-				vlib_frame_t * frame)
+static inline u32
+srv6_gtp4_decap_processing (vlib_main_t *vm, vlib_node_runtime_t *node,
+			    vlib_buffer_t *b0)
 {
   srv6_t_main_v4_decap_t *sm = &srv6_t_main_v4_decap;
   ip6_sr_main_t *sm2 = &sr_main;
-  u32 n_left_from, next_index, *from, *to_next;
 
-  u32 good_n = 0, bad_n = 0;
+  ip6_sr_sl_t *sl0;
+  srv6_end_gtp4_d_param_t *ls_param;
+  ip4_header_t *ip4;
 
-  from = vlib_frame_vector_args (frame);
-  n_left_from = frame->n_vectors;
-  next_index = node->cached_next_index;
+  uword len0;
 
-  while (n_left_from > 0)
-    {
-      u32 n_left_to_next;
+  u32 next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP6;
 
-      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
+  sl0 = pool_elt_at_index (sm2->sid_lists,
+			   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 
-      while (n_left_from > 0 && n_left_to_next > 0)
+  ls_param = (srv6_end_gtp4_d_param_t *) sl0->plugin_mem;
+
+  len0 = vlib_buffer_length_in_chain (vm, b0);
+
+  ip4 = vlib_buffer_get_current (b0);
+
+  if (ip4->protocol != IP_PROTOCOL_UDP || len0 < sizeof (ip4_gtpu_header_t))
+    {
+      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
+    }
+  else
+    {
+      uword *p;
+      ip6_sr_policy_t *sr_policy = NULL;
+      ip6_sr_sl_t *sl = NULL;
+      u32 *sl_index;
+      u32 hdr_len;
+
+      ip4_gtpu_header_t *hdr;
+      ip4_address_t src, dst;
+      u8 *srcp, *dstp;
+      ip6_header_t *encap = NULL;
+      ip6_address_t seg;
+      ip6_address_t src6;
+      u8 gtpu_type;
+      u32 teid;
+      u8 *teidp;
+      u8 qfi = 0;
+      u8 *qfip = NULL;
+      u16 seq = 0;
+      u8 *seqp;
+      u32 offset, shift, index;
+      ip6srv_combo_header_t *ip6srv;
+      gtpu_pdu_session_t *sess = NULL;
+      int ie_size = 0;
+      u16 tlv_siz = 0;
+      u8 ie_buf[GTPU_IE_MAX_SIZ];
+
+      // Decap from GTP-U.
+      hdr = (ip4_gtpu_header_t *) ip4;
+
+      hdr_len = sizeof (ip4_gtpu_header_t);
+
+      teid = hdr->gtpu.teid;
+      teidp = (u8 *) &teid;
+
+      seqp = (u8 *) &seq;
+
+      gtpu_type = hdr->gtpu.type;
+
+      if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
 	{
-	  u32 bi0;
-	  vlib_buffer_t *b0;
-	  ip6_sr_sl_t *sl0;
-	  srv6_end_gtp4_param_t *ls_param;
-	  ip4_header_t *ip4;
+	  // Extention header.
+	  hdr_len += sizeof (gtpu_exthdr_t);
 
-	  uword len0;
+	  seq = hdr->gtpu.ext->seq;
 
-	  u32 next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP;
+	  if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
+	    {
+	      // PDU Session Container.
+	      sess = (gtpu_pdu_session_t *) (((char *) hdr) + hdr_len);
+	      qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
+	      qfip = (u8 *) &qfi;
 
-	  // defaults
-	  bi0 = from[0];
-	  to_next[0] = bi0;
-	  from += 1;
-	  to_next += 1;
-	  n_left_from -= 1;
-	  n_left_to_next -= 1;
+	      hdr_len += sizeof (gtpu_pdu_session_t);
 
-	  b0 = vlib_get_buffer (vm, bi0);
+	      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
+		{
+		  hdr_len += sizeof (gtpu_paging_policy_t);
+		}
+	    }
+	}
 
-	  sl0 =
-	    pool_elt_at_index (sm2->sid_lists,
-			       vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+      src = hdr->ip4.src_address;
+      srcp = (u8 *) &src;
 
-	  ls_param = (srv6_end_gtp4_param_t *) sl0->plugin_mem;
+      dst = hdr->ip4.dst_address;
+      dstp = (u8 *) &dst;
 
-	  len0 = vlib_buffer_length_in_chain (vm, b0);
+      seg = ls_param->sr_prefix;
 
-	  ip4 = vlib_buffer_get_current (b0);
+      offset = ls_param->sr_prefixlen / 8;
+      shift = ls_param->sr_prefixlen % 8;
 
-	  if (ip4->protocol != IP_PROTOCOL_UDP
-	      || len0 < sizeof (ip4_gtpu_header_t))
+      if (PREDICT_TRUE (shift == 0))
+	{
+	  clib_memcpy_fast (&seg.as_u8[offset], dstp, 4);
+
+	  if (qfip)
 	    {
-	      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
+	      qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
+		    ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 
-	      bad_n++;
+	      if (sess->type)
+		{
+		  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
+		}
+
+	      seg.as_u8[offset + 4] = qfi;
+	    }
+
+	  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+	      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+	      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+	    {
+	      clib_memcpy_fast (&seg.as_u8[offset + 5], seqp, 2);
 	    }
 	  else
 	    {
-	      uword *p;
-	      ip6_sr_policy_t *sr_policy = NULL;
-	      ip6_sr_sl_t *sl = NULL;
-	      u32 *sl_index;
-	      u32 hdr_len;
-
-	      ip4_gtpu_header_t *hdr;
-	      ip4_address_t src, dst;
-	      u8 *srcp, *dstp;
-	      ip6_header_t *encap = NULL;
-	      ip6_address_t seg;
-	      ip6_address_t src6;
-	      u8 gtpu_type;
-	      u32 teid;
-	      u8 *teidp;
-	      u8 qfi = 0;
-	      u8 *qfip = NULL;
-	      u16 seq = 0;
-	      u8 *seqp;
-	      u32 offset, shift, index;
-	      ip6srv_combo_header_t *ip6srv;
-	      gtpu_pdu_session_t *sess = NULL;
-	      int ie_size = 0;
-	      u16 tlv_siz = 0;
-	      u8 ie_buf[GTPU_IE_MAX_SIZ];
+	      clib_memcpy_fast (&seg.as_u8[offset + 5], teidp, 4);
+	    }
+	}
+      else
+	{
+	  for (index = 0; index < 4; index++)
+	    {
+	      seg.as_u8[offset + index] |= dstp[index] >> shift;
+	      seg.as_u8[offset + index + 1] |= dstp[index] << (8 - shift);
+	    }
 
-	      // Decap from GTP-U.
-	      hdr = (ip4_gtpu_header_t *) ip4;
+	  if (qfip)
+	    {
+	      qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
+		    ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 
-	      hdr_len = sizeof (ip4_gtpu_header_t);
+	      if (sess->type)
+		{
+		  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
+		}
 
-	      teid = hdr->gtpu.teid;
-	      teidp = (u8 *) & teid;
+	      seg.as_u8[offset + 4] |= qfi >> shift;
+	      seg.as_u8[offset + 5] |= qfi << (8 - shift);
+	    }
 
-	      seqp = (u8 *) & seq;
+	  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+	      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+	      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+	    {
+	      for (index = 0; index < 2; index++)
+		{
+		  seg.as_u8[offset + 5 + index] |= seqp[index] >> shift;
+		  seg.as_u8[offset + 6 + index] |= seqp[index] << (8 - shift);
+		}
+	    }
+	  else
+	    {
+	      for (index = 0; index < 4; index++)
+		{
+		  seg.as_u8[offset + index + 5] |= teidp[index] >> shift;
+		  seg.as_u8[offset + index + 6] |= teidp[index] << (8 - shift);
+		}
+	    }
+	}
 
-	      gtpu_type = hdr->gtpu.type;
+      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
+	{
+	  u16 payload_len;
 
-	      if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
+	  payload_len = clib_net_to_host_u16 (hdr->gtpu.length);
+	  if (payload_len != 0)
+	    {
+	      ie_size = payload_len - (hdr_len - sizeof (ip4_gtpu_header_t));
+	      if (ie_size > 0)
 		{
-		  // Extention header.
-		  hdr_len += sizeof (gtpu_exthdr_t);
+		  u8 *ies;
 
-		  seq = hdr->gtpu.ext->seq;
+		  ies = (u8 *) ((u8 *) hdr + hdr_len);
+		  clib_memcpy_fast (ie_buf, ies, ie_size);
+		  hdr_len += ie_size;
+		}
+	    }
+	}
 
-		  if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
-		    {
-		      // PDU Session Container.
-		      sess =
-			(gtpu_pdu_session_t *) (((char *) hdr) + hdr_len);
-		      qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
-		      qfip = (u8 *) & qfi;
+      src6 = ls_param->v6src_prefix;
 
-		      hdr_len += sizeof (gtpu_pdu_session_t);
+      offset = ls_param->v6src_prefixlen / 8;
+      shift = ls_param->v6src_prefixlen % 8;
 
-		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
-			{
-			  hdr_len += sizeof (gtpu_paging_policy_t);
-			}
-		    }
-		}
+      if (PREDICT_TRUE (shift == 0))
+	{
+	  clib_memcpy_fast (&src6.as_u8[offset], srcp, 4);
+	}
+      else
+	{
+	  for (index = 0; index < 4; index++)
+	    {
+	      src6.as_u8[offset + index] |= srcp[offset] >> shift;
+	      src6.as_u8[offset + index + 1] |= srcp[offset] << (8 - shift);
+	    }
+	}
 
-	      src = hdr->ip4.src_address;
-	      srcp = (u8 *) & src;
+      vlib_buffer_advance (b0, (word) hdr_len);
 
-	      dst = hdr->ip4.dst_address;
-	      dstp = (u8 *) & dst;
+      // Encap to SRv6.
+      if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
+	{
+	  encap = vlib_buffer_get_current (b0);
+	}
 
-	      seg = ls_param->sr_prefix;
+      len0 = vlib_buffer_length_in_chain (vm, b0);
 
-	      offset = ls_param->sr_prefixlen / 8;
-	      shift = ls_param->sr_prefixlen % 8;
+      p = mhash_get (&sm2->sr_policies_index_hash, &ls_param->sr_prefix);
+      if (p)
+	{
+	  sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
+	}
 
-	      if (PREDICT_TRUE (shift == 0))
-		{
-		  clib_memcpy_fast (&seg.as_u8[offset], dstp, 4);
+      if (sr_policy)
+	{
+	  vec_foreach (sl_index, sr_policy->segments_lists)
+	    {
+	      sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
+	      if (sl != NULL)
+		break;
+	    }
+	}
 
-		  if (qfip)
-		    {
-		      qfi =
-			((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
-			((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
+      if (sl)
+	{
+	  hdr_len = sizeof (ip6srv_combo_header_t);
+	  hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
+	  hdr_len += sizeof (ip6_address_t);
+	}
+      else
+	{
+	  hdr_len = sizeof (ip6_header_t);
 
-		      if (sess->type)
-			{
-			  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
-			}
+	  if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
+	    {
+	      hdr_len += sizeof (ip6_sr_header_t);
+	      hdr_len += sizeof (ip6_address_t);
+	    }
+	}
 
-		      seg.as_u8[offset + 4] = qfi;
-		    }
+      if (ie_size)
+	{
+	  tlv_siz =
+	    sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) + ie_size;
 
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
-		    {
-		      clib_memcpy_fast (&seg.as_u8[offset + 5], seqp, 2);
-		    }
-		  else
-		    {
-		      clib_memcpy_fast (&seg.as_u8[offset + 5], teidp, 4);
-		    }
-		}
-	      else
-		{
-		  for (index = 0; index < 4; index++)
-		    {
-		      seg.as_u8[offset + index] |= dstp[index] >> shift;
-		      seg.as_u8[offset + index + 1] |=
-			dstp[index] << (8 - shift);
-		    }
+	  tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0);
+	  hdr_len += tlv_siz;
+	}
 
-		  if (qfip)
-		    {
-		      qfi =
-			((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
-			((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
+      vlib_buffer_advance (b0, -(word) hdr_len);
+      ip6srv = vlib_buffer_get_current (b0);
 
-		      if (sess->type)
-			{
-			  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
-			}
+      if (sl)
+	{
+	  clib_memcpy_fast (ip6srv, sl->rewrite, vec_len (sl->rewrite));
 
-		      seg.as_u8[offset + 4] |= qfi >> shift;
-		      seg.as_u8[offset + 5] |= qfi << (8 - shift);
-		    }
+	  if (vec_len (sl->segments) > 1)
+	    {
+	      ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
-		    {
-		      for (index = 0; index < 2; index++)
-			{
-			  seg.as_u8[offset + 5 + index] |=
-			    seqp[index] >> shift;
-			  seg.as_u8[offset + 6 + index] |=
-			    seqp[index] << (8 - shift);
-			}
-		    }
-		  else
-		    {
-		      for (index = 0; index < 4; index++)
-			{
-			  seg.as_u8[offset + index + 5] |=
-			    teidp[index] >> shift;
-			  seg.as_u8[offset + index + 6] |=
-			    teidp[index] << (8 - shift);
-			}
-		    }
-		}
+	      ip6srv->sr.segments_left += 1;
+	      ip6srv->sr.last_entry += 1;
 
-	      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
-		{
-		  u16 payload_len;
+	      ip6srv->sr.length += sizeof (ip6_address_t) / 8;
+	      ip6srv->sr.segments[0] = seg;
 
-		  payload_len = clib_net_to_host_u16 (hdr->gtpu.length);
-		  if (payload_len != 0)
-		    {
-		      ie_size =
-			payload_len - (hdr_len - sizeof (ip4_gtpu_header_t));
-		      if (ie_size > 0)
-			{
-			  u8 *ies;
+	      clib_memcpy_fast (&ip6srv->sr.segments[1],
+				(u8 *) (sl->rewrite + sizeof (ip6_header_t) +
+					sizeof (ip6_sr_header_t)),
+				vec_len (sl->segments) *
+				  sizeof (ip6_address_t));
+	    }
+	  else
+	    {
+	      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
 
-			  ies = (u8 *) ((u8 *) hdr + hdr_len);
-			  clib_memcpy_fast (ie_buf, ies, ie_size);
-			  hdr_len += ie_size;
-			}
-		    }
-		}
+	      ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
 
-	      src6 = ls_param->v6src_prefix;
+	      ip6srv->sr.segments_left = 1;
+	      ip6srv->sr.last_entry = 0;
 
-	      offset = ls_param->v6src_prefixlen / 8;
-	      shift = ls_param->v6src_prefixlen % 8;
+	      ip6srv->sr.length =
+		((sizeof (ip6_sr_header_t) + sizeof (ip6_address_t)) / 8) - 1;
+	      ip6srv->sr.flags = 0;
 
-	      if (PREDICT_TRUE (shift == 0))
+	      ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+
+	      ip6srv->sr.segments[0] = seg;
+	      if (vec_len (sl->segments))
 		{
-		  clib_memcpy_fast (&src6.as_u8[offset], srcp, 4);
+		  ip6srv->sr.segments[1] = sl->segments[0];
+		  ip6srv->sr.length += sizeof (ip6_address_t) / 8;
+		  ip6srv->sr.last_entry++;
 		}
-	      else
+	    }
+
+	  if (PREDICT_TRUE (encap != NULL))
+	    {
+	      if (ls_param->nhtype == SRV6_NHTYPE_NONE)
 		{
-		  for (index = 0; index < 4; index++)
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) == 6)
+		    ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
+		  else
+		    ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
+		}
+	      else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
+		{
+		  ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 4)
 		    {
-		      src6.as_u8[offset + index] |= srcp[offset] >> shift;
-		      src6.as_u8[offset + index + 1] |=
-			srcp[offset] << (8 - shift);
+		      // Bad encap packet.
+		      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
+		      goto DONE;
 		    }
 		}
-
-	      vlib_buffer_advance (b0, (word) hdr_len);
-
-	      // Encap to SRv6.
-	      if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
+	      else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
 		{
-		  encap = vlib_buffer_get_current (b0);
+		  ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 6)
+		    {
+		      // Bad encap packet.
+		      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
+		      goto DONE;
+		    }
 		}
-
-	      len0 = vlib_buffer_length_in_chain (vm, b0);
-
-	      p =
-		mhash_get (&sm2->sr_policies_index_hash,
-			   &ls_param->sr_prefix);
-	      if (p)
+	      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
 		{
-		  sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
+		  ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
 		}
+	    }
+	  else
+	    {
+	      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
+	    }
+	}
+      else
+	{
+	  clib_memcpy_fast (ip6srv, &sm->cache_hdr, sizeof (ip6_header_t));
 
-	      if (sr_policy)
+	  ip6srv->ip.dst_address = seg;
+
+	  if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
+	    {
+	      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+
+	      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
+
+	      ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
+
+	      ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+
+	      ip6srv->sr.segments_left = 0;
+	      ip6srv->sr.last_entry = 0;
+
+	      ip6srv->sr.length = sizeof (ip6_address_t) / 8;
+	      ip6srv->sr.segments[0] = seg;
+	    }
+	  else
+	    {
+	      if (ls_param->nhtype == SRV6_NHTYPE_NONE)
 		{
-		  vec_foreach (sl_index, sr_policy->segments_lists)
-		  {
-		    sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
-		    if (sl != NULL)
-		      break;
-		  }
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) == 6)
+		    ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
+		  else
+		    ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
 		}
-
-	      if (sl)
+	      else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
 		{
-		  hdr_len = sizeof (ip6srv_combo_header_t);
-		  hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
-		  hdr_len += sizeof (ip6_address_t);
+		  ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 4)
+		    {
+		      // Bad encap packet.
+		      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
+		      goto DONE;
+		    }
 		}
-	      else
+	      else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
 		{
-		  hdr_len = sizeof (ip6_header_t);
-
-		  if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
+		  ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 6)
 		    {
-		      hdr_len += sizeof (ip6_sr_header_t);
-		      hdr_len += sizeof (ip6_address_t);
+		      // Bad encap packet.
+		      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
+		      goto DONE;
 		    }
 		}
-
-	      if (ie_size)
+	      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
 		{
-		  tlv_siz =
-		    sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) +
-		    ie_size;
-
-		  tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0);
-		  hdr_len += tlv_siz;
+		  ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
 		}
+	    }
+	}
 
-	      vlib_buffer_advance (b0, -(word) hdr_len);
-	      ip6srv = vlib_buffer_get_current (b0);
+      ip6srv->ip.src_address = src6;
 
-	      if (sl)
-		{
-		  clib_memcpy_fast (ip6srv, sl->rewrite,
-				    vec_len (sl->rewrite));
+      if (PREDICT_FALSE (ie_size))
+	{
+	  ip6_sr_tlv_t *tlv;
+	  user_plane_sub_tlv_t *sub_tlv;
 
-		  if (vec_len (sl->segments) > 1)
-		    {
-		      ip6srv->sr.tag =
-			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+	  tlv = (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
+	  tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
+	  tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t));
+	  clib_memset (tlv->value, 0, tlv->length);
 
-		      ip6srv->sr.segments_left += 1;
-		      ip6srv->sr.last_entry += 1;
+	  sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
+	  sub_tlv->type = USER_PLANE_SUB_TLV_IE;
+	  sub_tlv->length = (u8) ie_size;
+	  clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
 
-		      ip6srv->sr.length += sizeof (ip6_address_t) / 8;
-		      ip6srv->sr.segments[0] = seg;
+	  ip6srv->sr.length += (u8) (tlv_siz / 8);
+	}
 
-		      clib_memcpy_fast (&ip6srv->sr.segments[1],
-					(u8 *) (sl->rewrite +
-						sizeof (ip6_header_t) +
-						sizeof (ip6_sr_header_t)),
-					vec_len (sl->segments) *
-					sizeof (ip6_address_t));
-		    }
-		  else
-		    {
-		      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+      ip6srv->ip.payload_length =
+	clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
 
-		      ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
+      vnet_buffer (b0)->sw_if_index[VLIB_TX] = 0; /* default FIB */
 
-		      ip6srv->sr.segments_left = 1;
-		      ip6srv->sr.last_entry = 0;
+      if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
+	  PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
+	{
+	  srv6_end_rewrite_trace_t *tr =
+	    vlib_add_trace (vm, node, b0, sizeof (*tr));
+	  clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
+		       sizeof (tr->src.as_u8));
+	  clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
+		       sizeof (tr->dst.as_u8));
+	}
+    }
 
-		      ip6srv->sr.length =
-			((sizeof (ip6_sr_header_t) +
-			  sizeof (ip6_address_t)) / 8) - 1;
-		      ip6srv->sr.flags = 0;
+DONE:
+  return next0;
+}
 
-		      ip6srv->sr.tag =
-			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+VLIB_NODE_FN (srv6_t_m_gtp4_d)
+(vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
+{
+  srv6_t_main_v4_decap_t *sm = &srv6_t_main_v4_decap;
+  ip6_sr_main_t *sm2 = &sr_main;
+  u32 n_left_from, next_index, *from, *to_next;
 
-		      ip6srv->sr.segments[0] = seg;
-		      if (vec_len (sl->segments))
-			{
-			  ip6srv->sr.segments[1] = sl->segments[0];
-			  ip6srv->sr.length += sizeof (ip6_address_t) / 8;
-			  ip6srv->sr.last_entry++;
-			}
-		    }
+  ip6_sr_sl_t *sl0;
+  srv6_end_gtp4_d_param_t *ls_param;
 
-		  if (PREDICT_TRUE (encap != NULL))
-		    {
-		      if (ls_param->nhtype == SRV6_NHTYPE_NONE)
-			{
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) == 6)
-			    ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
-			  else
-			    ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
-			{
-			  ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 4)
-			    {
-			      // Bad encap packet.
-			      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
-			      bad_n++;
-			      goto DONE;
-			    }
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
-			{
-			  ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 6)
-			    {
-			      // Bad encap packet.
-			      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
-			      bad_n++;
-			      goto DONE;
-			    }
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
-			{
-			  ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
-			}
-		    }
-		  else
-		    {
-		      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
-		    }
-		}
-	      else
-		{
-		  clib_memcpy_fast (ip6srv, &sm->cache_hdr,
-				    sizeof (ip6_header_t));
+  u32 good_n = 0, bad_n = 0;
 
-		  ip6srv->ip.dst_address = seg;
+  from = vlib_frame_vector_args (frame);
+  n_left_from = frame->n_vectors;
+  next_index = node->cached_next_index;
 
-		  if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
-		    {
-		      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+  while (n_left_from > 0)
+    {
+      u32 n_left_to_next;
 
-		      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
+      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 
-		      ip6srv->sr.tag =
-			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+      while (n_left_from > 0 && n_left_to_next > 0)
+	{
+	  u32 bi0;
+	  vlib_buffer_t *b0;
 
-		      ip6srv->sr.segments_left = 0;
-		      ip6srv->sr.last_entry = 0;
+	  u32 next0;
 
-		      ip6srv->sr.length = sizeof (ip6_address_t) / 8;
-		      ip6srv->sr.segments[0] = seg;
-		    }
-		  else
+	  ip4_gtpu_header_t *hdr;
+	  u32 hdrlen;
+	  u8 gtpu_type;
+	  bool gtp4;
+	  bool ipv4;
+
+	  // defaults
+	  bi0 = from[0];
+	  to_next[0] = bi0;
+	  from += 1;
+	  to_next += 1;
+	  n_left_from -= 1;
+	  n_left_to_next -= 1;
+
+	  b0 = vlib_get_buffer (vm, bi0);
+
+	  sl0 = pool_elt_at_index (sm2->sid_lists,
+				   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+
+	  ls_param = (srv6_end_gtp4_d_param_t *) sl0->plugin_mem;
+
+	  hdr = vlib_buffer_get_current (b0);
+	  gtpu_type = hdr->gtpu.type;
+
+	  gtp4 = false;
+	  ipv4 = true;
+
+	  if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU || ls_param->drop_in))
+	    {
+	      gtp4 = true;
+	    }
+	  else
+	    {
+	      ip6_header_t *ip6;
+
+	      hdrlen = sizeof (ip4_gtpu_header_t);
+
+	      if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
+		{
+		  hdrlen += sizeof (gtpu_exthdr_t);
+		  if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 		    {
-		      if (ls_param->nhtype == SRV6_NHTYPE_NONE)
-			{
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) == 6)
-			    ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
-			  else
-			    ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
-			{
-			  ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 4)
-			    {
-			      // Bad encap packet.
-			      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
-			      bad_n++;
-			      goto DONE;
-			    }
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
-			{
-			  ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 6)
-			    {
-			      // Bad encap packet.
-			      next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
-			      bad_n++;
-			      goto DONE;
-			    }
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
+		      gtpu_pdu_session_t *sess;
+		      sess = (gtpu_pdu_session_t *) (((char *) hdr) + hdrlen);
+		      hdrlen += sizeof (gtpu_pdu_session_t);
+
+		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
 			{
-			  ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
+			  hdrlen += sizeof (gtpu_paging_policy_t);
 			}
 		    }
 		}
 
-	      ip6srv->ip.src_address = src6;
-
-	      if (PREDICT_FALSE (ie_size))
+	      ip6 = (ip6_header_t *) (((char *) hdr) + hdrlen);
+	      if ((clib_net_to_host_u32 (
+		     ip6->ip_version_traffic_class_and_flow_label) >>
+		   28) == 6)
 		{
-		  ip6_sr_tlv_t *tlv;
-		  user_plane_sub_tlv_t *sub_tlv;
-
-		  tlv =
-		    (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
-		  tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
-		  tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t));
-		  clib_memset (tlv->value, 0, tlv->length);
-
-		  sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
-		  sub_tlv->type = USER_PLANE_SUB_TLV_IE;
-		  sub_tlv->length = (u8) ie_size;
-		  clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
-
-		  ip6srv->sr.length += (u8) (tlv_siz / 8);
+		  ipv4 = false;
+		  if (((ip6->dst_address.as_u8[0] == 0xff) &&
+		       (ip6->dst_address.as_u8[1] == 0x02)) ||
+		      ((ip6->dst_address.as_u8[0] == 0xfe) &&
+		       ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
+		    {
+		      // Inner desitnation is IPv6 link local
+		      gtp4 = true;
+		    }
 		}
+	    }
 
-	      ip6srv->ip.payload_length =
-		clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
-
-	      good_n++;
+	  if (gtp4)
+	    {
+	      next0 = srv6_gtp4_decap_processing (vm, node, b0);
+	      if (PREDICT_TRUE (next0 == SRV6_T_M_GTP4_D_NEXT_LOOKUP6))
+		good_n++;
+	      else
+		bad_n++;
+	    }
+	  else
+	    {
+	      /* Strip off the outer header (IPv4 + GTP + UDP + IEs) */
+	      vlib_buffer_advance (b0, (word) hdrlen);
 
-	      if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
-		  PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
+	      if (ipv4)
 		{
-		  srv6_end_rewrite_trace_t *tr =
-		    vlib_add_trace (vm, node, b0, sizeof (*tr));
-		  clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
-			       sizeof (tr->src.as_u8));
-		  clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
-			       sizeof (tr->dst.as_u8));
+		  next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP4;
+		  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
+		    ls_param->fib4_index;
+		}
+	      else
+		{
+		  next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP6;
+		  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
+		    ls_param->fib6_index;
 		}
 	    }
 
-	DONE:
 	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 					   n_left_to_next, bi0, next0);
 	}
@@ -1218,8 +1303,8 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp4_e) =
     srv6_end_error_v4_strings,.n_next_nodes =
     SRV6_END_M_GTP4_E_N_NEXT,.next_nodes =
   {
-  [SRV6_END_M_GTP4_E_NEXT_DROP] =
-      "error-drop",[SRV6_END_M_GTP4_E_NEXT_LOOKUP] = "ip4-lookup",}
+  [SRV6_END_M_GTP4_E_NEXT_DROP] = "error-drop",
+  [SRV6_END_M_GTP4_E_NEXT_LOOKUP] = "ip4-lookup",}
 ,};
 
 VLIB_REGISTER_NODE (srv6_t_m_gtp4_d) =
@@ -1230,14 +1315,14 @@ VLIB_REGISTER_NODE (srv6_t_m_gtp4_d) =
     srv6_t_error_v4_d_strings,.n_next_nodes =
     SRV6_T_M_GTP4_D_N_NEXT,.next_nodes =
   {
-  [SRV6_T_M_GTP4_D_NEXT_DROP] =
-      "error-drop",[SRV6_T_M_GTP4_D_NEXT_LOOKUP] = "ip6-lookup",}
+  [SRV6_T_M_GTP4_D_NEXT_DROP] = "error-drop",
+  [SRV6_T_M_GTP4_D_NEXT_LOOKUP4] = "ip4-lookup",
+  [SRV6_T_M_GTP4_D_NEXT_LOOKUP6] = "ip6-lookup",}
 ,};
 
 // Function for SRv6 GTP6.E function
-VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
-				  vlib_node_runtime_t * node,
-				  vlib_frame_t * frame)
+VLIB_NODE_FN (srv6_end_m_gtp6_e)
+(vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
 {
   srv6_end_main_v6_t *sm = &srv6_end_main_v6;
   ip6_sr_main_t *sm2 = &sr_main;
@@ -1261,6 +1346,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 	  u32 bi0;
 	  vlib_buffer_t *b0;
 	  ip6_sr_localsid_t *ls0;
+	  srv6_end_gtp6_e_param_t *ls_param;
 
 	  ip6srv_combo_header_t *ip6srv0;
 	  ip6_address_t dst0, src0, seg0;
@@ -1284,9 +1370,10 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 	  n_left_to_next -= 1;
 
 	  b0 = vlib_get_buffer (vm, bi0);
-	  ls0 =
-	    pool_elt_at_index (sm2->localsids,
-			       vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+	  ls0 = pool_elt_at_index (sm2->localsids,
+				   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+
+	  ls_param = (srv6_end_gtp6_e_param_t *) ls0->plugin_mem;
 
 	  ip6srv0 = vlib_buffer_get_current (b0);
 	  dst0 = ip6srv0->ip.dst_address;
@@ -1297,9 +1384,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 
 	  len0 = vlib_buffer_length_in_chain (vm, b0);
 
-	  if ((ip6srv0->ip.protocol != IPPROTO_IPV6_ROUTE)
-	      || (len0 <
-		  sizeof (ip6srv_combo_header_t) + 8 * ip6srv0->sr.length))
+	  if ((ip6srv0->ip.protocol != IPPROTO_IPV6_ROUTE) ||
+	      (len0 < sizeof (ip6srv_combo_header_t) + 8 * ip6srv0->sr.length))
 	    {
 	      next0 = SRV6_END_M_GTP6_E_NEXT_DROP;
 
@@ -1313,7 +1399,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 	      // logic
 
 	      u32 teid = 0;
-	      u8 *teid8p = (u8 *) & teid;
+	      u8 *teid8p = (u8 *) &teid;
 	      u8 qfi = 0;
 	      u16 seq = 0;
 	      u8 gtpu_type = 0;
@@ -1332,10 +1418,9 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 	      if (PREDICT_TRUE (shift == 0))
 		{
 		  qfi = dst0.as_u8[offset];
-
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		    {
 		      clib_memcpy_fast (&seq, &dst0.as_u8[offset + 1], 2);
 		    }
@@ -1351,14 +1436,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 		  qfi |= dst0.as_u8[offset] << shift;
 		  qfi |= dst0.as_u8[offset + 1] >> (8 - shift);
 
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		    {
-		      sp = (u8 *) & seq;
+		      sp = (u8 *) &seq;
 		      for (index = 0; index < 2; index++)
 			{
-			  sp[index] = dst0.as_u8[offset + index + 1] << shift;
+			  sp[index] = dst0.as_u8[offset + 1 + index] << shift;
 			  sp[index] |=
 			    dst0.as_u8[offset + index + 2] >> (8 - shift);
 			}
@@ -1380,9 +1465,9 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 		  hdrlen =
 		    sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
 		}
-	      else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		       || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+	      else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		       gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		       gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		{
 		  hdrlen = sizeof (gtpu_exthdr_t);
 		}
@@ -1402,11 +1487,10 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 		  if (ext_len >
 		      sizeof (ip6_address_t) * (ip6srv0->sr.last_entry + 1))
 		    {
-		      tlv =
-			(ip6_sr_tlv_t *) ((u8 *) & ip6srv0->sr +
-					  sizeof (ip6_sr_header_t) +
-					  sizeof (ip6_address_t) *
-					  (ip6srv0->sr.last_entry + 1));
+		      tlv = (ip6_sr_tlv_t *) ((u8 *) &ip6srv0->sr +
+					      sizeof (ip6_sr_header_t) +
+					      sizeof (ip6_address_t) *
+						(ip6srv0->sr.last_entry + 1));
 
 		      if (tlv->type == SRH_TLV_USER_PLANE_CONTAINER)
 			{
@@ -1422,9 +1506,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 		    }
 		}
 
-	      vlib_buffer_advance (b0,
-				   (word) sizeof (ip6srv_combo_header_t) +
-				   ip6srv0->sr.length * 8);
+	      vlib_buffer_advance (b0, (word) sizeof (ip6srv_combo_header_t) +
+					 ip6srv0->sr.length * 8);
 
 	      // get length of encapsulated IPv6 packet (the remaining part)
 	      p = vlib_buffer_get_current (b0);
@@ -1447,37 +1530,9 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 
 	      hdr0->gtpu.type = gtpu_type;
 
-	      if (qfi)
-		{
-		  u8 type = 0;
-		  gtpu_pdu_session_t *sess;
-
-		  hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
-
-		  hdr0->gtpu.ext->seq = 0;
-		  hdr0->gtpu.ext->npdu_num = 0;
-		  hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
-
-		  type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
-
-		  qfi =
-		    ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
-		    ((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
-
-		  sess =
-		    (gtpu_pdu_session_t *) (((char *) hdr0) +
-					    sizeof (ip6_gtpu_header_t) +
-					    sizeof (gtpu_exthdr_t));
-		  sess->exthdrlen = 1;
-		  sess->type = type;
-		  sess->spare = 0;
-		  sess->u.val = qfi;
-		  sess->nextexthdr = 0;
-		}
-
-	      if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		  || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		  || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+	      if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		  gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		  gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		{
 		  hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG;
 		  hdr0->gtpu.ext->seq = seq;
@@ -1506,29 +1561,57 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 			}
 		    }
 		}
+	      else
+		{
+		  if (qfi)
+		    {
+		      hdr0->gtpu.ext->seq = 0;
+		      hdr0->gtpu.ext->npdu_num = 0;
+		    }
+		}
+
+	      if (qfi)
+		{
+		  u8 type = 0;
+		  gtpu_pdu_session_t *sess;
+
+		  hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
 
-	      hdr0->udp.length = clib_host_to_net_u16 (len0 +
-						       sizeof (udp_header_t) +
-						       sizeof
-						       (gtpu_header_t));
+		  hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
+
+		  type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
+
+		  qfi = ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
+			((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
+
+		  sess = (gtpu_pdu_session_t *) (((char *) hdr0) +
+						 sizeof (ip6_gtpu_header_t) +
+						 sizeof (gtpu_exthdr_t));
+		  sess->exthdrlen = 1;
+		  sess->type = type;
+		  sess->spare = 0;
+		  sess->u.val = qfi;
+		  sess->nextexthdr = 0;
+		}
+
+	      hdr0->udp.length = clib_host_to_net_u16 (
+		len0 + sizeof (udp_header_t) + sizeof (gtpu_header_t));
 
 	      clib_memcpy_fast (hdr0->ip6.src_address.as_u8, src0.as_u8,
 				sizeof (ip6_address_t));
 	      clib_memcpy_fast (hdr0->ip6.dst_address.as_u8, &seg0.as_u8,
 				sizeof (ip6_address_t));
 
-	      hdr0->ip6.payload_length = clib_host_to_net_u16 (len0 +
-							       sizeof
-							       (udp_header_t)
-							       +
-							       sizeof
-							       (gtpu_header_t));
+	      hdr0->ip6.payload_length = clib_host_to_net_u16 (
+		len0 + sizeof (udp_header_t) + sizeof (gtpu_header_t));
 
 	      // UDP source port.
 	      key = hash_memory (p, plen < 40 ? plen : 40, 0);
 	      port = hash_uword_to_u16 (&key);
 	      hdr0->udp.src_port = port;
 
+	      vnet_buffer (b0)->sw_if_index[VLIB_TX] = ls_param->fib6_index;
+
 	      good_n++;
 
 	      if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
@@ -1544,11 +1627,12 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 		}
 	    }
 
-	  vlib_increment_combined_counter
-	    (((next0 ==
-	       SRV6_END_M_GTP6_E_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
-	      &(sm2->sr_ls_valid_counters)), thread_index,
-	     ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
+	  vlib_increment_combined_counter (
+	    ((next0 == SRV6_END_M_GTP6_E_NEXT_DROP) ?
+	       &(sm2->sr_ls_invalid_counters) :
+	       &(sm2->sr_ls_valid_counters)),
+	    thread_index, ls0 - sm2->localsids, 1,
+	    vlib_buffer_length_in_chain (vm, b0));
 
 	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 					   n_left_to_next, bi0, next0);
@@ -1567,491 +1651,570 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 }
 
 // Function for SRv6 GTP6.D function
-VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
-				  vlib_node_runtime_t * node,
-				  vlib_frame_t * frame)
+static inline u32
+srv6_gtp6_decap_processing (vlib_main_t *vm, vlib_node_runtime_t *node,
+			    vlib_buffer_t *b0)
 {
   srv6_end_main_v6_decap_t *sm = &srv6_end_main_v6_decap;
   ip6_sr_main_t *sm2 = &sr_main;
-  u32 n_left_from, next_index, *from, *to_next;
-  u32 thread_index = vm->thread_index;
 
-  u32 good_n = 0, bad_n = 0;
+  ip6_sr_localsid_t *ls0;
+  srv6_end_gtp6_d_param_t *ls_param;
 
-  from = vlib_frame_vector_args (frame);
-  n_left_from = frame->n_vectors;
-  next_index = node->cached_next_index;
+  ip6_gtpu_header_t *hdr0 = NULL;
+  uword len0;
 
-  while (n_left_from > 0)
+  ip6_address_t seg0, src0, dst0;
+  u32 teid = 0;
+  u8 *teidp;
+  u8 gtpu_type = 0;
+  u8 qfi;
+  u8 *qfip = NULL;
+  u16 seq = 0;
+  u8 *seqp;
+  u32 offset, shift;
+  u32 hdrlen;
+  ip6_header_t *encap = NULL;
+  gtpu_pdu_session_t *sess = NULL;
+  int ie_size = 0;
+  u16 tlv_siz = 0;
+  u8 ie_buf[GTPU_IE_MAX_SIZ];
+
+  u32 next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP6;
+
+  ls0 = pool_elt_at_index (sm2->localsids,
+			   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+
+  ls_param = (srv6_end_gtp6_d_param_t *) ls0->plugin_mem;
+
+  hdr0 = vlib_buffer_get_current (b0);
+
+  hdrlen = sizeof (ip6_gtpu_header_t);
+
+  len0 = vlib_buffer_length_in_chain (vm, b0);
+
+  if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP) ||
+      (hdr0->udp.dst_port != clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) ||
+      (len0 < sizeof (ip6_gtpu_header_t)))
     {
-      u32 n_left_to_next;
+      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
+    }
+  else
+    {
+      seg0 = ls_param->sr_prefix;
+      src0 = hdr0->ip6.src_address;
+      dst0 = hdr0->ip6.dst_address;
 
-      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
+      gtpu_type = hdr0->gtpu.type;
 
-      while (n_left_from > 0 && n_left_to_next > 0)
+      teid = hdr0->gtpu.teid;
+      teidp = (u8 *) &teid;
+
+      seqp = (u8 *) &seq;
+
+      if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
 	{
-	  u32 bi0;
-	  vlib_buffer_t *b0;
-	  ip6_sr_localsid_t *ls0;
-	  srv6_end_gtp6_param_t *ls_param;
+	  // Extention header.
+	  hdrlen += sizeof (gtpu_exthdr_t);
 
-	  ip6_gtpu_header_t *hdr0 = NULL;
-	  uword len0;
+	  seq = hdr0->gtpu.ext->seq;
 
-	  ip6_address_t seg0, src0;
-	  u32 teid = 0;
-	  u8 *teidp;
-	  u8 gtpu_type = 0;
-	  u8 qfi;
-	  u8 *qfip = NULL;
-	  u16 seq = 0;
-	  u8 *seqp;
-	  u32 offset, shift;
-	  u32 hdrlen;
-	  ip6_header_t *encap = NULL;
-	  gtpu_pdu_session_t *sess = NULL;
-	  int ie_size = 0;
-	  u16 tlv_siz = 0;
-	  u8 ie_buf[GTPU_IE_MAX_SIZ];
+	  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
+	    {
+	      // PDU Session Container.
+	      sess = (gtpu_pdu_session_t *) (((char *) hdr0) +
+					     sizeof (ip6_gtpu_header_t) +
+					     sizeof (gtpu_exthdr_t));
+	      qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
+	      qfip = (u8 *) &qfi;
 
-	  u32 next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP;
+	      hdrlen += sizeof (gtpu_pdu_session_t);
 
-	  // defaults
-	  bi0 = from[0];
-	  to_next[0] = bi0;
-	  from += 1;
-	  to_next += 1;
-	  n_left_from -= 1;
-	  n_left_to_next -= 1;
+	      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
+		{
+		  hdrlen += sizeof (gtpu_paging_policy_t);
+		}
+	    }
+	}
 
-	  b0 = vlib_get_buffer (vm, bi0);
-	  ls0 =
-	    pool_elt_at_index (sm2->localsids,
-			       vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+      offset = ls_param->sr_prefixlen / 8;
+      shift = ls_param->sr_prefixlen % 8;
 
-	  ls_param = (srv6_end_gtp6_param_t *) ls0->plugin_mem;
+      if (PREDICT_TRUE (shift == 0))
+	{
+	  if (qfip)
+	    {
+	      qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
+		    ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 
-	  hdr0 = vlib_buffer_get_current (b0);
+	      if (sess->type)
+		{
+		  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
+		}
 
-	  hdrlen = sizeof (ip6_gtpu_header_t);
+	      seg0.as_u8[offset] = qfi;
+	    }
 
-	  len0 = vlib_buffer_length_in_chain (vm, b0);
+	  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+	      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+	      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+	    {
+	      clib_memcpy_fast (&seg0.as_u8[offset + 1], seqp, 2);
+	    }
+	  else
+	    {
+	      clib_memcpy_fast (&seg0.as_u8[offset + 1], teidp, 4);
+	    }
+	}
+      else
+	{
+	  int idx;
 
-	  if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP)
-	      || (hdr0->udp.dst_port !=
-		  clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT))
-	      || (len0 < sizeof (ip6_gtpu_header_t)))
+	  if (qfip)
 	    {
-	      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
+	      qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
+		    ((qfi & ~GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 
-	      bad_n++;
+	      if (sess->type)
+		{
+		  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
+		}
+
+	      seg0.as_u8[offset] |= qfi >> shift;
+	      seg0.as_u8[offset + 1] |= qfi << (8 - shift);
+	    }
+
+	  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+	      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+	      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+	    {
+	      for (idx = 0; idx < 2; idx++)
+		{
+		  seg0.as_u8[offset + idx + 1] |= seqp[idx] >> shift;
+		  seg0.as_u8[offset + idx + 2] |= seqp[idx] << (8 - shift);
+		}
 	    }
 	  else
 	    {
-	      seg0 = ls_param->sr_prefix;
-	      src0 = hdr0->ip6.src_address;
+	      for (idx = 0; idx < 4; idx++)
+		{
+		  seg0.as_u8[offset + idx + 1] |= teidp[idx] >> shift;
+		  seg0.as_u8[offset + idx + 2] |= teidp[idx] << (8 - shift);
+		}
+	    }
+	}
 
-	      gtpu_type = hdr0->gtpu.type;
+      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
+	{
+	  u16 payload_len;
 
-	      teid = hdr0->gtpu.teid;
-	      teidp = (u8 *) & teid;
+	  payload_len = clib_net_to_host_u16 (hdr0->gtpu.length);
+	  if (payload_len != 0)
+	    {
+	      ie_size = payload_len - (hdrlen - sizeof (ip6_gtpu_header_t));
+	      if (ie_size > 0)
+		{
+		  u8 *ies;
 
-	      seqp = (u8 *) & seq;
+		  ies = (u8 *) ((u8 *) hdr0 + hdrlen);
+		  clib_memcpy_fast (ie_buf, ies, ie_size);
+		  hdrlen += ie_size;
+		}
+	    }
+	}
 
-	      if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
-		{
-		  // Extention header.
-		  hdrlen += sizeof (gtpu_exthdr_t);
+      // jump over variable length data
+      vlib_buffer_advance (b0, (word) hdrlen);
 
-		  seq = hdr0->gtpu.ext->seq;
+      // get length of encapsulated IPv6 packet (the remaining part)
+      len0 = vlib_buffer_length_in_chain (vm, b0);
 
-		  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
-		    {
-		      // PDU Session Container.
-		      sess =
-			(gtpu_pdu_session_t *) (((char *) hdr0) +
-						sizeof (ip6_gtpu_header_t) +
-						sizeof (gtpu_exthdr_t));
-		      qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
-		      qfip = (u8 *) & qfi;
+      if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
+	{
+	  encap = vlib_buffer_get_current (b0);
+	}
 
-		      hdrlen += sizeof (gtpu_pdu_session_t);
+      uword *p;
+      ip6srv_combo_header_t *ip6srv;
+      ip6_sr_policy_t *sr_policy = NULL;
+      ip6_sr_sl_t *sl = NULL;
+      u32 *sl_index;
+      u32 hdr_len;
 
-		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
-			{
-			  hdrlen += sizeof (gtpu_paging_policy_t);
-			}
-		    }
-		}
+      p = mhash_get (&sm2->sr_policies_index_hash, &ls_param->sr_prefix);
+      if (p)
+	{
+	  sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
+	}
 
-	      offset = ls_param->sr_prefixlen / 8;
-	      shift = ls_param->sr_prefixlen % 8;
+      if (sr_policy)
+	{
+	  vec_foreach (sl_index, sr_policy->segments_lists)
+	    {
+	      sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
+	      if (sl != NULL)
+		break;
+	    }
+	}
 
-	      if (PREDICT_TRUE (shift == 0))
-		{
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
-		    {
-		      clib_memcpy_fast (&seg0.as_u8[offset + 1], seqp, 2);
-		    }
-		  else
-		    {
-		      clib_memcpy_fast (&seg0.as_u8[offset + 1], teidp, 4);
-		    }
+      if (sl)
+	{
+	  hdr_len = sizeof (ip6srv_combo_header_t);
+	  hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
+	  hdr_len += sizeof (ip6_address_t) * 2;
+	}
+      else
+	{
+	  hdr_len = sizeof (ip6_header_t);
+	  hdr_len += sizeof (ip6_sr_header_t);
+	  hdr_len += sizeof (ip6_address_t) * 2;
+	}
 
-		  if (qfip)
-		    {
-		      qfi =
-			((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
-			((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
+      if (ie_size)
+	{
+	  tlv_siz =
+	    sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) + ie_size;
 
-		      if (sess->type)
-			{
-			  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
-			}
+	  tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0);
+	  hdr_len += tlv_siz;
+	}
 
-		      seg0.as_u8[offset] = qfi;
-		    }
-		}
-	      else
-		{
-		  int idx;
+      // jump back to data[0] or pre_data if required
+      vlib_buffer_advance (b0, -(word) hdr_len);
 
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
-		    {
-		      for (idx = 0; idx < 2; idx++)
-			{
-			  seg0.as_u8[offset + idx + 1] |= seqp[idx] >> shift;
-			  seg0.as_u8[offset + idx + 2] |=
-			    seqp[idx] << (8 - shift);
-			}
-		    }
-		  else
-		    {
-		      for (idx = 0; idx < 4; idx++)
-			{
-			  seg0.as_u8[offset + idx + 1] |= teidp[idx] >> shift;
-			  seg0.as_u8[offset + idx + 2] |=
-			    teidp[idx] << (8 - shift);
-			}
-		    }
+      ip6srv = vlib_buffer_get_current (b0);
 
-		  if (qfip)
-		    {
-		      qfi =
-			((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
-			((qfi & ~GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
+      if (sl)
+	{
+	  clib_memcpy_fast (ip6srv, sl->rewrite, vec_len (sl->rewrite));
 
-		      if (sess->type)
-			{
-			  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
-			}
+	  if (vec_len (sl->segments) > 1)
+	    {
+	      ip6srv->ip.src_address = src0;
 
-		      seg0.as_u8[offset] |= qfi >> shift;
-		      seg0.as_u8[offset + 1] |= qfi << (8 - shift);
-		    }
-		}
+	      ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 
-	      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
-		{
-		  u16 payload_len;
+	      ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
+	      ip6srv->sr.segments_left += 2;
+	      ip6srv->sr.last_entry += 2;
 
-		  payload_len = clib_net_to_host_u16 (hdr0->gtpu.length);
-		  if (payload_len != 0)
-		    {
-		      ie_size =
-			payload_len - (hdrlen - sizeof (ip6_gtpu_header_t));
-		      if (ie_size > 0)
-			{
-			  u8 *ies;
+	      ip6srv->sr.length += (sizeof (ip6_address_t) * 2) / 8;
+	      ip6srv->sr.segments[0] = dst0;
+	      ip6srv->sr.segments[1] = seg0;
 
-			  ies = (u8 *) ((u8 *) hdr0 + hdrlen);
-			  clib_memcpy_fast (ie_buf, ies, ie_size);
-			  hdrlen += ie_size;
-			}
-		    }
-		}
+	      clib_memcpy_fast (&ip6srv->sr.segments[2],
+				(u8 *) (sl->rewrite + sizeof (ip6_header_t) +
+					sizeof (ip6_sr_header_t)),
+				vec_len (sl->segments) *
+				  sizeof (ip6_address_t));
+	    }
+	  else
+	    {
+	      ip6srv->ip.src_address = src0;
+	      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
 
-	      // jump over variable length data
-	      vlib_buffer_advance (b0, (word) hdrlen);
+	      ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
+	      ip6srv->sr.segments_left = 2;
+	      ip6srv->sr.last_entry = 1;
+	      ip6srv->sr.length = (sizeof (ip6_address_t) * 2) / 8;
+	      ip6srv->sr.flags = 0;
 
-	      // get length of encapsulated IPv6 packet (the remaining part)
-	      len0 = vlib_buffer_length_in_chain (vm, b0);
+	      ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 
-	      if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
+	      ip6srv->sr.segments[0] = dst0;
+	      ip6srv->sr.segments[1] = seg0;
+
+	      if (vec_len (sl->segments))
 		{
-		  encap = vlib_buffer_get_current (b0);
+		  ip6srv->sr.segments[2] = sl->segments[0];
+		  ip6srv->sr.last_entry++;
+		  ip6srv->sr.length += sizeof (ip6_address_t) / 8;
 		}
+	    }
 
-	      uword *p;
-	      ip6srv_combo_header_t *ip6srv;
-	      ip6_sr_policy_t *sr_policy = NULL;
-	      ip6_sr_sl_t *sl = NULL;
-	      u32 *sl_index;
-	      u32 hdr_len;
-
-	      p =
-		mhash_get (&sm2->sr_policies_index_hash,
-			   &ls_param->sr_prefix);
-	      if (p)
+	  if (PREDICT_TRUE (encap != NULL))
+	    {
+	      if (ls_param->nhtype == SRV6_NHTYPE_NONE)
 		{
-		  sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) == 6)
+		    ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
+		  else
+		    ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
 		}
-
-	      if (sr_policy)
+	      else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
 		{
-		  vec_foreach (sl_index, sr_policy->segments_lists)
-		  {
-		    sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
-		    if (sl != NULL)
-		      break;
-		  }
+		  ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 4)
+		    {
+		      // Bad encap packet.
+		      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
+		      goto DONE;
+		    }
 		}
+	      else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
+		{
+		  ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 6)
+		    {
+		      // Bad encap packet.
+		      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
+		      goto DONE;
+		    }
+		}
+	      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
+		{
+		  ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
+		}
+	    }
+	  else
+	    {
+	      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
+	    }
+	}
+      else
+	{
+	  clib_memcpy_fast (ip6srv, &sm->cache_hdr, sizeof (ip6_header_t));
 
-	      if (sl)
+	  ip6srv->ip.src_address = src0;
+	  ip6srv->ip.dst_address = seg0;
+
+	  ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+
+	  ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
+	  ip6srv->sr.segments_left = 1;
+	  ip6srv->sr.last_entry = 1;
+
+	  ip6srv->sr.length = (sizeof (ip6_address_t) * 2) / 8;
+	  ip6srv->sr.segments[0] = dst0;
+	  ip6srv->sr.segments[1] = seg0;
+
+	  if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
+	    {
+	      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
+	      ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+	    }
+	  else
+	    {
+	      if (ls_param->nhtype == SRV6_NHTYPE_NONE)
 		{
-		  hdr_len = sizeof (ip6srv_combo_header_t);
-		  hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
-		  hdr_len += sizeof (ip6_address_t);
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 6)
+		    ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
 		}
-	      else
+	      else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
 		{
-		  hdr_len = sizeof (ip6_header_t);
-		  if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
+		  ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 4)
 		    {
-		      hdr_len += sizeof (ip6_sr_header_t);
-		      hdr_len += sizeof (ip6_address_t);
+		      // Bad encap packet.
+		      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
+		      goto DONE;
 		    }
 		}
-
-	      if (ie_size)
+	      else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
 		{
-		  tlv_siz =
-		    sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) +
-		    ie_size;
-
-		  tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0);
-		  hdr_len += tlv_siz;
+		  ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
+		  if ((clib_net_to_host_u32 (
+			 encap->ip_version_traffic_class_and_flow_label) >>
+		       28) != 6)
+		    {
+		      // Bad encap packet.
+		      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
+		      goto DONE;
+		    }
+		}
+	      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
+		{
+		  ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
 		}
+	    }
+	}
 
-	      // jump back to data[0] or pre_data if required
-	      vlib_buffer_advance (b0, -(word) hdr_len);
+      if (PREDICT_FALSE (ie_size))
+	{
+	  ip6_sr_tlv_t *tlv;
+	  user_plane_sub_tlv_t *sub_tlv;
 
-	      ip6srv = vlib_buffer_get_current (b0);
+	  tlv = (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
+	  tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
+	  tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t));
+	  clib_memset (tlv->value, 0, tlv->length);
 
-	      if (sl)
-		{
-		  clib_memcpy_fast (ip6srv, sl->rewrite,
-				    vec_len (sl->rewrite));
+	  sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
+	  sub_tlv->type = USER_PLANE_SUB_TLV_IE;
+	  sub_tlv->length = (u8) ie_size;
+	  clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
 
-		  if (vec_len (sl->segments) > 1)
-		    {
-		      ip6srv->ip.src_address = src0;
+	  ip6srv->sr.length += (u8) (tlv_siz / 8);
+	}
 
-		      ip6srv->sr.tag =
-			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+      ip6srv->ip.payload_length =
+	clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
 
-		      ip6srv->sr.segments_left += 1;
-		      ip6srv->sr.last_entry += 1;
+      vnet_buffer (b0)->sw_if_index[VLIB_TX] = 0; /* default FIB */
 
-		      ip6srv->sr.length += sizeof (ip6_address_t) / 8;
-		      ip6srv->sr.segments[0] = seg0;
+      if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
+	  PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
+	{
+	  srv6_end_rewrite_trace_t *tr =
+	    vlib_add_trace (vm, node, b0, sizeof (*tr));
+	  clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
+		       sizeof (ip6_address_t));
+	  clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
+		       sizeof (ip6_address_t));
+	  tr->teid = teid;
+	  clib_memcpy (tr->sr_prefix.as_u8, ls_param->sr_prefix.as_u8,
+		       sizeof (ip6_address_t));
+	  tr->sr_prefixlen = ls_param->sr_prefixlen;
+	}
+    }
 
-		      clib_memcpy_fast (&ip6srv->sr.segments[1],
-					(u8 *) (sl->rewrite +
-						sizeof (ip6_header_t) +
-						sizeof (ip6_sr_header_t)),
-					vec_len (sl->segments) *
-					sizeof (ip6_address_t));
-		    }
-		  else
-		    {
-		      ip6srv->ip.src_address = src0;
-		      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+DONE:
+  return next0;
+}
 
-		      ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
-		      ip6srv->sr.segments_left = 1;
-		      ip6srv->sr.last_entry = 0;
-		      ip6srv->sr.length =
-			((sizeof (ip6_sr_header_t) +
-			  sizeof (ip6_address_t)) / 8) - 1;
-		      ip6srv->sr.flags = 0;
+VLIB_NODE_FN (srv6_end_m_gtp6_d)
+(vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
+{
+  srv6_end_main_v6_decap_t *sm = &srv6_end_main_v6_decap;
+  ip6_sr_main_t *sm2 = &sr_main;
+  u32 n_left_from, next_index, *from, *to_next;
+  u32 thread_index = vm->thread_index;
+  ip6_sr_localsid_t *ls0;
+  srv6_end_gtp6_d_param_t *ls_param;
 
-		      ip6srv->sr.tag =
-			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+  u32 good_n = 0, bad_n = 0;
 
-		      ip6srv->sr.segments[0] = seg0;
+  from = vlib_frame_vector_args (frame);
+  n_left_from = frame->n_vectors;
+  next_index = node->cached_next_index;
 
-		      if (vec_len (sl->segments))
-			{
-			  ip6srv->sr.segments[1] = sl->segments[0];
-			  ip6srv->sr.last_entry++;
-			  ip6srv->sr.length += sizeof (ip6_address_t) / 8;
-			}
-		    }
+  while (n_left_from > 0)
+    {
+      u32 n_left_to_next;
 
-		  if (PREDICT_TRUE (encap != NULL))
-		    {
-		      if (ls_param->nhtype == SRV6_NHTYPE_NONE)
-			{
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) == 6)
-			    ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
-			  else
-			    ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
-			{
-			  ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 4)
-			    {
-			      // Bad encap packet.
-			      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
-			      bad_n++;
-			      goto DONE;
-			    }
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
-			{
-			  ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 6)
-			    {
-			      // Bad encap packet.
-			      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
-			      bad_n++;
-			      goto DONE;
-			    }
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
-			{
-			  ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
-			}
-		    }
-		  else
-		    {
-		      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
-		    }
-		}
-	      else
-		{
-		  clib_memcpy_fast (ip6srv, &sm->cache_hdr,
-				    sizeof (ip6_header_t));
+      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 
-		  ip6srv->ip.src_address = src0;
-		  ip6srv->ip.dst_address = seg0;
+      while (n_left_from > 0 && n_left_to_next > 0)
+	{
+	  u32 bi0;
+	  vlib_buffer_t *b0;
 
-		  if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
-		    {
-		      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+	  u32 next0;
 
-		      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
+	  ip6_gtpu_header_t *hdr;
+	  u32 hdrlen;
+	  u8 gtpu_type;
+	  bool gtp6;
+	  bool ipv4;
 
-		      ip6srv->sr.tag =
-			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+	  // defaults
+	  bi0 = from[0];
+	  to_next[0] = bi0;
+	  from += 1;
+	  to_next += 1;
+	  n_left_from -= 1;
+	  n_left_to_next -= 1;
+
+	  b0 = vlib_get_buffer (vm, bi0);
 
-		      ip6srv->sr.segments_left = 0;
-		      ip6srv->sr.last_entry = 0;
+	  ls0 = pool_elt_at_index (sm2->localsids,
+				   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 
-		      ip6srv->sr.length = sizeof (ip6_address_t) / 8;
-		      ip6srv->sr.segments[0] = seg0;
-		    }
-		  else
+	  ls_param = (srv6_end_gtp6_d_param_t *) ls0->plugin_mem;
+
+	  hdr = vlib_buffer_get_current (b0);
+	  gtpu_type = hdr->gtpu.type;
+
+	  gtp6 = false;
+	  ipv4 = true;
+
+	  if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU || ls_param->drop_in))
+	    {
+	      gtp6 = true;
+	    }
+	  else
+	    {
+	      ip6_header_t *ip6;
+
+	      hdrlen = sizeof (ip6_gtpu_header_t);
+
+	      if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
+		{
+		  hdrlen += sizeof (gtpu_exthdr_t);
+		  if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 		    {
-		      if (ls_param->nhtype == SRV6_NHTYPE_NONE)
-			{
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 6)
-			    ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
-			{
-			  ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 4)
-			    {
-			      // Bad encap packet.
-			      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
-			      bad_n++;
-			      goto DONE;
-			    }
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
-			{
-			  ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
-			  if ((clib_net_to_host_u32
-			       (encap->ip_version_traffic_class_and_flow_label)
-			       >> 28) != 6)
-			    {
-			      // Bad encap packet.
-			      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
-			      bad_n++;
-			      goto DONE;
-			    }
-			}
-		      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
+		      gtpu_pdu_session_t *sess;
+		      sess = (gtpu_pdu_session_t *) (((char *) hdr) + hdrlen);
+		      hdrlen += sizeof (gtpu_pdu_session_t);
+
+		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
 			{
-			  ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
+			  hdrlen += sizeof (gtpu_paging_policy_t);
 			}
 		    }
 		}
 
-	      if (PREDICT_FALSE (ie_size))
+	      ip6 = (ip6_header_t *) (((char *) hdr) + hdrlen);
+	      if ((clib_net_to_host_u32 (
+		     ip6->ip_version_traffic_class_and_flow_label) >>
+		   28) == 6)
 		{
-		  ip6_sr_tlv_t *tlv;
-		  user_plane_sub_tlv_t *sub_tlv;
-
-		  tlv =
-		    (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
-		  tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
-		  tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t));
-		  clib_memset (tlv->value, 0, tlv->length);
-
-		  sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
-		  sub_tlv->type = USER_PLANE_SUB_TLV_IE;
-		  sub_tlv->length = (u8) ie_size;
-		  clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
-
-		  ip6srv->sr.length += (u8) (tlv_siz / 8);
+		  ipv4 = false;
+		  if (((ip6->dst_address.as_u8[0] == 0xff) &&
+		       (ip6->dst_address.as_u8[1] == 0x02)) ||
+		      ((ip6->dst_address.as_u8[0] == 0xfe) &&
+		       ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
+		    {
+		      // Inner desitnation is IPv6 link local
+		      gtp6 = true;
+		    }
 		}
+	    }
 
-	      ip6srv->ip.payload_length =
-		clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
-
-	      good_n++;
+	  if (gtp6)
+	    {
+	      next0 = srv6_gtp6_decap_processing (vm, node, b0);
+	      if (PREDICT_TRUE (next0 == SRV6_END_M_GTP6_D_NEXT_LOOKUP6))
+		good_n++;
+	      else
+		bad_n++;
+
+	      vlib_increment_combined_counter (
+		((next0 == SRV6_END_M_GTP6_D_NEXT_DROP) ?
+		   &(sm2->sr_ls_invalid_counters) :
+		   &(sm2->sr_ls_valid_counters)),
+		thread_index, ls0 - sm2->localsids, 1,
+		vlib_buffer_length_in_chain (vm, b0));
+	    }
+	  else
+	    {
+	      /* Strip off the outer header (IPv6 + GTP + UDP + IEs) */
+	      vlib_buffer_advance (b0, (word) hdrlen);
 
-	      if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
-		  PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
+	      if (ipv4)
 		{
-		  srv6_end_rewrite_trace_t *tr =
-		    vlib_add_trace (vm, node, b0, sizeof (*tr));
-		  clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
-			       sizeof (ip6_address_t));
-		  clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
-			       sizeof (ip6_address_t));
-		  tr->teid = teid;
-		  clib_memcpy (tr->sr_prefix.as_u8, ls_param->sr_prefix.as_u8,
-			       sizeof (ip6_address_t));
-		  tr->sr_prefixlen = ls_param->sr_prefixlen;
+		  next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP4;
+		  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
+		    ls_param->fib4_index;
+		}
+	      else
+		{
+		  next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP6;
+		  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
+		    ls_param->fib6_index;
 		}
 	    }
 
-	DONE:
-	  vlib_increment_combined_counter
-	    (((next0 ==
-	       SRV6_END_M_GTP6_D_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
-	      &(sm2->sr_ls_valid_counters)), thread_index,
-	     ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
-
 	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 					   n_left_to_next, bi0, next0);
 	}
@@ -2069,15 +2232,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
 }
 
 // Function for SRv6 GTP6.D.DI function
-VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
-				     vlib_node_runtime_t * node,
-				     vlib_frame_t * frame)
+VLIB_NODE_FN (srv6_end_m_gtp6_d_di)
+(vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
 {
   srv6_end_main_v6_decap_di_t *sm = &srv6_end_main_v6_decap_di;
   ip6_sr_main_t *sm2 = &sr_main;
   u32 n_left_from, next_index, *from, *to_next;
   u32 thread_index = vm->thread_index;
-  srv6_end_gtp6_param_t *ls_param;
+  srv6_end_gtp6_d_param_t *ls_param;
 
   u32 good_n = 0, bad_n = 0;
 
@@ -2129,11 +2291,10 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 	  n_left_to_next -= 1;
 
 	  b0 = vlib_get_buffer (vm, bi0);
-	  ls0 =
-	    pool_elt_at_index (sm2->localsids,
-			       vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+	  ls0 = pool_elt_at_index (sm2->localsids,
+				   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 
-	  ls_param = (srv6_end_gtp6_param_t *) ls0->plugin_mem;
+	  ls_param = (srv6_end_gtp6_d_param_t *) ls0->plugin_mem;
 
 	  hdr0 = vlib_buffer_get_current (b0);
 
@@ -2141,10 +2302,10 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 
 	  len0 = vlib_buffer_length_in_chain (vm, b0);
 
-	  if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP)
-	      || (hdr0->udp.dst_port !=
-		  clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT))
-	      || (len0 < sizeof (ip6_gtpu_header_t)))
+	  if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP) ||
+	      (hdr0->udp.dst_port !=
+	       clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) ||
+	      (len0 < sizeof (ip6_gtpu_header_t)))
 	    {
 	      next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
 
@@ -2159,9 +2320,9 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 
 	      seg0 = ls_param->sr_prefix;
 	      teid = hdr0->gtpu.teid;
-	      teidp = (u8 *) & teid;
+	      teidp = (u8 *) &teid;
 
-	      seqp = (u8 *) & seq;
+	      seqp = (u8 *) &seq;
 
 	      if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
 		{
@@ -2173,8 +2334,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 		    {
 		      // PDU Session Container.
-		      sess =
-			(gtpu_pdu_session_t *) (((char *) hdr0) + hdrlen);
+		      sess = (gtpu_pdu_session_t *) (((char *) hdr0) + hdrlen);
 		      qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
 		      qfip = &qfi;
 
@@ -2190,71 +2350,70 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 	      offset = ls_param->sr_prefixlen / 8;
 	      shift = ls_param->sr_prefixlen % 8;
 
+	      offset += 1;
 	      if (PREDICT_TRUE (shift == 0))
 		{
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		    {
-		      clib_memcpy_fast (&seg0.as_u8[offset + 1], seqp, 2);
+		      clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
 		    }
 		  else
 		    {
-		      clib_memcpy_fast (&seg0.as_u8[offset + 1], teidp, 4);
+		      clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
 		    }
 
 		  if (qfip)
 		    {
-		      qfi =
-			((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
-			((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
+		      qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
+			    ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 
 		      if (sess->type)
 			{
 			  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
 			}
 
-		      seg0.as_u8[offset] = qfi;
+		      seg0.as_u8[offset + 4] = qfi;
 		    }
 		}
 	      else
 		{
 		  int idx;
 
-		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
-		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
-		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
+		      gtpu_type == GTPU_TYPE_ECHO_REPLY ||
+		      gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		    {
 		      for (idx = 0; idx < 2; idx++)
 			{
-			  seg0.as_u8[offset + idx + 1] |= seqp[idx] >> shift;
-			  seg0.as_u8[offset + idx + 2] |=
-			    seqp[idx] << (8 - shift);
+			  seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
+			  seg0.as_u8[offset + idx + 1] |= seqp[idx]
+							  << (8 - shift);
 			}
 		    }
 		  else
 		    {
 		      for (idx = 0; idx < 4; idx++)
 			{
-			  seg0.as_u8[offset + idx + 1] |= teidp[idx] >> shift;
-			  seg0.as_u8[offset + idx + 2] |=
-			    teidp[idx] << (8 - shift);
+			  seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
+			  seg0.as_u8[offset + idx + 1] |= teidp[idx]
+							  << (8 - shift);
 			}
 		    }
 
 		  if (qfip)
 		    {
-		      qfi =
-			((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
-			((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
+		      qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
+			    ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 
 		      if (sess->type)
 			{
 			  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
 			}
 
-		      seg0.as_u8[offset] |= qfi >> shift;
-		      seg0.as_u8[offset + 1] |= qfi << (8 - shift);
+		      seg0.as_u8[offset + 4] |= qfi >> shift;
+		      seg0.as_u8[offset + 5] |= qfi << (8 - shift);
 		    }
 		}
 
@@ -2297,8 +2456,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 	      u32 hdr_len;
 
 	      p =
-		mhash_get (&sm2->sr_policies_index_hash,
-			   &ls_param->sr_prefix);
+		mhash_get (&sm2->sr_policies_index_hash, &ls_param->sr_prefix);
 	      if (p)
 		{
 		  sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
@@ -2307,11 +2465,11 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 	      if (sr_policy)
 		{
 		  vec_foreach (sl_index, sr_policy->segments_lists)
-		  {
-		    sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
-		    if (sl != NULL)
-		      break;
-		  }
+		    {
+		      sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
+		      if (sl != NULL)
+			break;
+		    }
 		}
 
 	      hdr_len = sizeof (ip6srv_combo_header_t);
@@ -2323,9 +2481,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 
 	      if (ie_size)
 		{
-		  tlv_siz =
-		    sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) +
-		    ie_size;
+		  tlv_siz = sizeof (ip6_sr_tlv_t) +
+			    sizeof (user_plane_sub_tlv_t) + ie_size;
 
 		  tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0);
 		  hdr_len += tlv_siz;
@@ -2356,12 +2513,11 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		      ip6srv->sr.segments[0] = dst0;
 		      ip6srv->sr.segments[1] = seg0;
 
-		      clib_memcpy_fast (&ip6srv->sr.segments[2],
-					(u8 *) (sl->rewrite +
-						sizeof (ip6_header_t) +
-						sizeof (ip6_sr_header_t)),
-					vec_len (sl->segments) *
-					sizeof (ip6_address_t));
+		      clib_memcpy_fast (
+			&ip6srv->sr.segments[2],
+			(u8 *) (sl->rewrite + sizeof (ip6_header_t) +
+				sizeof (ip6_sr_header_t)),
+			vec_len (sl->segments) * sizeof (ip6_address_t));
 		    }
 		  else
 		    {
@@ -2371,9 +2527,10 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		      ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
 		      ip6srv->sr.segments_left = 2;
 		      ip6srv->sr.last_entry = 1;
-		      ip6srv->sr.length =
-			((sizeof (ip6_sr_header_t) +
-			  2 * sizeof (ip6_address_t)) / 8) - 1;
+		      ip6srv->sr.length = ((sizeof (ip6_sr_header_t) +
+					    2 * sizeof (ip6_address_t)) /
+					   8) -
+					  1;
 		      ip6srv->sr.flags = 0;
 
 		      ip6srv->sr.tag =
@@ -2402,8 +2559,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		  ip6srv->sr.segments_left = 1;
 		  ip6srv->sr.last_entry = 0;
 		  ip6srv->sr.length =
-		    ((sizeof (ip6_sr_header_t) +
-		      sizeof (ip6_address_t)) / 8) - 1;
+		    ((sizeof (ip6_sr_header_t) + sizeof (ip6_address_t)) / 8) -
+		    1;
 		  ip6srv->sr.flags = 0;
 
 		  ip6srv->sr.tag =
@@ -2417,8 +2574,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		  ip6_sr_tlv_t *tlv;
 		  user_plane_sub_tlv_t *sub_tlv;
 
-		  tlv =
-		    (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
+		  tlv = (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
 		  tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
 		  tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t));
 		  clib_memset (tlv->value, 0, tlv->length);
@@ -2438,8 +2594,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		{
 		  if (ls_param->nhtype == SRV6_NHTYPE_NONE)
 		    {
-		      if ((clib_net_to_host_u32
-			   (encap->ip_version_traffic_class_and_flow_label) >>
+		      if ((clib_net_to_host_u32 (
+			     encap->ip_version_traffic_class_and_flow_label) >>
 			   28) == 6)
 			ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
 		      else
@@ -2448,8 +2604,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		  else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
 		    {
 		      ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
-		      if ((clib_net_to_host_u32
-			   (encap->ip_version_traffic_class_and_flow_label) >>
+		      if ((clib_net_to_host_u32 (
+			     encap->ip_version_traffic_class_and_flow_label) >>
 			   28) != 4)
 			{
 			  // Bad encap packet.
@@ -2461,8 +2617,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		  else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
 		    {
 		      ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
-		      if ((clib_net_to_host_u32
-			   (encap->ip_version_traffic_class_and_flow_label) >>
+		      if ((clib_net_to_host_u32 (
+			     encap->ip_version_traffic_class_and_flow_label) >>
 			   28) != 6)
 			{
 			  // Bad encap packet.
@@ -2500,12 +2656,12 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 	    }
 
 	DONE:
-	  vlib_increment_combined_counter
-	    (((next0 ==
-	       SRV6_END_M_GTP6_D_DI_NEXT_DROP) ?
-	      &(sm2->sr_ls_invalid_counters) : &(sm2->sr_ls_valid_counters)),
-	     thread_index, ls0 - sm2->localsids, 1,
-	     vlib_buffer_length_in_chain (vm, b0));
+	  vlib_increment_combined_counter (
+	    ((next0 == SRV6_END_M_GTP6_D_DI_NEXT_DROP) ?
+	       &(sm2->sr_ls_invalid_counters) :
+	       &(sm2->sr_ls_valid_counters)),
+	    thread_index, ls0 - sm2->localsids, 1,
+	    vlib_buffer_length_in_chain (vm, b0));
 
 	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 					   n_left_to_next, bi0, next0);
@@ -2524,9 +2680,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 }
 
 // Function for SRv6 GTP6.DT function
-VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
-				   vlib_node_runtime_t * node,
-				   vlib_frame_t * frame)
+VLIB_NODE_FN (srv6_end_m_gtp6_dt)
+(vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
 {
   srv6_end_main_v6_dt_t *sm = &srv6_end_main_v6_dt;
   ip6_sr_main_t *sm2 = &sr_main;
@@ -2570,9 +2725,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
 	  n_left_to_next -= 1;
 
 	  b0 = vlib_get_buffer (vm, bi0);
-	  ls0 =
-	    pool_elt_at_index (sm2->localsids,
-			       vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+	  ls0 = pool_elt_at_index (sm2->localsids,
+				   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 
 	  ls_param = (srv6_end_gtp6_dt_param_t *) ls0->plugin_mem;
 
@@ -2582,10 +2736,10 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
 
 	  len0 = vlib_buffer_length_in_chain (vm, b0);
 
-	  if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP)
-	      || (hdr0->udp.dst_port !=
-		  clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT))
-	      || (len0 < sizeof (ip6_gtpu_header_t)))
+	  if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP) ||
+	      (hdr0->udp.dst_port !=
+	       clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) ||
+	      (len0 < sizeof (ip6_gtpu_header_t)))
 	    {
 	      next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
 
@@ -2638,9 +2792,9 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
 	      else if (ls_param->type == SRV6_GTP6_DT6)
 		{
 		  ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen);
-		  if ((clib_net_to_host_u32
-		       (ip6->ip_version_traffic_class_and_flow_label) >> 28)
-		      != 6)
+		  if ((clib_net_to_host_u32 (
+			 ip6->ip_version_traffic_class_and_flow_label) >>
+		       28) != 6)
 		    {
 		      next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
 		      bad_n++;
@@ -2648,8 +2802,10 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
 		    }
 
 		  next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
-		  if ((ip6->dst_address.as_u8[0] == 0xff)
-		      && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
+		  if (((ip6->dst_address.as_u8[0] == 0xff) &&
+		       (ip6->dst_address.as_u8[1] == 0x02)) ||
+		      ((ip6->dst_address.as_u8[0] == 0xfe) &&
+		       ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
 		    {
 		      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
 			ls_param->local_fib_index;
@@ -2664,13 +2820,15 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
 	      else if (ls_param->type == SRV6_GTP6_DT46)
 		{
 		  ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen);
-		  if ((clib_net_to_host_u32
-		       (ip6->ip_version_traffic_class_and_flow_label) >> 28)
-		      == 6)
+		  if ((clib_net_to_host_u32 (
+			 ip6->ip_version_traffic_class_and_flow_label) >>
+		       28) == 6)
 		    {
 		      next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
-		      if ((ip6->dst_address.as_u8[0] == 0xff)
-			  && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
+		      if (((ip6->dst_address.as_u8[0] == 0xff) &&
+			   (ip6->dst_address.as_u8[1] == 0x02)) ||
+			  ((ip6->dst_address.as_u8[0] == 0xfe) &&
+			   ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
 			{
 			  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
 			    ls_param->local_fib_index;
@@ -2682,10 +2840,9 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
 			    ls_param->fib6_index;
 			}
 		    }
-		  else
-		    if ((clib_net_to_host_u32
-			 (ip6->ip_version_traffic_class_and_flow_label) >> 28)
-			== 4)
+		  else if ((clib_net_to_host_u32 (
+			      ip6->ip_version_traffic_class_and_flow_label) >>
+			    28) == 4)
 		    {
 		      vlib_buffer_advance (b0, (word) hdrlen);
 		      next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP4;
@@ -2722,11 +2879,12 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
 	    }
 
 	DONE:
-	  vlib_increment_combined_counter
-	    (((next0 ==
-	       SRV6_END_M_GTP6_DT_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters)
-	      : &(sm2->sr_ls_valid_counters)), thread_index,
-	     ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
+	  vlib_increment_combined_counter (
+	    ((next0 == SRV6_END_M_GTP6_DT_NEXT_DROP) ?
+	       &(sm2->sr_ls_invalid_counters) :
+	       &(sm2->sr_ls_valid_counters)),
+	    thread_index, ls0 - sm2->localsids, 1,
+	    vlib_buffer_length_in_chain (vm, b0));
 
 	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 					   n_left_to_next, bi0, next0);
@@ -2745,9 +2903,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
 }
 
 // Function for SRv6 GTP4.DT function
-VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
-				 vlib_node_runtime_t * node,
-				 vlib_frame_t * frame)
+VLIB_NODE_FN (srv6_t_m_gtp4_dt)
+(vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
 {
   srv6_t_main_v4_dt_t *sm = &srv6_t_main_v4_dt;
   ip6_sr_main_t *sm2 = &sr_main;
@@ -2790,9 +2947,8 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
 	  n_left_to_next -= 1;
 
 	  b0 = vlib_get_buffer (vm, bi0);
-	  ls0 =
-	    pool_elt_at_index (sm2->sid_lists,
-			       vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
+	  ls0 = pool_elt_at_index (sm2->sid_lists,
+				   vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 
 	  ls_param = (srv6_t_gtp4_dt_param_t *) ls0->plugin_mem;
 
@@ -2802,10 +2958,10 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
 
 	  len0 = vlib_buffer_length_in_chain (vm, b0);
 
-	  if ((hdr0->ip4.protocol != IP_PROTOCOL_UDP)
-	      || (hdr0->udp.dst_port !=
-		  clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT))
-	      || (len0 < sizeof (ip4_gtpu_header_t)))
+	  if ((hdr0->ip4.protocol != IP_PROTOCOL_UDP) ||
+	      (hdr0->udp.dst_port !=
+	       clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) ||
+	      (len0 < sizeof (ip4_gtpu_header_t)))
 	    {
 	      next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
 
@@ -2858,9 +3014,9 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
 	      else if (ls_param->type == SRV6_GTP4_DT6)
 		{
 		  ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen);
-		  if ((clib_net_to_host_u32
-		       (ip6->ip_version_traffic_class_and_flow_label) >> 28)
-		      != 6)
+		  if ((clib_net_to_host_u32 (
+			 ip6->ip_version_traffic_class_and_flow_label) >>
+		       28) != 6)
 		    {
 		      next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
 		      bad_n++;
@@ -2868,8 +3024,10 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
 		    }
 
 		  next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
-		  if ((ip6->dst_address.as_u8[0] == 0xff)
-		      && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
+		  if (((ip6->dst_address.as_u8[0] == 0xff) &&
+		       (ip6->dst_address.as_u8[1] == 0x02)) ||
+		      ((ip6->dst_address.as_u8[0] == 0xfe) &&
+		       ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
 		    {
 		      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
 		      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
@@ -2885,13 +3043,15 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
 	      else if (ls_param->type == SRV6_GTP4_DT46)
 		{
 		  ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen);
-		  if ((clib_net_to_host_u32
-		       (ip6->ip_version_traffic_class_and_flow_label) >> 28)
-		      == 6)
+		  if ((clib_net_to_host_u32 (
+			 ip6->ip_version_traffic_class_and_flow_label) >>
+		       28) == 6)
 		    {
 		      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
-		      if ((ip6->dst_address.as_u8[0] == 0xff)
-			  && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
+		      if (((ip6->dst_address.as_u8[0] == 0xff) &&
+			   (ip6->dst_address.as_u8[1] == 0x02)) ||
+			  ((ip6->dst_address.as_u8[0] == 0xfe) &&
+			   ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
 			{
 			  next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
 			  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
@@ -2904,10 +3064,9 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
 			    ls_param->fib6_index;
 			}
 		    }
-		  else
-		    if ((clib_net_to_host_u32
-			 (ip6->ip_version_traffic_class_and_flow_label) >> 28)
-			== 4)
+		  else if ((clib_net_to_host_u32 (
+			      ip6->ip_version_traffic_class_and_flow_label) >>
+			    28) == 4)
 		    {
 		      vlib_buffer_advance (b0, (word) hdrlen);
 		      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
@@ -2968,8 +3127,8 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp6_e) =
     srv6_end_error_v6_e_strings,.n_next_nodes =
     SRV6_END_M_GTP6_E_N_NEXT,.next_nodes =
   {
-  [SRV6_END_M_GTP6_E_NEXT_DROP] =
-      "error-drop",[SRV6_END_M_GTP6_E_NEXT_LOOKUP] = "ip6-lookup",}
+  [SRV6_END_M_GTP6_E_NEXT_DROP] = "error-drop",
+  [SRV6_END_M_GTP6_E_NEXT_LOOKUP] = "ip6-lookup",}
 ,};
 
 VLIB_REGISTER_NODE (srv6_end_m_gtp6_d) =
@@ -2980,8 +3139,9 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp6_d) =
     srv6_end_error_v6_d_strings,.n_next_nodes =
     SRV6_END_M_GTP6_D_N_NEXT,.next_nodes =
   {
-  [SRV6_END_M_GTP6_D_NEXT_DROP] =
-      "error-drop",[SRV6_END_M_GTP6_D_NEXT_LOOKUP] = "ip6-lookup",}
+  [SRV6_END_M_GTP6_D_NEXT_DROP] = "error-drop",
+  [SRV6_END_M_GTP6_D_NEXT_LOOKUP4] = "ip4-lookup",
+  [SRV6_END_M_GTP6_D_NEXT_LOOKUP6] = "ip6-lookup",}
 ,};
 
 VLIB_REGISTER_NODE (srv6_end_m_gtp6_d_di) =
@@ -2993,7 +3153,7 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp6_d_di) =
     SRV6_END_M_GTP6_D_DI_N_NEXT,.next_nodes =
   {
   [SRV6_END_M_GTP6_D_DI_NEXT_DROP] = "error-drop",
-      [SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP] = "ip6-lookup",}
+  [SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP] = "ip6-lookup",}
 ,};
 
 VLIB_REGISTER_NODE (srv6_end_m_gtp6_dt) =
@@ -3004,10 +3164,9 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp6_dt) =
     srv6_end_error_v6_dt_strings,.n_next_nodes =
     SRV6_END_M_GTP6_DT_N_NEXT,.next_nodes =
   {
-  [SRV6_END_M_GTP6_DT_NEXT_DROP] =
-      "error-drop",
-      [SRV6_END_M_GTP6_DT_NEXT_LOOKUP4]
-      = "ip4-lookup",[SRV6_END_M_GTP6_DT_NEXT_LOOKUP6] = "ip6-lookup",}
+  [SRV6_END_M_GTP6_DT_NEXT_DROP] = "error-drop",
+  [SRV6_END_M_GTP6_DT_NEXT_LOOKUP4] = "ip4-lookup",
+  [SRV6_END_M_GTP6_DT_NEXT_LOOKUP6] = "ip6-lookup",}
 ,};
 
 VLIB_REGISTER_NODE (srv6_t_m_gtp4_dt) =
@@ -3018,10 +3177,9 @@ VLIB_REGISTER_NODE (srv6_t_m_gtp4_dt) =
     srv6_t_error_v4_dt_strings,.n_next_nodes =
     SRV6_T_M_GTP4_DT_N_NEXT,.next_nodes =
   {
-  [SRV6_T_M_GTP4_DT_NEXT_DROP] =
-      "error-drop",
-      [SRV6_T_M_GTP4_DT_NEXT_LOOKUP4] =
-      "ip4-lookup",[SRV6_T_M_GTP4_DT_NEXT_LOOKUP6] = "ip6-lookup",}
+  [SRV6_T_M_GTP4_DT_NEXT_DROP] = "error-drop",
+  [SRV6_T_M_GTP4_DT_NEXT_LOOKUP4] = "ip4-lookup",
+  [SRV6_T_M_GTP4_DT_NEXT_LOOKUP6] = "ip6-lookup",}
 ,};
 
 /*