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 (
 106     s, "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, &t->sr_prefix,
 109     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 #define foreach_srv6_end_v6_dt_error \
 133   _(M_GTP6_DT_PACKETS, "srv6 End.M.GTP6.DT packets") \
 134   _(M_GTP6_DT_BAD_PACKETS, "srv6 End.M.GTP6.DT bad packets")
 135
 136 #define foreach_srv6_t_v4_dt_error \
 137   _(M_GTP4_DT_PACKETS, "srv6 T.M.GTP4.DT packets") \
 138   _(M_GTP4_DT_BAD_PACKETS, "srv6 T.M.GTP4.DT bad packets")
 139
 140 typedef enum
 141 {
 142 #define _(sym,str) SRV6_END_ERROR_##sym,
 143   foreach_srv6_end_v4_error
 144 #undef _
 145     SRV6_END_N_V4_ERROR,
 146 } srv6_end_error_v4_t;
 147
 148 typedef enum
 149 {
 150 #define _(sym,str) SRV6_T_ERROR_##sym,
 151   foreach_srv6_t_v4_d_error
 152 #undef _
 153     SRV6_T_N_V4_D_ERROR,
 154 } srv6_t_error_v4_d_t;
 155
 156 typedef enum
 157 {
 158 #define _(sym,str) SRV6_END_ERROR_##sym,
 159   foreach_srv6_end_v6_e_error
 160 #undef _
 161     SRV6_END_N_V6_E_ERROR,
 162 } srv6_end_error_v6_e_t;
 163
 164 typedef enum
 165 {
 166 #define _(sym,str) SRV6_END_ERROR_##sym,
 167   foreach_srv6_end_v6_d_error
 168 #undef _
 169     SRV6_END_N_V6_D_ERROR,
 170 } srv6_end_error_v6_d_t;
 171
 172 typedef enum
 173 {
 174 #define _(sym,str) SRV6_END_ERROR_##sym,
 175   foreach_srv6_end_v6_d_di_error
 176 #undef _
 177     SRV6_END_N_V6_D_DI_ERROR,
 178 } srv6_end_error_v6_d_di_t;
 179
 180 typedef enum
 181 {
 182 #define _(sym,str) SRV6_END_ERROR_##sym,
 183   foreach_srv6_end_v6_dt_error
 184 #undef _
 185     SRV6_END_N_V6_DT_ERROR,
 186 } srv6_end_error_v6_dt_t;
 187
 188 typedef enum
 189 {
 190 #define _(sym,str) SRV6_T_ERROR_##sym,
 191   foreach_srv6_t_v4_dt_error
 192 #undef _
 193     SRV6_T_N_V4_DT_ERROR,
 194 } srv6_t_error_v4_dt_t;
 195
 196 static char *srv6_end_error_v4_strings[] = {
 197 #define _(sym,string) string,
 198   foreach_srv6_end_v4_error
 199 #undef _
 200 };
 201
 202 static char *srv6_t_error_v4_d_strings[] = {
 203 #define _(sym,string) string,
 204   foreach_srv6_t_v4_d_error
 205 #undef _
 206 };
 207
 208 static char *srv6_end_error_v6_e_strings[] = {
 209 #define _(sym,string) string,
 210   foreach_srv6_end_v6_e_error
 211 #undef _
 212 };
 213
 214 static char *srv6_end_error_v6_d_strings[] = {
 215 #define _(sym,string) string,
 216   foreach_srv6_end_v6_d_error
 217 #undef _
 218 };
 219
 220 static char *srv6_end_error_v6_d_di_strings[] = {
 221 #define _(sym,string) string,
 222   foreach_srv6_end_v6_d_di_error
 223 #undef _
 224 };
 225
 226 static char *srv6_end_error_v6_dt_strings[] = {
 227 #define _(sym,string) string,
 228   foreach_srv6_end_v6_dt_error
 229 #undef _
 230 };
 231
 232 static char *srv6_t_error_v4_dt_strings[] = {
 233 #define _(sym,string) string,
 234   foreach_srv6_t_v4_dt_error
 235 #undef _
 236 };
 237
 238 typedef enum
 239 {
 240   SRV6_END_M_GTP4_E_NEXT_DROP,
 241   SRV6_END_M_GTP4_E_NEXT_LOOKUP,
 242   SRV6_END_M_GTP4_E_N_NEXT,
 243 } srv6_end_m_gtp4_e_next_t;
 244
 245 typedef enum
 246 {
 247   SRV6_T_M_GTP4_D_NEXT_DROP,
 248   SRV6_T_M_GTP4_D_NEXT_LOOKUP4,
 249   SRV6_T_M_GTP4_D_NEXT_LOOKUP6,
 250   SRV6_T_M_GTP4_D_N_NEXT,
 251 } srv6_T_m_gtp4_d_next_t;
 252
 253 typedef enum
 254 {
 255   SRV6_END_M_GTP6_E_NEXT_DROP,
 256   SRV6_END_M_GTP6_E_NEXT_LOOKUP,
 257   SRV6_END_M_GTP6_E_N_NEXT,
 258 } srv6_end_m_gtp6_e_next_t;
 259
 260 typedef enum
 261 {
 262   SRV6_END_M_GTP6_D_NEXT_DROP,
 263   SRV6_END_M_GTP6_D_NEXT_LOOKUP4,
 264   SRV6_END_M_GTP6_D_NEXT_LOOKUP6,
 265   SRV6_END_M_GTP6_D_N_NEXT,
 266 } srv6_end_m_gtp6_d_next_t;
 267
 268 typedef enum
 269 {
 270   SRV6_END_M_GTP6_D_DI_NEXT_DROP,
 271   SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP,
 272   SRV6_END_M_GTP6_D_DI_N_NEXT,
 273 } srv6_end_m_gtp6_d_di_next_t;
 274
 275 typedef enum
 276 {
 277   SRV6_END_M_GTP6_DT_NEXT_DROP,
 278   SRV6_END_M_GTP6_DT_NEXT_LOOKUP4,
 279   SRV6_END_M_GTP6_DT_NEXT_LOOKUP6,
 280   SRV6_END_M_GTP6_DT_N_NEXT,
 281 } srv6_end_m_gtp6_dt_next_t;
 282
 283 typedef enum
 284 {
 285   SRV6_T_M_GTP4_DT_NEXT_DROP,
 286   SRV6_T_M_GTP4_DT_NEXT_LOOKUP4,
 287   SRV6_T_M_GTP4_DT_NEXT_LOOKUP6,
 288   SRV6_T_M_GTP4_DT_N_NEXT,
 289 } srv6_t_m_gtp4_dt_next_t;
 290
 291 static inline u16
 292 hash_uword_to_u16 (uword * key)
 293 {
 294   u16 *val;
 295   val = (u16 *) key;
 296 #if uword_bits == 64
 297   return val[0] ^ val[1] ^ val[2] ^ val[3];
 298 #else
 299   return val[0] ^ val[1];
 300 #endif
 301 }
 302
 303 static inline u8
 304 gtpu_type_get (u16 tag)
 305 {
 306   u16 val;
 307
 308   val = clib_net_to_host_u16 (tag);
 309   if (val & SRH_TAG_ECHO_REPLY)
 310     return GTPU_TYPE_ECHO_REPLY;
 311   else if (val & SRH_TAG_ECHO_REQUEST)
 312     return GTPU_TYPE_ECHO_REQUEST;
 313   else if (val & SRH_TAG_ERROR_INDICATION)
 314     return GTPU_TYPE_ERROR_INDICATION;
 315   else if (val & SRH_TAG_END_MARKER)
 316     return GTPU_TYPE_END_MARKER;
 317
 318   return GTPU_TYPE_GTPU;
 319 }
 320
 321 // Function for SRv6 GTP4.E function.
 322 VLIB_NODE_FN (srv6_end_m_gtp4_e)
 323 (vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
 324 {
 325   srv6_end_main_v4_t *sm = &srv6_end_main_v4;
 326   ip6_sr_main_t *sm2 = &sr_main;
 327   u32 n_left_from, next_index, *from, *to_next;
 328   u32 thread_index = vm->thread_index;
 329
 330   u32 good_n = 0, bad_n = 0;
 331
 332   from = vlib_frame_vector_args (frame);
 333   n_left_from = frame->n_vectors;
 334   next_index = node->cached_next_index;
 335
 336   while (n_left_from > 0)
 337     {
 338       u32 n_left_to_next;
 339
 340       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 341
 342       while (n_left_from > 0 && n_left_to_next > 0)
 343         {
 344           u32 bi0;
 345           vlib_buffer_t *b0;
 346           ip6_sr_localsid_t *ls0;
 347           srv6_end_gtp4_e_param_t *ls_param;
 348
 349           ip6srv_combo_header_t *ip6srv0;
 350           ip6_address_t src0, dst0;
 351
 352           ip4_gtpu_header_t *hdr0 = NULL;
 353           uword len0;
 354
 355           u32 next0 = SRV6_END_M_GTP4_E_NEXT_LOOKUP;
 356
 357           // defaults
 358           bi0 = from[0];
 359           to_next[0] = bi0;
 360           from += 1;
 361           to_next += 1;
 362           n_left_from -= 1;
 363           n_left_to_next -= 1;
 364
 365           b0 = vlib_get_buffer (vm, bi0);
 366           ls0 = pool_elt_at_index (sm2->localsids,
 367                                    vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 368
 369           ls_param = (srv6_end_gtp4_e_param_t *) ls0->plugin_mem;
 370
 371           ip6srv0 = vlib_buffer_get_current (b0);
 372           src0 = ip6srv0->ip.src_address;
 373           dst0 = ip6srv0->ip.dst_address;
 374
 375           len0 = vlib_buffer_length_in_chain (vm, b0);
 376
 377           if ((ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE &&
 378                len0 <
 379                  sizeof (ip6srv_combo_header_t) + ip6srv0->sr.length * 8) ||
 380               (len0 < sizeof (ip6_header_t)))
 381             {
 382               next0 = SRV6_END_M_GTP4_E_NEXT_DROP;
 383
 384               bad_n++;
 385             }
 386           else
 387             {
 388               u8 gtpu_type = 0;
 389               u16 tag = 0;
 390               u32 teid = 0;
 391               u8 *teid8p = (u8 *) &teid;
 392               u8 qfi = 0;
 393               u16 seq = 0;
 394               u32 index;
 395               u32 offset, shift;
 396               u32 hdrlen = 0;
 397               uword key;
 398               u16 port;
 399               ip4_address_t dst4;
 400               u16 ie_size = 0;
 401               u8 ie_buf[GTPU_IE_MAX_SIZ];
 402               void *p;
 403               uword plen;
 404
 405               if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE)
 406                 {
 407                   tag = ip6srv0->sr.tag;
 408                 }
 409
 410               offset = ls0->localsid_prefix_len / 8;
 411               shift = ls0->localsid_prefix_len % 8;
 412
 413               gtpu_type = gtpu_type_get (tag);
 414
 415               if (PREDICT_TRUE (shift == 0))
 416                 {
 417                   clib_memcpy_fast (&dst4.as_u8[0], &dst0.as_u8[offset], 4);
 418
 419                   qfi = dst0.as_u8[offset + 4];
 420
 421                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
 422                       gtpu_type == GTPU_TYPE_ECHO_REPLY ||
 423                       gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 424                     {
 425                       clib_memcpy_fast (&seq, &dst0.as_u8[offset + 5], 2);
 426                     }
 427                   else
 428                     {
 429                       clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 5], 4);
 430                     }
 431                 }
 432               else
 433                 {
 434                   u8 *sp;
 435
 436                   for (index = 0; index < 4; index++)
 437                     {
 438                       dst4.as_u8[index] = dst0.as_u8[offset + index] << shift;
 439                       dst4.as_u8[index] |=
 440                         dst0.as_u8[offset + index + 1] >> (8 - shift);
 441                     }
 442
 443                   qfi |= dst0.as_u8[offset + 4] << shift;
 444                   qfi |= dst0.as_u8[offset + 5] >> (8 - shift);
 445
 446                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
 447                       gtpu_type == GTPU_TYPE_ECHO_REPLY ||
 448                       gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 449                     {
 450                       sp = (u8 *) &seq;
 451                       for (index = 0; index < 2; index++)
 452                         {
 453                           sp[index] = dst0.as_u8[offset + 5 + index] << shift;
 454                           sp[index] |=
 455                             dst0.as_u8[offset + 6 + index] >> (8 - shift);
 456                         }
 457                     }
 458                   else
 459                     {
 460                       for (index = 0; index < 4; index++)
 461                         {
 462                           *teid8p = dst0.as_u8[offset + 5 + index] << shift;
 463                           *teid8p |=
 464                             dst0.as_u8[offset + 6 + index] >> (8 - shift);
 465                           teid8p++;
 466                         }
 467                     }
 468                 }
 469
 470               if (qfi)
 471                 {
 472                   hdrlen =
 473                     sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
 474                 }
 475               else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
 476                        gtpu_type == GTPU_TYPE_ECHO_REPLY ||
 477                        gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 478                 {
 479                   hdrlen = sizeof (gtpu_exthdr_t);
 480                 }
 481
 482               if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ECHO_REPLY))
 483                 {
 484                   hdrlen += sizeof (gtpu_recovery_ie);
 485                 }
 486
 487               if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
 488                 {
 489                   ip6_sr_tlv_t *tlv;
 490                   u16 ext_len;
 491
 492                   ext_len = ip6srv0->sr.length * 8;
 493
 494                   if (ext_len >
 495                       sizeof (ip6_address_t) * (ip6srv0->sr.last_entry + 1))
 496                     {
 497                       tlv = (ip6_sr_tlv_t *) ((u8 *) &ip6srv0->sr +
 498                                               sizeof (ip6_sr_header_t) +
 499                                               sizeof (ip6_address_t) *
 500                                                 (ip6srv0->sr.last_entry + 1));
 501
 502                       if (tlv->type == SRH_TLV_USER_PLANE_CONTAINER)
 503                         {
 504                           user_plane_sub_tlv_t *sub_tlv;
 505
 506                           sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
 507
 508                           ie_size = sub_tlv->length;
 509                           clib_memcpy_fast (ie_buf, sub_tlv->value, ie_size);
 510
 511                           hdrlen += ie_size;
 512                         }
 513                     }
 514                 }
 515
 516               if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE)
 517                 {
 518                   vlib_buffer_advance (b0,
 519                                        (word) sizeof (ip6srv_combo_header_t) +
 520                                          ip6srv0->sr.length * 8);
 521                 }
 522               else
 523                 {
 524                   vlib_buffer_advance (b0, (word) sizeof (ip6_header_t));
 525                 }
 526
 527               // get length of encapsulated IPv6 packet (the remaining part)
 528               p = vlib_buffer_get_current (b0);
 529
 530               plen = len0 = vlib_buffer_length_in_chain (vm, b0);
 531
 532               len0 += hdrlen;
 533
 534               hdrlen += sizeof (ip4_gtpu_header_t);
 535
 536               // IPv4 GTP-U header creation.
 537               vlib_buffer_advance (b0, -(word) hdrlen);
 538
 539               hdr0 = vlib_buffer_get_current (b0);
 540
 541               clib_memcpy_fast (hdr0, &sm->cache_hdr,
 542                                 sizeof (ip4_gtpu_header_t));
 543
 544               hdr0->ip4.dst_address.as_u32 = dst4.as_u32;
 545
 546               hdr0->gtpu.teid = teid;
 547               hdr0->gtpu.length = clib_host_to_net_u16 (len0);
 548
 549               hdr0->gtpu.type = gtpu_type;
 550
 551               if (gtpu_type == GTPU_TYPE_ECHO_REPLY ||
 552                   gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
 553                   gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 554                 {
 555                   hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG;
 556                   hdr0->gtpu.ext->seq = seq;
 557                   hdr0->gtpu.ext->npdu_num = 0;
 558                   hdr0->gtpu.ext->nextexthdr = 0;
 559
 560                   if (gtpu_type == GTPU_TYPE_ECHO_REPLY)
 561                     {
 562                       gtpu_recovery_ie *recovery;
 563
 564                       recovery =
 565                         (gtpu_recovery_ie *) ((u8 *) hdr0 +
 566                                               (hdrlen -
 567                                                sizeof (gtpu_recovery_ie)));
 568                       recovery->type = GTPU_RECOVERY_IE_TYPE;
 569                       recovery->restart_counter = 0;
 570                     }
 571                   else if (gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 572                     {
 573                       if (ie_size)
 574                         {
 575                           u8 *ie_ptr;
 576
 577                           ie_ptr = (u8 *) ((u8 *) hdr0 + (hdrlen - ie_size));
 578                           clib_memcpy_fast (ie_ptr, ie_buf, ie_size);
 579                         }
 580                     }
 581                 }
 582               else
 583                 {
 584                   if (qfi)
 585                     {
 586                       hdr0->gtpu.ext->seq = 0;
 587                       hdr0->gtpu.ext->npdu_num = 0;
 588                     }
 589                 }
 590
 591               if (qfi)
 592                 {
 593                   u8 type = 0;
 594                   gtpu_pdu_session_t *sess;
 595
 596                   hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
 597
 598                   hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
 599
 600                   type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
 601
 602                   qfi = ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
 603                         ((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
 604
 605                   sess = (gtpu_pdu_session_t *) (((char *) hdr0) +
 606                                                  sizeof (ip4_gtpu_header_t) +
 607                                                  sizeof (gtpu_exthdr_t));
 608                   sess->exthdrlen = 1;
 609                   sess->type = type;
 610                   sess->spare = 0;
 611                   sess->u.val = qfi;
 612                   sess->nextexthdr = 0;
 613                 }
 614
 615               vnet_buffer (b0)->sw_if_index[VLIB_TX] = ls_param->fib4_index;
 616
 617               if (ls_param->v4src_position)
 618                 {
 619                   offset = ls_param->v4src_position / 8;
 620                   shift = ls_param->v4src_position % 8;
 621
 622                   if (PREDICT_TRUE (shift == 0))
 623                     {
 624                       for (index = 0; index < 4; index++)
 625                         {
 626                           hdr0->ip4.src_address.as_u8[index] =
 627                             src0.as_u8[offset + index];
 628                         }
 629                     }
 630                   else
 631                     {
 632                       for (index = 0; index < 4; index++)
 633                         {
 634                           hdr0->ip4.src_address.as_u8[index] =
 635                             src0.as_u8[offset + index] << shift;
 636                           hdr0->ip4.src_address.as_u8[index] |=
 637                             src0.as_u8[offset + index + 1] >> (8 - shift);
 638                         }
 639                     }
 640                 }
 641               else
 642                 {
 643                   clib_memcpy_fast (&hdr0->ip4.src_address,
 644                                     &ls_param->v4src_addr, 4);
 645                 }
 646
 647               key = hash_memory (p, plen < 40 ? plen : 40, 0);
 648               port = hash_uword_to_u16 (&key);
 649               hdr0->udp.src_port = port;
 650
 651               hdr0->udp.length = clib_host_to_net_u16 (
 652                 len0 + sizeof (udp_header_t) + sizeof (gtpu_header_t));
 653
 654               hdr0->ip4.length =
 655                 clib_host_to_net_u16 (len0 + sizeof (ip4_gtpu_header_t));
 656
 657               hdr0->ip4.checksum = ip4_header_checksum (&hdr0->ip4);
 658
 659               good_n++;
 660
 661               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
 662                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
 663                 {
 664                   srv6_end_rewrite_trace_t *tr =
 665                     vlib_add_trace (vm, node, b0, sizeof (*tr));
 666                   clib_memcpy (tr->src.as_u8, hdr0->ip4.src_address.as_u8,
 667                                sizeof (hdr0->ip4.src_address.as_u8));
 668                   clib_memcpy (tr->dst.as_u8, hdr0->ip4.dst_address.as_u8,
 669                                sizeof (hdr0->ip4.dst_address.as_u8));
 670                   tr->teid = hdr0->gtpu.teid;
 671                 }
 672             }
 673
 674           vlib_increment_combined_counter (
 675             ((next0 == SRV6_END_M_GTP4_E_NEXT_DROP) ?
 676                &(sm2->sr_ls_invalid_counters) :
 677                &(sm2->sr_ls_valid_counters)),
 678             thread_index, ls0 - sm2->localsids, 1,
 679             vlib_buffer_length_in_chain (vm, b0));
 680
 681           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 682                                            n_left_to_next, bi0, next0);
 683         }
 684
 685       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
 686     }
 687
 688   vlib_node_increment_counter (vm, sm->end_m_gtp4_e_node_index,
 689                                SRV6_END_ERROR_M_GTP4_E_BAD_PACKETS, bad_n);
 690
 691   vlib_node_increment_counter (vm, sm->end_m_gtp4_e_node_index,
 692                                SRV6_END_ERROR_M_GTP4_E_PACKETS, good_n);
 693
 694   return frame->n_vectors;
 695 }
 696
 697 // Function for SRv6 GTP4.D function.
 698 static inline u32
 699 srv6_gtp4_decap_processing (vlib_main_t *vm, vlib_node_runtime_t *node,
 700                             vlib_buffer_t *b0)
 701 {
 702   srv6_t_main_v4_decap_t *sm = &srv6_t_main_v4_decap;
 703   ip6_sr_main_t *sm2 = &sr_main;
 704
 705   ip6_sr_sl_t *sl0;
 706   srv6_end_gtp4_d_param_t *ls_param;
 707   ip4_header_t *ip4;
 708
 709   uword len0;
 710
 711   u32 next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP6;
 712
 713   sl0 = pool_elt_at_index (sm2->sid_lists,
 714                            vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 715
 716   ls_param = (srv6_end_gtp4_d_param_t *) sl0->plugin_mem;
 717
 718   len0 = vlib_buffer_length_in_chain (vm, b0);
 719
 720   ip4 = vlib_buffer_get_current (b0);
 721
 722   if (ip4->protocol != IP_PROTOCOL_UDP || len0 < sizeof (ip4_gtpu_header_t))
 723     {
 724       next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
 725     }
 726   else
 727     {
 728       uword *p;
 729       ip6_sr_policy_t *sr_policy = NULL;
 730       ip6_sr_sl_t *sl = NULL;
 731       u32 *sl_index;
 732       u32 hdr_len;
 733
 734       ip4_gtpu_header_t *hdr;
 735       ip4_address_t src, dst;
 736       u8 *srcp, *dstp;
 737       ip6_header_t *encap = NULL;
 738       ip6_address_t seg;
 739       ip6_address_t src6;
 740       u8 gtpu_type;
 741       u32 teid;
 742       u8 *teidp;
 743       u8 qfi = 0;
 744       u8 *qfip = NULL;
 745       u16 seq = 0;
 746       u8 *seqp;
 747       u32 offset, shift, index;
 748       ip6srv_combo_header_t *ip6srv;
 749       gtpu_pdu_session_t *sess = NULL;
 750       int ie_size = 0;
 751       u16 tlv_siz = 0;
 752       u8 ie_buf[GTPU_IE_MAX_SIZ];
 753
 754       // Decap from GTP-U.
 755       hdr = (ip4_gtpu_header_t *) ip4;
 756
 757       hdr_len = sizeof (ip4_gtpu_header_t);
 758
 759       teid = hdr->gtpu.teid;
 760       teidp = (u8 *) &teid;
 761
 762       seqp = (u8 *) &seq;
 763
 764       gtpu_type = hdr->gtpu.type;
 765
 766       if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
 767         {
 768           // Extention header.
 769           hdr_len += sizeof (gtpu_exthdr_t);
 770
 771           seq = hdr->gtpu.ext->seq;
 772
 773           if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 774             {
 775               // PDU Session Container.
 776               sess = (gtpu_pdu_session_t *) (((char *) hdr) + hdr_len);
 777               qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
 778               qfip = (u8 *) &qfi;
 779
 780               hdr_len += sizeof (gtpu_pdu_session_t);
 781
 782               if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
 783                 {
 784                   hdr_len += sizeof (gtpu_paging_policy_t);
 785                 }
 786             }
 787         }
 788
 789       src = hdr->ip4.src_address;
 790       srcp = (u8 *) &src;
 791
 792       dst = hdr->ip4.dst_address;
 793       dstp = (u8 *) &dst;
 794
 795       seg = ls_param->sr_prefix;
 796
 797       offset = ls_param->sr_prefixlen / 8;
 798       shift = ls_param->sr_prefixlen % 8;
 799
 800       if (PREDICT_TRUE (shift == 0))
 801         {
 802           clib_memcpy_fast (&seg.as_u8[offset], dstp, 4);
 803
 804           if (qfip)
 805             {
 806               qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
 807                     ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 808
 809               if (sess->type)
 810                 {
 811                   qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
 812                 }
 813
 814               seg.as_u8[offset + 4] = qfi;
 815             }
 816
 817           if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
 818               gtpu_type == GTPU_TYPE_ECHO_REPLY ||
 819               gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 820             {
 821               clib_memcpy_fast (&seg.as_u8[offset + 5], seqp, 2);
 822             }
 823           else
 824             {
 825               clib_memcpy_fast (&seg.as_u8[offset + 5], teidp, 4);
 826             }
 827         }
 828       else
 829         {
 830           for (index = 0; index < 4; index++)
 831             {
 832               seg.as_u8[offset + index] |= dstp[index] >> shift;
 833               seg.as_u8[offset + index + 1] |= dstp[index] << (8 - shift);
 834             }
 835
 836           if (qfip)
 837             {
 838               qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
 839                     ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 840
 841               if (sess->type)
 842                 {
 843                   qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
 844                 }
 845
 846               seg.as_u8[offset + 4] |= qfi >> shift;
 847               seg.as_u8[offset + 5] |= qfi << (8 - shift);
 848             }
 849
 850           if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
 851               gtpu_type == GTPU_TYPE_ECHO_REPLY ||
 852               gtpu_type == GTPU_TYPE_ERROR_INDICATION)
 853             {
 854               for (index = 0; index < 2; index++)
 855                 {
 856                   seg.as_u8[offset + 5 + index] |= seqp[index] >> shift;
 857                   seg.as_u8[offset + 6 + index] |= seqp[index] << (8 - shift);
 858                 }
 859             }
 860           else
 861             {
 862               for (index = 0; index < 4; index++)
 863                 {
 864                   seg.as_u8[offset + index + 5] |= teidp[index] >> shift;
 865                   seg.as_u8[offset + index + 6] |= teidp[index] << (8 - shift);
 866                 }
 867             }
 868         }
 869
 870       if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
 871         {
 872           u16 payload_len;
 873
 874           payload_len = clib_net_to_host_u16 (hdr->gtpu.length);
 875           if (payload_len != 0)
 876             {
 877               ie_size = payload_len - (hdr_len - sizeof (ip4_gtpu_header_t));
 878               if (ie_size > 0)
 879                 {
 880                   u8 *ies;
 881
 882                   ies = (u8 *) ((u8 *) hdr + hdr_len);
 883                   clib_memcpy_fast (ie_buf, ies, ie_size);
 884                   hdr_len += ie_size;
 885                 }
 886             }
 887         }
 888
 889       src6 = ls_param->v6src_prefix;
 890
 891       offset = ls_param->v6src_prefixlen / 8;
 892       shift = ls_param->v6src_prefixlen % 8;
 893
 894       if (PREDICT_TRUE (shift == 0))
 895         {
 896           clib_memcpy_fast (&src6.as_u8[offset], srcp, 4);
 897         }
 898       else
 899         {
 900           for (index = 0; index < 4; index++)
 901             {
 902               src6.as_u8[offset + index] |= srcp[offset] >> shift;
 903               src6.as_u8[offset + index + 1] |= srcp[offset] << (8 - shift);
 904             }
 905         }
 906
 907       vlib_buffer_advance (b0, (word) hdr_len);
 908
 909       // Encap to SRv6.
 910       if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
 911         {
 912           encap = vlib_buffer_get_current (b0);
 913         }
 914
 915       len0 = vlib_buffer_length_in_chain (vm, b0);
 916
 917       p = mhash_get (&sm2->sr_policies_index_hash, &ls_param->sr_prefix);
 918       if (p)
 919         {
 920           sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
 921         }
 922
 923       if (sr_policy)
 924         {
 925           vec_foreach (sl_index, sr_policy->segments_lists)
 926             {
 927               sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
 928               if (sl != NULL)
 929                 break;
 930             }
 931         }
 932
 933       if (sl)
 934         {
 935           hdr_len = sizeof (ip6srv_combo_header_t);
 936           hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
 937           hdr_len += sizeof (ip6_address_t);
 938         }
 939       else
 940         {
 941           hdr_len = sizeof (ip6_header_t);
 942
 943           if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
 944             {
 945               hdr_len += sizeof (ip6_sr_header_t);
 946               hdr_len += sizeof (ip6_address_t);
 947             }
 948         }
 949
 950       if (ie_size)
 951         {
 952           tlv_siz =
 953             sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) + ie_size;
 954
 955           tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0);
 956           hdr_len += tlv_siz;
 957         }
 958
 959       vlib_buffer_advance (b0, -(word) hdr_len);
 960       ip6srv = vlib_buffer_get_current (b0);
 961
 962       if (sl)
 963         {
 964           clib_memcpy_fast (ip6srv, sl->rewrite, vec_len (sl->rewrite));
 965
 966           if (vec_len (sl->segments) > 1)
 967             {
 968               ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 969
 970               ip6srv->sr.segments_left += 1;
 971               ip6srv->sr.last_entry += 1;
 972
 973               ip6srv->sr.length += sizeof (ip6_address_t) / 8;
 974               ip6srv->sr.segments[0] = seg;
 975
 976               clib_memcpy_fast (&ip6srv->sr.segments[1],
 977                                 (u8 *) (sl->rewrite + sizeof (ip6_header_t) +
 978                                         sizeof (ip6_sr_header_t)),
 979                                 vec_len (sl->segments) *
 980                                   sizeof (ip6_address_t));
 981             }
 982           else
 983             {
 984               ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
 985
 986               ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
 987
 988               ip6srv->sr.segments_left = 1;
 989               ip6srv->sr.last_entry = 0;
 990
 991               ip6srv->sr.length =
 992                 ((sizeof (ip6_sr_header_t) + sizeof (ip6_address_t)) / 8) - 1;
 993               ip6srv->sr.flags = 0;
 994
 995               ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 996
 997               ip6srv->sr.segments[0] = seg;
 998               if (vec_len (sl->segments))
 999                 {
1000                   ip6srv->sr.segments[1] = sl->segments[0];
1001                   ip6srv->sr.length += sizeof (ip6_address_t) / 8;
1002                   ip6srv->sr.last_entry++;
1003                 }
1004             }
1005
1006           if (PREDICT_TRUE (encap != NULL))
1007             {
1008               if (ls_param->nhtype == SRV6_NHTYPE_NONE)
1009                 {
1010                   if ((clib_net_to_host_u32 (
1011                          encap->ip_version_traffic_class_and_flow_label) >>
1012                        28) == 6)
1013                     ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1014                   else
1015                     ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1016                 }
1017               else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
1018                 {
1019                   ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1020                   if ((clib_net_to_host_u32 (
1021                          encap->ip_version_traffic_class_and_flow_label) >>
1022                        28) != 4)
1023                     {
1024                       // Bad encap packet.
1025                       next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
1026                       goto DONE;
1027                     }
1028                 }
1029               else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
1030                 {
1031                   ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1032                   if ((clib_net_to_host_u32 (
1033                          encap->ip_version_traffic_class_and_flow_label) >>
1034                        28) != 6)
1035                     {
1036                       // Bad encap packet.
1037                       next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
1038                       goto DONE;
1039                     }
1040                 }
1041               else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1042                 {
1043                   ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
1044                 }
1045             }
1046           else
1047             {
1048               ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
1049             }
1050         }
1051       else
1052         {
1053           clib_memcpy_fast (ip6srv, &sm->cache_hdr, sizeof (ip6_header_t));
1054
1055           ip6srv->ip.dst_address = seg;
1056
1057           if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU))
1058             {
1059               ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
1060
1061               ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
1062
1063               ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
1064
1065               ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1066
1067               ip6srv->sr.segments_left = 0;
1068               ip6srv->sr.last_entry = 0;
1069
1070               ip6srv->sr.length = sizeof (ip6_address_t) / 8;
1071               ip6srv->sr.segments[0] = seg;
1072             }
1073           else
1074             {
1075               if (ls_param->nhtype == SRV6_NHTYPE_NONE)
1076                 {
1077                   if ((clib_net_to_host_u32 (
1078                          encap->ip_version_traffic_class_and_flow_label) >>
1079                        28) == 6)
1080                     ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
1081                   else
1082                     ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
1083                 }
1084               else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
1085                 {
1086                   ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
1087                   if ((clib_net_to_host_u32 (
1088                          encap->ip_version_traffic_class_and_flow_label) >>
1089                        28) != 4)
1090                     {
1091                       // Bad encap packet.
1092                       next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
1093                       goto DONE;
1094                     }
1095                 }
1096               else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
1097                 {
1098                   ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
1099                   if ((clib_net_to_host_u32 (
1100                          encap->ip_version_traffic_class_and_flow_label) >>
1101                        28) != 6)
1102                     {
1103                       // Bad encap packet.
1104                       next0 = SRV6_T_M_GTP4_D_NEXT_DROP;
1105                       goto DONE;
1106                     }
1107                 }
1108               else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1109                 {
1110                   ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET;
1111                 }
1112             }
1113         }
1114
1115       ip6srv->ip.src_address = src6;
1116
1117       if (PREDICT_FALSE (ie_size))
1118         {
1119           ip6_sr_tlv_t *tlv;
1120           user_plane_sub_tlv_t *sub_tlv;
1121
1122           tlv = (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
1123           tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
1124           tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t));
1125           clib_memset (tlv->value, 0, tlv->length);
1126
1127           sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
1128           sub_tlv->type = USER_PLANE_SUB_TLV_IE;
1129           sub_tlv->length = (u8) ie_size;
1130           clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
1131
1132           ip6srv->sr.length += (u8) (tlv_siz / 8);
1133         }
1134
1135       ip6srv->ip.payload_length =
1136         clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
1137
1138       vnet_buffer (b0)->sw_if_index[VLIB_TX] = 0; /* default FIB */
1139
1140       if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1141           PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1142         {
1143           srv6_end_rewrite_trace_t *tr =
1144             vlib_add_trace (vm, node, b0, sizeof (*tr));
1145           clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
1146                        sizeof (tr->src.as_u8));
1147           clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
1148                        sizeof (tr->dst.as_u8));
1149         }
1150     }
1151
1152 DONE:
1153   return next0;
1154 }
1155
1156 VLIB_NODE_FN (srv6_t_m_gtp4_d)
1157 (vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
1158 {
1159   srv6_t_main_v4_decap_t *sm = &srv6_t_main_v4_decap;
1160   ip6_sr_main_t *sm2 = &sr_main;
1161   u32 n_left_from, next_index, *from, *to_next;
1162
1163   ip6_sr_sl_t *sl0;
1164   srv6_end_gtp4_d_param_t *ls_param;
1165
1166   u32 good_n = 0, bad_n = 0;
1167
1168   from = vlib_frame_vector_args (frame);
1169   n_left_from = frame->n_vectors;
1170   next_index = node->cached_next_index;
1171
1172   while (n_left_from > 0)
1173     {
1174       u32 n_left_to_next;
1175
1176       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1177
1178       while (n_left_from > 0 && n_left_to_next > 0)
1179         {
1180           u32 bi0;
1181           vlib_buffer_t *b0;
1182
1183           u32 next0;
1184
1185           ip4_gtpu_header_t *hdr;
1186           u32 hdrlen;
1187           u8 gtpu_type;
1188           bool gtp4;
1189           bool ipv4;
1190
1191           // defaults
1192           bi0 = from[0];
1193           to_next[0] = bi0;
1194           from += 1;
1195           to_next += 1;
1196           n_left_from -= 1;
1197           n_left_to_next -= 1;
1198
1199           b0 = vlib_get_buffer (vm, bi0);
1200
1201           sl0 = pool_elt_at_index (sm2->sid_lists,
1202                                    vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1203
1204           ls_param = (srv6_end_gtp4_d_param_t *) sl0->plugin_mem;
1205
1206           hdr = vlib_buffer_get_current (b0);
1207           gtpu_type = hdr->gtpu.type;
1208
1209           gtp4 = false;
1210           ipv4 = true;
1211
1212           if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU || ls_param->drop_in))
1213             {
1214               gtp4 = true;
1215             }
1216           else
1217             {
1218               ip6_header_t *ip6;
1219
1220               hdrlen = sizeof (ip4_gtpu_header_t);
1221
1222               if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
1223                 {
1224                   hdrlen += sizeof (gtpu_exthdr_t);
1225                   if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
1226                     {
1227                       gtpu_pdu_session_t *sess;
1228                       sess = (gtpu_pdu_session_t *) (((char *) hdr) + hdrlen);
1229                       hdrlen += sizeof (gtpu_pdu_session_t);
1230
1231                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
1232                         {
1233                           hdrlen += sizeof (gtpu_paging_policy_t);
1234                         }
1235                     }
1236                 }
1237
1238               ip6 = (ip6_header_t *) (((char *) hdr) + hdrlen);
1239               if ((clib_net_to_host_u32 (
1240                      ip6->ip_version_traffic_class_and_flow_label) >>
1241                    28) == 6)
1242                 {
1243                   ipv4 = false;
1244                   if (((ip6->dst_address.as_u8[0] == 0xff) &&
1245                        (ip6->dst_address.as_u8[1] == 0x02)) ||
1246                       ((ip6->dst_address.as_u8[0] == 0xfe) &&
1247                        ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
1248                     {
1249                       // Inner desitnation is IPv6 link local
1250                       gtp4 = true;
1251                     }
1252                 }
1253             }
1254
1255           if (gtp4)
1256             {
1257               next0 = srv6_gtp4_decap_processing (vm, node, b0);
1258               if (PREDICT_TRUE (next0 == SRV6_T_M_GTP4_D_NEXT_LOOKUP6))
1259                 good_n++;
1260               else
1261                 bad_n++;
1262             }
1263           else
1264             {
1265               /* Strip off the outer header (IPv4 + GTP + UDP + IEs) */
1266               vlib_buffer_advance (b0, (word) hdrlen);
1267
1268               if (ipv4)
1269                 {
1270                   next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP4;
1271                   vnet_buffer (b0)->sw_if_index[VLIB_TX] =
1272                     ls_param->fib4_index;
1273                 }
1274               else
1275                 {
1276                   next0 = SRV6_T_M_GTP4_D_NEXT_LOOKUP6;
1277                   vnet_buffer (b0)->sw_if_index[VLIB_TX] =
1278                     ls_param->fib6_index;
1279                 }
1280             }
1281
1282           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1283                                            n_left_to_next, bi0, next0);
1284         }
1285
1286       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1287     }
1288
1289   vlib_node_increment_counter (vm, sm->t_m_gtp4_d_node_index,
1290                                SRV6_T_ERROR_M_GTP4_D_BAD_PACKETS, bad_n);
1291
1292   vlib_node_increment_counter (vm, sm->t_m_gtp4_d_node_index,
1293                                SRV6_T_ERROR_M_GTP4_D_PACKETS, good_n);
1294
1295   return frame->n_vectors;
1296 }
1297
1298 VLIB_REGISTER_NODE (srv6_end_m_gtp4_e) =
1299 {
1300   .name = "srv6-end-m-gtp4-e",.vector_size = sizeof (u32),.format_trace =
1301     format_srv6_end_rewrite_trace,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
1302     ARRAY_LEN (srv6_end_error_v4_strings),.error_strings =
1303     srv6_end_error_v4_strings,.n_next_nodes =
1304     SRV6_END_M_GTP4_E_N_NEXT,.next_nodes =
1305   {
1306   [SRV6_END_M_GTP4_E_NEXT_DROP] = "error-drop",
1307   [SRV6_END_M_GTP4_E_NEXT_LOOKUP] = "ip4-lookup",}
1308 ,};
1309
1310 VLIB_REGISTER_NODE (srv6_t_m_gtp4_d) =
1311 {
1312   .name = "srv6-t-m-gtp4-d",.vector_size = sizeof (u32),.format_trace =
1313     format_srv6_end_rewrite_trace,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
1314     ARRAY_LEN (srv6_t_error_v4_d_strings),.error_strings =
1315     srv6_t_error_v4_d_strings,.n_next_nodes =
1316     SRV6_T_M_GTP4_D_N_NEXT,.next_nodes =
1317   {
1318   [SRV6_T_M_GTP4_D_NEXT_DROP] = "error-drop",
1319   [SRV6_T_M_GTP4_D_NEXT_LOOKUP4] = "ip4-lookup",
1320   [SRV6_T_M_GTP4_D_NEXT_LOOKUP6] = "ip6-lookup",}
1321 ,};
1322
1323 // Function for SRv6 GTP6.E function
1324 VLIB_NODE_FN (srv6_end_m_gtp6_e)
1325 (vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
1326 {
1327   srv6_end_main_v6_t *sm = &srv6_end_main_v6;
1328   ip6_sr_main_t *sm2 = &sr_main;
1329   u32 n_left_from, next_index, *from, *to_next;
1330   u32 thread_index = vm->thread_index;
1331
1332   u32 good_n = 0, bad_n = 0;
1333
1334   from = vlib_frame_vector_args (frame);
1335   n_left_from = frame->n_vectors;
1336   next_index = node->cached_next_index;
1337
1338   while (n_left_from > 0)
1339     {
1340       u32 n_left_to_next;
1341
1342       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1343
1344       while (n_left_from > 0 && n_left_to_next > 0)
1345         {
1346           u32 bi0;
1347           vlib_buffer_t *b0;
1348           ip6_sr_localsid_t *ls0;
1349           srv6_end_gtp6_e_param_t *ls_param;
1350
1351           ip6srv_combo_header_t *ip6srv0;
1352           ip6_address_t dst0, src0, seg0;
1353
1354           ip6_gtpu_header_t *hdr0 = NULL;
1355           uword len0;
1356           uword key;
1357           u16 port;
1358           u16 tag;
1359           void *p;
1360           uword plen;
1361
1362           u32 next0 = SRV6_END_M_GTP6_E_NEXT_LOOKUP;
1363
1364           // defaults
1365           bi0 = from[0];
1366           to_next[0] = bi0;
1367           from += 1;
1368           to_next += 1;
1369           n_left_from -= 1;
1370           n_left_to_next -= 1;
1371
1372           b0 = vlib_get_buffer (vm, bi0);
1373           ls0 = pool_elt_at_index (sm2->localsids,
1374                                    vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1375
1376           ls_param = (srv6_end_gtp6_e_param_t *) ls0->plugin_mem;
1377
1378           ip6srv0 = vlib_buffer_get_current (b0);
1379           dst0 = ip6srv0->ip.dst_address;
1380           src0 = ip6srv0->ip.src_address;
1381           seg0 = ip6srv0->sr.segments[0];
1382
1383           tag = ip6srv0->sr.tag;
1384
1385           len0 = vlib_buffer_length_in_chain (vm, b0);
1386
1387           if ((ip6srv0->ip.protocol != IPPROTO_IPV6_ROUTE) ||
1388               (len0 < sizeof (ip6srv_combo_header_t) + 8 * ip6srv0->sr.length))
1389             {
1390               next0 = SRV6_END_M_GTP6_E_NEXT_DROP;
1391
1392               bad_n++;
1393             }
1394           else
1395             {
1396               // we need to be sure there is enough space before
1397               // ip6srv0 header, there is some extra space
1398               // in the pre_data area for this kind of
1399               // logic
1400
1401               u32 teid = 0;
1402               u8 *teid8p = (u8 *) &teid;
1403               u8 qfi = 0;
1404               u16 seq = 0;
1405               u8 gtpu_type = 0;
1406               u16 index;
1407               u16 offset, shift;
1408               u32 hdrlen = 0;
1409               u16 ie_size = 0;
1410               u8 ie_buf[GTPU_IE_MAX_SIZ];
1411
1412               index = ls0->localsid_prefix_len;
1413               offset = index / 8;
1414               shift = index % 8;
1415
1416               gtpu_type = gtpu_type_get (tag);
1417
1418               if (PREDICT_TRUE (shift == 0))
1419                 {
1420                   qfi = dst0.as_u8[offset];
1421                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
1422                       gtpu_type == GTPU_TYPE_ECHO_REPLY ||
1423                       gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1424                     {
1425                       clib_memcpy_fast (&seq, &dst0.as_u8[offset + 1], 2);
1426                     }
1427                   else
1428                     {
1429                       clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 1], 4);
1430                     }
1431                 }
1432               else
1433                 {
1434                   u8 *sp;
1435
1436                   qfi |= dst0.as_u8[offset] << shift;
1437                   qfi |= dst0.as_u8[offset + 1] >> (8 - shift);
1438
1439                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
1440                       gtpu_type == GTPU_TYPE_ECHO_REPLY ||
1441                       gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1442                     {
1443                       sp = (u8 *) &seq;
1444                       for (index = 0; index < 2; index++)
1445                         {
1446                           sp[index] = dst0.as_u8[offset + 1 + index] << shift;
1447                           sp[index] |=
1448                             dst0.as_u8[offset + index + 2] >> (8 - shift);
1449                         }
1450                     }
1451                   else
1452                     {
1453                       for (index = 0; index < 4; index++)
1454                         {
1455                           *teid8p = dst0.as_u8[offset + index + 1] << shift;
1456                           *teid8p |=
1457                             dst0.as_u8[offset + index + 2] >> (8 - shift);
1458                           teid8p++;
1459                         }
1460                     }
1461                 }
1462
1463               if (qfi)
1464                 {
1465                   hdrlen =
1466                     sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
1467                 }
1468               else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
1469                        gtpu_type == GTPU_TYPE_ECHO_REPLY ||
1470                        gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1471                 {
1472                   hdrlen = sizeof (gtpu_exthdr_t);
1473                 }
1474
1475               if (gtpu_type == GTPU_TYPE_ECHO_REPLY)
1476                 {
1477                   hdrlen += sizeof (gtpu_recovery_ie);
1478                 }
1479
1480               if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
1481                 {
1482                   ip6_sr_tlv_t *tlv;
1483                   u16 ext_len;
1484
1485                   ext_len = ip6srv0->sr.length * 8;
1486
1487                   if (ext_len >
1488                       sizeof (ip6_address_t) * (ip6srv0->sr.last_entry + 1))
1489                     {
1490                       tlv = (ip6_sr_tlv_t *) ((u8 *) &ip6srv0->sr +
1491                                               sizeof (ip6_sr_header_t) +
1492                                               sizeof (ip6_address_t) *
1493                                                 (ip6srv0->sr.last_entry + 1));
1494
1495                       if (tlv->type == SRH_TLV_USER_PLANE_CONTAINER)
1496                         {
1497                           user_plane_sub_tlv_t *sub_tlv;
1498
1499                           sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
1500
1501                           ie_size = sub_tlv->length;
1502                           clib_memcpy_fast (ie_buf, sub_tlv->value, ie_size);
1503
1504                           hdrlen += ie_size;
1505                         }
1506                     }
1507                 }
1508
1509               vlib_buffer_advance (b0, (word) sizeof (ip6srv_combo_header_t) +
1510                                          ip6srv0->sr.length * 8);
1511
1512               // get length of encapsulated IPv6 packet (the remaining part)
1513               p = vlib_buffer_get_current (b0);
1514
1515               plen = len0 = vlib_buffer_length_in_chain (vm, b0);
1516
1517               len0 += hdrlen;
1518
1519               hdrlen += sizeof (ip6_gtpu_header_t);
1520
1521               vlib_buffer_advance (b0, -(word) hdrlen);
1522
1523               hdr0 = vlib_buffer_get_current (b0);
1524
1525               clib_memcpy_fast (hdr0, &sm->cache_hdr,
1526                                 sizeof (ip6_gtpu_header_t));
1527
1528               hdr0->gtpu.teid = teid;
1529               hdr0->gtpu.length = clib_host_to_net_u16 (len0);
1530
1531               hdr0->gtpu.type = gtpu_type;
1532
1533               if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
1534                   gtpu_type == GTPU_TYPE_ECHO_REPLY ||
1535                   gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1536                 {
1537                   hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG;
1538                   hdr0->gtpu.ext->seq = seq;
1539                   hdr0->gtpu.ext->npdu_num = 0;
1540                   hdr0->gtpu.ext->nextexthdr = 0;
1541
1542                   if (gtpu_type == GTPU_TYPE_ECHO_REPLY)
1543                     {
1544                       gtpu_recovery_ie *recovery;
1545
1546                       recovery =
1547                         (gtpu_recovery_ie *) ((u8 *) hdr0 +
1548                                               (hdrlen -
1549                                                sizeof (gtpu_recovery_ie)));
1550                       recovery->type = GTPU_RECOVERY_IE_TYPE;
1551                       recovery->restart_counter = 0;
1552                     }
1553                   else if (gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1554                     {
1555                       if (ie_size)
1556                         {
1557                           u8 *ie_ptr;
1558
1559                           ie_ptr = (u8 *) ((u8 *) hdr0 + (hdrlen - ie_size));
1560                           clib_memcpy_fast (ie_ptr, ie_buf, ie_size);
1561                         }
1562                     }
1563                 }
1564               else
1565                 {
1566                   if (qfi)
1567                     {
1568                       hdr0->gtpu.ext->seq = 0;
1569                       hdr0->gtpu.ext->npdu_num = 0;
1570                     }
1571                 }
1572
1573               if (qfi)
1574                 {
1575                   u8 type = 0;
1576                   gtpu_pdu_session_t *sess;
1577
1578                   hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
1579
1580                   hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
1581
1582                   type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
1583
1584                   qfi = ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
1585                         ((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
1586
1587                   sess = (gtpu_pdu_session_t *) (((char *) hdr0) +
1588                                                  sizeof (ip6_gtpu_header_t) +
1589                                                  sizeof (gtpu_exthdr_t));
1590                   sess->exthdrlen = 1;
1591                   sess->type = type;
1592                   sess->spare = 0;
1593                   sess->u.val = qfi;
1594                   sess->nextexthdr = 0;
1595                 }
1596
1597               hdr0->udp.length = clib_host_to_net_u16 (
1598                 len0 + sizeof (udp_header_t) + sizeof (gtpu_header_t));
1599
1600               clib_memcpy_fast (hdr0->ip6.src_address.as_u8, src0.as_u8,
1601                                 sizeof (ip6_address_t));
1602               clib_memcpy_fast (hdr0->ip6.dst_address.as_u8, &seg0.as_u8,
1603                                 sizeof (ip6_address_t));
1604
1605               hdr0->ip6.payload_length = clib_host_to_net_u16 (
1606                 len0 + sizeof (udp_header_t) + sizeof (gtpu_header_t));
1607
1608               // UDP source port.
1609               key = hash_memory (p, plen < 40 ? plen : 40, 0);
1610               port = hash_uword_to_u16 (&key);
1611               hdr0->udp.src_port = port;
1612
1613               vnet_buffer (b0)->sw_if_index[VLIB_TX] = ls_param->fib6_index;
1614
1615               good_n++;
1616
1617               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1618                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1619                 {
1620                   srv6_end_rewrite_trace_t *tr =
1621                     vlib_add_trace (vm, node, b0, sizeof (*tr));
1622                   clib_memcpy (tr->src.as_u8, hdr0->ip6.src_address.as_u8,
1623                                sizeof (ip6_address_t));
1624                   clib_memcpy (tr->dst.as_u8, hdr0->ip6.dst_address.as_u8,
1625                                sizeof (ip6_address_t));
1626                   tr->teid = hdr0->gtpu.teid;
1627                 }
1628             }
1629
1630           vlib_increment_combined_counter (
1631             ((next0 == SRV6_END_M_GTP6_E_NEXT_DROP) ?
1632                &(sm2->sr_ls_invalid_counters) :
1633                &(sm2->sr_ls_valid_counters)),
1634             thread_index, ls0 - sm2->localsids, 1,
1635             vlib_buffer_length_in_chain (vm, b0));
1636
1637           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1638                                            n_left_to_next, bi0, next0);
1639         }
1640
1641       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1642     }
1643
1644   vlib_node_increment_counter (vm, sm->end_m_gtp6_e_node_index,
1645                                SRV6_END_ERROR_M_GTP6_E_BAD_PACKETS, bad_n);
1646
1647   vlib_node_increment_counter (vm, sm->end_m_gtp6_e_node_index,
1648                                SRV6_END_ERROR_M_GTP6_E_PACKETS, good_n);
1649
1650   return frame->n_vectors;
1651 }
1652
1653 // Function for SRv6 GTP6.D function
1654 static inline u32
1655 srv6_gtp6_decap_processing (vlib_main_t *vm, vlib_node_runtime_t *node,
1656                             vlib_buffer_t *b0)
1657 {
1658   srv6_end_main_v6_decap_t *sm = &srv6_end_main_v6_decap;
1659   ip6_sr_main_t *sm2 = &sr_main;
1660
1661   ip6_sr_localsid_t *ls0;
1662   srv6_end_gtp6_d_param_t *ls_param;
1663
1664   ip6_gtpu_header_t *hdr0 = NULL;
1665   uword len0;
1666
1667   ip6_address_t seg0, src0, dst0;
1668   u32 teid = 0;
1669   u8 *teidp;
1670   u8 gtpu_type = 0;
1671   u8 qfi;
1672   u8 *qfip = NULL;
1673   u16 seq = 0;
1674   u8 *seqp;
1675   u32 offset, shift;
1676   u32 hdrlen;
1677   ip6_header_t *encap = NULL;
1678   gtpu_pdu_session_t *sess = NULL;
1679   int ie_size = 0;
1680   u16 tlv_siz = 0;
1681   u8 ie_buf[GTPU_IE_MAX_SIZ];
1682
1683   u32 next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP6;
1684
1685   ls0 = pool_elt_at_index (sm2->localsids,
1686                            vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1687
1688   ls_param = (srv6_end_gtp6_d_param_t *) ls0->plugin_mem;
1689
1690   hdr0 = vlib_buffer_get_current (b0);
1691
1692   hdrlen = sizeof (ip6_gtpu_header_t);
1693
1694   len0 = vlib_buffer_length_in_chain (vm, b0);
1695
1696   if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP) ||
1697       (hdr0->udp.dst_port != clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) ||
1698       (len0 < sizeof (ip6_gtpu_header_t)))
1699     {
1700       next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1701     }
1702   else
1703     {
1704       seg0 = ls_param->sr_prefix;
1705       src0 = hdr0->ip6.src_address;
1706       dst0 = hdr0->ip6.dst_address;
1707
1708       gtpu_type = hdr0->gtpu.type;
1709
1710       teid = hdr0->gtpu.teid;
1711       teidp = (u8 *) &teid;
1712
1713       seqp = (u8 *) &seq;
1714
1715       if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
1716         {
1717           // Extention header.
1718           hdrlen += sizeof (gtpu_exthdr_t);
1719
1720           seq = hdr0->gtpu.ext->seq;
1721
1722           if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
1723             {
1724               // PDU Session Container.
1725               sess = (gtpu_pdu_session_t *) (((char *) hdr0) +
1726                                              sizeof (ip6_gtpu_header_t) +
1727                                              sizeof (gtpu_exthdr_t));
1728               qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
1729               qfip = (u8 *) &qfi;
1730
1731               hdrlen += sizeof (gtpu_pdu_session_t);
1732
1733               if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
1734                 {
1735                   hdrlen += sizeof (gtpu_paging_policy_t);
1736                 }
1737             }
1738         }
1739
1740       offset = ls_param->sr_prefixlen / 8;
1741       shift = ls_param->sr_prefixlen % 8;
1742
1743       if (PREDICT_TRUE (shift == 0))
1744         {
1745           if (qfip)
1746             {
1747               qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
1748                     ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
1749
1750               if (sess->type)
1751                 {
1752                   qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
1753                 }
1754
1755               seg0.as_u8[offset] = qfi;
1756             }
1757
1758           if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
1759               gtpu_type == GTPU_TYPE_ECHO_REPLY ||
1760               gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1761             {
1762               clib_memcpy_fast (&seg0.as_u8[offset + 1], seqp, 2);
1763             }
1764           else
1765             {
1766               clib_memcpy_fast (&seg0.as_u8[offset + 1], teidp, 4);
1767             }
1768         }
1769       else
1770         {
1771           int idx;
1772
1773           if (qfip)
1774             {
1775               qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
1776                     ((qfi & ~GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
1777
1778               if (sess->type)
1779                 {
1780                   qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
1781                 }
1782
1783               seg0.as_u8[offset] |= qfi >> shift;
1784               seg0.as_u8[offset + 1] |= qfi << (8 - shift);
1785             }
1786
1787           if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
1788               gtpu_type == GTPU_TYPE_ECHO_REPLY ||
1789               gtpu_type == GTPU_TYPE_ERROR_INDICATION)
1790             {
1791               for (idx = 0; idx < 2; idx++)
1792                 {
1793                   seg0.as_u8[offset + idx + 1] |= seqp[idx] >> shift;
1794                   seg0.as_u8[offset + idx + 2] |= seqp[idx] << (8 - shift);
1795                 }
1796             }
1797           else
1798             {
1799               for (idx = 0; idx < 4; idx++)
1800                 {
1801                   seg0.as_u8[offset + idx + 1] |= teidp[idx] >> shift;
1802                   seg0.as_u8[offset + idx + 2] |= teidp[idx] << (8 - shift);
1803                 }
1804             }
1805         }
1806
1807       if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
1808         {
1809           u16 payload_len;
1810
1811           payload_len = clib_net_to_host_u16 (hdr0->gtpu.length);
1812           if (payload_len != 0)
1813             {
1814               ie_size = payload_len - (hdrlen - sizeof (ip6_gtpu_header_t));
1815               if (ie_size > 0)
1816                 {
1817                   u8 *ies;
1818
1819                   ies = (u8 *) ((u8 *) hdr0 + hdrlen);
1820                   clib_memcpy_fast (ie_buf, ies, ie_size);
1821                   hdrlen += ie_size;
1822                 }
1823             }
1824         }
1825
1826       // jump over variable length data
1827       vlib_buffer_advance (b0, (word) hdrlen);
1828
1829       // get length of encapsulated IPv6 packet (the remaining part)
1830       len0 = vlib_buffer_length_in_chain (vm, b0);
1831
1832       if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
1833         {
1834           encap = vlib_buffer_get_current (b0);
1835         }
1836
1837       uword *p;
1838       ip6srv_combo_header_t *ip6srv;
1839       ip6_sr_policy_t *sr_policy = NULL;
1840       ip6_sr_sl_t *sl = NULL;
1841       u32 *sl_index;
1842       u32 hdr_len;
1843
1844       p = mhash_get (&sm2->sr_policies_index_hash, &ls_param->sr_prefix);
1845       if (p)
1846         {
1847           sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
1848         }
1849
1850       if (sr_policy)
1851         {
1852           vec_foreach (sl_index, sr_policy->segments_lists)
1853             {
1854               sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
1855               if (sl != NULL)
1856                 break;
1857             }
1858         }
1859
1860       if (sl)
1861         {
1862           hdr_len = sizeof (ip6srv_combo_header_t);
1863           hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
1864           hdr_len += sizeof (ip6_address_t) * 2;
1865         }
1866       else
1867         {
1868           hdr_len = sizeof (ip6_header_t);
1869           hdr_len += sizeof (ip6_sr_header_t);
1870           hdr_len += sizeof (ip6_address_t) * 2;
1871         }
1872
1873       if (ie_size)
1874         {
1875           tlv_siz =
1876             sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) + ie_size;
1877
1878           tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0);
1879           hdr_len += tlv_siz;
1880         }
1881
1882       // jump back to data[0] or pre_data if required
1883       vlib_buffer_advance (b0, -(word) hdr_len);
1884
1885       ip6srv = vlib_buffer_get_current (b0);
1886
1887       if (sl)
1888         {
1889           clib_memcpy_fast (ip6srv, sl->rewrite, vec_len (sl->rewrite));
1890
1891           if (vec_len (sl->segments) > 1)
1892             {
1893               ip6srv->ip.src_address = src0;
1894
1895               ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1896
1897               ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
1898               ip6srv->sr.segments_left += 2;
1899               ip6srv->sr.last_entry += 2;
1900
1901               ip6srv->sr.length += (sizeof (ip6_address_t) * 2) / 8;
1902               ip6srv->sr.segments[0] = dst0;
1903               ip6srv->sr.segments[1] = seg0;
1904
1905               clib_memcpy_fast (&ip6srv->sr.segments[2],
1906                                 (u8 *) (sl->rewrite + sizeof (ip6_header_t) +
1907                                         sizeof (ip6_sr_header_t)),
1908                                 vec_len (sl->segments) *
1909                                   sizeof (ip6_address_t));
1910             }
1911           else
1912             {
1913               ip6srv->ip.src_address = src0;
1914               ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
1915
1916               ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
1917               ip6srv->sr.segments_left = 2;
1918               ip6srv->sr.last_entry = 1;
1919               ip6srv->sr.length = (sizeof (ip6_address_t) * 2) / 8;
1920               ip6srv->sr.flags = 0;
1921
1922               ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1923
1924               ip6srv->sr.segments[0] = dst0;
1925               ip6srv->sr.segments[1] = seg0;
1926
1927               if (vec_len (sl->segments))
1928                 {
1929                   ip6srv->sr.segments[2] = sl->segments[0];
1930                   ip6srv->sr.last_entry++;
1931                   ip6srv->sr.length += sizeof (ip6_address_t) / 8;
1932                 }
1933             }
1934
1935           if (PREDICT_TRUE (encap != NULL))
1936             {
1937               if (ls_param->nhtype == SRV6_NHTYPE_NONE)
1938                 {
1939                   if ((clib_net_to_host_u32 (
1940                          encap->ip_version_traffic_class_and_flow_label) >>
1941                        28) == 6)
1942                     ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1943                   else
1944                     ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1945                 }
1946               else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
1947                 {
1948                   ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1949                   if ((clib_net_to_host_u32 (
1950                          encap->ip_version_traffic_class_and_flow_label) >>
1951                        28) != 4)
1952                     {
1953                       // Bad encap packet.
1954                       next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1955                       goto DONE;
1956                     }
1957                 }
1958               else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
1959                 {
1960                   ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1961                   if ((clib_net_to_host_u32 (
1962                          encap->ip_version_traffic_class_and_flow_label) >>
1963                        28) != 6)
1964                     {
1965                       // Bad encap packet.
1966                       next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1967                       goto DONE;
1968                     }
1969                 }
1970               else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1971                 {
1972                   ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
1973                 }
1974             }
1975           else
1976             {
1977               ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
1978             }
1979         }
1980       else
1981         {
1982           clib_memcpy_fast (ip6srv, &sm->cache_hdr, sizeof (ip6_header_t));
1983
1984           ip6srv->ip.src_address = src0;
1985           ip6srv->ip.dst_address = seg0;
1986
1987           ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
1988
1989           ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
1990           ip6srv->sr.segments_left = 1;
1991           ip6srv->sr.last_entry = 1;
1992
1993           ip6srv->sr.length = (sizeof (ip6_address_t) * 2) / 8;
1994           ip6srv->sr.segments[0] = dst0;
1995           ip6srv->sr.segments[1] = seg0;
1996
1997           if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
1998             {
1999               ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
2000               ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
2001             }
2002           else
2003             {
2004               if (ls_param->nhtype == SRV6_NHTYPE_NONE)
2005                 {
2006                   if ((clib_net_to_host_u32 (
2007                          encap->ip_version_traffic_class_and_flow_label) >>
2008                        28) != 6)
2009                     ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
2010                 }
2011               else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
2012                 {
2013                   ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
2014                   if ((clib_net_to_host_u32 (
2015                          encap->ip_version_traffic_class_and_flow_label) >>
2016                        28) != 4)
2017                     {
2018                       // Bad encap packet.
2019                       next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
2020                       goto DONE;
2021                     }
2022                 }
2023               else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
2024                 {
2025                   ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
2026                   if ((clib_net_to_host_u32 (
2027                          encap->ip_version_traffic_class_and_flow_label) >>
2028                        28) != 6)
2029                     {
2030                       // Bad encap packet.
2031                       next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
2032                       goto DONE;
2033                     }
2034                 }
2035               else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
2036                 {
2037                   ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
2038                 }
2039             }
2040         }
2041
2042       if (PREDICT_FALSE (ie_size))
2043         {
2044           ip6_sr_tlv_t *tlv;
2045           user_plane_sub_tlv_t *sub_tlv;
2046
2047           tlv = (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
2048           tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
2049           tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t));
2050           clib_memset (tlv->value, 0, tlv->length);
2051
2052           sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
2053           sub_tlv->type = USER_PLANE_SUB_TLV_IE;
2054           sub_tlv->length = (u8) ie_size;
2055           clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
2056
2057           ip6srv->sr.length += (u8) (tlv_siz / 8);
2058         }
2059
2060       ip6srv->ip.payload_length =
2061         clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
2062
2063       vnet_buffer (b0)->sw_if_index[VLIB_TX] = 0; /* default FIB */
2064
2065       if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
2066           PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
2067         {
2068           srv6_end_rewrite_trace_t *tr =
2069             vlib_add_trace (vm, node, b0, sizeof (*tr));
2070           clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
2071                        sizeof (ip6_address_t));
2072           clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
2073                        sizeof (ip6_address_t));
2074           tr->teid = teid;
2075           clib_memcpy (tr->sr_prefix.as_u8, ls_param->sr_prefix.as_u8,
2076                        sizeof (ip6_address_t));
2077           tr->sr_prefixlen = ls_param->sr_prefixlen;
2078         }
2079     }
2080
2081 DONE:
2082   return next0;
2083 }
2084
2085 VLIB_NODE_FN (srv6_end_m_gtp6_d)
2086 (vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
2087 {
2088   srv6_end_main_v6_decap_t *sm = &srv6_end_main_v6_decap;
2089   ip6_sr_main_t *sm2 = &sr_main;
2090   u32 n_left_from, next_index, *from, *to_next;
2091   u32 thread_index = vm->thread_index;
2092   ip6_sr_localsid_t *ls0;
2093   srv6_end_gtp6_d_param_t *ls_param;
2094
2095   u32 good_n = 0, bad_n = 0;
2096
2097   from = vlib_frame_vector_args (frame);
2098   n_left_from = frame->n_vectors;
2099   next_index = node->cached_next_index;
2100
2101   while (n_left_from > 0)
2102     {
2103       u32 n_left_to_next;
2104
2105       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
2106
2107       while (n_left_from > 0 && n_left_to_next > 0)
2108         {
2109           u32 bi0;
2110           vlib_buffer_t *b0;
2111
2112           u32 next0;
2113
2114           ip6_gtpu_header_t *hdr;
2115           u32 hdrlen;
2116           u8 gtpu_type;
2117           bool gtp6;
2118           bool ipv4;
2119
2120           // defaults
2121           bi0 = from[0];
2122           to_next[0] = bi0;
2123           from += 1;
2124           to_next += 1;
2125           n_left_from -= 1;
2126           n_left_to_next -= 1;
2127
2128           b0 = vlib_get_buffer (vm, bi0);
2129
2130           ls0 = pool_elt_at_index (sm2->localsids,
2131                                    vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2132
2133           ls_param = (srv6_end_gtp6_d_param_t *) ls0->plugin_mem;
2134
2135           hdr = vlib_buffer_get_current (b0);
2136           gtpu_type = hdr->gtpu.type;
2137
2138           gtp6 = false;
2139           ipv4 = true;
2140
2141           if (PREDICT_FALSE (gtpu_type != GTPU_TYPE_GTPU || ls_param->drop_in))
2142             {
2143               gtp6 = true;
2144             }
2145           else
2146             {
2147               ip6_header_t *ip6;
2148
2149               hdrlen = sizeof (ip6_gtpu_header_t);
2150
2151               if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
2152                 {
2153                   hdrlen += sizeof (gtpu_exthdr_t);
2154                   if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
2155                     {
2156                       gtpu_pdu_session_t *sess;
2157                       sess = (gtpu_pdu_session_t *) (((char *) hdr) + hdrlen);
2158                       hdrlen += sizeof (gtpu_pdu_session_t);
2159
2160                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
2161                         {
2162                           hdrlen += sizeof (gtpu_paging_policy_t);
2163                         }
2164                     }
2165                 }
2166
2167               ip6 = (ip6_header_t *) (((char *) hdr) + hdrlen);
2168               if ((clib_net_to_host_u32 (
2169                      ip6->ip_version_traffic_class_and_flow_label) >>
2170                    28) == 6)
2171                 {
2172                   ipv4 = false;
2173                   if (((ip6->dst_address.as_u8[0] == 0xff) &&
2174                        (ip6->dst_address.as_u8[1] == 0x02)) ||
2175                       ((ip6->dst_address.as_u8[0] == 0xfe) &&
2176                        ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
2177                     {
2178                       // Inner desitnation is IPv6 link local
2179                       gtp6 = true;
2180                     }
2181                 }
2182             }
2183
2184           if (gtp6)
2185             {
2186               next0 = srv6_gtp6_decap_processing (vm, node, b0);
2187               if (PREDICT_TRUE (next0 == SRV6_END_M_GTP6_D_NEXT_LOOKUP6))
2188                 good_n++;
2189               else
2190                 bad_n++;
2191
2192               vlib_increment_combined_counter (
2193                 ((next0 == SRV6_END_M_GTP6_D_NEXT_DROP) ?
2194                    &(sm2->sr_ls_invalid_counters) :
2195                    &(sm2->sr_ls_valid_counters)),
2196                 thread_index, ls0 - sm2->localsids, 1,
2197                 vlib_buffer_length_in_chain (vm, b0));
2198             }
2199           else
2200             {
2201               /* Strip off the outer header (IPv6 + GTP + UDP + IEs) */
2202               vlib_buffer_advance (b0, (word) hdrlen);
2203
2204               if (ipv4)
2205                 {
2206                   next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP4;
2207                   vnet_buffer (b0)->sw_if_index[VLIB_TX] =
2208                     ls_param->fib4_index;
2209                 }
2210               else
2211                 {
2212                   next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP6;
2213                   vnet_buffer (b0)->sw_if_index[VLIB_TX] =
2214                     ls_param->fib6_index;
2215                 }
2216             }
2217
2218           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
2219                                            n_left_to_next, bi0, next0);
2220         }
2221
2222       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
2223     }
2224
2225   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_node_index,
2226                                SRV6_END_ERROR_M_GTP6_D_BAD_PACKETS, bad_n);
2227
2228   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_node_index,
2229                                SRV6_END_ERROR_M_GTP6_D_PACKETS, good_n);
2230
2231   return frame->n_vectors;
2232 }
2233
2234 // Function for SRv6 GTP6.D.DI function
2235 VLIB_NODE_FN (srv6_end_m_gtp6_d_di)
2236 (vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
2237 {
2238   srv6_end_main_v6_decap_di_t *sm = &srv6_end_main_v6_decap_di;
2239   ip6_sr_main_t *sm2 = &sr_main;
2240   u32 n_left_from, next_index, *from, *to_next;
2241   u32 thread_index = vm->thread_index;
2242   srv6_end_gtp6_d_param_t *ls_param;
2243
2244   u32 good_n = 0, bad_n = 0;
2245
2246   from = vlib_frame_vector_args (frame);
2247   n_left_from = frame->n_vectors;
2248   next_index = node->cached_next_index;
2249
2250   while (n_left_from > 0)
2251     {
2252       u32 n_left_to_next;
2253
2254       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
2255
2256       while (n_left_from > 0 && n_left_to_next > 0)
2257         {
2258           u32 bi0;
2259           vlib_buffer_t *b0;
2260           ip6_sr_localsid_t *ls0;
2261
2262           ip6_gtpu_header_t *hdr0 = NULL;
2263           uword len0;
2264
2265           ip6_address_t dst0;
2266           ip6_address_t src0;
2267           ip6_address_t seg0;
2268           u32 teid = 0;
2269           u8 *teidp;
2270           u8 gtpu_type = 0;
2271           u8 qfi = 0;
2272           u8 *qfip = NULL;
2273           u16 seq = 0;
2274           u8 *seqp;
2275           u32 offset, shift;
2276           u32 hdrlen;
2277           ip6_header_t *encap = NULL;
2278           gtpu_pdu_session_t *sess;
2279           int ie_size = 0;
2280           u16 tlv_siz = 0;
2281           u8 ie_buf[GTPU_IE_MAX_SIZ];
2282
2283           u32 next0 = SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP;
2284
2285           // defaults
2286           bi0 = from[0];
2287           to_next[0] = bi0;
2288           from += 1;
2289           to_next += 1;
2290           n_left_from -= 1;
2291           n_left_to_next -= 1;
2292
2293           b0 = vlib_get_buffer (vm, bi0);
2294           ls0 = pool_elt_at_index (sm2->localsids,
2295                                    vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2296
2297           ls_param = (srv6_end_gtp6_d_param_t *) ls0->plugin_mem;
2298
2299           hdr0 = vlib_buffer_get_current (b0);
2300
2301           hdrlen = sizeof (ip6_gtpu_header_t);
2302
2303           len0 = vlib_buffer_length_in_chain (vm, b0);
2304
2305           if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP) ||
2306               (hdr0->udp.dst_port !=
2307                clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) ||
2308               (len0 < sizeof (ip6_gtpu_header_t)))
2309             {
2310               next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
2311
2312               bad_n++;
2313             }
2314           else
2315             {
2316               dst0 = hdr0->ip6.dst_address;
2317               src0 = hdr0->ip6.src_address;
2318
2319               gtpu_type = hdr0->gtpu.type;
2320
2321               seg0 = ls_param->sr_prefix;
2322               teid = hdr0->gtpu.teid;
2323               teidp = (u8 *) &teid;
2324
2325               seqp = (u8 *) &seq;
2326
2327               if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG))
2328                 {
2329                   // Extention header.
2330                   hdrlen += sizeof (gtpu_exthdr_t);
2331
2332                   seq = hdr0->gtpu.ext->seq;
2333
2334                   if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
2335                     {
2336                       // PDU Session Container.
2337                       sess = (gtpu_pdu_session_t *) (((char *) hdr0) + hdrlen);
2338                       qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
2339                       qfip = &qfi;
2340
2341                       hdrlen += sizeof (gtpu_pdu_session_t);
2342
2343                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
2344                         {
2345                           hdrlen += sizeof (gtpu_paging_policy_t);
2346                         }
2347                     }
2348                 }
2349
2350               offset = ls_param->sr_prefixlen / 8;
2351               shift = ls_param->sr_prefixlen % 8;
2352
2353               offset += 1;
2354               if (PREDICT_TRUE (shift == 0))
2355                 {
2356                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
2357                       gtpu_type == GTPU_TYPE_ECHO_REPLY ||
2358                       gtpu_type == GTPU_TYPE_ERROR_INDICATION)
2359                     {
2360                       clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
2361                     }
2362                   else
2363                     {
2364                       clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
2365                     }
2366
2367                   if (qfip)
2368                     {
2369                       qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
2370                             ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
2371
2372                       if (sess->type)
2373                         {
2374                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
2375                         }
2376
2377                       seg0.as_u8[offset + 4] = qfi;
2378                     }
2379                 }
2380               else
2381                 {
2382                   int idx;
2383
2384                   if (gtpu_type == GTPU_TYPE_ECHO_REQUEST ||
2385                       gtpu_type == GTPU_TYPE_ECHO_REPLY ||
2386                       gtpu_type == GTPU_TYPE_ERROR_INDICATION)
2387                     {
2388                       for (idx = 0; idx < 2; idx++)
2389                         {
2390                           seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
2391                           seg0.as_u8[offset + idx + 1] |= seqp[idx]
2392                                                           << (8 - shift);
2393                         }
2394                     }
2395                   else
2396                     {
2397                       for (idx = 0; idx < 4; idx++)
2398                         {
2399                           seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
2400                           seg0.as_u8[offset + idx + 1] |= teidp[idx]
2401                                                           << (8 - shift);
2402                         }
2403                     }
2404
2405                   if (qfip)
2406                     {
2407                       qfi = ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
2408                             ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
2409
2410                       if (sess->type)
2411                         {
2412                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
2413                         }
2414
2415                       seg0.as_u8[offset + 4] |= qfi >> shift;
2416                       seg0.as_u8[offset + 5] |= qfi << (8 - shift);
2417                     }
2418                 }
2419
2420               if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION))
2421                 {
2422                   u16 payload_len;
2423
2424                   payload_len = clib_net_to_host_u16 (hdr0->gtpu.length);
2425                   if (payload_len != 0)
2426                     {
2427                       ie_size =
2428                         payload_len - (hdrlen - sizeof (ip6_gtpu_header_t));
2429                       if (ie_size > 0)
2430                         {
2431                           u8 *ies;
2432
2433                           ies = (u8 *) ((u8 *) hdr0 + hdrlen);
2434                           clib_memcpy_fast (ie_buf, ies, ie_size);
2435                           hdrlen += ie_size;
2436                         }
2437                     }
2438                 }
2439
2440               // jump over variable length data
2441               vlib_buffer_advance (b0, (word) hdrlen);
2442
2443               // get length of encapsulated IPv6 packet (the remaining part)
2444               len0 = vlib_buffer_length_in_chain (vm, b0);
2445
2446               if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
2447                 {
2448                   encap = vlib_buffer_get_current (b0);
2449                 }
2450
2451               uword *p;
2452               ip6srv_combo_header_t *ip6srv;
2453               ip6_sr_policy_t *sr_policy = NULL;
2454               ip6_sr_sl_t *sl = NULL;
2455               u32 *sl_index;
2456               u32 hdr_len;
2457
2458               p =
2459                 mhash_get (&sm2->sr_policies_index_hash, &ls_param->sr_prefix);
2460               if (p)
2461                 {
2462                   sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
2463                 }
2464
2465               if (sr_policy)
2466                 {
2467                   vec_foreach (sl_index, sr_policy->segments_lists)
2468                     {
2469                       sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
2470                       if (sl != NULL)
2471                         break;
2472                     }
2473                 }
2474
2475               hdr_len = sizeof (ip6srv_combo_header_t);
2476
2477               if (sl)
2478                 hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
2479
2480               hdr_len += sizeof (ip6_address_t) * 2;
2481
2482               if (ie_size)
2483                 {
2484                   tlv_siz = sizeof (ip6_sr_tlv_t) +
2485                             sizeof (user_plane_sub_tlv_t) + ie_size;
2486
2487                   tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0);
2488                   hdr_len += tlv_siz;
2489                 }
2490
2491               // jump back to data[0] or pre_data if required
2492               vlib_buffer_advance (b0, -(word) hdr_len);
2493
2494               ip6srv = vlib_buffer_get_current (b0);
2495
2496               if (sl)
2497                 {
2498                   clib_memcpy_fast (ip6srv, sl->rewrite,
2499                                     vec_len (sl->rewrite));
2500
2501                   if (vec_len (sl->segments) > 1)
2502                     {
2503                       ip6srv->ip.src_address = src0;
2504
2505                       ip6srv->sr.tag =
2506                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
2507
2508                       ip6srv->sr.segments_left += 2;
2509                       ip6srv->sr.last_entry += 2;
2510
2511                       ip6srv->sr.length += ((sizeof (ip6_address_t) * 2) / 8);
2512
2513                       ip6srv->sr.segments[0] = dst0;
2514                       ip6srv->sr.segments[1] = seg0;
2515
2516                       clib_memcpy_fast (
2517                         &ip6srv->sr.segments[2],
2518                         (u8 *) (sl->rewrite + sizeof (ip6_header_t) +
2519                                 sizeof (ip6_sr_header_t)),
2520                         vec_len (sl->segments) * sizeof (ip6_address_t));
2521                     }
2522                   else
2523                     {
2524                       ip6srv->ip.src_address = src0;
2525                       ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
2526
2527                       ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
2528                       ip6srv->sr.segments_left = 2;
2529                       ip6srv->sr.last_entry = 1;
2530                       ip6srv->sr.length = ((sizeof (ip6_sr_header_t) +
2531                                             2 * sizeof (ip6_address_t)) /
2532                                            8) -
2533                                           1;
2534                       ip6srv->sr.flags = 0;
2535
2536                       ip6srv->sr.tag =
2537                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
2538
2539                       ip6srv->sr.segments[0] = dst0;
2540                       ip6srv->sr.segments[1] = seg0;
2541
2542                       if (vec_len (sl->segments))
2543                         {
2544                           ip6srv->sr.segments[2] = sl->segments[0];
2545                           ip6srv->sr.last_entry++;
2546                           ip6srv->sr.length += sizeof (ip6_address_t) / 8;
2547                         }
2548                     }
2549                 }
2550               else
2551                 {
2552                   clib_memcpy_fast (ip6srv, &sm->cache_hdr,
2553                                     sizeof (ip6_header_t));
2554
2555                   ip6srv->ip.src_address = src0;
2556                   ip6srv->ip.dst_address = seg0;
2557
2558                   ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
2559                   ip6srv->sr.segments_left = 1;
2560                   ip6srv->sr.last_entry = 0;
2561                   ip6srv->sr.length =
2562                     ((sizeof (ip6_sr_header_t) + sizeof (ip6_address_t)) / 8) -
2563                     1;
2564                   ip6srv->sr.flags = 0;
2565
2566                   ip6srv->sr.tag =
2567                     clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
2568
2569                   ip6srv->sr.segments[0] = dst0;
2570                 }
2571
2572               if (PREDICT_FALSE (ie_size))
2573                 {
2574                   ip6_sr_tlv_t *tlv;
2575                   user_plane_sub_tlv_t *sub_tlv;
2576
2577                   tlv = (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz));
2578                   tlv->type = SRH_TLV_USER_PLANE_CONTAINER;
2579                   tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t));
2580                   clib_memset (tlv->value, 0, tlv->length);
2581
2582                   sub_tlv = (user_plane_sub_tlv_t *) tlv->value;
2583                   sub_tlv->length = (u8) (ie_size);
2584                   clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size);
2585
2586                   ip6srv->sr.length += (u8) (tlv_siz / 8);
2587                 }
2588
2589               ip6srv->ip.payload_length =
2590                 clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
2591               ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
2592
2593               if (PREDICT_TRUE (encap != NULL))
2594                 {
2595                   if (ls_param->nhtype == SRV6_NHTYPE_NONE)
2596                     {
2597                       if ((clib_net_to_host_u32 (
2598                              encap->ip_version_traffic_class_and_flow_label) >>
2599                            28) == 6)
2600                         ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
2601                       else
2602                         ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
2603                     }
2604                   else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
2605                     {
2606                       ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
2607                       if ((clib_net_to_host_u32 (
2608                              encap->ip_version_traffic_class_and_flow_label) >>
2609                            28) != 4)
2610                         {
2611                           // Bad encap packet.
2612                           next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
2613                           bad_n++;
2614                           goto DONE;
2615                         }
2616                     }
2617                   else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
2618                     {
2619                       ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
2620                       if ((clib_net_to_host_u32 (
2621                              encap->ip_version_traffic_class_and_flow_label) >>
2622                            28) != 6)
2623                         {
2624                           // Bad encap packet.
2625                           next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
2626                           bad_n++;
2627                           goto DONE;
2628                         }
2629                     }
2630                   else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
2631                     {
2632                       ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
2633                     }
2634                 }
2635               else
2636                 {
2637                   ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET;
2638                 }
2639
2640               good_n++;
2641
2642               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
2643                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
2644                 {
2645                   srv6_end_rewrite_trace_t *tr =
2646                     vlib_add_trace (vm, node, b0, sizeof (*tr));
2647                   clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
2648                                sizeof (ip6_address_t));
2649                   clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
2650                                sizeof (ip6_address_t));
2651                   tr->teid = teid;
2652                   clib_memcpy (tr->sr_prefix.as_u8, ls_param->sr_prefix.as_u8,
2653                                sizeof (ip6_address_t));
2654                   tr->sr_prefixlen = ls_param->sr_prefixlen;
2655                 }
2656             }
2657
2658         DONE:
2659           vlib_increment_combined_counter (
2660             ((next0 == SRV6_END_M_GTP6_D_DI_NEXT_DROP) ?
2661                &(sm2->sr_ls_invalid_counters) :
2662                &(sm2->sr_ls_valid_counters)),
2663             thread_index, ls0 - sm2->localsids, 1,
2664             vlib_buffer_length_in_chain (vm, b0));
2665
2666           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
2667                                            n_left_to_next, bi0, next0);
2668         }
2669
2670       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
2671     }
2672
2673   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_di_node_index,
2674                                SRV6_END_ERROR_M_GTP6_D_DI_BAD_PACKETS, bad_n);
2675
2676   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_di_node_index,
2677                                SRV6_END_ERROR_M_GTP6_D_DI_PACKETS, good_n);
2678
2679   return frame->n_vectors;
2680 }
2681
2682 // Function for SRv6 GTP6.DT function
2683 VLIB_NODE_FN (srv6_end_m_gtp6_dt)
2684 (vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
2685 {
2686   srv6_end_main_v6_dt_t *sm = &srv6_end_main_v6_dt;
2687   ip6_sr_main_t *sm2 = &sr_main;
2688   u32 n_left_from, next_index, *from, *to_next;
2689   u32 thread_index = vm->thread_index;
2690
2691   u32 good_n = 0, bad_n = 0;
2692
2693   from = vlib_frame_vector_args (frame);
2694   n_left_from = frame->n_vectors;
2695   next_index = node->cached_next_index;
2696
2697   while (n_left_from > 0)
2698     {
2699       u32 n_left_to_next;
2700
2701       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
2702
2703       while (n_left_from > 0 && n_left_to_next > 0)
2704         {
2705           u32 bi0;
2706           vlib_buffer_t *b0;
2707           srv6_end_gtp6_dt_param_t *ls_param;
2708           ip6_sr_localsid_t *ls0;
2709
2710           ip6_gtpu_header_t *hdr0 = NULL;
2711           ip4_header_t *ip4 = NULL;
2712           ip6_header_t *ip6 = NULL;
2713           ip6_address_t src, dst;
2714           u32 teid;
2715           u32 hdrlen;
2716           u32 len0;
2717
2718           u32 next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
2719
2720           bi0 = from[0];
2721           to_next[0] = bi0;
2722           from += 1;
2723           to_next += 1;
2724           n_left_from -= 1;
2725           n_left_to_next -= 1;
2726
2727           b0 = vlib_get_buffer (vm, bi0);
2728           ls0 = pool_elt_at_index (sm2->localsids,
2729                                    vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2730
2731           ls_param = (srv6_end_gtp6_dt_param_t *) ls0->plugin_mem;
2732
2733           hdr0 = vlib_buffer_get_current (b0);
2734
2735           hdrlen = sizeof (ip6_gtpu_header_t);
2736
2737           len0 = vlib_buffer_length_in_chain (vm, b0);
2738
2739           if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP) ||
2740               (hdr0->udp.dst_port !=
2741                clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) ||
2742               (len0 < sizeof (ip6_gtpu_header_t)))
2743             {
2744               next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
2745
2746               bad_n++;
2747             }
2748           else
2749             {
2750               clib_memcpy_fast (src.as_u8, hdr0->ip6.src_address.as_u8,
2751                                 sizeof (ip6_address_t));
2752               clib_memcpy_fast (dst.as_u8, hdr0->ip6.dst_address.as_u8,
2753                                 sizeof (ip6_address_t));
2754
2755               teid = hdr0->gtpu.teid;
2756
2757               if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
2758                 {
2759                   hdrlen += sizeof (gtpu_exthdr_t);
2760                   if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
2761                     {
2762                       gtpu_pdu_session_t *sess;
2763
2764                       sess =
2765                         (gtpu_pdu_session_t *) (((char *) hdr0) +
2766                                                 sizeof (ip6_gtpu_header_t) +
2767                                                 sizeof (gtpu_exthdr_t));
2768
2769                       hdrlen += sizeof (gtpu_pdu_session_t);
2770                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
2771                         {
2772                           hdrlen += sizeof (gtpu_paging_policy_t);
2773                         }
2774                     }
2775                 }
2776
2777               if (ls_param->type == SRV6_GTP6_DT4)
2778                 {
2779                   vlib_buffer_advance (b0, (word) hdrlen);
2780                   ip4 = vlib_buffer_get_current (b0);
2781                   if ((ip4->ip_version_and_header_length & 0xf0) != 0x40)
2782                     {
2783                       next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
2784                       bad_n++;
2785                       goto DONE;
2786                     }
2787
2788                   next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP4;
2789                   vnet_buffer (b0)->sw_if_index[VLIB_TX] =
2790                     ls_param->fib4_index;
2791                 }
2792               else if (ls_param->type == SRV6_GTP6_DT6)
2793                 {
2794                   ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen);
2795                   if ((clib_net_to_host_u32 (
2796                          ip6->ip_version_traffic_class_and_flow_label) >>
2797                        28) != 6)
2798                     {
2799                       next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
2800                       bad_n++;
2801                       goto DONE;
2802                     }
2803
2804                   next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
2805                   if (((ip6->dst_address.as_u8[0] == 0xff) &&
2806                        (ip6->dst_address.as_u8[1] == 0x02)) ||
2807                       ((ip6->dst_address.as_u8[0] == 0xfe) &&
2808                        ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
2809                     {
2810                       vnet_buffer (b0)->sw_if_index[VLIB_TX] =
2811                         ls_param->local_fib_index;
2812                     }
2813                   else
2814                     {
2815                       vlib_buffer_advance (b0, (word) hdrlen);
2816                       vnet_buffer (b0)->sw_if_index[VLIB_TX] =
2817                         ls_param->fib6_index;
2818                     }
2819                 }
2820               else if (ls_param->type == SRV6_GTP6_DT46)
2821                 {
2822                   ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen);
2823                   if ((clib_net_to_host_u32 (
2824                          ip6->ip_version_traffic_class_and_flow_label) >>
2825                        28) == 6)
2826                     {
2827                       next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
2828                       if (((ip6->dst_address.as_u8[0] == 0xff) &&
2829                            (ip6->dst_address.as_u8[1] == 0x02)) ||
2830                           ((ip6->dst_address.as_u8[0] == 0xfe) &&
2831                            ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
2832                         {
2833                           vnet_buffer (b0)->sw_if_index[VLIB_TX] =
2834                             ls_param->local_fib_index;
2835                         }
2836                       else
2837                         {
2838                           vlib_buffer_advance (b0, (word) hdrlen);
2839                           vnet_buffer (b0)->sw_if_index[VLIB_TX] =
2840                             ls_param->fib6_index;
2841                         }
2842                     }
2843                   else if ((clib_net_to_host_u32 (
2844                               ip6->ip_version_traffic_class_and_flow_label) >>
2845                             28) == 4)
2846                     {
2847                       vlib_buffer_advance (b0, (word) hdrlen);
2848                       next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP4;
2849                       vnet_buffer (b0)->sw_if_index[VLIB_TX] =
2850                         ls_param->fib4_index;
2851                     }
2852                   else
2853                     {
2854                       next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
2855                       bad_n++;
2856                       goto DONE;
2857                     }
2858                 }
2859               else
2860                 {
2861                   next0 = SRV6_END_M_GTP6_DT_NEXT_DROP;
2862                   bad_n++;
2863                   goto DONE;
2864                 }
2865
2866               good_n++;
2867
2868               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
2869                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
2870                 {
2871                   srv6_end_rewrite_trace_t *tr =
2872                     vlib_add_trace (vm, node, b0, sizeof (*tr));
2873                   clib_memcpy (tr->src.as_u8, src.as_u8,
2874                                sizeof (ip6_address_t));
2875                   clib_memcpy (tr->dst.as_u8, dst.as_u8,
2876                                sizeof (ip6_address_t));
2877                   tr->teid = teid;
2878                 }
2879             }
2880
2881         DONE:
2882           vlib_increment_combined_counter (
2883             ((next0 == SRV6_END_M_GTP6_DT_NEXT_DROP) ?
2884                &(sm2->sr_ls_invalid_counters) :
2885                &(sm2->sr_ls_valid_counters)),
2886             thread_index, ls0 - sm2->localsids, 1,
2887             vlib_buffer_length_in_chain (vm, b0));
2888
2889           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
2890                                            n_left_to_next, bi0, next0);
2891         }
2892
2893       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
2894     }
2895
2896   vlib_node_increment_counter (vm, sm->end_m_gtp6_dt_node_index,
2897                                SRV6_END_ERROR_M_GTP6_DT_BAD_PACKETS, bad_n);
2898
2899   vlib_node_increment_counter (vm, sm->end_m_gtp6_dt_node_index,
2900                                SRV6_END_ERROR_M_GTP6_DT_PACKETS, good_n);
2901
2902   return frame->n_vectors;
2903 }
2904
2905 // Function for SRv6 GTP4.DT function
2906 VLIB_NODE_FN (srv6_t_m_gtp4_dt)
2907 (vlib_main_t *vm, vlib_node_runtime_t *node, vlib_frame_t *frame)
2908 {
2909   srv6_t_main_v4_dt_t *sm = &srv6_t_main_v4_dt;
2910   ip6_sr_main_t *sm2 = &sr_main;
2911   u32 n_left_from, next_index, *from, *to_next;
2912
2913   u32 good_n = 0, bad_n = 0;
2914
2915   from = vlib_frame_vector_args (frame);
2916   n_left_from = frame->n_vectors;
2917   next_index = node->cached_next_index;
2918
2919   while (n_left_from > 0)
2920     {
2921       u32 n_left_to_next;
2922
2923       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
2924
2925       while (n_left_from > 0 && n_left_to_next > 0)
2926         {
2927           u32 bi0;
2928           vlib_buffer_t *b0;
2929           srv6_t_gtp4_dt_param_t *ls_param;
2930           ip6_sr_sl_t *ls0;
2931
2932           ip4_gtpu_header_t *hdr0 = NULL;
2933           ip4_header_t *ip4 = NULL;
2934           ip6_header_t *ip6 = NULL;
2935           ip6_address_t src, dst;
2936           u32 teid;
2937           u32 hdrlen;
2938           u32 len0;
2939
2940           u32 next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
2941
2942           bi0 = from[0];
2943           to_next[0] = bi0;
2944           from += 1;
2945           to_next += 1;
2946           n_left_from -= 1;
2947           n_left_to_next -= 1;
2948
2949           b0 = vlib_get_buffer (vm, bi0);
2950           ls0 = pool_elt_at_index (sm2->sid_lists,
2951                                    vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2952
2953           ls_param = (srv6_t_gtp4_dt_param_t *) ls0->plugin_mem;
2954
2955           hdr0 = vlib_buffer_get_current (b0);
2956
2957           hdrlen = sizeof (ip4_gtpu_header_t);
2958
2959           len0 = vlib_buffer_length_in_chain (vm, b0);
2960
2961           if ((hdr0->ip4.protocol != IP_PROTOCOL_UDP) ||
2962               (hdr0->udp.dst_port !=
2963                clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) ||
2964               (len0 < sizeof (ip4_gtpu_header_t)))
2965             {
2966               next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
2967
2968               bad_n++;
2969             }
2970           else
2971             {
2972               clib_memcpy_fast (src.as_u8, hdr0->ip4.src_address.as_u8,
2973                                 sizeof (ip4_address_t));
2974               clib_memcpy_fast (dst.as_u8, hdr0->ip4.dst_address.as_u8,
2975                                 sizeof (ip4_address_t));
2976
2977               teid = hdr0->gtpu.teid;
2978
2979               if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
2980                 {
2981                   hdrlen += sizeof (gtpu_exthdr_t);
2982                   if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
2983                     {
2984                       gtpu_pdu_session_t *sess;
2985
2986                       sess =
2987                         (gtpu_pdu_session_t *) (((char *) hdr0) +
2988                                                 sizeof (ip6_gtpu_header_t) +
2989                                                 sizeof (gtpu_exthdr_t));
2990
2991                       hdrlen += sizeof (gtpu_pdu_session_t);
2992                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
2993                         {
2994                           hdrlen += sizeof (gtpu_paging_policy_t);
2995                         }
2996                     }
2997                 }
2998
2999               if (ls_param->type == SRV6_GTP4_DT4)
3000                 {
3001                   vlib_buffer_advance (b0, (word) hdrlen);
3002                   ip4 = vlib_buffer_get_current (b0);
3003                   if ((ip4->ip_version_and_header_length & 0xf0) != 0x40)
3004                     {
3005                       next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
3006                       bad_n++;
3007                       goto DONE;
3008                     }
3009
3010                   next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
3011                   vnet_buffer (b0)->sw_if_index[VLIB_TX] =
3012                     ls_param->fib4_index;
3013                 }
3014               else if (ls_param->type == SRV6_GTP4_DT6)
3015                 {
3016                   ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen);
3017                   if ((clib_net_to_host_u32 (
3018                          ip6->ip_version_traffic_class_and_flow_label) >>
3019                        28) != 6)
3020                     {
3021                       next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
3022                       bad_n++;
3023                       goto DONE;
3024                     }
3025
3026                   next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
3027                   if (((ip6->dst_address.as_u8[0] == 0xff) &&
3028                        (ip6->dst_address.as_u8[1] == 0x02)) ||
3029                       ((ip6->dst_address.as_u8[0] == 0xfe) &&
3030                        ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
3031                     {
3032                       next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
3033                       vnet_buffer (b0)->sw_if_index[VLIB_TX] =
3034                         ls_param->local_fib_index;
3035                     }
3036                   else
3037                     {
3038                       vlib_buffer_advance (b0, (word) hdrlen);
3039                       vnet_buffer (b0)->sw_if_index[VLIB_TX] =
3040                         ls_param->fib6_index;
3041                     }
3042                 }
3043               else if (ls_param->type == SRV6_GTP4_DT46)
3044                 {
3045                   ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen);
3046                   if ((clib_net_to_host_u32 (
3047                          ip6->ip_version_traffic_class_and_flow_label) >>
3048                        28) == 6)
3049                     {
3050                       next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
3051                       if (((ip6->dst_address.as_u8[0] == 0xff) &&
3052                            (ip6->dst_address.as_u8[1] == 0x02)) ||
3053                           ((ip6->dst_address.as_u8[0] == 0xfe) &&
3054                            ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)))
3055                         {
3056                           next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
3057                           vnet_buffer (b0)->sw_if_index[VLIB_TX] =
3058                             ls_param->local_fib_index;
3059                         }
3060                       else
3061                         {
3062                           vlib_buffer_advance (b0, (word) hdrlen);
3063                           vnet_buffer (b0)->sw_if_index[VLIB_TX] =
3064                             ls_param->fib6_index;
3065                         }
3066                     }
3067                   else if ((clib_net_to_host_u32 (
3068                               ip6->ip_version_traffic_class_and_flow_label) >>
3069                             28) == 4)
3070                     {
3071                       vlib_buffer_advance (b0, (word) hdrlen);
3072                       next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
3073                       vnet_buffer (b0)->sw_if_index[VLIB_TX] =
3074                         ls_param->fib4_index;
3075                     }
3076                   else
3077                     {
3078                       next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
3079                       bad_n++;
3080                       goto DONE;
3081                     }
3082                 }
3083               else
3084                 {
3085                   next0 = SRV6_T_M_GTP4_DT_NEXT_DROP;
3086                   bad_n++;
3087                   goto DONE;
3088                 }
3089
3090               good_n++;
3091
3092               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
3093                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
3094                 {
3095                   srv6_end_rewrite_trace_t *tr =
3096                     vlib_add_trace (vm, node, b0, sizeof (*tr));
3097                   clib_memcpy (tr->src.as_u8, src.as_u8,
3098                                sizeof (ip6_address_t));
3099                   clib_memcpy (tr->dst.as_u8, dst.as_u8,
3100                                sizeof (ip6_address_t));
3101                   tr->teid = teid;
3102                 }
3103             }
3104
3105         DONE:
3106           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
3107                                            n_left_to_next, bi0, next0);
3108         }
3109
3110       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
3111     }
3112
3113   vlib_node_increment_counter (vm, sm->t_m_gtp4_dt_node_index,
3114                                SRV6_T_ERROR_M_GTP4_DT_BAD_PACKETS, bad_n);
3115
3116   vlib_node_increment_counter (vm, sm->t_m_gtp4_dt_node_index,
3117                                SRV6_T_ERROR_M_GTP4_DT_PACKETS, good_n);
3118
3119   return frame->n_vectors;
3120 }
3121
3122 VLIB_REGISTER_NODE (srv6_end_m_gtp6_e) =
3123 {
3124   .name = "srv6-end-m-gtp6-e",.vector_size = sizeof (u32),.format_trace =
3125     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
3126     ARRAY_LEN (srv6_end_error_v6_e_strings),.error_strings =
3127     srv6_end_error_v6_e_strings,.n_next_nodes =
3128     SRV6_END_M_GTP6_E_N_NEXT,.next_nodes =
3129   {
3130   [SRV6_END_M_GTP6_E_NEXT_DROP] = "error-drop",
3131   [SRV6_END_M_GTP6_E_NEXT_LOOKUP] = "ip6-lookup",}
3132 ,};
3133
3134 VLIB_REGISTER_NODE (srv6_end_m_gtp6_d) =
3135 {
3136   .name = "srv6-end-m-gtp6-d",.vector_size = sizeof (u32),.format_trace =
3137     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
3138     ARRAY_LEN (srv6_end_error_v6_d_strings),.error_strings =
3139     srv6_end_error_v6_d_strings,.n_next_nodes =
3140     SRV6_END_M_GTP6_D_N_NEXT,.next_nodes =
3141   {
3142   [SRV6_END_M_GTP6_D_NEXT_DROP] = "error-drop",
3143   [SRV6_END_M_GTP6_D_NEXT_LOOKUP4] = "ip4-lookup",
3144   [SRV6_END_M_GTP6_D_NEXT_LOOKUP6] = "ip6-lookup",}
3145 ,};
3146
3147 VLIB_REGISTER_NODE (srv6_end_m_gtp6_d_di) =
3148 {
3149   .name = "srv6-end-m-gtp6-d-di",.vector_size = sizeof (u32),.format_trace =
3150     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
3151     ARRAY_LEN (srv6_end_error_v6_d_di_strings),.error_strings =
3152     srv6_end_error_v6_d_di_strings,.n_next_nodes =
3153     SRV6_END_M_GTP6_D_DI_N_NEXT,.next_nodes =
3154   {
3155   [SRV6_END_M_GTP6_D_DI_NEXT_DROP] = "error-drop",
3156   [SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP] = "ip6-lookup",}
3157 ,};
3158
3159 VLIB_REGISTER_NODE (srv6_end_m_gtp6_dt) =
3160 {
3161   .name = "srv6-end-m-gtp6-dt",.vector_size = sizeof (u32),.format_trace =
3162     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
3163     ARRAY_LEN (srv6_end_error_v6_dt_strings),.error_strings =
3164     srv6_end_error_v6_dt_strings,.n_next_nodes =
3165     SRV6_END_M_GTP6_DT_N_NEXT,.next_nodes =
3166   {
3167   [SRV6_END_M_GTP6_DT_NEXT_DROP] = "error-drop",
3168   [SRV6_END_M_GTP6_DT_NEXT_LOOKUP4] = "ip4-lookup",
3169   [SRV6_END_M_GTP6_DT_NEXT_LOOKUP6] = "ip6-lookup",}
3170 ,};
3171
3172 VLIB_REGISTER_NODE (srv6_t_m_gtp4_dt) =
3173 {
3174   .name = "srv6-t-m-gtp4-dt",.vector_size = sizeof (u32),.format_trace =
3175     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
3176     ARRAY_LEN (srv6_t_error_v4_dt_strings),.error_strings =
3177     srv6_t_error_v4_dt_strings,.n_next_nodes =
3178     SRV6_T_M_GTP4_DT_N_NEXT,.next_nodes =
3179   {
3180   [SRV6_T_M_GTP4_DT_NEXT_DROP] = "error-drop",
3181   [SRV6_T_M_GTP4_DT_NEXT_LOOKUP4] = "ip4-lookup",
3182   [SRV6_T_M_GTP4_DT_NEXT_LOOKUP6] = "ip6-lookup",}
3183 ,};
3184
3185 /*
3186 * fd.io coding-style-patch-verification: ON
3187 *
3188 * Local Variables:
3189 * eval: (c-set-style "gnu")
3190 * End:
3191 */