X-Git-Url: https://gerrit.fd.io/r/gitweb?a=blobdiff_plain;f=src%2Fplugins%2Fsrv6-mobile%2Fnode.c;h=448d6332b1553973313927ae8aa6146e2c93bf61;hb=c3cb2075d;hp=eb9a20e3c53afc45ba25d901bedba737986420df;hpb=70d8ef89a5d9e38d4f35517c0f64009c47b76c4d;p=vpp.git diff --git a/src/plugins/srv6-mobile/node.c b/src/plugins/srv6-mobile/node.c index eb9a20e3c53..448d6332b15 100644 --- a/src/plugins/srv6-mobile/node.c +++ b/src/plugins/srv6-mobile/node.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 Arrcus Inc and/or its affiliates. + * Copyright (c) 2020 Arrcus Inc and/or its affiliates. * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at: @@ -133,6 +133,10 @@ format_srv6_end_rewrite_trace6 (u8 * s, va_list * args) _(M_GTP6_DT_PACKETS, "srv6 End.M.GTP6.DT packets") \ _(M_GTP6_DT_BAD_PACKETS, "srv6 End.M.GTP6.DT bad packets") +#define foreach_srv6_t_v4_dt_error \ + _(M_GTP4_DT_PACKETS, "srv6 T.M.GTP4.DT packets") \ + _(M_GTP4_DT_BAD_PACKETS, "srv6 T.M.GTP4.DT bad packets") + typedef enum { #define _(sym,str) SRV6_END_ERROR_##sym, @@ -181,6 +185,14 @@ typedef enum SRV6_END_N_V6_DT_ERROR, } srv6_end_error_v6_dt_t; +typedef enum +{ +#define _(sym,str) SRV6_T_ERROR_##sym, + foreach_srv6_t_v4_dt_error +#undef _ + SRV6_T_N_V4_DT_ERROR, +} srv6_t_error_v4_dt_t; + static char *srv6_end_error_v4_strings[] = { #define _(sym,string) string, foreach_srv6_end_v4_error @@ -211,6 +223,18 @@ static char *srv6_end_error_v6_d_di_strings[] = { #undef _ }; +static char *srv6_end_error_v6_dt_strings[] = { +#define _(sym,string) string, + foreach_srv6_end_v6_dt_error +#undef _ +}; + +static char *srv6_t_error_v4_dt_strings[] = { +#define _(sym,string) string, + foreach_srv6_t_v4_dt_error +#undef _ +}; + typedef enum { SRV6_END_M_GTP4_E_NEXT_DROP, @@ -246,6 +270,22 @@ typedef enum SRV6_END_M_GTP6_D_DI_N_NEXT, } srv6_end_m_gtp6_d_di_next_t; +typedef enum +{ + SRV6_END_M_GTP6_DT_NEXT_DROP, + SRV6_END_M_GTP6_DT_NEXT_LOOKUP4, + SRV6_END_M_GTP6_DT_NEXT_LOOKUP6, + SRV6_END_M_GTP6_DT_N_NEXT, +} srv6_end_m_gtp6_dt_next_t; + +typedef enum +{ + SRV6_T_M_GTP4_DT_NEXT_DROP, + SRV6_T_M_GTP4_DT_NEXT_LOOKUP4, + SRV6_T_M_GTP4_DT_NEXT_LOOKUP6, + SRV6_T_M_GTP4_DT_N_NEXT, +} srv6_t_m_gtp4_dt_next_t; + static inline u16 hash_uword_to_u16 (uword * key) { @@ -258,20 +298,22 @@ hash_uword_to_u16 (uword * key) #endif } -static inline void -gtp_type_set (gtpu_header_t * gtpu, u16 tag) +static inline u8 +gtpu_type_get (u16 tag) { u16 val; val = clib_net_to_host_u16 (tag); if (val & SRH_TAG_ECHO_REPLY) - gtpu->type = GTPU_TYPE_ECHO_REPLY; + return GTPU_TYPE_ECHO_REPLY; else if (val & SRH_TAG_ECHO_REQUEST) - gtpu->type = GTPU_TYPE_ECHO_REQUEST; + return GTPU_TYPE_ECHO_REQUEST; else if (val & SRH_TAG_ERROR_INDICATION) - gtpu->type = GTPU_TYPE_ERROR_INDICATION; + return GTPU_TYPE_ERROR_INDICATION; else if (val & SRH_TAG_END_MARKER) - gtpu->type = GTPU_TYPE_END_MARKER; + return GTPU_TYPE_END_MARKER; + + return GTPU_TYPE_GTPU; } // Function for SRv6 GTP4.E function. @@ -343,45 +385,32 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, } else { + u8 gtpu_type = 0; u16 tag = 0; - u32 teid; + u32 teid = 0; u8 *teid8p = (u8 *) & teid; u8 qfi = 0; + u16 seq = 0; u32 index; u32 offset, shift; u32 hdrlen = 0; uword key; u16 port; ip4_address_t dst4; + u16 ie_size = 0; + u8 ie_buf[GTPU_IE_MAX_SIZ]; void *p; + uword plen; - // we need to be sure there is enough space before - // ip6srv0 header, there is some extra space - // in the pre_data area for this kind of - // logic - - // jump over variable length data - // not sure about the length if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE) { tag = ip6srv0->sr.tag; - - vlib_buffer_advance (b0, - (word) sizeof (ip6srv_combo_header_t) + - ip6srv0->sr.length * 8); - } - else - { - vlib_buffer_advance (b0, (word) sizeof (ip6_header_t)); } - // get length of encapsulated IPv6 packet (the remaining part) - p = vlib_buffer_get_current (b0); - - len0 = vlib_buffer_length_in_chain (vm, b0); + offset = ls0->localsid_prefix_len / 8; + shift = ls0->localsid_prefix_len % 8; - offset = ls0->localsid_len / 8; - shift = ls0->localsid_len % 8; + gtpu_type = gtpu_type_get (tag); if (PREDICT_TRUE (shift == 0)) { @@ -389,10 +418,21 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, qfi = dst0.as_u8[offset + 4]; - clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 5], 4); + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + clib_memcpy_fast (&seq, &dst0.as_u8[offset + 5], 2); + } + else + { + clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 5], 4); + } } else { + u8 *sp; + for (index = 0; index < 4; index++) { dst4.as_u8[index] = dst0.as_u8[offset + index] << shift; @@ -403,12 +443,27 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, qfi |= dst0.as_u8[offset + 4] << shift; qfi |= dst0.as_u8[offset + 5] >> (8 - shift); - for (index = 0; index < 4; index++) + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) { - *teid8p = dst0.as_u8[offset + 5 + index] << shift; - *teid8p |= - dst0.as_u8[offset + 6 + index] >> (8 - shift); - teid8p++; + sp = (u8 *) & seq; + for (index = 0; index < 2; index++) + { + sp[index] = dst0.as_u8[offset + 5 + index] << shift; + sp[index] |= + dst0.as_u8[offset + 6 + index] >> (8 - shift); + } + } + else + { + for (index = 0; index < 4; index++) + { + *teid8p = dst0.as_u8[offset + 5 + index] << shift; + *teid8p |= + dst0.as_u8[offset + 6 + index] >> (8 - shift); + teid8p++; + } } } @@ -416,8 +471,67 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, { hdrlen = sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t); - len0 += hdrlen; } + else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + hdrlen = sizeof (gtpu_exthdr_t); + } + + if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ECHO_REPLY)) + { + hdrlen += sizeof (gtpu_recovery_ie); + } + + if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION)) + { + ip6_sr_tlv_t *tlv; + u16 ext_len; + + ext_len = ip6srv0->sr.length * 8; + + if (ext_len > + sizeof (ip6_address_t) * (ip6srv0->sr.last_entry + 1)) + { + tlv = + (ip6_sr_tlv_t *) ((u8 *) & ip6srv0->sr + + sizeof (ip6_sr_header_t) + + sizeof (ip6_address_t) * + (ip6srv0->sr.last_entry + 1)); + + if (tlv->type == SRH_TLV_USER_PLANE_CONTAINER) + { + user_plane_sub_tlv_t *sub_tlv; + + sub_tlv = (user_plane_sub_tlv_t *) tlv->value; + + ie_size = sub_tlv->length; + clib_memcpy_fast (ie_buf, sub_tlv->value, ie_size); + + hdrlen += ie_size; + } + } + } + + if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE) + { + vlib_buffer_advance (b0, + (word) sizeof (ip6srv_combo_header_t) + + ip6srv0->sr.length * 8); + } + else + { + vlib_buffer_advance (b0, (word) sizeof (ip6_header_t)); + } + + // get length of encapsulated IPv6 packet (the remaining part) + p = vlib_buffer_get_current (b0); + + plen = len0 = vlib_buffer_length_in_chain (vm, b0); + + len0 += hdrlen; + hdrlen += sizeof (ip4_gtpu_header_t); // IPv4 GTP-U header creation. @@ -433,10 +547,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, hdr0->gtpu.teid = teid; hdr0->gtpu.length = clib_host_to_net_u16 (len0); - if (PREDICT_FALSE (tag != 0)) - { - gtp_type_set (&hdr0->gtpu, tag); - } + hdr0->gtpu.type = gtpu_type; if (qfi) { @@ -444,7 +555,9 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, gtpu_pdu_session_t *sess; hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG; + hdr0->gtpu.ext->seq = 0; + hdr0->gtpu.ext->npdu_num = 0; hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION; @@ -465,6 +578,38 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, sess->nextexthdr = 0; } + if (gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG; + hdr0->gtpu.ext->seq = seq; + hdr0->gtpu.ext->npdu_num = 0; + hdr0->gtpu.ext->nextexthdr = 0; + + if (gtpu_type == GTPU_TYPE_ECHO_REPLY) + { + gtpu_recovery_ie *recovery; + + recovery = + (gtpu_recovery_ie *) ((u8 *) hdr0 + + (hdrlen - + sizeof (gtpu_recovery_ie))); + recovery->type = GTPU_RECOVERY_IE_TYPE; + recovery->restart_counter = 0; + } + else if (gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + if (ie_size) + { + u8 *ie_ptr; + + ie_ptr = (u8 *) ((u8 *) hdr0 + (hdrlen - ie_size)); + clib_memcpy_fast (ie_ptr, ie_buf, ie_size); + } + } + } + offset = ls_param->v4src_position / 8; shift = ls_param->v4src_position % 8; @@ -487,7 +632,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, } } - key = hash_memory (p, len0, 0); + key = hash_memory (p, plen < 40 ? plen : 40, 0); port = hash_uword_to_u16 (&key); hdr0->udp.src_port = port; @@ -510,9 +655,9 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm, srv6_end_rewrite_trace_t *tr = vlib_add_trace (vm, node, b0, sizeof (*tr)); clib_memcpy (tr->src.as_u8, hdr0->ip4.src_address.as_u8, - sizeof (tr->src.as_u8)); + sizeof (hdr0->ip4.src_address.as_u8)); clib_memcpy (tr->dst.as_u8, hdr0->ip4.dst_address.as_u8, - sizeof (tr->dst.as_u8)); + sizeof (hdr0->ip4.dst_address.as_u8)); tr->teid = hdr0->gtpu.teid; } } @@ -618,9 +763,14 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, u8 *teidp; u8 qfi = 0; u8 *qfip = NULL; + u16 seq = 0; + u8 *seqp; u32 offset, shift, index; ip6srv_combo_header_t *ip6srv; gtpu_pdu_session_t *sess = NULL; + int ie_size = 0; + u16 tlv_siz = 0; + u8 ie_buf[GTPU_IE_MAX_SIZ]; // Decap from GTP-U. hdr = (ip4_gtpu_header_t *) ip4; @@ -630,12 +780,17 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, teid = hdr->gtpu.teid; teidp = (u8 *) & teid; + seqp = (u8 *) & seq; + gtpu_type = hdr->gtpu.type; - if (hdr->gtpu.ver_flags & GTPU_EXTHDR_FLAG) + if (hdr->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG)) { // Extention header. hdr_len += sizeof (gtpu_exthdr_t); + + seq = hdr->gtpu.ext->seq; + if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION) { // PDU Session Container. @@ -682,7 +837,16 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, seg.as_u8[offset + 4] = qfi; } - clib_memcpy_fast (&seg.as_u8[offset + 5], teidp, 4); + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + clib_memcpy_fast (&seg.as_u8[offset + 5], seqp, 2); + } + else + { + clib_memcpy_fast (&seg.as_u8[offset + 5], teidp, 4); + } } else { @@ -691,10 +855,6 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, seg.as_u8[offset + index] |= dstp[index] >> shift; seg.as_u8[offset + index + 1] |= dstp[index] << (8 - shift); - - seg.as_u8[offset + index + 5] |= teidp[index] >> shift; - seg.as_u8[offset + index + 6] |= - teidp[index] << (8 - shift); } if (qfip) @@ -711,6 +871,49 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, seg.as_u8[offset + 4] |= qfi >> shift; seg.as_u8[offset + 5] |= qfi << (8 - shift); } + + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + for (index = 0; index < 2; index++) + { + seg.as_u8[offset + 5 + index] |= + seqp[index] >> shift; + seg.as_u8[offset + 6 + index] |= + seqp[index] << (8 - shift); + } + } + else + { + for (index = 0; index < 4; index++) + { + seg.as_u8[offset + index + 5] |= + teidp[index] >> shift; + seg.as_u8[offset + index + 6] |= + teidp[index] << (8 - shift); + } + } + } + + if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION)) + { + u16 payload_len; + + payload_len = clib_net_to_host_u16 (hdr->gtpu.length); + if (payload_len != 0) + { + ie_size = + payload_len - (hdr_len - sizeof (ip4_gtpu_header_t)); + if (ie_size > 0) + { + u8 *ies; + + ies = (u8 *) ((u8 *) hdr + hdr_len); + clib_memcpy_fast (ie_buf, ies, ie_size); + hdr_len += ie_size; + } + } } src6 = ls_param->v6src_prefix; @@ -777,6 +980,16 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, } } + if (ie_size) + { + tlv_siz = + sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) + + ie_size; + + tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0); + hdr_len += tlv_siz; + } + vlib_buffer_advance (b0, -(word) hdr_len); ip6srv = vlib_buffer_get_current (b0); @@ -785,16 +998,49 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, clib_memcpy_fast (ip6srv, sl->rewrite, vec_len (sl->rewrite)); - ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE; + if (vec_len (sl->segments) > 1) + { + ip6srv->sr.tag = + clib_host_to_net_u16 (srh_tagfield[gtpu_type]); - ip6srv->sr.tag = - clib_host_to_net_u16 (srh_tagfield[gtpu_type]); + ip6srv->sr.segments_left += 1; + ip6srv->sr.last_entry += 1; + + ip6srv->sr.length += sizeof (ip6_address_t) / 8; + ip6srv->sr.segments[0] = seg; + + clib_memcpy_fast (&ip6srv->sr.segments[1], + (u8 *) (sl->rewrite + + sizeof (ip6_header_t) + + sizeof (ip6_sr_header_t)), + vec_len (sl->segments) * + sizeof (ip6_address_t)); + } + else + { + ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE; - ip6srv->sr.segments_left += 1; - ip6srv->sr.last_entry += 1; + ip6srv->sr.type = ROUTING_HEADER_TYPE_SR; - ip6srv->sr.length += sizeof (ip6_address_t) / 8; - ip6srv->sr.segments[0] = seg; + ip6srv->sr.segments_left = 1; + ip6srv->sr.last_entry = 0; + + ip6srv->sr.length = + ((sizeof (ip6_sr_header_t) + + sizeof (ip6_address_t)) / 8) - 1; + ip6srv->sr.flags = 0; + + ip6srv->sr.tag = + clib_host_to_net_u16 (srh_tagfield[gtpu_type]); + + ip6srv->sr.segments[0] = seg; + if (vec_len (sl->segments)) + { + ip6srv->sr.segments[1] = sl->segments[0]; + ip6srv->sr.length += sizeof (ip6_address_t) / 8; + ip6srv->sr.last_entry++; + } + } if (PREDICT_TRUE (encap != NULL)) { @@ -835,20 +1081,13 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, } else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP) { - ip6srv->sr.protocol = IP_PROTOCOL_NONE; + ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET; } } else { - ip6srv->sr.protocol = IP_PROTOCOL_NONE; + ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET; } - - clib_memcpy_fast (&ip6srv->sr.segments[1], - (u8 *) (sl->rewrite + - sizeof (ip6_header_t) + - sizeof (ip6_sr_header_t)), - vec_len (sl->segments) * - sizeof (ip6_address_t)); } else { @@ -861,7 +1100,7 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, { ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE; - ip6srv->sr.protocol = IP_PROTOCOL_NONE; + ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET; ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]); @@ -911,13 +1150,32 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm, } else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP) { - ip6srv->ip.protocol = IP_PROTOCOL_NONE; + ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET; } } } ip6srv->ip.src_address = src6; + if (PREDICT_FALSE (ie_size)) + { + ip6_sr_tlv_t *tlv; + user_plane_sub_tlv_t *sub_tlv; + + tlv = + (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz)); + tlv->type = SRH_TLV_USER_PLANE_CONTAINER; + tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t)); + clib_memset (tlv->value, 0, tlv->length); + + sub_tlv = (user_plane_sub_tlv_t *) tlv->value; + sub_tlv->type = USER_PLANE_SUB_TLV_IE; + sub_tlv->length = (u8) ie_size; + clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size); + + ip6srv->sr.length += (u8) (tlv_siz / 8); + } + ip6srv->ip.payload_length = clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t)); @@ -960,8 +1218,8 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp4_e) = srv6_end_error_v4_strings,.n_next_nodes = SRV6_END_M_GTP4_E_N_NEXT,.next_nodes = { - [SRV6_END_M_GTP4_E_NEXT_DROP] = "error-drop", - [SRV6_END_M_GTP4_E_NEXT_LOOKUP] = "ip4-lookup",} + [SRV6_END_M_GTP4_E_NEXT_DROP] = + "error-drop",[SRV6_END_M_GTP4_E_NEXT_LOOKUP] = "ip4-lookup",} ,}; VLIB_REGISTER_NODE (srv6_t_m_gtp4_d) = @@ -972,8 +1230,8 @@ VLIB_REGISTER_NODE (srv6_t_m_gtp4_d) = srv6_t_error_v4_d_strings,.n_next_nodes = SRV6_T_M_GTP4_D_N_NEXT,.next_nodes = { - [SRV6_T_M_GTP4_D_NEXT_DROP] = "error-drop", - [SRV6_T_M_GTP4_D_NEXT_LOOKUP] = "ip6-lookup",} + [SRV6_T_M_GTP4_D_NEXT_DROP] = + "error-drop",[SRV6_T_M_GTP4_D_NEXT_LOOKUP] = "ip6-lookup",} ,}; // Function for SRv6 GTP6.E function @@ -1013,6 +1271,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm, u16 port; u16 tag; void *p; + uword plen; u32 next0 = SRV6_END_M_GTP6_E_NEXT_LOOKUP; @@ -1053,53 +1312,127 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm, // in the pre_data area for this kind of // logic - // jump over variable length data - // not sure about the length - vlib_buffer_advance (b0, (word) sizeof (ip6srv_combo_header_t) + - ip6srv0->sr.length * 8); - - // get length of encapsulated IPv6 packet (the remaining part) - p = vlib_buffer_get_current (b0); - - len0 = vlib_buffer_length_in_chain (vm, b0); - - u32 teid; + u32 teid = 0; u8 *teid8p = (u8 *) & teid; u8 qfi = 0; + u16 seq = 0; + u8 gtpu_type = 0; u16 index; u16 offset, shift; u32 hdrlen = 0; + u16 ie_size = 0; + u8 ie_buf[GTPU_IE_MAX_SIZ]; - index = ls0->localsid_len; - index += 8; + index = ls0->localsid_prefix_len; offset = index / 8; shift = index % 8; + gtpu_type = gtpu_type_get (tag); + if (PREDICT_TRUE (shift == 0)) { - clib_memcpy_fast (teid8p, &dst0.as_u8[offset], 4); + qfi = dst0.as_u8[offset]; - qfi = dst0.as_u8[offset + 4]; + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + clib_memcpy_fast (&seq, &dst0.as_u8[offset + 1], 2); + } + else + { + clib_memcpy_fast (teid8p, &dst0.as_u8[offset + 1], 4); + } } else { - for (index = offset; index < offset + 4; index++) + u8 *sp; + + qfi |= dst0.as_u8[offset] << shift; + qfi |= dst0.as_u8[offset + 1] >> (8 - shift); + + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) { - *teid8p = dst0.as_u8[index] << shift; - *teid8p |= dst0.as_u8[index + 1] >> (8 - shift); - teid8p++; + sp = (u8 *) & seq; + for (index = 0; index < 2; index++) + { + sp[index] = dst0.as_u8[offset + index + 1] << shift; + sp[index] |= + dst0.as_u8[offset + index + 2] >> (8 - shift); + } + } + else + { + for (index = 0; index < 4; index++) + { + *teid8p = dst0.as_u8[offset + index + 1] << shift; + *teid8p |= + dst0.as_u8[offset + index + 2] >> (8 - shift); + teid8p++; + } } - - qfi |= dst0.as_u8[offset + 4] << shift; - qfi |= dst0.as_u8[offset + 5] >> (8 - shift); } if (qfi) { hdrlen = sizeof (gtpu_exthdr_t) + sizeof (gtpu_pdu_session_t); - len0 += hdrlen; } + else if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + hdrlen = sizeof (gtpu_exthdr_t); + } + + if (gtpu_type == GTPU_TYPE_ECHO_REPLY) + { + hdrlen += sizeof (gtpu_recovery_ie); + } + + if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION)) + { + ip6_sr_tlv_t *tlv; + u16 ext_len; + + ext_len = ip6srv0->sr.length * 8; + + if (ext_len > + sizeof (ip6_address_t) * (ip6srv0->sr.last_entry + 1)) + { + tlv = + (ip6_sr_tlv_t *) ((u8 *) & ip6srv0->sr + + sizeof (ip6_sr_header_t) + + sizeof (ip6_address_t) * + (ip6srv0->sr.last_entry + 1)); + + if (tlv->type == SRH_TLV_USER_PLANE_CONTAINER) + { + user_plane_sub_tlv_t *sub_tlv; + + sub_tlv = (user_plane_sub_tlv_t *) tlv->value; + + ie_size = sub_tlv->length; + clib_memcpy_fast (ie_buf, sub_tlv->value, ie_size); + + hdrlen += ie_size; + } + } + } + + vlib_buffer_advance (b0, + (word) sizeof (ip6srv_combo_header_t) + + ip6srv0->sr.length * 8); + + // get length of encapsulated IPv6 packet (the remaining part) + p = vlib_buffer_get_current (b0); + + plen = len0 = vlib_buffer_length_in_chain (vm, b0); + + len0 += hdrlen; + hdrlen += sizeof (ip6_gtpu_header_t); vlib_buffer_advance (b0, -(word) hdrlen); @@ -1112,10 +1445,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm, hdr0->gtpu.teid = teid; hdr0->gtpu.length = clib_host_to_net_u16 (len0); - if (PREDICT_FALSE (tag != 0)) - { - gtp_type_set (&hdr0->gtpu, tag); - } + hdr0->gtpu.type = gtpu_type; if (qfi) { @@ -1123,6 +1453,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm, gtpu_pdu_session_t *sess; hdr0->gtpu.ver_flags |= GTPU_EXTHDR_FLAG; + hdr0->gtpu.ext->seq = 0; hdr0->gtpu.ext->npdu_num = 0; hdr0->gtpu.ext->nextexthdr = GTPU_EXTHDR_PDU_SESSION; @@ -1144,6 +1475,38 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm, sess->nextexthdr = 0; } + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + hdr0->gtpu.ver_flags |= GTPU_SEQ_FLAG; + hdr0->gtpu.ext->seq = seq; + hdr0->gtpu.ext->npdu_num = 0; + hdr0->gtpu.ext->nextexthdr = 0; + + if (gtpu_type == GTPU_TYPE_ECHO_REPLY) + { + gtpu_recovery_ie *recovery; + + recovery = + (gtpu_recovery_ie *) ((u8 *) hdr0 + + (hdrlen - + sizeof (gtpu_recovery_ie))); + recovery->type = GTPU_RECOVERY_IE_TYPE; + recovery->restart_counter = 0; + } + else if (gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + if (ie_size) + { + u8 *ie_ptr; + + ie_ptr = (u8 *) ((u8 *) hdr0 + (hdrlen - ie_size)); + clib_memcpy_fast (ie_ptr, ie_buf, ie_size); + } + } + } + hdr0->udp.length = clib_host_to_net_u16 (len0 + sizeof (udp_header_t) + sizeof @@ -1162,7 +1525,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm, (gtpu_header_t)); // UDP source port. - key = hash_memory (p, len0, 0); + key = hash_memory (p, plen < 40 ? plen : 40, 0); port = hash_uword_to_u16 (&key); hdr0->udp.src_port = port; @@ -1241,10 +1604,15 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, u8 gtpu_type = 0; u8 qfi; u8 *qfip = NULL; + u16 seq = 0; + u8 *seqp; u32 offset, shift; u32 hdrlen; ip6_header_t *encap = NULL; gtpu_pdu_session_t *sess = NULL; + int ie_size = 0; + u16 tlv_siz = 0; + u8 ie_buf[GTPU_IE_MAX_SIZ]; u32 next0 = SRV6_END_M_GTP6_D_NEXT_LOOKUP; @@ -1288,10 +1656,15 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, teid = hdr0->gtpu.teid; teidp = (u8 *) & teid; - if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG) + seqp = (u8 *) & seq; + + if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG)) { // Extention header. hdrlen += sizeof (gtpu_exthdr_t); + + seq = hdr0->gtpu.ext->seq; + if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION) { // PDU Session Container. @@ -1314,10 +1687,19 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, offset = ls_param->sr_prefixlen / 8; shift = ls_param->sr_prefixlen % 8; - offset += 1; if (PREDICT_TRUE (shift == 0)) { - clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4); + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + clib_memcpy_fast (&seg0.as_u8[offset + 1], seqp, 2); + } + else + { + clib_memcpy_fast (&seg0.as_u8[offset + 1], teidp, 4); + } + if (qfip) { qfi = @@ -1329,18 +1711,32 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, qfi |= SRV6_PDU_SESSION_U_BIT_MASK; } - seg0.as_u8[offset + 4] = qfi; + seg0.as_u8[offset] = qfi; } } else { int idx; - for (idx = 0; idx < 4; idx++) + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) { - seg0.as_u8[offset + idx] |= teidp[idx] >> shift; - seg0.as_u8[offset + idx + 1] |= - teidp[idx] << (8 - shift); + for (idx = 0; idx < 2; idx++) + { + seg0.as_u8[offset + idx + 1] |= seqp[idx] >> shift; + seg0.as_u8[offset + idx + 2] |= + seqp[idx] << (8 - shift); + } + } + else + { + for (idx = 0; idx < 4; idx++) + { + seg0.as_u8[offset + idx + 1] |= teidp[idx] >> shift; + seg0.as_u8[offset + idx + 2] |= + teidp[idx] << (8 - shift); + } } if (qfip) @@ -1354,12 +1750,32 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, qfi |= SRV6_PDU_SESSION_U_BIT_MASK; } - seg0.as_u8[offset + 4] |= qfi >> shift; - seg0.as_u8[offset + 5] |= qfi << (8 - shift); + seg0.as_u8[offset] |= qfi >> shift; + seg0.as_u8[offset + 1] |= qfi << (8 - shift); } } - // jump over variable length data + if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION)) + { + u16 payload_len; + + payload_len = clib_net_to_host_u16 (hdr0->gtpu.length); + if (payload_len != 0) + { + ie_size = + payload_len - (hdrlen - sizeof (ip6_gtpu_header_t)); + if (ie_size > 0) + { + u8 *ies; + + ies = (u8 *) ((u8 *) hdr0 + hdrlen); + clib_memcpy_fast (ie_buf, ies, ie_size); + hdrlen += ie_size; + } + } + } + + // jump over variable length data vlib_buffer_advance (b0, (word) hdrlen); // get length of encapsulated IPv6 packet (the remaining part) @@ -1411,6 +1827,16 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, } } + if (ie_size) + { + tlv_siz = + sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) + + ie_size; + + tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0); + hdr_len += tlv_siz; + } + // jump back to data[0] or pre_data if required vlib_buffer_advance (b0, -(word) hdr_len); @@ -1421,17 +1847,51 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, clib_memcpy_fast (ip6srv, sl->rewrite, vec_len (sl->rewrite)); - ip6srv->ip.src_address = src0; - ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE; + if (vec_len (sl->segments) > 1) + { + ip6srv->ip.src_address = src0; - ip6srv->sr.tag = - clib_host_to_net_u16 (srh_tagfield[gtpu_type]); + ip6srv->sr.tag = + clib_host_to_net_u16 (srh_tagfield[gtpu_type]); + + ip6srv->sr.segments_left += 1; + ip6srv->sr.last_entry += 1; + + ip6srv->sr.length += sizeof (ip6_address_t) / 8; + ip6srv->sr.segments[0] = seg0; + + clib_memcpy_fast (&ip6srv->sr.segments[1], + (u8 *) (sl->rewrite + + sizeof (ip6_header_t) + + sizeof (ip6_sr_header_t)), + vec_len (sl->segments) * + sizeof (ip6_address_t)); + } + else + { + ip6srv->ip.src_address = src0; + ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE; - ip6srv->sr.segments_left += 1; - ip6srv->sr.last_entry += 1; + ip6srv->sr.type = ROUTING_HEADER_TYPE_SR; + ip6srv->sr.segments_left = 1; + ip6srv->sr.last_entry = 0; + ip6srv->sr.length = + ((sizeof (ip6_sr_header_t) + + sizeof (ip6_address_t)) / 8) - 1; + ip6srv->sr.flags = 0; - ip6srv->sr.length += sizeof (ip6_address_t) / 8; - ip6srv->sr.segments[0] = seg0; + ip6srv->sr.tag = + clib_host_to_net_u16 (srh_tagfield[gtpu_type]); + + ip6srv->sr.segments[0] = seg0; + + if (vec_len (sl->segments)) + { + ip6srv->sr.segments[1] = sl->segments[0]; + ip6srv->sr.last_entry++; + ip6srv->sr.length += sizeof (ip6_address_t) / 8; + } + } if (PREDICT_TRUE (encap != NULL)) { @@ -1472,20 +1932,13 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, } else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP) { - ip6srv->sr.protocol = IP_PROTOCOL_NONE; + ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET; } } else { - ip6srv->sr.protocol = IP_PROTOCOL_NONE; + ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET; } - - clib_memcpy_fast (&ip6srv->sr.segments[1], - (u8 *) (sl->rewrite + - sizeof (ip6_header_t) + - sizeof (ip6_sr_header_t)), - vec_len (sl->segments) * - sizeof (ip6_address_t)); } else { @@ -1499,7 +1952,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, { ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE; - ip6srv->sr.protocol = IP_PROTOCOL_NONE; + ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET; ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]); @@ -1547,11 +2000,30 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm, } else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP) { - ip6srv->ip.protocol = IP_PROTOCOL_NONE; + ip6srv->ip.protocol = IP_PROTOCOL_IP6_ETHERNET; } } } + if (PREDICT_FALSE (ie_size)) + { + ip6_sr_tlv_t *tlv; + user_plane_sub_tlv_t *sub_tlv; + + tlv = + (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz)); + tlv->type = SRH_TLV_USER_PLANE_CONTAINER; + tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t)); + clib_memset (tlv->value, 0, tlv->length); + + sub_tlv = (user_plane_sub_tlv_t *) tlv->value; + sub_tlv->type = USER_PLANE_SUB_TLV_IE; + sub_tlv->length = (u8) ie_size; + clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size); + + ip6srv->sr.length += (u8) (tlv_siz / 8); + } + ip6srv->ip.payload_length = clib_host_to_net_u16 (len0 + hdr_len - sizeof (ip6_header_t)); @@ -1636,10 +2108,15 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, u8 gtpu_type = 0; u8 qfi = 0; u8 *qfip = NULL; + u16 seq = 0; + u8 *seqp; u32 offset, shift; u32 hdrlen; ip6_header_t *encap = NULL; gtpu_pdu_session_t *sess; + int ie_size = 0; + u16 tlv_siz = 0; + u8 ie_buf[GTPU_IE_MAX_SIZ]; u32 next0 = SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP; @@ -1684,10 +2161,15 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, teid = hdr0->gtpu.teid; teidp = (u8 *) & teid; - if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG) + seqp = (u8 *) & seq; + + if (hdr0->gtpu.ver_flags & (GTPU_EXTHDR_FLAG | GTPU_SEQ_FLAG)) { // Extention header. hdrlen += sizeof (gtpu_exthdr_t); + + seq = hdr0->gtpu.ext->seq; + if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION) { // PDU Session Container. @@ -1708,10 +2190,18 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, offset = ls_param->sr_prefixlen / 8; shift = ls_param->sr_prefixlen % 8; - offset += 1; if (PREDICT_TRUE (shift == 0)) { - clib_memcpy_fast (&seg0.as_u8[offset], teidp, 4); + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) + { + clib_memcpy_fast (&seg0.as_u8[offset + 1], seqp, 2); + } + else + { + clib_memcpy_fast (&seg0.as_u8[offset + 1], teidp, 4); + } if (qfip) { @@ -1724,18 +2214,32 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, qfi |= SRV6_PDU_SESSION_U_BIT_MASK; } - seg0.as_u8[offset + 4] = qfi; + seg0.as_u8[offset] = qfi; } } else { int idx; - for (idx = 0; idx < 4; idx++) + if (gtpu_type == GTPU_TYPE_ECHO_REQUEST + || gtpu_type == GTPU_TYPE_ECHO_REPLY + || gtpu_type == GTPU_TYPE_ERROR_INDICATION) { - seg0.as_u8[offset + idx] |= teidp[idx] >> shift; - seg0.as_u8[offset + idx + 1] |= - teidp[idx] << (8 - shift); + for (idx = 0; idx < 2; idx++) + { + seg0.as_u8[offset + idx + 1] |= seqp[idx] >> shift; + seg0.as_u8[offset + idx + 2] |= + seqp[idx] << (8 - shift); + } + } + else + { + for (idx = 0; idx < 4; idx++) + { + seg0.as_u8[offset + idx + 1] |= teidp[idx] >> shift; + seg0.as_u8[offset + idx + 2] |= + teidp[idx] << (8 - shift); + } } if (qfip) @@ -1749,8 +2253,28 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, qfi |= SRV6_PDU_SESSION_U_BIT_MASK; } - seg0.as_u8[offset + 4] |= qfi >> shift; - seg0.as_u8[offset + 5] |= qfi << (8 - shift); + seg0.as_u8[offset] |= qfi >> shift; + seg0.as_u8[offset + 1] |= qfi << (8 - shift); + } + } + + if (PREDICT_FALSE (gtpu_type == GTPU_TYPE_ERROR_INDICATION)) + { + u16 payload_len; + + payload_len = clib_net_to_host_u16 (hdr0->gtpu.length); + if (payload_len != 0) + { + ie_size = + payload_len - (hdrlen - sizeof (ip6_gtpu_header_t)); + if (ie_size > 0) + { + u8 *ies; + + ies = (u8 *) ((u8 *) hdr0 + hdrlen); + clib_memcpy_fast (ie_buf, ies, ie_size); + hdrlen += ie_size; + } } } @@ -1797,6 +2321,16 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, hdr_len += sizeof (ip6_address_t) * 2; + if (ie_size) + { + tlv_siz = + sizeof (ip6_sr_tlv_t) + sizeof (user_plane_sub_tlv_t) + + ie_size; + + tlv_siz = (tlv_siz & ~0x07) + (tlv_siz & 0x07 ? 0x08 : 0x0); + hdr_len += tlv_siz; + } + // jump back to data[0] or pre_data if required vlib_buffer_advance (b0, -(word) hdr_len); @@ -1807,13 +2341,54 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, clib_memcpy_fast (ip6srv, sl->rewrite, vec_len (sl->rewrite)); - ip6srv->ip.src_address = src0; + if (vec_len (sl->segments) > 1) + { + ip6srv->ip.src_address = src0; - ip6srv->sr.tag = - clib_host_to_net_u16 (srh_tagfield[gtpu_type]); + ip6srv->sr.tag = + clib_host_to_net_u16 (srh_tagfield[gtpu_type]); + + ip6srv->sr.segments_left += 2; + ip6srv->sr.last_entry += 2; + + ip6srv->sr.length += ((sizeof (ip6_address_t) * 2) / 8); + + ip6srv->sr.segments[0] = dst0; + ip6srv->sr.segments[1] = seg0; + + clib_memcpy_fast (&ip6srv->sr.segments[2], + (u8 *) (sl->rewrite + + sizeof (ip6_header_t) + + sizeof (ip6_sr_header_t)), + vec_len (sl->segments) * + sizeof (ip6_address_t)); + } + else + { + ip6srv->ip.src_address = src0; + ip6srv->ip.protocol = IP_PROTOCOL_IPV6_ROUTE; - ip6srv->sr.segments_left += 2; - ip6srv->sr.last_entry += 2; + ip6srv->sr.type = ROUTING_HEADER_TYPE_SR; + ip6srv->sr.segments_left = 2; + ip6srv->sr.last_entry = 1; + ip6srv->sr.length = + ((sizeof (ip6_sr_header_t) + + 2 * sizeof (ip6_address_t)) / 8) - 1; + ip6srv->sr.flags = 0; + + ip6srv->sr.tag = + clib_host_to_net_u16 (srh_tagfield[gtpu_type]); + + ip6srv->sr.segments[0] = dst0; + ip6srv->sr.segments[1] = seg0; + + if (vec_len (sl->segments)) + { + ip6srv->sr.segments[2] = sl->segments[0]; + ip6srv->sr.last_entry++; + ip6srv->sr.length += sizeof (ip6_address_t) / 8; + } + } } else { @@ -1823,12 +2398,36 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, ip6srv->ip.src_address = src0; ip6srv->ip.dst_address = seg0; + ip6srv->sr.type = ROUTING_HEADER_TYPE_SR; + ip6srv->sr.segments_left = 1; + ip6srv->sr.last_entry = 0; + ip6srv->sr.length = + ((sizeof (ip6_sr_header_t) + + sizeof (ip6_address_t)) / 8) - 1; + ip6srv->sr.flags = 0; + ip6srv->sr.tag = clib_host_to_net_u16 (srh_tagfield[gtpu_type]); - ip6srv->sr.segments_left += 1; - ip6srv->sr.last_entry += 1; - ip6srv->sr.type = ROUTING_HEADER_TYPE_SR; + ip6srv->sr.segments[0] = dst0; + } + + if (PREDICT_FALSE (ie_size)) + { + ip6_sr_tlv_t *tlv; + user_plane_sub_tlv_t *sub_tlv; + + tlv = + (ip6_sr_tlv_t *) ((u8 *) ip6srv + (hdr_len - tlv_siz)); + tlv->type = SRH_TLV_USER_PLANE_CONTAINER; + tlv->length = (u8) (tlv_siz - sizeof (ip6_sr_tlv_t)); + clib_memset (tlv->value, 0, tlv->length); + + sub_tlv = (user_plane_sub_tlv_t *) tlv->value; + sub_tlv->length = (u8) (ie_size); + clib_memcpy_fast (sub_tlv->value, ie_buf, ie_size); + + ip6srv->sr.length += (u8) (tlv_siz / 8); } ip6srv->ip.payload_length = @@ -1874,26 +2473,12 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, } else if (ls_param->nhtype == SRV6_NHTYPE_NON_IP) { - ip6srv->sr.protocol = IP_PROTOCOL_NONE; + ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET; } } else { - ip6srv->sr.protocol = IP_PROTOCOL_NONE; - } - - ip6srv->sr.length += ((sizeof (ip6_address_t) * 2) / 8); - ip6srv->sr.segments[0] = dst0; - ip6srv->sr.segments[1] = seg0; - - if (sl) - { - clib_memcpy_fast (&ip6srv->sr.segments[2], - (u8 *) (sl->rewrite + - sizeof (ip6_header_t) + - sizeof (ip6_sr_header_t)), - vec_len (sl->segments) * - sizeof (ip6_address_t)); + ip6srv->sr.protocol = IP_PROTOCOL_IP6_ETHERNET; } good_n++; @@ -1938,6 +2523,443 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm, return frame->n_vectors; } +// Function for SRv6 GTP6.DT function +VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm, + vlib_node_runtime_t * node, + vlib_frame_t * frame) +{ + srv6_end_main_v6_dt_t *sm = &srv6_end_main_v6_dt; + ip6_sr_main_t *sm2 = &sr_main; + u32 n_left_from, next_index, *from, *to_next; + u32 thread_index = vm->thread_index; + + u32 good_n = 0, bad_n = 0; + + from = vlib_frame_vector_args (frame); + n_left_from = frame->n_vectors; + next_index = node->cached_next_index; + + while (n_left_from > 0) + { + u32 n_left_to_next; + + vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next); + + while (n_left_from > 0 && n_left_to_next > 0) + { + u32 bi0; + vlib_buffer_t *b0; + srv6_end_gtp6_dt_param_t *ls_param; + ip6_sr_localsid_t *ls0; + + ip6_gtpu_header_t *hdr0 = NULL; + ip4_header_t *ip4 = NULL; + ip6_header_t *ip6 = NULL; + ip6_address_t src, dst; + u32 teid; + u32 hdrlen; + u32 len0; + + u32 next0 = SRV6_END_M_GTP6_DT_NEXT_DROP; + + bi0 = from[0]; + to_next[0] = bi0; + from += 1; + to_next += 1; + n_left_from -= 1; + n_left_to_next -= 1; + + b0 = vlib_get_buffer (vm, bi0); + ls0 = + pool_elt_at_index (sm2->localsids, + vnet_buffer (b0)->ip.adj_index[VLIB_TX]); + + ls_param = (srv6_end_gtp6_dt_param_t *) ls0->plugin_mem; + + hdr0 = vlib_buffer_get_current (b0); + + hdrlen = sizeof (ip6_gtpu_header_t); + + len0 = vlib_buffer_length_in_chain (vm, b0); + + if ((hdr0->ip6.protocol != IP_PROTOCOL_UDP) + || (hdr0->udp.dst_port != + clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) + || (len0 < sizeof (ip6_gtpu_header_t))) + { + next0 = SRV6_END_M_GTP6_DT_NEXT_DROP; + + bad_n++; + } + else + { + clib_memcpy_fast (src.as_u8, hdr0->ip6.src_address.as_u8, + sizeof (ip6_address_t)); + clib_memcpy_fast (dst.as_u8, hdr0->ip6.dst_address.as_u8, + sizeof (ip6_address_t)); + + teid = hdr0->gtpu.teid; + + if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG) + { + hdrlen += sizeof (gtpu_exthdr_t); + if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION) + { + gtpu_pdu_session_t *sess; + + sess = + (gtpu_pdu_session_t *) (((char *) hdr0) + + sizeof (ip6_gtpu_header_t) + + sizeof (gtpu_exthdr_t)); + + hdrlen += sizeof (gtpu_pdu_session_t); + if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK) + { + hdrlen += sizeof (gtpu_paging_policy_t); + } + } + } + + if (ls_param->type == SRV6_GTP6_DT4) + { + vlib_buffer_advance (b0, (word) hdrlen); + ip4 = vlib_buffer_get_current (b0); + if ((ip4->ip_version_and_header_length & 0xf0) != 0x40) + { + next0 = SRV6_END_M_GTP6_DT_NEXT_DROP; + bad_n++; + goto DONE; + } + + next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP4; + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->fib4_index; + } + else if (ls_param->type == SRV6_GTP6_DT6) + { + ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen); + if ((clib_net_to_host_u32 + (ip6->ip_version_traffic_class_and_flow_label) >> 28) + != 6) + { + next0 = SRV6_END_M_GTP6_DT_NEXT_DROP; + bad_n++; + goto DONE; + } + + next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6; + if ((ip6->dst_address.as_u8[0] == 0xff) + && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)) + { + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->local_fib_index; + } + else + { + vlib_buffer_advance (b0, (word) hdrlen); + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->fib6_index; + } + } + else if (ls_param->type == SRV6_GTP6_DT46) + { + ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen); + if ((clib_net_to_host_u32 + (ip6->ip_version_traffic_class_and_flow_label) >> 28) + == 6) + { + next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6; + if ((ip6->dst_address.as_u8[0] == 0xff) + && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)) + { + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->local_fib_index; + } + else + { + vlib_buffer_advance (b0, (word) hdrlen); + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->fib6_index; + } + } + else + if ((clib_net_to_host_u32 + (ip6->ip_version_traffic_class_and_flow_label) >> 28) + == 4) + { + vlib_buffer_advance (b0, (word) hdrlen); + next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP4; + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->fib4_index; + } + else + { + next0 = SRV6_END_M_GTP6_DT_NEXT_DROP; + bad_n++; + goto DONE; + } + } + else + { + next0 = SRV6_END_M_GTP6_DT_NEXT_DROP; + bad_n++; + goto DONE; + } + + good_n++; + + if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) && + PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED)) + { + srv6_end_rewrite_trace_t *tr = + vlib_add_trace (vm, node, b0, sizeof (*tr)); + clib_memcpy (tr->src.as_u8, src.as_u8, + sizeof (ip6_address_t)); + clib_memcpy (tr->dst.as_u8, dst.as_u8, + sizeof (ip6_address_t)); + tr->teid = teid; + } + } + + DONE: + vlib_increment_combined_counter + (((next0 == + SRV6_END_M_GTP6_DT_NEXT_DROP) ? &(sm2->sr_ls_invalid_counters) + : &(sm2->sr_ls_valid_counters)), thread_index, + ls0 - sm2->localsids, 1, vlib_buffer_length_in_chain (vm, b0)); + + vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next, + n_left_to_next, bi0, next0); + } + + vlib_put_next_frame (vm, node, next_index, n_left_to_next); + } + + vlib_node_increment_counter (vm, sm->end_m_gtp6_dt_node_index, + SRV6_END_ERROR_M_GTP6_DT_BAD_PACKETS, bad_n); + + vlib_node_increment_counter (vm, sm->end_m_gtp6_dt_node_index, + SRV6_END_ERROR_M_GTP6_DT_PACKETS, good_n); + + return frame->n_vectors; +} + +// Function for SRv6 GTP4.DT function +VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm, + vlib_node_runtime_t * node, + vlib_frame_t * frame) +{ + srv6_t_main_v4_dt_t *sm = &srv6_t_main_v4_dt; + ip6_sr_main_t *sm2 = &sr_main; + u32 n_left_from, next_index, *from, *to_next; + + u32 good_n = 0, bad_n = 0; + + from = vlib_frame_vector_args (frame); + n_left_from = frame->n_vectors; + next_index = node->cached_next_index; + + while (n_left_from > 0) + { + u32 n_left_to_next; + + vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next); + + while (n_left_from > 0 && n_left_to_next > 0) + { + u32 bi0; + vlib_buffer_t *b0; + srv6_t_gtp4_dt_param_t *ls_param; + ip6_sr_sl_t *ls0; + + ip4_gtpu_header_t *hdr0 = NULL; + ip4_header_t *ip4 = NULL; + ip6_header_t *ip6 = NULL; + ip6_address_t src, dst; + u32 teid; + u32 hdrlen; + u32 len0; + + u32 next0 = SRV6_T_M_GTP4_DT_NEXT_DROP; + + bi0 = from[0]; + to_next[0] = bi0; + from += 1; + to_next += 1; + n_left_from -= 1; + n_left_to_next -= 1; + + b0 = vlib_get_buffer (vm, bi0); + ls0 = + pool_elt_at_index (sm2->sid_lists, + vnet_buffer (b0)->ip.adj_index[VLIB_TX]); + + ls_param = (srv6_t_gtp4_dt_param_t *) ls0->plugin_mem; + + hdr0 = vlib_buffer_get_current (b0); + + hdrlen = sizeof (ip4_gtpu_header_t); + + len0 = vlib_buffer_length_in_chain (vm, b0); + + if ((hdr0->ip4.protocol != IP_PROTOCOL_UDP) + || (hdr0->udp.dst_port != + clib_host_to_net_u16 (SRV6_GTP_UDP_DST_PORT)) + || (len0 < sizeof (ip4_gtpu_header_t))) + { + next0 = SRV6_T_M_GTP4_DT_NEXT_DROP; + + bad_n++; + } + else + { + clib_memcpy_fast (src.as_u8, hdr0->ip4.src_address.as_u8, + sizeof (ip4_address_t)); + clib_memcpy_fast (dst.as_u8, hdr0->ip4.dst_address.as_u8, + sizeof (ip4_address_t)); + + teid = hdr0->gtpu.teid; + + if (hdr0->gtpu.ver_flags & GTPU_EXTHDR_FLAG) + { + hdrlen += sizeof (gtpu_exthdr_t); + if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION) + { + gtpu_pdu_session_t *sess; + + sess = + (gtpu_pdu_session_t *) (((char *) hdr0) + + sizeof (ip6_gtpu_header_t) + + sizeof (gtpu_exthdr_t)); + + hdrlen += sizeof (gtpu_pdu_session_t); + if (sess->u.val & GTPU_PDU_SESSION_P_BIT_MASK) + { + hdrlen += sizeof (gtpu_paging_policy_t); + } + } + } + + if (ls_param->type == SRV6_GTP4_DT4) + { + vlib_buffer_advance (b0, (word) hdrlen); + ip4 = vlib_buffer_get_current (b0); + if ((ip4->ip_version_and_header_length & 0xf0) != 0x40) + { + next0 = SRV6_T_M_GTP4_DT_NEXT_DROP; + bad_n++; + goto DONE; + } + + next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4; + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->fib4_index; + } + else if (ls_param->type == SRV6_GTP4_DT6) + { + ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen); + if ((clib_net_to_host_u32 + (ip6->ip_version_traffic_class_and_flow_label) >> 28) + != 6) + { + next0 = SRV6_T_M_GTP4_DT_NEXT_DROP; + bad_n++; + goto DONE; + } + + next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6; + if ((ip6->dst_address.as_u8[0] == 0xff) + && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)) + { + next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4; + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->local_fib_index; + } + else + { + vlib_buffer_advance (b0, (word) hdrlen); + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->fib6_index; + } + } + else if (ls_param->type == SRV6_GTP4_DT46) + { + ip6 = (ip6_header_t *) ((u8 *) hdr0 + hdrlen); + if ((clib_net_to_host_u32 + (ip6->ip_version_traffic_class_and_flow_label) >> 28) + == 6) + { + next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6; + if ((ip6->dst_address.as_u8[0] == 0xff) + && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80)) + { + next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4; + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->local_fib_index; + } + else + { + vlib_buffer_advance (b0, (word) hdrlen); + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->fib6_index; + } + } + else + if ((clib_net_to_host_u32 + (ip6->ip_version_traffic_class_and_flow_label) >> 28) + == 4) + { + vlib_buffer_advance (b0, (word) hdrlen); + next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4; + vnet_buffer (b0)->sw_if_index[VLIB_TX] = + ls_param->fib4_index; + } + else + { + next0 = SRV6_T_M_GTP4_DT_NEXT_DROP; + bad_n++; + goto DONE; + } + } + else + { + next0 = SRV6_T_M_GTP4_DT_NEXT_DROP; + bad_n++; + goto DONE; + } + + good_n++; + + if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) && + PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED)) + { + srv6_end_rewrite_trace_t *tr = + vlib_add_trace (vm, node, b0, sizeof (*tr)); + clib_memcpy (tr->src.as_u8, src.as_u8, + sizeof (ip6_address_t)); + clib_memcpy (tr->dst.as_u8, dst.as_u8, + sizeof (ip6_address_t)); + tr->teid = teid; + } + } + + DONE: + vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next, + n_left_to_next, bi0, next0); + } + + vlib_put_next_frame (vm, node, next_index, n_left_to_next); + } + + vlib_node_increment_counter (vm, sm->t_m_gtp4_dt_node_index, + SRV6_T_ERROR_M_GTP4_DT_BAD_PACKETS, bad_n); + + vlib_node_increment_counter (vm, sm->t_m_gtp4_dt_node_index, + SRV6_T_ERROR_M_GTP4_DT_PACKETS, good_n); + + return frame->n_vectors; +} + VLIB_REGISTER_NODE (srv6_end_m_gtp6_e) = { .name = "srv6-end-m-gtp6-e",.vector_size = sizeof (u32),.format_trace = @@ -1946,8 +2968,8 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp6_e) = srv6_end_error_v6_e_strings,.n_next_nodes = SRV6_END_M_GTP6_E_N_NEXT,.next_nodes = { - [SRV6_END_M_GTP6_E_NEXT_DROP] = "error-drop", - [SRV6_END_M_GTP6_E_NEXT_LOOKUP] = "ip6-lookup",} + [SRV6_END_M_GTP6_E_NEXT_DROP] = + "error-drop",[SRV6_END_M_GTP6_E_NEXT_LOOKUP] = "ip6-lookup",} ,}; VLIB_REGISTER_NODE (srv6_end_m_gtp6_d) = @@ -1958,8 +2980,8 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp6_d) = srv6_end_error_v6_d_strings,.n_next_nodes = SRV6_END_M_GTP6_D_N_NEXT,.next_nodes = { - [SRV6_END_M_GTP6_D_NEXT_DROP] = "error-drop", - [SRV6_END_M_GTP6_D_NEXT_LOOKUP] = "ip6-lookup",} + [SRV6_END_M_GTP6_D_NEXT_DROP] = + "error-drop",[SRV6_END_M_GTP6_D_NEXT_LOOKUP] = "ip6-lookup",} ,}; VLIB_REGISTER_NODE (srv6_end_m_gtp6_d_di) = @@ -1974,6 +2996,34 @@ VLIB_REGISTER_NODE (srv6_end_m_gtp6_d_di) = [SRV6_END_M_GTP6_D_DI_NEXT_LOOKUP] = "ip6-lookup",} ,}; +VLIB_REGISTER_NODE (srv6_end_m_gtp6_dt) = +{ + .name = "srv6-end-m-gtp6-dt",.vector_size = sizeof (u32),.format_trace = + format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors = + ARRAY_LEN (srv6_end_error_v6_dt_strings),.error_strings = + srv6_end_error_v6_dt_strings,.n_next_nodes = + SRV6_END_M_GTP6_DT_N_NEXT,.next_nodes = + { + [SRV6_END_M_GTP6_DT_NEXT_DROP] = + "error-drop", + [SRV6_END_M_GTP6_DT_NEXT_LOOKUP4] + = "ip4-lookup",[SRV6_END_M_GTP6_DT_NEXT_LOOKUP6] = "ip6-lookup",} +,}; + +VLIB_REGISTER_NODE (srv6_t_m_gtp4_dt) = +{ + .name = "srv6-t-m-gtp4-dt",.vector_size = sizeof (u32),.format_trace = + format_srv6_end_rewrite_trace6,.type = VLIB_NODE_TYPE_INTERNAL,.n_errors = + ARRAY_LEN (srv6_t_error_v4_dt_strings),.error_strings = + srv6_t_error_v4_dt_strings,.n_next_nodes = + SRV6_T_M_GTP4_DT_N_NEXT,.next_nodes = + { + [SRV6_T_M_GTP4_DT_NEXT_DROP] = + "error-drop", + [SRV6_T_M_GTP4_DT_NEXT_LOOKUP4] = + "ip4-lookup",[SRV6_T_M_GTP4_DT_NEXT_LOOKUP6] = "ip6-lookup",} +,}; + /* * fd.io coding-style-patch-verification: ON *