srv6-mobile

Type: fix

Plug-in for IPv6 Segment Routing Mobile

Fix the packet forwarding issue on GTP4/6.D when configuring only 1 segment in SRH.

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

3 files changed, 459 insertions, 114 deletions

diff --git a/src/plugins/srv6-mobile/extra/runner.py b/src/plugins/srv6-mobile/extra/runner.py
index 9fffe419029..79ec2d007c5 100755
--- a/src/plugins/srv6-mobile/extra/runner.py
+++ b/src/plugins/srv6-mobile/extra/runner.py
@@ -865,6 +865,77 @@ class Program(object):
         print("Sending packet on {}:".format(c1.name))
         p.show2()
 
+        time.sleep(10) 
+        c1.enable_trace(10)
+        c4.enable_trace(10)
+
+        c4.pg_start_capture()
+
+        c1.pg_create_stream(p)
+        c1.pg_enable()
+
+        # timeout (sleep) if needed
+        print("Sleeping")
+        time.sleep(5)
+
+        print("Receiving packet on {}:".format(c4.name))
+        for p in c4.pg_read_packets():
+            p.show2()
+
+    def test_gtp4_usid(self):
+        # TESTS:
+        # trace add af-packet-input 10
+        # pg interface on c1 172.20.0.1
+        # pg interface on c4 B::1/120
+
+        self.start_containers()
+
+        c1 = self.containers.get(self.get_name(self.instance_names[0]))
+        c2 = self.containers.get(self.get_name(self.instance_names[1]))
+        c3 = self.containers.get(self.get_name(self.instance_names[2]))
+        c4 = self.containers.get(self.get_name(self.instance_names[-1]))
+
+        c1.pg_create_interface4(
+            local_ip="172.16.0.1/30",
+            remote_ip="172.16.0.2/30",
+            local_mac="aa:bb:cc:dd:ee:01",
+            remote_mac="aa:bb:cc:dd:ee:02")
+        c4.pg_create_interface4(
+            local_ip="1.0.0.2/30",
+            remote_ip="1.0.0.1",
+            local_mac="aa:bb:cc:dd:ee:11",
+            remote_mac="aa:bb:cc:dd:ee:22")
+
+        c1.vppctl_exec("set sr encaps source addr A1::1")
+        c1.vppctl_exec("sr policy add bsid D4:: next D2:1111:aaaa:bbbb::")
+        c1.vppctl_exec("sr policy add bsid D5:: behavior t.m.gtp4.d D4::/32 v6src_prefix C1::/64 nhtype ipv4")
+        c1.vppctl_exec("sr steer l3 172.20.0.1/32 via bsid D5::")
+
+        c2.vppctl_exec("sr localsid prefix D2:1111:aaaa::/48 behavior end usid 16")
+
+        c3.vppctl_exec("sr localsid prefix D2:1111:bbbb::/48 behavior end usid 16")
+
+        c4.vppctl_exec(
+            "sr localsid prefix D4::/32 "
+            "behavior end.m.gtp4.e v4src_position 64")
+
+        c2.set_ipv6_route("eth2", "A2::2", "D2:1111:bbbb::/48")
+        c2.set_ipv6_route("eth1", "A1::1", "C::/120")
+        c3.set_ipv6_route("eth2", "A3::2", "D4::/32")
+        c3.set_ipv6_route("eth1", "A2::1", "C::/120")
+        c4.set_ip_pgroute("pg0", "1.0.0.1", "172.20.0.1/32")
+
+        p = (Ether(src="aa:bb:cc:dd:ee:02", dst="aa:bb:cc:dd:ee:01") /
+             IP(src="172.20.0.2", dst="172.20.0.1") /
+             UDP(sport=2152, dport=2152) /
+             GTP_U_Header(gtp_type="g_pdu", teid=200) /
+             IP(src="172.99.0.1", dst="172.99.0.2") /
+             ICMP())
+
+        print("Sending packet on {}:".format(c1.name))
+        p.show2()
+
+        time.sleep(10) 
         c1.enable_trace(10)
         c4.enable_trace(10)
 
@@ -997,7 +1068,7 @@ class Program(object):
         p = (Ether(src="aa:bb:cc:dd:ee:02", dst="aa:bb:cc:dd:ee:01") /
              IP(src="172.20.0.2", dst="172.20.0.1") /
              UDP(sport=2152, dport=2152) /
-             GTP_U_Header(gtp_type="echo_request", teid=200))
+             GTP_U_Header(gtp_type="echo_request", S=1, teid=200, seq=200))
 
         print("Sending packet on {}:".format(c1.name))
         p.show2()
@@ -1360,7 +1431,7 @@ class Program(object):
         p = (Ether(src="aa:bb:cc:dd:ee:02", dst="aa:bb:cc:dd:ee:01") /
              IPv6(src="C::2", dst="D::2") /
              UDP(sport=2152, dport=2152) /
-             GTP_U_Header(gtp_type="echo_request", teid=200))
+             GTP_U_Header(gtp_type="echo_request", S=1, teid=200, seq=300))
 
         print("Sending packet on {}:".format(c1.name))
         p.show2()
@@ -1853,7 +1924,6 @@ class Program(object):
                 "running" if self.networks.get(name) else "missing"))
 
     def build_image(self):
-        # TODO: optimize build process for speed and image size
         print("VPP Path (build): {}".format(self.vpp_path))
         self.containers.build(self.path, self.vpp_path)
 
@@ -1935,6 +2005,7 @@ def get_args():
             # "tmap_ipv6",
             # "tmap_ipv6_5g",
             "gtp4",
+            "gtp4_usid",
             "gtp4_5g",
             "gtp4_echo",
             "gtp4_ipv6",
@@ -1967,8 +2038,10 @@ def main(op=None, prefix=None, verbose=None,
         image = "srv6m-release-image"
     elif image == 'debug':
         image = "srv6m-image"
+    else
+        image = "srv6m-image"
 
-    print("Verified image: {}".format(image))
+    print("Target image: {}".format(image))
 
     program = Program(image, prefix)
 
@@ -2001,6 +2074,8 @@ def main(op=None, prefix=None, verbose=None,
         #    program.test_tmap_ipv6_5g()
         elif op == 'gtp4':
             program.test_gtp4()
+        elif op == 'gtp4_usid':
+            program.test_gtp4_usid()
         elif op == 'gtp4_5g':
             program.test_gtp4_5g()
         elif op == 'gtp4_echo':
diff --git a/src/plugins/srv6-mobile/mobile.h b/src/plugins/srv6-mobile/mobile.h
index f1de99a306e..5a086b66c6e 100644
--- a/src/plugins/srv6-mobile/mobile.h
+++ b/src/plugins/srv6-mobile/mobile.h
@@ -51,6 +51,7 @@
 #endif
 
 #define GTPU_EXTHDR_FLAG                0x04
+#define GTPU_SEQ_FLAG			0x02
 #define GTPU_EXTHDR_PDU_SESSION         0x85
 
 #define SRH_TAG_ECHO_REPLY              0x0008
diff --git a/src/plugins/srv6-mobile/node.c b/src/plugins/srv6-mobile/node.c
index eb9a20e3c53..de3bc4a6f0d 100644
--- a/src/plugins/srv6-mobile/node.c
+++ b/src/plugins/srv6-mobile/node.c
@@ -129,10 +129,6 @@ format_srv6_end_rewrite_trace6 (u8 * s, va_list * args)
   _(M_GTP6_D_DI_PACKETS, "srv6 End.M.GTP6.D.DI packets") \
   _(M_GTP6_D_DI_BAD_PACKETS, "srv6 End.M.GTP6.D.DI bad packets")
 
-#define foreach_srv6_end_v6_dt_error \
-  _(M_GTP6_DT_PACKETS, "srv6 End.M.GTP6.DT packets") \
-  _(M_GTP6_DT_BAD_PACKETS, "srv6 End.M.GTP6.DT bad packets")
-
 typedef enum
 {
 #define _(sym,str) SRV6_END_ERROR_##sym,
@@ -173,14 +169,6 @@ typedef enum
     SRV6_END_N_V6_D_DI_ERROR,
 } srv6_end_error_v6_d_di_t;
 
-typedef enum
-{
-#define _(sym,str) SRV6_END_ERROR_##sym,
-  foreach_srv6_end_v6_dt_error
-#undef _
-    SRV6_END_N_V6_DT_ERROR,
-} srv6_end_error_v6_dt_t;
-
 static char *srv6_end_error_v4_strings[] = {
 #define _(sym,string) string,
   foreach_srv6_end_v4_error
@@ -258,20 +246,22 @@ hash_uword_to_u16 (uword * key)
 #endif
 }
 
-static inline void
-gtp_type_set (gtpu_header_t * gtpu, u16 tag)
+static inline u8
+gtpu_type_get (u16 tag)
 {
   u16 val;
 
   val = clib_net_to_host_u16 (tag);
   if (val & SRH_TAG_ECHO_REPLY)
-    gtpu->type = GTPU_TYPE_ECHO_REPLY;
+    return GTPU_TYPE_ECHO_REPLY;
   else if (val & SRH_TAG_ECHO_REQUEST)
-    gtpu->type = GTPU_TYPE_ECHO_REQUEST;
+    return GTPU_TYPE_ECHO_REQUEST;
   else if (val & SRH_TAG_ERROR_INDICATION)
-    gtpu->type = GTPU_TYPE_ERROR_INDICATION;
+    return GTPU_TYPE_ERROR_INDICATION;
   else if (val & SRH_TAG_END_MARKER)
-    gtpu->type = GTPU_TYPE_END_MARKER;
+    return GTPU_TYPE_END_MARKER;
+
+  return GTPU_TYPE_GTPU;
 }
 
 // Function for SRv6 GTP4.E function.
@@ -343,10 +333,12 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 	    }
 	  else
 	    {
+	      u8 gtpu_type = 0;
 	      u16 tag = 0;
-	      u32 teid;
+	      u32 teid = 0;
 	      u8 *teid8p = (u8 *) & teid;
 	      u8 qfi = 0;
+	      u16 seq = 0;
 	      u32 index;
 	      u32 offset, shift;
 	      u32 hdrlen = 0;
@@ -383,16 +375,29 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 	      offset = ls0->localsid_len / 8;
 	      shift = ls0->localsid_len % 8;
 
+	      gtpu_type = gtpu_type_get (tag);
+
 	      if (PREDICT_TRUE (shift == 0))
 		{
 		  clib_memcpy_fast (&dst4.as_u8[0], &dst0.as_u8[offset], 4);
 
 		  qfi = dst0.as_u8[offset + 4];
 
-		  clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 5], 4);
+		  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);
+		    }
+		  else
+		    {
+		      clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 5], 4);
+		    }
 		}
 	      else
 		{
+		  u8 *sp;
+
 		  for (index = 0; index < 4; index++)
 		    {
 		      dst4.as_u8[index] = dst0.as_u8[offset + index] << shift;
@@ -403,12 +408,27 @@ 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);
 
-		  for (index = 0; index < 4; index++)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
+		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
+		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		    {
+		      sp = (u8 *) & seq;
+		      for (index = 0; index < 2; index++)
+			{
+			  sp[index] = dst0.as_u8[offset + 5 + index] << shift;
+			  sp[index] =
+			    dst0.as_u8[offset + 6 + index] >> (8 - shift);
+			}
+		    }
+		  else
 		    {
-		      *teid8p = dst0.as_u8[offset + 5 + index] << shift;
-		      *teid8p |=
-			dst0.as_u8[offset + 6 + index] >> (8 - shift);
-		      teid8p++;
+		      for (index = 0; index < 4; index++)
+			{
+			  *teid8p = dst0.as_u8[offset + 5 + index] << shift;
+			  *teid8p |=
+			    dst0.as_u8[offset + 6 + index] >> (8 - shift);
+			  teid8p++;
+			}
 		    }
 		}
 
@@ -416,8 +436,15 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 		{
 		  hdrlen =
 		    sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
-		  len0 += hdrlen;
 		}
+	      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);
+		}
+	      len0 += hdrlen;
+
 	      hdrlen += sizeof (ip4_gtpu_header_t);
 
 	      // IPv4 GTP-U header creation.
@@ -433,10 +460,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 	      hdr0->gtpu.teid = teid;
 	      hdr0->gtpu.length = clib_host_to_net_u16 (len0);
 
-	      if (PREDICT_FALSE (tag != 0))
-		{
-		  gtp_type_set (&hdr0->gtpu, tag);
-		}
+	      hdr0->gtpu.type = gtpu_type;
 
 	      if (qfi)
 		{
@@ -444,7 +468,9 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 		  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;
 
@@ -465,6 +491,16 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 		  sess->nextexthdr = 0;
 		}
 
+	      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;
+		  hdr0->gtpu.ext->npdu_num = 0;
+		  hdr0->gtpu.ext->nextexthdr = 0;
+		}
+
 	      offset = ls_param->v4src_position / 8;
 	      shift = ls_param->v4src_position % 8;
 
@@ -618,6 +654,8 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
 	      u8 *teidp;
 	      u8 qfi = 0;
 	      u8 *qfip = NULL;
+	      u16 seq = 0;
+	      u8 *seqp = NULL;
 	      u32 offset, shift, index;
 	      ip6srv_combo_header_t *ip6srv;
 	      gtpu_pdu_session_t *sess = NULL;
@@ -632,10 +670,14 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
 
 	      gtpu_type = hdr->gtpu.type;
 
-	      if (hdr->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
+	      if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
 		{
 		  // Extention header.
 		  hdr_len += sizeof (gtpu_exthdr_t);
+
+		  seq = hdr->gtpu.ext->seq;
+		  seqp = (u8 *) & seq;
+
 		  if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 		    {
 		      // PDU Session Container.
@@ -682,7 +724,16 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
 		      seg.as_u8[offset + 4] = qfi;
 		    }
 
-		  clib_memcpy_fast (&seg.as_u8[offset + 5], teidp, 4);
+		  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
 		{
@@ -691,10 +742,6 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
 		      seg.as_u8[offset + index] |= dstp[index] >> shift;
 		      seg.as_u8[offset + index + 1] |=
 			dstp[index] << (8 - shift);
-
-		      seg.as_u8[offset + index + 5] |= teidp[index] >> shift;
-		      seg.as_u8[offset + index + 6] |=
-			teidp[index] << (8 - shift);
 		    }
 
 		  if (qfip)
@@ -711,6 +758,29 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
 		      seg.as_u8[offset + 4] |= qfi >> shift;
 		      seg.as_u8[offset + 5] |= qfi << (8 - shift);
 		    }
+
+		  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);
+			}
+		    }
 		}
 
 	      src6 = ls_param->v6src_prefix;
@@ -785,16 +855,49 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
 		  clib_memcpy_fast (ip6srv, sl->rewrite,
 				    vec_len (sl->rewrite));
 
-		  ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+		  if (vec_len (sl->segments) > 1)
+		    {
+		      ip6srv->sr.tag =
+			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+
+		      ip6srv->sr.segments_left += 1;
+		      ip6srv->sr.last_entry += 1;
 
-		  ip6srv->sr.tag =
-		    clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+		      ip6srv->sr.length += sizeof (ip6_address_t) / 8;
+		      ip6srv->sr.segments[0] = seg;
 
-		  ip6srv->sr.segments_left += 1;
-		  ip6srv->sr.last_entry += 1;
+		      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->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;
 
-		  ip6srv->sr.length += sizeof (ip6_address_t) / 8;
-		  ip6srv->sr.segments[0] = seg;
+		      ip6srv->sr.tag =
+			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+
+		      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++;
+			}
+		    }
 
 		  if (PREDICT_TRUE (encap != NULL))
 		    {
@@ -842,13 +945,6 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
 		    {
 		      ip6srv->sr.protocol = IP_PROTOCOL_NONE;
 		    }
-
-		  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
 		{
@@ -1063,9 +1159,11 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 
 	      len0 = vlib_buffer_length_in_chain (vm, b0);
 
-	      u32 teid;
+	      u32 teid = 0;
 	      u8 *teid8p = (u8 *) & teid;
 	      u8 qfi = 0;
+	      u16 seq = 0;
+	      u8 gtpu_type = 0;
 	      u16 index;
 	      u16 offset, shift;
 	      u32 hdrlen = 0;
@@ -1075,19 +1173,48 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 	      offset = index / 8;
 	      shift = index % 8;
 
+	      gtpu_type = gtpu_type_get (tag);
+
 	      if (PREDICT_TRUE (shift == 0))
 		{
-		  clib_memcpy_fast (teid8p, &dst0.as_u8[offset], 4);
+		  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], 2);
+		    }
+		  else
+		    {
+		      clib_memcpy_fast (teid8p, &dst0.as_u8[offset], 4);
+		    }
 
 		  qfi = dst0.as_u8[offset + 4];
 		}
 	      else
 		{
-		  for (index = offset; index < offset + 4; index++)
+		  u8 *sp;
+
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
+		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
+		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		    {
-		      *teid8p = dst0.as_u8[index] << shift;
-		      *teid8p |= dst0.as_u8[index + 1] >> (8 - shift);
-		      teid8p++;
+		      sp = (u8 *) & seq;
+		      for (index = 0; index < 2; index++)
+			{
+			  sp[index] = dst0.as_u8[offset + index] << shift;
+			  sp[index] =
+			    dst0.as_u8[offset + index + 1] >> (8 - shift);
+			}
+		    }
+		  else
+		    {
+		      for (index = 0; index < 4; index++)
+			{
+			  *teid8p = dst0.as_u8[offset + index] << shift;
+			  *teid8p |=
+			    dst0.as_u8[offset + index + 1] >> (8 - shift);
+			  teid8p++;
+			}
 		    }
 
 		  qfi |= dst0.as_u8[offset + 4] << shift;
@@ -1098,8 +1225,15 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 		{
 		  hdrlen =
 		    sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
-		  len0 += hdrlen;
 		}
+	      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);
+		}
+
+	      len0 += hdrlen;
 	      hdrlen += sizeof (ip6_gtpu_header_t);
 
 	      vlib_buffer_advance (b0, -(word) hdrlen);
@@ -1112,10 +1246,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 	      hdr0->gtpu.teid = teid;
 	      hdr0->gtpu.length = clib_host_to_net_u16 (len0);
 
-	      if (PREDICT_FALSE (tag != 0))
-		{
-		  gtp_type_set (&hdr0->gtpu, tag);
-		}
+	      hdr0->gtpu.type = gtpu_type;
 
 	      if (qfi)
 		{
@@ -1123,6 +1254,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 		  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;
@@ -1144,6 +1276,16 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 		  sess->nextexthdr = 0;
 		}
 
+	      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;
+		  hdr0->gtpu.ext->npdu_num = 0;
+		  hdr0->gtpu.ext->nextexthdr = 0;
+		}
+
 	      hdr0->udp.length = clib_host_to_net_u16 (len0 +
 						       sizeof (udp_header_t) +
 						       sizeof
@@ -1241,6 +1383,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
 	  u8 gtpu_type = 0;
 	  u8 qfi;
 	  u8 *qfip = NULL;
+	  u16 seq = 0;
+	  u8 *seqp = NULL;
 	  u32 offset, shift;
 	  u32 hdrlen;
 	  ip6_header_t *encap = NULL;
@@ -1288,10 +1432,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
 	      teid = hdr0->gtpu.teid;
 	      teidp = (u8 *) & teid;
 
-	      if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
+	      if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
 		{
 		  // Extention header.
 		  hdrlen += sizeof (gtpu_exthdr_t);
+
+		  seq = hdr0->gtpu.ext->seq;
+		  seqp = (u8 *) & seq;
+
 		  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 		    {
 		      // PDU Session Container.
@@ -1317,7 +1465,18 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
 	      offset += 1;
 	      if (PREDICT_TRUE (shift == 0))
 		{
-		  clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
+		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
+		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		    {
+		      if (seqp)
+			clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
+		    }
+		  else
+		    {
+		      clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
+		    }
+
 		  if (qfip)
 		    {
 		      qfi =
@@ -1336,11 +1495,28 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
 		{
 		  int idx;
 
-		  for (idx = 0; idx < 4; idx++)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
+		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
+		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		    {
+		      if (seqp)
+			{
+			  for (idx = 0; idx < 2; idx++)
+			    {
+			      seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
+			      seg0.as_u8[offset + idx + 1] |=
+				seqp[idx] << (8 - shift);
+			    }
+			}
+		    }
+		  else
 		    {
-		      seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
-		      seg0.as_u8[offset + idx + 1] |=
-			teidp[idx] << (8 - shift);
+		      for (idx = 0; idx < 4; idx++)
+			{
+			  seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
+			  seg0.as_u8[offset + idx + 1] |=
+			    teidp[idx] << (8 - shift);
+			}
 		    }
 
 		  if (qfip)
@@ -1421,17 +1597,51 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
 		  clib_memcpy_fast (ip6srv, sl->rewrite,
 				    vec_len (sl->rewrite));
 
-		  ip6srv->ip.src_address = src0;
-		  ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+		  if (vec_len (sl->segments) > 1)
+		    {
+		      ip6srv->ip.src_address = src0;
 
-		  ip6srv->sr.tag =
-		    clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+		      ip6srv->sr.tag =
+			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+
+		      ip6srv->sr.segments_left += 1;
+		      ip6srv->sr.last_entry += 1;
 
-		  ip6srv->sr.segments_left += 1;
-		  ip6srv->sr.last_entry += 1;
+		      ip6srv->sr.length += sizeof (ip6_address_t) / 8;
+		      ip6srv->sr.segments[0] = seg0;
 
-		  ip6srv->sr.length += sizeof (ip6_address_t) / 8;
-		  ip6srv->sr.segments[0] = seg0;
+		      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;
+
+		      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;
+
+		      ip6srv->sr.tag =
+			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+
+		      ip6srv->sr.segments[0] = seg0;
+
+		      if (vec_len (sl->segments))
+			{
+			  ip6srv->sr.segments[1] = sl->segments[0];
+			  ip6srv->sr.last_entry++;
+			  ip6srv->sr.length += sizeof (ip6_address_t) / 8;
+			}
+		    }
 
 		  if (PREDICT_TRUE (encap != NULL))
 		    {
@@ -1479,13 +1689,6 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
 		    {
 		      ip6srv->sr.protocol = IP_PROTOCOL_NONE;
 		    }
-
-		  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
 		{
@@ -1636,6 +1839,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 	  u8 gtpu_type = 0;
 	  u8 qfi = 0;
 	  u8 *qfip = NULL;
+	  u16 seq = 0;
+	  u8 *seqp = NULL;
 	  u32 offset, shift;
 	  u32 hdrlen;
 	  ip6_header_t *encap = NULL;
@@ -1684,10 +1889,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 	      teid = hdr0->gtpu.teid;
 	      teidp = (u8 *) & teid;
 
-	      if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
+	      if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
 		{
 		  // Extention header.
 		  hdrlen += sizeof (gtpu_exthdr_t);
+
+		  seq = hdr0->gtpu.ext->seq;
+		  seqp = (u8 *) & seq;
+
 		  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 		    {
 		      // PDU Session Container.
@@ -1711,7 +1920,17 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 	      offset += 1;
 	      if (PREDICT_TRUE (shift == 0))
 		{
-		  clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
+		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
+		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
+		    {
+		      if (seqp)
+			clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
+		    }
+		  else
+		    {
+		      clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
+		    }
 
 		  if (qfip)
 		    {
@@ -1731,11 +1950,28 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		{
 		  int idx;
 
-		  for (idx = 0; idx < 4; idx++)
+		  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
+		      || gtpu_type == GTPU_TYPE_ECHO_REPLY
+		      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 		    {
-		      seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
-		      seg0.as_u8[offset + idx + 1] |=
-			teidp[idx] << (8 - shift);
+		      if (seqp)
+			{
+			  for (idx = 0; idx < 2; idx++)
+			    {
+			      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] |= teidp[idx] >> shift;
+			  seg0.as_u8[offset + idx + 1] |=
+			    teidp[idx] << (8 - shift);
+			}
 		    }
 
 		  if (qfip)
@@ -1807,13 +2043,54 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		  clib_memcpy_fast (ip6srv, sl->rewrite,
 				    vec_len (sl->rewrite));
 
-		  ip6srv->ip.src_address = src0;
+		  if (vec_len (sl->segments) > 1)
+		    {
+		      ip6srv->ip.src_address = src0;
 
-		  ip6srv->sr.tag =
-		    clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+		      ip6srv->sr.tag =
+			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+
+		      ip6srv->sr.segments_left += 2;
+		      ip6srv->sr.last_entry += 2;
 
-		  ip6srv->sr.segments_left += 2;
-		  ip6srv->sr.last_entry += 2;
+		      ip6srv->sr.length += ((sizeof (ip6_address_t) * 2) / 8);
+
+		      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));
+		    }
+		  else
+		    {
+		      ip6srv->ip.src_address = src0;
+		      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
+
+		      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.flags = 0;
+
+		      ip6srv->sr.tag =
+			clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
+
+		      ip6srv->sr.segments[0] = dst0;
+		      ip6srv->sr.segments[1] = seg0;
+
+		      if (vec_len (sl->segments))
+			{
+			  ip6srv->sr.segments[2] = sl->segments[0];
+			  ip6srv->sr.last_entry++;
+			  ip6srv->sr.length += sizeof (ip6_address_t) / 8;
+			}
+		    }
 		}
 	      else
 		{
@@ -1823,12 +2100,18 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		  ip6srv->ip.src_address = src0;
 		  ip6srv->ip.dst_address = seg0;
 
+		  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;
+
 		  ip6srv->sr.tag =
 		    clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 
-		  ip6srv->sr.segments_left += 1;
-		  ip6srv->sr.last_entry += 1;
-		  ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
+		  ip6srv->sr.segments[0] = dst0;
 		}
 
 	      ip6srv->ip.payload_length =
@@ -1882,20 +2165,6 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
 		  ip6srv->sr.protocol = IP_PROTOCOL_NONE;
 		}
 
-	      ip6srv->sr.length += ((sizeof (ip6_address_t) * 2) / 8);
-	      ip6srv->sr.segments[0] = dst0;
-	      ip6srv->sr.segments[1] = seg0;
-
-	      if (sl)
-		{
-		  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));
-		}
-
 	      good_n++;
 
 	      if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&