http_static: fix reply data leak
[vpp.git] / src / vnet / ipsec / ipsec_spd_policy.c
index 8cdbe32..af08768 100644 (file)
@@ -24,22 +24,6 @@ vlib_combined_counter_main_t ipsec_spd_policy_counters = {
   .stat_segment_name = "/net/ipsec/policy",
 };
 
-static int
-ipsec_spd_entry_sort (void *a1, void *a2)
-{
-  ipsec_main_t *im = &ipsec_main;
-  u32 *id1 = a1;
-  u32 *id2 = a2;
-  ipsec_policy_t *p1, *p2;
-
-  p1 = pool_elt_at_index (im->policies, *id1);
-  p2 = pool_elt_at_index (im->policies, *id2);
-  if (p1 && p2)
-    return p2->priority - p1->priority;
-
-  return 0;
-}
-
 int
 ipsec_policy_mk_type (bool is_outbound,
                      bool is_ipv6,
@@ -80,6 +64,44 @@ ipsec_policy_mk_type (bool is_outbound,
   return (-1);
 }
 
+static_always_inline int
+ipsec_is_policy_inbound (ipsec_policy_t *policy)
+{
+  if (policy->type == IPSEC_SPD_POLICY_IP4_INBOUND_PROTECT ||
+      policy->type == IPSEC_SPD_POLICY_IP4_INBOUND_BYPASS ||
+      policy->type == IPSEC_SPD_POLICY_IP4_INBOUND_DISCARD ||
+      policy->type == IPSEC_SPD_POLICY_IP6_INBOUND_PROTECT ||
+      policy->type == IPSEC_SPD_POLICY_IP6_INBOUND_BYPASS ||
+      policy->type == IPSEC_SPD_POLICY_IP6_INBOUND_DISCARD)
+    return 1;
+
+  return 0;
+}
+
+static_always_inline int
+ipsec_is_fp_enabled (ipsec_main_t *im, ipsec_spd_t *spd,
+                    ipsec_policy_t *policy)
+{
+  if ((im->fp_spd_ipv4_out_is_enabled &&
+       PREDICT_TRUE (INDEX_INVALID != spd->fp_spd.ip4_out_lookup_hash_idx) &&
+       policy->type == IPSEC_SPD_POLICY_IP4_OUTBOUND) ||
+      (im->fp_spd_ipv4_in_is_enabled &&
+       PREDICT_TRUE (INDEX_INVALID != spd->fp_spd.ip4_in_lookup_hash_idx) &&
+       (policy->type == IPSEC_SPD_POLICY_IP4_INBOUND_PROTECT ||
+       policy->type == IPSEC_SPD_POLICY_IP4_INBOUND_BYPASS ||
+       policy->type == IPSEC_SPD_POLICY_IP4_INBOUND_DISCARD)) ||
+      (im->fp_spd_ipv6_in_is_enabled &&
+       PREDICT_TRUE (INDEX_INVALID != spd->fp_spd.ip6_in_lookup_hash_idx) &&
+       (policy->type == IPSEC_SPD_POLICY_IP6_INBOUND_PROTECT ||
+       policy->type == IPSEC_SPD_POLICY_IP6_INBOUND_BYPASS ||
+       policy->type == IPSEC_SPD_POLICY_IP6_INBOUND_DISCARD)) ||
+      (im->fp_spd_ipv6_out_is_enabled &&
+       PREDICT_TRUE (INDEX_INVALID != spd->fp_spd.ip6_out_lookup_hash_idx) &&
+       policy->type == IPSEC_SPD_POLICY_IP6_OUTBOUND))
+    return 1;
+  return 0;
+}
+
 int
 ipsec_add_del_policy (vlib_main_t * vm,
                      ipsec_policy_t * policy, int is_add, u32 * stat_index)
@@ -151,6 +173,7 @@ ipsec_add_del_policy (vlib_main_t * vm,
   if (is_add)
     {
       u32 policy_index;
+      u32 i;
 
       if (policy->policy == IPSEC_POLICY_ACTION_PROTECT)
        {
@@ -167,8 +190,7 @@ 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 (ipsec_is_fp_enabled (im, spd, policy))
        return ipsec_fp_add_del_policy ((void *) &spd->fp_spd, policy, 1,
                                        stat_index);
 
@@ -179,9 +201,20 @@ ipsec_add_del_policy (vlib_main_t * vm,
       vlib_validate_combined_counter (&ipsec_spd_policy_counters,
                                      policy_index);
       vlib_zero_combined_counter (&ipsec_spd_policy_counters, policy_index);
-      vec_add1 (spd->policies[policy->type], policy_index);
-      vec_sort_with_function (spd->policies[policy->type],
-                             ipsec_spd_entry_sort);
+
+      vec_foreach_index (i, spd->policies[policy->type])
+       {
+         ipsec_policy_t *p =
+           pool_elt_at_index (im->policies, spd->policies[policy->type][i]);
+
+         if (p->priority <= vp->priority)
+           {
+             break;
+           }
+       }
+
+      vec_insert_elts (spd->policies[policy->type], &policy_index, 1, i);
+
       *stat_index = policy_index;
     }
   else
@@ -192,14 +225,25 @@ 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))
-       return ipsec_fp_add_del_policy ((void *) &spd->fp_spd, policy, 0,
-                                       stat_index);
+
+      if (ipsec_is_fp_enabled (im, spd, policy))
+
+       {
+         if (policy->policy == IPSEC_POLICY_ACTION_PROTECT)
+           {
+             index_t sa_index = ipsec_sa_find_and_lock (policy->sa_id);
+
+             if (INDEX_INVALID == sa_index)
+               return VNET_API_ERROR_SYSCALL_ERROR_1;
+             policy->sa_index = sa_index;
+             ipsec_sa_unlock_id (policy->sa_id);
+           }
+         else
+           policy->sa_index = INDEX_INVALID;
+
+         return ipsec_fp_add_del_policy ((void *) &spd->fp_spd, policy, 0,
+                                         stat_index);
+       }
 
       vec_foreach_index (ii, (spd->policies[policy->type]))
       {
@@ -219,7 +263,7 @@ ipsec_add_del_policy (vlib_main_t * vm,
 }
 
 static_always_inline void
-release_mask_type_index (ipsec_main_t *im, u32 mask_type_index)
+ipsec_fp_release_mask_type (ipsec_main_t *im, u32 mask_type_index)
 {
   ipsec_fp_mask_type_entry_t *mte =
     pool_elt_at_index (im->fp_mask_types, mask_type_index);
@@ -247,62 +291,98 @@ 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 *pkey = (u64 *) &kv->key;
+  u64 *pmatch = (u64 *) match->kv_40_8.key;
+  u64 *pmask = (u64 *) mask->kv_40_8.key;
+  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;
-  u64 *pmask = (u64 *) &mask->laddr;
+  u64 *pmatch = (u64 *) match->kv_16_8.key;
+  u64 *pmask = (u64 *) mask->kv_16_8.key;
   u64 *pkey = (u64 *) kv->key;
 
   *pkey++ = *pmatch++ & *pmask++;
-  *pkey++ = *pmatch++ & *pmask++;
+  *pkey = *pmatch & *pmask;
 
   kv_val->as_u64 = 0;
 }
 
 static_always_inline u16
-get_highest_set_bit_u16 (u16 x)
+mask_out_highest_set_bit_u16 (u16 x)
 {
   x |= x >> 8;
   x |= x >> 4;
   x |= x >> 2;
   x |= x >> 1;
-  return x ^= x >> 1;
+  return ~x;
 }
 
 static_always_inline u32
-get_highest_set_bit_u32 (u32 x)
+mask_out_highest_set_bit_u32 (u32 x)
+{
+  x |= x >> 16;
+  x |= x >> 8;
+  x |= x >> 4;
+  x |= x >> 2;
+  x |= x >> 1;
+  return ~x;
+}
+
+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 ^= 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 = mask_out_highest_set_bit_u16 (mask->lport);
+
+      mask->rport = mask_out_highest_set_bit_u16 (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)
+ipsec_fp_ip4_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask,
+                             bool inbound)
 {
   u32 *pladdr_start = (u32 *) &policy->laddr.start.ip4;
   u32 *pladdr_stop = (u32 *) &policy->laddr.stop.ip4;
@@ -311,8 +391,18 @@ ipsec_fp_ip4_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask)
   u32 *praddr_stop = (u32 *) &policy->raddr.stop.ip4;
   u32 *prmask = (u32 *) &mask->raddr;
 
-  memset (mask, 0, sizeof (mask->l3_zero_pad));
-  memset (plmask, 1, sizeof (*mask) - sizeof (mask->l3_zero_pad));
+  clib_memset_u8 (mask, 0xff, sizeof (ipsec_fp_5tuple_t));
+  clib_memset_u8 (&mask->l3_zero_pad, 0, sizeof (mask->l3_zero_pad));
+
+  if (inbound && (policy->type == IPSEC_SPD_POLICY_IP4_INBOUND_PROTECT &&
+                 policy->sa_index != INDEX_INVALID))
+    {
+      ipsec_sa_t *s = ipsec_sa_get (policy->sa_index);
+
+      if (ipsec_sa_is_set_IS_TUNNEL (s))
+       goto set_spi_mask;
+    }
+
   /* find bits where start != stop */
   *plmask = *pladdr_start ^ *pladdr_stop;
   *prmask = *praddr_start ^ *praddr_stop;
@@ -321,81 +411,99 @@ ipsec_fp_ip4_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask)
    * the bit itself. Remember that policy stores start and stop in the net
    * order.
    */
-  *plmask = get_highest_set_bit_u32 (clib_net_to_host_u32 (*plmask));
-  *plmask = clib_host_to_net_u32 (~(*plmask - 1) & (~*plmask));
+  *plmask = clib_host_to_net_u32 (
+    mask_out_highest_set_bit_u32 (clib_net_to_host_u32 (*plmask)));
 
-  *prmask = get_highest_set_bit_u32 (clib_net_to_host_u32 (*prmask));
-  *prmask = clib_host_to_net_u32 (~(*prmask - 1) & (~*prmask));
+  *prmask = clib_host_to_net_u32 (
+    mask_out_highest_set_bit_u32 (clib_net_to_host_u32 (*prmask)));
 
-  if (PREDICT_TRUE ((policy->protocol == IP_PROTOCOL_TCP) ||
-                   (policy->protocol == IP_PROTOCOL_UDP) ||
-                   (policy->protocol == IP_PROTOCOL_SCTP)))
+set_spi_mask:
+  if (inbound)
     {
-      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);
+      if (policy->type != IPSEC_SPD_POLICY_IP4_INBOUND_PROTECT)
+       mask->spi = 0;
 
-      mask->rport = get_highest_set_bit_u16 (mask->rport);
-      mask->rport = ~(mask->rport - 1) & (~mask->rport);
+      mask->protocol = 0;
     }
   else
     {
-      mask->lport = 0;
-      mask->rport = 0;
+      mask->action = 0;
+      ipsec_fp_get_policy_ports_mask (policy, mask);
     }
-
-  mask->protocol = (policy->protocol == IPSEC_POLICY_PROTOCOL_ANY) ? 0 : ~0;
 }
 
-static_always_inline int
-ipsec_fp_ip6_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask)
+static_always_inline void
+ipsec_fp_ip6_get_policy_mask (ipsec_policy_t *policy, ipsec_fp_5tuple_t *mask,
+                             bool inbound)
 {
   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;
+  clib_memset_u8 (mask, 0xff, sizeof (ipsec_fp_5tuple_t));
 
-  if (((*plport_stop - *plport_start + 1) & (*plport_stop - *plport_start)))
-    return -1;
+  if (inbound && (policy->type == IPSEC_SPD_POLICY_IP6_INBOUND_PROTECT &&
+                 policy->sa_index != INDEX_INVALID))
+    {
+      ipsec_sa_t *s = ipsec_sa_get (policy->sa_index);
 
-  if (((*prport_stop - *prport_start + 1) & (*prport_stop - *prport_start)))
-    return -1;
+      if (ipsec_sa_is_set_IS_TUNNEL (s))
+       goto set_spi_mask;
+    }
+
+  *plmask = (*pladdr_start++ ^ *pladdr_stop++);
+
+  *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 = (*praddr_start ^ *praddr_stop);
+      *prmask = clib_host_to_net_u64 (
+       mask_out_highest_set_bit_u64 (clib_net_to_host_u64 (*prmask)));
+    }
+  else
+    *prmask = 0;
+set_spi_mask:
+  if (inbound)
+    {
+      if (policy->type != IPSEC_SPD_POLICY_IP6_INBOUND_PROTECT)
+       mask->spi = 0;
+
+      mask->protocol = 0;
+    }
+  else
+    {
+      mask->action = 0;
+      ipsec_fp_get_policy_ports_mask (policy, mask);
+    }
 }
 
 static_always_inline void
-ipsec_fp_get_policy_5tuple (ipsec_policy_t *policy, ipsec_fp_5tuple_t *tuple)
+ipsec_fp_get_policy_5tuple (ipsec_policy_t *policy, ipsec_fp_5tuple_t *tuple,
+                           bool inbound)
 {
   memset (tuple, 0, sizeof (*tuple));
   tuple->is_ipv6 = policy->is_ipv6;
@@ -410,17 +518,52 @@ ipsec_fp_get_policy_5tuple (ipsec_policy_t *policy, ipsec_fp_5tuple_t *tuple)
       tuple->raddr = policy->raddr.start.ip4;
     }
 
-  tuple->protocol = policy->protocol;
+  if (inbound)
+    {
 
+      if ((policy->type == IPSEC_SPD_POLICY_IP4_INBOUND_PROTECT ||
+          policy->type == IPSEC_SPD_POLICY_IP6_INBOUND_PROTECT) &&
+         policy->sa_index != INDEX_INVALID)
+       {
+         ipsec_sa_t *s = ipsec_sa_get (policy->sa_index);
+
+         tuple->spi = s->spi;
+         if (ipsec_sa_is_set_IS_TUNNEL (s))
+           {
+             if (tuple->is_ipv6)
+               {
+                 tuple->ip6_laddr = s->tunnel.t_dst.ip.ip6;
+                 tuple->ip6_raddr = s->tunnel.t_src.ip.ip6;
+               }
+             else
+               {
+                 tuple->laddr = s->tunnel.t_dst.ip.ip4;
+                 tuple->raddr = s->tunnel.t_src.ip.ip4;
+               }
+           }
+       }
+      else
+       tuple->spi = INDEX_INVALID;
+      tuple->action = policy->type;
+      return;
+    }
+
+  tuple->protocol = policy->protocol;
   tuple->lport = policy->lport.start;
   tuple->rport = policy->rport.start;
 }
 
+static_always_inline int
+ipsec_fp_mask_type_idx_cmp (ipsec_fp_mask_id_t *mask_id, u32 *idx)
+{
+  return mask_id->mask_type_idx == *idx;
+}
+
 int
 ipsec_fp_ip4_add_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
                         ipsec_policy_t *policy, u32 *stat_index)
 {
-  u32 mask_index;
+  u32 mask_index, searched_idx;
   ipsec_policy_t *vp;
   ipsec_fp_mask_type_entry_t *mte;
   u32 policy_index;
@@ -432,8 +575,14 @@ ipsec_fp_ip4_add_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
 
   ipsec_fp_5tuple_t mask, policy_5tuple;
   int res;
-
-  ipsec_fp_ip4_get_policy_mask (policy, &mask);
+  bool inbound = ipsec_is_policy_inbound (policy);
+  clib_bihash_16_8_t *bihash_table =
+    inbound ? pool_elt_at_index (im->fp_ip4_lookup_hashes_pool,
+                                fp_spd->ip4_in_lookup_hash_idx) :
+                   pool_elt_at_index (im->fp_ip4_lookup_hashes_pool,
+                                fp_spd->ip4_out_lookup_hash_idx);
+
+  ipsec_fp_ip4_get_policy_mask (policy, &mask, inbound);
   pool_get (im->policies, vp);
   policy_index = vp - im->policies;
   vlib_validate_combined_counter (&ipsec_spd_policy_counters, policy_index);
@@ -452,35 +601,41 @@ ipsec_fp_ip4_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_get_policy_5tuple (policy, &policy_5tuple);
+  ipsec_fp_get_policy_5tuple (policy, &policy_5tuple, inbound);
 
-  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);
+  res = clib_bihash_search_inline_2_16_8 (bihash_table, &kv, &result);
   if (res != 0)
     {
       /* key was not found crate a new entry */
       vec_add1 (key_val->fp_policies_ids, policy_index);
-      res = clib_bihash_add_del_16_8 (&fp_spd->fp_ip4_lookup_hash, &kv, 1);
+      res = clib_bihash_add_del_16_8 (bihash_table, &kv, 1);
+
       if (res != 0)
        goto error;
     }
   else
     {
+      u32 i;
+      u32 *old_fp_policies_ids = result_val->fp_policies_ids;
 
-      if (vec_max_len (result_val->fp_policies_ids) !=
-         vec_len (result_val->fp_policies_ids))
+      vec_foreach_index (i, result_val->fp_policies_ids)
        {
-         /* no need to resize */
-         vec_add1 (result_val->fp_policies_ids, policy_index);
+         ipsec_policy_t *p =
+           pool_elt_at_index (im->policies, result_val->fp_policies_ids[i]);
+
+         if (p->priority <= policy->priority)
+           {
+             break;
+           }
        }
-      else
-       {
-         vec_add1 (result_val->fp_policies_ids, policy_index);
 
-         res =
-           clib_bihash_add_del_16_8 (&fp_spd->fp_ip4_lookup_hash, &result, 1);
+      vec_insert_elts (result_val->fp_policies_ids, &policy_index, 1, i);
+
+      if (result_val->fp_policies_ids != old_fp_policies_ids)
+       {
+         res = clib_bihash_add_del_16_8 (bihash_table, &result, 1);
 
          if (res != 0)
            goto error;
@@ -491,18 +646,27 @@ ipsec_fp_ip4_add_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
     {
       clib_memcpy (&mte->mask, &mask, sizeof (mask));
       mte->refcount = 0;
-      vec_add1 (fp_spd->fp_mask_types[policy->type], mask_index);
     }
 
+  searched_idx =
+    vec_search_with_function (fp_spd->fp_mask_ids[policy->type], &mask_index,
+                             ipsec_fp_mask_type_idx_cmp);
+  if (~0 == searched_idx)
+    {
+      ipsec_fp_mask_id_t mask_id = { mask_index, 1 };
+      vec_add1 (fp_spd->fp_mask_ids[policy->type], mask_id);
+    }
+  else
+    (fp_spd->fp_mask_ids[policy->type] + searched_idx)->refcount++;
+
   mte->refcount++;
-  vec_add1 (fp_spd->fp_policies[policy->type], policy_index);
   clib_memcpy (vp, policy, sizeof (*vp));
 
   return 0;
 
 error:
   pool_put (im->policies, vp);
-  release_mask_type_index (im, mask_index);
+  ipsec_fp_release_mask_type (im, mask_index);
   return -1;
 }
 
@@ -511,7 +675,7 @@ ipsec_fp_ip6_add_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
                         ipsec_policy_t *policy, u32 *stat_index)
 {
 
-  u32 mask_index;
+  u32 mask_index, searched_idx;
   ipsec_policy_t *vp;
   ipsec_fp_mask_type_entry_t *mte;
   u32 policy_index;
@@ -523,26 +687,20 @@ 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;
+  bool inbound = ipsec_is_policy_inbound (policy);
 
+  ipsec_fp_ip6_get_policy_mask (policy, &mask, inbound);
   pool_get (im->policies, vp);
   policy_index = vp - im->policies;
   vlib_validate_combined_counter (&ipsec_spd_policy_counters, policy_index);
   vlib_zero_combined_counter (&ipsec_spd_policy_counters, policy_index);
   *stat_index = policy_index;
   mask_index = find_mask_type_index (im, &mask);
+  clib_bihash_40_8_t *bihash_table =
+    inbound ? pool_elt_at_index (im->fp_ip6_lookup_hashes_pool,
+                                fp_spd->ip6_in_lookup_hash_idx) :
+                   pool_elt_at_index (im->fp_ip6_lookup_hashes_pool,
+                                fp_spd->ip6_out_lookup_hash_idx);
 
   if (mask_index == ~0)
     {
@@ -555,36 +713,40 @@ 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);
+  ipsec_fp_get_policy_5tuple (policy, &policy_5tuple, inbound);
 
-  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);
+  res = clib_bihash_search_inline_2_40_8 (bihash_table, &kv, &result);
   if (res != 0)
     {
       /* key was not found crate a new entry */
       vec_add1 (key_val->fp_policies_ids, policy_index);
-      res = clib_bihash_add_del_40_8 (&fp_spd->fp_ip6_lookup_hash, &kv, 1);
+      res = clib_bihash_add_del_40_8 (bihash_table, &kv, 1);
       if (res != 0)
        goto error;
     }
   else
     {
+      u32 i;
+      u32 *old_fp_policies_ids = result_val->fp_policies_ids;
 
-      if (vec_max_len (result_val->fp_policies_ids) !=
-         vec_len (result_val->fp_policies_ids))
+      vec_foreach_index (i, result_val->fp_policies_ids)
        {
-         /* no need to resize */
-         vec_add1 (result_val->fp_policies_ids, policy_index);
+         ipsec_policy_t *p =
+           pool_elt_at_index (im->policies, result_val->fp_policies_ids[i]);
+
+         if (p->priority <= policy->priority)
+           {
+             break;
+           }
        }
-      else
-       {
-         vec_add1 (result_val->fp_policies_ids, policy_index);
 
-         res =
-           clib_bihash_add_del_40_8 (&fp_spd->fp_ip6_lookup_hash, &result, 1);
+      vec_insert_elts (result_val->fp_policies_ids, &policy_index, 1, i);
+
+      if (result_val->fp_policies_ids != old_fp_policies_ids)
+       {
+         res = clib_bihash_add_del_40_8 (bihash_table, &result, 1);
 
          if (res != 0)
            goto error;
@@ -595,18 +757,27 @@ ipsec_fp_ip6_add_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
     {
       clib_memcpy (&mte->mask, &mask, sizeof (mask));
       mte->refcount = 0;
-      vec_add1 (fp_spd->fp_mask_types[policy->type], mask_index);
     }
 
+  searched_idx =
+    vec_search_with_function (fp_spd->fp_mask_ids[policy->type], &mask_index,
+                             ipsec_fp_mask_type_idx_cmp);
+  if (~0 == searched_idx)
+    {
+      ipsec_fp_mask_id_t mask_id = { mask_index, 1 };
+      vec_add1 (fp_spd->fp_mask_ids[policy->type], mask_id);
+    }
+  else
+    (fp_spd->fp_mask_ids[policy->type] + searched_idx)->refcount++;
+
   mte->refcount++;
-  vec_add1 (fp_spd->fp_policies[policy->type], policy_index);
   clib_memcpy (vp, policy, sizeof (*vp));
 
   return 0;
 
 error:
   pool_put (im->policies, vp);
-  release_mask_type_index (im, mask_index);
+  ipsec_fp_release_mask_type (im, mask_index);
   return -1;
 }
 
@@ -620,71 +791,55 @@ ipsec_fp_ip6_del_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
   clib_bihash_kv_40_8_t result;
   ipsec_fp_lookup_value_t *result_val =
     (ipsec_fp_lookup_value_t *) &result.value;
+  bool inbound = ipsec_is_policy_inbound (policy);
+  clib_bihash_40_8_t *bihash_table =
+    inbound ? pool_elt_at_index (im->fp_ip6_lookup_hashes_pool,
+                                fp_spd->ip6_in_lookup_hash_idx) :
+                   pool_elt_at_index (im->fp_ip6_lookup_hashes_pool,
+                                fp_spd->ip6_out_lookup_hash_idx);
 
   ipsec_policy_t *vp;
-  u32 ii, iii, imt;
+  u32 ii, imt;
 
-  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);
-  res = clib_bihash_search_inline_2_40_8 (&fp_spd->fp_ip6_lookup_hash, &kv,
-                                         &result);
+  ipsec_fp_ip6_get_policy_mask (policy, &mask, inbound);
+  ipsec_fp_get_policy_5tuple (policy, &policy_5tuple, inbound);
+  fill_ip6_hash_policy_kv (&policy_5tuple, &mask, &kv);
+  res = clib_bihash_search_inline_2_40_8 (bihash_table, &kv, &result);
   if (res != 0)
     return -1;
 
-  res = -1;
   vec_foreach_index (ii, result_val->fp_policies_ids)
     {
       vp =
        pool_elt_at_index (im->policies, *(result_val->fp_policies_ids + ii));
       if (ipsec_policy_is_equal (vp, policy))
        {
-         vec_foreach_index (iii, fp_spd->fp_policies[policy->type])
+         if (vec_len (result_val->fp_policies_ids) == 1)
            {
-             if (*(fp_spd->fp_policies[policy->type] + iii) ==
-                 *(result_val->fp_policies_ids + ii))
+             vec_free (result_val->fp_policies_ids);
+             clib_bihash_add_del_40_8 (bihash_table, &result, 0);
+           }
+         else
+           vec_delete (result_val->fp_policies_ids, 1, ii);
+
+         vec_foreach_index (imt, fp_spd->fp_mask_ids[policy->type])
+           {
+             if ((fp_spd->fp_mask_ids[policy->type] + imt)->mask_type_idx ==
+                 vp->fp_mask_type_id)
                {
-                 if (vec_len (result_val->fp_policies_ids) == 1)
-                   {
-                     vec_free (result_val->fp_policies_ids);
-                     clib_bihash_add_del_40_8 (&fp_spd->fp_ip6_lookup_hash,
-                                               &result, 0);
-                   }
-                 else
-                   {
-                     vec_del1 (result_val->fp_policies_ids, ii);
-                   }
-                 vec_del1 (fp_spd->fp_policies[policy->type], iii);
-
-                 vec_foreach_index (imt, fp_spd->fp_mask_types[policy->type])
-                   {
-                     if (*(fp_spd->fp_mask_types[policy->type] + imt) ==
-                         vp->fp_mask_type_id)
-                       {
-                         ipsec_fp_mask_type_entry_t *mte = pool_elt_at_index (
-                           im->fp_mask_types, vp->fp_mask_type_id);
-
-                         if (mte->refcount == 1)
-                           vec_del1 (fp_spd->fp_mask_types[policy->type],
-                                     imt);
-                         break;
-                       }
-                   }
-
-                 res = 0;
+
+                 if ((fp_spd->fp_mask_ids[policy->type] + imt)->refcount-- ==
+                     1)
+                   vec_del1 (fp_spd->fp_mask_ids[policy->type], imt);
+
                  break;
                }
            }
 
-         if (res != 0)
-           continue;
-         else
-           {
-             release_mask_type_index (im, vp->fp_mask_type_id);
-             ipsec_sa_unlock (vp->sa_index);
-             pool_put (im->policies, vp);
-             return 0;
-           }
+         ipsec_fp_release_mask_type (im, vp->fp_mask_type_id);
+         ipsec_sa_unlock (vp->sa_index);
+         pool_put (im->policies, vp);
+         return 0;
        }
     }
   return -1;
@@ -700,71 +855,54 @@ ipsec_fp_ip4_del_policy (ipsec_main_t *im, ipsec_spd_fp_t *fp_spd,
   clib_bihash_kv_16_8_t result;
   ipsec_fp_lookup_value_t *result_val =
     (ipsec_fp_lookup_value_t *) &result.value;
-
+  bool inbound = ipsec_is_policy_inbound (policy);
   ipsec_policy_t *vp;
-  u32 ii, iii, imt;
+  u32 ii, imt;
+  clib_bihash_16_8_t *bihash_table =
+    inbound ? pool_elt_at_index (im->fp_ip4_lookup_hashes_pool,
+                                fp_spd->ip4_in_lookup_hash_idx) :
+                   pool_elt_at_index (im->fp_ip4_lookup_hashes_pool,
+                                fp_spd->ip4_out_lookup_hash_idx);
+
+  ipsec_fp_ip4_get_policy_mask (policy, &mask, inbound);
+  ipsec_fp_get_policy_5tuple (policy, &policy_5tuple, inbound);
+  fill_ip4_hash_policy_kv (&policy_5tuple, &mask, &kv);
+  res = clib_bihash_search_inline_2_16_8 (bihash_table, &kv, &result);
 
-  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);
-  res = clib_bihash_search_inline_2_16_8 (&fp_spd->fp_ip4_lookup_hash, &kv,
-                                         &result);
   if (res != 0)
     return -1;
 
-  res = -1;
   vec_foreach_index (ii, result_val->fp_policies_ids)
     {
       vp =
        pool_elt_at_index (im->policies, *(result_val->fp_policies_ids + ii));
       if (ipsec_policy_is_equal (vp, policy))
        {
-         vec_foreach_index (iii, fp_spd->fp_policies[policy->type])
+         if (vec_len (result_val->fp_policies_ids) == 1)
            {
-             if (*(fp_spd->fp_policies[policy->type] + iii) ==
-                 *(result_val->fp_policies_ids + ii))
-               {
-                 if (vec_len (result_val->fp_policies_ids) == 1)
-                   {
-                     vec_free (result_val->fp_policies_ids);
-                     clib_bihash_add_del_16_8 (&fp_spd->fp_ip4_lookup_hash,
-                                               &result, 0);
-                   }
-                 else
-                   {
-                     vec_del1 (result_val->fp_policies_ids, ii);
-                   }
-                 vec_del1 (fp_spd->fp_policies[policy->type], iii);
-
-                 vec_foreach_index (imt, fp_spd->fp_mask_types[policy->type])
-                   {
-                     if (*(fp_spd->fp_mask_types[policy->type] + imt) ==
-                         vp->fp_mask_type_id)
-                       {
-                         ipsec_fp_mask_type_entry_t *mte = pool_elt_at_index (
-                           im->fp_mask_types, vp->fp_mask_type_id);
-
-                         if (mte->refcount == 1)
-                           vec_del1 (fp_spd->fp_mask_types[policy->type],
-                                     imt);
-                         break;
-                       }
-                   }
-
-                 res = 0;
-                 break;
-               }
+             vec_free (result_val->fp_policies_ids);
+             clib_bihash_add_del_16_8 (bihash_table, &result, 0);
            }
-
-         if (res != 0)
-           continue;
          else
+           vec_delete (result_val->fp_policies_ids, 1, ii);
+
+         vec_foreach_index (imt, fp_spd->fp_mask_ids[policy->type])
            {
-             release_mask_type_index (im, vp->fp_mask_type_id);
-             ipsec_sa_unlock (vp->sa_index);
-             pool_put (im->policies, vp);
-             return 0;
+             if ((fp_spd->fp_mask_ids[policy->type] + imt)->mask_type_idx ==
+                 vp->fp_mask_type_id)
+               {
+
+                 if ((fp_spd->fp_mask_ids[policy->type] + imt)->refcount-- ==
+                     1)
+                   vec_del1 (fp_spd->fp_mask_ids[policy->type], imt);
+
+                 break;
+               }
            }
+         ipsec_fp_release_mask_type (im, vp->fp_mask_type_id);
+         ipsec_sa_unlock (vp->sa_index);
+         pool_put (im->policies, vp);
+         return 0;
        }
     }
   return -1;