ikev2: fix issue when decrypting packet with no keys
[vpp.git] / src / plugins / ikev2 / ikev2.c
index d8c94ff..63e89b5 100644 (file)
@@ -71,7 +71,9 @@ _(IKE_SA_INIT_IGNORE, "IKE_SA_INIT ignore (IKE SA already auth)") \
 _(IKE_REQ_RETRANSMIT, "IKE request retransmit") \
 _(IKE_REQ_IGNORE, "IKE request ignore (old msgid)") \
 _(NOT_IKEV2, "Non IKEv2 packets received") \
-_(BAD_LENGTH, "Bad packet length")
+_(BAD_LENGTH, "Bad packet length") \
+_(MALFORMED_PACKET, "Malformed packet") \
+_(NO_BUFF_SPACE, "No buffer space")
 
 typedef enum
 {
@@ -296,9 +298,12 @@ static void
 ikev2_sa_free_all_vec (ikev2_sa_t * sa)
 {
   vec_free (sa->i_nonce);
-  vec_free (sa->i_dh_data);
+  vec_free (sa->r_nonce);
+
   vec_free (sa->dh_shared_key);
   vec_free (sa->dh_private_key);
+  vec_free (sa->i_dh_data);
+  vec_free (sa->r_dh_data);
 
   ikev2_sa_free_proposal_vector (&sa->r_proposals);
   ikev2_sa_free_proposal_vector (&sa->i_proposals);
@@ -312,14 +317,24 @@ ikev2_sa_free_all_vec (ikev2_sa_t * sa)
   vec_free (sa->sk_pr);
 
   vec_free (sa->i_id.data);
-  vec_free (sa->i_auth.data);
   vec_free (sa->r_id.data);
+
+  vec_free (sa->i_auth.data);
+  if (sa->i_auth.key)
+    EVP_PKEY_free (sa->i_auth.key);
   vec_free (sa->r_auth.data);
   if (sa->r_auth.key)
     EVP_PKEY_free (sa->r_auth.key);
 
   vec_free (sa->del);
 
+  vec_free (sa->rekey);
+
+  vec_free (sa->last_sa_init_req_packet_data);
+  vec_free (sa->last_sa_init_res_packet_data);
+
+  vec_free (sa->last_res_packet_data);
+
   ikev2_sa_free_all_child_sa (&sa->childs);
 }
 
@@ -537,6 +552,7 @@ ikev2_calc_keys (ikev2_sa_t * sa)
   pos += tr_prf->key_len;
 
   vec_free (keymat);
+  sa->keys_generated = 1;
 }
 
 static void
@@ -625,12 +641,54 @@ ikev2_compute_nat_sha1 (u64 ispi, u64 rspi, u32 ip, u16 port)
   return res;
 }
 
-static void
-ikev2_process_sa_init_req (vlib_main_t * vm, ikev2_sa_t * sa,
-                          ike_header_t * ike, udp_header_t * udp, u32 len)
+static int
+ikev2_parse_ke_payload (const void *p, u32 rlen, ikev2_sa_t * sa,
+                       u8 ** ke_data)
 {
+  const ike_ke_payload_header_t *ke = p;
+  u16 plen = clib_net_to_host_u16 (ke->length);
+  ASSERT (plen >= sizeof (*ke) && plen <= rlen);
+  if (sizeof (*ke) > rlen)
+    return 0;
+
+  sa->dh_group = clib_net_to_host_u16 (ke->dh_group);
+  vec_reset_length (ke_data[0]);
+  vec_add (ke_data[0], ke->payload, plen - sizeof (*ke));
+  return 1;
+}
+
+static int
+ikev2_parse_nonce_payload (const void *p, u32 rlen, u8 * nonce)
+{
+  const ike_payload_header_t *ikep = p;
+  u16 plen = clib_net_to_host_u16 (ikep->length);
+  ASSERT (plen >= sizeof (*ikep) && plen <= rlen);
+  clib_memcpy_fast (nonce, ikep->payload, plen - sizeof (*ikep));
+  return 1;
+}
+
+static int
+ikev2_check_payload_length (const ike_payload_header_t * ikep, int rlen,
+                           u16 * plen)
+{
+  if (sizeof (*ikep) > rlen)
+    return 0;
+  *plen = clib_net_to_host_u16 (ikep->length);
+  if (*plen < sizeof (*ikep) || *plen > rlen)
+    return 0;
+  return 1;
+}
+
+static int
+ikev2_process_sa_init_req (vlib_main_t * vm,
+                          ikev2_sa_t * sa, ike_header_t * ike,
+                          udp_header_t * udp, u32 len)
+{
+  u8 nonce[IKEV2_NONCE_SIZE];
   int p = 0;
   u8 payload = ike->nextpayload;
+  ike_payload_header_t *ikep;
+  u16 plen;
 
   ikev2_elog_exchange ("ispi %lx rspi %lx IKE_INIT request received "
                       "from %d.%d.%d.%d",
@@ -640,44 +698,46 @@ ikev2_process_sa_init_req (vlib_main_t * vm, ikev2_sa_t * sa,
   sa->ispi = clib_net_to_host_u64 (ike->ispi);
 
   /* store whole IKE payload - needed for PSK auth */
-  vec_free (sa->last_sa_init_req_packet_data);
+  vec_reset_length (sa->last_sa_init_req_packet_data);
   vec_add (sa->last_sa_init_req_packet_data, ike, len);
 
+  if (len < sizeof (*ike))
+    return 0;
+
+  len -= sizeof (*ike);
   while (p < len && payload != IKEV2_PAYLOAD_NONE)
     {
-      ike_payload_header_t *ikep = (ike_payload_header_t *) & ike->payload[p];
-      u32 plen = clib_net_to_host_u16 (ikep->length);
-
-      if (plen < sizeof (ike_payload_header_t))
-       return;
+      ikep = (ike_payload_header_t *) & ike->payload[p];
+      int current_length = len - p;
+      if (!ikev2_check_payload_length (ikep, current_length, &plen))
+       return 0;
 
       if (payload == IKEV2_PAYLOAD_SA)
        {
          ikev2_sa_free_proposal_vector (&sa->i_proposals);
-         sa->i_proposals = ikev2_parse_sa_payload (ikep);
+         sa->i_proposals = ikev2_parse_sa_payload (ikep, current_length);
        }
       else if (payload == IKEV2_PAYLOAD_KE)
        {
-         ike_ke_payload_header_t *ke = (ike_ke_payload_header_t *) ikep;
-         sa->dh_group = clib_net_to_host_u16 (ke->dh_group);
-         vec_free (sa->i_dh_data);
-         vec_add (sa->i_dh_data, ke->payload, plen - sizeof (*ke));
+         if (!ikev2_parse_ke_payload (ikep, current_length, sa,
+                                      &sa->i_dh_data))
+           return 0;
        }
       else if (payload == IKEV2_PAYLOAD_NONCE)
        {
-         vec_free (sa->i_nonce);
-         vec_add (sa->i_nonce, ikep->payload, plen - sizeof (*ikep));
+         vec_reset_length (sa->i_nonce);
+         if (ikev2_parse_nonce_payload (ikep, current_length, nonce))
+           vec_add (sa->i_nonce, nonce, plen - sizeof (*ikep));
        }
       else if (payload == IKEV2_PAYLOAD_NOTIFY)
        {
-         ikev2_notify_t *n = ikev2_parse_notify_payload (ikep);
+         ikev2_notify_t *n =
+           ikev2_parse_notify_payload (ikep, current_length);
          if (n->msg_type == IKEV2_NOTIFY_MSG_NAT_DETECTION_SOURCE_IP)
            {
-             u8 *src_sha =
-               ikev2_compute_nat_sha1 (clib_net_to_host_u64 (ike->ispi), 0,
-                                       clib_net_to_host_u32 (sa->
-                                                             iaddr.as_u32),
-                                       udp->src_port);
+             u8 *src_sha = ikev2_compute_nat_sha1 (ike->ispi, 0,
+                                                   sa->iaddr.as_u32,
+                                                   udp->src_port);
              if (clib_memcmp (src_sha, n->data, vec_len (src_sha)))
                {
                  sa->natt = 1;
@@ -689,11 +749,9 @@ ikev2_process_sa_init_req (vlib_main_t * vm, ikev2_sa_t * sa,
          else if (n->msg_type ==
                   IKEV2_NOTIFY_MSG_NAT_DETECTION_DESTINATION_IP)
            {
-             u8 *dst_sha =
-               ikev2_compute_nat_sha1 (clib_net_to_host_u64 (ike->ispi), 0,
-                                       clib_net_to_host_u32 (sa->
-                                                             raddr.as_u32),
-                                       udp->dst_port);
+             u8 *dst_sha = ikev2_compute_nat_sha1 (ike->ispi, 0,
+                                                   sa->raddr.as_u32,
+                                                   udp->dst_port);
              if (clib_memcmp (dst_sha, n->data, vec_len (dst_sha)))
                {
                  sa->natt = 1;
@@ -716,7 +774,7 @@ ikev2_process_sa_init_req (vlib_main_t * vm, ikev2_sa_t * sa,
            {
              ikev2_set_state (sa, IKEV2_STATE_NOTIFY_AND_DELETE);
              sa->unsupported_cp = payload;
-             return;
+             return 0;
            }
        }
 
@@ -725,14 +783,19 @@ ikev2_process_sa_init_req (vlib_main_t * vm, ikev2_sa_t * sa,
     }
 
   ikev2_set_state (sa, IKEV2_STATE_SA_INIT);
+  return 1;
 }
 
 static void
-ikev2_process_sa_init_resp (vlib_main_t * vm, ikev2_sa_t * sa,
-                           ike_header_t * ike, udp_header_t * udp, u32 len)
+ikev2_process_sa_init_resp (vlib_main_t * vm,
+                           ikev2_sa_t * sa, ike_header_t * ike,
+                           udp_header_t * udp, u32 len)
 {
+  u8 nonce[IKEV2_NONCE_SIZE];
   int p = 0;
   u8 payload = ike->nextpayload;
+  ike_payload_header_t *ikep;
+  u16 plen;
 
   sa->ispi = clib_net_to_host_u64 (ike->ispi);
   sa->rspi = clib_net_to_host_u64 (ike->rspi);
@@ -742,21 +805,24 @@ ikev2_process_sa_init_resp (vlib_main_t * vm, ikev2_sa_t * sa,
                       sa->raddr.as_u32);
 
   /* store whole IKE payload - needed for PSK auth */
-  vec_free (sa->last_sa_init_res_packet_data);
+  vec_reset_length (sa->last_sa_init_res_packet_data);
   vec_add (sa->last_sa_init_res_packet_data, ike, len);
 
+  if (sizeof (*ike) > len)
+    return;
+
+  len -= sizeof (*ike);
   while (p < len && payload != IKEV2_PAYLOAD_NONE)
     {
-      ike_payload_header_t *ikep = (ike_payload_header_t *) & ike->payload[p];
-      u32 plen = clib_net_to_host_u16 (ikep->length);
-
-      if (plen < sizeof (ike_payload_header_t))
+      int current_length = len - p;
+      ikep = (ike_payload_header_t *) & ike->payload[p];
+      if (!ikev2_check_payload_length (ikep, current_length, &plen))
        return;
 
       if (payload == IKEV2_PAYLOAD_SA)
        {
          ikev2_sa_free_proposal_vector (&sa->r_proposals);
-         sa->r_proposals = ikev2_parse_sa_payload (ikep);
+         sa->r_proposals = ikev2_parse_sa_payload (ikep, current_length);
          if (sa->r_proposals)
            {
              ikev2_set_state (sa, IKEV2_STATE_SA_INIT);
@@ -766,25 +832,25 @@ ikev2_process_sa_init_resp (vlib_main_t * vm, ikev2_sa_t * sa,
        }
       else if (payload == IKEV2_PAYLOAD_KE)
        {
-         ike_ke_payload_header_t *ke = (ike_ke_payload_header_t *) ikep;
-         sa->dh_group = clib_net_to_host_u16 (ke->dh_group);
-         vec_free (sa->r_dh_data);
-         vec_add (sa->r_dh_data, ke->payload, plen - sizeof (*ke));
+         if (!ikev2_parse_ke_payload (ikep, current_length, sa,
+                                      &sa->r_dh_data))
+           return;
        }
       else if (payload == IKEV2_PAYLOAD_NONCE)
        {
-         vec_free (sa->r_nonce);
-         vec_add (sa->r_nonce, ikep->payload, plen - sizeof (*ikep));
+         vec_reset_length (sa->r_nonce);
+         if (ikev2_parse_nonce_payload (ikep, current_length, nonce))
+           vec_add (sa->r_nonce, nonce, plen - sizeof (*ikep));
        }
       else if (payload == IKEV2_PAYLOAD_NOTIFY)
        {
-         ikev2_notify_t *n = ikev2_parse_notify_payload (ikep);
+         ikev2_notify_t *n =
+           ikev2_parse_notify_payload (ikep, current_length);
          if (n->msg_type == IKEV2_NOTIFY_MSG_NAT_DETECTION_SOURCE_IP)
            {
              u8 *src_sha = ikev2_compute_nat_sha1 (ike->ispi,
                                                    ike->rspi,
-                                                   clib_net_to_host_u32
-                                                   (sa->raddr.as_u32),
+                                                   sa->raddr.as_u32,
                                                    udp->src_port);
              if (clib_memcmp (src_sha, n->data, vec_len (src_sha)))
                {
@@ -832,15 +898,15 @@ ikev2_process_sa_init_resp (vlib_main_t * vm, ikev2_sa_t * sa,
 }
 
 static u8 *
-ikev2_decrypt_sk_payload (ikev2_sa_t * sa, ike_header_t * ike, u8 * payload,
-                         u32 len)
+ikev2_decrypt_sk_payload (ikev2_sa_t * sa, ike_header_t * ike,
+                         u8 * payload, u32 rlen, u32 * out_len)
 {
   ikev2_main_per_thread_data_t *ptd = ikev2_get_per_thread_data ();
   int p = 0;
-  u8 last_payload = 0, *plaintext = 0;
-  u8 *hmac = 0;
+  u8 last_payload = 0, *hmac = 0, *plaintext = 0;
   ike_payload_header_t *ikep = 0;
-  u32 plen = 0;
+  u16 plen = 0;
+  u32 dlen = 0;
   ikev2_sa_transform_t *tr_integ;
   ikev2_sa_transform_t *tr_encr;
   tr_integ =
@@ -849,16 +915,19 @@ ikev2_decrypt_sk_payload (ikev2_sa_t * sa, ike_header_t * ike, u8 * payload,
     ikev2_sa_get_td_for_type (sa->r_proposals, IKEV2_TRANSFORM_TYPE_ENCR);
   int is_aead = tr_encr->encr_type == IKEV2_TRANSFORM_ENCR_TYPE_AES_GCM_16;
 
-  if ((!sa->sk_ar || !sa->sk_ai) && !is_aead)
+  if (((!sa->sk_ar || !sa->sk_ai) && !is_aead) || (!sa->sk_ei || !sa->sk_er))
+    return 0;
+
+  if (rlen <= sizeof (*ike))
     return 0;
 
+  int len = rlen - sizeof (*ike);
   while (p < len &&
         *payload != IKEV2_PAYLOAD_NONE && last_payload != IKEV2_PAYLOAD_SK)
     {
       ikep = (ike_payload_header_t *) & ike->payload[p];
-      plen = clib_net_to_host_u16 (ikep->length);
-
-      if (plen < sizeof (*ikep))
+      int current_length = len - p;
+      if (!ikev2_check_payload_length (ikep, current_length, &plen))
        return 0;
 
       if (*payload == IKEV2_PAYLOAD_SK)
@@ -896,24 +965,29 @@ ikev2_decrypt_sk_payload (ikev2_sa_t * sa, ike_header_t * ike, u8 * payload,
       u32 aad_len = ikep->payload - aad;
       u8 *tag = ikep->payload + plen;
 
-      plaintext = ikev2_decrypt_aead_data (ptd, sa, tr_encr, ikep->payload,
-                                          plen, aad, aad_len, tag);
+      int rc = ikev2_decrypt_aead_data (ptd, sa, tr_encr, ikep->payload,
+                                       plen, aad, aad_len, tag, &dlen);
+      if (rc)
+       {
+         *out_len = dlen;
+         plaintext = ikep->payload + IKEV2_GCM_IV_SIZE;
+       }
     }
   else
     {
-      if (len < tr_integ->key_trunc)
+      if (rlen < tr_integ->key_trunc)
        return 0;
 
       hmac =
        ikev2_calc_integr (tr_integ, sa->is_initiator ? sa->sk_ar : sa->sk_ai,
-                          (u8 *) ike, len - tr_integ->key_trunc);
+                          (u8 *) ike, rlen - tr_integ->key_trunc);
 
       if (plen < sizeof (*ikep) + tr_integ->key_trunc)
        return 0;
 
       plen = plen - sizeof (*ikep) - tr_integ->key_trunc;
 
-      if (memcmp (hmac, &ikep->payload[plen], tr_integ->key_trunc))
+      if (clib_memcmp (hmac, &ikep->payload[plen], tr_integ->key_trunc))
        {
          ikev2_elog_error ("message integrity check failed");
          vec_free (hmac);
@@ -921,7 +995,13 @@ ikev2_decrypt_sk_payload (ikev2_sa_t * sa, ike_header_t * ike, u8 * payload,
        }
       vec_free (hmac);
 
-      plaintext = ikev2_decrypt_data (ptd, sa, tr_encr, ikep->payload, plen);
+      int rc = ikev2_decrypt_data (ptd, sa, tr_encr, ikep->payload, plen,
+                                  &dlen);
+      if (rc)
+       {
+         *out_len = dlen;
+         plaintext = ikep->payload + tr_encr->block_size;
+       }
     }
 
   return plaintext;
@@ -936,7 +1016,7 @@ ikev2_is_id_equal (ikev2_id_t * i1, ikev2_id_t * i2)
   if (vec_len (i1->data) != vec_len (i2->data))
     return 0;
 
-  if (memcmp (i1->data, i2->data, vec_len (i1->data)))
+  if (clib_memcmp (i1->data, i2->data, vec_len (i1->data)))
     return 0;
 
   return 1;
@@ -980,16 +1060,44 @@ ikev2_initial_contact_cleanup (ikev2_sa_t * sa)
   sa->initial_contact = 0;
 }
 
-static void
-ikev2_process_auth_req (vlib_main_t * vm, ikev2_sa_t * sa, ike_header_t * ike,
-                       u32 len)
+static int
+ikev2_parse_id_payload (const void *p, u16 rlen, ikev2_id_t * sa_id)
+{
+  const ike_id_payload_header_t *id = p;
+  u16 plen = clib_net_to_host_u16 (id->length);
+  if (plen < sizeof (*id) || plen > rlen)
+    return 0;
+
+  sa_id->type = id->id_type;
+  vec_reset_length (sa_id->data);
+  vec_add (sa_id->data, id->payload, plen - sizeof (*id));
+
+  return 1;
+}
+
+static int
+ikev2_parse_auth_payload (const void *p, u32 rlen, ikev2_auth_t * a)
+{
+  const ike_auth_payload_header_t *ah = p;
+  u16 plen = clib_net_to_host_u16 (ah->length);
+
+  a->method = ah->auth_method;
+  vec_reset_length (a->data);
+  vec_add (a->data, ah->payload, plen - sizeof (*ah));
+  return 1;
+}
+
+static int
+ikev2_process_auth_req (vlib_main_t * vm, ikev2_sa_t * sa,
+                       ike_header_t * ike, u32 len)
 {
-  ikev2_child_sa_t *first_child_sa;
   int p = 0;
+  ikev2_child_sa_t *first_child_sa;
   u8 payload = ike->nextpayload;
   u8 *plaintext = 0;
   ike_payload_header_t *ikep;
-  u32 plen;
+  u16 plen;
+  u32 dlen = 0;
 
   ikev2_elog_exchange ("ispi %lx rspi %lx EXCHANGE_IKE_AUTH received "
                       "from %d.%d.%d.%d", clib_host_to_net_u64 (ike->ispi),
@@ -999,13 +1107,16 @@ ikev2_process_auth_req (vlib_main_t * vm, ikev2_sa_t * sa, ike_header_t * ike,
 
   ikev2_calc_keys (sa);
 
-  plaintext = ikev2_decrypt_sk_payload (sa, ike, &payload, len);
+  plaintext = ikev2_decrypt_sk_payload (sa, ike, &payload, len, &dlen);
 
   if (!plaintext)
     {
       if (sa->unsupported_cp)
-       ikev2_set_state (sa, IKEV2_STATE_NOTIFY_AND_DELETE);
-      goto cleanup_and_exit;
+       {
+         ikev2_set_state (sa, IKEV2_STATE_NOTIFY_AND_DELETE);
+         return 0;
+       }
+      goto malformed;
     }
 
   /* select or create 1st child SA */
@@ -1021,64 +1132,57 @@ ikev2_process_auth_req (vlib_main_t * vm, ikev2_sa_t * sa, ike_header_t * ike,
 
 
   /* process encrypted payload */
-  p = 0;
-  while (p < vec_len (plaintext) && payload != IKEV2_PAYLOAD_NONE)
+  while (p < dlen && payload != IKEV2_PAYLOAD_NONE)
     {
       ikep = (ike_payload_header_t *) & plaintext[p];
-      plen = clib_net_to_host_u16 (ikep->length);
-
-      if (plen < sizeof (ike_payload_header_t))
-       goto cleanup_and_exit;
+      int current_length = dlen - p;
+      if (!ikev2_check_payload_length (ikep, current_length, &plen))
+       goto malformed;
 
       if (payload == IKEV2_PAYLOAD_SA) /* 33 */
        {
          if (sa->is_initiator)
            {
              ikev2_sa_free_proposal_vector (&first_child_sa->r_proposals);
-             first_child_sa->r_proposals = ikev2_parse_sa_payload (ikep);
+             first_child_sa->r_proposals = ikev2_parse_sa_payload (ikep,
+                                                                   current_length);
            }
          else
            {
              ikev2_sa_free_proposal_vector (&first_child_sa->i_proposals);
-             first_child_sa->i_proposals = ikev2_parse_sa_payload (ikep);
+             first_child_sa->i_proposals = ikev2_parse_sa_payload (ikep,
+                                                                   current_length);
            }
        }
       else if (payload == IKEV2_PAYLOAD_IDI)   /* 35 */
        {
-         ike_id_payload_header_t *id = (ike_id_payload_header_t *) ikep;
-
-         sa->i_id.type = id->id_type;
-         vec_free (sa->i_id.data);
-         vec_add (sa->i_id.data, id->payload, plen - sizeof (*id));
+         if (!ikev2_parse_id_payload (ikep, current_length, &sa->i_id))
+           goto malformed;
        }
       else if (payload == IKEV2_PAYLOAD_IDR)   /* 36 */
        {
-         ike_id_payload_header_t *id = (ike_id_payload_header_t *) ikep;
-
-         sa->r_id.type = id->id_type;
-         vec_free (sa->r_id.data);
-         vec_add (sa->r_id.data, id->payload, plen - sizeof (*id));
+         if (!ikev2_parse_id_payload (ikep, current_length, &sa->r_id))
+           goto malformed;
        }
       else if (payload == IKEV2_PAYLOAD_AUTH)  /* 39 */
        {
-         ike_auth_payload_header_t *a = (ike_auth_payload_header_t *) ikep;
-
          if (sa->is_initiator)
            {
-             sa->r_auth.method = a->auth_method;
-             vec_free (sa->r_auth.data);
-             vec_add (sa->r_auth.data, a->payload, plen - sizeof (*a));
+             if (!ikev2_parse_auth_payload (ikep, current_length,
+                                            &sa->r_auth))
+               goto malformed;
            }
          else
            {
-             sa->i_auth.method = a->auth_method;
-             vec_free (sa->i_auth.data);
-             vec_add (sa->i_auth.data, a->payload, plen - sizeof (*a));
+             if (!ikev2_parse_auth_payload (ikep, current_length,
+                                            &sa->i_auth))
+               goto malformed;
            }
        }
       else if (payload == IKEV2_PAYLOAD_NOTIFY)        /* 41 */
        {
-         ikev2_notify_t *n = ikev2_parse_notify_payload (ikep);
+         ikev2_notify_t *n =
+           ikev2_parse_notify_payload (ikep, current_length);
          if (n->msg_type == IKEV2_NOTIFY_MSG_INITIAL_CONTACT)
            {
              sa->initial_contact = 1;
@@ -1092,12 +1196,12 @@ ikev2_process_auth_req (vlib_main_t * vm, ikev2_sa_t * sa, ike_header_t * ike,
       else if (payload == IKEV2_PAYLOAD_TSI)   /* 44 */
        {
          vec_free (first_child_sa->tsi);
-         first_child_sa->tsi = ikev2_parse_ts_payload (ikep);
+         first_child_sa->tsi = ikev2_parse_ts_payload (ikep, current_length);
        }
       else if (payload == IKEV2_PAYLOAD_TSR)   /* 45 */
        {
          vec_free (first_child_sa->tsr);
-         first_child_sa->tsr = ikev2_parse_ts_payload (ikep);
+         first_child_sa->tsr = ikev2_parse_ts_payload (ikep, current_length);
        }
       else
        {
@@ -1108,7 +1212,7 @@ ikev2_process_auth_req (vlib_main_t * vm, ikev2_sa_t * sa, ike_header_t * ike,
            {
              ikev2_set_state (sa, IKEV2_STATE_NOTIFY_AND_DELETE);
              sa->unsupported_cp = payload;
-             return;
+             return 0;
            }
        }
 
@@ -1116,50 +1220,60 @@ ikev2_process_auth_req (vlib_main_t * vm, ikev2_sa_t * sa, ike_header_t * ike,
       p += plen;
     }
 
-cleanup_and_exit:
-  vec_free (plaintext);
+  return 1;
+
+malformed:
+  ikev2_set_state (sa, IKEV2_STATE_DELETED);
+  return 0;
 }
 
-static void
-ikev2_process_informational_req (vlib_main_t * vm, ikev2_sa_t * sa,
-                                ike_header_t * ike, u32 len)
+static int
+ikev2_process_informational_req (vlib_main_t * vm,
+                                ikev2_sa_t * sa, ike_header_t * ike, u32 len)
 {
   int p = 0;
   u8 payload = ike->nextpayload;
   u8 *plaintext = 0;
   ike_payload_header_t *ikep;
-  u32 plen;
+  u32 dlen = 0;
+  ikev2_notify_t *n = 0;
 
   sa->liveness_retries = 0;
   ikev2_elog_exchange ("ispi %lx rspi %lx INFORMATIONAL received "
                       "from %d.%d.%d.%d", clib_host_to_net_u64 (ike->ispi),
                       clib_host_to_net_u64 (ike->rspi), sa->iaddr.as_u32);
 
-  plaintext = ikev2_decrypt_sk_payload (sa, ike, &payload, len);
+  plaintext = ikev2_decrypt_sk_payload (sa, ike, &payload, len, &dlen);
 
   if (!plaintext)
-    goto cleanup_and_exit;
+    return 0;
 
   /* process encrypted payload */
   p = 0;
-  while (p < vec_len (plaintext) && payload != IKEV2_PAYLOAD_NONE)
+  while (p < dlen && payload != IKEV2_PAYLOAD_NONE)
     {
+      u32 current_length = dlen - p;
+      if (p + sizeof (*ikep) > dlen)
+       return 0;
+
       ikep = (ike_payload_header_t *) & plaintext[p];
-      plen = clib_net_to_host_u16 (ikep->length);
+      u16 plen = clib_net_to_host_u16 (ikep->length);
 
-      if (plen < sizeof (ike_payload_header_t))
-       goto cleanup_and_exit;
+      if (plen < sizeof (*ikep) || plen > current_length)
+       return 0;
 
       if (payload == IKEV2_PAYLOAD_NOTIFY)     /* 41 */
        {
-         ikev2_notify_t *n = ikev2_parse_notify_payload (ikep);
+         n = ikev2_parse_notify_payload (ikep, current_length);
+         if (!n)
+           return 0;
          if (n->msg_type == IKEV2_NOTIFY_MSG_AUTHENTICATION_FAILED)
            ikev2_set_state (sa, IKEV2_STATE_AUTH_FAILED);
          vec_free (n);
        }
       else if (payload == IKEV2_PAYLOAD_DELETE)        /* 42 */
        {
-         sa->del = ikev2_parse_delete_payload (ikep);
+         sa->del = ikev2_parse_delete_payload (ikep, current_length);
        }
       else if (payload == IKEV2_PAYLOAD_VENDOR)        /* 43 */
        {
@@ -1172,21 +1286,19 @@ ikev2_process_informational_req (vlib_main_t * vm, ikev2_sa_t * sa,
          if (ikep->flags & IKEV2_PAYLOAD_FLAG_CRITICAL)
            {
              sa->unsupported_cp = payload;
-             return;
+             return 0;
            }
        }
-
       payload = ikep->nextpayload;
       p += plen;
     }
-
-cleanup_and_exit:
-  vec_free (plaintext);
+  return 1;
 }
 
-static void
-ikev2_process_create_child_sa_req (vlib_main_t * vm, ikev2_sa_t * sa,
-                                  ike_header_t * ike, u32 len)
+static int
+ikev2_process_create_child_sa_req (vlib_main_t * vm,
+                                  ikev2_sa_t * sa, ike_header_t * ike,
+                                  u32 len)
 {
   int p = 0;
   u8 payload = ike->nextpayload;
@@ -1195,39 +1307,39 @@ ikev2_process_create_child_sa_req (vlib_main_t * vm, ikev2_sa_t * sa,
   u8 nonce[IKEV2_NONCE_SIZE];
 
   ike_payload_header_t *ikep;
-  u32 plen;
   ikev2_notify_t *n = 0;
   ikev2_ts_t *tsi = 0;
   ikev2_ts_t *tsr = 0;
   ikev2_sa_proposal_t *proposal = 0;
   ikev2_child_sa_t *child_sa;
+  u32 dlen = 0;
+  u16 plen;
 
   ikev2_elog_exchange ("ispi %lx rspi %lx CREATE_CHILD_SA received "
                       "from %d.%d.%d.%d", clib_host_to_net_u64 (ike->ispi),
                       clib_host_to_net_u64 (ike->rspi), sa->raddr.as_u32);
 
-  plaintext = ikev2_decrypt_sk_payload (sa, ike, &payload, len);
+  plaintext = ikev2_decrypt_sk_payload (sa, ike, &payload, len, &dlen);
 
   if (!plaintext)
     goto cleanup_and_exit;
 
   /* process encrypted payload */
   p = 0;
-  while (p < vec_len (plaintext) && payload != IKEV2_PAYLOAD_NONE)
+  while (payload != IKEV2_PAYLOAD_NONE)
     {
       ikep = (ike_payload_header_t *) & plaintext[p];
-      plen = clib_net_to_host_u16 (ikep->length);
-
-      if (plen < sizeof (ike_payload_header_t))
+      int current_length = dlen - p;
+      if (!ikev2_check_payload_length (ikep, current_length, &plen))
        goto cleanup_and_exit;
 
-      else if (payload == IKEV2_PAYLOAD_SA)
+      if (payload == IKEV2_PAYLOAD_SA)
        {
-         proposal = ikev2_parse_sa_payload (ikep);
+         proposal = ikev2_parse_sa_payload (ikep, current_length);
        }
       else if (payload == IKEV2_PAYLOAD_NOTIFY)
        {
-         n = ikev2_parse_notify_payload (ikep);
+         n = ikev2_parse_notify_payload (ikep, current_length);
          if (n->msg_type == IKEV2_NOTIFY_MSG_REKEY_SA)
            {
              rekeying = 1;
@@ -1235,7 +1347,7 @@ ikev2_process_create_child_sa_req (vlib_main_t * vm, ikev2_sa_t * sa,
        }
       else if (payload == IKEV2_PAYLOAD_DELETE)
        {
-         sa->del = ikev2_parse_delete_payload (ikep);
+         sa->del = ikev2_parse_delete_payload (ikep, current_length);
        }
       else if (payload == IKEV2_PAYLOAD_VENDOR)
        {
@@ -1243,15 +1355,15 @@ ikev2_process_create_child_sa_req (vlib_main_t * vm, ikev2_sa_t * sa,
        }
       else if (payload == IKEV2_PAYLOAD_NONCE)
        {
-         clib_memcpy_fast (nonce, ikep->payload, plen - sizeof (*ikep));
+         ikev2_parse_nonce_payload (ikep, current_length, nonce);
        }
       else if (payload == IKEV2_PAYLOAD_TSI)
        {
-         tsi = ikev2_parse_ts_payload (ikep);
+         tsi = ikev2_parse_ts_payload (ikep, current_length);
        }
       else if (payload == IKEV2_PAYLOAD_TSR)
        {
-         tsr = ikev2_parse_ts_payload (ikep);
+         tsr = ikev2_parse_ts_payload (ikep, current_length);
        }
       else
        {
@@ -1263,7 +1375,6 @@ ikev2_process_create_child_sa_req (vlib_main_t * vm, ikev2_sa_t * sa,
              goto cleanup_and_exit;
            }
        }
-
       payload = ikep->nextpayload;
       p += plen;
     }
@@ -1279,7 +1390,7 @@ ikev2_process_create_child_sa_req (vlib_main_t * vm, ikev2_sa_t * sa,
       rekey->tsi = tsi;
       rekey->tsr = tsr;
       /* update Nr */
-      vec_free (sa->r_nonce);
+      vec_reset_length (sa->r_nonce);
       vec_add (sa->r_nonce, nonce, IKEV2_NONCE_SIZE);
       child_sa = ikev2_sa_get_child (sa, rekey->ispi, IKEV2_PROTOCOL_ESP, 1);
       if (child_sa)
@@ -1309,14 +1420,15 @@ ikev2_process_create_child_sa_req (vlib_main_t * vm, ikev2_sa_t * sa,
       vec_free (sa->i_nonce);
       vec_add (sa->i_nonce, nonce, IKEV2_NONCE_SIZE);
       /* generate new Nr */
-      vec_free (sa->r_nonce);
-      sa->r_nonce = vec_new (u8, IKEV2_NONCE_SIZE);
+      vec_validate (sa->r_nonce, IKEV2_NONCE_SIZE - 1);
       RAND_bytes ((u8 *) sa->r_nonce, IKEV2_NONCE_SIZE);
+      vec_free (n);
     }
+  return 1;
 
 cleanup_and_exit:
-  vec_free (plaintext);
   vec_free (n);
+  return 0;
 }
 
 static u8 *
@@ -1502,7 +1614,7 @@ ikev2_sa_auth (ikev2_sa_t * sa)
         psk = ikev2_calc_prf(tr_prf, p->auth.data, key_pad);
         auth = ikev2_calc_prf(tr_prf, psk, authmsg);
 
-        if (!memcmp(auth, sa_auth->data, vec_len(sa_auth->data)))
+        if (!clib_memcmp(auth, sa_auth->data, vec_len(sa_auth->data)))
           {
             ikev2_set_state(sa, IKEV2_STATE_AUTHENTICATED);
             vec_free(auth);
@@ -1728,11 +1840,11 @@ ikev2_add_tunnel_from_main (ikev2_add_ipsec_tunnel_args_t * a)
 
 static int
 ikev2_create_tunnel_interface (vlib_main_t * vm,
-                              u32 thread_index,
                               ikev2_sa_t * sa,
                               ikev2_child_sa_t * child, u32 sa_index,
                               u32 child_index, u8 is_rekey)
 {
+  u32 thread_index = vlib_get_thread_index ();
   ikev2_main_t *km = &ikev2_main;
   ipsec_crypto_alg_t encr_type;
   ipsec_integ_alg_t integ_type;
@@ -2060,9 +2172,11 @@ ikev2_delete_tunnel_interface (vnet_main_t * vnm, ikev2_sa_t * sa,
 }
 
 static u32
-ikev2_generate_message (ikev2_sa_t * sa, ike_header_t * ike, void *user,
-                       udp_header_t * udp)
+ikev2_generate_message (vlib_buffer_t * b, ikev2_sa_t * sa,
+                       ike_header_t * ike, void *user, udp_header_t * udp)
 {
+  ikev2_main_t *km = &ikev2_main;
+  u16 buffer_data_size = vlib_buffer_get_default_data_size (km->vlib_main);
   v8 *integ = 0;
   ike_payload_header_t *ph;
   u16 plen;
@@ -2125,7 +2239,7 @@ ikev2_generate_message (ikev2_sa_t * sa, ike_header_t * ike, void *user,
          u8 *nat_detection_sha1 =
            ikev2_compute_nat_sha1 (clib_host_to_net_u64 (sa->ispi),
                                    clib_host_to_net_u64 (sa->rspi),
-                                   clib_host_to_net_u32 (sa->raddr.as_u32),
+                                   sa->raddr.as_u32,
                                    udp->dst_port);
          ikev2_payload_add_notify (chain,
                                    IKEV2_NOTIFY_MSG_NAT_DETECTION_SOURCE_IP,
@@ -2319,10 +2433,17 @@ ikev2_generate_message (ikev2_sa_t * sa, ike_header_t * ike, void *user,
       tlen += vec_len (chain->data);
       ike->nextpayload = chain->first_payload_type;
       ike->length = clib_host_to_net_u32 (tlen);
+
+      if (tlen + b->current_length + b->current_data > buffer_data_size)
+       {
+         tlen = ~0;
+         goto done;
+       }
+
       clib_memcpy_fast (ike->payload, chain->data, vec_len (chain->data));
 
       /* store whole IKE payload - needed for PSK auth */
-      vec_free (sa->last_sa_init_res_packet_data);
+      vec_reset_length (sa->last_sa_init_res_packet_data);
       vec_add (sa->last_sa_init_res_packet_data, ike, tlen);
     }
   else
@@ -2347,21 +2468,36 @@ ikev2_generate_message (ikev2_sa_t * sa, ike_header_t * ike, void *user,
        plen += IKEV2_GCM_ICV_SIZE;
       tlen += plen;
 
+      if (tlen + b->current_length + b->current_data > buffer_data_size)
+       {
+         tlen = ~0;
+         goto done;
+       }
+
       /* payload and total length */
       ph->length = clib_host_to_net_u16 (plen);
       ike->length = clib_host_to_net_u32 (tlen);
 
       if (is_aead)
        {
-         ikev2_encrypt_aead_data (ptd, sa, tr_encr, chain->data,
-                                  ph->payload, (u8 *) ike,
-                                  sizeof (*ike) + sizeof (*ph),
-                                  ph->payload + plen - sizeof (*ph) -
-                                  IKEV2_GCM_ICV_SIZE);
+         if (!ikev2_encrypt_aead_data (ptd, sa, tr_encr, chain->data,
+                                       ph->payload, (u8 *) ike,
+                                       sizeof (*ike) + sizeof (*ph),
+                                       ph->payload + plen - sizeof (*ph) -
+                                       IKEV2_GCM_ICV_SIZE))
+           {
+             tlen = ~0;
+             goto done;
+           }
        }
       else
        {
-         ikev2_encrypt_data (ptd, sa, tr_encr, chain->data, ph->payload);
+         if (!ikev2_encrypt_data
+             (ptd, sa, tr_encr, chain->data, ph->payload))
+           {
+             tlen = ~0;
+             goto done;
+           }
          integ =
            ikev2_calc_integr (tr_integ,
                               sa->is_initiator ? sa->sk_ai : sa->sk_ar,
@@ -2371,7 +2507,7 @@ ikev2_generate_message (ikev2_sa_t * sa, ike_header_t * ike, void *user,
        }
 
       /* store whole IKE payload - needed for retransmit */
-      vec_free (sa->last_res_packet_data);
+      vec_reset_length (sa->last_res_packet_data);
       vec_add (sa->last_res_packet_data, ike, tlen);
     }
 
@@ -2382,8 +2518,8 @@ done:
 }
 
 static u32
-ikev2_retransmit_sa_init (ike_header_t * ike,
-                         ip4_address_t iaddr, ip4_address_t raddr, u32 rlen)
+ikev2_retransmit_sa_init (ike_header_t * ike, ip4_address_t iaddr,
+                         ip4_address_t raddr, u32 rlen)
 {
   ikev2_main_t *km = &ikev2_main;
   ikev2_sa_t *sa;
@@ -2400,14 +2536,17 @@ ikev2_retransmit_sa_init (ike_header_t * ike,
 
         while (p < rlen && payload!= IKEV2_PAYLOAD_NONE) {
           ike_payload_header_t * ikep = (ike_payload_header_t *) &ike->payload[p];
-          u32 plen = clib_net_to_host_u16(ikep->length);
+          u32 plen = clib_net_to_host_u16 (ikep->length);
+          if (plen > p + sizeof (*ike))
+            return ~0;
 
           if (plen < sizeof(ike_payload_header_t))
-            return -1;
+            return ~0;
 
           if (payload == IKEV2_PAYLOAD_NONCE)
             {
-              if (!memcmp(sa->i_nonce, ikep->payload, plen - sizeof(*ikep)))
+              if (!clib_memcmp(sa->i_nonce, ikep->payload,
+                    plen - sizeof(*ikep)))
                 {
                   /* req is retransmit */
                   if (sa->state == IKEV2_STATE_SA_INIT)
@@ -2454,7 +2593,7 @@ ikev2_retransmit_sa_init (ike_header_t * ike,
 }
 
 static u32
-ikev2_retransmit_resp (ikev2_sa_t * sa, ike_header_t * ike, u32 rlen)
+ikev2_retransmit_resp (ikev2_sa_t * sa, ike_header_t * ike)
 {
   u32 msg_id = clib_net_to_host_u32 (ike->msgid);
 
@@ -2523,450 +2662,454 @@ static uword
 ikev2_node_fn (vlib_main_t * vm,
               vlib_node_runtime_t * node, vlib_frame_t * frame)
 {
-  u32 n_left_from, *from, *to_next;
-  ikev2_next_t next_index;
+  u32 n_left = frame->n_vectors, *from;
   ikev2_main_t *km = &ikev2_main;
-  u32 thread_index = vlib_get_thread_index ();
+  vlib_buffer_t *bufs[VLIB_FRAME_SIZE], **b;
+  u16 nexts[VLIB_FRAME_SIZE], *next = nexts;
+  ikev2_main_per_thread_data_t *ptd = ikev2_get_per_thread_data ();
+  int res;
 
   from = vlib_frame_vector_args (frame);
-  n_left_from = frame->n_vectors;
-  next_index = node->cached_next_index;
-
-  while (n_left_from > 0)
-    {
-      u32 n_left_to_next;
-
-      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
-
-      while (n_left_from > 0 && n_left_to_next > 0)
+  vlib_get_buffers (vm, from, bufs, n_left);
+  b = bufs;
+
+  while (n_left > 0)
+    {
+      vlib_buffer_t *b0 = b[0];
+      next[0] = IKEV2_NEXT_ERROR_DROP;
+      ip4_header_t *ip40;
+      udp_header_t *udp0;
+      ike_header_t *ike0;
+      ikev2_sa_t *sa0 = 0;
+      ikev2_sa_t sa;           /* temporary store for SA */
+      u32 rlen, slen = 0;
+      int is_req = 0, has_non_esp_marker = 0;
+
+      if (b0->punt_reason == ipsec_punt_reason[IPSEC_PUNT_IP4_SPI_UDP_0])
        {
-         u32 bi0;
-         vlib_buffer_t *b0;
-         u32 next0 = IKEV2_NEXT_ERROR_DROP;
-         u32 sw_if_index0;
-         ip4_header_t *ip40;
-         udp_header_t *udp0;
-         ike_header_t *ike0;
-         ikev2_sa_t *sa0 = 0;
-         ikev2_sa_t sa;        /* temporary store for SA */
-         u32 rlen, slen = 0;
-         int is_req = 0, has_non_esp_marker = 0;
-
-         /* speculatively enqueue b0 to the current next frame */
-         bi0 = from[0];
-         to_next[0] = bi0;
-         from += 1;
-         to_next += 1;
-         n_left_from -= 1;
-         n_left_to_next -= 1;
-
-         b0 = vlib_get_buffer (vm, bi0);
-
-         if (b0->punt_reason == ipsec_punt_reason[IPSEC_PUNT_IP4_SPI_UDP_0])
-           {
-             u8 *ptr = vlib_buffer_get_current (b0);
-             ip40 = (ip4_header_t *) ptr;
-             ptr += sizeof (*ip40);
-             udp0 = (udp_header_t *) ptr;
-             ptr += sizeof (*udp0);
-             ike0 = (ike_header_t *) ptr;
-           }
-         else
-           {
-             ike0 = vlib_buffer_get_current (b0);
-             vlib_buffer_advance (b0, -sizeof (*udp0));
-             udp0 = vlib_buffer_get_current (b0);
-             vlib_buffer_advance (b0, -sizeof (*ip40));
-             ip40 = vlib_buffer_get_current (b0);
-           }
-
-         rlen = b0->current_length - sizeof (*ip40) - sizeof (*udp0);
-
-         /* check for non-esp marker */
-         if (*((u32 *) ike0) == 0)
-           {
-             ike0 =
-               (ike_header_t *) ((u8 *) ike0 +
-                                 sizeof (ikev2_non_esp_marker));
-             rlen -= sizeof (ikev2_non_esp_marker);
-             has_non_esp_marker = 1;
-           }
-
-         if (clib_net_to_host_u32 (ike0->length) != rlen)
-           {
-             vlib_node_increment_counter (vm, ikev2_node.index,
-                                          IKEV2_ERROR_BAD_LENGTH, 1);
-             goto dispatch0;
-           }
-
-         if (ike0->version != IKE_VERSION_2)
-           {
-             vlib_node_increment_counter (vm, ikev2_node.index,
-                                          IKEV2_ERROR_NOT_IKEV2, 1);
-             goto dispatch0;
-           }
-
-         if (ike0->exchange == IKEV2_EXCHANGE_SA_INIT)
-           {
-             sa0 = &sa;
-             clib_memset (sa0, 0, sizeof (*sa0));
-
-             if (ike0->flags & IKEV2_HDR_FLAG_INITIATOR)
-               {
-                 if (ike0->rspi == 0)
-                   {
-                     sa0->raddr.as_u32 = ip40->dst_address.as_u32;
-                     sa0->iaddr.as_u32 = ip40->src_address.as_u32;
-                     sa0->dst_port = clib_net_to_host_u16 (udp0->src_port);
-
-                     slen =
-                       ikev2_retransmit_sa_init (ike0, sa0->iaddr,
-                                                 sa0->raddr, rlen);
-                     if (slen)
-                       {
-                         vlib_node_increment_counter (vm, ikev2_node.index,
-                                                      ~0 ==
-                                                      slen ?
-                                                      IKEV2_ERROR_IKE_SA_INIT_IGNORE
-                                                      :
-                                                      IKEV2_ERROR_IKE_SA_INIT_RETRANSMIT,
-                                                      1);
-                         goto dispatch0;
-                       }
+         u8 *ptr = vlib_buffer_get_current (b0);
+         ip40 = (ip4_header_t *) ptr;
+         ptr += sizeof (*ip40);
+         udp0 = (udp_header_t *) ptr;
+         ptr += sizeof (*udp0);
+         ike0 = (ike_header_t *) ptr;
+       }
+      else
+       {
+         ike0 = vlib_buffer_get_current (b0);
+         vlib_buffer_advance (b0, -sizeof (*udp0));
+         udp0 = vlib_buffer_get_current (b0);
+         vlib_buffer_advance (b0, -sizeof (*ip40));
+         ip40 = vlib_buffer_get_current (b0);
+       }
 
-                     ikev2_process_sa_init_req (vm, sa0, ike0, udp0, rlen);
+      rlen = b0->current_length - sizeof (*ip40) - sizeof (*udp0);
 
-                     if (sa0->state == IKEV2_STATE_SA_INIT)
-                       {
-                         ikev2_sa_free_proposal_vector (&sa0->r_proposals);
-                         sa0->r_proposals =
-                           ikev2_select_proposal (sa0->i_proposals,
-                                                  IKEV2_PROTOCOL_IKE);
-                         ikev2_generate_sa_init_data (sa0);
-                       }
+      /* check for non-esp marker */
+      if (*((u32 *) ike0) == 0)
+       {
+         ike0 =
+           (ike_header_t *) ((u8 *) ike0 + sizeof (ikev2_non_esp_marker));
+         rlen -= sizeof (ikev2_non_esp_marker);
+         has_non_esp_marker = 1;
+       }
 
-                     if (sa0->state == IKEV2_STATE_SA_INIT
-                         || sa0->state == IKEV2_STATE_NOTIFY_AND_DELETE)
-                       {
-                         slen = ikev2_generate_message (sa0, ike0, 0, udp0);
-                       }
+      if (clib_net_to_host_u32 (ike0->length) != rlen)
+       {
+         vlib_node_increment_counter (vm, ikev2_node.index,
+                                      IKEV2_ERROR_BAD_LENGTH, 1);
+         goto dispatch0;
+       }
 
-                     if (sa0->state == IKEV2_STATE_SA_INIT)
-                       {
-                         /* add SA to the pool */
-                         pool_get (km->per_thread_data[thread_index].sas,
-                                   sa0);
-                         clib_memcpy_fast (sa0, &sa, sizeof (*sa0));
-                         ikev2_init_sa (vm, sa0);
-                         hash_set (km->
-                                   per_thread_data[thread_index].sa_by_rspi,
-                                   sa0->rspi,
-                                   sa0 -
-                                   km->per_thread_data[thread_index].sas);
-                       }
-                     else
-                       {
-                         ikev2_sa_free_all_vec (sa0);
-                       }
-                   }
-               }
-             else              //received sa_init without initiator flag
-               {
-                 sa0->raddr.as_u32 = ip40->src_address.as_u32;
-                 sa0->iaddr.as_u32 = ip40->dst_address.as_u32;
-                 ikev2_process_sa_init_resp (vm, sa0, ike0, udp0, rlen);
+      if (ike0->version != IKE_VERSION_2)
+       {
+         vlib_node_increment_counter (vm, ikev2_node.index,
+                                      IKEV2_ERROR_NOT_IKEV2, 1);
+         goto dispatch0;
+       }
 
-                 if (sa0->state == IKEV2_STATE_SA_INIT)
-                   {
-                     is_req = 1;
-                     ike0->exchange = IKEV2_EXCHANGE_IKE_AUTH;
-                     uword *p = hash_get (km->sa_by_ispi, ike0->ispi);
-                     if (p)
-                       {
-                         ikev2_sa_t *sai =
-                           pool_elt_at_index (km->sais, p[0]);
-
-                         if (clib_atomic_bool_cmp_and_swap
-                             (&sai->init_response_received, 0, 1))
-                           {
-                             ikev2_complete_sa_data (sa0, sai);
-                             ikev2_calc_keys (sa0);
-                             ikev2_sa_auth_init (sa0);
-                             slen =
-                               ikev2_generate_message (sa0, ike0, 0, udp0);
-                           }
-                         else
-                           {
-                             /* we've already processed sa-init response */
-                             sa0->state = IKEV2_STATE_UNKNOWN;
-                           }
-                       }
-                   }
+      if (ike0->exchange == IKEV2_EXCHANGE_SA_INIT)
+       {
+         sa0 = &sa;
+         clib_memset (sa0, 0, sizeof (*sa0));
 
-                 if (sa0->state == IKEV2_STATE_SA_INIT)
-                   {
-                     /* add SA to the pool */
-                     pool_get (km->per_thread_data[thread_index].sas, sa0);
-                     clib_memcpy_fast (sa0, &sa, sizeof (*sa0));
-                     hash_set (km->per_thread_data[thread_index].sa_by_rspi,
-                               sa0->rspi,
-                               sa0 - km->per_thread_data[thread_index].sas);
-                   }
-                 else
-                   {
-                     ikev2_sa_free_all_vec (sa0);
-                   }
-               }
-           }
-         else if (ike0->exchange == IKEV2_EXCHANGE_IKE_AUTH)
+         if (ike0->flags & IKEV2_HDR_FLAG_INITIATOR)
            {
-             uword *p;
-             p = hash_get (km->per_thread_data[thread_index].sa_by_rspi,
-                           clib_net_to_host_u64 (ike0->rspi));
-             if (p)
+             if (ike0->rspi == 0)
                {
-                 sa0 =
-                   pool_elt_at_index (km->per_thread_data[thread_index].sas,
-                                      p[0]);
+                 sa0->raddr.as_u32 = ip40->dst_address.as_u32;
+                 sa0->iaddr.as_u32 = ip40->src_address.as_u32;
+                 sa0->dst_port = clib_net_to_host_u16 (udp0->src_port);
 
-                 slen = ikev2_retransmit_resp (sa0, ike0, rlen);
+                 slen =
+                   ikev2_retransmit_sa_init (ike0, sa0->iaddr,
+                                             sa0->raddr, rlen);
                  if (slen)
                    {
                      vlib_node_increment_counter (vm, ikev2_node.index,
                                                   ~0 ==
                                                   slen ?
-                                                  IKEV2_ERROR_IKE_REQ_IGNORE
+                                                  IKEV2_ERROR_IKE_SA_INIT_IGNORE
                                                   :
-                                                  IKEV2_ERROR_IKE_REQ_RETRANSMIT,
+                                                  IKEV2_ERROR_IKE_SA_INIT_RETRANSMIT,
                                                   1);
                      goto dispatch0;
                    }
 
-                 sa0->dst_port = clib_net_to_host_u16 (udp0->src_port);
-                 ikev2_process_auth_req (vm, sa0, ike0, rlen);
-                 ikev2_sa_auth (sa0);
-                 if (sa0->state == IKEV2_STATE_AUTHENTICATED)
-                   {
-                     ikev2_initial_contact_cleanup (sa0);
-                     ikev2_sa_match_ts (sa0);
-                     if (sa0->state != IKEV2_STATE_TS_UNACCEPTABLE)
-                       ikev2_create_tunnel_interface (vm, thread_index, sa0,
-                                                      &sa0->childs[0],
-                                                      p[0], 0, 0);
-                   }
+                 res = ikev2_process_sa_init_req (vm, sa0, ike0, udp0, rlen);
+                 if (!res)
+                   vlib_node_increment_counter (vm, ikev2_node.index,
+                                                IKEV2_ERROR_MALFORMED_PACKET,
+                                                1);
 
-                 if (sa0->is_initiator)
-                   {
-                     ikev2_del_sa_init (ike0->ispi);
-                   }
-                 else
+                 if (sa0->state == IKEV2_STATE_SA_INIT)
                    {
-                     slen = ikev2_generate_message (sa0, ike0, 0, udp0);
+                     ikev2_sa_free_proposal_vector (&sa0->r_proposals);
+                     sa0->r_proposals =
+                       ikev2_select_proposal (sa0->i_proposals,
+                                              IKEV2_PROTOCOL_IKE);
+                     ikev2_generate_sa_init_data (sa0);
                    }
-               }
-           }
-         else if (ike0->exchange == IKEV2_EXCHANGE_INFORMATIONAL)
-           {
-             uword *p;
-             p = hash_get (km->per_thread_data[thread_index].sa_by_rspi,
-                           clib_net_to_host_u64 (ike0->rspi));
-             if (p)
-               {
-                 sa0 =
-                   pool_elt_at_index (km->per_thread_data[thread_index].sas,
-                                      p[0]);
 
-                 slen = ikev2_retransmit_resp (sa0, ike0, rlen);
-                 if (slen)
+                 if (sa0->state == IKEV2_STATE_SA_INIT
+                     || sa0->state == IKEV2_STATE_NOTIFY_AND_DELETE)
                    {
-                     vlib_node_increment_counter (vm, ikev2_node.index,
-                                                  ~0 ==
-                                                  slen ?
-                                                  IKEV2_ERROR_IKE_REQ_IGNORE
-                                                  :
-                                                  IKEV2_ERROR_IKE_REQ_RETRANSMIT,
-                                                  1);
-                     goto dispatch0;
+                     slen = ikev2_generate_message (b0, sa0, ike0, 0, udp0);
+                     if (~0 == slen)
+                       vlib_node_increment_counter (vm, ikev2_node.index,
+                                                    IKEV2_ERROR_NO_BUFF_SPACE,
+                                                    1);
                    }
 
-                 ikev2_process_informational_req (vm, sa0, ike0, rlen);
-                 if (sa0->del)
+                 if (sa0->state == IKEV2_STATE_SA_INIT)
                    {
-                     if (sa0->del[0].protocol_id != IKEV2_PROTOCOL_IKE)
-                       {
-                         ikev2_delete_t *d, *tmp, *resp = 0;
-                         vec_foreach (d, sa0->del)
-                         {
-                           ikev2_child_sa_t *ch_sa;
-                           ch_sa = ikev2_sa_get_child (sa0, d->spi,
-                                                       d->protocol_id,
-                                                       !sa0->is_initiator);
-                           if (ch_sa)
-                             {
-                               ikev2_delete_tunnel_interface (km->vnet_main,
-                                                              sa0, ch_sa);
-                               if (!sa0->is_initiator)
-                                 {
-                                   vec_add2 (resp, tmp, 1);
-                                   tmp->protocol_id = d->protocol_id;
-                                   tmp->spi = ch_sa->r_proposals[0].spi;
-                                 }
-                               ikev2_sa_del_child_sa (sa0, ch_sa);
-                             }
-                         }
-                         if (!sa0->is_initiator)
-                           {
-                             vec_free (sa0->del);
-                             sa0->del = resp;
-                           }
-                       }
+                     /* add SA to the pool */
+                     pool_get (ptd->sas, sa0);
+                     clib_memcpy_fast (sa0, &sa, sizeof (*sa0));
+                     ikev2_init_sa (vm, sa0);
+                     hash_set (ptd->sa_by_rspi, sa0->rspi, sa0 - ptd->sas);
                    }
-                 if (!(ike0->flags & IKEV2_HDR_FLAG_RESPONSE))
+                 else
                    {
-                     ike0->flags |= IKEV2_HDR_FLAG_RESPONSE;
-                     slen = ikev2_generate_message (sa0, ike0, 0, udp0);
+                     ikev2_sa_free_all_vec (sa0);
                    }
                }
            }
-         else if (ike0->exchange == IKEV2_EXCHANGE_CREATE_CHILD_SA)
+         else                  //received sa_init without initiator flag
            {
-             uword *p;
-             p = hash_get (km->per_thread_data[thread_index].sa_by_rspi,
-                           clib_net_to_host_u64 (ike0->rspi));
-             if (p)
-               {
-                 sa0 =
-                   pool_elt_at_index (km->per_thread_data[thread_index].sas,
-                                      p[0]);
+             sa0->raddr.as_u32 = ip40->src_address.as_u32;
+             sa0->iaddr.as_u32 = ip40->dst_address.as_u32;
+             ikev2_process_sa_init_resp (vm, sa0, ike0, udp0, rlen);
 
-                 slen = ikev2_retransmit_resp (sa0, ike0, rlen);
-                 if (slen)
+             if (sa0->state == IKEV2_STATE_SA_INIT)
+               {
+                 is_req = 1;
+                 ike0->exchange = IKEV2_EXCHANGE_IKE_AUTH;
+                 uword *p = hash_get (km->sa_by_ispi, sa0->ispi);
+                 if (p)
                    {
-                     vlib_node_increment_counter (vm, ikev2_node.index,
-                                                  ~0 ==
-                                                  slen ?
-                                                  IKEV2_ERROR_IKE_REQ_IGNORE
-                                                  :
-                                                  IKEV2_ERROR_IKE_REQ_RETRANSMIT,
-                                                  1);
-                     goto dispatch0;
-                   }
+                     ikev2_sa_t *sai = pool_elt_at_index (km->sais, p[0]);
 
-                 ikev2_process_create_child_sa_req (vm, sa0, ike0, rlen);
-                 if (sa0->rekey)
-                   {
-                     if (sa0->rekey[0].protocol_id != IKEV2_PROTOCOL_IKE)
+                     if (clib_atomic_bool_cmp_and_swap
+                         (&sai->init_response_received, 0, 1))
                        {
-                         if (sa0->childs)
-                           ikev2_sa_free_all_child_sa (&sa0->childs);
-                         ikev2_child_sa_t *child;
-                         vec_add2 (sa0->childs, child, 1);
-                         clib_memset (child, 0, sizeof (*child));
-                         child->r_proposals = sa0->rekey[0].r_proposal;
-                         child->i_proposals = sa0->rekey[0].i_proposal;
-                         child->tsi = sa0->rekey[0].tsi;
-                         child->tsr = sa0->rekey[0].tsr;
-                         ikev2_create_tunnel_interface (vm, thread_index,
-                                                        sa0, child, p[0],
-                                                        child - sa0->childs,
+                         ikev2_complete_sa_data (sa0, sai);
+                         ikev2_calc_keys (sa0);
+                         ikev2_sa_auth_init (sa0);
+                         slen =
+                           ikev2_generate_message (b0, sa0, ike0, 0, udp0);
+                         if (~0 == slen)
+                           vlib_node_increment_counter (vm,
+                                                        ikev2_node.index,
+                                                        IKEV2_ERROR_NO_BUFF_SPACE,
                                                         1);
                        }
-                     if (sa0->is_initiator)
-                       {
-                         vec_free (sa0->rekey);
-                       }
                      else
                        {
-                         slen = ikev2_generate_message (sa0, ike0, 0, udp0);
+                         /* we've already processed sa-init response */
+                         sa0->state = IKEV2_STATE_UNKNOWN;
                        }
                    }
                }
+
+             if (sa0->state == IKEV2_STATE_SA_INIT)
+               {
+                 /* add SA to the pool */
+                 pool_get (ptd->sas, sa0);
+                 clib_memcpy_fast (sa0, &sa, sizeof (*sa0));
+                 hash_set (ptd->sa_by_rspi, sa0->rspi, sa0 - ptd->sas);
+               }
+             else
+               {
+                 ikev2_sa_free_all_vec (sa0);
+               }
            }
-         else
+       }
+      else if (ike0->exchange == IKEV2_EXCHANGE_IKE_AUTH)
+       {
+         uword *p;
+         p = hash_get (ptd->sa_by_rspi, clib_net_to_host_u64 (ike0->rspi));
+         if (p)
            {
-             ikev2_elog_uint_peers (IKEV2_LOG_WARNING, "IKEv2 exchange %d "
-                                    "received from %d.%d.%d.%d to %d.%d.%d.%d",
-                                    ike0->exchange,
-                                    ip40->src_address.as_u32,
-                                    ip40->dst_address.as_u32);
-           }
+             sa0 = pool_elt_at_index (ptd->sas, p[0]);
+             slen = ikev2_retransmit_resp (sa0, ike0);
+             if (slen)
+               {
+                 vlib_node_increment_counter (vm, ikev2_node.index,
+                                              ~0 ==
+                                              slen ?
+                                              IKEV2_ERROR_IKE_REQ_IGNORE
+                                              :
+                                              IKEV2_ERROR_IKE_REQ_RETRANSMIT,
+                                              1);
+                 goto dispatch0;
+               }
+
+             sa0->dst_port = clib_net_to_host_u16 (udp0->src_port);
+             res = ikev2_process_auth_req (vm, sa0, ike0, rlen);
+             if (res)
+               ikev2_sa_auth (sa0);
+             else
+               vlib_node_increment_counter (vm, ikev2_node.index,
+                                            IKEV2_ERROR_MALFORMED_PACKET, 1);
+             if (sa0->state == IKEV2_STATE_AUTHENTICATED)
+               {
+                 ikev2_initial_contact_cleanup (sa0);
+                 ikev2_sa_match_ts (sa0);
+                 if (sa0->state != IKEV2_STATE_TS_UNACCEPTABLE)
+                   ikev2_create_tunnel_interface (vm, sa0,
+                                                  &sa0->childs[0],
+                                                  p[0], 0, 0);
+               }
 
-       dispatch0:
-         /* if we are sending packet back, rewrite headers */
-         if (slen && ~0 != slen)
-           {
-             next0 = IKEV2_NEXT_IP4_LOOKUP;
              if (sa0->is_initiator)
                {
-                 ip40->dst_address.as_u32 = sa0->raddr.as_u32;
-                 ip40->src_address.as_u32 = sa0->iaddr.as_u32;
+                 ikev2_del_sa_init (sa0->ispi);
                }
              else
                {
-                 ip40->dst_address.as_u32 = sa0->iaddr.as_u32;
-                 ip40->src_address.as_u32 = sa0->raddr.as_u32;
+                 slen = ikev2_generate_message (b0, sa0, ike0, 0, udp0);
+                 if (~0 == slen)
+                   vlib_node_increment_counter (vm, ikev2_node.index,
+                                                IKEV2_ERROR_NO_BUFF_SPACE,
+                                                1);
+               }
+           }
+       }
+      else if (ike0->exchange == IKEV2_EXCHANGE_INFORMATIONAL)
+       {
+         uword *p;
+         p = hash_get (ptd->sa_by_rspi, clib_net_to_host_u64 (ike0->rspi));
+         if (p)
+           {
+             sa0 = pool_elt_at_index (ptd->sas, p[0]);
+             slen = ikev2_retransmit_resp (sa0, ike0);
+             if (slen)
+               {
+                 vlib_node_increment_counter (vm, ikev2_node.index,
+                                              ~0 ==
+                                              slen ?
+                                              IKEV2_ERROR_IKE_REQ_IGNORE
+                                              :
+                                              IKEV2_ERROR_IKE_REQ_RETRANSMIT,
+                                              1);
+                 goto dispatch0;
                }
 
-             if (is_req)
+             res = ikev2_process_informational_req (vm, sa0, ike0, rlen);
+             if (!res)
                {
-                 udp0->dst_port = udp0->src_port =
-                   clib_net_to_host_u16 (ikev2_get_port (sa0));
+                 vlib_node_increment_counter (vm, ikev2_node.index,
+                                              IKEV2_ERROR_MALFORMED_PACKET,
+                                              1);
+                 slen = ~0;
+                 goto dispatch0;
+               }
 
-                 if (udp0->dst_port == clib_net_to_host_u16 (IKEV2_PORT_NATT)
-                     && sa0->natt)
+             if (sa0->del)
+               {
+                 if (sa0->del[0].protocol_id != IKEV2_PROTOCOL_IKE)
                    {
-                     if (!has_non_esp_marker)
-                       slen = ikev2_insert_non_esp_marker (ike0, slen);
+                     ikev2_delete_t *d, *tmp, *resp = 0;
+                     vec_foreach (d, sa0->del)
+                     {
+                       ikev2_child_sa_t *ch_sa;
+                       ch_sa = ikev2_sa_get_child (sa0, d->spi,
+                                                   d->protocol_id,
+                                                   !sa0->is_initiator);
+                       if (ch_sa)
+                         {
+                           ikev2_delete_tunnel_interface (km->vnet_main,
+                                                          sa0, ch_sa);
+                           if (!sa0->is_initiator)
+                             {
+                               vec_add2 (resp, tmp, 1);
+                               tmp->protocol_id = d->protocol_id;
+                               tmp->spi = ch_sa->r_proposals[0].spi;
+                             }
+                           ikev2_sa_del_child_sa (sa0, ch_sa);
+                         }
+                     }
+                     if (!sa0->is_initiator)
+                       {
+                         vec_free (sa0->del);
+                         sa0->del = resp;
+                       }
                    }
                }
-             else
+             if (!(ike0->flags & IKEV2_HDR_FLAG_RESPONSE))
                {
-                 if (has_non_esp_marker)
-                   slen += sizeof (ikev2_non_esp_marker);
+                 ike0->flags |= IKEV2_HDR_FLAG_RESPONSE;
+                 slen = ikev2_generate_message (b0, sa0, ike0, 0, udp0);
+                 if (~0 == slen)
+                   vlib_node_increment_counter (vm, ikev2_node.index,
+                                                IKEV2_ERROR_NO_BUFF_SPACE,
+                                                1);
+               }
+           }
+       }
+      else if (ike0->exchange == IKEV2_EXCHANGE_CREATE_CHILD_SA)
+       {
+         uword *p;
+         p = hash_get (ptd->sa_by_rspi, clib_net_to_host_u64 (ike0->rspi));
+         if (p)
+           {
+             sa0 = pool_elt_at_index (ptd->sas, p[0]);
+             slen = ikev2_retransmit_resp (sa0, ike0);
+             if (slen)
+               {
+                 vlib_node_increment_counter (vm, ikev2_node.index,
+                                              ~0 ==
+                                              slen ?
+                                              IKEV2_ERROR_IKE_REQ_IGNORE
+                                              :
+                                              IKEV2_ERROR_IKE_REQ_RETRANSMIT,
+                                              1);
+                 goto dispatch0;
+               }
 
-                 u16 tp = udp0->dst_port;
-                 udp0->dst_port = udp0->src_port;
-                 udp0->src_port = tp;
+             res = ikev2_process_create_child_sa_req (vm, sa0, ike0, rlen);
+             if (!res)
+               {
+                 vlib_node_increment_counter (vm, ikev2_node.index,
+                                              IKEV2_ERROR_MALFORMED_PACKET,
+                                              1);
+                 slen = ~0;
+                 goto dispatch0;
+               }
+
+             if (sa0->rekey)
+               {
+                 if (sa0->rekey[0].protocol_id != IKEV2_PROTOCOL_IKE)
+                   {
+                     if (sa0->childs)
+                       ikev2_sa_free_all_child_sa (&sa0->childs);
+                     ikev2_child_sa_t *child;
+                     vec_add2 (sa0->childs, child, 1);
+                     clib_memset (child, 0, sizeof (*child));
+                     child->r_proposals = sa0->rekey[0].r_proposal;
+                     child->i_proposals = sa0->rekey[0].i_proposal;
+                     child->tsi = sa0->rekey[0].tsi;
+                     child->tsr = sa0->rekey[0].tsr;
+                     ikev2_create_tunnel_interface (vm, sa0, child, p[0],
+                                                    child - sa0->childs, 1);
+                   }
+                 if (sa0->is_initiator)
+                   {
+                     vec_free (sa0->rekey);
+                   }
+                 else
+                   {
+                     slen = ikev2_generate_message (b0, sa0, ike0, 0, udp0);
+                     if (~0 == slen)
+                       vlib_node_increment_counter (vm, ikev2_node.index,
+                                                    IKEV2_ERROR_NO_BUFF_SPACE,
+                                                    1);
+                   }
                }
+           }
+       }
+      else
+       {
+         ikev2_elog_uint_peers (IKEV2_LOG_WARNING, "IKEv2 exchange %d "
+                                "received from %d.%d.%d.%d to %d.%d.%d.%d",
+                                ike0->exchange,
+                                ip40->src_address.as_u32,
+                                ip40->dst_address.as_u32);
+       }
 
-             udp0->length =
-               clib_host_to_net_u16 (slen + sizeof (udp_header_t));
-             udp0->checksum = 0;
-             b0->current_length =
-               slen + sizeof (ip4_header_t) + sizeof (udp_header_t);
-             ip40->length = clib_host_to_net_u16 (b0->current_length);
-             ip40->checksum = ip4_header_checksum (ip40);
+    dispatch0:
+      /* if we are sending packet back, rewrite headers */
+      if (slen && ~0 != slen)
+       {
+         next[0] = IKEV2_NEXT_IP4_LOOKUP;
+         if (sa0->is_initiator)
+           {
+             ip40->dst_address.as_u32 = sa0->raddr.as_u32;
+             ip40->src_address.as_u32 = sa0->iaddr.as_u32;
            }
-         /* delete sa */
-         if (sa0 && (sa0->state == IKEV2_STATE_DELETED ||
-                     sa0->state == IKEV2_STATE_NOTIFY_AND_DELETE))
+         else
            {
-             ikev2_child_sa_t *c;
+             ip40->dst_address.as_u32 = sa0->iaddr.as_u32;
+             ip40->src_address.as_u32 = sa0->raddr.as_u32;
+           }
 
-             vec_foreach (c, sa0->childs)
-               ikev2_delete_tunnel_interface (km->vnet_main, sa0, c);
+         if (is_req)
+           {
+             udp0->dst_port = udp0->src_port =
+               clib_net_to_host_u16 (ikev2_get_port (sa0));
 
-             ikev2_delete_sa (sa0);
+             if (udp0->dst_port == clib_net_to_host_u16 (IKEV2_PORT_NATT)
+                 && sa0->natt)
+               {
+                 if (!has_non_esp_marker)
+                   slen = ikev2_insert_non_esp_marker (ike0, slen);
+               }
            }
-         sw_if_index0 = vnet_buffer (b0)->sw_if_index[VLIB_RX];
-
-         if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE)
-                            && (b0->flags & VLIB_BUFFER_IS_TRACED)))
+         else
            {
-             ikev2_trace_t *t = vlib_add_trace (vm, node, b0, sizeof (*t));
-             t->sw_if_index = sw_if_index0;
-             t->next_index = next0;
+             if (has_non_esp_marker)
+               slen += sizeof (ikev2_non_esp_marker);
+
+             u16 tp = udp0->dst_port;
+             udp0->dst_port = udp0->src_port;
+             udp0->src_port = tp;
            }
 
-         vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
-                                          n_left_to_next, bi0, next0);
+         udp0->length = clib_host_to_net_u16 (slen + sizeof (udp_header_t));
+         udp0->checksum = 0;
+         b0->current_length =
+           slen + sizeof (ip4_header_t) + sizeof (udp_header_t);
+         ip40->length = clib_host_to_net_u16 (b0->current_length);
+         ip40->checksum = ip4_header_checksum (ip40);
        }
+      /* delete sa */
+      if (sa0 && (sa0->state == IKEV2_STATE_DELETED ||
+                 sa0->state == IKEV2_STATE_NOTIFY_AND_DELETE))
+       {
+         ikev2_child_sa_t *c;
+
+         vec_foreach (c, sa0->childs)
+           ikev2_delete_tunnel_interface (km->vnet_main, sa0, c);
+
+         ikev2_delete_sa (sa0);
+       }
+      if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE)
+                        && (b0->flags & VLIB_BUFFER_IS_TRACED)))
+       {
 
-      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
+         ikev2_trace_t *t = vlib_add_trace (vm, node, b0, sizeof (*t));
+         t->sw_if_index = vnet_buffer (b0)->sw_if_index[VLIB_RX];
+         t->next_index = next[0];
+       }
+      n_left -= 1;
+      next += 1;
+      b += 1;
     }
 
   vlib_node_increment_counter (vm, ikev2_node.index,
                               IKEV2_ERROR_PROCESSED, frame->n_vectors);
+  vlib_buffer_enqueue_to_next (vm, node, from, nexts, frame->n_vectors);
   return frame->n_vectors;
 }
 
@@ -2985,7 +3128,7 @@ VLIB_REGISTER_NODE (ikev2_node,static) = {
 
   .next_nodes = {
     [IKEV2_NEXT_IP4_LOOKUP] = "ip4-lookup",
-        [IKEV2_NEXT_ERROR_DROP] = "error-drop",
+    [IKEV2_NEXT_ERROR_DROP] = "error-drop",
   },
 };
 /* *INDENT-ON* */
@@ -3178,16 +3321,15 @@ ikev2_send_ike (vlib_main_t * vm, ip4_address_t * src, ip4_address_t * dst,
 }
 
 static u32
-ikev2_get_new_ike_header_buff (vlib_main_t * vm, ike_header_t ** ike)
+ikev2_get_new_ike_header_buff (vlib_main_t * vm, vlib_buffer_t ** b)
 {
   u32 bi0;
   if (vlib_buffer_alloc (vm, &bi0, 1) != 1)
     {
-      *ike = 0;
+      *b = 0;
       return 0;
     }
-  vlib_buffer_t *b0 = vlib_get_buffer (vm, bi0);
-  *ike = vlib_buffer_get_current (b0);
+  *b = vlib_get_buffer (vm, bi0);
   return bi0;
 }
 
@@ -3196,6 +3338,8 @@ ikev2_set_local_key (vlib_main_t * vm, u8 * file)
 {
   ikev2_main_t *km = &ikev2_main;
 
+  if (km->pkey)
+    EVP_PKEY_free (km->pkey);
   km->pkey = ikev2_load_key_file (file);
   if (km->pkey == NULL)
     return clib_error_return (0, "load key '%s' failed", file);
@@ -3262,19 +3406,21 @@ ikev2_initiate_delete_ike_sa_internal (vlib_main_t * vm,
 {
   ikev2_main_t *km = &ikev2_main;
   ip4_address_t *src, *dst;
+  vlib_buffer_t *b0;
 
   /* Create the Initiator notification for IKE SA removal */
   ike_header_t *ike0;
   u32 bi0 = 0;
   int len;
 
-  bi0 = ikev2_get_new_ike_header_buff (vm, &ike0);
+  bi0 = ikev2_get_new_ike_header_buff (vm, &b0);
   if (!bi0)
     {
       ikev2_log_error ("buffer alloc failure");
       return;
     }
 
+  ike0 = vlib_buffer_get_current (b0);
   ike0->exchange = IKEV2_EXCHANGE_INFORMATIONAL;
   ike0->ispi = clib_host_to_net_u64 (sa->ispi);
   ike0->rspi = clib_host_to_net_u64 (sa->rspi);
@@ -3283,7 +3429,9 @@ ikev2_initiate_delete_ike_sa_internal (vlib_main_t * vm,
   sa->del->spi = sa->ispi;
   ike0->msgid = clib_host_to_net_u32 (sa->last_init_msg_id + 1);
   sa->last_init_msg_id = clib_net_to_host_u32 (ike0->msgid);
-  len = ikev2_generate_message (sa, ike0, 0, 0);
+  len = ikev2_generate_message (b0, sa, ike0, 0, 0);
+  if (~0 == len)
+    return;
 
   if (sa->is_initiator)
     {
@@ -3344,6 +3492,19 @@ ikev2_cleanup_profile_sessions (ikev2_main_t * km, ikev2_profile_t * p)
   vec_free (del_sai);
 }
 
+static void
+ikev2_profile_free (ikev2_profile_t * p)
+{
+  vec_free (p->name);
+
+  vec_free (p->auth.data);
+  if (p->auth.key)
+    EVP_PKEY_free (p->auth.key);
+
+  vec_free (p->loc_id.data);
+  vec_free (p->rem_id.data);
+}
+
 clib_error_t *
 ikev2_add_del_profile (vlib_main_t * vm, u8 * name, int is_add)
 {
@@ -3373,7 +3534,7 @@ ikev2_add_del_profile (vlib_main_t * vm, u8 * name, int is_add)
       ikev2_unregister_udp_port (p);
       ikev2_cleanup_profile_sessions (km, p);
 
-      vec_free (p->name);
+      ikev2_profile_free (p);
       pool_put (km->profiles, p);
       mhash_unset (&km->profile_index_by_name, name, 0);
     }
@@ -3394,7 +3555,11 @@ ikev2_set_profile_auth (vlib_main_t * vm, u8 * name, u8 auth_method,
       r = clib_error_return (0, "unknown profile %v", name);
       return r;
     }
+
+  if (p->auth.key)
+    EVP_PKEY_free (p->auth.key);
   vec_free (p->auth.data);
+
   p->auth.method = auth_method;
   p->auth.data = vec_dup (auth_data);
   p->auth.hex = data_hex_format;
@@ -3402,8 +3567,6 @@ ikev2_set_profile_auth (vlib_main_t * vm, u8 * name, u8 auth_method,
   if (auth_method == IKEV2_AUTH_METHOD_RSA_SIG)
     {
       vec_add1 (p->auth.data, 0);
-      if (p->auth.key)
-       EVP_PKEY_free (p->auth.key);
       p->auth.key = ikev2_load_cert_file (p->auth.data);
       if (p->auth.key == NULL)
        return clib_error_return (0, "load cert '%s' failed", p->auth.data);
@@ -3676,6 +3839,7 @@ ikev2_initiate_sa_init (vlib_main_t * vm, u8 * name)
 
   /* Create the Initiator Request */
   {
+    vlib_buffer_t *b0;
     ike_header_t *ike0;
     u32 bi0 = 0;
     ip_lookup_main_t *lm = &im->lookup_main;
@@ -3689,13 +3853,14 @@ ikev2_initiate_sa_init (vlib_main_t * vm, u8 * name)
       pool_elt_at_index (lm->if_address_pool, if_add_index0);
     ip4_address_t *if_ip = ip_interface_address_get_address (lm, if_add);
 
-    bi0 = ikev2_get_new_ike_header_buff (vm, &ike0);
+    bi0 = ikev2_get_new_ike_header_buff (vm, &b0);
     if (!bi0)
       {
        char *errmsg = "buffer alloc failure";
        ikev2_log_error (errmsg);
        return clib_error_return (0, errmsg);
       }
+    ike0 = vlib_buffer_get_current (b0);
 
     /* Prepare the SA and the IKE payload */
     ikev2_sa_t sa;
@@ -3739,7 +3904,7 @@ ikev2_initiate_sa_init (vlib_main_t * vm, u8 * name)
     u8 *nat_detection_sha1 =
       ikev2_compute_nat_sha1 (clib_host_to_net_u64 (sa.ispi),
                              clib_host_to_net_u64 (sa.rspi),
-                             clib_host_to_net_u32 (if_ip->as_u32),
+                             if_ip->as_u32,
                              clib_host_to_net_u16 (IKEV2_PORT));
 
     ikev2_payload_add_notify (chain, IKEV2_NOTIFY_MSG_NAT_DETECTION_SOURCE_IP,
@@ -3748,7 +3913,7 @@ ikev2_initiate_sa_init (vlib_main_t * vm, u8 * name)
     nat_detection_sha1 =
       ikev2_compute_nat_sha1 (clib_host_to_net_u64 (sa.ispi),
                              clib_host_to_net_u64 (sa.rspi),
-                             clib_host_to_net_u32 (p->responder.ip4.as_u32),
+                             p->responder.ip4.as_u32,
                              clib_host_to_net_u16 (sa.dst_port));
     ikev2_payload_add_notify (chain,
                              IKEV2_NOTIFY_MSG_NAT_DETECTION_DESTINATION_IP,
@@ -3774,12 +3939,12 @@ ikev2_initiate_sa_init (vlib_main_t * vm, u8 * name)
     ike0->version = IKE_VERSION_2;
     ike0->flags = IKEV2_HDR_FLAG_INITIATOR;
     ike0->exchange = IKEV2_EXCHANGE_SA_INIT;
-    ike0->ispi = sa.ispi;
+    ike0->ispi = clib_host_to_net_u64 (sa.ispi);
     ike0->rspi = 0;
     ike0->msgid = 0;
 
     /* store whole IKE payload - needed for PSK auth */
-    vec_free (sa.last_sa_init_req_packet_data);
+    vec_reset_length (sa.last_sa_init_req_packet_data);
     vec_add (sa.last_sa_init_req_packet_data, ike0, len);
 
     /* add data to the SA then add it to the pool */
@@ -3823,15 +3988,17 @@ ikev2_delete_child_sa_internal (vlib_main_t * vm, ikev2_sa_t * sa,
   ikev2_main_t *km = &ikev2_main;
   ike_header_t *ike0;
   u32 bi0 = 0;
+  vlib_buffer_t *b0;
   int len;
 
-  bi0 = ikev2_get_new_ike_header_buff (vm, &ike0);
+  bi0 = ikev2_get_new_ike_header_buff (vm, &b0);
   if (!bi0)
     {
       ikev2_log_error ("buffer alloc failure");
       return;
     }
 
+  ike0 = vlib_buffer_get_current (b0);
   ike0->exchange = IKEV2_EXCHANGE_INFORMATIONAL;
   ike0->ispi = clib_host_to_net_u64 (sa->ispi);
   ike0->rspi = clib_host_to_net_u64 (sa->rspi);
@@ -3840,7 +4007,10 @@ ikev2_delete_child_sa_internal (vlib_main_t * vm, ikev2_sa_t * sa,
   sa->del->spi = csa->i_proposals->spi;
   ike0->msgid = clib_host_to_net_u32 (sa->last_init_msg_id + 1);
   sa->last_init_msg_id = clib_net_to_host_u32 (ike0->msgid);
-  len = ikev2_generate_message (sa, ike0, 0, 0);
+  len = ikev2_generate_message (b0, sa, ike0, 0, 0);
+  if (~0 == len)
+    return;
+
   if (sa->natt)
     len = ikev2_insert_non_esp_marker (ike0, len);
   ikev2_send_ike (vm, &sa->iaddr, &sa->raddr, bi0, len,
@@ -3934,16 +4104,18 @@ ikev2_rekey_child_sa_internal (vlib_main_t * vm, ikev2_sa_t * sa,
 {
   /* Create the Initiator request for create child SA */
   ike_header_t *ike0;
+  vlib_buffer_t *b0;
   u32 bi0 = 0;
   int len;
 
-  bi0 = ikev2_get_new_ike_header_buff (vm, &ike0);
+  bi0 = ikev2_get_new_ike_header_buff (vm, &b0);
   if (!bi0)
     {
       ikev2_log_error ("buffer alloc failure");
       return;
     }
 
+  ike0 = vlib_buffer_get_current (b0);
   ike0->version = IKE_VERSION_2;
   ike0->flags = IKEV2_HDR_FLAG_INITIATOR;
   ike0->exchange = IKEV2_EXCHANGE_CREATE_CHILD_SA;
@@ -3960,7 +4132,10 @@ ikev2_rekey_child_sa_internal (vlib_main_t * vm, ikev2_sa_t * sa,
   RAND_bytes ((u8 *) & proposals[0].spi, sizeof (proposals[0].spi));
   rekey->spi = proposals[0].spi;
   rekey->ispi = csa->i_proposals->spi;
-  len = ikev2_generate_message (sa, ike0, proposals, 0);
+  len = ikev2_generate_message (b0, sa, ike0, proposals, 0);
+  if (~0 == len)
+    return;
+
   if (sa->natt)
     len = ikev2_insert_non_esp_marker (ike0, len);
   ikev2_send_ike (vm, &sa->iaddr, &sa->raddr, bi0, len,
@@ -4271,23 +4446,28 @@ ikev2_send_informational_request (ikev2_sa_t * sa)
   ikev2_main_t *km = &ikev2_main;
   ip4_address_t *src, *dst;
   ike_header_t *ike0;
+  vlib_buffer_t *b0;
   u32 bi0 = 0;
   u16 dp;
   int len;
 
-  bi0 = ikev2_get_new_ike_header_buff (km->vlib_main, &ike0);
+  bi0 = ikev2_get_new_ike_header_buff (km->vlib_main, &b0);
   if (!bi0)
     {
       ikev2_log_error ("buffer alloc failure");
       return;
     }
 
+  ike0 = vlib_buffer_get_current (b0);
   ike0->exchange = IKEV2_EXCHANGE_INFORMATIONAL;
   ike0->ispi = clib_host_to_net_u64 (sa->ispi);
   ike0->rspi = clib_host_to_net_u64 (sa->rspi);
   ike0->msgid = clib_host_to_net_u32 (sa->last_init_msg_id + 1);
   sa->last_init_msg_id = clib_net_to_host_u32 (ike0->msgid);
-  len = ikev2_generate_message (sa, ike0, 0, 0);
+  len = ikev2_generate_message (b0, sa, ike0, 0, 0);
+  if (~0 == len)
+    return;
+
   if (sa->natt)
     len = ikev2_insert_non_esp_marker (ike0, len);
 
@@ -4313,7 +4493,7 @@ ikev2_mngr_process_responder_sas (ikev2_sa_t * sa)
   ikev2_main_t *km = &ikev2_main;
   vlib_main_t *vm = km->vlib_main;
 
-  if (!sa->sk_ai || !sa->sk_ar)
+  if (!sa->keys_generated)
     return 0;
 
   if (sa->liveness_retries >= km->liveness_max_retries)