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