ethernet: dmac filter checks secondary mac addrs

Maintain a list of secondary MAC addresses on ethernet_interface_t.
In ethernet-input dmac filtering, If packets do not match the
primary interface hardware address, check to see if they match the
other addresses.

Type: feature

Change-Id: Ie0edf45cae0d85c038a61086c47b3ae82d7e162d
Signed-off-by: Matthew Smith <mgsmith@netgate.com>

3 files changed, 186 insertions, 34 deletions

diff --git a/src/vnet/ethernet/ethernet.h b/src/vnet/ethernet/ethernet.h
index 344705b0cc3..821a576ddf5 100644
--- a/src/vnet/ethernet/ethernet.h
+++ b/src/vnet/ethernet/ethernet.h
@@ -154,6 +154,9 @@ typedef struct ethernet_interface
 
   /* Ethernet (MAC) address for this interface. */
   u8 address[6];
+
+  /* Secondary MAC addresses for this interface */
+  mac_address_t *secondary_addrs;
 } ethernet_interface_t;
 
 extern vnet_hw_interface_class_t ethernet_hw_interface_class;
@@ -310,6 +313,10 @@ ethernet_get_type_info (ethernet_main_t * em, ethernet_type_t type)
 
 ethernet_interface_t *ethernet_get_interface (ethernet_main_t * em,
 					      u32 hw_if_index);
+mac_address_t *ethernet_interface_add_del_address (ethernet_main_t * em,
+						   u32 hw_if_index,
+						   const u8 * address,
+						   u8 is_add);
 
 clib_error_t *ethernet_register_interface (vnet_main_t * vnm,
 					   u32 dev_class_index,
diff --git a/src/vnet/ethernet/interface.c b/src/vnet/ethernet/interface.c
index 0f54aa1e393..a0fa61c25f4 100644
--- a/src/vnet/ethernet/interface.c
+++ b/src/vnet/ethernet/interface.c
@@ -892,6 +892,53 @@ ethernet_get_interface (ethernet_main_t * em, u32 hw_if_index)
 	  index ? pool_elt_at_index (em->interfaces, i->hw_instance) : 0);
 }
 
+mac_address_t *
+ethernet_interface_add_del_address (ethernet_main_t * em,
+				    u32 hw_if_index, const u8 * address,
+				    u8 is_add)
+{
+  ethernet_interface_t *ei = ethernet_get_interface (em, hw_if_index);
+  mac_address_t *if_addr = 0;
+
+  /* return if there is not an ethernet interface for this hw interface */
+  if (!ei)
+    return 0;
+
+  /* determine whether the address is configured on the interface */
+  vec_foreach (if_addr, ei->secondary_addrs)
+  {
+    if (!ethernet_mac_address_equal (if_addr->bytes, address))
+      continue;
+
+    break;
+  }
+
+  if (if_addr && vec_is_member (ei->secondary_addrs, if_addr))
+    {
+      /* delete found address */
+      if (!is_add)
+	{
+	  vec_delete (ei->secondary_addrs, 1, if_addr - ei->secondary_addrs);
+	  if_addr = 0;
+	}
+      /* address already found, so nothing needs to be done if adding */
+    }
+  else
+    {
+      /* if_addr could be 0 or past the end of the vector. reset to 0 */
+      if_addr = 0;
+
+      /* add new address */
+      if (is_add)
+	{
+	  vec_add2 (ei->secondary_addrs, if_addr, 1);
+	  clib_memcpy (&if_addr->bytes, address, sizeof (if_addr->bytes));
+	}
+    }
+
+  return if_addr;
+}
+
 int
 vnet_delete_loopback_interface (u32 sw_if_index)
 {
diff --git a/src/vnet/ethernet/node.c b/src/vnet/ethernet/node.c
index 63ce0ce1c6c..488218e1a87 100755
--- a/src/vnet/ethernet/node.c
+++ b/src/vnet/ethernet/node.c
@@ -608,34 +608,80 @@ eth_input_tag_lookup (vlib_main_t * vm, vnet_main_t * vnm,
   l->n_bytes += vlib_buffer_length_in_chain (vm, b);
 }
 
+#define DMAC_MASK clib_net_to_host_u64 (0xFFFFFFFFFFFF0000)
+#define DMAC_IGBIT clib_net_to_host_u64 (0x0100000000000000)
+
+#ifdef CLIB_HAVE_VEC256
+static_always_inline u32
+is_dmac_bad_x4 (u64 * dmacs, u64 hwaddr)
+{
+  u64x4 r0 = u64x4_load_unaligned (dmacs) & u64x4_splat (DMAC_MASK);
+  r0 = (r0 != u64x4_splat (hwaddr)) & ((r0 & u64x4_splat (DMAC_IGBIT)) == 0);
+  return u8x32_msb_mask ((u8x32) (r0));
+}
+#else
+static_always_inline u8
+is_dmac_bad (u64 dmac, u64 hwaddr)
+{
+  u64 r0 = dmac & DMAC_MASK;
+  return (r0 != hwaddr) && ((r0 & DMAC_IGBIT) == 0);
+}
+#endif
+
+static_always_inline u8
+is_sec_dmac_bad (u64 dmac, u64 hwaddr)
+{
+  return ((dmac & DMAC_MASK) != hwaddr);
+}
+
+#ifdef CLIB_HAVE_VEC256
+static_always_inline u32
+is_sec_dmac_bad_x4 (u64 * dmacs, u64 hwaddr)
+{
+  u64x4 r0 = u64x4_load_unaligned (dmacs) & u64x4_splat (DMAC_MASK);
+  r0 = (r0 != u64x4_splat (hwaddr));
+  return u8x32_msb_mask ((u8x32) (r0));
+}
+#endif
+
+static_always_inline u8
+eth_input_sec_dmac_check_x1 (u64 hwaddr, u64 * dmac, u8 * dmac_bad)
+{
+  dmac_bad[0] &= is_sec_dmac_bad (dmac[0], hwaddr);
+  return dmac_bad[0];
+}
+
+static_always_inline u32
+eth_input_sec_dmac_check_x4 (u64 hwaddr, u64 * dmac, u8 * dmac_bad)
+{
+#ifdef CLIB_HAVE_VEC256
+  *(u32 *) (dmac_bad + 0) &= is_sec_dmac_bad_x4 (dmac + 0, hwaddr);
+#else
+  dmac_bad[0] &= is_sec_dmac_bad (dmac[0], hwaddr);
+  dmac_bad[1] &= is_sec_dmac_bad (dmac[1], hwaddr);
+  dmac_bad[2] &= is_sec_dmac_bad (dmac[2], hwaddr);
+  dmac_bad[3] &= is_sec_dmac_bad (dmac[3], hwaddr);
+#endif
+  return *(u32 *) dmac_bad;
+}
+
 static_always_inline void
 eth_input_process_frame_dmac_check (vnet_hw_interface_t * hi,
 				    u64 * dmacs, u8 * dmacs_bad,
-				    u32 n_packets)
+				    u32 n_packets, ethernet_interface_t * ei,
+				    u8 have_sec_dmac)
 {
-  u64 mask = clib_net_to_host_u64 (0xFFFFFFFFFFFF0000);
-  u64 igbit = clib_net_to_host_u64 (0x0100000000000000);
-  u64 hwaddr = (*(u64 *) hi->hw_address) & mask;
+  u64 hwaddr = (*(u64 *) hi->hw_address) & DMAC_MASK;
   u64 *dmac = dmacs;
   u8 *dmac_bad = dmacs_bad;
-
+  u32 bad = 0;
   i32 n_left = n_packets;
 
 #ifdef CLIB_HAVE_VEC256
-  u64x4 igbit4 = u64x4_splat (igbit);
-  u64x4 mask4 = u64x4_splat (mask);
-  u64x4 hwaddr4 = u64x4_splat (hwaddr);
   while (n_left > 0)
     {
-      u64x4 r0, r1;
-      r0 = u64x4_load_unaligned (dmac + 0) & mask4;
-      r1 = u64x4_load_unaligned (dmac + 4) & mask4;
-
-      r0 = (r0 != hwaddr4) & ((r0 & igbit4) == 0);
-      r1 = (r1 != hwaddr4) & ((r1 & igbit4) == 0);
-
-      *(u32 *) (dmac_bad + 0) = u8x32_msb_mask ((u8x32) (r0));
-      *(u32 *) (dmac_bad + 4) = u8x32_msb_mask ((u8x32) (r1));
+      bad |= *(u32 *) (dmac_bad + 0) = is_dmac_bad_x4 (dmac + 0, hwaddr);
+      bad |= *(u32 *) (dmac_bad + 4) = is_dmac_bad_x4 (dmac + 4, hwaddr);
 
       /* next */
       dmac += 8;
@@ -645,22 +691,10 @@ eth_input_process_frame_dmac_check (vnet_hw_interface_t * hi,
 #else
   while (n_left > 0)
     {
-      u64 r0, r1, r2, r3;
-
-      r0 = dmac[0] & mask;
-      r1 = dmac[1] & mask;
-      r2 = dmac[2] & mask;
-      r3 = dmac[3] & mask;
-
-      r0 = (r0 != hwaddr) && ((r0 & igbit) == 0);
-      r1 = (r1 != hwaddr) && ((r1 & igbit) == 0);
-      r2 = (r2 != hwaddr) && ((r2 & igbit) == 0);
-      r3 = (r3 != hwaddr) && ((r3 & igbit) == 0);
-
-      dmac_bad[0] = r0;
-      dmac_bad[1] = r1;
-      dmac_bad[2] = r2;
-      dmac_bad[3] = r3;
+      bad |= dmac_bad[0] = is_dmac_bad (dmac[0], hwaddr);
+      bad |= dmac_bad[1] = is_dmac_bad (dmac[1], hwaddr);
+      bad |= dmac_bad[2] = is_dmac_bad (dmac[2], hwaddr);
+      bad |= dmac_bad[3] = is_dmac_bad (dmac[3], hwaddr);
 
       /* next */
       dmac += 4;
@@ -668,6 +702,62 @@ eth_input_process_frame_dmac_check (vnet_hw_interface_t * hi,
       n_left -= 4;
     }
 #endif
+
+  if (have_sec_dmac && bad)
+    {
+      mac_address_t *addr;
+
+      vec_foreach (addr, ei->secondary_addrs)
+      {
+	u64 hwaddr = ((u64 *) addr)[0] & DMAC_MASK;
+	i32 n_left = n_packets;
+	u64 *dmac = dmacs;
+	u8 *dmac_bad = dmacs_bad;
+
+	bad = 0;
+
+	while (n_left > 0)
+	  {
+	    int adv = 0;
+	    int n_bad;
+
+	    /* skip any that have already matched */
+	    if (!dmac_bad[0])
+	      {
+		dmac += 1;
+		dmac_bad += 1;
+		n_left -= 1;
+		continue;
+	      }
+
+	    n_bad = clib_min (4, n_left);
+
+	    /* If >= 4 left, compare 4 together */
+	    if (n_bad == 4)
+	      {
+		bad |= eth_input_sec_dmac_check_x4 (hwaddr, dmac, dmac_bad);
+		adv = 4;
+		n_bad = 0;
+	      }
+
+	    /* handle individually */
+	    while (n_bad > 0)
+	      {
+		bad |= eth_input_sec_dmac_check_x1 (hwaddr, dmac + adv,
+						    dmac_bad + adv);
+		adv += 1;
+		n_bad -= 1;
+	      }
+
+	    dmac += adv;
+	    dmac_bad += adv;
+	    n_left -= adv;
+	  }
+
+	if (!bad)		/* can stop looping if everything matched */
+	  break;
+      }
+    }
 }
 
 /* process frame of buffers, store ethertype into array and update
@@ -701,6 +791,7 @@ eth_input_process_frame (vlib_main_t * vm, vlib_node_runtime_t * node,
   i32 n_left = n_packets;
   vlib_buffer_t *b[20];
   u32 *from;
+  ethernet_interface_t *ei = ethernet_get_interface (em, hi->hw_if_index);
 
   from = buffer_indices;
 
@@ -767,7 +858,14 @@ eth_input_process_frame (vlib_main_t * vm, vlib_node_runtime_t * node,
     }
 
   if (dmac_check)
-    eth_input_process_frame_dmac_check (hi, dmacs, dmacs_bad, n_packets);
+    {
+      if (vec_len (ei->secondary_addrs))
+	eth_input_process_frame_dmac_check (hi, dmacs, dmacs_bad, n_packets,
+					    ei, 1 /* have_sec_dmac */ );
+      else
+	eth_input_process_frame_dmac_check (hi, dmacs, dmacs_bad, n_packets,
+					    ei, 0 /* have_sec_dmac */ );
+    }
 
   next_ip4 = em->l3_next.input_next_ip4;
   next_ip6 = em->l3_next.input_next_ip6;