src/plugins/srv6-mobile/node.c

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