srv6-mobile: Fix the position of QFI

[vpp.git] / src / plugins / srv6-mobile / node.c

diff --git a/src/plugins/srv6-mobile/node.c b/src/plugins/srv6-mobile/node.c
index 0120c67..fd11c07 100644 (file)
--- a/src/plugins/srv6-mobile/node.c
+++ b/src/plugins/srv6-mobile/node.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019 Arrcus Inc and/or its affiliates.
+ * Copyright (c) 2020 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:
@@ -400,14 +400,15 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
              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_len / 8;
-             shift = ls0->localsid_len % 8;
+             offset = ls0->localsid_prefix_len / 8;
+             shift = ls0->localsid_prefix_len % 8;
 
              gtpu_type = gtpu_type_get (tag);
 
@@ -450,7 +451,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
                      for (index = 0; index < 2; index++)
                        {
                          sp[index] = dst0.as_u8[offset + 5 + index] << shift;
-                         sp[index] =
+                         sp[index] |=
                            dst0.as_u8[offset + 6 + index] >> (8 - shift);
                        }
                    }
@@ -527,7 +528,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
              // get length of encapsulated IPv6 packet (the remaining part)
              p = vlib_buffer_get_current (b0);
 
-             len0 = vlib_buffer_length_in_chain (vm, b0);
+             plen = len0 = vlib_buffer_length_in_chain (vm, b0);
 
              len0 += hdrlen;
 
@@ -631,7 +632,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
                    }
                }
 
-             key = hash_memory (p, len0, 0);
+             key = hash_memory (p, plen < 40 ? plen : 40, 0);
              port = hash_uword_to_u16 (&key);
              hdr0->udp.src_port = port;
 
@@ -763,11 +764,11 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
              u8 qfi = 0;
              u8 *qfip = NULL;
              u16 seq = 0;
-             u8 *seqp = NULL;
+             u8 *seqp;
              u32 offset, shift, index;
              ip6srv_combo_header_t *ip6srv;
              gtpu_pdu_session_t *sess = NULL;
-             u16 ie_size = 0;
+             int ie_size = 0;
              u16 tlv_siz = 0;
              u8 ie_buf[GTPU_IE_MAX_SIZ];
 
@@ -779,6 +780,8 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
              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))
@@ -787,7 +790,6 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
                  hdr_len += sizeof (gtpu_exthdr_t);
 
                  seq = hdr->gtpu.ext->seq;
-                 seqp = (u8 *) & seq;
 
                  if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
                    {
@@ -899,16 +901,18 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
                  u16 payload_len;
 
                  payload_len = clib_net_to_host_u16 (hdr->gtpu.length);
-                 if (payload_len != 0
-                     && payload_len > hdr_len - sizeof (ip4_gtpu_header_t))
+                 if (payload_len != 0)
                    {
-                     u8 *ies;
-
-                     ies = (u8 *) ((u8 *) hdr + hdr_len);
                      ie_size =
                        payload_len - (hdr_len - sizeof (ip4_gtpu_header_t));
-                     clib_memcpy_fast (ie_buf, ies, ie_size);
-                     hdr_len += ie_size;
+                     if (ie_size > 0)
+                       {
+                         u8 *ies;
+
+                         ies = (u8 *) ((u8 *) hdr + hdr_len);
+                         clib_memcpy_fast (ie_buf, ies, ie_size);
+                         hdr_len += ie_size;
+                       }
                    }
                }
 
@@ -1077,12 +1081,12 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
                        }
                      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
                        {
-                         ip6srv->sr.protocol = IP_PROTOCOL_NONE;
+                         ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
                        }
                    }
                  else
                    {
-                     ip6srv->sr.protocol = IP_PROTOCOL_NONE;
+                     ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
                    }
                }
              else
@@ -1096,7 +1100,7 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
                    {
                      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
 
-                     ip6srv->sr.protocol = IP_PROTOCOL_NONE;
+                     ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
 
                      ip6srv->sr.tag =
                        clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
@@ -1146,7 +1150,7 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
                        }
                      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
                        {
-                         ip6srv->ip.protocol = IP_PROTOCOL_NONE;
+                         ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
                        }
                    }
                }
@@ -1161,15 +1165,15 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
                  tlv =
                    (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
                  tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
-                 tlv->length = tlv_siz - sizeof (ip6_sr_tlv_t);
+                 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 = ie_size;
+                 sub_tlv->length = (u8) ie_size;
                  clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
 
-                 ip6srv->sr.length += tlv_siz / 8;
+                 ip6srv->sr.length += (u8) (tlv_siz / 8);
                }
 
              ip6srv->ip.payload_length =
@@ -1267,6 +1271,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
          u16 port;
          u16 tag;
          void *p;
+         uword plen;
 
          u32 next0 = SRV6_END_M_GTP6_E_NEXT_LOOKUP;
 
@@ -1318,8 +1323,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
              u16 ie_size = 0;
              u8 ie_buf[GTPU_IE_MAX_SIZ];
 
-             index = ls0->localsid_len;
-             index += 8;
+             index = ls0->localsid_prefix_len;
              offset = index / 8;
              shift = index % 8;
 
@@ -1327,23 +1331,26 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 
              if (PREDICT_TRUE (shift == 0))
                {
+                 qfi = dst0.as_u8[offset];
+
                  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     clib_memcpy_fast (&seq, &dst0.as_u8[offset], 2);
+                     clib_memcpy_fast (&seq, &dst0.as_u8[offset + 1], 2);
                    }
                  else
                    {
-                     clib_memcpy_fast (teid8p, &dst0.as_u8[offset], 4);
+                     clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 1], 4);
                    }
-
-                 qfi = dst0.as_u8[offset + 4];
                }
              else
                {
                  u8 *sp;
 
+                 qfi |= dst0.as_u8[offset] << shift;
+                 qfi |= dst0.as_u8[offset + 1] >> (8 - shift);
+
                  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
@@ -1351,24 +1358,21 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
                      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);
+                         sp[index] = dst0.as_u8[offset + index + 1] << shift;
+                         sp[index] |=
+                           dst0.as_u8[offset + index + 2] >> (8 - shift);
                        }
                    }
                  else
                    {
                      for (index = 0; index < 4; index++)
                        {
-                         *teid8p = dst0.as_u8[offset + index] << shift;
+                         *teid8p = dst0.as_u8[offset + index + 1] << shift;
                          *teid8p |=
-                           dst0.as_u8[offset + index + 1] >> (8 - shift);
+                           dst0.as_u8[offset + index + 2] >> (8 - shift);
                          teid8p++;
                        }
                    }
-
-                 qfi |= dst0.as_u8[offset + 4] << shift;
-                 qfi |= dst0.as_u8[offset + 5] >> (8 - shift);
                }
 
              if (qfi)
@@ -1418,21 +1422,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
                    }
                }
 
-             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));
-               }
+             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);
 
-             len0 = vlib_buffer_length_in_chain (vm, b0);
+             plen = len0 = vlib_buffer_length_in_chain (vm, b0);
 
              len0 += hdrlen;
 
@@ -1528,7 +1525,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
                                                               (gtpu_header_t));
 
              // UDP source port.
-             key = hash_memory (p, len0, 0);
+             key = hash_memory (p, plen < 40 ? plen : 40, 0);
              port = hash_uword_to_u16 (&key);
              hdr0->udp.src_port = port;
 
@@ -1608,12 +1605,12 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
          u8 qfi;
          u8 *qfip = NULL;
          u16 seq = 0;
-         u8 *seqp = NULL;
+         u8 *seqp;
          u32 offset, shift;
          u32 hdrlen;
          ip6_header_t *encap = NULL;
          gtpu_pdu_session_t *sess = NULL;
-         u16 ie_size = 0;
+         int ie_size = 0;
          u16 tlv_siz = 0;
          u8 ie_buf[GTPU_IE_MAX_SIZ];
 
@@ -1659,13 +1656,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
              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;
-                 seqp = (u8 *) & seq;
 
                  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
                    {
@@ -1689,19 +1687,17 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
              offset = ls_param->sr_prefixlen / 8;
              shift = ls_param->sr_prefixlen % 8;
 
-             offset += 1;
              if (PREDICT_TRUE (shift == 0))
                {
                  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     if (seqp)
-                       clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
+                     clib_memcpy_fast (&seg0.as_u8[offset + 1], seqp, 2);
                    }
                  else
                    {
-                     clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
+                     clib_memcpy_fast (&seg0.as_u8[offset + 1], teidp, 4);
                    }
 
                  if (qfip)
@@ -1715,7 +1711,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                          qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
                        }
 
-                     seg0.as_u8[offset + 4] = qfi;
+                     seg0.as_u8[offset] = qfi;
                    }
                }
              else
@@ -1726,22 +1722,19 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     if (seqp)
+                     for (idx = 0; idx < 2; idx++)
                        {
-                         for (idx = 0; idx < 2; idx++)
-                           {
-                             seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
-                             seg0.as_u8[offset + idx + 1] |=
-                               seqp[idx] << (8 - shift);
-                           }
+                         seg0.as_u8[offset + idx + 1] |= seqp[idx] >> shift;
+                         seg0.as_u8[offset + idx + 2] |=
+                           seqp[idx] << (8 - shift);
                        }
                    }
                  else
                    {
                      for (idx = 0; idx < 4; idx++)
                        {
-                         seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
-                         seg0.as_u8[offset + idx + 1] |=
+                         seg0.as_u8[offset + idx + 1] |= teidp[idx] >> shift;
+                         seg0.as_u8[offset + idx + 2] |=
                            teidp[idx] << (8 - shift);
                        }
                    }
@@ -1757,8 +1750,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                          qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
                        }
 
-                     seg0.as_u8[offset + 4] |= qfi >> shift;
-                     seg0.as_u8[offset + 5] |= qfi << (8 - shift);
+                     seg0.as_u8[offset] |= qfi >> shift;
+                     seg0.as_u8[offset + 1] |= qfi << (8 - shift);
                    }
                }
 
@@ -1767,16 +1760,18 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                  u16 payload_len;
 
                  payload_len = clib_net_to_host_u16 (hdr0->gtpu.length);
-                 if (payload_len != 0
-                     && payload_len > hdrlen - sizeof (ip6_gtpu_header_t))
+                 if (payload_len != 0)
                    {
-                     u8 *ies;
-
-                     ies = (u8 *) ((u8 *) hdr0 + hdrlen);
                      ie_size =
                        payload_len - (hdrlen - sizeof (ip6_gtpu_header_t));
-                     clib_memcpy_fast (ie_buf, ies, ie_size);
-                     hdrlen += ie_size;
+                     if (ie_size > 0)
+                       {
+                         u8 *ies;
+
+                         ies = (u8 *) ((u8 *) hdr0 + hdrlen);
+                         clib_memcpy_fast (ie_buf, ies, ie_size);
+                         hdrlen += ie_size;
+                       }
                    }
                }
 
@@ -1937,12 +1932,12 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                        }
                      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
                        {
-                         ip6srv->sr.protocol = IP_PROTOCOL_NONE;
+                         ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
                        }
                    }
                  else
                    {
-                     ip6srv->sr.protocol = IP_PROTOCOL_NONE;
+                     ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
                    }
                }
              else
@@ -1957,7 +1952,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                    {
                      ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
 
-                     ip6srv->sr.protocol = IP_PROTOCOL_NONE;
+                     ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
 
                      ip6srv->sr.tag =
                        clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
@@ -2005,7 +2000,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                        }
                      else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
                        {
-                         ip6srv->ip.protocol = IP_PROTOCOL_NONE;
+                         ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
                        }
                    }
                }
@@ -2018,15 +2013,15 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                  tlv =
                    (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
                  tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
-                 tlv->length = tlv_siz - sizeof (ip6_sr_tlv_t);
+                 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 = ie_size;
+                 sub_tlv->length = (u8) ie_size;
                  clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
 
-                 ip6srv->sr.length += tlv_siz / 8;
+                 ip6srv->sr.length += (u8) (tlv_siz / 8);
                }
 
              ip6srv->ip.payload_length =
@@ -2114,12 +2109,12 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
          u8 qfi = 0;
          u8 *qfip = NULL;
          u16 seq = 0;
-         u8 *seqp = NULL;
+         u8 *seqp;
          u32 offset, shift;
          u32 hdrlen;
          ip6_header_t *encap = NULL;
          gtpu_pdu_session_t *sess;
-         u16 ie_size = 0;
+         int ie_size = 0;
          u16 tlv_siz = 0;
          u8 ie_buf[GTPU_IE_MAX_SIZ];
 
@@ -2166,13 +2161,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
              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;
-                 seqp = (u8 *) & seq;
 
                  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
                    {
@@ -2194,19 +2190,17 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
              offset = ls_param->sr_prefixlen / 8;
              shift = ls_param->sr_prefixlen % 8;
 
-             offset += 1;
              if (PREDICT_TRUE (shift == 0))
                {
                  if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     if (seqp)
-                       clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
+                     clib_memcpy_fast (&seg0.as_u8[offset + 1], seqp, 2);
                    }
                  else
                    {
-                     clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
+                     clib_memcpy_fast (&seg0.as_u8[offset + 1], teidp, 4);
                    }
 
                  if (qfip)
@@ -2220,7 +2214,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
                          qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
                        }
 
-                     seg0.as_u8[offset + 4] = qfi;
+                     seg0.as_u8[offset] = qfi;
                    }
                }
              else
@@ -2231,22 +2225,19 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     if (seqp)
+                     for (idx = 0; idx < 2; idx++)
                        {
-                         for (idx = 0; idx < 2; idx++)
-                           {
-                             seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
-                             seg0.as_u8[offset + idx + 1] |=
-                               seqp[idx] << (8 - shift);
-                           }
+                         seg0.as_u8[offset + idx + 1] |= seqp[idx] >> shift;
+                         seg0.as_u8[offset + idx + 2] |=
+                           seqp[idx] << (8 - shift);
                        }
                    }
                  else
                    {
                      for (idx = 0; idx < 4; idx++)
                        {
-                         seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
-                         seg0.as_u8[offset + idx + 1] |=
+                         seg0.as_u8[offset + idx + 1] |= teidp[idx] >> shift;
+                         seg0.as_u8[offset + idx + 2] |=
                            teidp[idx] << (8 - shift);
                        }
                    }
@@ -2262,8 +2253,8 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
                          qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
                        }
 
-                     seg0.as_u8[offset + 4] |= qfi >> shift;
-                     seg0.as_u8[offset + 5] |= qfi << (8 - shift);
+                     seg0.as_u8[offset] |= qfi >> shift;
+                     seg0.as_u8[offset + 1] |= qfi << (8 - shift);
                    }
                }
 
@@ -2272,16 +2263,18 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
                  u16 payload_len;
 
                  payload_len = clib_net_to_host_u16 (hdr0->gtpu.length);
-                 if (payload_len != 0
-                     && payload_len > hdrlen - sizeof (ip6_gtpu_header_t))
+                 if (payload_len != 0)
                    {
-                     u8 *ies;
-
-                     ies = (u8 *) ((u8 *) hdr0 + hdrlen);
                      ie_size =
                        payload_len - (hdrlen - sizeof (ip6_gtpu_header_t));
-                     clib_memcpy_fast (ie_buf, ies, ie_size);
-                     hdrlen += ie_size;
+                     if (ie_size > 0)
+                       {
+                         u8 *ies;
+
+                         ies = (u8 *) ((u8 *) hdr0 + hdrlen);
+                         clib_memcpy_fast (ie_buf, ies, ie_size);
+                         hdrlen += ie_size;
+                       }
                    }
                }
 
@@ -2427,14 +2420,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
                  tlv =
                    (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
                  tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
-                 tlv->length = tlv_siz - sizeof (ip6_sr_tlv_t);
+                 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 = ie_size;
+                 sub_tlv->length = (u8) (ie_size);
                  clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
 
-                 ip6srv->sr.length += tlv_siz / 8;
+                 ip6srv->sr.length += (u8) (tlv_siz / 8);
                }
 
              ip6srv->ip.payload_length =
@@ -2480,12 +2473,12 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
                    }
                  else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
                    {
-                     ip6srv->sr.protocol = IP_PROTOCOL_NONE;
+                     ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
                    }
                }
              else
                {
-                 ip6srv->sr.protocol = IP_PROTOCOL_NONE;
+                 ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
                }
 
              good_n++;
@@ -2655,7 +2648,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
                    }
 
                  next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
-                 if ((ip6->dst_address.as_u8[0] == 0xfe)
+                 if ((ip6->dst_address.as_u8[0] == 0xff)
                      && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
                    {
                      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
@@ -2676,7 +2669,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
                      == 6)
                    {
                      next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
-                     if ((ip6->dst_address.as_u8[0] == 0xfe)
+                     if ((ip6->dst_address.as_u8[0] == 0xff)
                          && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
                        {
                          vnet_buffer (b0)->sw_if_index[VLIB_TX] =
@@ -2875,7 +2868,7 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
                    }
 
                  next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
-                 if ((ip6->dst_address.as_u8[0] == 0xfe)
+                 if ((ip6->dst_address.as_u8[0] == 0xff)
                      && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
                    {
                      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
@@ -2897,7 +2890,7 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
                      == 6)
                    {
                      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
-                     if ((ip6->dst_address.as_u8[0] == 0xfe)
+                     if ((ip6->dst_address.as_u8[0] == 0xff)
                          && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
                        {
                          next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;