[aarch64] Fixes CLI crashes on dpaa2 platform.
[vpp.git] / src / vnet / devices / af_packet / node.c
index d3af41b..ec7c273 100644 (file)
@@ -1,5 +1,4 @@
-/*
- *------------------------------------------------------------------
+/*------------------------------------------------------------------
  * af_packet.c - linux kernel packet interface
  *
  * Copyright (c) 2016 Cisco and/or its affiliates.
@@ -18,6 +17,7 @@
  */
 
 #include <linux/if_packet.h>
+#include <linux/virtio_net.h>
 
 #include <vlib/vlib.h>
 #include <vlib/unix/unix.h>
@@ -58,7 +58,7 @@ format_af_packet_input_trace (u8 * s, va_list * args)
   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
   af_packet_input_trace_t *t = va_arg (*args, af_packet_input_trace_t *);
-  uword indent = format_get_indent (s);
+  u32 indent = format_get_indent (s);
 
   s = format (s, "af_packet: hw_if_index %d next-index %d",
              t->hw_if_index, t->next_index);
@@ -155,9 +155,18 @@ af_packet_device_input_fn (vlib_main_t * vm, vlib_node_runtime_t * node,
       while ((tph->tp_status & TP_STATUS_USER) && (n_free_bufs > min_bufs) &&
             n_left_to_next)
        {
+
+         struct virtio_net_hdr *vh =
+           (struct virtio_net_hdr *) (((u8 *) tph) + tph->tp_mac -
+                                      sizeof (struct virtio_net_hdr));
          u32 data_len = tph->tp_snaplen;
          u32 offset = 0;
          u32 bi0 = 0, first_bi0 = 0, prev_bi0;
+         u32 vlan_len = 0;
+         ip_csum_t wsum = 0;
+         u16 *wsum_addr = NULL;
+         u32 do_vnet = apm->flags & AF_PACKET_USES_VNET_HEADERS;
+         u32 do_csum = tph->tp_status & TP_STATUS_CSUMNOTREADY;
 
          while (data_len)
            {
@@ -173,12 +182,74 @@ af_packet_device_input_fn (vlib_main_t * vm, vlib_node_runtime_t * node,
              /* copy data */
              u32 bytes_to_copy =
                data_len > n_buffer_bytes ? n_buffer_bytes : data_len;
+             u32 bytes_copied = 0;
              b0->current_data = 0;
-             clib_memcpy (vlib_buffer_get_current (b0),
-                          (u8 *) tph + tph->tp_mac + offset, bytes_to_copy);
+             /* Kernel removes VLAN headers, so reconstruct VLAN */
+             if (PREDICT_FALSE (tph->tp_status & TP_STATUS_VLAN_VALID))
+               {
+                 if (PREDICT_TRUE (offset == 0))
+                   {
+                     clib_memcpy (vlib_buffer_get_current (b0),
+                                  (u8 *) tph + tph->tp_mac,
+                                  sizeof (ethernet_header_t));
+                     ethernet_header_t *eth = vlib_buffer_get_current (b0);
+                     ethernet_vlan_header_t *vlan =
+                       (ethernet_vlan_header_t *) (eth + 1);
+                     vlan->priority_cfi_and_id =
+                       clib_host_to_net_u16 (tph->tp_vlan_tci);
+                     vlan->type = eth->type;
+                     eth->type = clib_host_to_net_u16 (ETHERNET_TYPE_VLAN);
+                     vlan_len = sizeof (ethernet_vlan_header_t);
+                     bytes_copied = sizeof (ethernet_header_t);
+                   }
+               }
+             /* Check if the incoming skb is marked as CSUM_PARTIAL,
+              * If VNET Headers are enabled TP_STATUS_CSUMNOTREADY is
+              * equivalent to the vnet csum flag.
+              **/
+             if (PREDICT_TRUE ((do_vnet != 0) && (do_csum != 0)))
+               {
+                 wsum_addr = (u16 *) (((u8 *) vlib_buffer_get_current (b0)) +
+                                      vlan_len + vh->csum_start +
+                                      vh->csum_offset);
+                 if (bytes_copied <= vh->csum_start)
+                   {
+                     clib_memcpy (((u8 *) vlib_buffer_get_current (b0)) +
+                                  bytes_copied + vlan_len,
+                                  (u8 *) tph + tph->tp_mac + offset +
+                                  bytes_copied,
+                                  (vh->csum_start - bytes_copied));
+                     wsum =
+                       ip_csum_and_memcpy (wsum,
+                                           ((u8 *)
+                                            vlib_buffer_get_current (b0)) +
+                                           vh->csum_start + vlan_len,
+                                           (u8 *) tph + tph->tp_mac +
+                                           offset + vh->csum_start,
+                                           (bytes_to_copy - vh->csum_start));
+                   }
+                 else
+                   {
+                     wsum =
+                       ip_csum_and_memcpy (wsum,
+                                           ((u8 *)
+                                            vlib_buffer_get_current (b0)) +
+                                           bytes_copied + vlan_len,
+                                           (u8 *) tph + tph->tp_mac +
+                                           offset + bytes_copied,
+                                           (bytes_to_copy - bytes_copied));
+                   }
+               }
+             else
+               {
+                 clib_memcpy (((u8 *) vlib_buffer_get_current (b0)) +
+                              bytes_copied + vlan_len,
+                              (u8 *) tph + tph->tp_mac + offset +
+                              bytes_copied, (bytes_to_copy - bytes_copied));
+               }
 
              /* fill buffer header */
-             b0->current_length = bytes_to_copy;
+             b0->current_length = bytes_to_copy + vlan_len;
 
              if (offset == 0)
                {
@@ -195,6 +266,10 @@ af_packet_device_input_fn (vlib_main_t * vm, vlib_node_runtime_t * node,
              offset += bytes_to_copy;
              data_len -= bytes_to_copy;
            }
+         if (PREDICT_TRUE ((do_vnet != 0) && (do_csum != 0)))
+           {
+             *wsum_addr = ~ip_csum_fold (wsum);
+           }
          n_rx_packets++;
          n_rx_bytes += tph->tp_snaplen;
          to_next[0] = first_bi0;