ipsec: fast path outbound policy matching implementation for ipv6

With this patch fast path for ipv6 policy lookup is enabled.
This impelentation scales and outperforms original implementation when
the number of defined flows is higher thatn 100k.

Type: feature
Signed-off-by: Piotr Bronowski <piotrx.bronowski@intel.com>
Change-Id: I9364b5b8db4fc708790d48c538add272c7cea400

8 files changed, 227 insertions, 171 deletions

diff --git a/src/vnet/ipsec/ipsec.c b/src/vnet/ipsec/ipsec.c
index 26761a432dd..3c22fbb87dd 100644
--- a/src/vnet/ipsec/ipsec.c
+++ b/src/vnet/ipsec/ipsec.c
@@ -483,7 +483,9 @@ ipsec_init (vlib_main_t * vm)
   if ((error = vlib_call_init_function (vm, ipsec_cli_init)))
     return error;
 
-  im->fp_spd_is_enabled = 0;
+  im->ipv4_fp_spd_is_enabled = 0;
+  im->ipv6_fp_spd_is_enabled = 0;
+
   im->fp_lookup_hash_buckets = IPSEC_FP_HASH_LOOKUP_HASH_BUCKETS;
 
   vec_validate (im->crypto_algs, IPSEC_CRYPTO_N_ALG - 1);
@@ -641,13 +643,19 @@ ipsec_config (vlib_main_t *vm, unformat_input_t *input)
 
   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
     {
-      if (unformat (input, "ipv4-outbound-spd-fast-path on"))
+      if (unformat (input, "ipv6-outbound-spd-fast-path on"))
+	{
+	  im->ipv6_fp_spd_is_enabled = 1;
+	}
+      else if (unformat (input, "ipv6-outbound-spd-fast-path off"))
+	im->ipv6_fp_spd_is_enabled = 0;
+      else if (unformat (input, "ipv4-outbound-spd-fast-path on"))
 	{
-	  im->fp_spd_is_enabled = 1;
+	  im->ipv4_fp_spd_is_enabled = 1;
 	  im->output_flow_cache_flag = 0;
 	}
       else if (unformat (input, "ipv4-outbound-spd-fast-path off"))
-	im->fp_spd_is_enabled = 0;
+	im->ipv4_fp_spd_is_enabled = 0;
       else if (unformat (input, "spd-fast-path-num-buckets %d",
 			 &ipsec_spd_fp_num_buckets))
 	{
@@ -656,7 +664,7 @@ ipsec_config (vlib_main_t *vm, unformat_input_t *input)
 				       << max_log2 (ipsec_spd_fp_num_buckets);
 	}
       else if (unformat (input, "ipv4-outbound-spd-flow-cache on"))
-	im->output_flow_cache_flag = im->fp_spd_is_enabled ? 0 : 1;
+	im->output_flow_cache_flag = im->ipv4_fp_spd_is_enabled ? 0 : 1;
       else if (unformat (input, "ipv4-outbound-spd-flow-cache off"))
 	im->output_flow_cache_flag = 0;
       else if (unformat (input, "ipv4-outbound-spd-hash-buckets %d",
diff --git a/src/vnet/ipsec/ipsec.h b/src/vnet/ipsec/ipsec.h
index c4f5326c962..fc7b6cd2454 100644
--- a/src/vnet/ipsec/ipsec.h
+++ b/src/vnet/ipsec/ipsec.h
@@ -144,7 +144,9 @@ typedef struct
   /* pool of policies */
   ipsec_policy_t *policies;
 
-  u32 fp_spd_is_enabled;
+  u32 ipv4_fp_spd_is_enabled;
+  u32 ipv6_fp_spd_is_enabled;
+
   ipsec_fp_mask_type_entry_t *fp_mask_types;
   u32 fp_lookup_hash_buckets; /* number of buckets should be power of two */
 
diff --git a/src/vnet/ipsec/ipsec_output.c b/src/vnet/ipsec/ipsec_output.c
index 96c6f27fee1..028d9761c07 100644
--- a/src/vnet/ipsec/ipsec_output.c
+++ b/src/vnet/ipsec/ipsec_output.c
@@ -64,91 +64,6 @@ format_ipsec_output_trace (u8 * s, va_list * args)
   return s;
 }
 
-always_inline uword
-ip6_addr_match_range (ip6_address_t * a, ip6_address_t * la,
-		      ip6_address_t * ua)
-{
-  if ((memcmp (a->as_u64, la->as_u64, 2 * sizeof (u64)) >= 0) &&
-      (memcmp (a->as_u64, ua->as_u64, 2 * sizeof (u64)) <= 0))
-    return 1;
-  return 0;
-}
-
-always_inline void
-ipsec_fp_5tuple_from_ip6_range (ipsec_fp_5tuple_t *tuple, ip6_address_t *la,
-				ip6_address_t *ra, u16 lp, u16 rp, u8 pr)
-
-{
-  clib_memcpy_fast (&tuple->ip6_laddr, la, sizeof (ip6_address_t));
-  clib_memcpy_fast (&tuple->ip6_laddr, la, sizeof (ip6_address_t));
-
-  tuple->lport = lp;
-  tuple->rport = rp;
-  tuple->protocol = pr;
-  tuple->is_ipv6 = 1;
-}
-
-always_inline ipsec_policy_t *
-ipsec6_output_policy_match (ipsec_spd_t * spd,
-			    ip6_address_t * la,
-			    ip6_address_t * ra, u16 lp, u16 rp, u8 pr)
-{
-  ipsec_main_t *im = &ipsec_main;
-  ipsec_policy_t *p;
-  ipsec_policy_t *policies[1];
-  ipsec_fp_5tuple_t tuples[1];
-  u32 fp_policy_ids[1];
-
-  u32 *i;
-
-  if (!spd)
-    return 0;
-
-  ipsec_fp_5tuple_from_ip6_range (&tuples[0], la, ra, lp, rp, pr);
-  if (im->fp_spd_is_enabled &&
-      (0 == ipsec_fp_out_policy_match_n (&spd->fp_spd, 1, tuples, policies,
-					 fp_policy_ids, 1)))
-    {
-      p = policies[0];
-      i = fp_policy_ids;
-    }
-
-  vec_foreach (i, spd->policies[IPSEC_SPD_POLICY_IP6_OUTBOUND])
-  {
-    p = pool_elt_at_index (im->policies, *i);
-    if (PREDICT_FALSE ((p->protocol != IPSEC_POLICY_PROTOCOL_ANY) &&
-		       (p->protocol != pr)))
-      continue;
-
-    if (!ip6_addr_match_range (ra, &p->raddr.start.ip6, &p->raddr.stop.ip6))
-      continue;
-
-    if (!ip6_addr_match_range (la, &p->laddr.start.ip6, &p->laddr.stop.ip6))
-      continue;
-
-    if (PREDICT_FALSE
-	((pr != IP_PROTOCOL_TCP) && (pr != IP_PROTOCOL_UDP)
-	 && (pr != IP_PROTOCOL_SCTP)))
-      return p;
-
-    if (lp < p->lport.start)
-      continue;
-
-    if (lp > p->lport.stop)
-      continue;
-
-    if (rp < p->rport.start)
-      continue;
-
-    if (rp > p->rport.stop)
-      continue;
-
-    return p;
-  }
-
-  return 0;
-}
-
 static inline uword
 ipsec_output_inline (vlib_main_t * vm, vlib_node_runtime_t * node,
 		     vlib_frame_t * from_frame, int is_ipv6)
diff --git a/src/vnet/ipsec/ipsec_output.h b/src/vnet/ipsec/ipsec_output.h
index 6608e3c6259..1239ed1f192 100644
--- a/src/vnet/ipsec/ipsec_output.h
+++ b/src/vnet/ipsec/ipsec_output.h
@@ -179,7 +179,7 @@ ipsec_output_policy_match_n (ipsec_spd_t *spd,
 
   clib_memset (policies, 0, n * sizeof (ipsec_policy_t *));
 
-  if (im->fp_spd_is_enabled)
+  if (im->ipv4_fp_spd_is_enabled)
     {
       ipsec_fp_5tuple_from_ip4_range_n (tuples, ip4_5tuples, n);
       counter += ipsec_fp_out_policy_match_n (&spd->fp_spd, 0, tuples,
@@ -330,12 +330,11 @@ ipsec_output_policy_match (ipsec_spd_t *spd, u8 pr, u32 la, u32 ra, u16 lp,
   if (!spd)
     return 0;
 
-  ipsec_fp_5tuple_from_ip4_range (&tuples[0], la, ra, lp, rp, pr);
-
-  if (im->fp_spd_is_enabled &&
-      (1 == ipsec_fp_out_policy_match_n (&spd->fp_spd, 0, tuples, policies,
-					 fp_policy_ids, 1)))
+  if (im->ipv4_fp_spd_is_enabled)
     {
+      ipsec_fp_5tuple_from_ip4_range (&tuples[0], la, ra, lp, rp, pr);
+      ipsec_fp_out_policy_match_n (&spd->fp_spd, 0, tuples, policies,
+				   fp_policy_ids, 1);
       p = policies[0];
       i = fp_policy_ids;
       if (PREDICT_FALSE ((pr != IP_PROTOCOL_TCP) && (pr != IP_PROTOCOL_UDP) &&
@@ -400,4 +399,88 @@ ipsec_output_policy_match (ipsec_spd_t *spd, u8 pr, u32 la, u32 ra, u16 lp,
   return 0;
 }
 
+always_inline uword
+ip6_addr_match_range (ip6_address_t *a, ip6_address_t *la, ip6_address_t *ua)
+{
+  if ((memcmp (a->as_u64, la->as_u64, 2 * sizeof (u64)) >= 0) &&
+      (memcmp (a->as_u64, ua->as_u64, 2 * sizeof (u64)) <= 0))
+    return 1;
+  return 0;
+}
+
+always_inline void
+ipsec_fp_5tuple_from_ip6_range (ipsec_fp_5tuple_t *tuple, ip6_address_t *la,
+				ip6_address_t *ra, u16 lp, u16 rp, u8 pr)
+
+{
+  clib_memcpy (&tuple->ip6_laddr, la, sizeof (ip6_address_t));
+  clib_memcpy (&tuple->ip6_raddr, ra, sizeof (ip6_address_t));
+
+  tuple->lport = lp;
+  tuple->rport = rp;
+  tuple->protocol = pr;
+  tuple->is_ipv6 = 1;
+}
+
+always_inline ipsec_policy_t *
+ipsec6_output_policy_match (ipsec_spd_t *spd, ip6_address_t *la,
+			    ip6_address_t *ra, u16 lp, u16 rp, u8 pr)
+{
+  ipsec_main_t *im = &ipsec_main;
+  ipsec_policy_t *p;
+  ipsec_policy_t *policies[1];
+  ipsec_fp_5tuple_t tuples[1];
+  u32 fp_policy_ids[1];
+
+  u32 *i;
+
+  if (!spd)
+    return 0;
+
+  if (im->ipv6_fp_spd_is_enabled)
+    {
+
+      ipsec_fp_5tuple_from_ip6_range (&tuples[0], la, ra, lp, rp, pr);
+      ipsec_fp_out_policy_match_n (&spd->fp_spd, 1, tuples, policies,
+				   fp_policy_ids, 1);
+      p = policies[0];
+      i = fp_policy_ids;
+      return p;
+    }
+
+  vec_foreach (i, spd->policies[IPSEC_SPD_POLICY_IP6_OUTBOUND])
+    {
+      p = pool_elt_at_index (im->policies, *i);
+      if (PREDICT_FALSE ((p->protocol != IPSEC_POLICY_PROTOCOL_ANY) &&
+			 (p->protocol != pr)))
+	continue;
+
+      if (!ip6_addr_match_range (ra, &p->raddr.start.ip6, &p->raddr.stop.ip6))
+	continue;
+
+      if (!ip6_addr_match_range (la, &p->laddr.start.ip6, &p->laddr.stop.ip6))
+	continue;
+
+      if (PREDICT_FALSE ((pr != IP_PROTOCOL_TCP) && (pr != IP_PROTOCOL_UDP) &&
+			 (pr != IP_PROTOCOL_SCTP)))
+	return p;
+
+      if (lp < p->lport.start)
+	continue;
+
+      if (lp > p->lport.stop)
+	continue;
+
+      if (rp < p->rport.start)
+	continue;
+
+      if (rp > p->rport.stop)
+	continue;
+
+      return p;
+    }
+
+  return 0;
+}
+
 #endif /* !IPSEC_OUTPUT_H */
diff --git a/src/vnet/ipsec/ipsec_spd.c b/src/vnet/ipsec/ipsec_spd.c
index aecb50dd346..22dddfd3fa8 100644
--- a/src/vnet/ipsec/ipsec_spd.c
+++ b/src/vnet/ipsec/ipsec_spd.c
@@ -46,22 +46,30 @@ ipsec_add_del_spd (vlib_main_t * vm, u32 spd_id, int is_add)
 #define _(s,v) vec_free(spd->policies[IPSEC_SPD_POLICY_##s]);
       foreach_ipsec_spd_policy_type
 #undef _
-	if (im->fp_spd_is_enabled)
+	if (im->ipv4_fp_spd_is_enabled)
       {
 	ipsec_spd_fp_t *fp_spd = &spd->fp_spd;
 
 	clib_bihash_free_16_8 (&fp_spd->fp_ip4_lookup_hash);
       }
+
+      if (im->ipv6_fp_spd_is_enabled)
+	{
+	  ipsec_spd_fp_t *fp_spd = &spd->fp_spd;
+
+	  clib_bihash_free_40_8 (&fp_spd->fp_ip6_lookup_hash);
+	}
+
       pool_put (im->spds, spd);
     }
-  else				/* create new SPD */
+  else /* create new SPD */
     {
       pool_get (im->spds, spd);
       clib_memset (spd, 0, sizeof (*spd));
       spd_index = spd - im->spds;
       spd->id = spd_id;
       hash_set (im->spd_index_by_spd_id, spd_id, spd_index);
-      if (im->fp_spd_is_enabled)
+      if (im->ipv4_fp_spd_is_enabled)
 	{
 	  ipsec_spd_fp_t *fp_spd = &spd->fp_spd;
 
@@ -70,6 +78,16 @@ ipsec_add_del_spd (vlib_main_t * vm, u32 spd_id, int is_add)
 	    im->fp_lookup_hash_buckets,
 	    im->fp_lookup_hash_buckets * IPSEC_FP_IP4_HASH_MEM_PER_BUCKET);
 	}
+      if (im->ipv6_fp_spd_is_enabled)
+	{
+	  ipsec_spd_fp_t *fp_spd = &spd->fp_spd;
+
+	  clib_bihash_init_40_8 (
+	    &fp_spd->fp_ip6_lookup_hash, "SPD_FP ip6 rules lookup bihash",
+	    im->fp_lookup_hash_buckets,
+	    im->fp_lookup_hash_buckets * IPSEC_FP_IP6_HASH_MEM_PER_BUCKET);
+	  fp_spd->fp_ip6_lookup_hash_initialized = 1;
+	}
     }
   return 0;
 }
diff --git a/src/vnet/ipsec/ipsec_spd_fp_lookup.h b/src/vnet/ipsec/ipsec_spd_fp_lookup.h
index 571a4b82fd2..912e18a3f8a 100644
--- a/src/vnet/ipsec/ipsec_spd_fp_lookup.h
+++ b/src/vnet/ipsec/ipsec_spd_fp_lookup.h
@@ -128,7 +128,7 @@ ipsec_fp_ip6_out_policy_match_n (void *spd_fp, ipsec_fp_5tuple_t *tuples,
   u64 *pkey, *pmatch, *pmask;
   ipsec_main_t *im = &ipsec_main;
   ipsec_spd_fp_t *pspd_fp = (ipsec_spd_fp_t *) spd_fp;
-  u32 *mask_type_ids = pspd_fp->fp_mask_types[IPSEC_SPD_POLICY_IP4_OUTBOUND];
+  u32 *mask_type_ids = pspd_fp->fp_mask_types[IPSEC_SPD_POLICY_IP6_OUTBOUND];
 
   /*clear the list of matched policies pointers */
   clib_memset (policies, 0, n * sizeof (*policies));
@@ -140,16 +140,15 @@ ipsec_fp_ip6_out_policy_match_n (void *spd_fp, ipsec_fp_5tuple_t *tuples,
 	{
 	  mte = im->fp_mask_types + *mti;
 
-	  pmatch = (u64 *) match;
-	  pmask = (u64 *) &mte->mask;
+	  pmatch = (u64 *) &match->ip6_laddr;
+	  pmask = (u64 *) &mte->mask.ip6_laddr;
 	  pkey = (u64 *) kv.key;
 
 	  *pkey++ = *pmatch++ & *pmask++;
 	  *pkey++ = *pmatch++ & *pmask++;
 	  *pkey++ = *pmatch++ & *pmask++;
 	  *pkey++ = *pmatch++ & *pmask++;
-	  *pkey++ = *pmatch++ & *pmask++;
-	  *pkey++ = *pmatch++ & *pmask++;
+	  *pkey = *pmatch & *pmask;
 
 	  int res = clib_bihash_search_inline_2_40_8 (
 	    &pspd_fp->fp_ip6_lookup_hash, &kv, &result);
diff --git a/src/vnet/ipsec/ipsec_spd_policy.c b/src/vnet/ipsec/ipsec_spd_policy.c
index 8cdbe3257d7..b198c205510 100644
--- a/src/vnet/ipsec/ipsec_spd_policy.c
+++ b/src/vnet/ipsec/ipsec_spd_policy.c
@@ -167,8 +167,10 @@ ipsec_add_del_policy (vlib_main_t * vm,
        * Try adding the policy into fast path SPD first. Only adding to
        * traditional SPD when failed.
        **/
-      if (im->fp_spd_is_enabled &&
-	  (policy->type == IPSEC_SPD_POLICY_IP4_OUTBOUND))
+      if ((im->ipv4_fp_spd_is_enabled &&
+	   policy->type == IPSEC_SPD_POLICY_IP4_OUTBOUND) ||
+	  (im->ipv6_fp_spd_is_enabled &&
+	   policy->type == IPSEC_SPD_POLICY_IP6_OUTBOUND))
 	return ipsec_fp_add_del_policy ((void *) &spd->fp_spd, policy, 1,
 					stat_index);
 
@@ -192,12 +194,11 @@ ipsec_add_del_policy (vlib_main_t * vm,
        * Try to delete the policy from the fast path SPD first. Delete from
        * traditional SPD when fp delete fails.
        **/
-      /**
-       * TODO: add ipv6 fast path support for outbound and
-       * ipv4/v6 inbound support for fast path
-       */
-      if (im->fp_spd_is_enabled &&
-	  (policy->type == IPSEC_SPD_POLICY_IP4_OUTBOUND))
+
+      if ((im->ipv4_fp_spd_is_enabled &&
+	   policy->type == IPSEC_SPD_POLICY_IP4_OUTBOUND) ||
+	  (im->ipv6_fp_spd_is_enabled &&
+	   policy->type == IPSEC_SPD_POLICY_IP6_OUTBOUND))
 	return ipsec_fp_add_del_policy ((void *) &spd->fp_spd, policy, 0,
 					stat_index);
 
@@ -247,27 +248,26 @@ find_mask_type_index (ipsec_main_t *im, ipsec_fp_5tuple_t *mask)
 }
 
 static_always_inline void
-fill_ip6_hash_policy_kv (ipsec_main_t *im, ipsec_fp_5tuple_t *match,
-			 ipsec_fp_5tuple_t *mask, clib_bihash_kv_40_8_t *kv)
+fill_ip6_hash_policy_kv (ipsec_fp_5tuple_t *match, ipsec_fp_5tuple_t *mask,
+			 clib_bihash_kv_40_8_t *kv)
 {
   ipsec_fp_lookup_value_t *kv_val = (ipsec_fp_lookup_value_t *) &kv->value;
-  u64 *pmatch = (u64 *) &match;
-  u64 *pmask = (u64 *) &mask;
+  u64 *pmatch = (u64 *) &match->ip6_laddr;
+  u64 *pmask = (u64 *) &mask->ip6_laddr;
   u64 *pkey = (u64 *) &kv->key;
 
   *pkey++ = *pmatch++ & *pmask++;
   *pkey++ = *pmatch++ & *pmask++;
   *pkey++ = *pmatch++ & *pmask++;
   *pkey++ = *pmatch++ & *pmask++;
-  *pkey++ = *pmatch++ & *pmask++;
-  *pkey++ = *pmatch++ & *pmask++;
+  *pkey = *pmatch & *pmask;
 
   kv_val->as_u64 = 0;
 }
 
 static_always_inline void
-fill_ip4_hash_policy_kv (ipsec_main_t *im, ipsec_fp_5tuple_t *match,
-			 ipsec_fp_5tuple_t *mask, clib_bihash_kv_16_8_t *kv)
+fill_ip4_hash_policy_kv (ipsec_fp_5tuple_t *match, ipsec_fp_5tuple_t *mask,
+			 clib_bihash_kv_16_8_t *kv)
 {
   ipsec_fp_lookup_value_t *kv_val = (ipsec_fp_lookup_value_t *) &kv->value;
   u64 *pmatch = (u64 *) &match->laddr;
@@ -301,6 +301,44 @@ get_highest_set_bit_u32 (u32 x)
   return x ^= x >> 1;
 }
 
+static_always_inline u64
+mask_out_highest_set_bit_u64 (u64 x)
+{
+  x |= x >> 32;
+  x |= x >> 16;
+  x |= x >> 8;
+  x |= x >> 4;
+  x |= x >> 2;
+  x |= x >> 1;
+  return ~x;
+}
+
+static_always_inline void
+ipsec_fp_get_policy_ports_mask (ipsec_policy_t *policy,
+				ipsec_fp_5tuple_t *mask)
+{
+  if (PREDICT_TRUE ((policy->protocol == IP_PROTOCOL_TCP) ||
+		    (policy->protocol == IP_PROTOCOL_UDP) ||
+		    (policy->protocol == IP_PROTOCOL_SCTP)))
+    {
+      mask->lport = policy->lport.start ^ policy->lport.stop;
+      mask->rport = policy->rport.start ^ policy->rport.stop;
+
+      mask->lport = get_highest_set_bit_u16 (mask->lport);
+      mask->lport = ~(mask->lport - 1) & (~mask->lport);
+
+      mask->rport = get_highest_set_bit_u16 (mask->rport);
+      mask->rport = ~(mask->rport - 1) & (~mask->rport);
+    }
+  else
+    {
+      mask->lport = 0;
+      mask->rport = 0;
+    }
+
+  mask->protocol = (policy->protocol == IPSEC_POLICY_PROTOCOL_ANY) ? 0 : ~0;
+}
+
 static_always_inline void
 ipsec_fp_ip4_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask)
 {
@@ -312,7 +350,7 @@ ipsec_fp_ip4_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask)
   u32 *prmask = (u32 *) &mask->raddr;
 
   memset (mask, 0, sizeof (mask->l3_zero_pad));
-  memset (plmask, 1, sizeof (*mask) - sizeof (mask->l3_zero_pad));
+  memset (plmask, 0xff, sizeof (*mask) - sizeof (mask->l3_zero_pad));
   /* find bits where start != stop */
   *plmask = *pladdr_start ^ *pladdr_stop;
   *prmask = *praddr_start ^ *praddr_stop;
@@ -349,49 +387,52 @@ ipsec_fp_ip4_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask)
   mask->protocol = (policy->protocol == IPSEC_POLICY_PROTOCOL_ANY) ? 0 : ~0;
 }
 
-static_always_inline int
+static_always_inline void
 ipsec_fp_ip6_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask)
 {
   u64 *pladdr_start = (u64 *) &policy->laddr.start;
   u64 *pladdr_stop = (u64 *) &policy->laddr.stop;
-  u64 *plmask = (u64 *) &mask->laddr;
+  u64 *plmask = (u64 *) &mask->ip6_laddr;
   u64 *praddr_start = (u64 *) &policy->raddr.start;
   u64 *praddr_stop = (u64 *) &policy->raddr.stop;
   u64 *prmask = (u64 *) &mask->ip6_raddr;
-  u16 *plport_start = (u16 *) &policy->lport.start;
-  u16 *plport_stop = (u16 *) &policy->lport.stop;
-  u16 *prport_start = (u16 *) &policy->rport.start;
-  u16 *prport_stop = (u16 *) &policy->rport.stop;
-
-  /* test if x is not power of 2. The test form is  !((x & (x - 1)) == 0) */
-  if (((*pladdr_stop - *pladdr_start + 1) & (*pladdr_stop - *pladdr_start)) &&
-      (((*(pladdr_stop + 1) - *(pladdr_start + 1)) + 1) &
-       (*(pladdr_stop + 1) - *(pladdr_start + 1))))
-    return -1;
 
-  if (((*praddr_stop - *praddr_start + 1) & (*praddr_stop - *praddr_start)) &&
-      (((*(praddr_stop + 1) - *(praddr_start + 1)) + 1) &
-       (*(praddr_stop + 1) - *(praddr_start + 1))))
-    return -1;
+  memset (mask, 0xff, sizeof (ipsec_fp_5tuple_t));
 
-  if (((*plport_stop - *plport_start + 1) & (*plport_stop - *plport_start)))
-    return -1;
+  *plmask = (*pladdr_start++ ^ *pladdr_stop++);
 
-  if (((*prport_stop - *prport_start + 1) & (*prport_stop - *prport_start)))
-    return -1;
+  *prmask = (*praddr_start++ ^ *praddr_stop++);
 
-  memset (mask, 1, sizeof (ipsec_fp_5tuple_t));
+  /* Find most significant bit set (that is the first position
+   * start differs from stop). Mask out everything after that bit and
+   * the bit itself. Remember that policy stores start and stop in the net
+   * order.
+   */
+  *plmask = clib_host_to_net_u64 (
+    mask_out_highest_set_bit_u64 (clib_net_to_host_u64 (*plmask)));
 
-  *plmask++ = ~(*pladdr_start++ ^ *pladdr_stop++);
-  *plmask++ = ~(*pladdr_start++ ^ *pladdr_stop++);
+  if (*plmask++ & clib_host_to_net_u64 (0x1))
+    {
+      *plmask = (*pladdr_start ^ *pladdr_stop);
+      *plmask = clib_host_to_net_u64 (
+	mask_out_highest_set_bit_u64 (clib_net_to_host_u64 (*plmask)));
+    }
+  else
+    *plmask = 0;
 
-  *prmask++ = ~(*praddr_start++ ^ *praddr_stop++);
-  *prmask++ = ~(*praddr_start++ ^ *praddr_stop++);
+  *prmask = clib_host_to_net_u64 (
+    mask_out_highest_set_bit_u64 (clib_net_to_host_u64 (*prmask)));
 
-  mask->lport = ~(policy->lport.start ^ policy->lport.stop);
-  mask->rport = ~(policy->rport.start ^ policy->rport.stop);
-  mask->protocol = 0;
-  return 0;
+  if (*prmask++ & clib_host_to_net_u64 (0x1))
+    {
+      *prmask = (*pladdr_start ^ *pladdr_stop);
+      *prmask = clib_host_to_net_u64 (
+	mask_out_highest_set_bit_u64 (clib_net_to_host_u64 (*prmask)));
+    }
+  else
+    *prmask = 0;
+
+  ipsec_fp_get_policy_ports_mask (policy, mask);
 }
 
 static_always_inline void
@@ -454,7 +495,7 @@ ipsec_fp_ip4_add_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
   policy->fp_mask_type_id = mask_index;
   ipsec_fp_get_policy_5tuple (policy, &policy_5tuple);
 
-  fill_ip4_hash_policy_kv (im, &policy_5tuple, &mask, &kv);
+  fill_ip4_hash_policy_kv (&policy_5tuple, &mask, &kv);
 
   res = clib_bihash_search_inline_2_16_8 (&fp_spd->fp_ip4_lookup_hash, &kv,
 					  &result);
@@ -523,19 +564,7 @@ ipsec_fp_ip6_add_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
 
   ipsec_fp_5tuple_t mask, policy_5tuple;
   int res;
-  /* u64 hash; */
-
-  if (PREDICT_FALSE (!fp_spd->fp_ip6_lookup_hash_initialized))
-    {
-      clib_bihash_init_40_8 (
-	&fp_spd->fp_ip6_lookup_hash, "SPD_FP ip6 rules lookup bihash",
-	im->fp_lookup_hash_buckets,
-	im->fp_lookup_hash_buckets * IPSEC_FP_IP6_HASH_MEM_PER_BUCKET);
-      fp_spd->fp_ip6_lookup_hash_initialized = 1;
-    }
-
-  if (ipsec_fp_ip6_get_policy_mask (policy, &mask) != 0)
-    return -1;
+  ipsec_fp_ip6_get_policy_mask (policy, &mask);
 
   pool_get (im->policies, vp);
   policy_index = vp - im->policies;
@@ -555,10 +584,9 @@ ipsec_fp_ip6_add_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
     mte = im->fp_mask_types + mask_index;
 
   policy->fp_mask_type_id = mask_index;
-  ipsec_fp_ip6_get_policy_mask (policy, &mask);
   ipsec_fp_get_policy_5tuple (policy, &policy_5tuple);
 
-  fill_ip6_hash_policy_kv (im, &policy_5tuple, &mask, &kv);
+  fill_ip6_hash_policy_kv (&policy_5tuple, &mask, &kv);
 
   res = clib_bihash_search_inline_2_40_8 (&fp_spd->fp_ip6_lookup_hash, &kv,
 					  &result);
@@ -626,7 +654,7 @@ ipsec_fp_ip6_del_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
 
   ipsec_fp_ip6_get_policy_mask (policy, &mask);
   ipsec_fp_get_policy_5tuple (policy, &policy_5tuple);
-  fill_ip6_hash_policy_kv (im, &policy_5tuple, &mask, &kv);
+  fill_ip6_hash_policy_kv (&policy_5tuple, &mask, &kv);
   res = clib_bihash_search_inline_2_40_8 (&fp_spd->fp_ip6_lookup_hash, &kv,
 					  &result);
   if (res != 0)
@@ -706,7 +734,7 @@ ipsec_fp_ip4_del_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
 
   ipsec_fp_ip4_get_policy_mask (policy, &mask);
   ipsec_fp_get_policy_5tuple (policy, &policy_5tuple);
-  fill_ip4_hash_policy_kv (im, &policy_5tuple, &mask, &kv);
+  fill_ip4_hash_policy_kv (&policy_5tuple, &mask, &kv);
   res = clib_bihash_search_inline_2_16_8 (&fp_spd->fp_ip4_lookup_hash, &kv,
 					  &result);
   if (res != 0)
diff --git a/src/vnet/ipsec/ipsec_spd_policy.h b/src/vnet/ipsec/ipsec_spd_policy.h
index 24c0e4cefb1..57985116c94 100644
--- a/src/vnet/ipsec/ipsec_spd_policy.h
+++ b/src/vnet/ipsec/ipsec_spd_policy.h
@@ -128,8 +128,11 @@ typedef union
 	ip4_address_t laddr;
 	ip4_address_t raddr;
       };
-      ip6_address_t ip6_laddr;
-      ip6_address_t ip6_raddr;
+      struct
+      {
+	ip6_address_t ip6_laddr;
+	ip6_address_t ip6_raddr;
+      };
     };
 
     u16 lport;