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;