/*
 * 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:
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <vlib/vlib.h>
#include <vnet/vnet.h>
#include <vppinfra/error.h>
#include <vppinfra/hash.h>
#include <srv6-mobile/mobile.h>

extern ip6_address_t sr_pr_encaps_src;

typedef struct
{
  ip6_address_t src, dst;
  ip6_address_t sr_prefix;
  u16 sr_prefixlen;
  u32 teid;
} srv6_end_rewrite_trace_t;

static u16 srh_tagfield[256] = {
  /* 0 */
  0x0,
  /* 1 : Echo Request */
  0x0004,
  /* 2 : Echo Reply */
  0x0008,
  /* 3 - 7 */
  0x0, 0x0, 0x0, 0x0, 0x0,
  /* 8 - 15 */
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  /* 16 - 23 */
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  /* 24 - 25 */
  0x0, 0x0,
  /* 26 : Error Indication */
  0x0002,
  /* 27 - 31 */
  0x0, 0x0, 0x0, 0x0, 0x0,
  /* 32 - 247 */
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  /* 248 - 253 */
  0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  /* 254 : End Maker */
  0x0001,
  /* 255 : G_PDU */
  0x0
};

static u8 *
format_srv6_end_rewrite_trace (u8 * s, va_list * args)
{
  CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
  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",
		 format_ip4_address, &t->src, format_ip4_address, &t->dst,
		 clib_net_to_host_u32 (t->teid));
}

static u8 *
format_srv6_end_rewrite_trace6 (u8 * s, va_list * args)
{
  CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
  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);
}

#define foreach_srv6_end_v4_error \
  _(M_GTP4_E_PACKETS, "srv6 End.M.GTP4.E packets") \
  _(M_GTP4_E_BAD_PACKETS, "srv6 End.M.GTP4.E bad packets")

#define foreach_srv6_t_v4_d_error \
  _(M_GTP4_D_PACKETS, "srv6 T.M.GTP4.D packets") \
  _(M_GTP4_D_BAD_PACKETS, "srv6 T.M.GTP4.D bad packets")

#define foreach_srv6_end_v6_e_error \
  _(M_GTP6_E_PACKETS, "srv6 End.M.GTP6.E packets") \
  _(M_GTP6_E_BAD_PACKETS, "srv6 End.M.GTP6.E bad packets")

#define foreach_srv6_end_v6_d_error \
  _(M_GTP6_D_PACKETS, "srv6 End.M.GTP6.D packets") \
  _(M_GTP6_D_BAD_PACKETS, "srv6 End.M.GTP6.D bad packets")

#define foreach_srv6_end_v6_d_di_error \
  _(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")

#define foreach_srv6_t_v4_dt_error \
  _(M_GTP4_DT_PACKETS, "srv6 T.M.GTP4.DT packets") \
  _(M_GTP4_DT_BAD_PACKETS, "srv6 T.M.GTP4.DT bad packets")

typedef enum
{
#define _(sym,str) SRV6_END_ERROR_##sym,
  foreach_srv6_end_v4_error
#undef _
    SRV6_END_N_V4_ERROR,
} srv6_end_error_v4_t;

typedef enum
{
#define _(sym,str) SRV6_T_ERROR_##sym,
  foreach_srv6_t_v4_d_error
#undef _
    SRV6_T_N_V4_D_ERROR,
} srv6_t_error_v4_d_t;

typedef enum
{
#define _(sym,str) SRV6_END_ERROR_##sym,
  foreach_srv6_end_v6_e_error
#undef _
    SRV6_END_N_V6_E_ERROR,
} srv6_end_error_v6_e_t;

typedef enum
{
#define _(sym,str) SRV6_END_ERROR_##sym,
  foreach_srv6_end_v6_d_error
#undef _
    SRV6_END_N_V6_D_ERROR,
} srv6_end_error_v6_d_t;

typedef enum
{
#define _(sym,str) SRV6_END_ERROR_##sym,
  foreach_srv6_end_v6_d_di_error
#undef _
    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;

typedef enum
{
#define _(sym,str) SRV6_T_ERROR_##sym,
  foreach_srv6_t_v4_dt_error
#undef _
    SRV6_T_N_V4_DT_ERROR,
} srv6_t_error_v4_dt_t;

static char *srv6_end_error_v4_strings[] = {
#define _(sym,string) string,
  foreach_srv6_end_v4_error
#undef _
};

static char *srv6_t_error_v4_d_strings[] = {
#define _(sym,string) string,
  foreach_srv6_t_v4_d_error
#undef _
};

static char *srv6_end_error_v6_e_strings[] = {
#define _(sym,string) string,
  foreach_srv6_end_v6_e_error
#undef _
};

static char *srv6_end_error_v6_d_strings[] = {
#define _(sym,string) string,
  foreach_srv6_end_v6_d_error
#undef _
};

static char *srv6_end_error_v6_d_di_strings[] = {
#define _(sym,string) string,
  foreach_srv6_end_v6_d_di_error
#undef _
};

static char *srv6_end_error_v6_dt_strings[] = {
#define _(sym,string) string,
  foreach_srv6_end_v6_dt_error
#undef _
};

static char *srv6_t_error_v4_dt_strings[] = {
#define _(sym,string) string,
  foreach_srv6_t_v4_dt_error
#undef _
};

typedef enum
{
  SRV6_END_M_GTP4_E_NEXT_DROP,
  SRV6_END_M_GTP4_E_NEXT_LOOKUP,
  SRV6_END_M_GTP4_E_N_NEXT,
} srv6_end_m_gtp4_e_next_t;

typedef enum
{
  SRV6_T_M_GTP4_D_NEXT_DROP,
  SRV6_T_M_GTP4_D_NEXT_LOOKUP,
  SRV6_T_M_GTP4_D_N_NEXT,
} srv6_T_m_gtp4_d_next_t;

typedef enum
{
  SRV6_END_M_GTP6_E_NEXT_DROP,
  SRV6_END_M_GTP6_E_NEXT_LOOKUP,
  SRV6_END_M_GTP6_E_N_NEXT,
} srv6_end_m_gtp6_e_next_t;

typedef enum
{
  SRV6_END_M_GTP6_D_NEXT_DROP,
  SRV6_END_M_GTP6_D_NEXT_LOOKUP,
  SRV6_END_M_GTP6_D_N_NEXT,
} srv6_end_m_gtp6_d_next_t;

typedef enum
{
  SRV6_END_M_GTP6_D_DI_NEXT_DROP,
  SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP,
  SRV6_END_M_GTP6_D_DI_N_NEXT,
} srv6_end_m_gtp6_d_di_next_t;

typedef enum
{
  SRV6_END_M_GTP6_DT_NEXT_DROP,
  SRV6_END_M_GTP6_DT_NEXT_LOOKUP4,
  SRV6_END_M_GTP6_DT_NEXT_LOOKUP6,
  SRV6_END_M_GTP6_DT_N_NEXT,
} srv6_end_m_gtp6_dt_next_t;

typedef enum
{
  SRV6_T_M_GTP4_DT_NEXT_DROP,
  SRV6_T_M_GTP4_DT_NEXT_LOOKUP4,
  SRV6_T_M_GTP4_DT_NEXT_LOOKUP6,
  SRV6_T_M_GTP4_DT_N_NEXT,
} srv6_t_m_gtp4_dt_next_t;

static inline u16
hash_uword_to_u16 (uword * key)
{
  u16 *val;
  val = (u16 *) key;
#if uword_bits == 64
  return val[0] ^ val[1] ^ val[2] ^ val[3];
#else
  return val[0] ^ val[1];
#endif
}

static inline u8
gtpu_type_get (u16 tag)
{
  u16 val;

  val = clib_net_to_host_u16 (tag);
  if (val & SRH_TAG_ECHO_REPLY)
    return GTPU_TYPE_ECHO_REPLY;
  else if (val & SRH_TAG_ECHO_REQUEST)
    return GTPU_TYPE_ECHO_REQUEST;
  else if (val & SRH_TAG_ERROR_INDICATION)
    return GTPU_TYPE_ERROR_INDICATION;
  else if (val & SRH_TAG_END_MARKER)
    return GTPU_TYPE_END_MARKER;

  return GTPU_TYPE_GTPU;
}

// 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)
{
  srv6_end_main_v4_t *sm = &srv6_end_main_v4;
  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;

  from = vlib_frame_vector_args (frame);
  n_left_from = frame->n_vectors;
  next_index = node->cached_next_index;

  while (n_left_from > 0)
    {
      u32 n_left_to_next;

      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);

      while (n_left_from > 0 && n_left_to_next > 0)
	{
	  u32 bi0;
	  vlib_buffer_t *b0;
	  ip6_sr_localsid_t *ls0;
	  srv6_end_gtp4_param_t *ls_param;

	  ip6srv_combo_header_t *ip6srv0;
	  ip6_address_t src0, dst0;

	  ip4_gtpu_header_t *hdr0 = NULL;
	  uword len0;

	  u32 next0 = SRV6_END_M_GTP4_E_NEXT_LOOKUP;

	  // 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);
	  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;

	  ip6srv0 = vlib_buffer_get_current (b0);
	  src0 = ip6srv0->ip.src_address;
	  dst0 = ip6srv0->ip.dst_address;

	  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)))
	    {
	      next0 = SRV6_END_M_GTP4_E_NEXT_DROP;

	      bad_n++;
	    }
	  else
	    {
	      u8 gtpu_type = 0;
	      u16 tag = 0;
	      u32 teid = 0;
	      u8 *teid8p = (u8 *) & teid;
	      u8 qfi = 0;
	      u16 seq = 0;
	      u32 index;
	      u32 offset, shift;
	      u32 hdrlen = 0;
	      uword key;
	      u16 port;
	      ip4_address_t dst4;
	      u16 ie_size = 0;
	      u8 ie_buf[GTPU_IE_MAX_SIZ];
	      void *p;
	      uword plen;

	      if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE)
		{
		  tag = ip6srv0->sr.tag;
		}

	      offset = ls0->localsid_prefix_len / 8;
	      shift = ls0->localsid_prefix_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];

		  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;
		      dst4.as_u8[index] |=
			dst0.as_u8[offset + index + 1] >> (8 - shift);
		    }

		  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)
		    {
		      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
		    {
		      for (index = 0; index < 4; index++)
			{
			  *teid8p = dst0.as_u8[offset + 5 + index] << shift;
			  *teid8p |=
			    dst0.as_u8[offset + 6 + index] >> (8 - shift);
			  teid8p++;
			}
		    }
		}

	      if (qfi)
		{
		  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)
		{
		  hdrlen = sizeof (gtpu_exthdr_t);
		}

	      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ECHO_REPLY))
		{
		  hdrlen += sizeof (gtpu_recovery_ie);
		}

	      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
		{
		  ip6_sr_tlv_t *tlv;
		  u16 ext_len;

		  ext_len = ip6srv0->sr.length * 8;

		  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));

		      if (tlv->type == SRH_TLV_USER_PLANE_CONTAINER)
			{
			  user_plane_sub_tlv_t *sub_tlv;

			  sub_tlv = (user_plane_sub_tlv_t *) tlv->value;

			  ie_size = sub_tlv->length;
			  clib_memcpy_fast (ie_buf, sub_tlv->value, ie_size);

			  hdrlen += ie_size;
			}
		    }
		}

	      if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE)
		{
		  vlib_buffer_advance (b0,
				       (word) sizeof (ip6srv_combo_header_t) +
				       ip6srv0->sr.length * 8);
		}
	      else
		{
		  vlib_buffer_advance (b0, (word) sizeof (ip6_header_t));
		}

	      // get length of encapsulated IPv6 packet (the remaining part)
	      p = vlib_buffer_get_current (b0);

	      plen = len0 = vlib_buffer_length_in_chain (vm, b0);

	      len0 += hdrlen;

	      hdrlen += sizeof (ip4_gtpu_header_t);

	      // IPv4 GTP-U header creation.
	      vlib_buffer_advance (b0, -(word) hdrlen);

	      hdr0 = vlib_buffer_get_current (b0);

	      clib_memcpy_fast (hdr0, &sm->cache_hdr,
				sizeof (ip4_gtpu_header_t));

	      hdr0->ip4.dst_address.as_u32 = dst4.as_u32;

	      hdr0->gtpu.teid = teid;
	      hdr0->gtpu.length = clib_host_to_net_u16 (len0);

	      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)
		{
		  hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG;
		  hdr0->gtpu.ext->seq = seq;
		  hdr0->gtpu.ext->npdu_num = 0;
		  hdr0->gtpu.ext->nextexthdr = 0;

		  if (gtpu_type == GTPU_TYPE_ECHO_REPLY)
		    {
		      gtpu_recovery_ie *recovery;

		      recovery =
			(gtpu_recovery_ie *) ((u8 *) hdr0 +
					      (hdrlen -
					       sizeof (gtpu_recovery_ie)));
		      recovery->type = GTPU_RECOVERY_IE_TYPE;
		      recovery->restart_counter = 0;
		    }
		  else if (gtpu_type == GTPU_TYPE_ERROR_INDICATION)
		    {
		      if (ie_size)
			{
			  u8 *ie_ptr;

			  ie_ptr = (u8 *) ((u8 *) hdr0 + (hdrlen - ie_size));
			  clib_memcpy_fast (ie_ptr, ie_buf, ie_size);
			}
		    }
		}

	      offset = ls_param->v4src_position / 8;
	      shift = ls_param->v4src_position % 8;

	      if (PREDICT_TRUE (shift == 0))
		{
		  for (index = 0; index < 4; index++)
		    {
		      hdr0->ip4.src_address.as_u8[index] =
			src0.as_u8[offset + index];
		    }
		}
	      else
		{
		  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);
		    }
		}

	      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->ip4.length = clib_host_to_net_u16 (len0 +
						       sizeof
						       (ip4_gtpu_header_t));

	      hdr0->ip4.checksum = ip4_header_checksum (&hdr0->ip4);

	      good_n++;

	      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, hdr0->ip4.src_address.as_u8,
			       sizeof (tr->src.as_u8));
		  clib_memcpy (tr->dst.as_u8, 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_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
					   n_left_to_next, bi0, next0);
	}

      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
    }

  vlib_node_increment_counter (vm, sm->end_m_gtp4_e_node_index,
			       SRV6_END_ERROR_M_GTP4_E_BAD_PACKETS, bad_n);

  vlib_node_increment_counter (vm, sm->end_m_gtp4_e_node_index,
			       SRV6_END_ERROR_M_GTP4_E_PACKETS, good_n);

  return frame->n_vectors;
}

// 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)
{
  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;

  from = vlib_frame_vector_args (frame);
  n_left_from = frame->n_vectors;
  next_index = node->cached_next_index;

  while (n_left_from > 0)
    {
      u32 n_left_to_next;

      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);

      while (n_left_from > 0 && n_left_to_next > 0)
	{
	  u32 bi0;
	  vlib_buffer_t *b0;
	  ip6_sr_sl_t *sl0;
	  srv6_end_gtp4_param_t *ls_param;
	  ip4_header_t *ip4;

	  uword len0;

	  u32 next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP;

	  // 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_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;

	      bad_n++;
	    }
	  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))
		{
		  // Extention header.
		  hdr_len += sizeof (gtpu_exthdr_t);

		  seq = hdr->gtpu.ext->seq;

		  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;

		      hdr_len += sizeof (gtpu_pdu_session_t);

		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
			{
			  hdr_len += sizeof (gtpu_paging_policy_t);
			}
		    }
		}

	      src = hdr->ip4.src_address;
	      srcp = (u8 *) & src;

	      dst = hdr->ip4.dst_address;
	      dstp = (u8 *) & dst;

	      seg = ls_param->sr_prefix;

	      offset = ls_param->sr_prefixlen / 8;
	      shift = ls_param->sr_prefixlen % 8;

	      if (PREDICT_TRUE (shift == 0))
		{
		  clib_memcpy_fast (&seg.as_u8[offset], dstp, 4);

		  if (qfip)
		    {
		      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;
			}

		      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
		    {
		      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);
		    }

		  if (qfip)
		    {
		      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;
			}

		      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);
			}
		    }
		}

	      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
		{
		  u16 payload_len;

		  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;

			  ies = (u8 *) ((u8 *) hdr + hdr_len);
			  clib_memcpy_fast (ie_buf, ies, ie_size);
			  hdr_len += ie_size;
			}
		    }
		}

	      src6 = ls_param->v6src_prefix;

	      offset = ls_param->v6src_prefixlen / 8;
	      shift = ls_param->v6src_prefixlen % 8;

	      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);
		    }
		}

	      vlib_buffer_advance (b0, (word) hdr_len);

	      // Encap to SRv6.
	      if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
		{
		  encap = vlib_buffer_get_current (b0);
		}

	      len0 = vlib_buffer_length_in_chain (vm, b0);

	      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 (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 (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 (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
		    {
		      hdr_len += sizeof (ip6_sr_header_t);
		      hdr_len += sizeof (ip6_address_t);
		    }
		}

	      if (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;
		}

	      vlib_buffer_advance (b0, -(word) hdr_len);
	      ip6srv = vlib_buffer_get_current (b0);

	      if (sl)
		{
		  clib_memcpy_fast (ip6srv, sl->rewrite,
				    vec_len (sl->rewrite));

		  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.length += sizeof (ip6_address_t) / 8;
		      ip6srv->sr.segments[0] = seg;

		      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.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))
		    {
		      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));

		  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.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)
			{
			  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)
			{
			  ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
			}
		    }
		}

	      ip6srv->ip.src_address = src6;

	      if (PREDICT_FALSE (ie_size))
		{
		  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);
		}

	      ip6srv->ip.payload_length =
		clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));

	      good_n++;

	      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));
		}
	    }

	DONE:
	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
					   n_left_to_next, bi0, next0);
	}

      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
    }

  vlib_node_increment_counter (vm, sm->t_m_gtp4_d_node_index,
			       SRV6_T_ERROR_M_GTP4_D_BAD_PACKETS, bad_n);

  vlib_node_increment_counter (vm, sm->t_m_gtp4_d_node_index,
			       SRV6_T_ERROR_M_GTP4_D_PACKETS, good_n);

  return frame->n_vectors;
}

VLIB_REGISTER_NODE (srv6_end_m_gtp4_e) =
{
  .name = "srv6-end-m-gtp4-e",.vector_size = sizeof (u32),.format_trace =
    format_srv6_end_rewrite_trace,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
    ARRAY_LEN (srv6_end_error_v4_strings),.error_strings =
    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",}
,};

VLIB_REGISTER_NODE (srv6_t_m_gtp4_d) =
{
  .name = "srv6-t-m-gtp4-d",.vector_size = sizeof (u32),.format_trace =
    format_srv6_end_rewrite_trace,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
    ARRAY_LEN (srv6_t_error_v4_d_strings),.error_strings =
    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",}
,};

// 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)
{
  srv6_end_main_v6_t *sm = &srv6_end_main_v6;
  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;

  from = vlib_frame_vector_args (frame);
  n_left_from = frame->n_vectors;
  next_index = node->cached_next_index;

  while (n_left_from > 0)
    {
      u32 n_left_to_next;

      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);

      while (n_left_from > 0 && n_left_to_next > 0)
	{
	  u32 bi0;
	  vlib_buffer_t *b0;
	  ip6_sr_localsid_t *ls0;

	  ip6srv_combo_header_t *ip6srv0;
	  ip6_address_t dst0, src0, seg0;

	  ip6_gtpu_header_t *hdr0 = NULL;
	  uword len0;
	  uword key;
	  u16 port;
	  u16 tag;
	  void *p;
	  uword plen;

	  u32 next0 = SRV6_END_M_GTP6_E_NEXT_LOOKUP;

	  // 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);
	  ls0 =
	    pool_elt_at_index (sm2->localsids,
			       vnet_buffer (b0)->ip.adj_index[VLIB_TX]);

	  ip6srv0 = vlib_buffer_get_current (b0);
	  dst0 = ip6srv0->ip.dst_address;
	  src0 = ip6srv0->ip.src_address;
	  seg0 = ip6srv0->sr.segments[0];

	  tag = ip6srv0->sr.tag;

	  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))
	    {
	      next0 = SRV6_END_M_GTP6_E_NEXT_DROP;

	      bad_n++;
	    }
	  else
	    {
	      // we need to be sure there is enough space before
	      // ip6srv0 header, there is some extra space
	      // in the pre_data area for this kind of
	      // logic

	      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;
	      u16 ie_size = 0;
	      u8 ie_buf[GTPU_IE_MAX_SIZ];

	      index = ls0->localsid_prefix_len;
	      index += 8;
	      offset = index / 8;
	      shift = index % 8;

	      gtpu_type = gtpu_type_get (tag);

	      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 (&seq, &dst0.as_u8[offset], 2);
		    }
		  else
		    {
		      clib_memcpy_fast (teid8p, &dst0.as_u8[offset], 4);
		    }

		  qfi = dst0.as_u8[offset + 4];
		}
	      else
		{
		  u8 *sp;

		  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 + 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;
		  qfi |= dst0.as_u8[offset + 5] >> (8 - shift);
		}

	      if (qfi)
		{
		  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)
		{
		  hdrlen = sizeof (gtpu_exthdr_t);
		}

	      if (gtpu_type == GTPU_TYPE_ECHO_REPLY)
		{
		  hdrlen += sizeof (gtpu_recovery_ie);
		}

	      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
		{
		  ip6_sr_tlv_t *tlv;
		  u16 ext_len;

		  ext_len = ip6srv0->sr.length * 8;

		  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));

		      if (tlv->type == SRH_TLV_USER_PLANE_CONTAINER)
			{
			  user_plane_sub_tlv_t *sub_tlv;

			  sub_tlv = (user_plane_sub_tlv_t *) tlv->value;

			  ie_size = sub_tlv->length;
			  clib_memcpy_fast (ie_buf, sub_tlv->value, ie_size);

			  hdrlen += ie_size;
			}
		    }
		}

	      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);

	      plen = len0 = vlib_buffer_length_in_chain (vm, b0);

	      len0 += hdrlen;

	      hdrlen += sizeof (ip6_gtpu_header_t);

	      vlib_buffer_advance (b0, -(word) hdrlen);

	      hdr0 = vlib_buffer_get_current (b0);

	      clib_memcpy_fast (hdr0, &sm->cache_hdr,
				sizeof (ip6_gtpu_header_t));

	      hdr0->gtpu.teid = teid;
	      hdr0->gtpu.length = clib_host_to_net_u16 (len0);

	      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)
		{
		  hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG;
		  hdr0->gtpu.ext->seq = seq;
		  hdr0->gtpu.ext->npdu_num = 0;
		  hdr0->gtpu.ext->nextexthdr = 0;

		  if (gtpu_type == GTPU_TYPE_ECHO_REPLY)
		    {
		      gtpu_recovery_ie *recovery;

		      recovery =
			(gtpu_recovery_ie *) ((u8 *) hdr0 +
					      (hdrlen -
					       sizeof (gtpu_recovery_ie)));
		      recovery->type = GTPU_RECOVERY_IE_TYPE;
		      recovery->restart_counter = 0;
		    }
		  else if (gtpu_type == GTPU_TYPE_ERROR_INDICATION)
		    {
		      if (ie_size)
			{
			  u8 *ie_ptr;

			  ie_ptr = (u8 *) ((u8 *) hdr0 + (hdrlen - ie_size));
			  clib_memcpy_fast (ie_ptr, ie_buf, ie_size);
			}
		    }
		}

	      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));

	      // UDP source port.
	      key = hash_memory (p, plen < 40 ? plen : 40, 0);
	      port = hash_uword_to_u16 (&key);
	      hdr0->udp.src_port = port;

	      good_n++;

	      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, hdr0->ip6.src_address.as_u8,
			       sizeof (ip6_address_t));
		  clib_memcpy (tr->dst.as_u8, hdr0->ip6.dst_address.as_u8,
			       sizeof (ip6_address_t));
		  tr->teid = hdr0->gtpu.teid;
		}
	    }

	  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);
	}

      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
    }

  vlib_node_increment_counter (vm, sm->end_m_gtp6_e_node_index,
			       SRV6_END_ERROR_M_GTP6_E_BAD_PACKETS, bad_n);

  vlib_node_increment_counter (vm, sm->end_m_gtp6_e_node_index,
			       SRV6_END_ERROR_M_GTP6_E_PACKETS, good_n);

  return frame->n_vectors;
}

// 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)
{
  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;

  from = vlib_frame_vector_args (frame);
  n_left_from = frame->n_vectors;
  next_index = node->cached_next_index;

  while (n_left_from > 0)
    {
      u32 n_left_to_next;

      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);

      while (n_left_from > 0 && n_left_to_next > 0)
	{
	  u32 bi0;
	  vlib_buffer_t *b0;
	  ip6_sr_localsid_t *ls0;
	  srv6_end_gtp6_param_t *ls_param;

	  ip6_gtpu_header_t *hdr0 = NULL;
	  uword len0;

	  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];

	  u32 next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP;

	  // 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);
	  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;

	  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)))
	    {
	      next0 = SRV6_END_M_GTP6_D_NEXT_DROP;

	      bad_n++;
	    }
	  else
	    {
	      seg0 = ls_param->sr_prefix;
	      src0 = hdr0->ip6.src_address;

	      gtpu_type = hdr0->gtpu.type;

	      teid = hdr0->gtpu.teid;
	      teidp = (u8 *) & teid;

	      seqp = (u8 *) & seq;

	      if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
		{
		  // Extention header.
		  hdrlen += sizeof (gtpu_exthdr_t);

		  seq = hdr0->gtpu.ext->seq;

		  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;

		      hdrlen += sizeof (gtpu_pdu_session_t);

		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
			{
			  hdrlen += sizeof (gtpu_paging_policy_t);
			}
		    }
		}

	      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)
		    {
		      clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
		    }
		  else
		    {
		      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);

		      if (sess->type)
			{
			  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
			}

		      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)
		    {
		      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)
		    {
		      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 + 4] |= qfi >> shift;
		      seg0.as_u8[offset + 5] |= qfi << (8 - shift);
		    }
		}

	      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
		{
		  u16 payload_len;

		  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;

			  ies = (u8 *) ((u8 *) hdr0 + hdrlen);
			  clib_memcpy_fast (ie_buf, ies, ie_size);
			  hdrlen += ie_size;
			}
		    }
		}

	      // jump over variable length data
	      vlib_buffer_advance (b0, (word) hdrlen);

	      // get length of encapsulated IPv6 packet (the remaining part)
	      len0 = vlib_buffer_length_in_chain (vm, b0);

	      if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
		{
		  encap = vlib_buffer_get_current (b0);
		}

	      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)
		{
		  sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
		}

	      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 (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 (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
		    {
		      hdr_len += sizeof (ip6_sr_header_t);
		      hdr_len += sizeof (ip6_address_t);
		    }
		}

	      if (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;
		}

	      // jump back to data[0] or pre_data if required
	      vlib_buffer_advance (b0, -(word) hdr_len);

	      ip6srv = vlib_buffer_get_current (b0);

	      if (sl)
		{
		  clib_memcpy_fast (ip6srv, sl->rewrite,
				    vec_len (sl->rewrite));

		  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.segments_left += 1;
		      ip6srv->sr.last_entry += 1;

		      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))
		    {
		      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));

		  ip6srv->ip.src_address = src0;
		  ip6srv->ip.dst_address = seg0;

		  if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
		    {
		      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;

		      ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;

		      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] = seg0;
		    }
		  else
		    {
		      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)
			{
			  ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
			}
		    }
		}

	      if (PREDICT_FALSE (ie_size))
		{
		  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);
		}

	      ip6srv->ip.payload_length =
		clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));

	      good_n++;

	      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;
		}
	    }

	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);
	}

      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
    }

  vlib_node_increment_counter (vm, sm->end_m_gtp6_d_node_index,
			       SRV6_END_ERROR_M_GTP6_D_BAD_PACKETS, bad_n);

  vlib_node_increment_counter (vm, sm->end_m_gtp6_d_node_index,
			       SRV6_END_ERROR_M_GTP6_D_PACKETS, good_n);

  return frame->n_vectors;
}

// 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)
{
  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;

  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;

  while (n_left_from > 0)
    {
      u32 n_left_to_next;

      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);

      while (n_left_from > 0 && n_left_to_next > 0)
	{
	  u32 bi0;
	  vlib_buffer_t *b0;
	  ip6_sr_localsid_t *ls0;

	  ip6_gtpu_header_t *hdr0 = NULL;
	  uword len0;

	  ip6_address_t dst0;
	  ip6_address_t src0;
	  ip6_address_t seg0;
	  u32 teid = 0;
	  u8 *teidp;
	  u8 gtpu_type = 0;
	  u8 qfi = 0;
	  u8 *qfip = NULL;
	  u16 seq = 0;
	  u8 *seqp;
	  u32 offset, shift;
	  u32 hdrlen;
	  ip6_header_t *encap = NULL;
	  gtpu_pdu_session_t *sess;
	  int ie_size = 0;
	  u16 tlv_siz = 0;
	  u8 ie_buf[GTPU_IE_MAX_SIZ];

	  u32 next0 = SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP;

	  // 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);
	  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;

	  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)))
	    {
	      next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;

	      bad_n++;
	    }
	  else
	    {
	      dst0 = hdr0->ip6.dst_address;
	      src0 = hdr0->ip6.src_address;

	      gtpu_type = hdr0->gtpu.type;

	      seg0 = ls_param->sr_prefix;
	      teid = hdr0->gtpu.teid;
	      teidp = (u8 *) & teid;

	      seqp = (u8 *) & seq;

	      if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
		{
		  // Extention header.
		  hdrlen += sizeof (gtpu_exthdr_t);

		  seq = hdr0->gtpu.ext->seq;

		  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
		    {
		      // PDU Session Container.
		      sess =
			(gtpu_pdu_session_t *) (((char *) hdr0) + hdrlen);
		      qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
		      qfip = &qfi;

		      hdrlen += sizeof (gtpu_pdu_session_t);

		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
			{
			  hdrlen += sizeof (gtpu_paging_policy_t);
			}
		    }
		}

	      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)
		    {
		      clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
		    }
		  else
		    {
		      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);

		      if (sess->type)
			{
			  qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
			}

		      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)
		    {
		      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)
		    {
		      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 + 4] |= qfi >> shift;
		      seg0.as_u8[offset + 5] |= qfi << (8 - shift);
		    }
		}

	      if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
		{
		  u16 payload_len;

		  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;

			  ies = (u8 *) ((u8 *) hdr0 + hdrlen);
			  clib_memcpy_fast (ie_buf, ies, ie_size);
			  hdrlen += ie_size;
			}
		    }
		}

	      // jump over variable length data
	      vlib_buffer_advance (b0, (word) hdrlen);

	      // get length of encapsulated IPv6 packet (the remaining part)
	      len0 = vlib_buffer_length_in_chain (vm, b0);

	      if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
		{
		  encap = vlib_buffer_get_current (b0);
		}

	      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)
		{
		  sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
		}

	      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;
		  }
		}

	      hdr_len = sizeof (ip6srv_combo_header_t);

	      if (sl)
		hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);

	      hdr_len += sizeof (ip6_address_t) * 2;

	      if (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;
		}

	      // jump back to data[0] or pre_data if required
	      vlib_buffer_advance (b0, -(word) hdr_len);

	      ip6srv = vlib_buffer_get_current (b0);

	      if (sl)
		{
		  clib_memcpy_fast (ip6srv, sl->rewrite,
				    vec_len (sl->rewrite));

		  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.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
		{
		  clib_memcpy_fast (ip6srv, &sm->cache_hdr,
				    sizeof (ip6_header_t));

		  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[0] = dst0;
		}

	      if (PREDICT_FALSE (ie_size))
		{
		  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->length = (u8) (ie_size);
		  clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);

		  ip6srv->sr.length += (u8) (tlv_siz / 8);
		}

	      ip6srv->ip.payload_length =
		clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
	      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;

	      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_DI_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_DI_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;
		}

	      good_n++;

	      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;
		}
	    }

	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_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
					   n_left_to_next, bi0, next0);
	}

      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
    }

  vlib_node_increment_counter (vm, sm->end_m_gtp6_d_di_node_index,
			       SRV6_END_ERROR_M_GTP6_D_DI_BAD_PACKETS, bad_n);

  vlib_node_increment_counter (vm, sm->end_m_gtp6_d_di_node_index,
			       SRV6_END_ERROR_M_GTP6_D_DI_PACKETS, good_n);

  return frame->n_vectors;
}

// 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)
{
  srv6_end_main_v6_dt_t *sm = &srv6_end_main_v6_dt;
  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;

  from = vlib_frame_vector_args (frame);
  n_left_from = frame->n_vectors;
  next_index = node->cached_next_index;

  while (n_left_from > 0)
    {
      u32 n_left_to_next;

      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);

      while (n_left_from > 0 && n_left_to_next > 0)
	{
	  u32 bi0;
	  vlib_buffer_t *b0;
	  srv6_end_gtp6_dt_param_t *ls_param;
	  ip6_sr_localsid_t *ls0;

	  ip6_gtpu_header_t *hdr0 = NULL;
	  ip4_header_t *ip4 = NULL;
	  ip6_header_t *ip6 = NULL;
	  ip6_address_t src, dst;
	  u32 teid;
	  u32 hdrlen;
	  u32 len0;

	  u32 next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;

	  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);
	  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;

	  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)))
	    {
	      next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;

	      bad_n++;
	    }
	  else
	    {
	      clib_memcpy_fast (src.as_u8, hdr0->ip6.src_address.as_u8,
				sizeof (ip6_address_t));
	      clib_memcpy_fast (dst.as_u8, hdr0->ip6.dst_address.as_u8,
				sizeof (ip6_address_t));

	      teid = hdr0->gtpu.teid;

	      if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
		{
		  hdrlen += sizeof (gtpu_exthdr_t);
		  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
		    {
		      gtpu_pdu_session_t *sess;

		      sess =
			(gtpu_pdu_session_t *) (((char *) hdr0) +
						sizeof (ip6_gtpu_header_t) +
						sizeof (gtpu_exthdr_t));

		      hdrlen += sizeof (gtpu_pdu_session_t);
		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
			{
			  hdrlen += sizeof (gtpu_paging_policy_t);
			}
		    }
		}

	      if (ls_param->type == SRV6_GTP6_DT4)
		{
		  vlib_buffer_advance (b0, (word) hdrlen);
		  ip4 = vlib_buffer_get_current (b0);
		  if ((ip4->ip_version_and_header_length & 0xf0) != 0x40)
		    {
		      next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
		      bad_n++;
		      goto DONE;
		    }

		  next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP4;
		  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
		    ls_param->fib4_index;
		}
	      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)
		    {
		      next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
		      bad_n++;
		      goto DONE;
		    }

		  next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
		  if ((ip6->dst_address.as_u8[0] == 0xff)
		      && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
		    {
		      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			ls_param->local_fib_index;
		    }
		  else
		    {
		      vlib_buffer_advance (b0, (word) hdrlen);
		      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			ls_param->fib6_index;
		    }
		}
	      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)
		    {
		      next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
		      if ((ip6->dst_address.as_u8[0] == 0xff)
			  && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
			{
			  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			    ls_param->local_fib_index;
			}
		      else
			{
			  vlib_buffer_advance (b0, (word) hdrlen);
			  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			    ls_param->fib6_index;
			}
		    }
		  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;
		      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			ls_param->fib4_index;
		    }
		  else
		    {
		      next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
		      bad_n++;
		      goto DONE;
		    }
		}
	      else
		{
		  next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
		  bad_n++;
		  goto DONE;
		}

	      good_n++;

	      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, src.as_u8,
			       sizeof (ip6_address_t));
		  clib_memcpy (tr->dst.as_u8, dst.as_u8,
			       sizeof (ip6_address_t));
		  tr->teid = teid;
		}
	    }

	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_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
					   n_left_to_next, bi0, next0);
	}

      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
    }

  vlib_node_increment_counter (vm, sm->end_m_gtp6_dt_node_index,
			       SRV6_END_ERROR_M_GTP6_DT_BAD_PACKETS, bad_n);

  vlib_node_increment_counter (vm, sm->end_m_gtp6_dt_node_index,
			       SRV6_END_ERROR_M_GTP6_DT_PACKETS, good_n);

  return frame->n_vectors;
}

// 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)
{
  srv6_t_main_v4_dt_t *sm = &srv6_t_main_v4_dt;
  ip6_sr_main_t *sm2 = &sr_main;
  u32 n_left_from, next_index, *from, *to_next;

  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;

  while (n_left_from > 0)
    {
      u32 n_left_to_next;

      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);

      while (n_left_from > 0 && n_left_to_next > 0)
	{
	  u32 bi0;
	  vlib_buffer_t *b0;
	  srv6_t_gtp4_dt_param_t *ls_param;
	  ip6_sr_sl_t *ls0;

	  ip4_gtpu_header_t *hdr0 = NULL;
	  ip4_header_t *ip4 = NULL;
	  ip6_header_t *ip6 = NULL;
	  ip6_address_t src, dst;
	  u32 teid;
	  u32 hdrlen;
	  u32 len0;

	  u32 next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;

	  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);
	  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;

	  hdr0 = vlib_buffer_get_current (b0);

	  hdrlen = sizeof (ip4_gtpu_header_t);

	  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)))
	    {
	      next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;

	      bad_n++;
	    }
	  else
	    {
	      clib_memcpy_fast (src.as_u8, hdr0->ip4.src_address.as_u8,
				sizeof (ip4_address_t));
	      clib_memcpy_fast (dst.as_u8, hdr0->ip4.dst_address.as_u8,
				sizeof (ip4_address_t));

	      teid = hdr0->gtpu.teid;

	      if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
		{
		  hdrlen += sizeof (gtpu_exthdr_t);
		  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
		    {
		      gtpu_pdu_session_t *sess;

		      sess =
			(gtpu_pdu_session_t *) (((char *) hdr0) +
						sizeof (ip6_gtpu_header_t) +
						sizeof (gtpu_exthdr_t));

		      hdrlen += sizeof (gtpu_pdu_session_t);
		      if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
			{
			  hdrlen += sizeof (gtpu_paging_policy_t);
			}
		    }
		}

	      if (ls_param->type == SRV6_GTP4_DT4)
		{
		  vlib_buffer_advance (b0, (word) hdrlen);
		  ip4 = vlib_buffer_get_current (b0);
		  if ((ip4->ip_version_and_header_length & 0xf0) != 0x40)
		    {
		      next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
		      bad_n++;
		      goto DONE;
		    }

		  next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
		  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
		    ls_param->fib4_index;
		}
	      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)
		    {
		      next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
		      bad_n++;
		      goto DONE;
		    }

		  next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
		  if ((ip6->dst_address.as_u8[0] == 0xff)
		      && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
		    {
		      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
		      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			ls_param->local_fib_index;
		    }
		  else
		    {
		      vlib_buffer_advance (b0, (word) hdrlen);
		      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			ls_param->fib6_index;
		    }
		}
	      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)
		    {
		      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
		      if ((ip6->dst_address.as_u8[0] == 0xff)
			  && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
			{
			  next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
			  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			    ls_param->local_fib_index;
			}
		      else
			{
			  vlib_buffer_advance (b0, (word) hdrlen);
			  vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			    ls_param->fib6_index;
			}
		    }
		  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;
		      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
			ls_param->fib4_index;
		    }
		  else
		    {
		      next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
		      bad_n++;
		      goto DONE;
		    }
		}
	      else
		{
		  next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
		  bad_n++;
		  goto DONE;
		}

	      good_n++;

	      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, src.as_u8,
			       sizeof (ip6_address_t));
		  clib_memcpy (tr->dst.as_u8, dst.as_u8,
			       sizeof (ip6_address_t));
		  tr->teid = teid;
		}
	    }

	DONE:
	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
					   n_left_to_next, bi0, next0);
	}

      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
    }

  vlib_node_increment_counter (vm, sm->t_m_gtp4_dt_node_index,
			       SRV6_T_ERROR_M_GTP4_DT_BAD_PACKETS, bad_n);

  vlib_node_increment_counter (vm, sm->t_m_gtp4_dt_node_index,
			       SRV6_T_ERROR_M_GTP4_DT_PACKETS, good_n);

  return frame->n_vectors;
}

VLIB_REGISTER_NODE (srv6_end_m_gtp6_e) =
{
  .name = "srv6-end-m-gtp6-e",.vector_size = sizeof (u32),.format_trace =
    format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
    ARRAY_LEN (srv6_end_error_v6_e_strings),.error_strings =
    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",}
,};

VLIB_REGISTER_NODE (srv6_end_m_gtp6_d) =
{
  .name = "srv6-end-m-gtp6-d",.vector_size = sizeof (u32),.format_trace =
    format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
    ARRAY_LEN (srv6_end_error_v6_d_strings),.error_strings =
    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",}
,};

VLIB_REGISTER_NODE (srv6_end_m_gtp6_d_di) =
{
  .name = "srv6-end-m-gtp6-d-di",.vector_size = sizeof (u32),.format_trace =
    format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
    ARRAY_LEN (srv6_end_error_v6_d_di_strings),.error_strings =
    srv6_end_error_v6_d_di_strings,.n_next_nodes =
    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",}
,};

VLIB_REGISTER_NODE (srv6_end_m_gtp6_dt) =
{
  .name = "srv6-end-m-gtp6-dt",.vector_size = sizeof (u32),.format_trace =
    format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
    ARRAY_LEN (srv6_end_error_v6_dt_strings),.error_strings =
    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",}
,};

VLIB_REGISTER_NODE (srv6_t_m_gtp4_dt) =
{
  .name = "srv6-t-m-gtp4-dt",.vector_size = sizeof (u32),.format_trace =
    format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
    ARRAY_LEN (srv6_t_error_v4_dt_strings),.error_strings =
    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",}
,};

/*
* fd.io coding-style-patch-verification: ON
*
* Local Variables:
* eval: (c-set-style "gnu")
* End:
*/