]> git.saurik.com Git - apple/xnu.git/blobdiff - bsd/netinet6/ipsec.c
xnu-1699.22.81.tar.gz
[apple/xnu.git] / bsd / netinet6 / ipsec.c
index 86f4639dcc8e7c0395d5b78c939aa22e46874ea9..91fd6db6dd7b6ce8320506d2721ed7a62c4b9ae6 100644 (file)
@@ -1,3 +1,31 @@
+/*
+ * Copyright (c) 2008-2011 Apple Inc. All rights reserved.
+ *
+ * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
+ * 
+ * This file contains Original Code and/or Modifications of Original Code
+ * as defined in and that are subject to the Apple Public Source License
+ * Version 2.0 (the 'License'). You may not use this file except in
+ * compliance with the License. The rights granted to you under the License
+ * may not be used to create, or enable the creation or redistribution of,
+ * unlawful or unlicensed copies of an Apple operating system, or to
+ * circumvent, violate, or enable the circumvention or violation of, any
+ * terms of an Apple operating system software license agreement.
+ * 
+ * Please obtain a copy of the License at
+ * http://www.opensource.apple.com/apsl/ and read it before using this file.
+ * 
+ * The Original Code and all software distributed under the License are
+ * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
+ * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
+ * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
+ * Please see the License for the specific language governing rights and
+ * limitations under the License.
+ * 
+ * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
+ */
+
 /*     $FreeBSD: src/sys/netinet6/ipsec.c,v 1.3.2.7 2001/07/19 06:37:23 kris Exp $     */
 /*     $KAME: ipsec.c,v 1.103 2001/05/24 07:14:18 sakane Exp $ */
 
@@ -47,6 +75,9 @@
 #include <sys/kernel.h>
 #include <sys/syslog.h>
 #include <sys/sysctl.h>
+#include <kern/locks.h>
+#include <sys/kauth.h>
+#include <libkern/OSAtomic.h>
 
 #include <net/if.h>
 #include <net/route.h>
@@ -111,6 +142,7 @@ int ipsec_debug = 0;
 #define DBG_FNC_GETPOL_ADDR            NETDBG_CODE(DBG_NETIPSEC, (2 << 8))
 #define DBG_FNC_IPSEC_OUT              NETDBG_CODE(DBG_NETIPSEC, (3 << 8))
 
+extern lck_mtx_t *sadb_mutex;
 
 struct ipsecstat ipsecstat;
 int ip4_ah_cleartos = 1;
@@ -125,41 +157,44 @@ int ip4_ipsec_ecn = 0;            /* ECN ignore(-1)/forbidden(0)/allowed(1) */
 int ip4_esp_randpad = -1;
 int    esp_udp_encap_port = 0;
 static int sysctl_def_policy SYSCTL_HANDLER_ARGS;
+extern int natt_keepalive_interval;
 extern u_int32_t natt_now;
 
+struct ipsec_tag;
+
 SYSCTL_DECL(_net_inet_ipsec);
 #if INET6
 SYSCTL_DECL(_net_inet6_ipsec6);
 #endif
 /* net.inet.ipsec */
 SYSCTL_STRUCT(_net_inet_ipsec, IPSECCTL_STATS,
-       stats, CTLFLAG_RD     &ipsecstat,     ipsecstat, "");
-SYSCTL_PROC(_net_inet_ipsec, IPSECCTL_DEF_POLICY, def_policy, CTLTYPE_INT|CTLFLAG_RW,
+       stats, CTLFLAG_RD | CTLFLAG_LOCKED,     &ipsecstat,     ipsecstat, "");
+SYSCTL_PROC(_net_inet_ipsec, IPSECCTL_DEF_POLICY, def_policy, CTLTYPE_INT|CTLFLAG_RW | CTLFLAG_LOCKED,
        &ip4_def_policy.policy, 0, &sysctl_def_policy, "I", "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_TRANSLEV, esp_trans_deflev,
-       CTLFLAG_RW, &ip4_esp_trans_deflev,      0, "");
+       CTLFLAG_RW | CTLFLAG_LOCKED, &ip4_esp_trans_deflev,     0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_NETLEV, esp_net_deflev,
-       CTLFLAG_RW, &ip4_esp_net_deflev,        0, "");
+       CTLFLAG_RW | CTLFLAG_LOCKED, &ip4_esp_net_deflev,       0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
-       CTLFLAG_RW, &ip4_ah_trans_deflev,       0, "");
+       CTLFLAG_RW | CTLFLAG_LOCKED, &ip4_ah_trans_deflev,      0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
-       CTLFLAG_RW, &ip4_ah_net_deflev, 0, "");
+       CTLFLAG_RW | CTLFLAG_LOCKED, &ip4_ah_net_deflev,        0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_AH_CLEARTOS,
-       ah_cleartos, CTLFLAG_RW       &ip4_ah_cleartos,       0, "");
+       ah_cleartos, CTLFLAG_RW | CTLFLAG_LOCKED,       &ip4_ah_cleartos,       0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_AH_OFFSETMASK,
-       ah_offsetmask, CTLFLAG_RW     &ip4_ah_offsetmask,     0, "");
+       ah_offsetmask, CTLFLAG_RW | CTLFLAG_LOCKED,     &ip4_ah_offsetmask,     0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DFBIT,
-       dfbit, CTLFLAG_RW     &ip4_ipsec_dfbit,       0, "");
+       dfbit, CTLFLAG_RW | CTLFLAG_LOCKED,     &ip4_ipsec_dfbit,       0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ECN,
-       ecn, CTLFLAG_RW       &ip4_ipsec_ecn, 0, "");
+       ecn, CTLFLAG_RW | CTLFLAG_LOCKED,       &ip4_ipsec_ecn, 0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEBUG,
-       debug, CTLFLAG_RW     &ipsec_debug,   0, "");
+       debug, CTLFLAG_RW | CTLFLAG_LOCKED,     &ipsec_debug,   0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ESP_RANDPAD,
-       esp_randpad, CTLFLAG_RW       &ip4_esp_randpad,       0, "");
+       esp_randpad, CTLFLAG_RW | CTLFLAG_LOCKED,       &ip4_esp_randpad,       0, "");
 
 /* for performance, we bypass ipsec until a security policy is set */
 int ipsec_bypass = 1;
-SYSCTL_INT(_net_inet_ipsec, OID_AUTO, bypass, CTLFLAG_RD, &ipsec_bypass,0, "");
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO, bypass, CTLFLAG_RD | CTLFLAG_LOCKED, &ipsec_bypass,0, "");
 
 /*
  * NAT Traversal requires a UDP port for encapsulation,
@@ -168,7 +203,7 @@ SYSCTL_INT(_net_inet_ipsec, OID_AUTO, bypass, CTLFLAG_RD, &ipsec_bypass,0, "");
  * for nat traversal.
  */
 SYSCTL_INT(_net_inet_ipsec, OID_AUTO, esp_port,
-                  CTLFLAG_RW, &esp_udp_encap_port, 0, "");
+                  CTLFLAG_RW | CTLFLAG_LOCKED, &esp_udp_encap_port, 0, "");
 
 #if INET6
 struct ipsecstat ipsec6stat;
@@ -182,63 +217,63 @@ int ip6_esp_randpad = -1;
 
 /* net.inet6.ipsec6 */
 SYSCTL_STRUCT(_net_inet6_ipsec6, IPSECCTL_STATS,
-       stats, CTLFLAG_RD, &ipsec6stat, ipsecstat, "");
+       stats, CTLFLAG_RD | CTLFLAG_LOCKED, &ipsec6stat, ipsecstat, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_POLICY,
-       def_policy, CTLFLAG_RW, &ip6_def_policy.policy, 0, "");
+       def_policy, CTLFLAG_RW | CTLFLAG_LOCKED,        &ip6_def_policy.policy, 0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_ESP_TRANSLEV, esp_trans_deflev,
-       CTLFLAG_RW, &ip6_esp_trans_deflev,      0, "");
+       CTLFLAG_RW | CTLFLAG_LOCKED, &ip6_esp_trans_deflev,     0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_ESP_NETLEV, esp_net_deflev,
-       CTLFLAG_RW, &ip6_esp_net_deflev,        0, "");
+       CTLFLAG_RW | CTLFLAG_LOCKED, &ip6_esp_net_deflev,       0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
-       CTLFLAG_RW, &ip6_ah_trans_deflev,       0, "");
+       CTLFLAG_RW | CTLFLAG_LOCKED, &ip6_ah_trans_deflev,      0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
-       CTLFLAG_RW, &ip6_ah_net_deflev, 0, "");
+       CTLFLAG_RW | CTLFLAG_LOCKED, &ip6_ah_net_deflev,        0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ECN,
-       ecn, CTLFLAG_RW       &ip6_ipsec_ecn, 0, "");
+       ecn, CTLFLAG_RW | CTLFLAG_LOCKED,       &ip6_ipsec_ecn, 0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEBUG,
-       debug, CTLFLAG_RW     &ipsec_debug,   0, "");
+       debug, CTLFLAG_RW | CTLFLAG_LOCKED,     &ipsec_debug,   0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ESP_RANDPAD,
-       esp_randpad, CTLFLAG_RW       &ip6_esp_randpad,       0, "");
+       esp_randpad, CTLFLAG_RW | CTLFLAG_LOCKED,       &ip6_esp_randpad,       0, "");
 #endif /* INET6 */
 
-static int ipsec_setspidx_mbuf
-       __P((struct secpolicyindex *, u_int, u_int, struct mbuf *, int));
-static int ipsec4_setspidx_inpcb __P((struct mbuf *, struct inpcb *pcb));
+static int ipsec_setspidx_mbuf(struct secpolicyindex *, u_int, u_int,
+       struct mbuf *, int);
+static int ipsec4_setspidx_inpcb(struct mbuf *, struct inpcb *pcb);
 #if INET6
-static int ipsec6_setspidx_in6pcb __P((struct mbuf *, struct in6pcb *pcb));
+static int ipsec6_setspidx_in6pcb(struct mbuf *, struct in6pcb *pcb);
 #endif
-static int ipsec_setspidx __P((struct mbuf *, struct secpolicyindex *, int));
-static void ipsec4_get_ulp __P((struct mbuf *m, struct secpolicyindex *, int));
-static int ipsec4_setspidx_ipaddr __P((struct mbuf *, struct secpolicyindex *));
+static int ipsec_setspidx(struct mbuf *, struct secpolicyindex *, int);
+static void ipsec4_get_ulp(struct mbuf *m, struct secpolicyindex *, int);
+static int ipsec4_setspidx_ipaddr(struct mbuf *, struct secpolicyindex *);
 #if INET6
-static void ipsec6_get_ulp __P((struct mbuf *m, struct secpolicyindex *, int));
-static int ipsec6_setspidx_ipaddr __P((struct mbuf *, struct secpolicyindex *));
+static void ipsec6_get_ulp(struct mbuf *m, struct secpolicyindex *, int);
+static int ipsec6_setspidx_ipaddr(struct mbuf *, struct secpolicyindex *);
 #endif
-static struct inpcbpolicy *ipsec_newpcbpolicy __P((void));
-static void ipsec_delpcbpolicy __P((struct inpcbpolicy *));
-static struct secpolicy *ipsec_deepcopy_policy __P((struct secpolicy *src));
-static int ipsec_set_policy __P((struct secpolicy **pcb_sp,
-       int optname, caddr_t request, size_t len, int priv));
-static int ipsec_get_policy __P((struct secpolicy *pcb_sp, struct mbuf **mp));
-static void vshiftl __P((unsigned char *, int, int));
-static int ipsec_in_reject __P((struct secpolicy *, struct mbuf *));
-static size_t ipsec_hdrsiz __P((struct secpolicy *));
+static struct inpcbpolicy *ipsec_newpcbpolicy(void);
+static void ipsec_delpcbpolicy(struct inpcbpolicy *);
+static struct secpolicy *ipsec_deepcopy_policy(struct secpolicy *src);
+static int ipsec_set_policy(struct secpolicy **pcb_sp,
+       int optname, caddr_t request, size_t len, int priv);
+static int ipsec_get_policy(struct secpolicy *pcb_sp, struct mbuf **mp);
+static void vshiftl(unsigned char *, int, int);
+static int ipsec_in_reject(struct secpolicy *, struct mbuf *);
 #if INET
-static struct mbuf *ipsec4_splithdr __P((struct mbuf *));
+static struct mbuf *ipsec4_splithdr(struct mbuf *);
 #endif
 #if INET6
-static struct mbuf *ipsec6_splithdr __P((struct mbuf *));
+static struct mbuf *ipsec6_splithdr(struct mbuf *);
 #endif
 #if INET
-static int ipsec4_encapsulate __P((struct mbuf *, struct secasvar *));
+static int ipsec4_encapsulate(struct mbuf *, struct secasvar *);
 #endif
 #if INET6
-static int ipsec6_encapsulate __P((struct mbuf *, struct secasvar *));
+static int ipsec6_encapsulate(struct mbuf *, struct secasvar *);
+static int ipsec64_encapsulate(struct mbuf *, struct secasvar *);
 #endif
-static struct mbuf *ipsec_addaux __P((struct mbuf *));
-static struct mbuf *ipsec_findaux __P((struct mbuf *));
-static void ipsec_optaux __P((struct mbuf *, struct mbuf *));
-void ipsec_send_natt_keepalive(struct secasvar *sav);
+static struct ipsec_tag *ipsec_addaux(struct mbuf *);
+static struct ipsec_tag *ipsec_findaux(struct mbuf *);
+static void ipsec_optaux(struct mbuf *, struct ipsec_tag *);
+int ipsec_send_natt_keepalive(struct secasvar *sav);
 
 static int
 sysctl_def_policy SYSCTL_HANDLER_ARGS
@@ -246,6 +281,8 @@ sysctl_def_policy SYSCTL_HANDLER_ARGS
        int old_policy = ip4_def_policy.policy;
        int error = sysctl_handle_int(oidp, oidp->oid_arg1, oidp->oid_arg2, req);
 
+#pragma unused(arg1, arg2)
+
        if (ip4_def_policy.policy != IPSEC_POLICY_NONE &&
                ip4_def_policy.policy != IPSEC_POLICY_DISCARD) {
                ip4_def_policy.policy = old_policy;
@@ -266,7 +303,7 @@ sysctl_def_policy SYSCTL_HANDLER_ARGS
  *             0       : bypass
  *             EACCES  : discard packet.
  *             ENOENT  : ipsec_acquire() in progress, maybe.
- *             others  : error occured.
+ *             others  : error occurred.
  *     others: a pointer to SP
  *
  * NOTE: IPv6 mapped adddress concern is implemented here.
@@ -282,10 +319,16 @@ ipsec4_getpolicybysock(m, dir, so, error)
        struct secpolicy *currsp = NULL;        /* policy on socket */
        struct secpolicy *kernsp = NULL;        /* policy on kernel */
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        /* sanity check */
        if (m == NULL || so == NULL || error == NULL)
                panic("ipsec4_getpolicybysock: NULL pointer was passed.\n");
        
+       if (so->so_pcb == NULL) {
+               printf("ipsec4_getpolicybysock: so->so_pcb == NULL\n");
+               return ipsec4_getpolicybyaddr(m, dir, 0, error);
+       }
+       
        switch (so->so_proto->pr_domain->dom_family) {
        case AF_INET:
                pcbsp = sotoinpcb(so)->inp_sp;
@@ -346,7 +389,9 @@ ipsec4_getpolicybysock(m, dir, so, error)
        if (pcbsp->priv) {
                switch (currsp->policy) {
                case IPSEC_POLICY_BYPASS:
+                       lck_mtx_lock(sadb_mutex);
                        currsp->refcnt++;
+                       lck_mtx_unlock(sadb_mutex);
                        *error = 0;
                        KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 2,*error,0,0,0);
                        return currsp;
@@ -366,6 +411,7 @@ ipsec4_getpolicybysock(m, dir, so, error)
                        }
 
                        /* no SP found */
+                       lck_mtx_lock(sadb_mutex);
                        if (ip4_def_policy.policy != IPSEC_POLICY_DISCARD
                         && ip4_def_policy.policy != IPSEC_POLICY_NONE) {
                                ipseclog((LOG_INFO,
@@ -374,12 +420,15 @@ ipsec4_getpolicybysock(m, dir, so, error)
                                ip4_def_policy.policy = IPSEC_POLICY_NONE;
                        }
                        ip4_def_policy.refcnt++;
+                       lck_mtx_unlock(sadb_mutex);
                        *error = 0;
                        KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 4,*error,0,0,0);
                        return &ip4_def_policy;
                        
                case IPSEC_POLICY_IPSEC:
+                       lck_mtx_lock(sadb_mutex);
                        currsp->refcnt++;
+                       lck_mtx_unlock(sadb_mutex);
                        *error = 0;
                        KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 5,*error,0,0,0);
                        return currsp;
@@ -419,6 +468,7 @@ ipsec4_getpolicybysock(m, dir, so, error)
                return NULL;
 
        case IPSEC_POLICY_ENTRUST:
+               lck_mtx_lock(sadb_mutex);
                if (ip4_def_policy.policy != IPSEC_POLICY_DISCARD
                 && ip4_def_policy.policy != IPSEC_POLICY_NONE) {
                        ipseclog((LOG_INFO,
@@ -427,12 +477,15 @@ ipsec4_getpolicybysock(m, dir, so, error)
                        ip4_def_policy.policy = IPSEC_POLICY_NONE;
                }
                ip4_def_policy.refcnt++;
+               lck_mtx_unlock(sadb_mutex);
                *error = 0;
                KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 9,*error,0,0,0);
                return &ip4_def_policy;
 
        case IPSEC_POLICY_IPSEC:
+               lck_mtx_lock(sadb_mutex);
                currsp->refcnt++;
+               lck_mtx_unlock(sadb_mutex);
                *error = 0;
                KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 10,*error,0,0,0);
                return currsp;
@@ -455,7 +508,7 @@ ipsec4_getpolicybysock(m, dir, so, error)
  *             0       : bypass
  *             EACCES  : discard packet.
  *             ENOENT  : ipsec_acquire() in progress, maybe.
- *             others  : error occured.
+ *             others  : error occurred.
  */
 struct secpolicy *
 ipsec4_getpolicybyaddr(m, dir, flag, error)
@@ -469,6 +522,8 @@ ipsec4_getpolicybyaddr(m, dir, flag, error)
        if (ipsec_bypass != 0)
                return 0;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
        /* sanity check */
        if (m == NULL || error == NULL)
                panic("ipsec4_getpolicybyaddr: NULL pointer was passed.\n");
@@ -502,6 +557,7 @@ ipsec4_getpolicybyaddr(m, dir, flag, error)
        }
 
        /* no SP found */
+       lck_mtx_lock(sadb_mutex);
        if (ip4_def_policy.policy != IPSEC_POLICY_DISCARD
         && ip4_def_policy.policy != IPSEC_POLICY_NONE) {
                ipseclog((LOG_INFO, "fixed system default policy:%d->%d\n",
@@ -510,6 +566,7 @@ ipsec4_getpolicybyaddr(m, dir, flag, error)
                ip4_def_policy.policy = IPSEC_POLICY_NONE;
        }
        ip4_def_policy.refcnt++;
+       lck_mtx_unlock(sadb_mutex);
        *error = 0;
        KERNEL_DEBUG(DBG_FNC_GETPOL_ADDR | DBG_FUNC_END, 3,*error,0,0,0);
        return &ip4_def_policy;
@@ -523,7 +580,7 @@ ipsec4_getpolicybyaddr(m, dir, flag, error)
  *             0       : bypass
  *             EACCES  : discard packet.
  *             ENOENT  : ipsec_acquire() in progress, maybe.
- *             others  : error occured.
+ *             others  : error occurred.
  *     others: a pointer to SP
  */
 struct secpolicy *
@@ -537,6 +594,8 @@ ipsec6_getpolicybysock(m, dir, so, error)
        struct secpolicy *currsp = NULL;        /* policy on socket */
        struct secpolicy *kernsp = NULL;        /* policy on kernel */
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
        /* sanity check */
        if (m == NULL || so == NULL || error == NULL)
                panic("ipsec6_getpolicybysock: NULL pointer was passed.\n");
@@ -578,7 +637,9 @@ ipsec6_getpolicybysock(m, dir, so, error)
        if (pcbsp->priv) {
                switch (currsp->policy) {
                case IPSEC_POLICY_BYPASS:
+                       lck_mtx_lock(sadb_mutex);
                        currsp->refcnt++;
+                       lck_mtx_unlock(sadb_mutex);
                        *error = 0;
                        return currsp;
 
@@ -596,6 +657,7 @@ ipsec6_getpolicybysock(m, dir, so, error)
                        }
 
                        /* no SP found */
+                       lck_mtx_lock(sadb_mutex);
                        if (ip6_def_policy.policy != IPSEC_POLICY_DISCARD
                         && ip6_def_policy.policy != IPSEC_POLICY_NONE) {
                                ipseclog((LOG_INFO,
@@ -604,11 +666,14 @@ ipsec6_getpolicybysock(m, dir, so, error)
                                ip6_def_policy.policy = IPSEC_POLICY_NONE;
                        }
                        ip6_def_policy.refcnt++;
+                       lck_mtx_unlock(sadb_mutex);
                        *error = 0;
                        return &ip6_def_policy;
                        
                case IPSEC_POLICY_IPSEC:
+                       lck_mtx_lock(sadb_mutex);
                        currsp->refcnt++;
+                       lck_mtx_unlock(sadb_mutex);
                        *error = 0;
                        return currsp;
 
@@ -644,6 +709,7 @@ ipsec6_getpolicybysock(m, dir, so, error)
                return NULL;
 
        case IPSEC_POLICY_ENTRUST:
+               lck_mtx_lock(sadb_mutex);
                if (ip6_def_policy.policy != IPSEC_POLICY_DISCARD
                 && ip6_def_policy.policy != IPSEC_POLICY_NONE) {
                        ipseclog((LOG_INFO,
@@ -652,11 +718,14 @@ ipsec6_getpolicybysock(m, dir, so, error)
                        ip6_def_policy.policy = IPSEC_POLICY_NONE;
                }
                ip6_def_policy.refcnt++;
+               lck_mtx_unlock(sadb_mutex);
                *error = 0;
                return &ip6_def_policy;
 
        case IPSEC_POLICY_IPSEC:
+               lck_mtx_lock(sadb_mutex);
                currsp->refcnt++;
+               lck_mtx_unlock(sadb_mutex);
                *error = 0;
                return currsp;
 
@@ -680,7 +749,7 @@ ipsec6_getpolicybysock(m, dir, so, error)
  *             0       : bypass
  *             EACCES  : discard packet.
  *             ENOENT  : ipsec_acquire() in progress, maybe.
- *             others  : error occured.
+ *             others  : error occurred.
  */
 #ifndef IP_FORWARDING
 #define IP_FORWARDING 1
@@ -695,6 +764,8 @@ ipsec6_getpolicybyaddr(m, dir, flag, error)
 {
        struct secpolicy *sp = NULL;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
        /* sanity check */
        if (m == NULL || error == NULL)
                panic("ipsec6_getpolicybyaddr: NULL pointer was passed.\n");
@@ -724,6 +795,7 @@ ipsec6_getpolicybyaddr(m, dir, flag, error)
        }
 
        /* no SP found */
+       lck_mtx_lock(sadb_mutex);
        if (ip6_def_policy.policy != IPSEC_POLICY_DISCARD
         && ip6_def_policy.policy != IPSEC_POLICY_NONE) {
                ipseclog((LOG_INFO, "fixed system default policy: %d->%d\n",
@@ -731,6 +803,7 @@ ipsec6_getpolicybyaddr(m, dir, flag, error)
                ip6_def_policy.policy = IPSEC_POLICY_NONE;
        }
        ip6_def_policy.refcnt++;
+       lck_mtx_unlock(sadb_mutex);
        *error = 0;
        return &ip6_def_policy;
 }
@@ -747,11 +820,12 @@ ipsec6_getpolicybyaddr(m, dir, flag, error)
  *     other:  failure, and set errno.
  */
 int
-ipsec_setspidx_mbuf(spidx, dir, family, m, needport)
-       struct secpolicyindex *spidx;
-       u_int dir, family;
-       struct mbuf *m;
-       int needport;
+ipsec_setspidx_mbuf(
+       struct secpolicyindex *spidx,
+       u_int dir,
+       __unused u_int family,
+       struct mbuf *m,
+       int needport)
 {
        int error;
 
@@ -921,7 +995,7 @@ ipsec_setspidx(m, spidx, needport)
                        return error;
                ipsec4_get_ulp(m, spidx, needport);
                return 0;
-#ifdef INET6
+#if INET6
        case 6:
                if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
                        KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
@@ -1006,7 +1080,7 @@ ipsec4_get_ulp(m, spidx, needport)
                            uh.uh_dport;
                        return;
                case IPPROTO_AH:
-                       if (m->m_pkthdr.len > off + sizeof(ip6e))
+                       if (off + sizeof(ip6e) > m->m_pkthdr.len)
                                return;
                        m_copydata(m, off, sizeof(ip6e), (caddr_t)&ip6e);
                        off += (ip6e.ip6e_len + 2) << 2;
@@ -1192,7 +1266,7 @@ ipsec_init_policy(so, pcb_sp)
 #ifdef __APPLE__
        if (so->so_uid == 0)
 #else
-       if (so->so_cred != 0 && so->so_cred->pc_ucred->cr_uid == 0)
+       if (so->so_cred != 0 && !suser(so->so_cred->pc_ucred, NULL))
 #endif
                new->priv = 1;
        else
@@ -1206,7 +1280,7 @@ ipsec_init_policy(so, pcb_sp)
        new->sp_in->policy = IPSEC_POLICY_ENTRUST;
 
        if ((new->sp_out = key_newsp()) == NULL) {
-               key_freesp(new->sp_in);
+               key_freesp(new->sp_in, KEY_SADB_UNLOCKED);
                ipsec_delpcbpolicy(new);
                return ENOBUFS;
        }
@@ -1230,14 +1304,14 @@ ipsec_copy_policy(old, new)
 
        sp = ipsec_deepcopy_policy(old->sp_in);
        if (sp) {
-               key_freesp(new->sp_in);
+               key_freesp(new->sp_in, KEY_SADB_UNLOCKED);
                new->sp_in = sp;
        } else
                return ENOBUFS;
 
        sp = ipsec_deepcopy_policy(old->sp_out);
        if (sp) {
-               key_freesp(new->sp_out);
+               key_freesp(new->sp_out, KEY_SADB_UNLOCKED);
                new->sp_out = sp;
        } else
                return ENOBUFS;
@@ -1258,8 +1332,10 @@ ipsec_deepcopy_policy(src)
        struct ipsecrequest *r;
        struct secpolicy *dst;
 
+       if (src == NULL)
+               return NULL;
        dst = key_newsp();
-       if (src == NULL || dst == NULL)
+       if (dst == NULL)
                return NULL;
 
        /*
@@ -1283,7 +1359,6 @@ ipsec_deepcopy_policy(src)
                bcopy(&p->saidx.src, &(*q)->saidx.src, sizeof((*q)->saidx.src));
                bcopy(&p->saidx.dst, &(*q)->saidx.dst, sizeof((*q)->saidx.dst));
 
-               (*q)->sav = NULL;
                (*q)->sp = dst;
 
                q = &((*q)->next);
@@ -1302,17 +1377,18 @@ fail:
                FREE(p, M_SECA);
                p = NULL;
        }
+       key_freesp(dst, KEY_SADB_UNLOCKED);
        return NULL;
 }
 
 /* set policy and ipsec request if present. */
 static int
-ipsec_set_policy(pcb_sp, optname, request, len, priv)
-       struct secpolicy **pcb_sp;
-       int optname;
-       caddr_t request;
-       size_t len;
-       int priv;
+ipsec_set_policy(
+       struct secpolicy **pcb_sp,
+       __unused int optname,
+       caddr_t request,
+       size_t len,
+       int priv)
 {
        struct sadb_x_policy *xpl;
        struct secpolicy *newsp = NULL;
@@ -1346,7 +1422,7 @@ ipsec_set_policy(pcb_sp, optname, request, len, priv)
        newsp->state = IPSEC_SPSTATE_ALIVE;
 
        /* clear old SP and set new SP */
-       key_freesp(*pcb_sp);
+       key_freesp(*pcb_sp, KEY_SADB_UNLOCKED);
        *pcb_sp = newsp;
        KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
                printf("ipsec_set_policy: new policy\n");
@@ -1361,6 +1437,7 @@ ipsec_get_policy(pcb_sp, mp)
        struct mbuf **mp;
 {
 
+
        /* sanity check. */
        if (pcb_sp == NULL || mp == NULL)
                return EINVAL;
@@ -1371,7 +1448,7 @@ ipsec_get_policy(pcb_sp, mp)
                return ENOBUFS;
        }
 
-       (*mp)->m_type = MT_DATA;
+       m_mchtype(*mp, MT_DATA);
        KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
                printf("ipsec_get_policy:\n");
                kdebug_mbuf(*mp));
@@ -1436,6 +1513,8 @@ ipsec4_get_policy(inp, request, len, mp)
        struct secpolicy *pcb_sp;
        int     error = 0;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+
        /* sanity check. */
        if (inp == NULL || request == NULL || mp == NULL)
                return EINVAL;
@@ -1471,6 +1550,7 @@ int
 ipsec4_delete_pcbpolicy(inp)
        struct inpcb *inp;
 {
+
        /* sanity check. */
        if (inp == NULL)
                panic("ipsec4_delete_pcbpolicy: NULL pointer was passed.\n");
@@ -1479,12 +1559,12 @@ ipsec4_delete_pcbpolicy(inp)
                return 0;
 
        if (inp->inp_sp->sp_in != NULL) {
-               key_freesp(inp->inp_sp->sp_in);
+               key_freesp(inp->inp_sp->sp_in, KEY_SADB_UNLOCKED);
                inp->inp_sp->sp_in = NULL;
        }
 
        if (inp->inp_sp->sp_out != NULL) {
-               key_freesp(inp->inp_sp->sp_out);
+               key_freesp(inp->inp_sp->sp_out, KEY_SADB_UNLOCKED);
                inp->inp_sp->sp_out = NULL;
        }
 
@@ -1586,6 +1666,7 @@ int
 ipsec6_delete_pcbpolicy(in6p)
        struct in6pcb *in6p;
 {
+
        /* sanity check. */
        if (in6p == NULL)
                panic("ipsec6_delete_pcbpolicy: NULL pointer was passed.\n");
@@ -1594,12 +1675,12 @@ ipsec6_delete_pcbpolicy(in6p)
                return 0;
 
        if (in6p->in6p_sp->sp_in != NULL) {
-               key_freesp(in6p->in6p_sp->sp_in);
+               key_freesp(in6p->in6p_sp->sp_in, KEY_SADB_UNLOCKED);
                in6p->in6p_sp->sp_in = NULL;
        }
 
        if (in6p->in6p_sp->sp_out != NULL) {
-               key_freesp(in6p->in6p_sp->sp_out);
+               key_freesp(in6p->in6p_sp->sp_out, KEY_SADB_UNLOCKED);
                in6p->in6p_sp->sp_out = NULL;
        }
 
@@ -1619,7 +1700,7 @@ ipsec_get_reqlevel(isr)
        struct ipsecrequest *isr;
 {
        u_int level = 0;
-       u_int esp_trans_deflev, esp_net_deflev, ah_trans_deflev, ah_net_deflev;
+       u_int esp_trans_deflev = 0, esp_net_deflev = 0, ah_trans_deflev = 0, ah_net_deflev = 0;
 
        /* sanity check */
        if (isr == NULL || isr->sp == NULL)
@@ -1635,7 +1716,7 @@ ipsec_get_reqlevel(isr)
                ? (ipsec_debug                                                \
                        ? log(LOG_INFO, "fixed system default level " #lev ":%d->%d\n",\
                                (lev), IPSEC_LEVEL_REQUIRE)                   \
-                       : 0),                                                 \
+                       : (void)0),                                                                       \
                        (lev) = IPSEC_LEVEL_REQUIRE,                          \
                        (lev)                                                 \
                : (lev))
@@ -1680,6 +1761,7 @@ ipsec_get_reqlevel(isr)
                                level = ah_net_deflev;
                        else
                                level = ah_trans_deflev;
+                       break;
                case IPPROTO_IPCOMP:
                        /*
                         * we don't really care, as IPcomp document says that
@@ -1732,6 +1814,7 @@ ipsec_in_reject(sp, m)
        /* check policy */
        switch (sp->policy) {
        case IPSEC_POLICY_DISCARD:
+       case IPSEC_POLICY_GENERATE:
                return 1;
        case IPSEC_POLICY_BYPASS:
        case IPSEC_POLICY_NONE:
@@ -1761,10 +1844,22 @@ ipsec_in_reject(sp, m)
                        if (level == IPSEC_LEVEL_REQUIRE) {
                                need_conf++;
 
+#if 0
+               /* this won't work with multiple input threads - isr->sav would change 
+                * with every packet and is not necessarily related to the current packet 
+                * being processed.  If ESP processing is required - the esp code should
+                * make sure that the integrity check is present and correct.  I don't see
+                * why it would be necessary to check for the presence of the integrity
+                * check value here.  I think this is just wrong.
+                * isr->sav has been removed.
+                * %%%%%% this needs to be re-worked at some point but I think the code below can 
+                * be ignored for now.
+                */
                                if (isr->sav != NULL
                                 && isr->sav->flags == SADB_X_EXT_NONE
                                 && isr->sav->alg_auth != SADB_AALG_NONE)
                                        need_icv++;
+#endif
                        }
                        break;
                case IPPROTO_AH:
@@ -1809,6 +1904,7 @@ ipsec4_in_reject_so(m, so)
        int error;
        int result;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        /* sanity check */
        if (m == NULL)
                return 0;       /* XXX should be panic ? */
@@ -1829,7 +1925,7 @@ ipsec4_in_reject_so(m, so)
        result = ipsec_in_reject(sp, m);
        KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
                printf("DP ipsec4_in_reject_so call free SP:%p\n", sp));
-       key_freesp(sp);
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return result;
 }
@@ -1839,12 +1935,17 @@ ipsec4_in_reject(m, inp)
        struct mbuf *m;
        struct inpcb *inp;
 {
+       
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        if (inp == NULL)
                return ipsec4_in_reject_so(m, NULL);
        if (inp->inp_socket)
                return ipsec4_in_reject_so(m, inp->inp_socket);
        else
                panic("ipsec4_in_reject: invalid inpcb/socket");
+
+       /* NOTREACHED */
+       return 0;
 }
 
 #if INET6
@@ -1862,6 +1963,7 @@ ipsec6_in_reject_so(m, so)
        int error;
        int result;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        /* sanity check */
        if (m == NULL)
                return 0;       /* XXX should be panic ? */
@@ -1881,7 +1983,7 @@ ipsec6_in_reject_so(m, so)
        result = ipsec_in_reject(sp, m);
        KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
                printf("DP ipsec6_in_reject_so call free SP:%p\n", sp));
-       key_freesp(sp);
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return result;
 }
@@ -1891,12 +1993,17 @@ ipsec6_in_reject(m, in6p)
        struct mbuf *m;
        struct in6pcb *in6p;
 {
+
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        if (in6p == NULL)
                return ipsec6_in_reject_so(m, NULL);
        if (in6p->in6p_socket)
                return ipsec6_in_reject_so(m, in6p->in6p_socket);
        else
                panic("ipsec6_in_reject: invalid in6p/socket");
+
+       /* NOTREACHED */
+       return 0;
 }
 #endif
 
@@ -1905,13 +2012,14 @@ ipsec6_in_reject(m, in6p)
  * in case it is tunneled, it includes the size of outer IP header.
  * NOTE: SP passed is free in this function.
  */
-static size_t
+size_t
 ipsec_hdrsiz(sp)
        struct secpolicy *sp;
 {
        struct ipsecrequest *isr;
        size_t siz, clen;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        KEYDEBUG(KEYDEBUG_IPSEC_DATA,
                printf("ipsec_hdrsiz: using SP\n");
                kdebug_secpolicy(sp));
@@ -1919,6 +2027,7 @@ ipsec_hdrsiz(sp)
        /* check policy */
        switch (sp->policy) {
        case IPSEC_POLICY_DISCARD:
+       case IPSEC_POLICY_GENERATE:
        case IPSEC_POLICY_BYPASS:
        case IPSEC_POLICY_NONE:
                return 0;
@@ -1987,6 +2096,7 @@ ipsec4_hdrsiz(m, dir, inp)
        int error;
        size_t size;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        /* sanity check */
        if (m == NULL)
                return 0;       /* XXX should be panic ? */
@@ -2009,8 +2119,8 @@ ipsec4_hdrsiz(m, dir, inp)
        KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
                printf("DP ipsec4_hdrsiz call free SP:%p\n", sp));
        KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-               printf("ipsec4_hdrsiz: size:%lu.\n", (unsigned long)size));
-       key_freesp(sp);
+               printf("ipsec4_hdrsiz: size:%lu.\n", (u_int32_t)size));
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return size;
 }
@@ -2029,6 +2139,7 @@ ipsec6_hdrsiz(m, dir, in6p)
        int error;
        size_t size;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        /* sanity check */
        if (m == NULL)
                return 0;       /* XXX shoud be panic ? */
@@ -2048,8 +2159,8 @@ ipsec6_hdrsiz(m, dir, in6p)
        KEYDEBUG(KEYDEBUG_IPSEC_STAMP,
                printf("DP ipsec6_hdrsiz call free SP:%p\n", sp));
        KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-               printf("ipsec6_hdrsiz: size:%lu.\n", (unsigned long)size));
-       key_freesp(sp);
+               printf("ipsec6_hdrsiz: size:%lu.\n", (u_int32_t)size));
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return size;
 }
@@ -2257,6 +2368,92 @@ ipsec6_encapsulate(m, sav)
 
        return 0;
 }
+
+static int
+ipsec64_encapsulate(m, sav)
+       struct mbuf *m;
+       struct secasvar *sav;
+{
+       struct ip6_hdr *ip6, *ip6i;
+       struct ip *ip;
+       size_t plen;
+       u_int8_t hlim;
+
+       /* tunneling over IPv4 */
+       if (((struct sockaddr *)&sav->sah->saidx.src)->sa_family
+               != ((struct sockaddr *)&sav->sah->saidx.dst)->sa_family
+        || ((struct sockaddr *)&sav->sah->saidx.src)->sa_family != AF_INET) {
+               m_freem(m);
+               return EINVAL;
+       }
+#if 0
+       /* XXX if the dst is myself, perform nothing. */
+       if (key_ismyaddr((struct sockaddr *)&sav->sah->saidx.dst)) {
+               m_freem(m);
+               return EINVAL;
+       }
+#endif
+
+       plen = m->m_pkthdr.len;
+       ip6 = mtod(m, struct ip6_hdr *);
+       hlim = ip6->ip6_hlim; 
+       /*
+        * grow the mbuf to accomodate the new IPv4 header.
+        */
+       if (m->m_len != sizeof(struct ip6_hdr))
+               panic("ipsec6_encapsulate: assumption failed (first mbuf length)");
+       if (M_LEADINGSPACE(m->m_next) < sizeof(struct ip6_hdr)) {
+               struct mbuf *n;
+               MGET(n, M_DONTWAIT, MT_DATA);
+               if (!n) {
+                       m_freem(m);
+                       return ENOBUFS;
+               }
+               n->m_len = sizeof(struct ip6_hdr);
+               n->m_next = m->m_next;
+               m->m_next = n;
+               m->m_pkthdr.len += sizeof(struct ip);
+               ip6i = mtod(n, struct ip6_hdr *);
+       } else {
+               m->m_next->m_len += sizeof(struct ip6_hdr);
+               m->m_next->m_data -= sizeof(struct ip6_hdr);
+               m->m_pkthdr.len += sizeof(struct ip);
+               ip6i = mtod(m->m_next, struct ip6_hdr *);
+       }
+       /* construct new IPv4 header. see RFC 2401 5.1.2.1 */
+       /* ECN consideration. */
+       /* XXX To be fixed later if needed */
+       // ip_ecn_ingress(ip4_ipsec_ecn, &ip->ip_tos, &oip->ip_tos);    
+
+       bcopy(ip6, ip6i, sizeof(struct ip6_hdr));
+       ip = mtod(m, struct ip *);
+       m->m_len = sizeof(struct ip);
+       /* 
+        * Fill in some of the IPv4 fields - we don't need all of them
+        * because the rest will be filled in by ip_output
+        */
+       ip->ip_v = IPVERSION;
+       ip->ip_hl = sizeof(struct ip) >> 2;
+       ip->ip_id = 0;
+       ip->ip_sum = 0;
+       ip->ip_tos = 0; 
+       ip->ip_off = 0;
+       ip->ip_ttl = hlim;
+       ip->ip_p = IPPROTO_IPV6;
+       if (plen + sizeof(struct ip) < IP_MAXPACKET)
+               ip->ip_len = htons(plen + sizeof(struct ip));
+       else {
+               ip->ip_len = htons(plen);
+               ipseclog((LOG_ERR, "IPv4 ipsec: size exceeds limit: "
+                       "leave ip_len as is (invalid packet)\n"));
+       }
+       bcopy(&((struct sockaddr_in *)&sav->sah->saidx.src)->sin_addr,
+               &ip->ip_src, sizeof(ip->ip_src));
+       bcopy(&((struct sockaddr_in *)&sav->sah->saidx.dst)->sin_addr,
+               &ip->ip_dst, sizeof(ip->ip_dst));
+
+       return 0;
+}
 #endif /*INET6*/
 
 /*
@@ -2280,45 +2477,59 @@ ipsec_chkreplay(seq, sav)
        u_int32_t wsizeb;       /* constant: bits of window size */
        int frlast;             /* constant: last frame */
 
+       
        /* sanity check */
        if (sav == NULL)
                panic("ipsec_chkreplay: NULL pointer was passed.\n");
 
+       lck_mtx_lock(sadb_mutex);
        replay = sav->replay;
 
-       if (replay->wsize == 0)
+       if (replay->wsize == 0) {
+               lck_mtx_unlock(sadb_mutex);
                return 1;       /* no need to check replay. */
+       }
 
        /* constant */
        frlast = replay->wsize - 1;
        wsizeb = replay->wsize << 3;
 
        /* sequence number of 0 is invalid */
-       if (seq == 0)
+       if (seq == 0) {
+               lck_mtx_unlock(sadb_mutex);
                return 0;
+       }
 
        /* first time is always okay */
-       if (replay->count == 0)
+       if (replay->count == 0) {
+               lck_mtx_unlock(sadb_mutex);
                return 1;
+       }
 
        if (seq > replay->lastseq) {
                /* larger sequences are okay */
+               lck_mtx_unlock(sadb_mutex);
                return 1;
        } else {
                /* seq is equal or less than lastseq. */
                diff = replay->lastseq - seq;
 
                /* over range to check, i.e. too old or wrapped */
-               if (diff >= wsizeb)
+               if (diff >= wsizeb) {
+                       lck_mtx_unlock(sadb_mutex);
                        return 0;
+               }
 
                fr = frlast - diff / 8;
 
                /* this packet already seen ? */
-               if ((replay->bitmap)[fr] & (1 << (diff % 8)))
+               if ((replay->bitmap)[fr] & (1 << (diff % 8))) {
+                       lck_mtx_unlock(sadb_mutex);
                        return 0;
+               }
 
                /* out of order but good */
+               lck_mtx_unlock(sadb_mutex);
                return 1;
        }
 }
@@ -2338,11 +2549,12 @@ ipsec_updatereplay(seq, sav)
        int fr;
        u_int32_t wsizeb;       /* constant: bits of window size */
        int frlast;             /* constant: last frame */
-
+       
        /* sanity check */
        if (sav == NULL)
                panic("ipsec_chkreplay: NULL pointer was passed.\n");
 
+       lck_mtx_lock(sadb_mutex);
        replay = sav->replay;
 
        if (replay->wsize == 0)
@@ -2372,7 +2584,7 @@ ipsec_updatereplay(seq, sav)
                if (diff < wsizeb) {
                        /* In window */
                        /* set bit for this packet */
-                       vshiftl(replay->bitmap, diff, replay->wsize);
+                       vshiftl((unsigned char *) replay->bitmap, diff, replay->wsize);
                        (replay->bitmap)[frlast] |= 1;
                } else {
                        /* this packet has a "way larger" */
@@ -2387,14 +2599,18 @@ ipsec_updatereplay(seq, sav)
                diff = replay->lastseq - seq;
 
                /* over range to check, i.e. too old or wrapped */
-               if (diff >= wsizeb)
+               if (diff >= wsizeb) {
+                       lck_mtx_unlock(sadb_mutex);
                        return 1;
+               }
 
                fr = frlast - diff / 8;
 
                /* this packet already seen ? */
-               if ((replay->bitmap)[fr] & (1 << (diff % 8)))
+               if ((replay->bitmap)[fr] & (1 << (diff % 8))) {
+                       lck_mtx_unlock(sadb_mutex);
                        return 1;
+               }
 
                /* mark as seen */
                (replay->bitmap)[fr] |= (1 << (diff % 8));
@@ -2409,15 +2625,18 @@ ok:
                replay->overflow++;
 
                /* don't increment, no more packets accepted */
-               if ((sav->flags & SADB_X_EXT_CYCSEQ) == 0)
+               if ((sav->flags & SADB_X_EXT_CYCSEQ) == 0) {
+                       lck_mtx_unlock(sadb_mutex);
                        return 1;
+               }
 
                ipseclog((LOG_WARNING, "replay counter made %d cycle. %s\n",
                    replay->overflow, ipsec_logsastr(sav)));
        }
 
        replay->count++;
-
+       
+       lck_mtx_unlock(sadb_mutex);
        return 0;
 }
 
@@ -2578,19 +2797,21 @@ ipsec_dumpmbuf(m)
  * IPsec output logic for IPv4.
  */
 int
-ipsec4_output(state, sp, flags)
-       struct ipsec_output_state *state;
-       struct secpolicy *sp;
-       int flags;
+ipsec4_output(
+       struct ipsec_output_state *state,
+       struct secpolicy *sp,
+       __unused int flags)
 {
        struct ip *ip = NULL;
        struct ipsecrequest *isr = NULL;
        struct secasindex saidx;
-       int s;
-       int error;
+       struct secasvar *sav = NULL;
+       int error = 0;
        struct sockaddr_in *dst4;
        struct sockaddr_in *sin;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
        if (!state)
                panic("state == NULL in ipsec4_output");
        if (!state->m)
@@ -2637,11 +2858,42 @@ ipsec4_output(state, sp, flags)
                        sin->sin_len = sizeof(*sin);
                        sin->sin_family = AF_INET;
                        sin->sin_port = IPSEC_PORT_ANY;
+                       /* 
+                       * Get port from packet if upper layer is UDP and nat traversal 
+                       * is enabled and transport mode.                        
+                       */
+
+                       if ((esp_udp_encap_port & 0xFFFF) != 0 &&
+                               isr->saidx.mode == IPSEC_MODE_TRANSPORT) {      
+                               
+                               if (ip->ip_p == IPPROTO_UDP) {
+                                       struct udphdr  *udp;
+                                       size_t hlen;
+#ifdef _IP_VHL
+                                       hlen = IP_VHL_HL(ip->ip_vhl) << 2;
+#else
+                                       hlen = ip->ip_hl << 2;
+#endif
+                                       if (state->m->m_len < hlen + sizeof(struct udphdr)) {
+                                               state->m = m_pullup(state->m, hlen + sizeof(struct udphdr));
+                                               if (!state->m) {
+                                                       ipseclog((LOG_DEBUG,
+                                                               "IPv4 output: can't pullup UDP header\n"));
+                                                       IPSEC_STAT_INCREMENT(ipsecstat.in_inval);
+                                                       goto bad;
+                                               }
+                                               ip = mtod(state->m, struct ip *);
+                                       }
+                                       udp = (struct udphdr *)(((u_int8_t *)ip) + hlen);
+                                       sin->sin_port = udp->uh_dport;
+                               }
+                       }
+
                        bcopy(&ip->ip_dst, &sin->sin_addr,
                            sizeof(sin->sin_addr));
                }
 
-               if ((error = key_checkrequest(isr, &saidx)) != 0) {
+               if ((error = key_checkrequest(isr, &saidx, &sav)) != 0) {
                        /*
                         * IPsec processing is required, but no SA found.
                         * I assume that key_acquire() had been called
@@ -2649,12 +2901,12 @@ ipsec4_output(state, sp, flags)
                         * this packet because it is responsibility for
                         * upper layer to retransmit the packet.
                         */
-                       ipsecstat.out_nosa++;
+                       IPSEC_STAT_INCREMENT(ipsecstat.out_nosa);
                        goto bad;
                }
 
                /* validity check */
-               if (isr->sav == NULL) {
+               if (sav == NULL) {
                        switch (ipsec_get_reqlevel(isr)) {
                        case IPSEC_LEVEL_USE:
                                continue;
@@ -2671,9 +2923,9 @@ ipsec4_output(state, sp, flags)
                 * send to the receiver by dead SA, the receiver can
                 * not decode a packet because SA has been dead.
                 */
-               if (isr->sav->state != SADB_SASTATE_MATURE
-                && isr->sav->state != SADB_SASTATE_DYING) {
-                       ipsecstat.out_nosa++;
+               if (sav->state != SADB_SASTATE_MATURE
+                && sav->state != SADB_SASTATE_DYING) {
+                       IPSEC_STAT_INCREMENT(ipsecstat.out_nosa);
                        error = EINVAL;
                        goto bad;
                }
@@ -2682,42 +2934,45 @@ ipsec4_output(state, sp, flags)
                 * There may be the case that SA status will be changed when
                 * we are refering to one. So calling splsoftnet().
                 */
-               s = splnet();
 
                if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
                        /*
                         * build IPsec tunnel.
                         */
                        /* XXX should be processed with other familiy */
-                       if (((struct sockaddr *)&isr->sav->sah->saidx.src)->sa_family != AF_INET) {
+                       if (((struct sockaddr *)&sav->sah->saidx.src)->sa_family != AF_INET) {
                                ipseclog((LOG_ERR, "ipsec4_output: "
                                    "family mismatched between inner and outer spi=%u\n",
-                                   (u_int32_t)ntohl(isr->sav->spi)));
-                               splx(s);
+                                   (u_int32_t)ntohl(sav->spi)));
                                error = EAFNOSUPPORT;
                                goto bad;
                        }
 
                        state->m = ipsec4_splithdr(state->m);
                        if (!state->m) {
-                               splx(s);
                                error = ENOMEM;
                                goto bad;
                        }
-                       error = ipsec4_encapsulate(state->m, isr->sav);
-                       splx(s);
+                       error = ipsec4_encapsulate(state->m, sav);
                        if (error) {
                                state->m = NULL;
                                goto bad;
                        }
                        ip = mtod(state->m, struct ip *);
 
-                       state->ro = &isr->sav->sah->sa_route;
+                       // grab sadb_mutex, before updating sah's route cache
+                       lck_mtx_lock(sadb_mutex);
+                       state->ro = &sav->sah->sa_route;
                        state->dst = (struct sockaddr *)&state->ro->ro_dst;
                        dst4 = (struct sockaddr_in *)state->dst;
-                       if (state->ro->ro_rt
-                        && ((state->ro->ro_rt->rt_flags & RTF_UP) == 0
-                         || dst4->sin_addr.s_addr != ip->ip_dst.s_addr)) {
+                       if (state->ro->ro_rt != NULL) {
+                               RT_LOCK(state->ro->ro_rt);
+                       }
+                       if (state->ro->ro_rt != NULL &&
+                           (state->ro->ro_rt->generation_id != route_generation ||
+                           !(state->ro->ro_rt->rt_flags & RTF_UP) ||
+                           dst4->sin_addr.s_addr != ip->ip_dst.s_addr)) {
+                               RT_UNLOCK(state->ro->ro_rt);
                                rtfree(state->ro->ro_rt);
                                state->ro->ro_rt = NULL;
                        }
@@ -2726,20 +2981,33 @@ ipsec4_output(state, sp, flags)
                                dst4->sin_len = sizeof(*dst4);
                                dst4->sin_addr = ip->ip_dst;
                                rtalloc(state->ro);
-                       }
-                       if (state->ro->ro_rt == 0) {
-                               ipstat.ips_noroute++;
-                               error = EHOSTUNREACH;
-                               goto bad;
+                               if (state->ro->ro_rt == 0) {
+                                       OSAddAtomic(1, &ipstat.ips_noroute);
+                                       error = EHOSTUNREACH;
+                                       // release sadb_mutex, after updating sah's route cache
+                                       lck_mtx_unlock(sadb_mutex);
+                                       goto bad;
+                               }
+                               RT_LOCK(state->ro->ro_rt);
                        }
 
-                       /* adjust state->dst if tunnel endpoint is offlink */
+                       /*
+                        * adjust state->dst if tunnel endpoint is offlink
+                        *
+                        * XXX: caching rt_gateway value in the state is
+                        * not really good, since it may point elsewhere
+                        * when the gateway gets modified to a larger
+                        * sockaddr via rt_setgate().  This is currently
+                        * addressed by SA_SIZE roundup in that routine.
+                        */
                        if (state->ro->ro_rt->rt_flags & RTF_GATEWAY) {
                                state->dst = (struct sockaddr *)state->ro->ro_rt->rt_gateway;
                                dst4 = (struct sockaddr_in *)state->dst;
                        }
-               } else
-                       splx(s);
+                       RT_UNLOCK(state->ro->ro_rt);
+                       // release sadb_mutex, after updating sah's route cache
+                       lck_mtx_unlock(sadb_mutex);
+               }
 
                state->m = ipsec4_splithdr(state->m);
                if (!state->m) {
@@ -2749,7 +3017,7 @@ ipsec4_output(state, sp, flags)
                switch (isr->saidx.proto) {
                case IPPROTO_ESP:
 #if IPSEC_ESP
-                       if ((error = esp4_output(state->m, isr)) != 0) {
+                       if ((error = esp4_output(state->m, sav)) != 0) {
                                state->m = NULL;
                                goto bad;
                        }
@@ -2761,13 +3029,13 @@ ipsec4_output(state, sp, flags)
                        goto bad;
 #endif
                case IPPROTO_AH:
-                       if ((error = ah4_output(state->m, isr)) != 0) {
+                       if ((error = ah4_output(state->m, sav)) != 0) {
                                state->m = NULL;
                                goto bad;
                        }
                        break;
                case IPPROTO_IPCOMP:
-                       if ((error = ipcomp4_output(state->m, isr)) != 0) {
+                       if ((error = ipcomp4_output(state->m, sav)) != 0) {
                                state->m = NULL;
                                goto bad;
                        }
@@ -2790,9 +3058,13 @@ ipsec4_output(state, sp, flags)
        }
 
        KERNEL_DEBUG(DBG_FNC_IPSEC_OUT | DBG_FUNC_END, 0,0,0,0,0);
+       if (sav)
+               key_freesav(sav, KEY_SADB_UNLOCKED);
        return 0;
 
 bad:
+       if (sav)
+               key_freesav(sav, KEY_SADB_UNLOCKED);
        m_freem(state->m);
        state->m = NULL;
        KERNEL_DEBUG(DBG_FNC_IPSEC_OUT | DBG_FUNC_END, error,0,0,0,0);
@@ -2805,13 +3077,13 @@ bad:
  * IPsec output logic for IPv6, transport mode.
  */
 int
-ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
-       struct ipsec_output_state *state;
-       u_char *nexthdrp;
-       struct mbuf *mprev;
-       struct secpolicy *sp;
-       int flags;
-       int *tun;
+ipsec6_output_trans(
+       struct ipsec_output_state *state,
+       u_char *nexthdrp,
+       struct mbuf *mprev,
+       struct secpolicy *sp,
+       __unused int flags,
+       int *tun)
 {
        struct ip6_hdr *ip6;
        struct ipsecrequest *isr = NULL;
@@ -2819,7 +3091,10 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
        int error = 0;
        int plen;
        struct sockaddr_in6 *sin6;
+       struct secasvar *sav = NULL;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
        if (!state)
                panic("state == NULL in ipsec6_output_trans");
        if (!state->m)
@@ -2836,7 +3111,7 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
        KEYDEBUG(KEYDEBUG_IPSEC_DATA,
                printf("ipsec6_output_trans: applyed SP\n");
                kdebug_secpolicy(sp));
-
+       
        *tun = 0;
        for (isr = sp->req; isr; isr = isr->next) {
                if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
@@ -2876,7 +3151,7 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
                        }
                }
 
-               if (key_checkrequest(isr, &saidx) == ENOENT) {
+               if (key_checkrequest(isr, &saidx, &sav) == ENOENT) {
                        /*
                         * IPsec processing is required, but no SA found.
                         * I assume that key_acquire() had been called
@@ -2884,7 +3159,7 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
                         * this packet because it is responsibility for
                         * upper layer to retransmit the packet.
                         */
-                       ipsec6stat.out_nosa++;
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_nosa);
                        error = ENOENT;
 
                        /*
@@ -2902,7 +3177,7 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
                }
 
                /* validity check */
-               if (isr->sav == NULL) {
+               if (sav == NULL) {
                        switch (ipsec_get_reqlevel(isr)) {
                        case IPSEC_LEVEL_USE:
                                continue;
@@ -2916,9 +3191,9 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
                 * If there is no valid SA, we give up to process.
                 * see same place at ipsec4_output().
                 */
-               if (isr->sav->state != SADB_SASTATE_MATURE
-                && isr->sav->state != SADB_SASTATE_DYING) {
-                       ipsec6stat.out_nosa++;
+               if (sav->state != SADB_SASTATE_MATURE
+                && sav->state != SADB_SASTATE_DYING) {
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_nosa);
                        error = EINVAL;
                        goto bad;
                }
@@ -2926,23 +3201,23 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
                switch (isr->saidx.proto) {
                case IPPROTO_ESP:
 #if IPSEC_ESP
-                       error = esp6_output(state->m, nexthdrp, mprev->m_next, isr);
+                       error = esp6_output(state->m, nexthdrp, mprev->m_next, sav);
 #else
                        m_freem(state->m);
                        error = EINVAL;
 #endif
                        break;
                case IPPROTO_AH:
-                       error = ah6_output(state->m, nexthdrp, mprev->m_next, isr);
+                       error = ah6_output(state->m, nexthdrp, mprev->m_next, sav);
                        break;
                case IPPROTO_IPCOMP:
-                       error = ipcomp6_output(state->m, nexthdrp, mprev->m_next, isr);
+                       error = ipcomp6_output(state->m, nexthdrp, mprev->m_next, sav);
                        break;
                default:
                        ipseclog((LOG_ERR, "ipsec6_output_trans: "
                            "unknown ipsec protocol %d\n", isr->saidx.proto));
                        m_freem(state->m);
-                       ipsec6stat.out_inval++;
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_inval);
                        error = EINVAL;
                        break;
                }
@@ -2954,7 +3229,7 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
                if (plen > IPV6_MAXPACKET) {
                        ipseclog((LOG_ERR, "ipsec6_output_trans: "
                            "IPsec with IPv6 jumbogram is not supported\n"));
-                       ipsec6stat.out_inval++;
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_inval);
                        error = EINVAL; /*XXX*/
                        goto bad;
                }
@@ -2966,9 +3241,13 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
        if (isr != NULL)
                *tun = 1;
 
+       if (sav)
+               key_freesav(sav, KEY_SADB_UNLOCKED);
        return 0;
 
 bad:
+       if (sav)
+               key_freesav(sav, KEY_SADB_UNLOCKED);
        m_freem(state->m);
        state->m = NULL;
        return error;
@@ -2978,19 +3257,24 @@ bad:
  * IPsec output logic for IPv6, tunnel mode.
  */
 int
-ipsec6_output_tunnel(state, sp, flags)
-       struct ipsec_output_state *state;
-       struct secpolicy *sp;
-       int flags;
+ipsec6_output_tunnel(
+       struct ipsec_output_state *state,
+       struct secpolicy *sp,
+       __unused int flags,
+       int *tunneledv4)
 {
        struct ip6_hdr *ip6;
        struct ipsecrequest *isr = NULL;
        struct secasindex saidx;
+       struct secasvar *sav = NULL;
        int error = 0;
        int plen;
        struct sockaddr_in6* dst6;
-       int s;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
+       *tunneledv4 = 0;
+       
        if (!state)
                panic("state == NULL in ipsec6_output_tunnel");
        if (!state->m)
@@ -3053,7 +3337,7 @@ ipsec6_output_tunnel(state, sp, flags)
                        }
                }
 
-               if (key_checkrequest(isr, &saidx) == ENOENT) {
+               if (key_checkrequest(isr, &saidx, &sav) == ENOENT) {
                        /*
                         * IPsec processing is required, but no SA found.
                         * I assume that key_acquire() had been called
@@ -3061,13 +3345,13 @@ ipsec6_output_tunnel(state, sp, flags)
                         * this packet because it is responsibility for
                         * upper layer to retransmit the packet.
                         */
-                       ipsec6stat.out_nosa++;
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_nosa);
                        error = ENOENT;
                        goto bad;
                }
 
                /* validity check */
-               if (isr->sav == NULL) {
+               if (sav == NULL) {
                        switch (ipsec_get_reqlevel(isr)) {
                        case IPSEC_LEVEL_USE:
                                continue;
@@ -3081,55 +3365,184 @@ ipsec6_output_tunnel(state, sp, flags)
                 * If there is no valid SA, we give up to process.
                 * see same place at ipsec4_output().
                 */
-               if (isr->sav->state != SADB_SASTATE_MATURE
-                && isr->sav->state != SADB_SASTATE_DYING) {
-                       ipsec6stat.out_nosa++;
+               if (sav->state != SADB_SASTATE_MATURE
+                && sav->state != SADB_SASTATE_DYING) {
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_nosa);
                        error = EINVAL;
                        goto bad;
                }
 
-               /*
-                * There may be the case that SA status will be changed when
-                * we are refering to one. So calling splsoftnet().
-                */
-               s = splnet();
-
                if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
                        /*
                         * build IPsec tunnel.
                         */
-                       /* XXX should be processed with other familiy */
-                       if (((struct sockaddr *)&isr->sav->sah->saidx.src)->sa_family != AF_INET6) {
-                               ipseclog((LOG_ERR, "ipsec6_output_tunnel: "
-                                   "family mismatched between inner and outer, spi=%u\n",
-                                   (u_int32_t)ntohl(isr->sav->spi)));
-                               splx(s);
-                               ipsec6stat.out_inval++;
-                               error = EAFNOSUPPORT;
-                               goto bad;
-                       }
-
                        state->m = ipsec6_splithdr(state->m);
                        if (!state->m) {
-                               splx(s);
-                               ipsec6stat.out_nomem++;
+                               IPSEC_STAT_INCREMENT(ipsec6stat.out_nomem);
                                error = ENOMEM;
                                goto bad;
                        }
-                       error = ipsec6_encapsulate(state->m, isr->sav);
-                       splx(s);
-                       if (error) {
-                               state->m = 0;
+
+                       if (((struct sockaddr *)&sav->sah->saidx.src)->sa_family == AF_INET6) {                 
+                               error = ipsec6_encapsulate(state->m, sav);
+                               if (error) {
+                                       state->m = 0;
+                                       goto bad;
+                               }
+                               ip6 = mtod(state->m, struct ip6_hdr *);
+                       } else if (((struct sockaddr *)&sav->sah->saidx.src)->sa_family == AF_INET) {
+                       
+                               struct ip *ip;
+                               struct sockaddr_in* dst4;
+                               struct route *ro4 = NULL;
+                               struct route  ro4_copy;
+                               struct ip_out_args ipoa = { IFSCOPE_NONE, 0 };
+
+                               /*
+                                * must be last isr because encapsulated IPv6 packet
+                                * will be sent by calling ip_output
+                                */
+                               if (isr->next) {
+                                       ipseclog((LOG_ERR, "ipsec6_output_tunnel: "
+                                       "IPv4 must be outer layer, spi=%u\n",
+                                       (u_int32_t)ntohl(sav->spi)));
+                                       error = EINVAL;
+                                       goto bad;
+                               }
+                               *tunneledv4 = 1; /* must not process any further in ip6_output */
+                               error = ipsec64_encapsulate(state->m, sav);
+                               if (error) {
+                                       state->m = 0;
+                                       goto bad;
+                               }
+                               /* Now we have an IPv4 packet */
+                               ip = mtod(state->m, struct ip *);
+
+                               // grab sadb_mutex, to update sah's route cache and get a local copy of it
+                               lck_mtx_lock(sadb_mutex);
+                               ro4 = &sav->sah->sa_route;
+                               dst4 = (struct sockaddr_in *)&ro4->ro_dst;
+                               if (ro4->ro_rt) {
+                                       RT_LOCK(ro4->ro_rt);
+                               }
+                               if (ro4->ro_rt != NULL &&
+                                   (ro4->ro_rt->generation_id != route_generation ||
+                                   !(ro4->ro_rt->rt_flags & RTF_UP) ||
+                                   dst4->sin_addr.s_addr != ip->ip_dst.s_addr)) {
+                                       RT_UNLOCK(ro4->ro_rt);
+                                       rtfree(ro4->ro_rt);
+                                       ro4->ro_rt = NULL;
+                               }
+                               if (ro4->ro_rt == NULL) {
+                                       dst4->sin_family = AF_INET;
+                                       dst4->sin_len = sizeof(*dst4);
+                                       dst4->sin_addr = ip->ip_dst;
+                               } else {
+                                       RT_UNLOCK(ro4->ro_rt);
+                               }
+                               route_copyout(&ro4_copy, ro4, sizeof(ro4_copy));
+                               // release sadb_mutex, after updating sah's route cache and getting a local copy
+                               lck_mtx_unlock(sadb_mutex);
+                               state->m = ipsec4_splithdr(state->m);
+                               if (!state->m) {
+                                       error = ENOMEM;
+                                       if (ro4_copy.ro_rt != NULL) {
+                                               rtfree(ro4_copy.ro_rt);
+                                       }
+                                       goto bad;
+                               }
+                               switch (isr->saidx.proto) {
+                               case IPPROTO_ESP:
+#if IPSEC_ESP
+                                       if ((error = esp4_output(state->m, sav)) != 0) {
+                                               state->m = NULL;
+                                               if (ro4_copy.ro_rt != NULL) {
+                                                       rtfree(ro4_copy.ro_rt);
+                                               }
+                                               goto bad;
+                                       }
+                                       break;
+
+#else
+                                       m_freem(state->m);
+                                       state->m = NULL;
+                                       error = EINVAL;
+                                       if (ro4_copy.ro_rt != NULL) {
+                                               rtfree(ro4_copy.ro_rt);
+                                       }
+                                       goto bad;
+#endif
+                               case IPPROTO_AH:
+                                       if ((error = ah4_output(state->m, sav)) != 0) {
+                                               state->m = NULL;
+                                               if (ro4_copy.ro_rt != NULL) {
+                                                       rtfree(ro4_copy.ro_rt);
+                                               }
+                                               goto bad;
+                                       }
+                                       break;
+                               case IPPROTO_IPCOMP:
+                                       if ((error = ipcomp4_output(state->m, sav)) != 0) {
+                                               state->m = NULL;
+                                               if (ro4_copy.ro_rt != NULL) {
+                                                       rtfree(ro4_copy.ro_rt);
+                                               }
+                                               goto bad;
+                                       }
+                                       break;
+                               default:
+                                       ipseclog((LOG_ERR,
+                                               "ipsec4_output: unknown ipsec protocol %d\n",
+                                               isr->saidx.proto));
+                                       m_freem(state->m);
+                                       state->m = NULL;
+                                       error = EINVAL;
+                                       if (ro4_copy.ro_rt != NULL) {
+                                               rtfree(ro4_copy.ro_rt);
+                                       }
+                                       goto bad;
+                               }
+               
+                               if (state->m == 0) {
+                                       error = ENOMEM;
+                                       if (ro4_copy.ro_rt != NULL) {
+                                               rtfree(ro4_copy.ro_rt);
+                                       }
+                                       goto bad;
+                               }
+                               ip = mtod(state->m, struct ip *);
+                               ip->ip_len = ntohs(ip->ip_len);  /* flip len field before calling ip_output */
+                               error = ip_output(state->m, NULL, &ro4_copy, IP_OUTARGS, NULL, &ipoa);
+                               state->m = NULL;
+                               // grab sadb_mutex, to synchronize the sah's route cache with the local copy
+                               lck_mtx_lock(sadb_mutex);
+                               route_copyin(&ro4_copy, ro4, sizeof(ro4_copy));
+                               lck_mtx_unlock(sadb_mutex);
+                               if (error != 0)
+                                       goto bad;
+                               goto done;
+                       } else {
+                               ipseclog((LOG_ERR, "ipsec6_output_tunnel: "
+                                   "unsupported inner family, spi=%u\n",
+                                   (u_int32_t)ntohl(sav->spi)));
+                               IPSEC_STAT_INCREMENT(ipsec6stat.out_inval);
+                               error = EAFNOSUPPORT;
                                goto bad;
                        }
-                       ip6 = mtod(state->m, struct ip6_hdr *);
 
-                       state->ro = &isr->sav->sah->sa_route;
+                       // grab sadb_mutex, before updating sah's route cache
+                       lck_mtx_lock(sadb_mutex);
+                       state->ro = &sav->sah->sa_route;
                        state->dst = (struct sockaddr *)&state->ro->ro_dst;
                        dst6 = (struct sockaddr_in6 *)state->dst;
-                       if (state->ro->ro_rt
-                        && ((state->ro->ro_rt->rt_flags & RTF_UP) == 0
-                         || !IN6_ARE_ADDR_EQUAL(&dst6->sin6_addr, &ip6->ip6_dst))) {
+                       if (state->ro->ro_rt) {
+                               RT_LOCK(state->ro->ro_rt);
+                       }
+                       if (state->ro->ro_rt != NULL &&
+                           (state->ro->ro_rt->generation_id != route_generation ||
+                           !(state->ro->ro_rt->rt_flags & RTF_UP) ||
+                           !IN6_ARE_ADDR_EQUAL(&dst6->sin6_addr, &ip6->ip6_dst))) {
+                               RT_UNLOCK(state->ro->ro_rt);
                                rtfree(state->ro->ro_rt);
                                state->ro->ro_rt = NULL;
                        }
@@ -3139,25 +3552,40 @@ ipsec6_output_tunnel(state, sp, flags)
                                dst6->sin6_len = sizeof(*dst6);
                                dst6->sin6_addr = ip6->ip6_dst;
                                rtalloc(state->ro);
+                               if (state->ro->ro_rt) {
+                                       RT_LOCK(state->ro->ro_rt);
+                               }
                        }
                        if (state->ro->ro_rt == 0) {
                                ip6stat.ip6s_noroute++;
-                               ipsec6stat.out_noroute++;
+                               IPSEC_STAT_INCREMENT(ipsec6stat.out_noroute);
                                error = EHOSTUNREACH;
+                               // release sadb_mutex, after updating sah's route cache
+                               lck_mtx_unlock(sadb_mutex);
                                goto bad;
                        }
 
-                       /* adjust state->dst if tunnel endpoint is offlink */
+                       /*
+                        * adjust state->dst if tunnel endpoint is offlink
+                        *
+                        * XXX: caching rt_gateway value in the state is
+                        * not really good, since it may point elsewhere
+                        * when the gateway gets modified to a larger
+                        * sockaddr via rt_setgate().  This is currently
+                        * addressed by SA_SIZE roundup in that routine.
+                        */
                        if (state->ro->ro_rt->rt_flags & RTF_GATEWAY) {
                                state->dst = (struct sockaddr *)state->ro->ro_rt->rt_gateway;
                                dst6 = (struct sockaddr_in6 *)state->dst;
                        }
-               } else
-                       splx(s);
+                       RT_UNLOCK(state->ro->ro_rt);
+                       // release sadb_mutex, after updating sah's route cache
+                       lck_mtx_unlock(sadb_mutex);
+               }
 
                state->m = ipsec6_splithdr(state->m);
                if (!state->m) {
-                       ipsec6stat.out_nomem++;
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_nomem);
                        error = ENOMEM;
                        goto bad;
                }
@@ -3165,14 +3593,14 @@ ipsec6_output_tunnel(state, sp, flags)
                switch (isr->saidx.proto) {
                case IPPROTO_ESP:
 #if IPSEC_ESP
-                       error = esp6_output(state->m, &ip6->ip6_nxt, state->m->m_next, isr);
+                       error = esp6_output(state->m, &ip6->ip6_nxt, state->m->m_next, sav);
 #else
                        m_freem(state->m);
                        error = EINVAL;
 #endif
                        break;
                case IPPROTO_AH:
-                       error = ah6_output(state->m, &ip6->ip6_nxt, state->m->m_next, isr);
+                       error = ah6_output(state->m, &ip6->ip6_nxt, state->m->m_next, sav);
                        break;
                case IPPROTO_IPCOMP:
                        /* XXX code should be here */
@@ -3181,7 +3609,7 @@ ipsec6_output_tunnel(state, sp, flags)
                        ipseclog((LOG_ERR, "ipsec6_output_tunnel: "
                            "unknown ipsec protocol %d\n", isr->saidx.proto));
                        m_freem(state->m);
-                       ipsec6stat.out_inval++;
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_inval);
                        error = EINVAL;
                        break;
                }
@@ -3193,18 +3621,23 @@ ipsec6_output_tunnel(state, sp, flags)
                if (plen > IPV6_MAXPACKET) {
                        ipseclog((LOG_ERR, "ipsec6_output_tunnel: "
                            "IPsec with IPv6 jumbogram is not supported\n"));
-                       ipsec6stat.out_inval++;
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_inval);
                        error = EINVAL; /*XXX*/
                        goto bad;
                }
                ip6 = mtod(state->m, struct ip6_hdr *);
                ip6->ip6_plen = htons(plen);
        }
-
+done:
+       if (sav)
+               key_freesav(sav, KEY_SADB_UNLOCKED);
        return 0;
 
 bad:
-       m_freem(state->m);
+       if (sav)
+               key_freesav(sav, KEY_SADB_UNLOCKED);
+       if (state->m)
+               m_freem(state->m);
        state->m = NULL;
        return error;
 }
@@ -3231,7 +3664,7 @@ ipsec4_splithdr(m)
        hlen = ip->ip_hl << 2;
 #endif
        if (m->m_len > hlen) {
-               MGETHDR(mh, M_DONTWAIT, MT_HEADER);
+               MGETHDR(mh, M_DONTWAIT, MT_HEADER);     /* MAC-OK */
                if (!mh) {
                        m_freem(m);
                        return NULL;
@@ -3239,6 +3672,7 @@ ipsec4_splithdr(m)
                M_COPY_PKTHDR(mh, m);
                MH_ALIGN(mh, hlen);
                m->m_flags &= ~M_PKTHDR;
+               m_mchtype(m, MT_DATA);
                m->m_len -= hlen;
                m->m_data += hlen;
                mh->m_next = m;
@@ -3268,7 +3702,7 @@ ipsec6_splithdr(m)
        ip6 = mtod(m, struct ip6_hdr *);
        hlen = sizeof(struct ip6_hdr);
        if (m->m_len > hlen) {
-               MGETHDR(mh, M_DONTWAIT, MT_HEADER);
+               MGETHDR(mh, M_DONTWAIT, MT_HEADER);     /* MAC-OK */
                if (!mh) {
                        m_freem(m);
                        return NULL;
@@ -3276,6 +3710,7 @@ ipsec6_splithdr(m)
                M_COPY_PKTHDR(mh, m);
                MH_ALIGN(mh, hlen);
                m->m_flags &= ~M_PKTHDR;
+               m_mchtype(m, MT_DATA);
                m->m_len -= hlen;
                m->m_data += hlen;
                mh->m_next = m;
@@ -3293,24 +3728,28 @@ ipsec6_splithdr(m)
 
 /* validate inbound IPsec tunnel packet. */
 int
-ipsec4_tunnel_validate(m, off, nxt0, sav)
+ipsec4_tunnel_validate(m, off, nxt0, sav, ifamily)
        struct mbuf *m;         /* no pullup permitted, m->m_len >= ip */
        int off;
        u_int nxt0;
        struct secasvar *sav;
+       sa_family_t *ifamily;
 {
        u_int8_t nxt = nxt0 & 0xff;
        struct sockaddr_in *sin;
-       struct sockaddr_in osrc, odst, isrc, idst;
+       struct sockaddr_in osrc, odst, i4src, i4dst;
+       struct sockaddr_in6 i6src, i6dst;
        int hlen;
        struct secpolicy *sp;
        struct ip *oip;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+
 #if DIAGNOSTIC
        if (m->m_len < sizeof(struct ip))
                panic("too short mbuf on ipsec4_tunnel_validate");
 #endif
-       if (nxt != IPPROTO_IPV4)
+       if (nxt != IPPROTO_IPV4 && nxt != IPPROTO_IPV6)
                return 0;
        if (m->m_pkthdr.len < off + sizeof(struct ip))
                return 0;
@@ -3327,7 +3766,6 @@ ipsec4_tunnel_validate(m, off, nxt0, sav)
        if (hlen != sizeof(struct ip))
                return 0;
 
-       /* AF_INET6 should be supported, but at this moment we don't. */
        sin = (struct sockaddr_in *)&sav->sah->saidx.dst;
        if (sin->sin_family != AF_INET)
                return 0;
@@ -3337,19 +3775,10 @@ ipsec4_tunnel_validate(m, off, nxt0, sav)
        /* XXX slow */
        bzero(&osrc, sizeof(osrc));
        bzero(&odst, sizeof(odst));
-       bzero(&isrc, sizeof(isrc));
-       bzero(&idst, sizeof(idst));
-       osrc.sin_family = odst.sin_family = isrc.sin_family = idst.sin_family = 
-           AF_INET;
-       osrc.sin_len = odst.sin_len = isrc.sin_len = idst.sin_len = 
-           sizeof(struct sockaddr_in);
+       osrc.sin_family = odst.sin_family = AF_INET;
+       osrc.sin_len = odst.sin_len = sizeof(struct sockaddr_in);
        osrc.sin_addr = oip->ip_src;
        odst.sin_addr = oip->ip_dst;
-       m_copydata(m, off + offsetof(struct ip, ip_src), sizeof(isrc.sin_addr),
-           (caddr_t)&isrc.sin_addr);
-       m_copydata(m, off + offsetof(struct ip, ip_dst), sizeof(idst.sin_addr),
-           (caddr_t)&idst.sin_addr);
-
        /*
         * RFC2401 5.2.1 (b): (assume that we are using tunnel mode)
         * - if the inner destination is multicast address, there can be
@@ -3370,12 +3799,35 @@ ipsec4_tunnel_validate(m, off, nxt0, sav)
         *
         * therefore, we do not do anything special about inner source.
         */
-
-       sp = key_gettunnel((struct sockaddr *)&osrc, (struct sockaddr *)&odst,
-           (struct sockaddr *)&isrc, (struct sockaddr *)&idst);
-       if (!sp)
+       if (nxt == IPPROTO_IPV4) {
+               bzero(&i4src, sizeof(struct sockaddr_in));
+               bzero(&i4dst, sizeof(struct sockaddr_in));
+               i4src.sin_family = i4dst.sin_family = *ifamily = AF_INET;
+               i4src.sin_len = i4dst.sin_len = sizeof(struct sockaddr_in);
+               m_copydata(m, off + offsetof(struct ip, ip_src), sizeof(i4src.sin_addr),
+                          (caddr_t)&i4src.sin_addr);
+               m_copydata(m, off + offsetof(struct ip, ip_dst), sizeof(i4dst.sin_addr),
+                          (caddr_t)&i4dst.sin_addr);
+               sp = key_gettunnel((struct sockaddr *)&osrc, (struct sockaddr *)&odst,
+                                  (struct sockaddr *)&i4src, (struct sockaddr *)&i4dst);
+       } else if (nxt == IPPROTO_IPV6) {
+               bzero(&i6src, sizeof(struct sockaddr_in6));
+               bzero(&i6dst, sizeof(struct sockaddr_in6));
+               i6src.sin6_family = i6dst.sin6_family = *ifamily = AF_INET6;
+               i6src.sin6_len = i6dst.sin6_len = sizeof(struct sockaddr_in6);
+               m_copydata(m, off + offsetof(struct ip6_hdr, ip6_src), sizeof(i6src.sin6_addr),
+                          (caddr_t)&i6src.sin6_addr);
+               m_copydata(m, off + offsetof(struct ip6_hdr, ip6_dst), sizeof(i6dst.sin6_addr),
+                          (caddr_t)&i6dst.sin6_addr);
+               sp = key_gettunnel((struct sockaddr *)&osrc, (struct sockaddr *)&odst,
+                                  (struct sockaddr *)&i6src, (struct sockaddr *)&i6dst);
+       } else 
+               return 0;       /* unsupported family */
+
+       if (!sp) 
                return 0;
-       key_freesp(sp);
+
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return 1;
 }
@@ -3395,6 +3847,8 @@ ipsec6_tunnel_validate(m, off, nxt0, sav)
        struct secpolicy *sp;
        struct ip6_hdr *oip6;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
 #if DIAGNOSTIC
        if (m->m_len < sizeof(struct ip6_hdr))
                panic("too short mbuf on ipsec6_tunnel_validate");
@@ -3448,7 +3902,7 @@ ipsec6_tunnel_validate(m, off, nxt0, sav)
         */
        if (!sp)
                return 0;
-       key_freesp(sp);
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return 1;
 }
@@ -3483,19 +3937,10 @@ ipsec_copypkt(m)
                                struct mbuf *mm;
 
                                if (n->m_flags & M_PKTHDR) {
-                                       MGETHDR(mnew, M_DONTWAIT, MT_HEADER);
+                                       MGETHDR(mnew, M_DONTWAIT, MT_HEADER); /* MAC-OK */
                                        if (mnew == NULL)
                                                goto fail;
-                                       mnew->m_pkthdr = n->m_pkthdr;
-#if 0
-                                       if (n->m_pkthdr.aux) {
-                                               mnew->m_pkthdr.aux =
-                                                   m_copym(n->m_pkthdr.aux,
-                                                   0, M_COPYALL, M_DONTWAIT);
-                                       }
-#endif
                                        M_COPY_PKTHDR(mnew, n);
-                                       mnew->m_flags = n->m_flags & M_COPYFLAGS;
                                }
                                else {
                                        MGET(mnew, M_DONTWAIT, MT_DATA);
@@ -3542,7 +3987,7 @@ ipsec_copypkt(m)
                                                break;
 
                                        /* need another mbuf */
-                                       MGETHDR(mn, M_DONTWAIT, MT_HEADER);
+                                       MGETHDR(mn, M_DONTWAIT, MT_HEADER);     /* XXXMAC: tags copied next time in loop? */
                                        if (mn == NULL)
                                                goto fail;
                                        mn->m_pkthdr.rcvif = NULL;
@@ -3569,162 +4014,194 @@ ipsec_copypkt(m)
        return(NULL);
 }
 
-static struct mbuf *
-ipsec_addaux(m)
-       struct mbuf *m;
+/*
+ * Tags are allocated as mbufs for now, since our minimum size is MLEN, we
+ * should make use of up to that much space.
+ */
+#define        IPSEC_TAG_HEADER \
+
+struct ipsec_tag {
+       struct socket                   *socket;
+       u_int32_t                               history_count;
+       struct ipsec_history    history[];
+};
+
+#define        IPSEC_TAG_SIZE          (MLEN - sizeof(struct m_tag))
+#define        IPSEC_TAG_HDR_SIZE      (offsetof(struct ipsec_tag, history[0]))
+#define IPSEC_HISTORY_MAX      ((IPSEC_TAG_SIZE - IPSEC_TAG_HDR_SIZE) / \
+                                                        sizeof(struct ipsec_history))
+
+static struct ipsec_tag *
+ipsec_addaux(
+       struct mbuf *m)
 {
-       struct mbuf *n;
-
-       n = m_aux_find(m, AF_INET, IPPROTO_ESP);
-       if (!n)
-               n = m_aux_add(m, AF_INET, IPPROTO_ESP);
-       if (!n)
-               return n;       /* ENOBUFS */
-       n->m_len = sizeof(struct socket *);
-       bzero(mtod(n, void *), n->m_len);
-       return n;
+       struct m_tag            *tag;
+       
+       /* Check if the tag already exists */
+       tag = m_tag_locate(m, KERNEL_MODULE_TAG_ID, KERNEL_TAG_TYPE_IPSEC, NULL);
+       
+       if (tag == NULL) {
+               struct ipsec_tag        *itag;
+               
+               /* Allocate a tag */
+               tag = m_tag_create(KERNEL_MODULE_TAG_ID, KERNEL_TAG_TYPE_IPSEC,
+                                                 IPSEC_TAG_SIZE, M_DONTWAIT, m);
+               
+               if (tag) {
+                       itag = (struct ipsec_tag*)(tag + 1);
+                       itag->socket = 0;
+                       itag->history_count = 0;
+                       
+                       m_tag_prepend(m, tag);
+               }
+       }
+       
+       return tag ? (struct ipsec_tag*)(tag + 1) : NULL;
 }
 
-static struct mbuf *
-ipsec_findaux(m)
-       struct mbuf *m;
+static struct ipsec_tag *
+ipsec_findaux(
+       struct mbuf *m)
 {
-       struct mbuf *n;
-
-       n = m_aux_find(m, AF_INET, IPPROTO_ESP);
-#if DIAGNOSTIC
-       if (n && n->m_len < sizeof(struct socket *))
-               panic("invalid ipsec m_aux");
-#endif
-       return n;
+       struct m_tag    *tag;
+       
+       tag = m_tag_locate(m, KERNEL_MODULE_TAG_ID, KERNEL_TAG_TYPE_IPSEC, NULL);
+       
+       return tag ? (struct ipsec_tag*)(tag + 1) : NULL;
 }
 
 void
-ipsec_delaux(m)
-       struct mbuf *m;
+ipsec_delaux(
+       struct mbuf *m)
 {
-       struct mbuf *n;
-
-       n = m_aux_find(m, AF_INET, IPPROTO_ESP);
-       if (n)
-               m_aux_delete(m, n);
+       struct m_tag    *tag;
+       
+       tag = m_tag_locate(m, KERNEL_MODULE_TAG_ID, KERNEL_TAG_TYPE_IPSEC, NULL);
+       
+       if (tag) {
+               m_tag_delete(m, tag);
+       }
 }
 
 /* if the aux buffer is unnecessary, nuke it. */
 static void
-ipsec_optaux(m, n)
-       struct mbuf *m;
-       struct mbuf *n;
+ipsec_optaux(
+       struct mbuf                     *m,
+       struct ipsec_tag        *itag)
 {
-
-       if (!n)
-               return;
-       if (n->m_len == sizeof(struct socket *) && !*mtod(n, struct socket **))
-               ipsec_delaux(m);
+       if (itag && itag->socket == NULL && itag->history_count == 0) {
+               m_tag_delete(m, ((struct m_tag*)itag) - 1);
+       }
 }
 
 int
-ipsec_setsocket(m, so)
-       struct mbuf *m;
-       struct socket *so;
+ipsec_setsocket(
+       struct mbuf *m,
+       struct socket *so)
 {
-       struct mbuf *n;
+       struct ipsec_tag        *tag;
 
        /* if so == NULL, don't insist on getting the aux mbuf */
        if (so) {
-               n = ipsec_addaux(m);
-               if (!n)
+               tag = ipsec_addaux(m);
+               if (!tag)
                        return ENOBUFS;
        } else
-               n = ipsec_findaux(m);
-       if (n && n->m_len >= sizeof(struct socket *))
-               *mtod(n, struct socket **) = so;
-       ipsec_optaux(m, n);
+               tag = ipsec_findaux(m);
+       if (tag) {
+               tag->socket = so;
+               ipsec_optaux(m, tag);
+       }
        return 0;
 }
 
 struct socket *
-ipsec_getsocket(m)
-       struct mbuf *m;
+ipsec_getsocket(
+       struct mbuf *m)
 {
-       struct mbuf *n;
-
-       n = ipsec_findaux(m);
-       if (n && n->m_len >= sizeof(struct socket *))
-               return *mtod(n, struct socket **);
+       struct ipsec_tag        *itag;
+       
+       itag = ipsec_findaux(m);
+       if (itag)
+               return itag->socket;
        else
                return NULL;
 }
 
 int
-ipsec_addhist(m, proto, spi)
-       struct mbuf *m;
-       int proto;
-       u_int32_t spi;
+ipsec_addhist(
+       struct mbuf *m,
+       int proto,
+       u_int32_t spi)
 {
-       struct mbuf *n;
-       struct ipsec_history *p;
-
-       n = ipsec_addaux(m);
-       if (!n)
+       struct ipsec_tag                *itag;
+       struct ipsec_history    *p;
+       itag = ipsec_addaux(m);
+       if (!itag)
                return ENOBUFS;
-       if (M_TRAILINGSPACE(n) < sizeof(*p))
+       if (itag->history_count == IPSEC_HISTORY_MAX)
                return ENOSPC;  /* XXX */
-       p = (struct ipsec_history *)(mtod(n, caddr_t) + n->m_len);
-       n->m_len += sizeof(*p);
+       
+       p = &itag->history[itag->history_count];
+       itag->history_count++;
+       
        bzero(p, sizeof(*p));
        p->ih_proto = proto;
        p->ih_spi = spi;
+       
        return 0;
 }
 
 struct ipsec_history *
-ipsec_gethist(m, lenp)
-       struct mbuf *m;
-       int *lenp;
+ipsec_gethist(
+       struct mbuf *m,
+       int *lenp)
 {
-       struct mbuf *n;
-       int l;
-
-       n = ipsec_findaux(m);
-       if (!n)
-               return NULL;
-       l = n->m_len;
-       if (sizeof(struct socket *) > l)
+       struct ipsec_tag        *itag;
+       
+       itag = ipsec_findaux(m);
+       if (!itag)
                return NULL;
-       if ((l - sizeof(struct socket *)) % sizeof(struct ipsec_history))
+       if (itag->history_count == 0)
                return NULL;
-       /* XXX does it make more sense to divide by sizeof(ipsec_history)? */
        if (lenp)
-               *lenp = l - sizeof(struct socket *);
-       return (struct ipsec_history *)
-           (mtod(n, caddr_t) + sizeof(struct socket *));
+               *lenp = (int)(itag->history_count * sizeof(struct ipsec_history));
+       return itag->history;
 }
 
 void
-ipsec_clearhist(m)
-       struct mbuf *m;
+ipsec_clearhist(
+       struct mbuf *m)
 {
-       struct mbuf *n;
-
-       n = ipsec_findaux(m);
-       if ((n) && n->m_len > sizeof(struct socket *))
-               n->m_len = sizeof(struct socket *);
-       ipsec_optaux(m, n);
+       struct ipsec_tag        *itag;
+       
+       itag = ipsec_findaux(m);
+       if (itag) {
+               itag->history_count = 0;
+       }
+       ipsec_optaux(m, itag);
 }
 
-__private_extern__ void
+__private_extern__ int
 ipsec_send_natt_keepalive(
        struct secasvar *sav)
 {
        struct mbuf     *m;
        struct udphdr *uh;
        struct ip *ip;
+       int error;
+       struct ip_out_args ipoa = { IFSCOPE_NONE, 0 };
+       struct route ro;
+
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
        
-       if ((esp_udp_encap_port & 0xFFFF) == 0 || sav->remote_ike_port == 0) return;
-       
+       if ((esp_udp_encap_port & 0xFFFF) == 0 || sav->remote_ike_port == 0) return FALSE;
+
+       // natt timestamp may have changed... reverify
+       if ((natt_now - sav->natt_last_activity) < natt_keepalive_interval) return FALSE;
+
        m = m_gethdr(M_NOWAIT, MT_DATA);
-       if (m == NULL) return;
+       if (m == NULL) return FALSE;
        
        /*
         * Create a UDP packet complete with IP header.
@@ -3737,17 +4214,43 @@ ipsec_send_natt_keepalive(
        uh = (struct udphdr*)((char*)m_mtod(m) + sizeof(struct ip));
        m->m_len = sizeof(struct udpiphdr) + 1;
        bzero(m_mtod(m), m->m_len);
-       ip->ip_len = ntohs(m->m_len);
+       m->m_pkthdr.len = m->m_len;
+
+       ip->ip_len = m->m_len;
        ip->ip_ttl = ip_defttl;
        ip->ip_p = IPPROTO_UDP;
-       ip->ip_src = ((struct sockaddr_in*)&sav->sah->saidx.src)->sin_addr;
-       ip->ip_dst = ((struct sockaddr_in*)&sav->sah->saidx.dst)->sin_addr;
-       uh->uh_sport = ntohs((u_short)esp_udp_encap_port);
-       uh->uh_dport = ntohs(sav->remote_ike_port);
+       if (sav->sah->dir != IPSEC_DIR_INBOUND) {
+               ip->ip_src = ((struct sockaddr_in*)&sav->sah->saidx.src)->sin_addr;
+               ip->ip_dst = ((struct sockaddr_in*)&sav->sah->saidx.dst)->sin_addr;
+       } else {
+               ip->ip_src = ((struct sockaddr_in*)&sav->sah->saidx.dst)->sin_addr;
+               ip->ip_dst = ((struct sockaddr_in*)&sav->sah->saidx.src)->sin_addr;
+       }
+       uh->uh_sport = htons((u_short)esp_udp_encap_port);
+       uh->uh_dport = htons(sav->remote_ike_port);
        uh->uh_ulen = htons(1 + sizeof(struct udphdr));
        uh->uh_sum = 0;
        *(u_int8_t*)((char*)m_mtod(m) + sizeof(struct ip) + sizeof(struct udphdr)) = 0xFF;
-       
-       if (ip_output(m, NULL, &sav->sah->sa_route, IP_NOIPSEC, NULL) == 0)
+
+       // grab sadb_mutex, to get a local copy of sah's route cache
+       lck_mtx_lock(sadb_mutex);
+       if (sav->sah->sa_route.ro_rt != NULL &&
+           rt_key(sav->sah->sa_route.ro_rt)->sa_family != AF_INET) {
+               rtfree(sav->sah->sa_route.ro_rt);
+               sav->sah->sa_route.ro_rt = NULL;
+       }
+       route_copyout(&ro, &sav->sah->sa_route, sizeof(ro));
+       lck_mtx_unlock(sadb_mutex);
+
+       error = ip_output(m, NULL, &ro, IP_OUTARGS | IP_NOIPSEC, NULL, &ipoa);
+
+       // grab sadb_mutex, to synchronize the sah's route cache with the local copy
+       lck_mtx_lock(sadb_mutex);
+       route_copyin(&ro, &sav->sah->sa_route, sizeof(ro));
+       lck_mtx_unlock(sadb_mutex);
+       if (error == 0) {
                sav->natt_last_activity = natt_now;
+               return TRUE;
+       }
+       return FALSE;
 }