src/plugins/srv6-mobile/node.c

   1 /*
   2  * Copyright (c) 2019 Arrcus Inc and/or its affiliates.
   3  * Licensed under the Apache License, Version 2.0 (the "License");
   4  * you may not use this file except in compliance with the License.
   5  * You may obtain a copy of the License at:
   6  *
   7  *     http://www.apache.org/licenses/LICENSE-2.0
   8  *
   9  * Unless required by applicable law or agreed to in writing, software
  10  * distributed under the License is distributed on an "AS IS" BASIS,
  11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  12  * See the License for the specific language governing permissions and
  13  * limitations under the License.
  14  */
  15 #include <vlib/vlib.h>
  16 #include <vnet/vnet.h>
  17 #include <vppinfra/error.h>
  18 #include <vppinfra/hash.h>
  19 #include <srv6-mobile/mobile.h>
  20
  21 extern ip6_address_t sr_pr_encaps_src;
  22
  23 typedef struct
  24 {
  25   ip6_address_t src, dst;
  26   ip6_address_t sr_prefix;
  27   u16 sr_prefixlen;
  28   u32 teid;
  29 } srv6_end_rewrite_trace_t;
  30
  31 static u16 srh_tagfield[256] = {
  32   /* 0 */
  33   0x0,
  34   /* 1 : Echo Request */
  35   0x0004,
  36   /* 2 : Echo Reply */
  37   0x0008,
  38   /* 3 - 7 */
  39   0x0, 0x0, 0x0, 0x0, 0x0,
  40   /* 8 - 15 */
  41   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  42   /* 16 - 23 */
  43   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  44   /* 24 - 25 */
  45   0x0, 0x0,
  46   /* 26 : Error Indication */
  47   0x0002,
  48   /* 27 - 31 */
  49   0x0, 0x0, 0x0, 0x0, 0x0,
  50   /* 32 - 247 */
  51   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  52   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  53   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  54   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  55   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  56   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  57   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  58   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  59   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  60   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  61   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  62   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  63   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  64   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  65   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  66   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  67   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  68   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  69   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  70   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  71   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  72   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  73   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  74   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  75   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  76   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  77   0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  78   /* 248 - 253 */
  79   0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
  80   /* 254 : End Maker */
  81   0x0001,
  82   /* 255 : G_PDU */
  83   0x0
  84 };
  85
  86 static u8 *
  87 format_srv6_end_rewrite_trace (u8 * s, va_list * args)
  88 {
  89   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
  90   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
  91   srv6_end_rewrite_trace_t *t = va_arg (*args, srv6_end_rewrite_trace_t *);
  92
  93   return format (s, "SRv6-END-rewrite: src %U dst %U\n\tTEID: 0x%x",
  94                  format_ip4_address, &t->src, format_ip4_address, &t->dst,
  95                  clib_net_to_host_u32 (t->teid));
  96 }
  97
  98 static u8 *
  99 format_srv6_end_rewrite_trace6 (u8 * s, va_list * args)
 100 {
 101   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
 102   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
 103   srv6_end_rewrite_trace_t *t = va_arg (*args, srv6_end_rewrite_trace_t *);
 104
 105   return format (s,
 106                  "SRv6-END-rewrite: src %U dst %U\n\tTEID: 0x%x\n\tsr_prefix: %U/%d",
 107                  format_ip6_address, &t->src, format_ip6_address, &t->dst,
 108                  clib_net_to_host_u32 (t->teid), format_ip6_address,
 109                  &t->sr_prefix, t->sr_prefixlen);
 110 }
 111
 112 #define foreach_srv6_end_v4_error \
 113   _(M_GTP4_E_PACKETS, "srv6 End.M.GTP4.E packets") \
 114   _(M_GTP4_E_BAD_PACKETS, "srv6 End.M.GTP4.E bad packets")
 115
 116 #define foreach_srv6_t_v4_d_error \
 117   _(M_GTP4_D_PACKETS, "srv6 T.M.GTP4.D packets") \
 118   _(M_GTP4_D_BAD_PACKETS, "srv6 T.M.GTP4.D bad packets")
 119
 120 #define foreach_srv6_end_v6_e_error \
 121   _(M_GTP6_E_PACKETS, "srv6 End.M.GTP6.E packets") \
 122   _(M_GTP6_E_BAD_PACKETS, "srv6 End.M.GTP6.E bad packets")
 123
 124 #define foreach_srv6_end_v6_d_error \
 125   _(M_GTP6_D_PACKETS, "srv6 End.M.GTP6.D packets") \
 126   _(M_GTP6_D_BAD_PACKETS, "srv6 End.M.GTP6.D bad packets")
 127
 128 #define foreach_srv6_end_v6_d_di_error \
 129   _(M_GTP6_D_DI_PACKETS, "srv6 End.M.GTP6.D.DI packets") \
 130   _(M_GTP6_D_DI_BAD_PACKETS, "srv6 End.M.GTP6.D.DI bad packets")
 131
 132 typedef enum
 133 {
 134 #define _(sym,str) SRV6_END_ERROR_##sym,
 135   foreach_srv6_end_v4_error
 136 #undef _
 137     SRV6_END_N_V4_ERROR,
 138 } srv6_end_error_v4_t;
 139
 140 typedef enum
 141 {
 142 #define _(sym,str) SRV6_T_ERROR_##sym,
 143   foreach_srv6_t_v4_d_error
 144 #undef _
 145     SRV6_T_N_V4_D_ERROR,
 146 } srv6_t_error_v4_d_t;
 147
 148 typedef enum
 149 {
 150 #define _(sym,str) SRV6_END_ERROR_##sym,
 151   foreach_srv6_end_v6_e_error
 152 #undef _
 153     SRV6_END_N_V6_E_ERROR,
 154 } srv6_end_error_v6_e_t;
 155
 156 typedef enum
 157 {
 158 #define _(sym,str) SRV6_END_ERROR_##sym,
 159   foreach_srv6_end_v6_d_error
 160 #undef _
 161     SRV6_END_N_V6_D_ERROR,
 162 } srv6_end_error_v6_d_t;
 163
 164 typedef enum
 165 {
 166 #define _(sym,str) SRV6_END_ERROR_##sym,
 167   foreach_srv6_end_v6_d_di_error
 168 #undef _
 169     SRV6_END_N_V6_D_DI_ERROR,
 170 } srv6_end_error_v6_d_di_t;
 171
 172 static char *srv6_end_error_v4_strings[] = {
 173 #define _(sym,string) string,
 174   foreach_srv6_end_v4_error
 175 #undef _
 176 };
 177
 178 static char *srv6_t_error_v4_d_strings[] = {
 179 #define _(sym,string) string,
 180   foreach_srv6_t_v4_d_error
 181 #undef _
 182 };
 183
 184 static char *srv6_end_error_v6_e_strings[] = {
 185 #define _(sym,string) string,
 186   foreach_srv6_end_v6_e_error
 187 #undef _
 188 };
 189
 190 static char *srv6_end_error_v6_d_strings[] = {
 191 #define _(sym,string) string,
 192   foreach_srv6_end_v6_d_error
 193 #undef _
 194 };
 195
 196 static char *srv6_end_error_v6_d_di_strings[] = {
 197 #define _(sym,string) string,
 198   foreach_srv6_end_v6_d_di_error
 199 #undef _
 200 };
 201
 202 typedef enum
 203 {
 204   SRV6_END_M_GTP4_E_NEXT_DROP,
 205   SRV6_END_M_GTP4_E_NEXT_LOOKUP,
 206   SRV6_END_M_GTP4_E_N_NEXT,
 207 } srv6_end_m_gtp4_e_next_t;
 208
 209 typedef enum
 210 {
 211   SRV6_T_M_GTP4_D_NEXT_DROP,
 212   SRV6_T_M_GTP4_D_NEXT_LOOKUP,
 213   SRV6_T_M_GTP4_D_N_NEXT,
 214 } srv6_T_m_gtp4_d_next_t;
 215
 216 typedef enum
 217 {
 218   SRV6_END_M_GTP6_E_NEXT_DROP,
 219   SRV6_END_M_GTP6_E_NEXT_LOOKUP,
 220   SRV6_END_M_GTP6_E_N_NEXT,
 221 } srv6_end_m_gtp6_e_next_t;
 222
 223 typedef enum
 224 {
 225   SRV6_END_M_GTP6_D_NEXT_DROP,
 226   SRV6_END_M_GTP6_D_NEXT_LOOKUP,
 227   SRV6_END_M_GTP6_D_N_NEXT,
 228 } srv6_end_m_gtp6_d_next_t;
 229
 230 typedef enum
 231 {
 232   SRV6_END_M_GTP6_D_DI_NEXT_DROP,
 233   SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP,
 234   SRV6_END_M_GTP6_D_DI_N_NEXT,
 235 } srv6_end_m_gtp6_d_di_next_t;
 236
 237 static inline u16
 238 hash_uword_to_u16 (uword * key)
 239 {
 240   u16 *val;
 241   val = (u16 *) key;
 242 #if uword_bits == 64
 243   return val[0] ^ val[1] ^ val[2] ^ val[3];
 244 #else
 245   return val[0] ^ val[1];
 246 #endif
 247 }
 248
 249 static inline u8
 250 gtpu_type_get (u16 tag)
 251 {
 252   u16 val;
 253
 254   val = clib_net_to_host_u16 (tag);
 255   if (val & SRH_TAG_ECHO_REPLY)
 256     return GTPU_TYPE_ECHO_REPLY;
 257   else if (val & SRH_TAG_ECHO_REQUEST)
 258     return GTPU_TYPE_ECHO_REQUEST;
 259   else if (val & SRH_TAG_ERROR_INDICATION)
 260     return GTPU_TYPE_ERROR_INDICATION;
 261   else if (val & SRH_TAG_END_MARKER)
 262     return GTPU_TYPE_END_MARKER;
 263
 264   return GTPU_TYPE_GTPU;
 265 }
 266
 267 // Function for SRv6 GTP4.E function.
 268 VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 269                                   vlib_node_runtime_t * node,
 270                                   vlib_frame_t * frame)
 271 {
 272   srv6_end_main_v4_t *sm = &srv6_end_main_v4;
 273   ip6_sr_main_t *sm2 = &sr_main;
 274   u32 n_left_from, next_index, *from, *to_next;
 275   u32 thread_index = vm->thread_index;
 276
 277   u32 good_n = 0, bad_n = 0;
 278
 279   from = vlib_frame_vector_args (frame);
 280   n_left_from = frame->n_vectors;
 281   next_index = node->cached_next_index;
 282
 283   while (n_left_from > 0)
 284     {
 285       u32 n_left_to_next;
 286
 287       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 288
 289       while (n_left_from > 0 && n_left_to_next > 0)
 290         {
 291           u32 bi0;
 292           vlib_buffer_t *b0;
 293           ip6_sr_localsid_t *ls0;
 294           srv6_end_gtp4_param_t *ls_param;
 295
 296           ip6srv_combo_header_t *ip6srv0;
 297           ip6_address_t src0, dst0;
 298
 299           ip4_gtpu_header_t *hdr0 = NULL;
 300           uword len0;
 301
 302           u32 next0 = SRV6_END_M_GTP4_E_NEXT_LOOKUP;
 303
 304           // defaults
 305           bi0 = from[0];
 306           to_next[0] = bi0;
 307           from += 1;
 308           to_next += 1;
 309           n_left_from -= 1;
 310           n_left_to_next -= 1;
 311
 312           b0 = vlib_get_buffer (vm, bi0);
 313           ls0 =
 314             pool_elt_at_index (sm2->localsids,
 315                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 316
 317           ls_param = (srv6_end_gtp4_param_t *) ls0->plugin_mem;
 318
 319           ip6srv0 = vlib_buffer_get_current (b0);
 320           src0 = ip6srv0->ip.src_address;
 321           dst0 = ip6srv0->ip.dst_address;
 322
 323           len0 = vlib_buffer_length_in_chain (vm, b0);
 324
 325           if ((ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE
 326                && len0 <
 327                sizeof (ip6srv_combo_header_t) + ip6srv0->sr.length * 8)
 328               || (len0 < sizeof (ip6_header_t)))
 329             {
 330               next0 = SRV6_END_M_GTP4_E_NEXT_DROP;
 331
 332               bad_n++;
 333             }
 334           else
 335             {
 336               u8 gtpu_type = 0;
 337               u16 tag = 0;
 338               u32 teid = 0;
 339               u8 *teid8p = (u8 *) & teid;
 340               u8 qfi = 0;
 341               u16 seq = 0;
 342               u32 index;
 343               u32 offset, shift;
 344               u32 hdrlen = 0;
 345               uword key;
 346               u16 port;
 347               ip4_address_t dst4;
 348               void *p;
 349
 350               // we need to be sure there is enough space before
 351               // ip6srv0 header, there is some extra space
 352               // in the pre_data area for this kind of
 353               // logic
 354
 355               // jump over variable length data
 356               // not sure about the length
 357               if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE)
 358                 {
 359                   tag = ip6srv0->sr.tag;
 360
 361                   vlib_buffer_advance (b0,
 362                                        (word) sizeof (ip6srv_combo_header_t) +
 363                                        ip6srv0->sr.length * 8);
 364                 }
 365               else
 366                 {
 367                   vlib_buffer_advance (b0, (word) sizeof (ip6_header_t));
 368                 }
 369
 370               // get length of encapsulated IPv6 packet (the remaining part)
 371               p = vlib_buffer_get_current (b0);
 372
 373               len0 = vlib_buffer_length_in_chain (vm, b0);
 374
 375               offset = ls0->localsid_prefix_len / 8;
 376               shift = ls0->localsid_prefix_len % 8;
 377
 378               gtpu_type = gtpu_type_get (tag);
 379
 380               if (PREDICT_TRUE (shift == 0))
 381                 {
 382                   clib_memcpy_fast (&dst4.as_u8[0], &dst0.as_u8[offset], 4);
 383
 384                   qfi = dst0.as_u8[offset + 4];
 385
 386                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
 387                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
 388                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 389                     {
 390                       clib_memcpy_fast (&seq, &dst0.as_u8[offset + 5], 2);
 391                     }
 392                   else
 393                     {
 394                       clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 5], 4);
 395                     }
 396                 }
 397               else
 398                 {
 399                   u8 *sp;
 400
 401                   for (index = 0; index < 4; index++)
 402                     {
 403                       dst4.as_u8[index] = dst0.as_u8[offset + index] << shift;
 404                       dst4.as_u8[index] |=
 405                         dst0.as_u8[offset + index + 1] >> (8 - shift);
 406                     }
 407
 408                   qfi |= dst0.as_u8[offset + 4] << shift;
 409                   qfi |= dst0.as_u8[offset + 5] >> (8 - shift);
 410
 411                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
 412                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
 413                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 414                     {
 415                       sp = (u8 *) & seq;
 416                       for (index = 0; index < 2; index++)
 417                         {
 418                           sp[index] = dst0.as_u8[offset + 5 + index] << shift;
 419                           sp[index] |=
 420                             dst0.as_u8[offset + 6 + index] >> (8 - shift);
 421                         }
 422                     }
 423                   else
 424                     {
 425                       for (index = 0; index < 4; index++)
 426                         {
 427                           *teid8p = dst0.as_u8[offset + 5 + index] << shift;
 428                           *teid8p |=
 429                             dst0.as_u8[offset + 6 + index] >> (8 - shift);
 430                           teid8p++;
 431                         }
 432                     }
 433                 }
 434
 435               if (qfi)
 436                 {
 437                   hdrlen =
 438                     sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
 439                 }
 440               else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
 441                        || gtpu_type == GTPU_TYPE_ECHO_REPLY
 442                        || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 443                 {
 444                   hdrlen = sizeof (gtpu_exthdr_t);
 445                 }
 446               len0 += hdrlen;
 447
 448               hdrlen += sizeof (ip4_gtpu_header_t);
 449
 450               // IPv4 GTP-U header creation.
 451               vlib_buffer_advance (b0, -(word) hdrlen);
 452
 453               hdr0 = vlib_buffer_get_current (b0);
 454
 455               clib_memcpy_fast (hdr0, &sm->cache_hdr,
 456                                 sizeof (ip4_gtpu_header_t));
 457
 458               hdr0->ip4.dst_address.as_u32 = dst4.as_u32;
 459
 460               hdr0->gtpu.teid = teid;
 461               hdr0->gtpu.length = clib_host_to_net_u16 (len0);
 462
 463               hdr0->gtpu.type = gtpu_type;
 464
 465               if (qfi)
 466                 {
 467                   u8 type = 0;
 468                   gtpu_pdu_session_t *sess;
 469
 470                   hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
 471
 472                   hdr0->gtpu.ext->seq = 0;
 473
 474                   hdr0->gtpu.ext->npdu_num = 0;
 475                   hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
 476
 477                   type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
 478
 479                   qfi =
 480                     ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
 481                     ((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
 482
 483                   sess =
 484                     (gtpu_pdu_session_t *) (((char *) hdr0) +
 485                                             sizeof (ip4_gtpu_header_t) +
 486                                             sizeof (gtpu_exthdr_t));
 487                   sess->exthdrlen = 1;
 488                   sess->type = type;
 489                   sess->spare = 0;
 490                   sess->u.val = qfi;
 491                   sess->nextexthdr = 0;
 492                 }
 493
 494               if (gtpu_type == GTPU_TYPE_ECHO_REPLY
 495                   || gtpu_type == GTPU_TYPE_ECHO_REQUEST
 496                   || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 497                 {
 498                   hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG;
 499                   hdr0->gtpu.ext->seq = seq;
 500                   hdr0->gtpu.ext->npdu_num = 0;
 501                   hdr0->gtpu.ext->nextexthdr = 0;
 502                 }
 503
 504               offset = ls_param->v4src_position / 8;
 505               shift = ls_param->v4src_position % 8;
 506
 507               if (PREDICT_TRUE (shift == 0))
 508                 {
 509                   for (index = 0; index < 4; index++)
 510                     {
 511                       hdr0->ip4.src_address.as_u8[index] =
 512                         src0.as_u8[offset + index];
 513                     }
 514                 }
 515               else
 516                 {
 517                   for (index = 0; index < 4; index++)
 518                     {
 519                       hdr0->ip4.src_address.as_u8[index] =
 520                         src0.as_u8[offset + index] << shift;
 521                       hdr0->ip4.src_address.as_u8[index] |=
 522                         src0.as_u8[offset + index + 1] >> (8 - shift);
 523                     }
 524                 }
 525
 526               key = hash_memory (p, len0, 0);
 527               port = hash_uword_to_u16 (&key);
 528               hdr0->udp.src_port = port;
 529
 530               hdr0->udp.length = clib_host_to_net_u16 (len0 +
 531                                                        sizeof (udp_header_t) +
 532                                                        sizeof
 533                                                        (gtpu_header_t));
 534
 535               hdr0->ip4.length = clib_host_to_net_u16 (len0 +
 536                                                        sizeof
 537                                                        (ip4_gtpu_header_t));
 538
 539               hdr0->ip4.checksum = ip4_header_checksum (&hdr0->ip4);
 540
 541               good_n++;
 542
 543               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
 544                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
 545                 {
 546                   srv6_end_rewrite_trace_t *tr =
 547                     vlib_add_trace (vm, node, b0, sizeof (*tr));
 548                   clib_memcpy (tr->src.as_u8, hdr0->ip4.src_address.as_u8,
 549                                sizeof (tr->src.as_u8));
 550                   clib_memcpy (tr->dst.as_u8, hdr0->ip4.dst_address.as_u8,
 551                                sizeof (tr->dst.as_u8));
 552                   tr->teid = hdr0->gtpu.teid;
 553                 }
 554             }
 555
 556           vlib_increment_combined_counter
 557             (((next0 ==
 558                SRV6_END_M_GTP4_E_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
 559               &(sm2->sr_ls_valid_counters)), thread_index,
 560              ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
 561
 562           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 563                                            n_left_to_next, bi0, next0);
 564         }
 565
 566       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
 567     }
 568
 569   vlib_node_increment_counter (vm, sm->end_m_gtp4_e_node_index,
 570                                SRV6_END_ERROR_M_GTP4_E_BAD_PACKETS, bad_n);
 571
 572   vlib_node_increment_counter (vm, sm->end_m_gtp4_e_node_index,
 573                                SRV6_END_ERROR_M_GTP4_E_PACKETS, good_n);
 574
 575   return frame->n_vectors;
 576 }
 577
 578 // Function for SRv6 GTP4.D function.
 579 VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
 580                                 vlib_node_runtime_t * node,
 581                                 vlib_frame_t * frame)
 582 {
 583   srv6_t_main_v4_decap_t *sm = &srv6_t_main_v4_decap;
 584   ip6_sr_main_t *sm2 = &sr_main;
 585   u32 n_left_from, next_index, *from, *to_next;
 586
 587   u32 good_n = 0, bad_n = 0;
 588
 589   from = vlib_frame_vector_args (frame);
 590   n_left_from = frame->n_vectors;
 591   next_index = node->cached_next_index;
 592
 593   while (n_left_from > 0)
 594     {
 595       u32 n_left_to_next;
 596
 597       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 598
 599       while (n_left_from > 0 && n_left_to_next > 0)
 600         {
 601           u32 bi0;
 602           vlib_buffer_t *b0;
 603           ip6_sr_sl_t *sl0;
 604           srv6_end_gtp4_param_t *ls_param;
 605           ip4_header_t *ip4;
 606
 607           uword len0;
 608
 609           u32 next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP;
 610
 611           // defaults
 612           bi0 = from[0];
 613           to_next[0] = bi0;
 614           from += 1;
 615           to_next += 1;
 616           n_left_from -= 1;
 617           n_left_to_next -= 1;
 618
 619           b0 = vlib_get_buffer (vm, bi0);
 620
 621           sl0 =
 622             pool_elt_at_index (sm2->sid_lists,
 623                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 624
 625           ls_param = (srv6_end_gtp4_param_t *) sl0->plugin_mem;
 626
 627           len0 = vlib_buffer_length_in_chain (vm, b0);
 628
 629           ip4 = vlib_buffer_get_current (b0);
 630
 631           if (ip4->protocol != IP_PROTOCOL_UDP
 632               || len0 < sizeof (ip4_gtpu_header_t))
 633             {
 634               next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
 635
 636               bad_n++;
 637             }
 638           else
 639             {
 640               uword *p;
 641               ip6_sr_policy_t *sr_policy = NULL;
 642               ip6_sr_sl_t *sl = NULL;
 643               u32 *sl_index;
 644               u32 hdr_len;
 645
 646               ip4_gtpu_header_t *hdr;
 647               ip4_address_t src, dst;
 648               u8 *srcp, *dstp;
 649               ip6_header_t *encap = NULL;
 650               ip6_address_t seg;
 651               ip6_address_t src6;
 652               u8 gtpu_type;
 653               u32 teid;
 654               u8 *teidp;
 655               u8 qfi = 0;
 656               u8 *qfip = NULL;
 657               u16 seq = 0;
 658               u8 *seqp;
 659               u32 offset, shift, index;
 660               ip6srv_combo_header_t *ip6srv;
 661               gtpu_pdu_session_t *sess = NULL;
 662
 663               // Decap from GTP-U.
 664               hdr = (ip4_gtpu_header_t *) ip4;
 665
 666               hdr_len = sizeof (ip4_gtpu_header_t);
 667
 668               teid = hdr->gtpu.teid;
 669               teidp = (u8 *) & teid;
 670
 671               seqp = (u8 *) & seq;
 672
 673               gtpu_type = hdr->gtpu.type;
 674
 675               if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
 676                 {
 677                   // Extention header.
 678                   hdr_len += sizeof (gtpu_exthdr_t);
 679
 680                   seq = hdr->gtpu.ext->seq;
 681
 682                   if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 683                     {
 684                       // PDU Session Container.
 685                       sess =
 686                         (gtpu_pdu_session_t *) (((char *) hdr) + hdr_len);
 687                       qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
 688                       qfip = (u8 *) & qfi;
 689
 690                       hdr_len += sizeof (gtpu_pdu_session_t);
 691
 692                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
 693                         {
 694                           hdr_len += sizeof (gtpu_paging_policy_t);
 695                         }
 696                     }
 697                 }
 698
 699               src = hdr->ip4.src_address;
 700               srcp = (u8 *) & src;
 701
 702               dst = hdr->ip4.dst_address;
 703               dstp = (u8 *) & dst;
 704
 705               seg = ls_param->sr_prefix;
 706
 707               offset = ls_param->sr_prefixlen / 8;
 708               shift = ls_param->sr_prefixlen % 8;
 709
 710               if (PREDICT_TRUE (shift == 0))
 711                 {
 712                   clib_memcpy_fast (&seg.as_u8[offset], dstp, 4);
 713
 714                   if (qfip)
 715                     {
 716                       qfi =
 717                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
 718                         ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 719
 720                       if (sess->type)
 721                         {
 722                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
 723                         }
 724
 725                       seg.as_u8[offset + 4] = qfi;
 726                     }
 727
 728                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
 729                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
 730                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 731                     {
 732                       clib_memcpy_fast (&seg.as_u8[offset + 5], seqp, 2);
 733                     }
 734                   else
 735                     {
 736                       clib_memcpy_fast (&seg.as_u8[offset + 5], teidp, 4);
 737                     }
 738                 }
 739               else
 740                 {
 741                   for (index = 0; index < 4; index++)
 742                     {
 743                       seg.as_u8[offset + index] |= dstp[index] >> shift;
 744                       seg.as_u8[offset + index + 1] |=
 745                         dstp[index] << (8 - shift);
 746                     }
 747
 748                   if (qfip)
 749                     {
 750                       qfi =
 751                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
 752                         ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 753
 754                       if (sess->type)
 755                         {
 756                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
 757                         }
 758
 759                       seg.as_u8[offset + 4] |= qfi >> shift;
 760                       seg.as_u8[offset + 5] |= qfi << (8 - shift);
 761                     }
 762
 763                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
 764                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
 765                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 766                     {
 767                       for (index = 0; index < 2; index++)
 768                         {
 769                           seg.as_u8[offset + 5 + index] |=
 770                             seqp[index] >> shift;
 771                           seg.as_u8[offset + 6 + index] |=
 772                             seqp[index] << (8 - shift);
 773                         }
 774                     }
 775                   else
 776                     {
 777                       for (index = 0; index < 4; index++)
 778                         {
 779                           seg.as_u8[offset + index + 5] |=
 780                             teidp[index] >> shift;
 781                           seg.as_u8[offset + index + 6] |=
 782                             teidp[index] << (8 - shift);
 783                         }
 784                     }
 785                 }
 786
 787               src6 = ls_param->v6src_prefix;
 788
 789               offset = ls_param->v6src_prefixlen / 8;
 790               shift = ls_param->v6src_prefixlen % 8;
 791
 792               if (PREDICT_TRUE (shift == 0))
 793                 {
 794                   clib_memcpy_fast (&src6.as_u8[offset], srcp, 4);
 795                 }
 796               else
 797                 {
 798                   for (index = 0; index < 4; index++)
 799                     {
 800                       src6.as_u8[offset + index] |= srcp[offset] >> shift;
 801                       src6.as_u8[offset + index + 1] |=
 802                         srcp[offset] << (8 - shift);
 803                     }
 804                 }
 805
 806               vlib_buffer_advance (b0, (word) hdr_len);
 807
 808               // Encap to SRv6.
 809               if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
 810                 {
 811                   encap = vlib_buffer_get_current (b0);
 812                 }
 813
 814               len0 = vlib_buffer_length_in_chain (vm, b0);
 815
 816               p =
 817                 mhash_get (&sm2->sr_policies_index_hash,
 818                            &ls_param->sr_prefix);
 819               if (p)
 820                 {
 821                   sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
 822                 }
 823
 824               if (sr_policy)
 825                 {
 826                   vec_foreach (sl_index, sr_policy->segments_lists)
 827                   {
 828                     sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
 829                     if (sl != NULL)
 830                       break;
 831                   }
 832                 }
 833
 834               if (sl)
 835                 {
 836                   hdr_len = sizeof (ip6srv_combo_header_t);
 837                   hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
 838                   hdr_len += sizeof (ip6_address_t);
 839                 }
 840               else
 841                 {
 842                   hdr_len = sizeof (ip6_header_t);
 843
 844                   if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
 845                     {
 846                       hdr_len += sizeof (ip6_sr_header_t);
 847                       hdr_len += sizeof (ip6_address_t);
 848                     }
 849                 }
 850
 851               vlib_buffer_advance (b0, -(word) hdr_len);
 852               ip6srv = vlib_buffer_get_current (b0);
 853
 854               if (sl)
 855                 {
 856                   clib_memcpy_fast (ip6srv, sl->rewrite,
 857                                     vec_len (sl->rewrite));
 858
 859                   if (vec_len (sl->segments) > 1)
 860                     {
 861                       ip6srv->sr.tag =
 862                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 863
 864                       ip6srv->sr.segments_left += 1;
 865                       ip6srv->sr.last_entry += 1;
 866
 867                       ip6srv->sr.length += sizeof (ip6_address_t) / 8;
 868                       ip6srv->sr.segments[0] = seg;
 869
 870                       clib_memcpy_fast (&ip6srv->sr.segments[1],
 871                                         (u8 *) (sl->rewrite +
 872                                                 sizeof (ip6_header_t) +
 873                                                 sizeof (ip6_sr_header_t)),
 874                                         vec_len (sl->segments) *
 875                                         sizeof (ip6_address_t));
 876                     }
 877                   else
 878                     {
 879                       ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
 880
 881                       ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
 882
 883                       ip6srv->sr.segments_left = 1;
 884                       ip6srv->sr.last_entry = 0;
 885
 886                       ip6srv->sr.length =
 887                         ((sizeof (ip6_sr_header_t) +
 888                           sizeof (ip6_address_t)) / 8) - 1;
 889                       ip6srv->sr.flags = 0;
 890
 891                       ip6srv->sr.tag =
 892                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 893
 894                       ip6srv->sr.segments[0] = seg;
 895                       if (vec_len (sl->segments))
 896                         {
 897                           ip6srv->sr.segments[1] = sl->segments[0];
 898                           ip6srv->sr.length += sizeof (ip6_address_t) / 8;
 899                           ip6srv->sr.last_entry++;
 900                         }
 901                     }
 902
 903                   if (PREDICT_TRUE (encap != NULL))
 904                     {
 905                       if (ls_param->nhtype == SRV6_NHTYPE_NONE)
 906                         {
 907                           if ((clib_net_to_host_u32
 908                                (encap->ip_version_traffic_class_and_flow_label)
 909                                >> 28) == 6)
 910                             ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
 911                           else
 912                             ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
 913                         }
 914                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
 915                         {
 916                           ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
 917                           if ((clib_net_to_host_u32
 918                                (encap->ip_version_traffic_class_and_flow_label)
 919                                >> 28) != 4)
 920                             {
 921                               // Bad encap packet.
 922                               next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
 923                               bad_n++;
 924                               goto DONE;
 925                             }
 926                         }
 927                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
 928                         {
 929                           ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
 930                           if ((clib_net_to_host_u32
 931                                (encap->ip_version_traffic_class_and_flow_label)
 932                                >> 28) != 6)
 933                             {
 934                               // Bad encap packet.
 935                               next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
 936                               bad_n++;
 937                               goto DONE;
 938                             }
 939                         }
 940                       else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
 941                         {
 942                           ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
 943                         }
 944                     }
 945                   else
 946                     {
 947                       ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
 948                     }
 949                 }
 950               else
 951                 {
 952                   clib_memcpy_fast (ip6srv, &sm->cache_hdr,
 953                                     sizeof (ip6_header_t));
 954
 955                   ip6srv->ip.dst_address = seg;
 956
 957                   if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
 958                     {
 959                       ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
 960
 961                       ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
 962
 963                       ip6srv->sr.tag =
 964                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 965
 966                       ip6srv->sr.segments_left = 0;
 967                       ip6srv->sr.last_entry = 0;
 968
 969                       ip6srv->sr.length = sizeof (ip6_address_t) / 8;
 970                       ip6srv->sr.segments[0] = seg;
 971                     }
 972                   else
 973                     {
 974                       if (ls_param->nhtype == SRV6_NHTYPE_NONE)
 975                         {
 976                           if ((clib_net_to_host_u32
 977                                (encap->ip_version_traffic_class_and_flow_label)
 978                                >> 28) == 6)
 979                             ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
 980                           else
 981                             ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
 982                         }
 983                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
 984                         {
 985                           ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
 986                           if ((clib_net_to_host_u32
 987                                (encap->ip_version_traffic_class_and_flow_label)
 988                                >> 28) != 4)
 989                             {
 990                               // Bad encap packet.
 991                               next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
 992                               bad_n++;
 993                               goto DONE;
 994                             }
 995                         }
 996                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
 997                         {
 998                           ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
 999                           if ((clib_net_to_host_u32
1000                                (encap->ip_version_traffic_class_and_flow_label)
1001                                >> 28) != 6)
1002                             {
1003                               // Bad encap packet.
1004                               next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
1005                               bad_n++;
1006                               goto DONE;
1007                             }
1008                         }
1009                       else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1010                         {
1011                           ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
1012                         }
1013                     }
1014                 }
1015
1016               ip6srv->ip.src_address = src6;
1017
1018               ip6srv->ip.payload_length =
1019                 clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
1020
1021               good_n++;
1022
1023               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1024                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1025                 {
1026                   srv6_end_rewrite_trace_t *tr =
1027                     vlib_add_trace (vm, node, b0, sizeof (*tr));
1028                   clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
1029                                sizeof (tr->src.as_u8));
1030                   clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
1031                                sizeof (tr->dst.as_u8));
1032                 }
1033             }
1034
1035         DONE:
1036           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1037                                            n_left_to_next, bi0, next0);
1038         }
1039
1040       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1041     }
1042
1043   vlib_node_increment_counter (vm, sm->t_m_gtp4_d_node_index,
1044                                SRV6_T_ERROR_M_GTP4_D_BAD_PACKETS, bad_n);
1045
1046   vlib_node_increment_counter (vm, sm->t_m_gtp4_d_node_index,
1047                                SRV6_T_ERROR_M_GTP4_D_PACKETS, good_n);
1048
1049   return frame->n_vectors;
1050 }
1051
1052 VLIB_REGISTER_NODE (srv6_end_m_gtp4_e) =
1053 {
1054   .name = "srv6-end-m-gtp4-e",.vector_size = sizeof (u32),.format_trace =
1055     format_srv6_end_rewrite_trace,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
1056     ARRAY_LEN (srv6_end_error_v4_strings),.error_strings =
1057     srv6_end_error_v4_strings,.n_next_nodes =
1058     SRV6_END_M_GTP4_E_N_NEXT,.next_nodes =
1059   {
1060   [SRV6_END_M_GTP4_E_NEXT_DROP] = "error-drop",
1061       [SRV6_END_M_GTP4_E_NEXT_LOOKUP] = "ip4-lookup",}
1062 ,};
1063
1064 VLIB_REGISTER_NODE (srv6_t_m_gtp4_d) =
1065 {
1066   .name = "srv6-t-m-gtp4-d",.vector_size = sizeof (u32),.format_trace =
1067     format_srv6_end_rewrite_trace,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
1068     ARRAY_LEN (srv6_t_error_v4_d_strings),.error_strings =
1069     srv6_t_error_v4_d_strings,.n_next_nodes =
1070     SRV6_T_M_GTP4_D_N_NEXT,.next_nodes =
1071   {
1072   [SRV6_T_M_GTP4_D_NEXT_DROP] = "error-drop",
1073       [SRV6_T_M_GTP4_D_NEXT_LOOKUP] = "ip6-lookup",}
1074 ,};
1075
1076 // Function for SRv6 GTP6.E function
1077 VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
1078                                   vlib_node_runtime_t * node,
1079                                   vlib_frame_t * frame)
1080 {
1081   srv6_end_main_v6_t *sm = &srv6_end_main_v6;
1082   ip6_sr_main_t *sm2 = &sr_main;
1083   u32 n_left_from, next_index, *from, *to_next;
1084   u32 thread_index = vm->thread_index;
1085
1086   u32 good_n = 0, bad_n = 0;
1087
1088   from = vlib_frame_vector_args (frame);
1089   n_left_from = frame->n_vectors;
1090   next_index = node->cached_next_index;
1091
1092   while (n_left_from > 0)
1093     {
1094       u32 n_left_to_next;
1095
1096       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1097
1098       while (n_left_from > 0 && n_left_to_next > 0)
1099         {
1100           u32 bi0;
1101           vlib_buffer_t *b0;
1102           ip6_sr_localsid_t *ls0;
1103
1104           ip6srv_combo_header_t *ip6srv0;
1105           ip6_address_t dst0, src0, seg0;
1106
1107           ip6_gtpu_header_t *hdr0 = NULL;
1108           uword len0;
1109           uword key;
1110           u16 port;
1111           u16 tag;
1112           void *p;
1113
1114           u32 next0 = SRV6_END_M_GTP6_E_NEXT_LOOKUP;
1115
1116           // defaults
1117           bi0 = from[0];
1118           to_next[0] = bi0;
1119           from += 1;
1120           to_next += 1;
1121           n_left_from -= 1;
1122           n_left_to_next -= 1;
1123
1124           b0 = vlib_get_buffer (vm, bi0);
1125           ls0 =
1126             pool_elt_at_index (sm2->localsids,
1127                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1128
1129           ip6srv0 = vlib_buffer_get_current (b0);
1130           dst0 = ip6srv0->ip.dst_address;
1131           src0 = ip6srv0->ip.src_address;
1132           seg0 = ip6srv0->sr.segments[0];
1133
1134           tag = ip6srv0->sr.tag;
1135
1136           len0 = vlib_buffer_length_in_chain (vm, b0);
1137
1138           if ((ip6srv0->ip.protocol != IPPROTO_IPV6_ROUTE)
1139               || (len0 <
1140                   sizeof (ip6srv_combo_header_t) + 8 * ip6srv0->sr.length))
1141             {
1142               next0 = SRV6_END_M_GTP6_E_NEXT_DROP;
1143
1144               bad_n++;
1145             }
1146           else
1147             {
1148               // we need to be sure there is enough space before
1149               // ip6srv0 header, there is some extra space
1150               // in the pre_data area for this kind of
1151               // logic
1152
1153               // jump over variable length data
1154               // not sure about the length
1155               vlib_buffer_advance (b0, (word) sizeof (ip6srv_combo_header_t) +
1156                                    ip6srv0->sr.length * 8);
1157
1158               // get length of encapsulated IPv6 packet (the remaining part)
1159               p = vlib_buffer_get_current (b0);
1160
1161               len0 = vlib_buffer_length_in_chain (vm, b0);
1162
1163               u32 teid = 0;
1164               u8 *teid8p = (u8 *) & teid;
1165               u8 qfi = 0;
1166               u16 seq = 0;
1167               u8 gtpu_type = 0;
1168               u16 index;
1169               u16 offset, shift;
1170               u32 hdrlen = 0;
1171
1172               index = ls0->localsid_prefix_len;
1173               index += 8;
1174               offset = index / 8;
1175               shift = index % 8;
1176
1177               gtpu_type = gtpu_type_get (tag);
1178
1179               if (PREDICT_TRUE (shift == 0))
1180                 {
1181                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
1182                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
1183                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1184                     {
1185                       clib_memcpy_fast (&seq, &dst0.as_u8[offset], 2);
1186                     }
1187                   else
1188                     {
1189                       clib_memcpy_fast (teid8p, &dst0.as_u8[offset], 4);
1190                     }
1191
1192                   qfi = dst0.as_u8[offset + 4];
1193                 }
1194               else
1195                 {
1196                   u8 *sp;
1197
1198                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
1199                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
1200                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1201                     {
1202                       sp = (u8 *) & seq;
1203                       for (index = 0; index < 2; index++)
1204                         {
1205                           sp[index] = dst0.as_u8[offset + index] << shift;
1206                           sp[index] |=
1207                             dst0.as_u8[offset + index + 1] >> (8 - shift);
1208                         }
1209                     }
1210                   else
1211                     {
1212                       for (index = 0; index < 4; index++)
1213                         {
1214                           *teid8p = dst0.as_u8[offset + index] << shift;
1215                           *teid8p |=
1216                             dst0.as_u8[offset + index + 1] >> (8 - shift);
1217                           teid8p++;
1218                         }
1219                     }
1220
1221                   qfi |= dst0.as_u8[offset + 4] << shift;
1222                   qfi |= dst0.as_u8[offset + 5] >> (8 - shift);
1223                 }
1224
1225               if (qfi)
1226                 {
1227                   hdrlen =
1228                     sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
1229                 }
1230               else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
1231                        || gtpu_type == GTPU_TYPE_ECHO_REPLY
1232                        || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1233                 {
1234                   hdrlen = sizeof (gtpu_exthdr_t);
1235                 }
1236
1237               len0 += hdrlen;
1238               hdrlen += sizeof (ip6_gtpu_header_t);
1239
1240               vlib_buffer_advance (b0, -(word) hdrlen);
1241
1242               hdr0 = vlib_buffer_get_current (b0);
1243
1244               clib_memcpy_fast (hdr0, &sm->cache_hdr,
1245                                 sizeof (ip6_gtpu_header_t));
1246
1247               hdr0->gtpu.teid = teid;
1248               hdr0->gtpu.length = clib_host_to_net_u16 (len0);
1249
1250               hdr0->gtpu.type = gtpu_type;
1251
1252               if (qfi)
1253                 {
1254                   u8 type = 0;
1255                   gtpu_pdu_session_t *sess;
1256
1257                   hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
1258
1259                   hdr0->gtpu.ext->seq = 0;
1260                   hdr0->gtpu.ext->npdu_num = 0;
1261                   hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
1262
1263                   type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
1264
1265                   qfi =
1266                     ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
1267                     ((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
1268
1269                   sess =
1270                     (gtpu_pdu_session_t *) (((char *) hdr0) +
1271                                             sizeof (ip6_gtpu_header_t) +
1272                                             sizeof (gtpu_exthdr_t));
1273                   sess->exthdrlen = 1;
1274                   sess->type = type;
1275                   sess->spare = 0;
1276                   sess->u.val = qfi;
1277                   sess->nextexthdr = 0;
1278                 }
1279
1280               if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
1281                   || gtpu_type == GTPU_TYPE_ECHO_REPLY
1282                   || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1283                 {
1284                   hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG;
1285                   hdr0->gtpu.ext->seq = seq;
1286                   hdr0->gtpu.ext->npdu_num = 0;
1287                   hdr0->gtpu.ext->nextexthdr = 0;
1288                 }
1289
1290               hdr0->udp.length = clib_host_to_net_u16 (len0 +
1291                                                        sizeof (udp_header_t) +
1292                                                        sizeof
1293                                                        (gtpu_header_t));
1294
1295               clib_memcpy_fast (hdr0->ip6.src_address.as_u8, src0.as_u8,
1296                                 sizeof (ip6_address_t));
1297               clib_memcpy_fast (hdr0->ip6.dst_address.as_u8, &seg0.as_u8,
1298                                 sizeof (ip6_address_t));
1299
1300               hdr0->ip6.payload_length = clib_host_to_net_u16 (len0 +
1301                                                                sizeof
1302                                                                (udp_header_t)
1303                                                                +
1304                                                                sizeof
1305                                                                (gtpu_header_t));
1306
1307               // UDP source port.
1308               key = hash_memory (p, len0, 0);
1309               port = hash_uword_to_u16 (&key);
1310               hdr0->udp.src_port = port;
1311
1312               good_n++;
1313
1314               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1315                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1316                 {
1317                   srv6_end_rewrite_trace_t *tr =
1318                     vlib_add_trace (vm, node, b0, sizeof (*tr));
1319                   clib_memcpy (tr->src.as_u8, hdr0->ip6.src_address.as_u8,
1320                                sizeof (ip6_address_t));
1321                   clib_memcpy (tr->dst.as_u8, hdr0->ip6.dst_address.as_u8,
1322                                sizeof (ip6_address_t));
1323                   tr->teid = hdr0->gtpu.teid;
1324                 }
1325             }
1326
1327           vlib_increment_combined_counter
1328             (((next0 ==
1329                SRV6_END_M_GTP6_E_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
1330               &(sm2->sr_ls_valid_counters)), thread_index,
1331              ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
1332
1333           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1334                                            n_left_to_next, bi0, next0);
1335         }
1336
1337       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1338     }
1339
1340   vlib_node_increment_counter (vm, sm->end_m_gtp6_e_node_index,
1341                                SRV6_END_ERROR_M_GTP6_E_BAD_PACKETS, bad_n);
1342
1343   vlib_node_increment_counter (vm, sm->end_m_gtp6_e_node_index,
1344                                SRV6_END_ERROR_M_GTP6_E_PACKETS, good_n);
1345
1346   return frame->n_vectors;
1347 }
1348
1349 // Function for SRv6 GTP6.D function
1350 VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
1351                                   vlib_node_runtime_t * node,
1352                                   vlib_frame_t * frame)
1353 {
1354   srv6_end_main_v6_decap_t *sm = &srv6_end_main_v6_decap;
1355   ip6_sr_main_t *sm2 = &sr_main;
1356   u32 n_left_from, next_index, *from, *to_next;
1357   u32 thread_index = vm->thread_index;
1358
1359   u32 good_n = 0, bad_n = 0;
1360
1361   from = vlib_frame_vector_args (frame);
1362   n_left_from = frame->n_vectors;
1363   next_index = node->cached_next_index;
1364
1365   while (n_left_from > 0)
1366     {
1367       u32 n_left_to_next;
1368
1369       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1370
1371       while (n_left_from > 0 && n_left_to_next > 0)
1372         {
1373           u32 bi0;
1374           vlib_buffer_t *b0;
1375           ip6_sr_localsid_t *ls0;
1376           srv6_end_gtp6_param_t *ls_param;
1377
1378           ip6_gtpu_header_t *hdr0 = NULL;
1379           uword len0;
1380
1381           ip6_address_t seg0, src0;
1382           u32 teid = 0;
1383           u8 *teidp;
1384           u8 gtpu_type = 0;
1385           u8 qfi;
1386           u8 *qfip = NULL;
1387           u16 seq = 0;
1388           u8 *seqp;
1389           u32 offset, shift;
1390           u32 hdrlen;
1391           ip6_header_t *encap = NULL;
1392           gtpu_pdu_session_t *sess = NULL;
1393
1394           u32 next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP;
1395
1396           // defaults
1397           bi0 = from[0];
1398           to_next[0] = bi0;
1399           from += 1;
1400           to_next += 1;
1401           n_left_from -= 1;
1402           n_left_to_next -= 1;
1403
1404           b0 = vlib_get_buffer (vm, bi0);
1405           ls0 =
1406             pool_elt_at_index (sm2->localsids,
1407                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1408
1409           ls_param = (srv6_end_gtp6_param_t *) ls0->plugin_mem;
1410
1411           hdr0 = vlib_buffer_get_current (b0);
1412
1413           hdrlen = sizeof (ip6_gtpu_header_t);
1414
1415           len0 = vlib_buffer_length_in_chain (vm, b0);
1416
1417           if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP)
1418               || (hdr0->udp.dst_port !=
1419                   clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT))
1420               || (len0 < sizeof (ip6_gtpu_header_t)))
1421             {
1422               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1423
1424               bad_n++;
1425             }
1426           else
1427             {
1428               seg0 = ls_param->sr_prefix;
1429               src0 = hdr0->ip6.src_address;
1430
1431               gtpu_type = hdr0->gtpu.type;
1432
1433               teid = hdr0->gtpu.teid;
1434               teidp = (u8 *) & teid;
1435
1436               seqp = (u8 *) & seq;
1437
1438               if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
1439                 {
1440                   // Extention header.
1441                   hdrlen += sizeof (gtpu_exthdr_t);
1442
1443                   seq = hdr0->gtpu.ext->seq;
1444
1445                   if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
1446                     {
1447                       // PDU Session Container.
1448                       sess =
1449                         (gtpu_pdu_session_t *) (((char *) hdr0) +
1450                                                 sizeof (ip6_gtpu_header_t) +
1451                                                 sizeof (gtpu_exthdr_t));
1452                       qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
1453                       qfip = (u8 *) & qfi;
1454
1455                       hdrlen += sizeof (gtpu_pdu_session_t);
1456
1457                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
1458                         {
1459                           hdrlen += sizeof (gtpu_paging_policy_t);
1460                         }
1461                     }
1462                 }
1463
1464               offset = ls_param->sr_prefixlen / 8;
1465               shift = ls_param->sr_prefixlen % 8;
1466
1467               offset += 1;
1468               if (PREDICT_TRUE (shift == 0))
1469                 {
1470                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
1471                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
1472                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1473                     {
1474                       clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
1475                     }
1476                   else
1477                     {
1478                       clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
1479                     }
1480
1481                   if (qfip)
1482                     {
1483                       qfi =
1484                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
1485                         ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
1486
1487                       if (sess->type)
1488                         {
1489                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
1490                         }
1491
1492                       seg0.as_u8[offset + 4] = qfi;
1493                     }
1494                 }
1495               else
1496                 {
1497                   int idx;
1498
1499                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
1500                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
1501                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1502                     {
1503                       for (idx = 0; idx < 2; idx++)
1504                         {
1505                           seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
1506                           seg0.as_u8[offset + idx + 1] |=
1507                             seqp[idx] << (8 - shift);
1508                         }
1509                     }
1510                   else
1511                     {
1512                       for (idx = 0; idx < 4; idx++)
1513                         {
1514                           seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
1515                           seg0.as_u8[offset + idx + 1] |=
1516                             teidp[idx] << (8 - shift);
1517                         }
1518                     }
1519
1520                   if (qfip)
1521                     {
1522                       qfi =
1523                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
1524                         ((qfi & ~GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
1525
1526                       if (sess->type)
1527                         {
1528                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
1529                         }
1530
1531                       seg0.as_u8[offset + 4] |= qfi >> shift;
1532                       seg0.as_u8[offset + 5] |= qfi << (8 - shift);
1533                     }
1534                 }
1535
1536               // jump over variable length data
1537               vlib_buffer_advance (b0, (word) hdrlen);
1538
1539               // get length of encapsulated IPv6 packet (the remaining part)
1540               len0 = vlib_buffer_length_in_chain (vm, b0);
1541
1542               if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
1543                 {
1544                   encap = vlib_buffer_get_current (b0);
1545                 }
1546
1547               uword *p;
1548               ip6srv_combo_header_t *ip6srv;
1549               ip6_sr_policy_t *sr_policy = NULL;
1550               ip6_sr_sl_t *sl = NULL;
1551               u32 *sl_index;
1552               u32 hdr_len;
1553
1554               p =
1555                 mhash_get (&sm2->sr_policies_index_hash,
1556                            &ls_param->sr_prefix);
1557               if (p)
1558                 {
1559                   sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
1560                 }
1561
1562               if (sr_policy)
1563                 {
1564                   vec_foreach (sl_index, sr_policy->segments_lists)
1565                   {
1566                     sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
1567                     if (sl != NULL)
1568                       break;
1569                   }
1570                 }
1571
1572               if (sl)
1573                 {
1574                   hdr_len = sizeof (ip6srv_combo_header_t);
1575                   hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
1576                   hdr_len += sizeof (ip6_address_t);
1577                 }
1578               else
1579                 {
1580                   hdr_len = sizeof (ip6_header_t);
1581                   if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
1582                     {
1583                       hdr_len += sizeof (ip6_sr_header_t);
1584                       hdr_len += sizeof (ip6_address_t);
1585                     }
1586                 }
1587
1588               // jump back to data[0] or pre_data if required
1589               vlib_buffer_advance (b0, -(word) hdr_len);
1590
1591               ip6srv = vlib_buffer_get_current (b0);
1592
1593               if (sl)
1594                 {
1595                   clib_memcpy_fast (ip6srv, sl->rewrite,
1596                                     vec_len (sl->rewrite));
1597
1598                   if (vec_len (sl->segments) > 1)
1599                     {
1600                       ip6srv->ip.src_address = src0;
1601
1602                       ip6srv->sr.tag =
1603                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1604
1605                       ip6srv->sr.segments_left += 1;
1606                       ip6srv->sr.last_entry += 1;
1607
1608                       ip6srv->sr.length += sizeof (ip6_address_t) / 8;
1609                       ip6srv->sr.segments[0] = seg0;
1610
1611                       clib_memcpy_fast (&ip6srv->sr.segments[1],
1612                                         (u8 *) (sl->rewrite +
1613                                                 sizeof (ip6_header_t) +
1614                                                 sizeof (ip6_sr_header_t)),
1615                                         vec_len (sl->segments) *
1616                                         sizeof (ip6_address_t));
1617                     }
1618                   else
1619                     {
1620                       ip6srv->ip.src_address = src0;
1621                       ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
1622
1623                       ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
1624                       ip6srv->sr.segments_left = 1;
1625                       ip6srv->sr.last_entry = 0;
1626                       ip6srv->sr.length =
1627                         ((sizeof (ip6_sr_header_t) +
1628                           sizeof (ip6_address_t)) / 8) - 1;
1629                       ip6srv->sr.flags = 0;
1630
1631                       ip6srv->sr.tag =
1632                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1633
1634                       ip6srv->sr.segments[0] = seg0;
1635
1636                       if (vec_len (sl->segments))
1637                         {
1638                           ip6srv->sr.segments[1] = sl->segments[0];
1639                           ip6srv->sr.last_entry++;
1640                           ip6srv->sr.length += sizeof (ip6_address_t) / 8;
1641                         }
1642                     }
1643
1644                   if (PREDICT_TRUE (encap != NULL))
1645                     {
1646                       if (ls_param->nhtype == SRV6_NHTYPE_NONE)
1647                         {
1648                           if ((clib_net_to_host_u32
1649                                (encap->ip_version_traffic_class_and_flow_label)
1650                                >> 28) == 6)
1651                             ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1652                           else
1653                             ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1654                         }
1655                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
1656                         {
1657                           ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1658                           if ((clib_net_to_host_u32
1659                                (encap->ip_version_traffic_class_and_flow_label)
1660                                >> 28) != 4)
1661                             {
1662                               // Bad encap packet.
1663                               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1664                               bad_n++;
1665                               goto DONE;
1666                             }
1667                         }
1668                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
1669                         {
1670                           ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1671                           if ((clib_net_to_host_u32
1672                                (encap->ip_version_traffic_class_and_flow_label)
1673                                >> 28) != 6)
1674                             {
1675                               // Bad encap packet.
1676                               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1677                               bad_n++;
1678                               goto DONE;
1679                             }
1680                         }
1681                       else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1682                         {
1683                           ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
1684                         }
1685                     }
1686                   else
1687                     {
1688                       ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
1689                     }
1690                 }
1691               else
1692                 {
1693                   clib_memcpy_fast (ip6srv, &sm->cache_hdr,
1694                                     sizeof (ip6_header_t));
1695
1696                   ip6srv->ip.src_address = src0;
1697                   ip6srv->ip.dst_address = seg0;
1698
1699                   if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
1700                     {
1701                       ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
1702
1703                       ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
1704
1705                       ip6srv->sr.tag =
1706                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1707
1708                       ip6srv->sr.segments_left = 0;
1709                       ip6srv->sr.last_entry = 0;
1710
1711                       ip6srv->sr.length = sizeof (ip6_address_t) / 8;
1712                       ip6srv->sr.segments[0] = seg0;
1713                     }
1714                   else
1715                     {
1716                       if (ls_param->nhtype == SRV6_NHTYPE_NONE)
1717                         {
1718                           if ((clib_net_to_host_u32
1719                                (encap->ip_version_traffic_class_and_flow_label)
1720                                >> 28) != 6)
1721                             ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
1722                         }
1723                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
1724                         {
1725                           ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
1726                           if ((clib_net_to_host_u32
1727                                (encap->ip_version_traffic_class_and_flow_label)
1728                                >> 28) != 4)
1729                             {
1730                               // Bad encap packet.
1731                               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1732                               bad_n++;
1733                               goto DONE;
1734                             }
1735                         }
1736                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
1737                         {
1738                           ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
1739                           if ((clib_net_to_host_u32
1740                                (encap->ip_version_traffic_class_and_flow_label)
1741                                >> 28) != 6)
1742                             {
1743                               // Bad encap packet.
1744                               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1745                               bad_n++;
1746                               goto DONE;
1747                             }
1748                         }
1749                       else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1750                         {
1751                           ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
1752                         }
1753                     }
1754                 }
1755
1756               ip6srv->ip.payload_length =
1757                 clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
1758
1759               good_n++;
1760
1761               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1762                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1763                 {
1764                   srv6_end_rewrite_trace_t *tr =
1765                     vlib_add_trace (vm, node, b0, sizeof (*tr));
1766                   clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
1767                                sizeof (ip6_address_t));
1768                   clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
1769                                sizeof (ip6_address_t));
1770                   tr->teid = teid;
1771                   clib_memcpy (tr->sr_prefix.as_u8, ls_param->sr_prefix.as_u8,
1772                                sizeof (ip6_address_t));
1773                   tr->sr_prefixlen = ls_param->sr_prefixlen;
1774                 }
1775             }
1776
1777         DONE:
1778           vlib_increment_combined_counter
1779             (((next0 ==
1780                SRV6_END_M_GTP6_D_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
1781               &(sm2->sr_ls_valid_counters)), thread_index,
1782              ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
1783
1784           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1785                                            n_left_to_next, bi0, next0);
1786         }
1787
1788       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1789     }
1790
1791   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_node_index,
1792                                SRV6_END_ERROR_M_GTP6_D_BAD_PACKETS, bad_n);
1793
1794   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_node_index,
1795                                SRV6_END_ERROR_M_GTP6_D_PACKETS, good_n);
1796
1797   return frame->n_vectors;
1798 }
1799
1800 // Function for SRv6 GTP6.D.DI function
1801 VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
1802                                      vlib_node_runtime_t * node,
1803                                      vlib_frame_t * frame)
1804 {
1805   srv6_end_main_v6_decap_di_t *sm = &srv6_end_main_v6_decap_di;
1806   ip6_sr_main_t *sm2 = &sr_main;
1807   u32 n_left_from, next_index, *from, *to_next;
1808   u32 thread_index = vm->thread_index;
1809   srv6_end_gtp6_param_t *ls_param;
1810
1811   u32 good_n = 0, bad_n = 0;
1812
1813   from = vlib_frame_vector_args (frame);
1814   n_left_from = frame->n_vectors;
1815   next_index = node->cached_next_index;
1816
1817   while (n_left_from > 0)
1818     {
1819       u32 n_left_to_next;
1820
1821       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1822
1823       while (n_left_from > 0 && n_left_to_next > 0)
1824         {
1825           u32 bi0;
1826           vlib_buffer_t *b0;
1827           ip6_sr_localsid_t *ls0;
1828
1829           ip6_gtpu_header_t *hdr0 = NULL;
1830           uword len0;
1831
1832           ip6_address_t dst0;
1833           ip6_address_t src0;
1834           ip6_address_t seg0;
1835           u32 teid = 0;
1836           u8 *teidp;
1837           u8 gtpu_type = 0;
1838           u8 qfi = 0;
1839           u8 *qfip = NULL;
1840           u16 seq = 0;
1841           u8 *seqp;
1842           u32 offset, shift;
1843           u32 hdrlen;
1844           ip6_header_t *encap = NULL;
1845           gtpu_pdu_session_t *sess;
1846
1847           u32 next0 = SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP;
1848
1849           // defaults
1850           bi0 = from[0];
1851           to_next[0] = bi0;
1852           from += 1;
1853           to_next += 1;
1854           n_left_from -= 1;
1855           n_left_to_next -= 1;
1856
1857           b0 = vlib_get_buffer (vm, bi0);
1858           ls0 =
1859             pool_elt_at_index (sm2->localsids,
1860                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1861
1862           ls_param = (srv6_end_gtp6_param_t *) ls0->plugin_mem;
1863
1864           hdr0 = vlib_buffer_get_current (b0);
1865
1866           hdrlen = sizeof (ip6_gtpu_header_t);
1867
1868           len0 = vlib_buffer_length_in_chain (vm, b0);
1869
1870           if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP)
1871               || (hdr0->udp.dst_port !=
1872                   clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT))
1873               || (len0 < sizeof (ip6_gtpu_header_t)))
1874             {
1875               next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
1876
1877               bad_n++;
1878             }
1879           else
1880             {
1881               dst0 = hdr0->ip6.dst_address;
1882               src0 = hdr0->ip6.src_address;
1883
1884               gtpu_type = hdr0->gtpu.type;
1885
1886               seg0 = ls_param->sr_prefix;
1887               teid = hdr0->gtpu.teid;
1888               teidp = (u8 *) & teid;
1889
1890               seqp = (u8 *) & seq;
1891
1892               if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
1893                 {
1894                   // Extention header.
1895                   hdrlen += sizeof (gtpu_exthdr_t);
1896
1897                   seq = hdr0->gtpu.ext->seq;
1898
1899                   if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
1900                     {
1901                       // PDU Session Container.
1902                       sess =
1903                         (gtpu_pdu_session_t *) (((char *) hdr0) + hdrlen);
1904                       qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
1905                       qfip = &qfi;
1906
1907                       hdrlen += sizeof (gtpu_pdu_session_t);
1908
1909                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
1910                         {
1911                           hdrlen += sizeof (gtpu_paging_policy_t);
1912                         }
1913                     }
1914                 }
1915
1916               offset = ls_param->sr_prefixlen / 8;
1917               shift = ls_param->sr_prefixlen % 8;
1918
1919               offset += 1;
1920               if (PREDICT_TRUE (shift == 0))
1921                 {
1922                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
1923                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
1924                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1925                     {
1926                       clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
1927                     }
1928                   else
1929                     {
1930                       clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
1931                     }
1932
1933                   if (qfip)
1934                     {
1935                       qfi =
1936                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
1937                         ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
1938
1939                       if (sess->type)
1940                         {
1941                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
1942                         }
1943
1944                       seg0.as_u8[offset + 4] = qfi;
1945                     }
1946                 }
1947               else
1948                 {
1949                   int idx;
1950
1951                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST
1952                       || gtpu_type == GTPU_TYPE_ECHO_REPLY
1953                       || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1954                     {
1955                       for (idx = 0; idx < 2; idx++)
1956                         {
1957                           seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
1958                           seg0.as_u8[offset + idx + 1] |=
1959                             seqp[idx] << (8 - shift);
1960                         }
1961                     }
1962                   else
1963                     {
1964                       for (idx = 0; idx < 4; idx++)
1965                         {
1966                           seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
1967                           seg0.as_u8[offset + idx + 1] |=
1968                             teidp[idx] << (8 - shift);
1969                         }
1970                     }
1971
1972                   if (qfip)
1973                     {
1974                       qfi =
1975                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
1976                         ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
1977
1978                       if (sess->type)
1979                         {
1980                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
1981                         }
1982
1983                       seg0.as_u8[offset + 4] |= qfi >> shift;
1984                       seg0.as_u8[offset + 5] |= qfi << (8 - shift);
1985                     }
1986                 }
1987
1988               // jump over variable length data
1989               vlib_buffer_advance (b0, (word) hdrlen);
1990
1991               // get length of encapsulated IPv6 packet (the remaining part)
1992               len0 = vlib_buffer_length_in_chain (vm, b0);
1993
1994               if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
1995                 {
1996                   encap = vlib_buffer_get_current (b0);
1997                 }
1998
1999               uword *p;
2000               ip6srv_combo_header_t *ip6srv;
2001               ip6_sr_policy_t *sr_policy = NULL;
2002               ip6_sr_sl_t *sl = NULL;
2003               u32 *sl_index;
2004               u32 hdr_len;
2005
2006               p =
2007                 mhash_get (&sm2->sr_policies_index_hash,
2008                            &ls_param->sr_prefix);
2009               if (p)
2010                 {
2011                   sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
2012                 }
2013
2014               if (sr_policy)
2015                 {
2016                   vec_foreach (sl_index, sr_policy->segments_lists)
2017                   {
2018                     sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
2019                     if (sl != NULL)
2020                       break;
2021                   }
2022                 }
2023
2024               hdr_len = sizeof (ip6srv_combo_header_t);
2025
2026               if (sl)
2027                 hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
2028
2029               hdr_len += sizeof (ip6_address_t) * 2;
2030
2031               // jump back to data[0] or pre_data if required
2032               vlib_buffer_advance (b0, -(word) hdr_len);
2033
2034               ip6srv = vlib_buffer_get_current (b0);
2035
2036               if (sl)
2037                 {
2038                   clib_memcpy_fast (ip6srv, sl->rewrite,
2039                                     vec_len (sl->rewrite));
2040
2041                   if (vec_len (sl->segments) > 1)
2042                     {
2043                       ip6srv->ip.src_address = src0;
2044
2045                       ip6srv->sr.tag =
2046                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
2047
2048                       ip6srv->sr.segments_left += 2;
2049                       ip6srv->sr.last_entry += 2;
2050
2051                       ip6srv->sr.length += ((sizeof (ip6_address_t) * 2) / 8);
2052
2053                       ip6srv->sr.segments[0] = dst0;
2054                       ip6srv->sr.segments[1] = seg0;
2055
2056                       clib_memcpy_fast (&ip6srv->sr.segments[2],
2057                                         (u8 *) (sl->rewrite +
2058                                                 sizeof (ip6_header_t) +
2059                                                 sizeof (ip6_sr_header_t)),
2060                                         vec_len (sl->segments) *
2061                                         sizeof (ip6_address_t));
2062                     }
2063                   else
2064                     {
2065                       ip6srv->ip.src_address = src0;
2066                       ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
2067
2068                       ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
2069                       ip6srv->sr.segments_left = 2;
2070                       ip6srv->sr.last_entry = 1;
2071                       ip6srv->sr.length =
2072                         ((sizeof (ip6_sr_header_t) +
2073                           2 * sizeof (ip6_address_t)) / 8) - 1;
2074                       ip6srv->sr.flags = 0;
2075
2076                       ip6srv->sr.tag =
2077                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
2078
2079                       ip6srv->sr.segments[0] = dst0;
2080                       ip6srv->sr.segments[1] = seg0;
2081
2082                       if (vec_len (sl->segments))
2083                         {
2084                           ip6srv->sr.segments[2] = sl->segments[0];
2085                           ip6srv->sr.last_entry++;
2086                           ip6srv->sr.length += sizeof (ip6_address_t) / 8;
2087                         }
2088                     }
2089                 }
2090               else
2091                 {
2092                   clib_memcpy_fast (ip6srv, &sm->cache_hdr,
2093                                     sizeof (ip6_header_t));
2094
2095                   ip6srv->ip.src_address = src0;
2096                   ip6srv->ip.dst_address = seg0;
2097
2098                   ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
2099                   ip6srv->sr.segments_left = 1;
2100                   ip6srv->sr.last_entry = 0;
2101                   ip6srv->sr.length =
2102                     ((sizeof (ip6_sr_header_t) +
2103                       sizeof (ip6_address_t)) / 8) - 1;
2104                   ip6srv->sr.flags = 0;
2105
2106                   ip6srv->sr.tag =
2107                     clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
2108
2109                   ip6srv->sr.segments[0] = dst0;
2110                 }
2111
2112               ip6srv->ip.payload_length =
2113                 clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
2114               ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
2115
2116               if (PREDICT_TRUE (encap != NULL))
2117                 {
2118                   if (ls_param->nhtype == SRV6_NHTYPE_NONE)
2119                     {
2120                       if ((clib_net_to_host_u32
2121                            (encap->ip_version_traffic_class_and_flow_label) >>
2122                            28) == 6)
2123                         ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
2124                       else
2125                         ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
2126                     }
2127                   else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
2128                     {
2129                       ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
2130                       if ((clib_net_to_host_u32
2131                            (encap->ip_version_traffic_class_and_flow_label) >>
2132                            28) != 4)
2133                         {
2134                           // Bad encap packet.
2135                           next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
2136                           bad_n++;
2137                           goto DONE;
2138                         }
2139                     }
2140                   else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
2141                     {
2142                       ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
2143                       if ((clib_net_to_host_u32
2144                            (encap->ip_version_traffic_class_and_flow_label) >>
2145                            28) != 6)
2146                         {
2147                           // Bad encap packet.
2148                           next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
2149                           bad_n++;
2150                           goto DONE;
2151                         }
2152                     }
2153                   else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
2154                     {
2155                       ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
2156                     }
2157                 }
2158               else
2159                 {
2160                   ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
2161                 }
2162
2163               good_n++;
2164
2165               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
2166                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
2167                 {
2168                   srv6_end_rewrite_trace_t *tr =
2169                     vlib_add_trace (vm, node, b0, sizeof (*tr));
2170                   clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
2171                                sizeof (ip6_address_t));
2172                   clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
2173                                sizeof (ip6_address_t));
2174                   tr->teid = teid;
2175                   clib_memcpy (tr->sr_prefix.as_u8, ls_param->sr_prefix.as_u8,
2176                                sizeof (ip6_address_t));
2177                   tr->sr_prefixlen = ls_param->sr_prefixlen;
2178                 }
2179             }
2180
2181         DONE:
2182           vlib_increment_combined_counter
2183             (((next0 ==
2184                SRV6_END_M_GTP6_D_DI_NEXT_DROP) ?
2185               &(sm2->sr_ls_invalid_counters) : &(sm2->sr_ls_valid_counters)),
2186              thread_index, ls0 - sm2->localsids, 1,
2187              vlib_buffer_length_in_chain (vm, b0));
2188
2189           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
2190                                            n_left_to_next, bi0, next0);
2191         }
2192
2193       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
2194     }
2195
2196   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_di_node_index,
2197                                SRV6_END_ERROR_M_GTP6_D_DI_BAD_PACKETS, bad_n);
2198
2199   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_di_node_index,
2200                                SRV6_END_ERROR_M_GTP6_D_DI_PACKETS, good_n);
2201
2202   return frame->n_vectors;
2203 }
2204
2205 VLIB_REGISTER_NODE (srv6_end_m_gtp6_e) =
2206 {
2207   .name = "srv6-end-m-gtp6-e",.vector_size = sizeof (u32),.format_trace =
2208     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
2209     ARRAY_LEN (srv6_end_error_v6_e_strings),.error_strings =
2210     srv6_end_error_v6_e_strings,.n_next_nodes =
2211     SRV6_END_M_GTP6_E_N_NEXT,.next_nodes =
2212   {
2213   [SRV6_END_M_GTP6_E_NEXT_DROP] = "error-drop",
2214       [SRV6_END_M_GTP6_E_NEXT_LOOKUP] = "ip6-lookup",}
2215 ,};
2216
2217 VLIB_REGISTER_NODE (srv6_end_m_gtp6_d) =
2218 {
2219   .name = "srv6-end-m-gtp6-d",.vector_size = sizeof (u32),.format_trace =
2220     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
2221     ARRAY_LEN (srv6_end_error_v6_d_strings),.error_strings =
2222     srv6_end_error_v6_d_strings,.n_next_nodes =
2223     SRV6_END_M_GTP6_D_N_NEXT,.next_nodes =
2224   {
2225   [SRV6_END_M_GTP6_D_NEXT_DROP] = "error-drop",
2226       [SRV6_END_M_GTP6_D_NEXT_LOOKUP] = "ip6-lookup",}
2227 ,};
2228
2229 VLIB_REGISTER_NODE (srv6_end_m_gtp6_d_di) =
2230 {
2231   .name = "srv6-end-m-gtp6-d-di",.vector_size = sizeof (u32),.format_trace =
2232     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
2233     ARRAY_LEN (srv6_end_error_v6_d_di_strings),.error_strings =
2234     srv6_end_error_v6_d_di_strings,.n_next_nodes =
2235     SRV6_END_M_GTP6_D_DI_N_NEXT,.next_nodes =
2236   {
2237   [SRV6_END_M_GTP6_D_DI_NEXT_DROP] = "error-drop",
2238       [SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP] = "ip6-lookup",}
2239 ,};
2240
2241 /*
2242 * fd.io coding-style-patch-verification: ON
2243 *
2244 * Local Variables:
2245 * eval: (c-set-style "gnu")
2246 * End:
2247 */