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_end_v4_d_error \
 117   _(M_GTP4_D_PACKETS, "srv6 End.M.GTP4.D packets") \
 118   _(M_GTP4_D_BAD_PACKETS, "srv6 End.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_END_ERROR_##sym,
 143   foreach_srv6_end_v4_d_error
 144 #undef _
 145     SRV6_END_N_V4_D_ERROR,
 146 } srv6_end_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_end_error_v6_e_strings[] = {
 179 #define _(sym,string) string,
 180   foreach_srv6_end_v6_e_error
 181 #undef _
 182 };
 183
 184 static char *srv6_end_error_v6_d_strings[] = {
 185 #define _(sym,string) string,
 186   foreach_srv6_end_v6_d_error
 187 #undef _
 188 };
 189
 190 static char *srv6_end_error_v6_d_di_strings[] = {
 191 #define _(sym,string) string,
 192   foreach_srv6_end_v6_d_di_error
 193 #undef _
 194 };
 195
 196 typedef enum
 197 {
 198   SRV6_END_M_GTP4_E_NEXT_DROP,
 199   SRV6_END_M_GTP4_E_NEXT_LOOKUP,
 200   SRV6_END_M_GTP4_E_N_NEXT,
 201 } srv6_end_m_gtp4_e_next_t;
 202
 203 typedef enum
 204 {
 205   SRV6_END_M_GTP6_E_NEXT_DROP,
 206   SRV6_END_M_GTP6_E_NEXT_LOOKUP,
 207   SRV6_END_M_GTP6_E_N_NEXT,
 208 } srv6_end_m_gtp6_e_next_t;
 209
 210 typedef enum
 211 {
 212   SRV6_END_M_GTP6_D_NEXT_DROP,
 213   SRV6_END_M_GTP6_D_NEXT_LOOKUP,
 214   SRV6_END_M_GTP6_D_N_NEXT,
 215 } srv6_end_m_gtp6_d_next_t;
 216
 217 typedef enum
 218 {
 219   SRV6_END_M_GTP6_D_DI_NEXT_DROP,
 220   SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP,
 221   SRV6_END_M_GTP6_D_DI_N_NEXT,
 222 } srv6_end_m_gtp6_d_di_next_t;
 223
 224 static inline u16
 225 hash_uword_to_u16 (uword * key)
 226 {
 227   u16 *val;
 228   val = (u16 *) key;
 229 #if uword_bits == 64
 230   return val[0] ^ val[1] ^ val[2] ^ val[3];
 231 #else
 232   return val[0] ^ val[1];
 233 #endif
 234 }
 235
 236 static inline void
 237 gtp_type_set (gtpu_header_t * gtpu, u16 tag)
 238 {
 239   u16 val;
 240
 241   val = clib_net_to_host_u16 (tag);
 242   if (val & SRH_TAG_ECHO_REPLY)
 243     gtpu->type = GTPU_TYPE_ECHO_REPLY;
 244   else if (val & SRH_TAG_ECHO_REQUEST)
 245     gtpu->type = GTPU_TYPE_ECHO_REQUEST;
 246   else if (val & SRH_TAG_ERROR_INDICATION)
 247     gtpu->type = GTPU_TYPE_ERROR_INDICATION;
 248   else if (val & SRH_TAG_END_MARKER)
 249     gtpu->type = GTPU_TYPE_END_MARKER;
 250 }
 251
 252 // Function for SRv6 GTP4.E function.
 253 VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
 254                                   vlib_node_runtime_t * node,
 255                                   vlib_frame_t * frame)
 256 {
 257   srv6_end_main_v4_t *sm = &srv6_end_main_v4;
 258   ip6_sr_main_t *sm2 = &sr_main;
 259   u32 n_left_from, next_index, *from, *to_next;
 260   u32 thread_index = vm->thread_index;
 261
 262   u32 good_n = 0, bad_n = 0;
 263
 264   from = vlib_frame_vector_args (frame);
 265   n_left_from = frame->n_vectors;
 266   next_index = node->cached_next_index;
 267
 268   while (n_left_from > 0)
 269     {
 270       u32 n_left_to_next;
 271
 272       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 273
 274       while (n_left_from > 0 && n_left_to_next > 0)
 275         {
 276           u32 bi0;
 277           vlib_buffer_t *b0;
 278           ip6_sr_localsid_t *ls0;
 279           srv6_end_gtp4_param_t *ls_param;
 280
 281           ip6srv_combo_header_t *ip6srv0;
 282           ip6_address_t src0, dst0;
 283
 284           ip4_gtpu_header_t *hdr0 = NULL;
 285           uword len0;
 286
 287           u32 next0 = SRV6_END_M_GTP4_E_NEXT_LOOKUP;
 288
 289           // defaults
 290           bi0 = from[0];
 291           to_next[0] = bi0;
 292           from += 1;
 293           to_next += 1;
 294           n_left_from -= 1;
 295           n_left_to_next -= 1;
 296
 297           b0 = vlib_get_buffer (vm, bi0);
 298           ls0 =
 299             pool_elt_at_index (sm2->localsids,
 300                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 301
 302           ls_param = (srv6_end_gtp4_param_t *) ls0->plugin_mem;
 303
 304           ip6srv0 = vlib_buffer_get_current (b0);
 305           src0 = ip6srv0->ip.src_address;
 306           dst0 = ip6srv0->ip.dst_address;
 307
 308           len0 = vlib_buffer_length_in_chain (vm, b0);
 309
 310           if ((ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE
 311                && len0 <
 312                sizeof (ip6srv_combo_header_t) + ip6srv0->sr.length * 8)
 313               || (len0 < sizeof (ip6_header_t)))
 314             {
 315               next0 = SRV6_END_M_GTP4_E_NEXT_DROP;
 316
 317               bad_n++;
 318             }
 319           else
 320             {
 321               u16 tag = 0;
 322               u32 teid;
 323               u8 *teid8p = (u8 *) & teid;
 324               u8 qfi = 0;
 325               u32 index;
 326               u32 offset, shift;
 327               u32 hdrlen = 0;
 328               uword key;
 329               u16 port;
 330               ip4_address_t dst4;
 331               void *p;
 332
 333               // we need to be sure there is enough space before
 334               // ip6srv0 header, there is some extra space
 335               // in the pre_data area for this kind of
 336               // logic
 337
 338               // jump over variable length data
 339               // not sure about the length
 340               if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE)
 341                 {
 342                   tag = ip6srv0->sr.tag;
 343
 344                   vlib_buffer_advance (b0,
 345                                        (word) sizeof (ip6srv_combo_header_t) +
 346                                        ip6srv0->sr.length * 8);
 347                 }
 348               else
 349                 {
 350                   vlib_buffer_advance (b0, (word) sizeof (ip6_header_t));
 351                 }
 352
 353               // get length of encapsulated IPv6 packet (the remaining part)
 354               p = vlib_buffer_get_current (b0);
 355
 356               len0 = vlib_buffer_length_in_chain (vm, b0);
 357
 358               offset = ls0->localsid_len / 8;
 359               shift = ls0->localsid_len % 8;
 360
 361               if (PREDICT_TRUE (shift == 0))
 362                 {
 363                   clib_memcpy_fast (&dst4.as_u8[0], &dst0.as_u8[offset], 4);
 364
 365                   qfi = dst0.as_u8[offset + 4];
 366
 367                   clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 5], 4);
 368                 }
 369               else
 370                 {
 371                   for (index = 0; index < 4; index++)
 372                     {
 373                       dst4.as_u8[index] = dst0.as_u8[offset + index] << shift;
 374                       dst4.as_u8[index] |=
 375                         dst0.as_u8[offset + index + 1] >> (8 - shift);
 376                     }
 377
 378                   qfi |= dst0.as_u8[offset + 4] << shift;
 379                   qfi |= dst0.as_u8[offset + 5] >> (8 - shift);
 380
 381                   for (index = 0; index < 4; index++)
 382                     {
 383                       *teid8p = dst0.as_u8[offset + 5 + index] << shift;
 384                       *teid8p |=
 385                         dst0.as_u8[offset + 6 + index] >> (8 - shift);
 386                       teid8p++;
 387                     }
 388                 }
 389
 390               if (qfi)
 391                 {
 392                   hdrlen =
 393                     sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
 394                   len0 += hdrlen;
 395                 }
 396               hdrlen += sizeof (ip4_gtpu_header_t);
 397
 398               // IPv4 GTP-U header creation.
 399               vlib_buffer_advance (b0, -(word) hdrlen);
 400
 401               hdr0 = vlib_buffer_get_current (b0);
 402
 403               clib_memcpy_fast (hdr0, &sm->cache_hdr,
 404                                 sizeof (ip4_gtpu_header_t));
 405
 406               hdr0->ip4.dst_address.as_u32 = dst4.as_u32;
 407
 408               hdr0->gtpu.teid = teid;
 409               hdr0->gtpu.length = clib_host_to_net_u16 (len0);
 410
 411               if (PREDICT_FALSE (tag != 0))
 412                 {
 413                   gtp_type_set (&hdr0->gtpu, tag);
 414                 }
 415
 416               if (qfi)
 417                 {
 418                   u8 type = 0;
 419                   gtpu_pdu_session_t *sess;
 420
 421                   hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
 422                   hdr0->gtpu.ext->seq = 0;
 423                   hdr0->gtpu.ext->npdu_num = 0;
 424                   hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
 425
 426                   type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
 427
 428                   qfi =
 429                     ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
 430                     ((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
 431
 432                   sess =
 433                     (gtpu_pdu_session_t *) (((char *) hdr0) +
 434                                             sizeof (ip4_gtpu_header_t) +
 435                                             sizeof (gtpu_exthdr_t));
 436                   sess->exthdrlen = 1;
 437                   sess->type = type;
 438                   sess->spare = 0;
 439                   sess->u.val = qfi;
 440                   sess->nextexthdr = 0;
 441                 }
 442
 443               offset = ls_param->v4src_position / 8;
 444               shift = ls_param->v4src_position % 8;
 445
 446               if (PREDICT_TRUE (shift == 0))
 447                 {
 448                   for (index = 0; index < 4; index++)
 449                     {
 450                       hdr0->ip4.src_address.as_u8[index] =
 451                         src0.as_u8[offset + index];
 452                     }
 453                 }
 454               else
 455                 {
 456                   for (index = 0; index < 4; index++)
 457                     {
 458                       hdr0->ip4.src_address.as_u8[index] =
 459                         src0.as_u8[offset + index] << shift;
 460                       hdr0->ip4.src_address.as_u8[index] |=
 461                         src0.as_u8[offset + index + 1] >> (8 - shift);
 462                     }
 463                 }
 464
 465               key = hash_memory (p, len0, 0);
 466               port = hash_uword_to_u16 (&key);
 467               hdr0->udp.src_port = port;
 468
 469               hdr0->udp.length = clib_host_to_net_u16 (len0 +
 470                                                        sizeof (udp_header_t) +
 471                                                        sizeof
 472                                                        (gtpu_header_t));
 473
 474               hdr0->ip4.length = clib_host_to_net_u16 (len0 +
 475                                                        sizeof
 476                                                        (ip4_gtpu_header_t));
 477
 478               hdr0->ip4.checksum = ip4_header_checksum (&hdr0->ip4);
 479
 480               good_n++;
 481
 482               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
 483                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
 484                 {
 485                   srv6_end_rewrite_trace_t *tr =
 486                     vlib_add_trace (vm, node, b0, sizeof (*tr));
 487                   clib_memcpy (tr->src.as_u8, hdr0->ip4.src_address.as_u8,
 488                                sizeof (tr->src.as_u8));
 489                   clib_memcpy (tr->dst.as_u8, hdr0->ip4.dst_address.as_u8,
 490                                sizeof (tr->dst.as_u8));
 491                   tr->teid = hdr0->gtpu.teid;
 492                 }
 493             }
 494
 495           vlib_increment_combined_counter
 496             (((next0 ==
 497                SRV6_END_M_GTP4_E_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
 498               &(sm2->sr_ls_valid_counters)), thread_index,
 499              ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
 500
 501           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 502                                            n_left_to_next, bi0, next0);
 503         }
 504
 505       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
 506     }
 507
 508   vlib_node_increment_counter (vm, sm->end_m_gtp4_e_node_index,
 509                                SRV6_END_ERROR_M_GTP4_E_BAD_PACKETS, bad_n);
 510
 511   vlib_node_increment_counter (vm, sm->end_m_gtp4_e_node_index,
 512                                SRV6_END_ERROR_M_GTP4_E_PACKETS, good_n);
 513
 514   return frame->n_vectors;
 515 }
 516
 517 VLIB_REGISTER_NODE (srv6_end_m_gtp4_e) =
 518 {
 519   .name = "srv6-end-m-gtp4-e",.vector_size = sizeof (u32),.format_trace =
 520     format_srv6_end_rewrite_trace,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
 521     ARRAY_LEN (srv6_end_error_v4_strings),.error_strings =
 522     srv6_end_error_v4_strings,.n_next_nodes =
 523     SRV6_END_M_GTP4_E_N_NEXT,.next_nodes =
 524   {
 525   [SRV6_END_M_GTP4_E_NEXT_DROP] =
 526       "error-drop",[SRV6_END_M_GTP4_E_NEXT_LOOKUP] = "ip4-lookup",}
 527 ,};
 528
 529 // Function for SRv6 GTP6.E function
 530 VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
 531                                   vlib_node_runtime_t * node,
 532                                   vlib_frame_t * frame)
 533 {
 534   srv6_end_main_v6_t *sm = &srv6_end_main_v6;
 535   ip6_sr_main_t *sm2 = &sr_main;
 536   u32 n_left_from, next_index, *from, *to_next;
 537   u32 thread_index = vm->thread_index;
 538
 539   u32 good_n = 0, bad_n = 0;
 540
 541   from = vlib_frame_vector_args (frame);
 542   n_left_from = frame->n_vectors;
 543   next_index = node->cached_next_index;
 544
 545   while (n_left_from > 0)
 546     {
 547       u32 n_left_to_next;
 548
 549       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 550
 551       while (n_left_from > 0 && n_left_to_next > 0)
 552         {
 553           u32 bi0;
 554           vlib_buffer_t *b0;
 555           ip6_sr_localsid_t *ls0;
 556
 557           ip6srv_combo_header_t *ip6srv0;
 558           ip6_address_t dst0, src0, seg0;
 559
 560           ip6_gtpu_header_t *hdr0 = NULL;
 561           uword len0;
 562           uword key;
 563           u16 port;
 564           u16 tag;
 565           void *p;
 566
 567           u32 next0 = SRV6_END_M_GTP6_E_NEXT_LOOKUP;
 568
 569           // defaults
 570           bi0 = from[0];
 571           to_next[0] = bi0;
 572           from += 1;
 573           to_next += 1;
 574           n_left_from -= 1;
 575           n_left_to_next -= 1;
 576
 577           b0 = vlib_get_buffer (vm, bi0);
 578           ls0 =
 579             pool_elt_at_index (sm2->localsids,
 580                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 581
 582           ip6srv0 = vlib_buffer_get_current (b0);
 583           dst0 = ip6srv0->ip.dst_address;
 584           src0 = ip6srv0->ip.src_address;
 585           seg0 = ip6srv0->sr.segments[0];
 586
 587           tag = ip6srv0->sr.tag;
 588
 589           len0 = vlib_buffer_length_in_chain (vm, b0);
 590
 591           if ((ip6srv0->ip.protocol != IPPROTO_IPV6_ROUTE)
 592               || (len0 <
 593                   sizeof (ip6srv_combo_header_t) + 8 * ip6srv0->sr.length))
 594             {
 595               next0 = SRV6_END_M_GTP6_E_NEXT_DROP;
 596
 597               bad_n++;
 598             }
 599           else
 600             {
 601               // we need to be sure there is enough space before
 602               // ip6srv0 header, there is some extra space
 603               // in the pre_data area for this kind of
 604               // logic
 605
 606               // jump over variable length data
 607               // not sure about the length
 608               vlib_buffer_advance (b0, (word) sizeof (ip6srv_combo_header_t) +
 609                                    ip6srv0->sr.length * 8);
 610
 611               // get length of encapsulated IPv6 packet (the remaining part)
 612               p = vlib_buffer_get_current (b0);
 613
 614               len0 = vlib_buffer_length_in_chain (vm, b0);
 615
 616               u32 teid;
 617               u8 *teid8p = (u8 *) & teid;
 618               u8 qfi = 0;
 619               u16 index;
 620               u16 offset, shift;
 621               u32 hdrlen = 0;
 622
 623               index = ls0->localsid_len;
 624               index += 8;
 625               offset = index / 8;
 626               shift = index % 8;
 627
 628               if (PREDICT_TRUE (shift == 0))
 629                 {
 630                   clib_memcpy_fast (teid8p, &dst0.as_u8[offset], 4);
 631
 632                   qfi = dst0.as_u8[offset + 4];
 633                 }
 634               else
 635                 {
 636                   for (index = offset; index < offset + 4; index++)
 637                     {
 638                       *teid8p = dst0.as_u8[index] << shift;
 639                       *teid8p |= dst0.as_u8[index + 1] >> (8 - shift);
 640                       teid8p++;
 641                     }
 642
 643                   qfi |= dst0.as_u8[offset + 4] << shift;
 644                   qfi |= dst0.as_u8[offset + 5] >> (8 - shift);
 645                 }
 646
 647               if (qfi)
 648                 {
 649                   hdrlen =
 650                     sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t);
 651                   len0 += hdrlen;
 652                 }
 653               hdrlen += sizeof (ip6_gtpu_header_t);
 654
 655               vlib_buffer_advance (b0, -(word) hdrlen);
 656
 657               hdr0 = vlib_buffer_get_current (b0);
 658
 659               clib_memcpy_fast (hdr0, &sm->cache_hdr,
 660                                 sizeof (ip6_gtpu_header_t));
 661
 662               hdr0->gtpu.teid = teid;
 663               hdr0->gtpu.length = clib_host_to_net_u16 (len0);
 664
 665               if (PREDICT_FALSE (tag != 0))
 666                 {
 667                   gtp_type_set (&hdr0->gtpu, tag);
 668                 }
 669
 670               if (qfi)
 671                 {
 672                   u8 type = 0;
 673                   gtpu_pdu_session_t *sess;
 674
 675                   hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG;
 676                   hdr0->gtpu.ext->seq = 0;
 677                   hdr0->gtpu.ext->npdu_num = 0;
 678                   hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION;
 679
 680                   type = qfi & SRV6_PDU_SESSION_U_BIT_MASK;
 681
 682                   qfi =
 683                     ((qfi & SRV6_PDU_SESSION_QFI_MASK) >> 2) |
 684                     ((qfi & SRV6_PDU_SESSION_R_BIT_MASK) << 5);
 685
 686                   sess =
 687                     (gtpu_pdu_session_t *) (((char *) hdr0) +
 688                                             sizeof (ip6_gtpu_header_t) +
 689                                             sizeof (gtpu_exthdr_t));
 690                   sess->exthdrlen = 1;
 691                   sess->type = type;
 692                   sess->spare = 0;
 693                   sess->u.val = qfi;
 694                   sess->nextexthdr = 0;
 695                 }
 696
 697               hdr0->udp.length = clib_host_to_net_u16 (len0 +
 698                                                        sizeof (udp_header_t) +
 699                                                        sizeof
 700                                                        (gtpu_header_t));
 701
 702               clib_memcpy_fast (hdr0->ip6.src_address.as_u8, src0.as_u8,
 703                                 sizeof (ip6_address_t));
 704               clib_memcpy_fast (hdr0->ip6.dst_address.as_u8, &seg0.as_u8,
 705                                 sizeof (ip6_address_t));
 706
 707               hdr0->ip6.payload_length = clib_host_to_net_u16 (len0 +
 708                                                                sizeof
 709                                                                (udp_header_t)
 710                                                                +
 711                                                                sizeof
 712                                                                (gtpu_header_t));
 713
 714               // UDP source port.
 715               key = hash_memory (p, len0, 0);
 716               port = hash_uword_to_u16 (&key);
 717               hdr0->udp.src_port = port;
 718
 719               good_n++;
 720
 721               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
 722                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
 723                 {
 724                   srv6_end_rewrite_trace_t *tr =
 725                     vlib_add_trace (vm, node, b0, sizeof (*tr));
 726                   clib_memcpy (tr->src.as_u8, hdr0->ip6.src_address.as_u8,
 727                                sizeof (ip6_address_t));
 728                   clib_memcpy (tr->dst.as_u8, hdr0->ip6.dst_address.as_u8,
 729                                sizeof (ip6_address_t));
 730                   tr->teid = hdr0->gtpu.teid;
 731                 }
 732             }
 733
 734           vlib_increment_combined_counter
 735             (((next0 ==
 736                SRV6_END_M_GTP6_E_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
 737               &(sm2->sr_ls_valid_counters)), thread_index,
 738              ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
 739
 740           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
 741                                            n_left_to_next, bi0, next0);
 742         }
 743
 744       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
 745     }
 746
 747   vlib_node_increment_counter (vm, sm->end_m_gtp6_e_node_index,
 748                                SRV6_END_ERROR_M_GTP6_E_BAD_PACKETS, bad_n);
 749
 750   vlib_node_increment_counter (vm, sm->end_m_gtp6_e_node_index,
 751                                SRV6_END_ERROR_M_GTP6_E_PACKETS, good_n);
 752
 753   return frame->n_vectors;
 754 }
 755
 756 // Function for SRv6 GTP6.D function
 757 VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
 758                                   vlib_node_runtime_t * node,
 759                                   vlib_frame_t * frame)
 760 {
 761   srv6_end_main_v6_decap_t *sm = &srv6_end_main_v6_decap;
 762   ip6_sr_main_t *sm2 = &sr_main;
 763   u32 n_left_from, next_index, *from, *to_next;
 764   u32 thread_index = vm->thread_index;
 765
 766   u32 good_n = 0, bad_n = 0;
 767
 768   from = vlib_frame_vector_args (frame);
 769   n_left_from = frame->n_vectors;
 770   next_index = node->cached_next_index;
 771
 772   while (n_left_from > 0)
 773     {
 774       u32 n_left_to_next;
 775
 776       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 777
 778       while (n_left_from > 0 && n_left_to_next > 0)
 779         {
 780           u32 bi0;
 781           vlib_buffer_t *b0;
 782           ip6_sr_localsid_t *ls0;
 783           srv6_end_gtp6_param_t *ls_param;
 784
 785           ip6_gtpu_header_t *hdr0 = NULL;
 786           uword len0;
 787
 788           ip6_address_t seg0, src0;
 789           u32 teid = 0;
 790           u8 *teidp;
 791           u8 gtpu_type = 0;
 792           u8 qfi;
 793           u8 *qfip = NULL;
 794           u32 offset, shift;
 795           u32 hdrlen;
 796           ip6_header_t *encap = NULL;
 797           gtpu_pdu_session_t *sess = NULL;
 798
 799           u32 next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP;
 800
 801           // defaults
 802           bi0 = from[0];
 803           to_next[0] = bi0;
 804           from += 1;
 805           to_next += 1;
 806           n_left_from -= 1;
 807           n_left_to_next -= 1;
 808
 809           b0 = vlib_get_buffer (vm, bi0);
 810           ls0 =
 811             pool_elt_at_index (sm2->localsids,
 812                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
 813
 814           ls_param = (srv6_end_gtp6_param_t *) ls0->plugin_mem;
 815
 816           hdr0 = vlib_buffer_get_current (b0);
 817
 818           hdrlen = sizeof (ip6_gtpu_header_t);
 819
 820           len0 = vlib_buffer_length_in_chain (vm, b0);
 821
 822           if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP)
 823               || (hdr0->udp.dst_port !=
 824                   clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT))
 825               || (len0 < sizeof (ip6_gtpu_header_t)))
 826             {
 827               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
 828
 829               bad_n++;
 830             }
 831           else
 832             {
 833               seg0 = ls_param->sr_prefix;
 834               src0 = hdr0->ip6.src_address;
 835
 836               gtpu_type = hdr0->gtpu.type;
 837
 838               teid = hdr0->gtpu.teid;
 839               teidp = (u8 *) & teid;
 840
 841               if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
 842                 {
 843                   // Extention header.
 844                   hdrlen += sizeof (gtpu_exthdr_t);
 845                   if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
 846                     {
 847                       // PDU Session Container.
 848                       sess =
 849                         (gtpu_pdu_session_t *) (((char *) hdr0) +
 850                                                 sizeof (ip6_gtpu_header_t) +
 851                                                 sizeof (gtpu_exthdr_t));
 852                       qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
 853                       qfip = (u8 *) & qfi;
 854
 855                       hdrlen += sizeof (gtpu_pdu_session_t);
 856
 857                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
 858                         {
 859                           hdrlen += sizeof (gtpu_paging_policy_t);
 860                         }
 861                     }
 862                 }
 863
 864               offset = ls_param->sr_prefixlen / 8;
 865               shift = ls_param->sr_prefixlen % 8;
 866
 867               offset += 1;
 868               if (PREDICT_TRUE (shift == 0))
 869                 {
 870                   clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
 871                   if (qfip)
 872                     {
 873                       qfi =
 874                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
 875                         ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 876
 877                       if (sess->type)
 878                         {
 879                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
 880                         }
 881
 882                       seg0.as_u8[offset + 4] = qfi;
 883                     }
 884                 }
 885               else
 886                 {
 887                   int idx;
 888
 889                   for (idx = 0; idx < 4; idx++)
 890                     {
 891                       seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
 892                       seg0.as_u8[offset + idx + 1] |=
 893                         teidp[idx] << (8 - shift);
 894                     }
 895
 896                   if (qfip)
 897                     {
 898                       qfi =
 899                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
 900                         ((qfi & ~GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
 901
 902                       if (sess->type)
 903                         {
 904                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
 905                         }
 906
 907                       seg0.as_u8[offset + 4] |= qfi >> shift;
 908                       seg0.as_u8[offset + 5] |= qfi << (8 - shift);
 909                     }
 910                 }
 911
 912               // jump over variable length data
 913               vlib_buffer_advance (b0, (word) hdrlen);
 914
 915               // get length of encapsulated IPv6 packet (the remaining part)
 916               len0 = vlib_buffer_length_in_chain (vm, b0);
 917
 918               if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
 919                 {
 920                   encap = vlib_buffer_get_current (b0);
 921                 }
 922
 923               uword *p;
 924               ip6srv_combo_header_t *ip6srv;
 925               ip6_sr_policy_t *sr_policy = NULL;
 926               ip6_sr_sl_t *sl = NULL;
 927               u32 *sl_index;
 928               u32 hdr_len;
 929
 930               p =
 931                 mhash_get (&sm2->sr_policies_index_hash,
 932                            &ls_param->sr_prefix);
 933               if (p)
 934                 {
 935                   sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
 936                 }
 937
 938               if (sr_policy)
 939                 {
 940                   vec_foreach (sl_index, sr_policy->segments_lists)
 941                   {
 942                     sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
 943                     if (sl != NULL)
 944                       break;
 945                   }
 946                 }
 947
 948               if (sl)
 949                 {
 950                   hdr_len = sizeof (ip6srv_combo_header_t);
 951                   hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
 952                   hdr_len += sizeof (ip6_address_t);
 953                 }
 954               else
 955                 {
 956                   hdr_len = sizeof (ip6_header_t);
 957                   if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
 958                     {
 959                       hdr_len += sizeof (ip6_sr_header_t);
 960                       hdr_len += sizeof (ip6_address_t);
 961                     }
 962                 }
 963
 964               // jump back to data[0] or pre_data if required
 965               vlib_buffer_advance (b0, -(word) hdr_len);
 966
 967               ip6srv = vlib_buffer_get_current (b0);
 968
 969               if (sl)
 970                 {
 971                   clib_memcpy_fast (ip6srv, sl->rewrite,
 972                                     vec_len (sl->rewrite));
 973
 974                   ip6srv->ip.src_address = src0;
 975                   ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
 976
 977                   ip6srv->sr.tag =
 978                     clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
 979
 980                   ip6srv->sr.segments_left += 1;
 981                   ip6srv->sr.last_entry += 1;
 982
 983                   ip6srv->sr.length += sizeof (ip6_address_t) / 8;
 984                   ip6srv->sr.segments[0] = seg0;
 985
 986                   if (PREDICT_TRUE (encap != NULL))
 987                     {
 988                       if (ls_param->nhtype == SRV6_NHTYPE_NONE)
 989                         {
 990                           if ((clib_net_to_host_u32
 991                                (encap->ip_version_traffic_class_and_flow_label)
 992                                >> 28) == 6)
 993                             ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
 994                           else
 995                             ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
 996                         }
 997                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
 998                         {
 999                           ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1000                           if ((clib_net_to_host_u32
1001                                (encap->ip_version_traffic_class_and_flow_label)
1002                                >> 28) != 4)
1003                             {
1004                               // Bad encap packet.
1005                               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1006                               bad_n++;
1007                               goto DONE;
1008                             }
1009                         }
1010                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
1011                         {
1012                           ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1013                           if ((clib_net_to_host_u32
1014                                (encap->ip_version_traffic_class_and_flow_label)
1015                                >> 28) != 6)
1016                             {
1017                               // Bad encap packet.
1018                               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1019                               bad_n++;
1020                               goto DONE;
1021                             }
1022                         }
1023                       else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1024                         {
1025                           ip6srv->sr.protocol = IP_PROTOCOL_NONE;
1026                         }
1027                     }
1028                   else
1029                     {
1030                       ip6srv->sr.protocol = IP_PROTOCOL_NONE;
1031                     }
1032
1033                   clib_memcpy_fast (&ip6srv->sr.segments[1],
1034                                     (u8 *) (sl->rewrite +
1035                                             sizeof (ip6_header_t) +
1036                                             sizeof (ip6_sr_header_t)),
1037                                     vec_len (sl->segments) *
1038                                     sizeof (ip6_address_t));
1039                 }
1040               else
1041                 {
1042                   clib_memcpy_fast (ip6srv, &sm->cache_hdr,
1043                                     sizeof (ip6_header_t));
1044
1045                   ip6srv->ip.src_address = src0;
1046                   ip6srv->ip.dst_address = seg0;
1047
1048                   if (PREDICT_FALSE (gtpu_type) != GTPU_TYPE_GTPU)
1049                     {
1050                       ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
1051
1052                       ip6srv->sr.protocol = IP_PROTOCOL_NONE;
1053
1054                       ip6srv->sr.tag =
1055                         clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1056
1057                       ip6srv->sr.segments_left = 0;
1058                       ip6srv->sr.last_entry = 0;
1059
1060                       ip6srv->sr.length = sizeof (ip6_address_t) / 8;
1061                       ip6srv->sr.segments[0] = seg0;
1062                     }
1063                   else
1064                     {
1065                       if (ls_param->nhtype == SRV6_NHTYPE_NONE)
1066                         {
1067                           if ((clib_net_to_host_u32
1068                                (encap->ip_version_traffic_class_and_flow_label)
1069                                >> 28) != 6)
1070                             ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
1071                         }
1072                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
1073                         {
1074                           ip6srv->ip.protocol = IP_PROTOCOL_IP_IN_IP;
1075                           if ((clib_net_to_host_u32
1076                                (encap->ip_version_traffic_class_and_flow_label)
1077                                >> 28) != 4)
1078                             {
1079                               // Bad encap packet.
1080                               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1081                               bad_n++;
1082                               goto DONE;
1083                             }
1084                         }
1085                       else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
1086                         {
1087                           ip6srv->ip.protocol = IP_PROTOCOL_IPV6;
1088                           if ((clib_net_to_host_u32
1089                                (encap->ip_version_traffic_class_and_flow_label)
1090                                >> 28) != 6)
1091                             {
1092                               // Bad encap packet.
1093                               next0 = SRV6_END_M_GTP6_D_NEXT_DROP;
1094                               bad_n++;
1095                               goto DONE;
1096                             }
1097                         }
1098                       else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1099                         {
1100                           ip6srv->ip.protocol = IP_PROTOCOL_NONE;
1101                         }
1102                     }
1103                 }
1104
1105               ip6srv->ip.payload_length =
1106                 clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
1107
1108               good_n++;
1109
1110               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1111                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1112                 {
1113                   srv6_end_rewrite_trace_t *tr =
1114                     vlib_add_trace (vm, node, b0, sizeof (*tr));
1115                   clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
1116                                sizeof (ip6_address_t));
1117                   clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
1118                                sizeof (ip6_address_t));
1119                   tr->teid = teid;
1120                   clib_memcpy (tr->sr_prefix.as_u8, ls_param->sr_prefix.as_u8,
1121                                sizeof (ip6_address_t));
1122                   tr->sr_prefixlen = ls_param->sr_prefixlen;
1123                 }
1124             }
1125
1126         DONE:
1127           vlib_increment_combined_counter
1128             (((next0 ==
1129                SRV6_END_M_GTP6_D_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) :
1130               &(sm2->sr_ls_valid_counters)), thread_index,
1131              ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0));
1132
1133           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1134                                            n_left_to_next, bi0, next0);
1135         }
1136
1137       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1138     }
1139
1140   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_node_index,
1141                                SRV6_END_ERROR_M_GTP6_D_BAD_PACKETS, bad_n);
1142
1143   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_node_index,
1144                                SRV6_END_ERROR_M_GTP6_D_PACKETS, good_n);
1145
1146   return frame->n_vectors;
1147 }
1148
1149 // Function for SRv6 GTP6.D.DI function
1150 VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
1151                                      vlib_node_runtime_t * node,
1152                                      vlib_frame_t * frame)
1153 {
1154   srv6_end_main_v6_decap_di_t *sm = &srv6_end_main_v6_decap_di;
1155   ip6_sr_main_t *sm2 = &sr_main;
1156   u32 n_left_from, next_index, *from, *to_next;
1157   u32 thread_index = vm->thread_index;
1158   srv6_end_gtp6_param_t *ls_param;
1159
1160   u32 good_n = 0, bad_n = 0;
1161
1162   from = vlib_frame_vector_args (frame);
1163   n_left_from = frame->n_vectors;
1164   next_index = node->cached_next_index;
1165
1166   while (n_left_from > 0)
1167     {
1168       u32 n_left_to_next;
1169
1170       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1171
1172       while (n_left_from > 0 && n_left_to_next > 0)
1173         {
1174           u32 bi0;
1175           vlib_buffer_t *b0;
1176           ip6_sr_localsid_t *ls0;
1177
1178           ip6_gtpu_header_t *hdr0 = NULL;
1179           uword len0;
1180
1181           ip6_address_t dst0;
1182           ip6_address_t src0;
1183           ip6_address_t seg0;
1184           u32 teid = 0;
1185           u8 *teidp;
1186           u8 gtpu_type = 0;
1187           u8 qfi = 0;
1188           u8 *qfip = NULL;
1189           u32 offset, shift;
1190           u32 hdrlen;
1191           ip6_header_t *encap = NULL;
1192           gtpu_pdu_session_t *sess;
1193
1194           u32 next0 = SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP;
1195
1196           // defaults
1197           bi0 = from[0];
1198           to_next[0] = bi0;
1199           from += 1;
1200           to_next += 1;
1201           n_left_from -= 1;
1202           n_left_to_next -= 1;
1203
1204           b0 = vlib_get_buffer (vm, bi0);
1205           ls0 =
1206             pool_elt_at_index (sm2->localsids,
1207                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1208
1209           ls_param = (srv6_end_gtp6_param_t *) ls0->plugin_mem;
1210
1211           hdr0 = vlib_buffer_get_current (b0);
1212
1213           hdrlen = sizeof (ip6_gtpu_header_t);
1214
1215           len0 = vlib_buffer_length_in_chain (vm, b0);
1216
1217           if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP)
1218               || (hdr0->udp.dst_port !=
1219                   clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT))
1220               || (len0 < sizeof (ip6_gtpu_header_t)))
1221             {
1222               next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
1223
1224               bad_n++;
1225             }
1226           else
1227             {
1228               dst0 = hdr0->ip6.dst_address;
1229               src0 = hdr0->ip6.src_address;
1230
1231               gtpu_type = hdr0->gtpu.type;
1232
1233               seg0 = ls_param->sr_prefix;
1234               teid = hdr0->gtpu.teid;
1235               teidp = (u8 *) & teid;
1236
1237               if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG)
1238                 {
1239                   // Extention header.
1240                   hdrlen += sizeof (gtpu_exthdr_t);
1241                   if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
1242                     {
1243                       // PDU Session Container.
1244                       sess =
1245                         (gtpu_pdu_session_t *) (((char *) hdr0) + hdrlen);
1246                       qfi = sess->u.val & ~GTPU_PDU_SESSION_P_BIT_MASK;
1247                       qfip = &qfi;
1248
1249                       hdrlen += sizeof (gtpu_pdu_session_t);
1250
1251                       if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK)
1252                         {
1253                           hdrlen += sizeof (gtpu_paging_policy_t);
1254                         }
1255                     }
1256                 }
1257
1258               offset = ls_param->sr_prefixlen / 8;
1259               shift = ls_param->sr_prefixlen % 8;
1260
1261               offset += 1;
1262               if (PREDICT_TRUE (shift == 0))
1263                 {
1264                   clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4);
1265
1266                   if (qfip)
1267                     {
1268                       qfi =
1269                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
1270                         ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
1271
1272                       if (sess->type)
1273                         {
1274                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
1275                         }
1276
1277                       seg0.as_u8[offset + 4] = qfi;
1278                     }
1279                 }
1280               else
1281                 {
1282                   int idx;
1283
1284                   for (idx = 0; idx < 4; idx++)
1285                     {
1286                       seg0.as_u8[offset + idx] |= teidp[idx] >> shift;
1287                       seg0.as_u8[offset + idx + 1] |=
1288                         teidp[idx] << (8 - shift);
1289                     }
1290
1291                   if (qfip)
1292                     {
1293                       qfi =
1294                         ((qfi & GTPU_PDU_SESSION_QFI_MASK) << 2) |
1295                         ((qfi & GTPU_PDU_SESSION_R_BIT_MASK) >> 5);
1296
1297                       if (sess->type)
1298                         {
1299                           qfi |= SRV6_PDU_SESSION_U_BIT_MASK;
1300                         }
1301
1302                       seg0.as_u8[offset + 4] |= qfi >> shift;
1303                       seg0.as_u8[offset + 5] |= qfi << (8 - shift);
1304                     }
1305                 }
1306
1307               // jump over variable length data
1308               vlib_buffer_advance (b0, (word) hdrlen);
1309
1310               // get length of encapsulated IPv6 packet (the remaining part)
1311               len0 = vlib_buffer_length_in_chain (vm, b0);
1312
1313               if (PREDICT_TRUE (gtpu_type == GTPU_TYPE_GTPU))
1314                 {
1315                   encap = vlib_buffer_get_current (b0);
1316                 }
1317
1318               uword *p;
1319               ip6srv_combo_header_t *ip6srv;
1320               ip6_sr_policy_t *sr_policy = NULL;
1321               ip6_sr_sl_t *sl = NULL;
1322               u32 *sl_index;
1323               u32 hdr_len;
1324
1325               p =
1326                 mhash_get (&sm2->sr_policies_index_hash,
1327                            &ls_param->sr_prefix);
1328               if (p)
1329                 {
1330                   sr_policy = pool_elt_at_index (sm2->sr_policies, p[0]);
1331                 }
1332
1333               if (sr_policy)
1334                 {
1335                   vec_foreach (sl_index, sr_policy->segments_lists)
1336                   {
1337                     sl = pool_elt_at_index (sm2->sid_lists, *sl_index);
1338                     if (sl != NULL)
1339                       break;
1340                   }
1341                 }
1342
1343               hdr_len = sizeof (ip6srv_combo_header_t);
1344
1345               if (sl)
1346                 hdr_len += vec_len (sl->segments) * sizeof (ip6_address_t);
1347
1348               hdr_len += sizeof (ip6_address_t) * 2;
1349
1350               // jump back to data[0] or pre_data if required
1351               vlib_buffer_advance (b0, -(word) hdr_len);
1352
1353               ip6srv = vlib_buffer_get_current (b0);
1354
1355               if (sl)
1356                 {
1357                   clib_memcpy_fast (ip6srv, sl->rewrite,
1358                                     vec_len (sl->rewrite));
1359
1360                   ip6srv->ip.src_address = src0;
1361
1362                   ip6srv->sr.tag =
1363                     clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1364
1365                   ip6srv->sr.segments_left += 2;
1366                   ip6srv->sr.last_entry += 2;
1367                 }
1368               else
1369                 {
1370                   clib_memcpy_fast (ip6srv, &sm->cache_hdr,
1371                                     sizeof (ip6_header_t));
1372
1373                   ip6srv->ip.src_address = src0;
1374                   ip6srv->ip.dst_address = seg0;
1375
1376                   ip6srv->sr.tag =
1377                     clib_host_to_net_u16 (srh_tagfield[gtpu_type]);
1378
1379                   ip6srv->sr.segments_left += 1;
1380                   ip6srv->sr.last_entry += 1;
1381                   ip6srv->sr.type = ROUTING_HEADER_TYPE_SR;
1382                 }
1383
1384               ip6srv->ip.payload_length =
1385                 clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t));
1386               ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE;
1387
1388               if (PREDICT_TRUE (encap != NULL))
1389                 {
1390                   if (ls_param->nhtype == SRV6_NHTYPE_NONE)
1391                     {
1392                       if ((clib_net_to_host_u32
1393                            (encap->ip_version_traffic_class_and_flow_label) >>
1394                            28) == 6)
1395                         ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1396                       else
1397                         ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1398                     }
1399                   else if (ls_param->nhtype == SRV6_NHTYPE_IPV4)
1400                     {
1401                       ip6srv->sr.protocol = IP_PROTOCOL_IP_IN_IP;
1402                       if ((clib_net_to_host_u32
1403                            (encap->ip_version_traffic_class_and_flow_label) >>
1404                            28) != 4)
1405                         {
1406                           // Bad encap packet.
1407                           next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
1408                           bad_n++;
1409                           goto DONE;
1410                         }
1411                     }
1412                   else if (ls_param->nhtype == SRV6_NHTYPE_IPV6)
1413                     {
1414                       ip6srv->sr.protocol = IP_PROTOCOL_IPV6;
1415                       if ((clib_net_to_host_u32
1416                            (encap->ip_version_traffic_class_and_flow_label) >>
1417                            28) != 6)
1418                         {
1419                           // Bad encap packet.
1420                           next0 = SRV6_END_M_GTP6_D_DI_NEXT_DROP;
1421                           bad_n++;
1422                           goto DONE;
1423                         }
1424                     }
1425                   else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP)
1426                     {
1427                       ip6srv->sr.protocol = IP_PROTOCOL_NONE;
1428                     }
1429                 }
1430               else
1431                 {
1432                   ip6srv->sr.protocol = IP_PROTOCOL_NONE;
1433                 }
1434
1435               ip6srv->sr.length += ((sizeof (ip6_address_t) * 2) / 8);
1436               ip6srv->sr.segments[0] = dst0;
1437               ip6srv->sr.segments[1] = seg0;
1438
1439               if (sl)
1440                 {
1441                   clib_memcpy_fast (&ip6srv->sr.segments[2],
1442                                     (u8 *) (sl->rewrite +
1443                                             sizeof (ip6_header_t) +
1444                                             sizeof (ip6_sr_header_t)),
1445                                     vec_len (sl->segments) *
1446                                     sizeof (ip6_address_t));
1447                 }
1448
1449               good_n++;
1450
1451               if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1452                   PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1453                 {
1454                   srv6_end_rewrite_trace_t *tr =
1455                     vlib_add_trace (vm, node, b0, sizeof (*tr));
1456                   clib_memcpy (tr->src.as_u8, ip6srv->ip.src_address.as_u8,
1457                                sizeof (ip6_address_t));
1458                   clib_memcpy (tr->dst.as_u8, ip6srv->ip.dst_address.as_u8,
1459                                sizeof (ip6_address_t));
1460                   tr->teid = teid;
1461                   clib_memcpy (tr->sr_prefix.as_u8, ls_param->sr_prefix.as_u8,
1462                                sizeof (ip6_address_t));
1463                   tr->sr_prefixlen = ls_param->sr_prefixlen;
1464                 }
1465             }
1466
1467         DONE:
1468           vlib_increment_combined_counter
1469             (((next0 ==
1470                SRV6_END_M_GTP6_D_DI_NEXT_DROP) ?
1471               &(sm2->sr_ls_invalid_counters) : &(sm2->sr_ls_valid_counters)),
1472              thread_index, ls0 - sm2->localsids, 1,
1473              vlib_buffer_length_in_chain (vm, b0));
1474
1475           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1476                                            n_left_to_next, bi0, next0);
1477         }
1478
1479       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1480     }
1481
1482   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_di_node_index,
1483                                SRV6_END_ERROR_M_GTP6_D_DI_BAD_PACKETS, bad_n);
1484
1485   vlib_node_increment_counter (vm, sm->end_m_gtp6_d_di_node_index,
1486                                SRV6_END_ERROR_M_GTP6_D_DI_PACKETS, good_n);
1487
1488   return frame->n_vectors;
1489 }
1490
1491 VLIB_REGISTER_NODE (srv6_end_m_gtp6_e) =
1492 {
1493   .name = "srv6-end-m-gtp6-e",.vector_size = sizeof (u32),.format_trace =
1494     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
1495     ARRAY_LEN (srv6_end_error_v6_e_strings),.error_strings =
1496     srv6_end_error_v6_e_strings,.n_next_nodes =
1497     SRV6_END_M_GTP6_E_N_NEXT,.next_nodes =
1498   {
1499   [SRV6_END_M_GTP6_E_NEXT_DROP] =
1500       "error-drop",[SRV6_END_M_GTP6_E_NEXT_LOOKUP] = "ip6-lookup",}
1501 ,};
1502
1503 VLIB_REGISTER_NODE (srv6_end_m_gtp6_d) =
1504 {
1505   .name = "srv6-end-m-gtp6-d",.vector_size = sizeof (u32),.format_trace =
1506     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
1507     ARRAY_LEN (srv6_end_error_v6_d_strings),.error_strings =
1508     srv6_end_error_v6_d_strings,.n_next_nodes =
1509     SRV6_END_M_GTP6_D_N_NEXT,.next_nodes =
1510   {
1511   [SRV6_END_M_GTP6_D_NEXT_DROP] =
1512       "error-drop",[SRV6_END_M_GTP6_D_NEXT_LOOKUP] = "ip6-lookup",}
1513 ,};
1514
1515 VLIB_REGISTER_NODE (srv6_end_m_gtp6_d_di) =
1516 {
1517   .name = "srv6-end-m-gtp6-d-di",.vector_size = sizeof (u32),.format_trace =
1518     format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors =
1519     ARRAY_LEN (srv6_end_error_v6_d_di_strings),.error_strings =
1520     srv6_end_error_v6_d_di_strings,.n_next_nodes =
1521     SRV6_END_M_GTP6_D_DI_N_NEXT,.next_nodes =
1522   {
1523   [SRV6_END_M_GTP6_D_DI_NEXT_DROP] = "error-drop",
1524       [SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP] = "ip6-lookup",}
1525 ,};
1526
1527 /*
1528 * fd.io coding-style-patch-verification: ON
1529 *
1530 * Local Variables:
1531 * eval: (c-set-style "gnu")
1532 * End:
1533 */