srv6-mobile: Support GTP4/6.DT and User Plane message mapping

[vpp.git] / src / plugins / srv6-mobile / node.c

diff --git a/src/plugins/srv6-mobile/node.c b/src/plugins/srv6-mobile/node.c
index 0120c67..6d3240e 100644 (file)
--- a/src/plugins/srv6-mobile/node.c
+++ b/src/plugins/srv6-mobile/node.c
@@ -400,14 +400,15 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
              u16 ie_size = 0;
              u8 ie_buf[GTPU_IE_MAX_SIZ];
              void *p;
+             uword plen;
 
              if (ip6srv0->ip.protocol == IPPROTO_IPV6_ROUTE)
                {
                  tag = ip6srv0->sr.tag;
                }
 
-             offset = ls0->localsid_len / 8;
-             shift = ls0->localsid_len % 8;
+             offset = ls0->localsid_prefix_len / 8;
+             shift = ls0->localsid_prefix_len % 8;
 
              gtpu_type = gtpu_type_get (tag);
 
@@ -450,7 +451,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
                      for (index = 0; index < 2; index++)
                        {
                          sp[index] = dst0.as_u8[offset + 5 + index] << shift;
-                         sp[index] =
+                         sp[index] |=
                            dst0.as_u8[offset + 6 + index] >> (8 - shift);
                        }
                    }
@@ -527,7 +528,7 @@ VLIB_NODE_FN (srv6_end_m_gtp4_e) (vlib_main_t * vm,
              // get length of encapsulated IPv6 packet (the remaining part)
              p = vlib_buffer_get_current (b0);
 
-             len0 = vlib_buffer_length_in_chain (vm, b0);
+             plen = len0 = vlib_buffer_length_in_chain (vm, b0);
 
              len0 += hdrlen;
 
@@ -631,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;
 
@@ -763,7 +764,7 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
              u8 qfi = 0;
              u8 *qfip = NULL;
              u16 seq = 0;
-             u8 *seqp = NULL;
+             u8 *seqp;
              u32 offset, shift, index;
              ip6srv_combo_header_t *ip6srv;
              gtpu_pdu_session_t *sess = NULL;
@@ -779,6 +780,8 @@ 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 | GTPU_SEQ_FLAG))
@@ -787,7 +790,6 @@ VLIB_NODE_FN (srv6_t_m_gtp4_d) (vlib_main_t * vm,
                  hdr_len += sizeof (gtpu_exthdr_t);
 
                  seq = hdr->gtpu.ext->seq;
-                 seqp = (u8 *) & seq;
 
                  if (hdr->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
                    {
@@ -1077,12 +1079,12 @@ 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;
                    }
                }
              else
@@ -1096,7 +1098,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]);
@@ -1146,7 +1148,7 @@ 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;
                        }
                    }
                }
@@ -1267,6 +1269,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;
 
@@ -1318,7 +1321,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
              u16 ie_size = 0;
              u8 ie_buf[GTPU_IE_MAX_SIZ];
 
-             index = ls0->localsid_len;
+             index = ls0->localsid_prefix_len;
              index += 8;
              offset = index / 8;
              shift = index % 8;
@@ -1352,7 +1355,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
                      for (index = 0; index < 2; index++)
                        {
                          sp[index] = dst0.as_u8[offset + index] << shift;
-                         sp[index] =
+                         sp[index] |=
                            dst0.as_u8[offset + index + 1] >> (8 - shift);
                        }
                    }
@@ -1418,21 +1421,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_e) (vlib_main_t * vm,
                    }
                }
 
-             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));
-               }
+             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);
+             plen = len0 = vlib_buffer_length_in_chain (vm, b0);
 
              len0 += hdrlen;
 
@@ -1528,7 +1524,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;
 
@@ -1608,7 +1604,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
          u8 qfi;
          u8 *qfip = NULL;
          u16 seq = 0;
-         u8 *seqp = NULL;
+         u8 *seqp;
          u32 offset, shift;
          u32 hdrlen;
          ip6_header_t *encap = NULL;
@@ -1659,13 +1655,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
              teid = hdr0->gtpu.teid;
              teidp = (u8 *) & teid;
 
+             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;
-                 seqp = (u8 *) & seq;
 
                  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
                    {
@@ -1696,8 +1693,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     if (seqp)
-                       clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
+                     clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
                    }
                  else
                    {
@@ -1726,14 +1722,11 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d) (vlib_main_t * vm,
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     if (seqp)
+                     for (idx = 0; idx < 2; idx++)
                        {
-                         for (idx = 0; idx < 2; idx++)
-                           {
-                             seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
-                             seg0.as_u8[offset + idx + 1] |=
-                               seqp[idx] << (8 - shift);
-                           }
+                         seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
+                         seg0.as_u8[offset + idx + 1] |=
+                           seqp[idx] << (8 - shift);
                        }
                    }
                  else
@@ -1937,12 +1930,12 @@ 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;
                    }
                }
              else
@@ -1957,7 +1950,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]);
@@ -2005,7 +1998,7 @@ 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;
                        }
                    }
                }
@@ -2114,7 +2107,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
          u8 qfi = 0;
          u8 *qfip = NULL;
          u16 seq = 0;
-         u8 *seqp = NULL;
+         u8 *seqp;
          u32 offset, shift;
          u32 hdrlen;
          ip6_header_t *encap = NULL;
@@ -2166,13 +2159,14 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
              teid = hdr0->gtpu.teid;
              teidp = (u8 *) & teid;
 
+             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;
-                 seqp = (u8 *) & seq;
 
                  if (hdr0->gtpu.ext->nextexthdr == GTPU_EXTHDR_PDU_SESSION)
                    {
@@ -2201,8 +2195,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     if (seqp)
-                       clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
+                     clib_memcpy_fast (&seg0.as_u8[offset], seqp, 2);
                    }
                  else
                    {
@@ -2231,14 +2224,11 @@ VLIB_NODE_FN (srv6_end_m_gtp6_d_di) (vlib_main_t * vm,
                      || gtpu_type == GTPU_TYPE_ECHO_REPLY
                      || gtpu_type == GTPU_TYPE_ERROR_INDICATION)
                    {
-                     if (seqp)
+                     for (idx = 0; idx < 2; idx++)
                        {
-                         for (idx = 0; idx < 2; idx++)
-                           {
-                             seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
-                             seg0.as_u8[offset + idx + 1] |=
-                               seqp[idx] << (8 - shift);
-                           }
+                         seg0.as_u8[offset + idx] |= seqp[idx] >> shift;
+                         seg0.as_u8[offset + idx + 1] |=
+                           seqp[idx] << (8 - shift);
                        }
                    }
                  else
@@ -2480,12 +2470,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.protocol = IP_PROTOCOL_IP6_ETHERNET;
                }
 
              good_n++;
@@ -2655,7 +2645,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
                    }
 
                  next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
-                 if ((ip6->dst_address.as_u8[0] == 0xfe)
+                 if ((ip6->dst_address.as_u8[0] == 0xff)
                      && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
                    {
                      vnet_buffer (b0)->sw_if_index[VLIB_TX] =
@@ -2676,7 +2666,7 @@ VLIB_NODE_FN (srv6_end_m_gtp6_dt) (vlib_main_t * vm,
                      == 6)
                    {
                      next0 = SRV6_END_M_GTP6_DT_NEXT_LOOKUP6;
-                     if ((ip6->dst_address.as_u8[0] == 0xfe)
+                     if ((ip6->dst_address.as_u8[0] == 0xff)
                          && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
                        {
                          vnet_buffer (b0)->sw_if_index[VLIB_TX] =
@@ -2875,7 +2865,7 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
                    }
 
                  next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
-                 if ((ip6->dst_address.as_u8[0] == 0xfe)
+                 if ((ip6->dst_address.as_u8[0] == 0xff)
                      && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
                    {
                      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;
@@ -2897,7 +2887,7 @@ VLIB_NODE_FN (srv6_t_m_gtp4_dt) (vlib_main_t * vm,
                      == 6)
                    {
                      next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP6;
-                     if ((ip6->dst_address.as_u8[0] == 0xfe)
+                     if ((ip6->dst_address.as_u8[0] == 0xff)
                          && ((ip6->dst_address.as_u8[1] & 0xc0) == 0x80))
                        {
                          next0 = SRV6_T_M_GTP4_DT_NEXT_LOOKUP4;