srv6-mobile: Update GTP4/6.D function 12/34712/8
authorTetsuya Murakami <tetsuya.mrk@gmail.com>
Fri, 10 Dec 2021 16:11:07 +0000 (08:11 -0800)
committerOle Tr�an <otroan@employees.org>
Wed, 5 Jan 2022 10:52:22 +0000 (10:52 +0000)
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>
src/plugins/srv6-mobile/extra/Dockerfile.j2
src/plugins/srv6-mobile/extra/Dockerfile.j2.release
src/plugins/srv6-mobile/gtp4_d.c
src/plugins/srv6-mobile/gtp4_e.c
src/plugins/srv6-mobile/gtp6_d.c
src/plugins/srv6-mobile/gtp6_d_di.c
src/plugins/srv6-mobile/gtp6_e.c
src/plugins/srv6-mobile/mobile.h
src/plugins/srv6-mobile/node.c
test/test_srv6_mobile.py

index 8e42af0..e8120bb 100644 (file)
@@ -12,6 +12,7 @@ RUN set -eux; \
     net-tools \
     iproute2 \
     tcpdump \
+    python3-cffi \
     asciidoc \
     xmlto \
     libssl-dev \
index aec520b..7507f50 100644 (file)
@@ -15,6 +15,7 @@ RUN set -eux; \
     net-tools \
     iproute2 \
     tcpdump \
+    python3-cffi \
     python2.7 \
     libssl-dev \
     netcat; \
index 7bafa56..bf5dcd2 100644 (file)
@@ -68,12 +68,13 @@ static u8 keyword_str[] = "t.m.gtp4.d";
 static u8 def_str[] =
   "Transit function with decapsulation for IPv4/GTP tunnel";
 static u8 param_str[] =
-  "<sr-prefix>/<sr-prefixlen> v6src_prefix <v6src_prefix>/<prefixlen> [nhtype <nhtype>]";
+  "<sr-prefix>/<sr-prefixlen> v6src_prefix <v6src_prefix>/<prefixlen> [nhtype "
+  "<nhtype>] fib-table <id>";
 
 static u8 *
 clb_format_srv6_t_m_gtp4_d (u8 * s, va_list * args)
 {
-  srv6_end_gtp4_param_t *ls_mem = va_arg (*args, void *);
+  srv6_end_gtp4_d_param_t *ls_mem = va_arg (*args, void *);
 
   s = format (s, "SRv6 T.M.GTP4.D\n\t");
 
@@ -88,16 +89,18 @@ clb_format_srv6_t_m_gtp4_d (u8 * s, va_list * args)
   if (ls_mem->nhtype != SRV6_NHTYPE_NONE)
     {
       if (ls_mem->nhtype == SRV6_NHTYPE_IPV4)
-       s = format (s, ", NHType IPv4\n");
+       s = format (s, ", NHType IPv4");
       else if (ls_mem->nhtype == SRV6_NHTYPE_IPV6)
-       s = format (s, ", NHType IPv6\n");
+       s = format (s, ", NHType IPv6");
       else if (ls_mem->nhtype == SRV6_NHTYPE_NON_IP)
-       s = format (s, ", NHType Non-IP\n");
+       s = format (s, ", NHType Non-IP");
       else
-       s = format (s, ", NHType Unknow(%d)\n", ls_mem->nhtype);
+       s = format (s, ", NHType Unknow(%d)", ls_mem->nhtype);
     }
-  else
-    s = format (s, "\n");
+
+  s = format (s, ", FIB table %d", ls_mem->fib_table);
+
+  s = format (s, ", Drop In %d\n", ls_mem->drop_in);
 
   return s;
 }
@@ -106,42 +109,65 @@ static uword
 clb_unformat_srv6_t_m_gtp4_d (unformat_input_t * input, va_list * args)
 {
   void **plugin_mem_p = va_arg (*args, void **);
-  srv6_end_gtp4_param_t *ls_mem;
+  srv6_end_gtp4_d_param_t *ls_mem;
   ip6_address_t sr_prefix;
   u32 sr_prefixlen;
   ip6_address_t v6src_prefix;
   u32 v6src_prefixlen;
-  u8 nhtype;
+  u32 fib_table = 0;
+  bool drop_in = false;
+  u8 nhtype = SRV6_NHTYPE_NONE;
+  bool config = false;
 
-  if (unformat (input, "t.m.gtp4.d %U/%d v6src_prefix %U/%d nhtype ipv4",
-               unformat_ip6_address, &sr_prefix, &sr_prefixlen,
-               unformat_ip6_address, &v6src_prefix, &v6src_prefixlen))
-    {
-      nhtype = SRV6_NHTYPE_IPV4;
-    }
-  else
-    if (unformat
-       (input, "t.m.gtp4.d %U/%d v6src_prefix %U/%d nhtype ipv6",
-        unformat_ip6_address, &sr_prefix, &sr_prefixlen,
-        unformat_ip6_address, &v6src_prefix, &v6src_prefixlen))
-    {
-      nhtype = SRV6_NHTYPE_IPV6;
-    }
-  else
-    if (unformat
-       (input, "t.m.gtp4.d %U/%d v6src_prefix %U/%d nhtype non-ip",
-        unformat_ip6_address, &sr_prefix, &sr_prefixlen,
-        unformat_ip6_address, &v6src_prefix, &v6src_prefixlen))
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
     {
-      nhtype = SRV6_NHTYPE_NON_IP;
-    }
-  else if (unformat (input, "t.m.gtp4.d %U/%d v6src_prefix %U/%d",
-                    unformat_ip6_address, &sr_prefix, &sr_prefixlen,
-                    unformat_ip6_address, &v6src_prefix, &v6src_prefixlen))
-    {
-      nhtype = SRV6_NHTYPE_NONE;
+      if (unformat (
+           input,
+           "t.m.gtp4.d %U/%d v6src_prefix %U/%d nhtype ipv4 fib-table %d",
+           unformat_ip6_address, &sr_prefix, &sr_prefixlen,
+           unformat_ip6_address, &v6src_prefix, &v6src_prefixlen, &fib_table))
+       {
+         config = true;
+         nhtype = SRV6_NHTYPE_IPV4;
+       }
+      else if (unformat (input,
+                        "t.m.gtp4.d %U/%d v6src_prefix %U/%d nhtype ipv6 "
+                        "fib-table %d",
+                        unformat_ip6_address, &sr_prefix, &sr_prefixlen,
+                        unformat_ip6_address, &v6src_prefix, &v6src_prefixlen,
+                        &fib_table))
+       {
+         config = true;
+         nhtype = SRV6_NHTYPE_IPV6;
+       }
+      else if (unformat (
+                input, "t.m.gtp4.d %U/%d v6src_prefix %U/%d nhtype non-ip",
+                unformat_ip6_address, &sr_prefix, &sr_prefixlen,
+                unformat_ip6_address, &v6src_prefix, &v6src_prefixlen))
+       {
+         config = true;
+         nhtype = SRV6_NHTYPE_NON_IP;
+       }
+      else if (unformat (input,
+                        "t.m.gtp4.d %U/%d v6src_prefix %U/%d fib-table %d",
+                        unformat_ip6_address, &sr_prefix, &sr_prefixlen,
+                        unformat_ip6_address, &v6src_prefix, &v6src_prefixlen,
+                        &fib_table))
+       {
+         config = true;
+         nhtype = SRV6_NHTYPE_NONE;
+       }
+      else if (unformat (input, "drop-in"))
+       {
+         drop_in = true;
+       }
+      else
+       {
+         return 0;
+       }
     }
-  else
+
+  if (!config)
     {
       return 0;
     }
@@ -158,6 +184,12 @@ clb_unformat_srv6_t_m_gtp4_d (unformat_input_t * input, va_list * args)
 
   ls_mem->nhtype = nhtype;
 
+  ls_mem->drop_in = drop_in;
+
+  ls_mem->fib_table = fib_table;
+  ls_mem->fib4_index = ip4_fib_index_from_table_id (fib_table);
+  ls_mem->fib6_index = ip6_fib_index_from_table_id (fib_table);
+
   return 1;
 }
 
@@ -170,9 +202,9 @@ clb_creation_srv6_t_m_gtp4_d (ip6_sr_policy_t * sr_policy)
 static int
 clb_removal_srv6_t_m_gtp4_d (ip6_sr_policy_t * sr_policy)
 {
-  srv6_end_gtp4_param_t *ls_mem;
+  srv6_end_gtp4_d_param_t *ls_mem;
 
-  ls_mem = (srv6_end_gtp4_param_t *) sr_policy->plugin_mem;
+  ls_mem = (srv6_end_gtp4_d_param_t *) sr_policy->plugin_mem;
 
   clib_mem_free (ls_mem);
 
index 211e95d..580619f 100644 (file)
@@ -66,11 +66,16 @@ static u8 param_str[] = "";
 static u8 *
 clb_format_srv6_end_m_gtp4_e (u8 * s, va_list * args)
 {
-  srv6_end_gtp4_param_t *ls_mem = va_arg (*args, void *);
+  srv6_end_gtp4_e_param_t *ls_mem = va_arg (*args, void *);
 
-  s = format (s, "SRv6 End gtp4.e\n\t");
+  s = format (s, "SRv6 End gtp4.e\n");
 
-  s = format (s, "IPv4 address position: %d\n", ls_mem->v4src_position);
+  s = format (s, "\tIPv4 address position: %d\n", ls_mem->v4src_position);
+
+  s = format (s, "\tIPv4 source address: %U\n", format_ip4_address,
+             &ls_mem->v4src_addr);
+
+  s = format (s, "\tFib Table %d\n", ls_mem->fib_table);
 
   return s;
 }
@@ -79,10 +84,33 @@ static uword
 clb_unformat_srv6_end_m_gtp4_e (unformat_input_t * input, va_list * args)
 {
   void **plugin_mem_p = va_arg (*args, void **);
-  srv6_end_gtp4_param_t *ls_mem;
-  u32 v4src_position;
-
-  if (!unformat (input, "end.m.gtp4.e v4src_position %d", &v4src_position))
+  srv6_end_gtp4_e_param_t *ls_mem;
+  ip4_address_t v4src_addr;
+  u32 v4src_position = 0;
+  u32 fib_table;
+  bool config = false;
+
+  memset (&v4src_addr, 0, sizeof (ip4_address_t));
+
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (input, "end.m.gtp4.e v4src_position %d fib-table %d",
+                   &v4src_position, &fib_table))
+       {
+         config = true;
+       }
+      else if (unformat (input, "end.m.gtp4.e v4src_addr %U fib-table %d",
+                        unformat_ip4_address, &v4src_addr, &fib_table))
+       {
+         config = true;
+       }
+      else
+       {
+         return 0;
+       }
+    }
+
+  if (!config)
     return 0;
 
   ls_mem = clib_mem_alloc_aligned_at_offset (sizeof *ls_mem, 0, 0, 1);
@@ -90,6 +118,11 @@ clb_unformat_srv6_end_m_gtp4_e (unformat_input_t * input, va_list * args)
   *plugin_mem_p = ls_mem;
 
   ls_mem->v4src_position = v4src_position;
+  memcpy (&ls_mem->v4src_addr, &v4src_addr, sizeof (ip4_address_t));
+
+  ls_mem->fib_table = fib_table;
+  ls_mem->fib4_index = ip4_fib_index_from_table_id (fib_table);
+  ls_mem->fib6_index = ip6_fib_index_from_table_id (fib_table);
 
   return 1;
 }
@@ -103,7 +136,7 @@ clb_creation_srv6_end_m_gtp4_e (ip6_sr_localsid_t * localsid)
 static int
 clb_removal_srv6_end_m_gtp4_e (ip6_sr_localsid_t * localsid)
 {
-  srv6_end_gtp4_param_t *ls_mem;
+  srv6_end_gtp4_e_param_t *ls_mem;
 
   ls_mem = localsid->plugin_mem;
 
index c62320b..2d464fb 100644 (file)
@@ -61,12 +61,13 @@ static u8 fn_name[] = "SRv6-End.M.GTP6.D-plugin";
 static u8 keyword_str[] = "end.m.gtp6.d";
 static u8 def_str[] =
   "Endpoint function with dencapsulation for IPv6/GTP tunnel";
-static u8 param_str[] = "<sr-prefix>/<sr-prefixlen> [nhtype <nhtype>]";
+static u8 param_str[] =
+  "<sr-prefix>/<sr-prefixlen> [nhtype <nhtype>] fib-table <id>";
 
 static u8 *
 clb_format_srv6_end_m_gtp6_d (u8 * s, va_list * args)
 {
-  srv6_end_gtp6_param_t *ls_mem = va_arg (*args, void *);
+  srv6_end_gtp6_d_param_t *ls_mem = va_arg (*args, void *);
 
   s = format (s, "SRv6 End gtp6.d\n\t");
 
@@ -77,16 +78,18 @@ clb_format_srv6_end_m_gtp6_d (u8 * s, va_list * args)
   if (ls_mem->nhtype != SRV6_NHTYPE_NONE)
     {
       if (ls_mem->nhtype == SRV6_NHTYPE_IPV4)
-       s = format (s, ", NHType IPv4\n");
+       s = format (s, ", NHType IPv4");
       else if (ls_mem->nhtype == SRV6_NHTYPE_IPV6)
-       s = format (s, ", NHType IPv6\n");
+       s = format (s, ", NHType IPv6");
       else if (ls_mem->nhtype == SRV6_NHTYPE_NON_IP)
-       s = format (s, ", NHType Non-IP\n");
+       s = format (s, ", NHType Non-IP");
       else
-       s = format (s, ", NHType Unknow(%d)\n", ls_mem->nhtype);
+       s = format (s, ", NHType Unknow(%d)", ls_mem->nhtype);
     }
-  else
-    s = format (s, "\n");
+
+  s = format (s, " FIB table %d", ls_mem->fib_table);
+
+  s = format (s, " Drop In %d", ls_mem->drop_in);
 
   return s;
 }
@@ -95,32 +98,54 @@ static uword
 clb_unformat_srv6_end_m_gtp6_d (unformat_input_t * input, va_list * args)
 {
   void **plugin_mem_p = va_arg (*args, void **);
-  srv6_end_gtp6_param_t *ls_mem;
+  srv6_end_gtp6_d_param_t *ls_mem;
   ip6_address_t sr_prefix;
   u32 sr_prefixlen;
-  u8 nhtype;
+  u8 nhtype = SRV6_NHTYPE_NONE;
+  bool drop_in = false;
+  bool config = false;
+  u32 fib_table = 0;
 
-  if (unformat (input, "end.m.gtp6.d %U/%d nh-type ipv4",
-               unformat_ip6_address, &sr_prefix, &sr_prefixlen))
-    {
-      nhtype = SRV6_NHTYPE_IPV4;
-    }
-  else if (unformat (input, "end.m.gtp6.d %U/%d nh-type ipv6",
-                    unformat_ip6_address, &sr_prefix, &sr_prefixlen))
-    {
-      nhtype = SRV6_NHTYPE_IPV6;
-    }
-  else if (unformat (input, "end.m.gtp6.d %U/%d nh-type none",
-                    unformat_ip6_address, &sr_prefix, &sr_prefixlen))
-    {
-      nhtype = SRV6_NHTYPE_NON_IP;
-    }
-  else if (unformat (input, "end.m.gtp6.d %U/%d",
-                    unformat_ip6_address, &sr_prefix, &sr_prefixlen))
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
     {
-      nhtype = SRV6_NHTYPE_NONE;
+      if (unformat (input, "end.m.gtp6.d %U/%d nh-type ipv4 fib-table %d",
+                   unformat_ip6_address, &sr_prefix, &sr_prefixlen,
+                   &fib_table))
+       {
+         config = true;
+         nhtype = SRV6_NHTYPE_IPV4;
+       }
+      else if (unformat (input, "end.m.gtp6.d %U/%d nh-type ipv6 fib-table %d",
+                        unformat_ip6_address, &sr_prefix, &sr_prefixlen,
+                        &fib_table))
+       {
+         config = true;
+         nhtype = SRV6_NHTYPE_IPV6;
+       }
+      else if (unformat (input, "end.m.gtp6.d %U/%d nh-type none",
+                        unformat_ip6_address, &sr_prefix, &sr_prefixlen))
+       {
+         config = true;
+         nhtype = SRV6_NHTYPE_NON_IP;
+       }
+      else if (unformat (input, "end.m.gtp6.d %U/%d fib-table %d",
+                        unformat_ip6_address, &sr_prefix, &sr_prefixlen,
+                        &fib_table))
+       {
+         config = true;
+         nhtype = SRV6_NHTYPE_NONE;
+       }
+      else if (unformat (input, "drop-in"))
+       {
+         drop_in = true;
+       }
+      else
+       {
+         return 0;
+       }
     }
-  else
+
+  if (!config)
     {
       return 0;
     }
@@ -134,6 +159,12 @@ clb_unformat_srv6_end_m_gtp6_d (unformat_input_t * input, va_list * args)
 
   ls_mem->nhtype = nhtype;
 
+  ls_mem->drop_in = drop_in;
+
+  ls_mem->fib_table = fib_table;
+  ls_mem->fib4_index = ip4_fib_index_from_table_id (fib_table);
+  ls_mem->fib6_index = ip6_fib_index_from_table_id (fib_table);
+
   return 1;
 }
 
@@ -143,10 +174,16 @@ clb_creation_srv6_end_m_gtp6_d (ip6_sr_localsid_t * localsid)
   return 0;
 }
 
+static int
+clb_creation_srv6_end_m_gtp6_d_2 (ip6_sr_policy_t *sr_policy)
+{
+  return 0;
+}
+
 static int
 clb_removal_srv6_end_m_gtp6_d (ip6_sr_localsid_t * localsid)
 {
-  srv6_end_gtp6_param_t *ls_mem;
+  srv6_end_gtp6_d_param_t *ls_mem;
 
   ls_mem = localsid->plugin_mem;
 
@@ -155,6 +192,18 @@ clb_removal_srv6_end_m_gtp6_d (ip6_sr_localsid_t * localsid)
   return 0;
 }
 
+static int
+clb_removal_srv6_end_m_gtp6_d_2 (ip6_sr_policy_t *sr_policy)
+{
+  srv6_end_gtp6_d_param_t *ls_mem;
+
+  ls_mem = sr_policy->plugin_mem;
+
+  clib_mem_free (ls_mem);
+
+  return 0;
+}
+
 static clib_error_t *
 srv6_end_m_gtp6_d_init (vlib_main_t * vm)
 {
@@ -193,6 +242,15 @@ srv6_end_m_gtp6_d_init (vlib_main_t * vm)
   if (rc < 0)
     clib_error_return (0, "SRv6 Endpoint GTP6.D LocalSID function"
                       "couldn't be registered");
+
+  rc = sr_policy_register_function (
+    vm, fn_name, keyword_str, def_str, param_str, 128, // prefix len
+    &dpo_type, clb_format_srv6_end_m_gtp6_d, clb_unformat_srv6_end_m_gtp6_d,
+    clb_creation_srv6_end_m_gtp6_d_2, clb_removal_srv6_end_m_gtp6_d_2);
+  if (rc < 0)
+    clib_error_return (0, "SRv6 GTP6.D Steering function"
+                         "couldn't be registered");
+
   return 0;
 }
 
index 1431856..acebcd6 100644 (file)
@@ -66,7 +66,7 @@ static u8 param_str[] = "<sr-prefix>/<sr-prefixlen> [nhtype <nhtype>]";
 static u8 *
 clb_format_srv6_end_m_gtp6_d_di (u8 * s, va_list * args)
 {
-  srv6_end_gtp6_param_t *ls_mem = va_arg (*args, void *);
+  srv6_end_gtp6_d_param_t *ls_mem = va_arg (*args, void *);
 
   s = format (s, "SRv6 End gtp6.d Drop-in\n\t");
 
@@ -95,7 +95,7 @@ static uword
 clb_unformat_srv6_end_m_gtp6_d_di (unformat_input_t * input, va_list * args)
 {
   void **plugin_mem_p = va_arg (*args, void **);
-  srv6_end_gtp6_param_t *ls_mem;
+  srv6_end_gtp6_d_param_t *ls_mem;
   ip6_address_t sr_prefix;
   u32 sr_prefixlen = 0;
   u8 nhtype;
@@ -145,7 +145,7 @@ clb_creation_srv6_end_m_gtp6_d_di (ip6_sr_localsid_t * localsid)
 static int
 clb_removal_srv6_end_m_gtp6_d_di (ip6_sr_localsid_t * localsid)
 {
-  srv6_end_gtp6_param_t *ls_mem;
+  srv6_end_gtp6_d_param_t *ls_mem;
 
   ls_mem = localsid->plugin_mem;
 
index d139a64..16ed023 100644 (file)
@@ -66,15 +66,34 @@ static u8 param_str[] = "";
 static u8 *
 clb_format_srv6_end_m_gtp6_e (u8 * s, va_list * args)
 {
-  s = format (s, "SRv6 End format function unsupported.");
+  srv6_end_gtp6_e_param_t *ls_mem = va_arg (*args, void *);
+  ;
+
+  s = format (s, "SRv6 End.M.GTP6.E function.");
+
+  s = format (s, "\tFib Table %d\n", ls_mem->fib_table);
+
   return s;
 }
 
 static uword
 clb_unformat_srv6_end_m_gtp6_e (unformat_input_t * input, va_list * args)
 {
-  if (!unformat (input, "end.m.gtp6.e"))
+  void **plugin_mem_p = va_arg (*args, void **);
+  srv6_end_gtp6_e_param_t *ls_mem;
+  u32 fib_table;
+
+  if (!unformat (input, "end.m.gtp6.e fib-table %d", &fib_table))
     return 0;
+
+  ls_mem = clib_mem_alloc_aligned_at_offset (sizeof *ls_mem, 0, 0, 1);
+  clib_memset (ls_mem, 0, sizeof *ls_mem);
+  *plugin_mem_p = ls_mem;
+
+  ls_mem->fib_table = fib_table;
+  ls_mem->fib4_index = ip4_fib_index_from_table_id (fib_table);
+  ls_mem->fib6_index = ip6_fib_index_from_table_id (fib_table);
+
   return 1;
 }
 
@@ -87,6 +106,12 @@ clb_creation_srv6_end_m_gtp6_e (ip6_sr_localsid_t * localsid)
 static int
 clb_removal_srv6_end_m_gtp6_e (ip6_sr_localsid_t * localsid)
 {
+  srv6_end_gtp6_e_param_t *ls_mem;
+
+  ls_mem = localsid->plugin_mem;
+
+  clib_mem_free (ls_mem);
+
   return 0;
 }
 
index 517e7c8..5f6064f 100644 (file)
@@ -20,6 +20,8 @@
 
 #include <vnet/vnet.h>
 #include <vnet/ip/ip.h>
+#include <vnet/fib/ip4_fib.h>
+#include <vnet/fib/ip6_fib.h>
 #include <vnet/srv6/sr.h>
 #include <vnet/srv6/sr_packet.h>
 
@@ -174,14 +176,24 @@ typedef struct
 
 #define USER_PLANE_SUB_TLV_IE  0x01
 
-typedef struct srv6_end_gtp6_param_s
+/* SRv6 mobile Plugin Params */
+
+/* GTP6.D, GTP6.Di */
+typedef struct srv6_end_gtp6_d_param_s
 {
   u8 nhtype;
 
   ip6_address_t sr_prefix;
   u32 sr_prefixlen;
-} srv6_end_gtp6_param_t;
 
+  bool drop_in;
+
+  u32 fib_table;
+  u32 fib4_index;
+  u32 fib6_index;
+} srv6_end_gtp6_d_param_t;
+
+/* GTP6.DT */
 typedef struct srv6_end_gtp6_dt_param_s
 {
   u8 type;
@@ -191,6 +203,15 @@ typedef struct srv6_end_gtp6_dt_param_s
   u32 local_fib_index;
 } srv6_end_gtp6_dt_param_t;
 
+/* GTP6.E */
+typedef struct srv6_end_gtp6_e_param_s
+{
+  u32 fib_table;
+  u32 fib4_index;
+  u32 fib6_index;
+} srv6_end_gtp6_e_param_t;
+
+/* GTP4.DT */
 typedef struct srv6_t_gtp4_dt_param_s
 {
   u8 type;
@@ -200,7 +221,19 @@ typedef struct srv6_t_gtp4_dt_param_s
   u32 local_fib_index;
 } srv6_t_gtp4_dt_param_t;
 
-typedef struct srv6_end_gtp4_param_s
+/* GTP4.E */
+typedef struct srv6_end_gtp4_e_param_s
+{
+  u32 v4src_position;
+  ip4_address_t v4src_addr;
+
+  u32 fib_table;
+  u32 fib4_index;
+  u32 fib6_index;
+} srv6_end_gtp4_e_param_t;
+
+/* GTP4.D */
+typedef struct srv6_end_gtp4_d_param_s
 {
   u8 nhtype;
 
@@ -210,8 +243,12 @@ typedef struct srv6_end_gtp4_param_s
   ip6_address_t v6src_prefix;
   u32 v6src_prefixlen;
 
-  u32 v4src_position;
-} srv6_end_gtp4_param_t;
+  bool drop_in;
+
+  u32 fib_table;
+  u32 fib4_index;
+  u32 fib6_index;
+} srv6_end_gtp4_d_param_t;
 
 typedef struct srv6_end_main_v4_s
 {
index 448d633..36445a6 100644 (file)
@@ -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))
                    {
-                     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];
+                       }
+                   }
+                 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);
+                       }
                    }
                }
              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;
+
+      offset = ls_param->sr_prefixlen / 8;
+      shift = ls_param->sr_prefixlen % 8;
 
-         ip4 = vlib_buffer_get_current (b0);
+      if (PREDICT_TRUE (shift == 0))
+       {
+         clib_memcpy_fast (&seg.as_u8[offset], dstp, 4);
 
-         if (ip4->protocol != IP_PROTOCOL_UDP
-             || len0 < sizeof (ip4_gtpu_header_t))
+         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->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));
+             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 (sl)
+       {
+         clib_memcpy_fast (ip6srv, sl->rewrite, vec_len (sl->rewrite));
 
-                 if (qfip)
-                   {
-                     qfi =
-                       ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
-                       ((qfi & ~GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
+         if (vec_len (sl->segments) > 1)
+           {
+             ip6srv->ip.src_address = src0;
 
-                     if (sess->type)
-                       {
-                         qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
-                       }
+             ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 
-                     seg0.as_u8[offset] |= qfi >> shift;
-                     seg0.as_u8[offset + 1] |= qfi << (8 - shift);
-                   }
-               }
+             ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
+             ip6srv->sr.segments_left += 2;
+             ip6srv->sr.last_entry += 2;
 
-             if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
-               {
-                 u16 payload_len;
+             ip6srv->sr.length += (sizeof (ip6_address_t) * 2) / 8;
+             ip6srv->sr.segments[0] = dst0;
+             ip6srv->sr.segments[1] = seg0;
 
-                 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;
+             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;
 
-                         ies = (u8 *) ((u8 *) hdr0 + hdrlen);
-                         clib_memcpy_fast (ie_buf, ies, ie_size);
-                         hdrlen += ie_size;
-                       }
-                   }
-               }
+             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;
 
-             // jump over variable length data
-             vlib_buffer_advance (b0, (word) hdrlen);
+             ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 
-             // get length of encapsulated IPv6 packet (the remaining part)
-             len0 = vlib_buffer_length_in_chain (vm, b0);
+             ip6srv->sr.segments[0] = dst0;
+             ip6srv->sr.segments[1] = seg0;
 
-             if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
+             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;
+
+  from = vlib_frame_vector_args (frame);
+  n_left_from = frame->n_vectors;
+  next_index = node->cached_next_index;
 
-                     ip6srv->sr.segments[0] = seg0;
+  while (n_left_from > 0)
+    {
+      u32 n_left_to_next;
 
-                     if (vec_len (sl->segments))
-                       {
-                         ip6srv->sr.segments[1] = sl->segments[0];
-                         ip6srv->sr.last_entry++;
-                         ip6srv->sr.length += sizeof (ip6_address_t) / 8;
-                       }
-                   }
+      vlib_get_next_frame (vm, node, next_index, to_next, 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));
+      while (n_left_from > 0 && n_left_to_next > 0)
+       {
+         u32 bi0;
+         vlib_buffer_t *b0;
 
-                 ip6srv->ip.src_address = src0;
-                 ip6srv->ip.dst_address = seg0;
+         u32 next0;
 
-                 if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
-                   {
-                     ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+         ip6_gtpu_header_t *hdr;
+         u32 hdrlen;
+         u8 gtpu_type;
+         bool gtp6;
+         bool ipv4;
 
-                     ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
+         // defaults
+         bi0 = from[0];
+         to_next[0] = bi0;
+         from += 1;
+         to_next += 1;
+         n_left_from -= 1;
+         n_left_to_next -= 1;
 
-                     ip6srv->sr.tag =
-                       clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+         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",}
 ,};
 
 /*
index a695c9d..e617ace 100644 (file)
@@ -69,9 +69,10 @@ class TestSRv6EndMGTP4E(VppTestCase):
         pkts = self.create_packets([("A::1", "B::1"), ("C::1", "D::1")])
 
         self.vapi.cli(
-            "sr localsid address {} behavior end.m.gtp4.e v4src_position 64"
-            .format(pkts[0]['IPv6'].dst))
-        self.logger.info(self.vapi.cli("show sr localsids"))
+            "sr localsid address {} behavior end.m.gtp4.e "
+            .format(pkts[0]['IPv6'].dst) +
+            "v4src_position 64 fib-table 0")
+        self.logger.info(self.vapi.cli("show sr localsid"))
 
         self.vapi.cli("clear errors")
 
@@ -150,8 +151,8 @@ class TestSRv6TMGTP4D(VppTestCase):
         self.vapi.cli("set sr encaps source addr A1::1")
         self.vapi.cli("sr policy add bsid D4:: next D2:: next D3::")
         self.vapi.cli(
-            "sr policy add bsid D5:: behavior t.m.gtp4.d"
-            "D4::/32 v6src_prefix C1::/64 nhtype ipv6")
+            "sr policy add bsid D5:: behavior t.m.gtp4.d D4::/32 " +
+            "v6src_prefix C1::/64 nhtype ipv6 fib-table 0 drop-in")
         self.vapi.cli("sr steer l3 {}/32 via bsid D5::".format(self.ip4_dst))
         self.vapi.cli("ip route add D2::/32 via {}".format(self.ip6_dst))
 
@@ -236,11 +237,11 @@ class TestSRv6EndMGTP6E(VppTestCase):
         pkts = self.create_packets([("A::1", "B::1"), ("C::1", "D::1")])
 
         self.vapi.cli(
-            "sr localsid prefix {}/64 behavior end.m.gtp6.e"
+            "sr localsid prefix {}/64 behavior end.m.gtp6.e fib-table 0"
             .format(pkts[0]['IPv6'].dst))
         self.vapi.cli(
             "ip route add a1::/64 via {}".format(self.ip6_nhop))
-        self.logger.info(self.vapi.cli("show sr localsids"))
+        self.logger.info(self.vapi.cli("show sr localsid"))
 
         self.vapi.cli("clear errors")
 
@@ -316,10 +317,12 @@ class TestSRv6EndMGTP6D(VppTestCase):
         self.vapi.cli("set sr encaps source addr A1::1")
         self.vapi.cli("sr policy add bsid D4:: next D2:: next D3::")
         self.vapi.cli(
-            "sr localsid prefix 2001::/64 behavior end.m.gtp6.d D4::/64")
+            "sr localsid prefix 2001::/64 behavior end.m.gtp6.d " +
+            "D4::/64 fib-table 0 drop-in")
         self.vapi.cli("ip route add D2::/64 via {}".format(self.ip6_nhop))
 
         self.logger.info(self.vapi.cli("show sr policies"))
+        self.logger.info(self.vapi.cli("show sr localsid"))
 
         self.vapi.cli("clear errors")
 
@@ -334,7 +337,11 @@ class TestSRv6EndMGTP6D(VppTestCase):
 
         for pkt in capture:
             self.logger.info(pkt.show2(dump=True))
-            self.logger.info("GTP6.D Address={}".format(
+            self.logger.info("GTP6.D SID0={}".format(
                 str(pkt[IPv6ExtHdrSegmentRouting].addresses[0])))
+            self.logger.info("GTP6.D SID1={}".format(
+                str(pkt[IPv6ExtHdrSegmentRouting].addresses[1])))
+            self.assertEqual(
+                str(pkt[IPv6ExtHdrSegmentRouting].addresses[0]), "2001::1")
             self.assertEqual(
-                str(pkt[IPv6ExtHdrSegmentRouting].addresses[0]), "d4::c800:0")
+                str(pkt[IPv6ExtHdrSegmentRouting].addresses[1]), "d4::c800:0")