]> git.saurik.com Git - apple/xnu.git/blobdiff - bsd/netinet6/ipsec.c
xnu-1228.15.4.tar.gz
[apple/xnu.git] / bsd / netinet6 / ipsec.c
index c494fb4dabc69ce9a5b0040d42245a8c79ffe604..97dd1761eb38753794ef2a9831fa53021e31a5a8 100644 (file)
@@ -1,4 +1,5 @@
-/*     $KAME: ipsec.c,v 1.56 2000/04/04 08:47:34 itojun Exp $  */
+/*     $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 $ */
 
 /*
  * Copyright (C) 1995, 1996, 1997, and 1998 WIDE Project.
 /*
  * IPsec controller part.
  */
-#define _IP_VHL
-
-#if (defined(__FreeBSD__) && __FreeBSD__ >= 3) || defined(__NetBSD__)
-#include "opt_inet.h"
-#if __NetBSD__ /*XXX*/
-#include "opt_ipsec.h"
-#endif
-#endif
 
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/time.h>
 #include <sys/kernel.h>
 #include <sys/syslog.h>
-#ifdef __NetBSD__
-#include <vm/vm.h>
-#endif
-#if defined(__NetBSD__) || defined(__FreeBSD__) || defined (__APPLE__)
 #include <sys/sysctl.h>
-#endif
+#include <kern/locks.h>
+#include <sys/kauth.h>
+#include <libkern/OSAtomic.h>
 
 #include <net/if.h>
 #include <net/route.h>
 #include <netinet/udp.h>
 #include <netinet/udp_var.h>
 #include <netinet/ip_ecn.h>
-
 #if INET6
+#include <netinet6/ip6_ecn.h>
+#endif
+#include <netinet/tcp.h>
+#include <netinet/udp.h>
+
 #include <netinet/ip6.h>
+#if INET6
 #include <netinet6/ip6_var.h>
 #endif
 #include <netinet/in_pcb.h>
 #if INET6
-#if !((defined(__FreeBSD__) && __FreeBSD__ >= 3) || defined(__OpenBSD__) || (defined(__bsdi__) && _BSDI_VERSION >= 199802)) || defined (__APPLE__)
-#include <netinet6/in6_pcb.h>
-#endif
 #include <netinet/icmp6.h>
 #endif
 
 #include <netinet6/ipsec.h>
+#if INET6
+#include <netinet6/ipsec6.h>
+#endif
 #include <netinet6/ah.h>
+#if INET6
+#include <netinet6/ah6.h>
+#endif
 #if IPSEC_ESP
 #include <netinet6/esp.h>
+#if INET6
+#include <netinet6/esp6.h>
+#endif
 #endif
 #include <netinet6/ipcomp.h>
+#if INET6
+#include <netinet6/ipcomp6.h>
+#endif
 #include <netkey/key.h>
 #include <netkey/keydb.h>
 #include <netkey/key_debug.h>
 
 #include <net/net_osdep.h>
 
-#ifdef HAVE_NRL_INPCB
-#define in6pcb inpcb
-#define in6p_sp        inp_sp
-#define in6p_fport     inp_fport
-#define in6p_lport     inp_lport
-#define in6p_socket    inp_socket
-#define sotoin6pcb(so) ((struct inpcb *)(so)->so_pcb)
-#endif
-
-#ifdef __NetBSD__
-#define ovbcopy        bcopy
-#endif
-
-#ifdef IPSEC_DEBUG
+#if IPSEC_DEBUG
 int ipsec_debug = 1;
 #else
 int ipsec_debug = 0;
 #endif
 
+#include <sys/kdebug.h>
+#define DBG_LAYER_BEG                  NETDBG_CODE(DBG_NETIPSEC, 1)
+#define DBG_LAYER_END                  NETDBG_CODE(DBG_NETIPSEC, 3)
+#define DBG_FNC_GETPOL_SOCK            NETDBG_CODE(DBG_NETIPSEC, (1 << 8))
+#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;
+extern lck_mtx_t *ip6_mutex;
+
 struct ipsecstat ipsecstat;
-int ip4_inbound_call_ike = 0;
 int ip4_ah_cleartos = 1;
 int ip4_ah_offsetmask = 0;     /* maybe IP_DF? */
 int ip4_ipsec_dfbit = 0;       /* DF bit on encap. 0: clear 1: set 2: copy */
@@ -126,14 +127,22 @@ int ip4_ah_trans_deflev = IPSEC_LEVEL_USE;
 int ip4_ah_net_deflev = IPSEC_LEVEL_USE;
 struct secpolicy ip4_def_policy;
 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 u_int32_t natt_now;
+
+struct ipsec_tag;
 
-#if defined(__FreeBSD__) || defined(__APPLE__)
 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_INT(_net_inet_ipsec, IPSECCTL_DEF_POLICY,
-       def_policy, CTLFLAG_RW, &ip4_def_policy.policy, 0, "");
+SYSCTL_PROC(_net_inet_ipsec, IPSECCTL_DEF_POLICY, def_policy, CTLTYPE_INT|CTLFLAG_RW,
+       &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, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_ESP_NETLEV, esp_net_deflev,
@@ -142,8 +151,6 @@ SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
        CTLFLAG_RW, &ip4_ah_trans_deflev,       0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
        CTLFLAG_RW, &ip4_ah_net_deflev, 0, "");
-SYSCTL_INT(_net_inet_ipsec, IPSECCTL_INBOUND_CALL_IKE,
-       inbound_call_ike, CTLFLAG_RW,   &ip4_inbound_call_ike,  0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_AH_CLEARTOS,
        ah_cleartos, CTLFLAG_RW,        &ip4_ah_cleartos,       0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_AH_OFFSETMASK,
@@ -154,20 +161,32 @@ SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ECN,
        ecn, CTLFLAG_RW,        &ip4_ipsec_ecn, 0, "");
 SYSCTL_INT(_net_inet_ipsec, IPSECCTL_DEBUG,
        debug, CTLFLAG_RW,      &ipsec_debug,   0, "");
-#endif /* __FreeBSD__ */
+SYSCTL_INT(_net_inet_ipsec, IPSECCTL_ESP_RANDPAD,
+       esp_randpad, CTLFLAG_RW,        &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, "");
+
+/*
+ * NAT Traversal requires a UDP port for encapsulation,
+ * esp_udp_encap_port controls which port is used. Racoon
+ * must set this port to the port racoon is using locally
+ * for nat traversal.
+ */
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO, esp_port,
+                  CTLFLAG_RW, &esp_udp_encap_port, 0, "");
 
 #if INET6
 struct ipsecstat ipsec6stat;
-int ip6_inbound_call_ike = 0;
 int ip6_esp_trans_deflev = IPSEC_LEVEL_USE;
 int ip6_esp_net_deflev = IPSEC_LEVEL_USE;
 int ip6_ah_trans_deflev = IPSEC_LEVEL_USE;
 int ip6_ah_net_deflev = IPSEC_LEVEL_USE;
 struct secpolicy ip6_def_policy;
 int ip6_ipsec_ecn = 0;         /* ECN ignore(-1)/forbidden(0)/allowed(1) */
+int ip6_esp_randpad = -1;
 
-#if defined(__FreeBSD__) || defined(__APPLE__)
-SYSCTL_DECL(_net_inet6_ipsec6);
 /* net.inet6.ipsec6 */
 SYSCTL_STRUCT(_net_inet6_ipsec6, IPSECCTL_STATS,
        stats, CTLFLAG_RD, &ipsec6stat, ipsecstat, "");
@@ -181,41 +200,71 @@ SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_TRANSLEV, ah_trans_deflev,
        CTLFLAG_RW, &ip6_ah_trans_deflev,       0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEF_AH_NETLEV, ah_net_deflev,
        CTLFLAG_RW, &ip6_ah_net_deflev, 0, "");
-SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_INBOUND_CALL_IKE,
-       inbound_call_ike, CTLFLAG_RW,   &ip6_inbound_call_ike,  0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ECN,
        ecn, CTLFLAG_RW,        &ip6_ipsec_ecn, 0, "");
 SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_DEBUG,
        debug, CTLFLAG_RW,      &ipsec_debug,   0, "");
-#endif /*__FreeBSD__*/
+SYSCTL_INT(_net_inet6_ipsec6, IPSECCTL_ESP_RANDPAD,
+       esp_randpad, CTLFLAG_RW,        &ip6_esp_randpad,       0, "");
 #endif /* INET6 */
 
-static int ipsec_setspidx_mbuf
-       __P((struct secpolicyindex *, u_int, u_int, struct mbuf *));
-static void ipsec4_setspidx_inpcb __P((struct mbuf *, struct inpcb *pcb));
-static void ipsec4_setspidx_ipaddr __P((struct mbuf *, struct secpolicyindex *));
+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(struct mbuf *, struct in6pcb *pcb);
+#endif
+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 *));
-static void ipsec6_setspidx_in6pcb __P((struct mbuf *, struct in6pcb *pcb));
-static void 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(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(struct mbuf *);
 #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 mbuf *ipsec4_splithdr __P((struct mbuf *));
 #if INET6
-static struct mbuf *ipsec6_splithdr __P((struct mbuf *));
+static struct mbuf *ipsec6_splithdr(struct mbuf *);
+#endif
+#if INET
+static int ipsec4_encapsulate(struct mbuf *, struct secasvar *);
 #endif
-static int ipsec4_encapsulate __P((struct mbuf *, struct secasvar *));
 #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 ipsec_tag *ipsec_addaux(struct mbuf *);
+static struct ipsec_tag *ipsec_findaux(struct mbuf *);
+static void ipsec_optaux(struct mbuf *, struct ipsec_tag *);
+void ipsec_send_natt_keepalive(struct secasvar *sav);
+
+static int
+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);
+
+       if (ip4_def_policy.policy != IPSEC_POLICY_NONE &&
+               ip4_def_policy.policy != IPSEC_POLICY_DISCARD) {
+               ip4_def_policy.policy = old_policy;
+               return EINVAL;
+       }
+
+       /* Turn off the bypass if the default security policy changes */
+       if (ipsec_bypass != 0 && ip4_def_policy.policy != IPSEC_POLICY_NONE)
+               ipsec_bypass = 0;
+
+       return error;
+}
 
 /*
  * For OUTBOUND packet having a socket. Searching SPD for packet,
@@ -224,7 +273,7 @@ static int ipsec6_encapsulate __P((struct mbuf *, struct secasvar *));
  *             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.
@@ -240,40 +289,57 @@ 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;
+               break;
+#if INET6
+       case AF_INET6:
+               pcbsp = sotoin6pcb(so)->in6p_sp;
+               break;
+#endif
+       }
+       
+       if (!pcbsp){
+               /* Socket has not specified an IPSEC policy */
+               return ipsec4_getpolicybyaddr(m, dir, 0, error);
+       }
+
+       KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_START, 0,0,0,0,0);
 
        switch (so->so_proto->pr_domain->dom_family) {
        case AF_INET:
                /* set spidx in pcb */
-               ipsec4_setspidx_inpcb(m, sotoinpcb(so));
-               pcbsp = sotoinpcb(so)->inp_sp;
+               *error = ipsec4_setspidx_inpcb(m, sotoinpcb(so));
                break;
 #if INET6
        case AF_INET6:
                /* set spidx in pcb */
-               ipsec6_setspidx_in6pcb(m, sotoin6pcb(so));
-               pcbsp = sotoin6pcb(so)->in6p_sp;
+               *error = ipsec6_setspidx_in6pcb(m, sotoin6pcb(so));
                break;
 #endif
        default:
                panic("ipsec4_getpolicybysock: unsupported address family\n");
        }
+       if (*error) {
+               KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 1,*error,0,0,0);
+               return NULL;
+       }
 
        /* sanity check */
        if (pcbsp == NULL)
                panic("ipsec4_getpolicybysock: pcbsp is NULL.\n");
 
-#if defined(__FreeBSD__) && __FreeBSD__ >= 3 || defined(__APPLE__)
-       KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-               printf("send: priv=%d ", pcbsp->priv);
-               if (so->so_cred) {
-                       printf("p_ruid=%d ", so->so_cred->p_ruid);
-                       printf("p_svuid=%d ", so->so_cred->p_svuid);
-                       printf("cr_uid=%d\n", so->so_cred->pc_ucred->cr_uid);
-               });
-#endif
        switch (dir) {
        case IPSEC_DIR_INBOUND:
                currsp = pcbsp->sp_in;
@@ -293,8 +359,11 @@ 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;
 
                case IPSEC_POLICY_ENTRUST:
@@ -307,10 +376,12 @@ ipsec4_getpolicybysock(m, dir, so, error)
                                        printf("DP ipsec4_getpolicybysock called "
                                               "to allocate SP:%p\n", kernsp));
                                *error = 0;
+                               KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 3,*error,0,0,0);
                                return kernsp;
                        }
 
                        /* 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,
@@ -319,18 +390,24 @@ 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;
 
                default:
                        ipseclog((LOG_ERR, "ipsec4_getpolicybysock: "
                              "Invalid policy for PCB %d\n", currsp->policy));
                        *error = EINVAL;
+                       KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 6,*error,0,0,0);
                        return NULL;
                }
                /* NOTREACHED */
@@ -346,6 +423,7 @@ ipsec4_getpolicybysock(m, dir, so, error)
                        printf("DP ipsec4_getpolicybysock called "
                               "to allocate SP:%p\n", kernsp));
                *error = 0;
+               KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 7,*error,0,0,0);
                return kernsp;
        }
 
@@ -356,9 +434,11 @@ ipsec4_getpolicybysock(m, dir, so, error)
                       "Illegal policy for non-priviliged defined %d\n",
                        currsp->policy));
                *error = EINVAL;
+               KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 8,*error,0,0,0);
                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,
@@ -367,18 +447,24 @@ 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;
 
        default:
                ipseclog((LOG_ERR, "ipsec4_getpolicybysock: "
                   "Invalid policy for PCB %d\n", currsp->policy));
                *error = EINVAL;
+               KERNEL_DEBUG(DBG_FNC_GETPOL_SOCK | DBG_FUNC_END, 11,*error,0,0,0);
                return NULL;
        }
        /* NOTREACHED */
@@ -392,7 +478,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)
@@ -403,6 +489,11 @@ ipsec4_getpolicybyaddr(m, dir, flag, error)
 {
        struct secpolicy *sp = NULL;
 
+       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");
@@ -410,13 +501,17 @@ ipsec4_getpolicybyaddr(m, dir, flag, error)
     {
        struct secpolicyindex spidx;
 
+       KERNEL_DEBUG(DBG_FNC_GETPOL_ADDR | DBG_FUNC_START, 0,0,0,0,0);
        bzero(&spidx, sizeof(spidx));
 
        /* make a index to look for a policy */
-       *error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET, m);
+       *error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET, m,
+           (flag & IP_FORWARDING) ? 0 : 1);
 
-       if (*error != 0)
+       if (*error != 0) {
+               KERNEL_DEBUG(DBG_FNC_GETPOL_ADDR | DBG_FUNC_END, 1,*error,0,0,0);
                return NULL;
+       }
 
        sp = key_allocsp(&spidx, dir);
     }
@@ -427,10 +522,12 @@ ipsec4_getpolicybyaddr(m, dir, flag, error)
                        printf("DP ipsec4_getpolicybyaddr called "
                               "to allocate SP:%p\n", sp));
                *error = 0;
+               KERNEL_DEBUG(DBG_FNC_GETPOL_ADDR | DBG_FUNC_END, 2,*error,0,0,0);
                return sp;
        }
 
        /* 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",
@@ -439,7 +536,9 @@ 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;
 }
 
@@ -451,7 +550,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 *
@@ -465,14 +564,25 @@ 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");
 
-       /* set spidx in pcb */
-       ipsec6_setspidx_in6pcb(m, sotoin6pcb(so));
+#if DIAGNOSTIC
+       if (so->so_proto->pr_domain->dom_family != AF_INET6)
+               panic("ipsec6_getpolicybysock: socket domain != inet6\n");
+#endif
 
        pcbsp = sotoin6pcb(so)->in6p_sp;
+       
+       if (!pcbsp){
+               return ipsec6_getpolicybyaddr(m, dir, 0, error);
+       }
+
+       /* set spidx in pcb */
+       ipsec6_setspidx_in6pcb(m, sotoin6pcb(so));
 
        /* sanity check */
        if (pcbsp == NULL)
@@ -497,7 +607,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;
 
@@ -515,6 +627,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,
@@ -523,11 +636,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;
 
@@ -563,6 +679,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,
@@ -571,11 +688,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;
 
@@ -599,7 +719,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
@@ -614,6 +734,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");
@@ -624,7 +746,8 @@ ipsec6_getpolicybyaddr(m, dir, flag, error)
        bzero(&spidx, sizeof(spidx));
 
        /* make a index to look for a policy */
-       *error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET6, m);
+       *error = ipsec_setspidx_mbuf(&spidx, dir, AF_INET6, m,
+           (flag & IP_FORWARDING) ? 0 : 1);
 
        if (*error != 0)
                return NULL;
@@ -642,6 +765,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",
@@ -649,6 +773,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;
 }
@@ -665,139 +790,25 @@ ipsec6_getpolicybyaddr(m, dir, flag, error)
  *     other:  failure, and set errno.
  */
 int
-ipsec_setspidx_mbuf(spidx, dir, family, m)
-       struct secpolicyindex *spidx;
-       u_int dir, family;
-       struct mbuf *m;
+ipsec_setspidx_mbuf(
+       struct secpolicyindex *spidx,
+       u_int dir,
+       __unused u_int family,
+       struct mbuf *m,
+       int needport)
 {
-       struct sockaddr *sa1, *sa2;
+       int error;
 
        /* sanity check */
        if (spidx == NULL || m == NULL)
                panic("ipsec_setspidx_mbuf: NULL pointer was passed.\n");
 
-       KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-               printf("ipsec_setspidx_mbuf: begin\n"); kdebug_mbuf(m));
-
-       /* initialize */
        bzero(spidx, sizeof(*spidx));
 
-       spidx->dir = dir;
-       sa1 = (struct sockaddr *)&spidx->src;
-       sa2 = (struct sockaddr *)&spidx->dst;
-       sa1->sa_len = sa2->sa_len = _SALENBYAF(family);
-       sa1->sa_family = sa2->sa_family = family;
-       spidx->prefs = spidx->prefd = _INALENBYAF(family) << 3;
-
-    {
-       /* sanity check for packet length. */
-       struct mbuf *n;
-       int tlen;
-
-       tlen = 0;
-       for (n = m; n; n = n->m_next)
-               tlen += n->m_len;
-       if (m->m_pkthdr.len != tlen) {
-               KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-                       printf("ipsec_setspidx_mbuf: "
-                              "total of m_len(%d) != pkthdr.len(%d), "
-                              "ignored.\n",
-                               tlen, m->m_pkthdr.len));
+       error = ipsec_setspidx(m, spidx, needport);
+       if (error)
                goto bad;
-       }
-    }
-
-       switch (family) {
-       case AF_INET:
-       {
-               struct ip *ip;
-               struct ip ipbuf;
-
-               /* sanity check 1 for minimum ip header length */
-               if (m->m_pkthdr.len < sizeof(struct ip)) {
-                       KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-                               printf("ipsec_setspidx_mbuf: "
-                                      "pkthdr.len(%d) < sizeof(struct ip), "
-                                      "ignored.\n",
-                                       m->m_pkthdr.len));
-                       goto bad;
-               }
-
-               /*
-                * get IPv4 header packet.  usually the mbuf is contiguous
-                * and we need no copies.
-                */
-               if (m->m_len >= sizeof(*ip))
-                       ip = mtod(m, struct ip *);
-               else {
-                       m_copydata(m, 0, sizeof(ipbuf), (caddr_t)&ipbuf);
-                       ip = &ipbuf;
-               }
-
-               /* some more checks on IPv4 header. */
-               bcopy(&ip->ip_src, _INADDRBYSA(&spidx->src),
-                       sizeof(ip->ip_src));
-               bcopy(&ip->ip_dst, _INADDRBYSA(&spidx->dst),
-                       sizeof(ip->ip_dst));
-
-               spidx->ul_proto = ip->ip_p;
-               _INPORTBYSA(&spidx->src) = IPSEC_PORT_ANY;
-               _INPORTBYSA(&spidx->dst) = IPSEC_PORT_ANY;
-               break;
-       }
-
-#if INET6
-       case AF_INET6:
-       {
-               struct ip6_hdr *ip6;
-               struct ip6_hdr ip6buf;
-
-               /* sanity check 1 for minimum ip header length */
-               if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
-                       KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-                               printf("ipsec_setspidx_mbuf: "
-                                      "pkthdr.len(%d) < sizeof(struct ip6_hdr), "
-                                      "ignored.\n",
-                                       m->m_pkthdr.len));
-                       goto bad;
-               }
-
-               /*
-                * get IPv6 header packet.  usually the mbuf is contiguous
-                * and we need no copies.
-                */
-               if (m->m_len >= sizeof(*ip6))
-                       ip6 = mtod(m, struct ip6_hdr *);
-               else {
-                       m_copydata(m, 0, sizeof(ip6buf), (caddr_t)&ip6buf);
-                       ip6 = &ip6buf;
-               }
-
-               /* some more checks on IPv4 header. */
-               if ((ip6->ip6_vfc & IPV6_VERSION_MASK) != IPV6_VERSION) {
-                       KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-                               printf("ipsec_setspidx_mbuf: "
-                                       "wrong ip version on packet "
-                                       "(expected IPv6), ignored.\n"));
-                       goto bad;
-               }
-
-               bcopy(&ip6->ip6_src, _INADDRBYSA(&spidx->src),
-                       sizeof(ip6->ip6_src));
-               bcopy(&ip6->ip6_dst, _INADDRBYSA(&spidx->dst),
-                       sizeof(ip6->ip6_dst));
-
-               ipsec6_get_ulp(m, spidx);
-               break;
-       }
-#endif /* INET6 */
-       default:
-               panic("ipsec_secsecidx: no supported family passed.\n");
-       }
-
-       KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-               printf("ipsec_setspidx_mbuf: end\n");
-               kdebug_secpolicyindex(spidx));
+       spidx->dir = dir;
 
        return 0;
 
@@ -807,215 +818,352 @@ ipsec_setspidx_mbuf(spidx, dir, family, m)
        return EINVAL;
 }
 
-#if INET6
-/*
- * Get upper layer protocol number and port number if there.
- * Assumed all extension headers are in single mbuf.
- */
-#include <netinet/tcp.h>
-#include <netinet/udp.h>
-static void
-ipsec6_get_ulp(m, spidx)
-       struct mbuf *m;
-       struct secpolicyindex *spidx;
-{
-       int off, nxt;
-
-       /* sanity check */
-       if (m == NULL)
-               panic("ipsec6_get_ulp: NULL pointer was passed.\n");
-
-       KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-               printf("ipsec6_get_ulp:\n"); kdebug_mbuf(m));
-
-       /* set default */
-       spidx->ul_proto = IPSEC_ULPROTO_ANY;
-       _INPORTBYSA(&spidx->src) = IPSEC_PORT_ANY;
-       _INPORTBYSA(&spidx->dst) = IPSEC_PORT_ANY;
-
-       nxt = -1;
-       off = ip6_lasthdr(m, 0, IPPROTO_IPV6, &nxt);
-       if (off < 0 || m->m_pkthdr.len < off)
-               return;
-
-       switch (nxt) {
-       case IPPROTO_TCP:
-               spidx->ul_proto = nxt;
-               if (off + sizeof(struct tcphdr) <= m->m_pkthdr.len) {
-                       struct tcphdr th;
-                       m_copydata(m, off, sizeof(th), (caddr_t)&th);
-                       _INPORTBYSA(&spidx->src) = th.th_sport;
-                       _INPORTBYSA(&spidx->dst) = th.th_dport;
-               }
-               break;
-       case IPPROTO_UDP:
-               spidx->ul_proto = nxt;
-               if (off + sizeof(struct udphdr) <= m->m_pkthdr.len) {
-                       struct udphdr uh;
-                       m_copydata(m, off, sizeof(uh), (caddr_t)&uh);
-                       _INPORTBYSA(&spidx->src) = uh.uh_sport;
-                       _INPORTBYSA(&spidx->dst) = uh.uh_dport;
-               }
-               break;
-       case IPPROTO_ICMPV6:
-               spidx->ul_proto = nxt;
-               break;
-       default:
-               break;
-       }
-}
-#endif
-
-static void
+static int
 ipsec4_setspidx_inpcb(m, pcb)
        struct mbuf *m;
        struct inpcb *pcb;
 {
        struct secpolicyindex *spidx;
-       struct sockaddr *sa1, *sa2;
+       int error;
+
+       if (ipsec_bypass != 0)
+               return 0;
 
        /* sanity check */
        if (pcb == NULL)
                panic("ipsec4_setspidx_inpcb: no PCB found.\n");
        if (pcb->inp_sp == NULL)
                panic("ipsec4_setspidx_inpcb: no inp_sp found.\n");
-       if (pcb->inp_sp->sp_out ==NULL || pcb->inp_sp->sp_in == NULL)
+       if (pcb->inp_sp->sp_out == NULL || pcb->inp_sp->sp_in == NULL)
                panic("ipsec4_setspidx_inpcb: no sp_in/out found.\n");
 
        bzero(&pcb->inp_sp->sp_in->spidx, sizeof(*spidx));
        bzero(&pcb->inp_sp->sp_out->spidx, sizeof(*spidx));
 
        spidx = &pcb->inp_sp->sp_in->spidx;
+       error = ipsec_setspidx(m, spidx, 1);
+       if (error)
+               goto bad;
        spidx->dir = IPSEC_DIR_INBOUND;
-       sa1 = (struct sockaddr *)&spidx->src;
-       sa2 = (struct sockaddr *)&spidx->dst;
-       sa1->sa_len = sa2->sa_len = _SALENBYAF(AF_INET);
-       sa1->sa_family = sa2->sa_family = AF_INET;
-       spidx->prefs = _INALENBYAF(AF_INET) << 3;
-       spidx->prefd = _INALENBYAF(AF_INET) << 3;
-       spidx->ul_proto = pcb->inp_socket->so_proto->pr_protocol;
-       _INPORTBYSA(&spidx->src) = pcb->inp_fport;
-       _INPORTBYSA(&spidx->dst) = pcb->inp_lport;
-       ipsec4_setspidx_ipaddr(m, spidx);
 
        spidx = &pcb->inp_sp->sp_out->spidx;
+       error = ipsec_setspidx(m, spidx, 1);
+       if (error)
+               goto bad;
        spidx->dir = IPSEC_DIR_OUTBOUND;
-       sa1 = (struct sockaddr *)&spidx->src;
-       sa2 = (struct sockaddr *)&spidx->dst;
-       sa1->sa_len = sa2->sa_len = _SALENBYAF(AF_INET);
-       sa1->sa_family = sa2->sa_family = AF_INET;
-       spidx->prefs = _INALENBYAF(AF_INET) << 3;
-       spidx->prefd = _INALENBYAF(AF_INET) << 3;
-       spidx->ul_proto = pcb->inp_socket->so_proto->pr_protocol;
-       _INPORTBYSA(&spidx->src) = pcb->inp_lport;
-       _INPORTBYSA(&spidx->dst) = pcb->inp_fport;
-       ipsec4_setspidx_ipaddr(m, spidx);
-
-       return;
-}
-
-static void
-ipsec4_setspidx_ipaddr(m, spidx)
-       struct mbuf *m;
-       struct secpolicyindex *spidx;
-{
-       struct ip *ip = NULL;
-       struct ip ipbuf;
 
-       /* sanity check 1 for minimum ip header length */
-       if (m == NULL)
-               panic("ipsec4_setspidx_ipaddr: m == 0 passed.\n");
-
-       if (m->m_pkthdr.len < sizeof(struct ip)) {
-                       printf("ipsec4_setspidx_ipaddr: "
-                              "pkthdr.len(%d) < sizeof(struct ip), "
-                              "ignored.\n",
-                               m->m_pkthdr.len);
-               return;
-       }
-
-       if (m && m->m_len >= sizeof(*ip))
-               ip = mtod(m, struct ip *);
-       else {
-               m_copydata(m, 0, sizeof(ipbuf), (caddr_t)&ipbuf);
-               ip = &ipbuf;
-       }
-
-       bcopy(&ip->ip_src, _INADDRBYSA(&spidx->src), sizeof(ip->ip_src));
-       bcopy(&ip->ip_dst, _INADDRBYSA(&spidx->dst), sizeof(ip->ip_dst));
+       return 0;
 
-       return;
+bad:
+       bzero(&pcb->inp_sp->sp_in->spidx, sizeof(*spidx));
+       bzero(&pcb->inp_sp->sp_out->spidx, sizeof(*spidx));
+       return error;
 }
 
 #if INET6
-static void
+static int
 ipsec6_setspidx_in6pcb(m, pcb)
        struct mbuf *m;
        struct in6pcb *pcb;
 {
        struct secpolicyindex *spidx;
-       struct sockaddr *sa1, *sa2;
+       int error;
 
        /* sanity check */
        if (pcb == NULL)
                panic("ipsec6_setspidx_in6pcb: no PCB found.\n");
        if (pcb->in6p_sp == NULL)
                panic("ipsec6_setspidx_in6pcb: no in6p_sp found.\n");
-       if (pcb->in6p_sp->sp_out ==NULL || pcb->in6p_sp->sp_in == NULL)
+       if (pcb->in6p_sp->sp_out == NULL || pcb->in6p_sp->sp_in == NULL)
                panic("ipsec6_setspidx_in6pcb: no sp_in/out found.\n");
 
        bzero(&pcb->in6p_sp->sp_in->spidx, sizeof(*spidx));
        bzero(&pcb->in6p_sp->sp_out->spidx, sizeof(*spidx));
 
        spidx = &pcb->in6p_sp->sp_in->spidx;
+       error = ipsec_setspidx(m, spidx, 1);
+       if (error)
+               goto bad;
        spidx->dir = IPSEC_DIR_INBOUND;
-       sa1 = (struct sockaddr *)&spidx->src;
-       sa2 = (struct sockaddr *)&spidx->dst;
-       sa1->sa_len = sa2->sa_len = _SALENBYAF(AF_INET6);
-       sa1->sa_family = sa2->sa_family = AF_INET6;
-       spidx->prefs = _INALENBYAF(AF_INET6) << 3;
-       spidx->prefd = _INALENBYAF(AF_INET6) << 3;
-       spidx->ul_proto = pcb->in6p_socket->so_proto->pr_protocol;
-       _INPORTBYSA(&spidx->src) = pcb->in6p_fport;
-       _INPORTBYSA(&spidx->dst) = pcb->in6p_lport;
-       ipsec6_setspidx_ipaddr(m, spidx);
 
        spidx = &pcb->in6p_sp->sp_out->spidx;
+       error = ipsec_setspidx(m, spidx, 1);
+       if (error)
+               goto bad;
        spidx->dir = IPSEC_DIR_OUTBOUND;
-       sa1 = (struct sockaddr *)&spidx->src;
-       sa2 = (struct sockaddr *)&spidx->dst;
-       sa1->sa_len = sa2->sa_len = _SALENBYAF(AF_INET6);
-       sa1->sa_family = sa2->sa_family = AF_INET6;
-       spidx->prefs = _INALENBYAF(AF_INET6) << 3;
-       spidx->prefd = _INALENBYAF(AF_INET6) << 3;
-       spidx->ul_proto = pcb->in6p_socket->so_proto->pr_protocol;
-       _INPORTBYSA(&spidx->src) = pcb->in6p_lport;
-       _INPORTBYSA(&spidx->dst) = pcb->in6p_fport;
-       ipsec6_setspidx_ipaddr(m, spidx);
 
-       return;
+       return 0;
+
+bad:
+       bzero(&pcb->in6p_sp->sp_in->spidx, sizeof(*spidx));
+       bzero(&pcb->in6p_sp->sp_out->spidx, sizeof(*spidx));
+       return error;
 }
+#endif
 
-static void
-ipsec6_setspidx_ipaddr(m, spidx)
+/*
+ * configure security policy index (src/dst/proto/sport/dport)
+ * by looking at the content of mbuf.
+ * the caller is responsible for error recovery (like clearing up spidx).
+ */
+static int
+ipsec_setspidx(m, spidx, needport)
        struct mbuf *m;
        struct secpolicyindex *spidx;
+       int needport;
 {
-       struct ip6_hdr *ip6 = NULL;
-       struct ip6_hdr ip6buf;
+       struct ip *ip = NULL;
+       struct ip ipbuf;
+       u_int v;
+       struct mbuf *n;
+       int len;
+       int error;
 
-       /* sanity check 1 for minimum ip header length */
        if (m == NULL)
-               panic("ipsec6_setspidx_in6pcb: m == 0 passed.\n");
+               panic("ipsec_setspidx: m == 0 passed.\n");
 
-       if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
+       /*
+        * validate m->m_pkthdr.len.  we see incorrect length if we
+        * mistakenly call this function with inconsistent mbuf chain
+        * (like 4.4BSD tcp/udp processing).  XXX should we panic here?
+        */
+       len = 0;
+       for (n = m; n; n = n->m_next)
+               len += n->m_len;
+       if (m->m_pkthdr.len != len) {
                KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-                       printf("ipsec6_setspidx_ipaddr: "
-                              "pkthdr.len(%d) < sizeof(struct ip6_hdr), "
+                       printf("ipsec_setspidx: "
+                              "total of m_len(%d) != pkthdr.len(%d), "
                               "ignored.\n",
-                               m->m_pkthdr.len));
-               return;
+                               len, m->m_pkthdr.len));
+               return EINVAL;
+       }
+
+       if (m->m_pkthdr.len < sizeof(struct ip)) {
+               KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+                       printf("ipsec_setspidx: "
+                           "pkthdr.len(%d) < sizeof(struct ip), ignored.\n",
+                           m->m_pkthdr.len));
+               return EINVAL;
+       }
+
+       if (m->m_len >= sizeof(*ip))
+               ip = mtod(m, struct ip *);
+       else {
+               m_copydata(m, 0, sizeof(ipbuf), (caddr_t)&ipbuf);
+               ip = &ipbuf;
+       }
+#ifdef _IP_VHL
+       v = _IP_VHL_V(ip->ip_vhl);
+#else
+       v = ip->ip_v;
+#endif
+       switch (v) {
+       case 4:
+               error = ipsec4_setspidx_ipaddr(m, spidx);
+               if (error)
+                       return error;
+               ipsec4_get_ulp(m, spidx, needport);
+               return 0;
+#if INET6
+       case 6:
+               if (m->m_pkthdr.len < sizeof(struct ip6_hdr)) {
+                       KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+                               printf("ipsec_setspidx: "
+                                   "pkthdr.len(%d) < sizeof(struct ip6_hdr), "
+                                   "ignored.\n", m->m_pkthdr.len));
+                       return EINVAL;
+               }
+               error = ipsec6_setspidx_ipaddr(m, spidx);
+               if (error)
+                       return error;
+               ipsec6_get_ulp(m, spidx, needport);
+               return 0;
+#endif
+       default:
+               KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+                       printf("ipsec_setspidx: "
+                           "unknown IP version %u, ignored.\n", v));
+               return EINVAL;
+       }
+}
+
+static void
+ipsec4_get_ulp(m, spidx, needport)
+       struct mbuf *m;
+       struct secpolicyindex *spidx;
+       int needport;
+{
+       struct ip ip;
+       struct ip6_ext ip6e;
+       u_int8_t nxt;
+       int off;
+       struct tcphdr th;
+       struct udphdr uh;
+
+       /* sanity check */
+       if (m == NULL)
+               panic("ipsec4_get_ulp: NULL pointer was passed.\n");
+       if (m->m_pkthdr.len < sizeof(ip))
+               panic("ipsec4_get_ulp: too short\n");
+
+       /* set default */
+       spidx->ul_proto = IPSEC_ULPROTO_ANY;
+       ((struct sockaddr_in *)&spidx->src)->sin_port = IPSEC_PORT_ANY;
+       ((struct sockaddr_in *)&spidx->dst)->sin_port = IPSEC_PORT_ANY;
+
+       m_copydata(m, 0, sizeof(ip), (caddr_t)&ip);
+       /* ip_input() flips it into host endian XXX need more checking */
+       if (ip.ip_off & (IP_MF | IP_OFFMASK))
+               return;
+
+       nxt = ip.ip_p;
+#ifdef _IP_VHL
+       off = _IP_VHL_HL(ip->ip_vhl) << 2;
+#else
+       off = ip.ip_hl << 2;
+#endif
+       while (off < m->m_pkthdr.len) {
+               switch (nxt) {
+               case IPPROTO_TCP:
+                       spidx->ul_proto = nxt;
+                       if (!needport)
+                               return;
+                       if (off + sizeof(struct tcphdr) > m->m_pkthdr.len)
+                               return;
+                       m_copydata(m, off, sizeof(th), (caddr_t)&th);
+                       ((struct sockaddr_in *)&spidx->src)->sin_port =
+                           th.th_sport;
+                       ((struct sockaddr_in *)&spidx->dst)->sin_port =
+                           th.th_dport;
+                       return;
+               case IPPROTO_UDP:
+                       spidx->ul_proto = nxt;
+                       if (!needport)
+                               return;
+                       if (off + sizeof(struct udphdr) > m->m_pkthdr.len)
+                               return;
+                       m_copydata(m, off, sizeof(uh), (caddr_t)&uh);
+                       ((struct sockaddr_in *)&spidx->src)->sin_port =
+                           uh.uh_sport;
+                       ((struct sockaddr_in *)&spidx->dst)->sin_port =
+                           uh.uh_dport;
+                       return;
+               case IPPROTO_AH:
+                       if (off + sizeof(ip6e) > m->m_pkthdr.len)
+                               return;
+                       m_copydata(m, off, sizeof(ip6e), (caddr_t)&ip6e);
+                       off += (ip6e.ip6e_len + 2) << 2;
+                       nxt = ip6e.ip6e_nxt;
+                       break;
+               case IPPROTO_ICMP:
+               default:
+                       /* XXX intermediate headers??? */
+                       spidx->ul_proto = nxt;
+                       return;
+               }
+       }
+}
+
+/* assumes that m is sane */
+static int
+ipsec4_setspidx_ipaddr(m, spidx)
+       struct mbuf *m;
+       struct secpolicyindex *spidx;
+{
+       struct ip *ip = NULL;
+       struct ip ipbuf;
+       struct sockaddr_in *sin;
+
+       if (m->m_len >= sizeof(*ip))
+               ip = mtod(m, struct ip *);
+       else {
+               m_copydata(m, 0, sizeof(ipbuf), (caddr_t)&ipbuf);
+               ip = &ipbuf;
+       }
+
+       sin = (struct sockaddr_in *)&spidx->src;
+       bzero(sin, sizeof(*sin));
+       sin->sin_family = AF_INET;
+       sin->sin_len = sizeof(struct sockaddr_in);
+       bcopy(&ip->ip_src, &sin->sin_addr, sizeof(ip->ip_src));
+       spidx->prefs = sizeof(struct in_addr) << 3;
+
+       sin = (struct sockaddr_in *)&spidx->dst;
+       bzero(sin, sizeof(*sin));
+       sin->sin_family = AF_INET;
+       sin->sin_len = sizeof(struct sockaddr_in);
+       bcopy(&ip->ip_dst, &sin->sin_addr, sizeof(ip->ip_dst));
+       spidx->prefd = sizeof(struct in_addr) << 3;
+       return 0;
+}
+
+#if INET6
+static void
+ipsec6_get_ulp(m, spidx, needport)
+       struct mbuf *m;
+       struct secpolicyindex *spidx;
+       int needport;
+{
+       int off, nxt;
+       struct tcphdr th;
+       struct udphdr uh;
+
+       /* sanity check */
+       if (m == NULL)
+               panic("ipsec6_get_ulp: NULL pointer was passed.\n");
+
+       KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
+               printf("ipsec6_get_ulp:\n"); kdebug_mbuf(m));
+
+       /* set default */
+       spidx->ul_proto = IPSEC_ULPROTO_ANY;
+       ((struct sockaddr_in6 *)&spidx->src)->sin6_port = IPSEC_PORT_ANY;
+       ((struct sockaddr_in6 *)&spidx->dst)->sin6_port = IPSEC_PORT_ANY;
+
+       nxt = -1;
+       off = ip6_lasthdr(m, 0, IPPROTO_IPV6, &nxt);
+       if (off < 0 || m->m_pkthdr.len < off)
+               return;
+
+       switch (nxt) {
+       case IPPROTO_TCP:
+               spidx->ul_proto = nxt;
+               if (!needport)
+                       break;
+               if (off + sizeof(struct tcphdr) > m->m_pkthdr.len)
+                       break;
+               m_copydata(m, off, sizeof(th), (caddr_t)&th);
+               ((struct sockaddr_in6 *)&spidx->src)->sin6_port = th.th_sport;
+               ((struct sockaddr_in6 *)&spidx->dst)->sin6_port = th.th_dport;
+               break;
+       case IPPROTO_UDP:
+               spidx->ul_proto = nxt;
+               if (!needport)
+                       break;
+               if (off + sizeof(struct udphdr) > m->m_pkthdr.len)
+                       break;
+               m_copydata(m, off, sizeof(uh), (caddr_t)&uh);
+               ((struct sockaddr_in6 *)&spidx->src)->sin6_port = uh.uh_sport;
+               ((struct sockaddr_in6 *)&spidx->dst)->sin6_port = uh.uh_dport;
+               break;
+       case IPPROTO_ICMPV6:
+       default:
+               /* XXX intermediate headers??? */
+               spidx->ul_proto = nxt;
+               break;
        }
+}
+
+/* assumes that m is sane */
+static int
+ipsec6_setspidx_ipaddr(m, spidx)
+       struct mbuf *m;
+       struct secpolicyindex *spidx;
+{
+       struct ip6_hdr *ip6 = NULL;
+       struct ip6_hdr ip6buf;
+       struct sockaddr_in6 *sin6;
 
        if (m->m_len >= sizeof(*ip6))
                ip6 = mtod(m, struct ip6_hdr *);
@@ -1024,18 +1172,29 @@ ipsec6_setspidx_ipaddr(m, spidx)
                ip6 = &ip6buf;
        }
 
-       if ((ip6->ip6_vfc & IPV6_VERSION_MASK) != IPV6_VERSION) {
-               KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
-                       printf("ipsec_setspidx_mbuf: "
-                               "wrong ip version on packet "
-                               "(expected IPv6), ignored.\n"));
-               return;
+       sin6 = (struct sockaddr_in6 *)&spidx->src;
+       bzero(sin6, sizeof(*sin6));
+       sin6->sin6_family = AF_INET6;
+       sin6->sin6_len = sizeof(struct sockaddr_in6);
+       bcopy(&ip6->ip6_src, &sin6->sin6_addr, sizeof(ip6->ip6_src));
+       if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
+               sin6->sin6_addr.s6_addr16[1] = 0;
+               sin6->sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
        }
+       spidx->prefs = sizeof(struct in6_addr) << 3;
+
+       sin6 = (struct sockaddr_in6 *)&spidx->dst;
+       bzero(sin6, sizeof(*sin6));
+       sin6->sin6_family = AF_INET6;
+       sin6->sin6_len = sizeof(struct sockaddr_in6);
+       bcopy(&ip6->ip6_dst, &sin6->sin6_addr, sizeof(ip6->ip6_dst));
+       if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
+               sin6->sin6_addr.s6_addr16[1] = 0;
+               sin6->sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
+       }
+       spidx->prefd = sizeof(struct in6_addr) << 3;
 
-       bcopy(&ip6->ip6_src, _INADDRBYSA(&spidx->src), sizeof(ip6->ip6_src));
-       bcopy(&ip6->ip6_dst, _INADDRBYSA(&spidx->dst), sizeof(ip6->ip6_dst));
-
-       return;
+       return 0;
 }
 #endif
 
@@ -1052,7 +1211,7 @@ static void
 ipsec_delpcbpolicy(p)
        struct inpcbpolicy *p;
 {
-       _FREE(p, M_SECA);
+       FREE(p, M_SECA);
 }
 
 /* initialize policy in PCB */
@@ -1074,30 +1233,15 @@ ipsec_init_policy(so, pcb_sp)
        }
        bzero(new, sizeof(*new));
 
-#if defined(__NetBSD__) || defined (__APPLE__)
-       if (so->so_uid == 0)    /*XXX*/
-               new->priv = 1;
-       else
-               new->priv = 0;
-#elif defined(__FreeBSD__) && __FreeBSD__ >= 3
-       if (so->so_cred != 0 && so->so_cred->pc_ucred->cr_uid == 0)
+#ifdef __APPLE__
+       if (so->so_uid == 0)
+#else
+       if (so->so_cred != 0 && !suser(so->so_cred->pc_ucred, NULL))
+#endif
                new->priv = 1;
        else
                new->priv = 0;
 
-       KEYDEBUG(KEYDEBUG_IPSEC_DATA,
-               printf("init: priv=%d ", new->priv);
-               if (so->so_cred) {
-                       printf("p_ruid=%d ", so->so_cred->p_ruid);
-                       printf("p_svuid=%d ", so->so_cred->p_svuid);
-                       printf("cr_uid=%d\n", so->so_cred->pc_ucred->cr_uid);
-               } else
-                       printf("so_cred is NULL\n");
-               );
-#else
-       new->priv = so->so_state & SS_PRIV;
-#endif
-
        if ((new->sp_in = key_newsp()) == NULL) {
                ipsec_delpcbpolicy(new);
                return ENOBUFS;
@@ -1106,7 +1250,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;
        }
@@ -1125,16 +1269,19 @@ ipsec_copy_policy(old, new)
 {
        struct secpolicy *sp;
 
+       if (ipsec_bypass != 0)
+               return 0;
+
        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;
@@ -1155,8 +1302,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;
 
        /*
@@ -1180,7 +1329,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);
@@ -1196,20 +1344,21 @@ ipsec_deepcopy_policy(src)
 fail:
        for (p = newchain; p; p = r) {
                r = p->next;
-               _FREE(p, M_SECA);
+               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;
@@ -1243,7 +1392,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");
@@ -1258,6 +1407,7 @@ ipsec_get_policy(pcb_sp, mp)
        struct mbuf **mp;
 {
 
+
        /* sanity check. */
        if (pcb_sp == NULL || mp == NULL)
                return EINVAL;
@@ -1268,11 +1418,7 @@ ipsec_get_policy(pcb_sp, mp)
                return ENOBUFS;
        }
 
-#if defined(__FreeBSD__) && __FreeBSD__ >= 3 || defined (__APPLE__)
-       (*mp)->m_type = MT_DATA;
-#else
-       (*mp)->m_type = MT_SOOPTS;
-#endif
+       m_mchtype(*mp, MT_DATA);
        KEYDEBUG(KEYDEBUG_IPSEC_DUMP,
                printf("ipsec_get_policy:\n");
                kdebug_mbuf(*mp));
@@ -1290,6 +1436,7 @@ ipsec4_set_policy(inp, optname, request, len, priv)
 {
        struct sadb_x_policy *xpl;
        struct secpolicy **pcb_sp;
+       int     error = 0;
 
        /* sanity check. */
        if (inp == NULL || request == NULL)
@@ -1298,6 +1445,12 @@ ipsec4_set_policy(inp, optname, request, len, priv)
                return EINVAL;
        xpl = (struct sadb_x_policy *)request;
 
+       if (inp->inp_sp == NULL) {
+               error = ipsec_init_policy(inp->inp_socket, &inp->inp_sp);
+               if (error)
+                       return error;
+       }
+
        /* select direction */
        switch (xpl->sadb_x_policy_dir) {
        case IPSEC_DIR_INBOUND:
@@ -1312,6 +1465,10 @@ ipsec4_set_policy(inp, optname, request, len, priv)
                return EINVAL;
        }
 
+       /* turn bypass off */
+       if (ipsec_bypass != 0)
+               ipsec_bypass = 0;
+
        return ipsec_set_policy(pcb_sp, optname, request, len, priv);
 }
 
@@ -1324,15 +1481,22 @@ ipsec4_get_policy(inp, request, len, mp)
 {
        struct sadb_x_policy *xpl;
        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;
-       if (inp->inp_sp == NULL)
-               panic("policy in PCB is NULL\n");
        if (len < sizeof(*xpl))
                return EINVAL;
        xpl = (struct sadb_x_policy *)request;
+       
+       if (inp->inp_sp == NULL) {
+               error = ipsec_init_policy(inp->inp_socket, &inp->inp_sp);
+               if (error)
+                       return error;
+       }
 
        /* select direction */
        switch (xpl->sadb_x_policy_dir) {
@@ -1356,6 +1520,7 @@ int
 ipsec4_delete_pcbpolicy(inp)
        struct inpcb *inp;
 {
+
        /* sanity check. */
        if (inp == NULL)
                panic("ipsec4_delete_pcbpolicy: NULL pointer was passed.\n");
@@ -1364,12 +1529,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;
        }
 
@@ -1390,6 +1555,7 @@ ipsec6_set_policy(in6p, optname, request, len, priv)
 {
        struct sadb_x_policy *xpl;
        struct secpolicy **pcb_sp;
+       int error = 0;
 
        /* sanity check. */
        if (in6p == NULL || request == NULL)
@@ -1397,6 +1563,12 @@ ipsec6_set_policy(in6p, optname, request, len, priv)
        if (len < sizeof(*xpl))
                return EINVAL;
        xpl = (struct sadb_x_policy *)request;
+       
+       if (in6p->in6p_sp == NULL) {
+               error = ipsec_init_policy(in6p->inp_socket, &in6p->in6p_sp);
+               if (error)
+                       return error;
+       }
 
        /* select direction */
        switch (xpl->sadb_x_policy_dir) {
@@ -1412,6 +1584,10 @@ ipsec6_set_policy(in6p, optname, request, len, priv)
                return EINVAL;
        }
 
+       /* turn bypass off */
+       if (ipsec_bypass != 0)
+               ipsec_bypass = 0;
+
        return ipsec_set_policy(pcb_sp, optname, request, len, priv);
 }
 
@@ -1424,15 +1600,20 @@ ipsec6_get_policy(in6p, request, len, mp)
 {
        struct sadb_x_policy *xpl;
        struct secpolicy *pcb_sp;
+       int error = 0;
 
        /* sanity check. */
        if (in6p == NULL || request == NULL || mp == NULL)
                return EINVAL;
-       if (in6p->in6p_sp == NULL)
-               panic("policy in PCB is NULL\n");
        if (len < sizeof(*xpl))
                return EINVAL;
        xpl = (struct sadb_x_policy *)request;
+       
+       if (in6p->in6p_sp == NULL) {
+               error = ipsec_init_policy(in6p->inp_socket, &in6p->in6p_sp);
+               if (error)
+                       return error;
+       }
 
        /* select direction */
        switch (xpl->sadb_x_policy_dir) {
@@ -1455,6 +1636,7 @@ int
 ipsec6_delete_pcbpolicy(in6p)
        struct in6pcb *in6p;
 {
+
        /* sanity check. */
        if (in6p == NULL)
                panic("ipsec6_delete_pcbpolicy: NULL pointer was passed.\n");
@@ -1463,12 +1645,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;
        }
 
@@ -1488,7 +1670,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)
@@ -1532,7 +1714,7 @@ ipsec_get_reqlevel(isr)
                        ((struct sockaddr *)&isr->sp->spidx.src)->sa_family);
        }
 
-#undef IPSEC_CHECK_DEFAULT(lev)
+#undef IPSEC_CHECK_DEFAULT
 
        /* set level */
        switch (isr->level) {
@@ -1549,6 +1731,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
@@ -1601,6 +1784,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:
@@ -1618,6 +1802,8 @@ ipsec_in_reject(sp, m)
        need_conf = 0;
        need_icv = 0;
 
+       /* XXX should compare policy against ipsec header history */
+
        for (isr = sp->req; isr != NULL; isr = isr->next) {
 
                /* get current level */
@@ -1628,10 +1814,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:
@@ -1643,7 +1841,8 @@ ipsec_in_reject(sp, m)
                case IPPROTO_IPCOMP:
                        /*
                         * we don't really care, as IPcomp document says that
-                        * we shouldn't compress small packets
+                        * we shouldn't compress small packets, IPComp policy
+                        * should always be treated as being in "use" level.
                         */
                        break;
                }
@@ -1675,6 +1874,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 ? */
@@ -1695,7 +1895,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;
 }
@@ -1705,14 +1905,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);
-       else {
-               if (inp->inp_socket)
-                       return ipsec4_in_reject_so(m, inp->inp_socket);
-               else
-                       panic("ipsec4_in_reject: invalid inpcb/socket");
-       }
+       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
@@ -1730,6 +1933,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 ? */
@@ -1749,7 +1953,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;
 }
@@ -1759,14 +1963,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);
-       else {
-               if (in6p->in6p_socket)
-                       return ipsec6_in_reject_so(m, in6p->in6p_socket);
-               else
-                       panic("ipsec6_in_reject: invalid in6p/socket");
-       }
+       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
 
@@ -1775,20 +1982,22 @@ 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_in_reject: using SP\n");
+               printf("ipsec_hdrsiz: using SP\n");
                kdebug_secpolicy(sp));
 
        /* check policy */
        switch (sp->policy) {
        case IPSEC_POLICY_DISCARD:
+       case IPSEC_POLICY_GENERATE:
        case IPSEC_POLICY_BYPASS:
        case IPSEC_POLICY_NONE:
                return 0;
@@ -1857,6 +2066,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 ? */
@@ -1880,7 +2090,7 @@ ipsec4_hdrsiz(m, dir, inp)
                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);
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return size;
 }
@@ -1899,6 +2109,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 ? */
@@ -1919,7 +2130,7 @@ ipsec6_hdrsiz(m, dir, in6p)
                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);
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return size;
 }
@@ -1949,7 +2160,7 @@ ipsec4_encapsulate(m, sav)
        }
 #if 0
        /* XXX if the dst is myself, perform nothing. */
-       if (key_ismyaddr(AF_INET, _INADDRBYSA(&sav->sah->saidx.dst))) {
+       if (key_ismyaddr((struct sockaddr *)&sav->sah->saidx.dst)) {
                m_freem(m);
                return EINVAL;
        }
@@ -1960,7 +2171,7 @@ ipsec4_encapsulate(m, sav)
 
        ip = mtod(m, struct ip *);
 #ifdef _IP_VHL
-       hlen = IP_VHL_HL(ip->ip_vhl) << 2;
+       hlen = _IP_VHL_HL(ip->ip_vhl) << 2;
 #else
        hlen = ip->ip_hl << 2;
 #endif
@@ -2016,13 +2227,13 @@ ipsec4_encapsulate(m, sav)
        ip->ip_off &= htons(~IP_OFFMASK);
        ip->ip_off &= htons(~IP_MF);
        switch (ip4_ipsec_dfbit) {
-       case 0: /*clear DF bit*/
+       case 0: /* clear DF bit */
                ip->ip_off &= htons(~IP_DF);
                break;
-       case 1: /*set DF bit*/
+       case 1: /* set DF bit */
                ip->ip_off |= htons(IP_DF);
                break;
-       default:        /*copy DF bit*/
+       default:        /* copy DF bit */
                break;
        }
        ip->ip_p = IPPROTO_IPIP;
@@ -2032,11 +2243,16 @@ ipsec4_encapsulate(m, sav)
                ipseclog((LOG_ERR, "IPv4 ipsec: size exceeds limit: "
                        "leave ip_len as is (invalid packet)\n"));
        }
+#ifdef RANDOM_IP_ID
+       ip->ip_id = ip_randomid();
+#else
        ip->ip_id = htons(ip_id++);
+#endif
        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));
+       ip->ip_ttl = IPDEFTTL;
 
        /* XXX Should ip_src be updated later ? */
 
@@ -2063,7 +2279,7 @@ ipsec6_encapsulate(m, sav)
        }
 #if 0
        /* XXX if the dst is myself, perform nothing. */
-       if (key_ismyaddr(AF_INET6, _INADDRBYSA(&sav->sah->saidx.dst))) {
+       if (key_ismyaddr((struct sockaddr *)&sav->sah->saidx.dst)) {
                m_freem(m);
                return EINVAL;
        }
@@ -2116,11 +2332,98 @@ ipsec6_encapsulate(m, sav)
                &ip6->ip6_src, sizeof(ip6->ip6_src));
        bcopy(&((struct sockaddr_in6 *)&sav->sah->saidx.dst)->sin6_addr,
                &ip6->ip6_dst, sizeof(ip6->ip6_dst));
+       ip6->ip6_hlim = IPV6_DEFHLIM;
 
        /* XXX Should ip6_src be updated later ? */
 
        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*/
 
 /*
@@ -2144,45 +2447,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;
        }
 }
@@ -2202,11 +2519,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)
@@ -2236,7 +2554,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" */
@@ -2251,14 +2569,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));
@@ -2273,20 +2595,23 @@ 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;
 }
 
 /*
- * shift variable length bunffer to left.
+ * shift variable length buffer to left.
  * IN: bitmap: pointer to the buffer
  *     nbit:   the number of to shift.
  *     wsize:  buffer size (bytes).
@@ -2328,11 +2653,11 @@ ipsec4_logpacketstr(ip, spi)
        snprintf(buf, sizeof(buf), "packet(SPI=%u ", (u_int32_t)ntohl(spi));
        while (p && *p)
                p++;
-       snprintf(p, sizeof(buf) - (p - buf), "src=%d.%d.%d.%d",
+       snprintf(p, sizeof(buf) - (p - buf), "src=%u.%u.%u.%u",
                s[0], s[1], s[2], s[3]);
        while (p && *p)
                p++;
-       snprintf(p, sizeof(buf) - (p - buf), " dst=%d.%d.%d.%d",
+       snprintf(p, sizeof(buf) - (p - buf), " dst=%u.%u.%u.%u",
                d[0], d[1], d[2], d[3]);
        while (p && *p)
                p++;
@@ -2437,26 +2762,26 @@ ipsec_dumpmbuf(m)
        printf("---\n");
 }
 
+#if INET
 /*
  * 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;
-#if IPSEC_SRCSEL
-       struct in_ifaddr *ia;
-#endif
+       struct secasvar *sav = NULL;
+       int error = 0;
        struct sockaddr_in *dst4;
-       struct sockaddr *sa;
+       struct sockaddr_in *sin;
 
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
        if (!state)
                panic("state == NULL in ipsec4_output");
        if (!state->m)
@@ -2466,6 +2791,8 @@ ipsec4_output(state, sp, flags)
        if (!state->dst)
                panic("state->dst == NULL in ipsec4_output");
 
+       KERNEL_DEBUG(DBG_FNC_IPSEC_OUT | DBG_FUNC_START, 0,0,0,0,0);
+
        KEYDEBUG(KEYDEBUG_IPSEC_DATA,
                printf("ipsec4_output: applyed SP\n");
                kdebug_secpolicy(sp));
@@ -2486,24 +2813,57 @@ ipsec4_output(state, sp, flags)
                /* make SA index for search proper SA */
                ip = mtod(state->m, struct ip *);
                bcopy(&isr->saidx, &saidx, sizeof(saidx));
-               sa = (struct sockaddr *)&saidx.src;
-               if (sa->sa_len == 0) {
-                       sa->sa_len = _SALENBYAF(AF_INET);
-                       sa->sa_family = AF_INET;
-                       _INPORTBYSA(&saidx.src) = IPSEC_PORT_ANY;
-                       bcopy(&ip->ip_src, _INADDRBYSA(&saidx.src),
-                               sizeof(ip->ip_src));
+               saidx.mode = isr->saidx.mode;
+               saidx.reqid = isr->saidx.reqid;
+               sin = (struct sockaddr_in *)&saidx.src;
+               if (sin->sin_len == 0) {
+                       sin->sin_len = sizeof(*sin);
+                       sin->sin_family = AF_INET;
+                       sin->sin_port = IPSEC_PORT_ANY;
+                       bcopy(&ip->ip_src, &sin->sin_addr,
+                           sizeof(sin->sin_addr));
                }
-               sa = (struct sockaddr *)&saidx.dst;
-               if (sa->sa_len == 0) {
-                       sa->sa_len = _SALENBYAF(AF_INET);
-                       sa->sa_family = AF_INET;
-                       _INPORTBYSA(&saidx.dst) = IPSEC_PORT_ANY;
-                       bcopy(&ip->ip_dst, _INADDRBYSA(&saidx.dst),
-                               sizeof(ip->ip_dst));
+               sin = (struct sockaddr_in *)&saidx.dst;
+               if (sin->sin_len == 0) {
+                       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
@@ -2511,12 +2871,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;
@@ -2533,9 +2893,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;
                }
@@ -2544,50 +2904,40 @@ 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().
                 */
-#if __NetBSD__
-               s = splsoftnet();
-#else
-               s = splnet();
-#endif
 
                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;
                        }
 
-                       ip = mtod(state->m, struct ip *);
-
                        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;
+                       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)) {
-                               RTFREE(state->ro->ro_rt);
-                               bzero((caddr_t)state->ro, sizeof (*state->ro));
+                               rtfree(state->ro->ro_rt);
+                               state->ro->ro_rt = NULL;
                        }
                        if (state->ro->ro_rt == 0) {
                                dst4->sin_family = AF_INET;
@@ -2596,26 +2946,17 @@ ipsec4_output(state, sp, flags)
                                rtalloc(state->ro);
                        }
                        if (state->ro->ro_rt == 0) {
-                               ipstat.ips_noroute++;
+                               OSAddAtomic(1, (SInt32*)&ipstat.ips_noroute);
                                error = EHOSTUNREACH;
                                goto bad;
                        }
 
-#if IPSEC_SRCSEL
-                       /*
-                        * Which address in SA or in routing table should I
-                        * select from ?  But I had set from SA at
-                        * ipsec4_encapsulate().
-                        */
-                       ia = (struct in_ifaddr *)(state->ro->ro_rt->rt_ifa);
+                       /* adjust state->dst if tunnel endpoint is offlink */
                        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;
                        }
-                       ip->ip_src = IA_SIN(ia)->sin_addr;
-#endif
-               } else
-                       splx(s);
+               }
 
                state->m = ipsec4_splithdr(state->m);
                if (!state->m) {
@@ -2625,7 +2966,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;
                        }
@@ -2637,13 +2978,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;
                        }
@@ -2665,51 +3006,61 @@ ipsec4_output(state, sp, flags)
                ip = mtod(state->m, struct ip *);
        }
 
+       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);
        return error;
 }
+#endif
 
 #if INET6
 /*
  * 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;
        struct secasindex saidx;
        int error = 0;
        int plen;
-       struct sockaddr *sa;
+       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");
+               panic("state == NULL in ipsec6_output_trans");
        if (!state->m)
-               panic("state->m == NULL in ipsec6_output");
+               panic("state->m == NULL in ipsec6_output_trans");
        if (!nexthdrp)
-               panic("nexthdrp == NULL in ipsec6_output");
+               panic("nexthdrp == NULL in ipsec6_output_trans");
        if (!mprev)
-               panic("mprev == NULL in ipsec6_output");
+               panic("mprev == NULL in ipsec6_output_trans");
        if (!sp)
-               panic("sp == NULL in ipsec6_output");
+               panic("sp == NULL in ipsec6_output_trans");
        if (!tun)
-               panic("tun == NULL in ipsec6_output");
+               panic("tun == NULL in ipsec6_output_trans");
 
        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) {
@@ -2720,24 +3071,36 @@ ipsec6_output_trans(state, nexthdrp, mprev, sp, flags, tun)
                /* make SA index for search proper SA */
                ip6 = mtod(state->m, struct ip6_hdr *);
                bcopy(&isr->saidx, &saidx, sizeof(saidx));
-               sa = (struct sockaddr *)&saidx.src;
-               if (sa->sa_len == 0) {
-                       sa->sa_len = _SALENBYAF(AF_INET6);
-                       sa->sa_family = AF_INET6;
-                       _INPORTBYSA(&saidx.src) = IPSEC_PORT_ANY;
-                       bcopy(&ip6->ip6_src, _INADDRBYSA(&saidx.src),
-                               sizeof(ip6->ip6_src));
+               saidx.mode = isr->saidx.mode;
+               saidx.reqid = isr->saidx.reqid;
+               sin6 = (struct sockaddr_in6 *)&saidx.src;
+               if (sin6->sin6_len == 0) {
+                       sin6->sin6_len = sizeof(*sin6);
+                       sin6->sin6_family = AF_INET6;
+                       sin6->sin6_port = IPSEC_PORT_ANY;
+                       bcopy(&ip6->ip6_src, &sin6->sin6_addr,
+                           sizeof(ip6->ip6_src));
+                       if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
+                               /* fix scope id for comparing SPD */
+                               sin6->sin6_addr.s6_addr16[1] = 0;
+                               sin6->sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
+                       }
                }
-               sa = (struct sockaddr *)&saidx.dst;
-               if (sa->sa_len == 0) {
-                       sa->sa_len = _SALENBYAF(AF_INET6);
-                       sa->sa_family = AF_INET6;
-                       _INPORTBYSA(&saidx.dst) = IPSEC_PORT_ANY;
-                       bcopy(&ip6->ip6_dst, _INADDRBYSA(&saidx.dst),
-                               sizeof(ip6->ip6_dst));
+               sin6 = (struct sockaddr_in6 *)&saidx.dst;
+               if (sin6->sin6_len == 0) {
+                       sin6->sin6_len = sizeof(*sin6);
+                       sin6->sin6_family = AF_INET6;
+                       sin6->sin6_port = IPSEC_PORT_ANY;
+                       bcopy(&ip6->ip6_dst, &sin6->sin6_addr,
+                           sizeof(ip6->ip6_dst));
+                       if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
+                               /* fix scope id for comparing SPD */
+                               sin6->sin6_addr.s6_addr16[1] = 0;
+                               sin6->sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
+                       }
                }
 
-               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
@@ -2745,13 +3108,25 @@ 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;
+
+                       /*
+                        * Notify the fact that the packet is discarded
+                        * to ourselves. I believe this is better than
+                        * just silently discarding. (jinmei@kame.net)
+                        * XXX: should we restrict the error to TCP packets?
+                        * XXX: should we directly notify sockets via
+                        *      pfctlinputs?
+                        */
+                       icmp6_error(state->m, ICMP6_DST_UNREACH,
+                                   ICMP6_DST_UNREACH_ADMIN, 0);
+                       state->m = NULL; /* icmp6_error freed the mbuf */
                        goto bad;
                }
 
                /* validity check */
-               if (isr->sav == NULL) {
+               if (sav == NULL) {
                        switch (ipsec_get_reqlevel(isr)) {
                        case IPSEC_LEVEL_USE:
                                continue;
@@ -2765,9 +3140,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;
                }
@@ -2775,23 +3150,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;
                }
@@ -2803,7 +3178,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;
                }
@@ -2815,9 +3190,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;
@@ -2827,28 +3206,30 @@ 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;
-#if IPSEC_SRCSEL
-       struct in6_addr *ia6;
-#endif
        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");
+               panic("state == NULL in ipsec6_output_tunnel");
        if (!state->m)
-               panic("state->m == NULL in ipsec6_output");
+               panic("state->m == NULL in ipsec6_output_tunnel");
        if (!sp)
-               panic("sp == NULL in ipsec6_output");
+               panic("sp == NULL in ipsec6_output_tunnel");
 
        KEYDEBUG(KEYDEBUG_IPSEC_DATA,
                printf("ipsec6_output_tunnel: applyed SP\n");
@@ -2863,10 +3244,49 @@ ipsec6_output_tunnel(state, sp, flags)
                        break;
        }
 
-       for (/*already initialized*/; isr; isr = isr->next) {
-               /* When tunnel mode, SA peers must be specified. */
-               bcopy(&isr->saidx, &saidx, sizeof(saidx));
-               if (key_checkrequest(isr, &saidx) == ENOENT) {
+       for (/* already initialized */; isr; isr = isr->next) {
+               if (isr->saidx.mode == IPSEC_MODE_TUNNEL) {
+                       /* When tunnel mode, SA peers must be specified. */
+                       bcopy(&isr->saidx, &saidx, sizeof(saidx));
+               } else {
+                       /* make SA index to look for a proper SA */
+                       struct sockaddr_in6 *sin6;
+
+                       bzero(&saidx, sizeof(saidx));
+                       saidx.proto = isr->saidx.proto;
+                       saidx.mode = isr->saidx.mode;
+                       saidx.reqid = isr->saidx.reqid;
+
+                       ip6 = mtod(state->m, struct ip6_hdr *);
+                       sin6 = (struct sockaddr_in6 *)&saidx.src;
+                       if (sin6->sin6_len == 0) {
+                               sin6->sin6_len = sizeof(*sin6);
+                               sin6->sin6_family = AF_INET6;
+                               sin6->sin6_port = IPSEC_PORT_ANY;
+                               bcopy(&ip6->ip6_src, &sin6->sin6_addr,
+                                   sizeof(ip6->ip6_src));
+                               if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_src)) {
+                                       /* fix scope id for comparing SPD */
+                                       sin6->sin6_addr.s6_addr16[1] = 0;
+                                       sin6->sin6_scope_id = ntohs(ip6->ip6_src.s6_addr16[1]);
+                               }
+                       }
+                       sin6 = (struct sockaddr_in6 *)&saidx.dst;
+                       if (sin6->sin6_len == 0) {
+                               sin6->sin6_len = sizeof(*sin6);
+                               sin6->sin6_family = AF_INET6;
+                               sin6->sin6_port = IPSEC_PORT_ANY;
+                               bcopy(&ip6->ip6_dst, &sin6->sin6_addr,
+                                   sizeof(ip6->ip6_dst));
+                               if (IN6_IS_SCOPE_LINKLOCAL(&ip6->ip6_dst)) {
+                                       /* fix scope id for comparing SPD */
+                                       sin6->sin6_addr.s6_addr16[1] = 0;
+                                       sin6->sin6_scope_id = ntohs(ip6->ip6_dst.s6_addr16[1]);
+                               }
+                       }
+               }
+
+               if (key_checkrequest(isr, &saidx, &sav) == ENOENT) {
                        /*
                         * IPsec processing is required, but no SA found.
                         * I assume that key_acquire() had been called
@@ -2874,13 +3294,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;
@@ -2894,63 +3314,145 @@ 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().
-                */
-#if __NetBSD__
-               s = splsoftnet();
-#else
-               s = splnet();
-#endif
-
                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;
-                       }
-
-                       ip6 = mtod(state->m, struct ip6_hdr *);
-
                        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;
+
+                               /*
+                                * 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 *);
+
+                               ro4 = &sav->sah->sa_route;
+                               dst4 = (struct sockaddr_in *)&ro4->ro_dst;
+                               if (ro4->ro_rt
+                                && ((ro4->ro_rt->rt_flags & RTF_UP) == 0
+                                 || dst4->sin_addr.s_addr != ip->ip_dst.s_addr)) {
+                                       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;
+                                       rtalloc(ro4);
+                               }
+                               if (ro4->ro_rt == NULL) {
+                                       OSAddAtomic(1, (SInt32*)&ipstat.ips_noroute);
+                                       error = EHOSTUNREACH;
+                                       goto bad;
+                               }
+       
+                               state->m = ipsec4_splithdr(state->m);
+                               if (!state->m) {
+                                       error = ENOMEM;
+                                       goto bad;
+                               }
+                               switch (isr->saidx.proto) {
+                               case IPPROTO_ESP:
+#if IPSEC_ESP
+                                       if ((error = esp4_output(state->m, sav)) != 0) {
+                                               state->m = NULL;
+                                               goto bad;
+                                       }
+                                       break;
+
+#else
+                                       m_freem(state->m);
+                                       state->m = NULL;
+                                       error = EINVAL;
+                                       goto bad;
+#endif
+                               case IPPROTO_AH:
+                                       if ((error = ah4_output(state->m, sav)) != 0) {
+                                               state->m = NULL;
+                                               goto bad;
+                                       }
+                                       break;
+                               case IPPROTO_IPCOMP:
+                                       if ((error = ipcomp4_output(state->m, sav)) != 0) {
+                                               state->m = NULL;
+                                               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;
+                                       goto bad;
+                               }
+               
+                               if (state->m == 0) {
+                                       error = ENOMEM;
+                                       goto bad;
+                               }
+                               ip = mtod(state->m, struct ip *);
+                               ip->ip_len = ntohs(ip->ip_len);  /* flip len field before calling ip_output */
+                               ip_output(state->m, NULL, ro4, 0, NULL, NULL);
+                               state->m = NULL;
+                               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;
+                       
+                       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))) {
-                               RTFREE(state->ro->ro_rt);
-                               bzero((caddr_t)state->ro, sizeof (*state->ro));
+                               rtfree(state->ro->ro_rt);
+                               state->ro->ro_rt = NULL;
                        }
                        if (state->ro->ro_rt == 0) {
                                bzero(dst6, sizeof(*dst6));
@@ -2961,38 +3463,21 @@ ipsec6_output_tunnel(state, sp, flags)
                        }
                        if (state->ro->ro_rt == 0) {
                                ip6stat.ip6s_noroute++;
-                               ipsec6stat.out_noroute++;
+                               IPSEC_STAT_INCREMENT(ipsec6stat.out_noroute);
                                error = EHOSTUNREACH;
                                goto bad;
                        }
-#if 0  /* XXX Is the following need ? */
+
+                       /* adjust state->dst if tunnel endpoint is offlink */
                        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;
                        }
-#endif
-#if IPSEC_SRCSEL
-                       /*
-                        * Which address in SA or in routing table should I
-                        * select from ?  But I had set from SA at
-                        * ipsec6_encapsulate().
-                        */
-                       ia6 = in6_selectsrc(dst6, NULL, NULL,
-                                           (struct route_in6 *)state->ro,
-                                           NULL, &error);
-                       if (ia6 == NULL) {
-                               ip6stat.ip6s_noroute++;
-                               ipsec6stat.out_noroute++;
-                               goto bad;
-                       }
-                       ip6->ip6_src = *ia6;
-#endif
-               } else
-                       splx(s);
+               }
 
                state->m = ipsec6_splithdr(state->m);
                if (!state->m) {
-                       ipsec6stat.out_nomem++;
+                       IPSEC_STAT_INCREMENT(ipsec6stat.out_nomem);
                        error = ENOMEM;
                        goto bad;
                }
@@ -3000,14 +3485,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 */
@@ -3016,7 +3501,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;
                }
@@ -3028,23 +3513,29 @@ 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;
 }
 #endif /*INET6*/
 
+#if INET
 /*
  * Chop IP header and option off from the payload.
  */
@@ -3060,12 +3551,12 @@ ipsec4_splithdr(m)
                panic("ipsec4_splithdr: first mbuf too short");
        ip = mtod(m, struct ip *);
 #ifdef _IP_VHL
-       hlen = IP_VHL_HL(ip->ip_vhl) << 2;
+       hlen = _IP_VHL_HL(ip->ip_vhl) << 2;
 #else
        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;
@@ -3073,6 +3564,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;
@@ -3086,6 +3578,7 @@ ipsec4_splithdr(m)
        }
        return m;
 }
+#endif
 
 #if INET6
 static struct mbuf *
@@ -3101,7 +3594,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;
@@ -3109,6 +3602,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;
@@ -3126,38 +3620,106 @@ ipsec6_splithdr(m)
 
 /* validate inbound IPsec tunnel packet. */
 int
-ipsec4_tunnel_validate(ip, nxt0, sav)
-       struct ip *ip;
+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, i4src, i4dst;
+       struct sockaddr_in6 i6src, i6dst;
        int hlen;
+       struct secpolicy *sp;
+       struct ip *oip;
 
-       if (nxt != IPPROTO_IPV4)
+       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 && nxt != IPPROTO_IPV6)
+               return 0;
+       if (m->m_pkthdr.len < off + sizeof(struct ip))
+               return 0;
+       /* do not decapsulate if the SA is for transport mode only */
+       if (sav->sah->saidx.mode == IPSEC_MODE_TRANSPORT)
                return 0;
+
+       oip = mtod(m, struct ip *);
 #ifdef _IP_VHL
-       hlen = IP_VHL_HL(ip->ip_vhl) << 2;
+       hlen = _IP_VHL_HL(oip->ip_vhl) << 2;
 #else
-       hlen = ip->ip_hl << 2;
+       hlen = oip->ip_hl << 2;
 #endif
        if (hlen != sizeof(struct ip))
                return 0;
-       switch (((struct sockaddr *)&sav->sah->saidx.dst)->sa_family) {
-       case AF_INET:
-               sin = (struct sockaddr_in *)&sav->sah->saidx.dst;
-               if (bcmp(&ip->ip_dst, &sin->sin_addr, sizeof(ip->ip_dst)) != 0)
-                       return 0;
-               break;
-#if INET6
-       case AF_INET6:
-               /* should be supported, but at this moment we don't. */
-               /*FALLTHROUGH*/
-#endif
-       default:
+
+       sin = (struct sockaddr_in *)&sav->sah->saidx.dst;
+       if (sin->sin_family != AF_INET)
+               return 0;
+       if (bcmp(&oip->ip_dst, &sin->sin_addr, sizeof(oip->ip_dst)) != 0)
                return 0;
-       }
+
+       /* XXX slow */
+       bzero(&osrc, sizeof(osrc));
+       bzero(&odst, sizeof(odst));
+       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;
+       /*
+        * RFC2401 5.2.1 (b): (assume that we are using tunnel mode)
+        * - if the inner destination is multicast address, there can be
+        *   multiple permissible inner source address.  implementation
+        *   may want to skip verification of inner source address against
+        *   SPD selector.
+        * - if the inner protocol is ICMP, the packet may be an error report
+        *   from routers on the other side of the VPN cloud (R in the
+        *   following diagram).  in this case, we cannot verify inner source
+        *   address against SPD selector.
+        *      me -- gw === gw -- R -- you
+        *
+        * we consider the first bullet to be users responsibility on SPD entry
+        * configuration (if you need to encrypt multicast traffic, set
+        * the source range of SPD selector to 0.0.0.0/0, or have explicit
+        * address ranges for possible senders).
+        * the second bullet is not taken care of (yet).
+        *
+        * therefore, we do not do anything special about inner source.
+        */
+       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_SADB_UNLOCKED);
 
        return 1;
 }
@@ -3165,28 +3727,74 @@ ipsec4_tunnel_validate(ip, nxt0, sav)
 #if INET6
 /* validate inbound IPsec tunnel packet. */
 int
-ipsec6_tunnel_validate(ip6, nxt0, sav)
-       struct ip6_hdr *ip6;
+ipsec6_tunnel_validate(m, off, nxt0, sav)
+       struct mbuf *m;         /* no pullup permitted, m->m_len >= ip */
+       int off;
        u_int nxt0;
        struct secasvar *sav;
 {
        u_int8_t nxt = nxt0 & 0xff;
        struct sockaddr_in6 *sin6;
+       struct sockaddr_in6 osrc, odst, isrc, idst;
+       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");
+#endif
        if (nxt != IPPROTO_IPV6)
                return 0;
-       switch (((struct sockaddr *)&sav->sah->saidx.dst)->sa_family) {
-       case AF_INET6:
-               sin6 = ((struct sockaddr_in6 *)&sav->sah->saidx.dst);
-               if (!IN6_ARE_ADDR_EQUAL(&ip6->ip6_dst, &sin6->sin6_addr))
-                       return 0;
-               break;
-       case AF_INET:
-               /* should be supported, but at this moment we don't. */
-               /*FALLTHROUGH*/
-       default:
+       if (m->m_pkthdr.len < off + sizeof(struct ip6_hdr))
+               return 0;
+       /* do not decapsulate if the SA is for transport mode only */
+       if (sav->sah->saidx.mode == IPSEC_MODE_TRANSPORT)
                return 0;
-       }
+
+       oip6 = mtod(m, struct ip6_hdr *);
+       /* AF_INET should be supported, but at this moment we don't. */
+       sin6 = (struct sockaddr_in6 *)&sav->sah->saidx.dst;
+       if (sin6->sin6_family != AF_INET6)
+               return 0;
+       if (!IN6_ARE_ADDR_EQUAL(&oip6->ip6_dst, &sin6->sin6_addr))
+               return 0;
+
+       /* XXX slow */
+       bzero(&osrc, sizeof(osrc));
+       bzero(&odst, sizeof(odst));
+       bzero(&isrc, sizeof(isrc));
+       bzero(&idst, sizeof(idst));
+       osrc.sin6_family = odst.sin6_family = isrc.sin6_family =
+           idst.sin6_family = AF_INET6;
+       osrc.sin6_len = odst.sin6_len = isrc.sin6_len = idst.sin6_len = 
+           sizeof(struct sockaddr_in6);
+       osrc.sin6_addr = oip6->ip6_src;
+       odst.sin6_addr = oip6->ip6_dst;
+       m_copydata(m, off + offsetof(struct ip6_hdr, ip6_src),
+           sizeof(isrc.sin6_addr), (caddr_t)&isrc.sin6_addr);
+       m_copydata(m, off + offsetof(struct ip6_hdr, ip6_dst),
+           sizeof(idst.sin6_addr), (caddr_t)&idst.sin6_addr);
+
+       /*
+        * regarding to inner source address validation, see a long comment
+        * in ipsec4_tunnel_validate.
+        */
+
+       sp = key_gettunnel((struct sockaddr *)&osrc, (struct sockaddr *)&odst,
+           (struct sockaddr *)&isrc, (struct sockaddr *)&idst);
+       /*
+        * when there is no suitable inbound policy for the packet of the ipsec
+        * tunnel mode, the kernel never decapsulate the tunneled packet
+        * as the ipsec tunnel mode even when the system wide policy is "none".
+        * then the kernel leaves the generic tunnel module to process this
+        * packet.  if there is no rule of the generic tunnel, the packet
+        * is rejected and the statistics will be counted up.
+        */
+       if (!sp)
+               return 0;
+       key_freesp(sp, KEY_SADB_UNLOCKED);
 
        return 1;
 }
@@ -3213,33 +3821,18 @@ ipsec_copypkt(m)
                         * XXX: is this approach effective?
                         */
                        if (
-#if __bsdi__
-                               n->m_ext.ext_func ||
-#else
                                n->m_ext.ext_free ||
-#endif 
-#if __NetBSD__
-                               MCLISREFERENCED(n)
-#else
-                               mclrefcnt[mtocl(n->m_ext.ext_buf)] > 1
-#endif
+                               m_mclhasreference(n)
                            )
                        {
                                int remain, copied;
                                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;
                                }
@@ -3261,7 +3854,7 @@ ipsec_copypkt(m)
                                 */
                                remain = n->m_len;
                                copied = 0;
-                               while(1) {
+                               while (1) {
                                        int len;
                                        struct mbuf *mn;
 
@@ -3288,9 +3881,10 @@ 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;
                                        mm->m_next = mn;
                                        mm = mn;
                                }
@@ -3314,355 +3908,216 @@ ipsec_copypkt(m)
        return(NULL);
 }
 
-void
-ipsec_setsocket(m, so)
-       struct mbuf *m;
-       struct socket *so;
-{
-       struct mbuf *n;
-
-       n = m_aux_find(m, AF_INET, IPPROTO_ESP);
-       if (so && !n)
-               n = m_aux_add(m, AF_INET, IPPROTO_ESP);
-       if (n) {
-               if (so) {
-                       *mtod(n, struct socket **) = so;
-                       /*
-                        * XXX think again about it when we put decryption
-                        * histrory into aux mbuf
-                        */
-                       n->m_len = sizeof(struct socket *);
-               } else
-                       m_aux_delete(m, n);
-       }
-}
-
-struct socket *
-ipsec_getsocket(m)
-       struct mbuf *m;
-{
-       struct mbuf *n;
-
-       n = m_aux_find(m, AF_INET, IPPROTO_ESP);
-       if (n && n->m_len >= sizeof(struct socket *))
-               return *mtod(n, struct socket **);
-       else
-               return NULL;
-}
-
-#ifdef __bsdi__
 /*
- * System control for IP
+ * Tags are allocated as mbufs for now, since our minimum size is MLEN, we
+ * should make use of up to that much space.
  */
-u_char ipsecctlerrmap[PRC_NCMDS] = {
-       0,              0,              0,              0,
-       0,              EMSGSIZE,       EHOSTDOWN,      EHOSTUNREACH,
-       EHOSTUNREACH,   EHOSTUNREACH,   ECONNREFUSED,   ECONNREFUSED,
-       EMSGSIZE,       EHOSTUNREACH,   0,              0,
-       0,              0,              0,              0,
-       ENOPROTOOPT
+#define        IPSEC_TAG_HEADER \
+
+struct ipsec_tag {
+       struct socket                   *socket;
+       u_int32_t                               history_count;
+       struct ipsec_history    history[];
 };
 
-int *ipsec_sysvars[] = IPSECCTL_VARS;
+#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))
 
-int
-ipsec_sysctl(name, namelen, oldp, oldlenp, newp, newlen)
-       int     *name;
-       u_int   namelen;
-       void    *oldp;
-       size_t  *oldlenp;
-       void    *newp;
-       size_t  newlen;
+static struct ipsec_tag *
+ipsec_addaux(
+       struct mbuf *m)
 {
-       if (name[0] >= IPSECCTL_MAXID)
-               return (EOPNOTSUPP);
-
-       switch (name[0]) {
-       case IPSECCTL_STATS:
-               return sysctl_rdtrunc(oldp, oldlenp, newp, &ipsecstat,
-                   sizeof(ipsecstat));
-       case IPSECCTL_DEF_POLICY:
-               if (newp != NULL && newlen == sizeof(int)) {
-                       switch (*(int *)newp) {
-                       case IPSEC_POLICY_DISCARD:
-                       case IPSEC_POLICY_NONE:
-                               break;
-                       default:
-                               return EINVAL;
-                       }
-               }
-               return (sysctl_int_arr(ipsec_sysvars, name, namelen,
-                   oldp, oldlenp, newp, newlen));
-       case IPSECCTL_DEF_ESP_TRANSLEV:
-       case IPSECCTL_DEF_ESP_NETLEV:
-       case IPSECCTL_DEF_AH_TRANSLEV:  
-       case IPSECCTL_DEF_AH_NETLEV:
-               if (newp != NULL && newlen == sizeof(int)) {
-                       switch (*(int *)newp) {
-                       case IPSEC_LEVEL_USE:
-                       case IPSEC_LEVEL_REQUIRE:
-                               break;
-                       default:
-                               return EINVAL;
-                       }
+       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_alloc(KERNEL_MODULE_TAG_ID, KERNEL_TAG_TYPE_IPSEC,
+                                                 IPSEC_TAG_SIZE, M_DONTWAIT);
+               
+               if (tag) {
+                       itag = (struct ipsec_tag*)(tag + 1);
+                       itag->socket = 0;
+                       itag->history_count = 0;
+                       
+                       m_tag_prepend(m, tag);
                }
-               return (sysctl_int_arr(ipsec_sysvars, name, namelen,
-                   oldp, oldlenp, newp, newlen));
-       default:
-               return (sysctl_int_arr(ipsec_sysvars, name, namelen,
-                   oldp, oldlenp, newp, newlen));
        }
+       
+       return tag ? (struct ipsec_tag*)(tag + 1) : NULL;
 }
 
-#if INET6
-/*
- * System control for IP6
- */
-u_char ipsec6ctlerrmap[PRC_NCMDS] = {
-       0,              0,              0,              0,
-       0,              EMSGSIZE,       EHOSTDOWN,      EHOSTUNREACH,
-       EHOSTUNREACH,   EHOSTUNREACH,   ECONNREFUSED,   ECONNREFUSED,
-       EMSGSIZE,       EHOSTUNREACH,   0,              0,
-       0,              0,              0,              0,
-       ENOPROTOOPT
-};
-
-int *ipsec6_sysvars[] = IPSEC6CTL_VARS;
+static struct ipsec_tag *
+ipsec_findaux(
+       struct mbuf *m)
+{
+       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;
+}
 
-int
-ipsec6_sysctl(name, namelen, oldp, oldlenp, newp, newlen)
-       int     *name;
-       u_int   namelen;
-       void    *oldp;
-       size_t  *oldlenp;
-       void    *newp;
-       size_t  newlen;
+void
+ipsec_delaux(
+       struct mbuf *m)
 {
-       if (name[0] >= IPSECCTL_MAXID)  /* xxx no 6 in this definition */
-               return (EOPNOTSUPP);
-
-       switch (name[0]) {
-       case IPSECCTL_STATS:    /* xxx no 6 in this definition */
-               return sysctl_rdtrunc(oldp, oldlenp, newp, &ipsec6stat,
-                   sizeof(ipsec6stat));
-       case IPSECCTL_DEF_POLICY:
-               if (newp != NULL && newlen == sizeof(int)) {
-                       switch (*(int *)newp) {
-                       case IPSEC_POLICY_DISCARD:
-                       case IPSEC_POLICY_NONE:
-                               break;
-                       default:
-                               return EINVAL;
-                       }
-               }
-               return (sysctl_int_arr(ipsec6_sysvars, name, namelen,
-                   oldp, oldlenp, newp, newlen));
-       case IPSECCTL_DEF_ESP_TRANSLEV:
-       case IPSECCTL_DEF_ESP_NETLEV:
-       case IPSECCTL_DEF_AH_TRANSLEV:  
-       case IPSECCTL_DEF_AH_NETLEV:
-               if (newp != NULL && newlen == sizeof(int)) {
-                       switch (*(int *)newp) {
-                       case IPSEC_LEVEL_USE:
-                       case IPSEC_LEVEL_REQUIRE:
-                               break;
-                       default:
-                               return EINVAL;
-                       }
-               }
-               return (sysctl_int_arr(ipsec6_sysvars, name, namelen,
-                   oldp, oldlenp, newp, newlen));
-       default:
-               return (sysctl_int_arr(ipsec6_sysvars, name, namelen,
-                   oldp, oldlenp, newp, newlen));
+       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);
        }
 }
-#endif /*INET6*/
-#endif /*__bsdi__*/
 
-
-#if __NetBSD__
-/*
- * System control for IPSEC
- */
-u_char ipsecctlermap[PRC_NCMDS] = {
-       0,              0,              0,              0,
-       0,              EMSGSIZE,       EHOSTDOWN,      EHOSTUNREACH,
-       EHOSTUNREACH,   EHOSTUNREACH,   ECONNREFUSED,   ECONNREFUSED,
-       EMSGSIZE,       EHOSTUNREACH,   0,              0,
-       0,              0,              0,              0,
-       ENOPROTOOPT
-};
-
-int *ipsec_sysvars[] = IPSECCTL_VARS;
-
-int
-ipsec_sysctl(name, namelen, oldp, oldlenp, newp, newlen)
-       int *name;
-       u_int namelen;
-       void *oldp;
-       size_t *oldlenp;
-       void *newp;
-       size_t newlen;
+/* if the aux buffer is unnecessary, nuke it. */
+static void
+ipsec_optaux(
+       struct mbuf                     *m,
+       struct ipsec_tag        *itag)
 {
-       /* All sysctl names at this level are terminal. */
-       if (namelen != 1)
-               return ENOTDIR;
-
-       /* common sanity checks */
-       switch (name[0]) {
-       case IPSECCTL_DEF_ESP_TRANSLEV:
-       case IPSECCTL_DEF_ESP_NETLEV:
-       case IPSECCTL_DEF_AH_TRANSLEV:
-       case IPSECCTL_DEF_AH_NETLEV:
-               if (newp != NULL && newlen == sizeof(int)) {
-                       switch (*(int *)newp) {
-                       case IPSEC_LEVEL_USE:
-                       case IPSEC_LEVEL_REQUIRE:
-                               break;
-                       default:
-                               return EINVAL;
-                       }
-               }
+       if (itag && itag->socket == NULL && itag->history_count == 0) {
+               m_tag_delete(m, ((struct m_tag*)itag) - 1);
        }
+}
 
-       switch (name[0]) {
+int
+ipsec_setsocket(
+       struct mbuf *m,
+       struct socket *so)
+{
+       struct ipsec_tag        *tag;
 
-       case IPSECCTL_STATS:
-               return sysctl_struct(oldp, oldlenp, newp, newlen,
-                                    &ipsecstat, sizeof(ipsecstat));
-       case IPSECCTL_DEF_POLICY:
-               if (newp != NULL && newlen == sizeof(int)) {
-                       switch (*(int *)newp) {
-                       case IPSEC_POLICY_DISCARD:
-                       case IPSEC_POLICY_NONE:
-                               break;
-                       default:
-                               return EINVAL;
-                       }
-               }
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_def_policy.policy);
-       case IPSECCTL_DEF_ESP_TRANSLEV:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_esp_trans_deflev);
-       case IPSECCTL_DEF_ESP_NETLEV:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_esp_net_deflev);
-       case IPSECCTL_DEF_AH_TRANSLEV:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_ah_trans_deflev);
-       case IPSECCTL_DEF_AH_NETLEV:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_ah_net_deflev);
-       case IPSECCTL_INBOUND_CALL_IKE:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_inbound_call_ike);
-       case IPSECCTL_AH_CLEARTOS:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_ah_cleartos);
-       case IPSECCTL_AH_OFFSETMASK:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_ah_offsetmask);
-       case IPSECCTL_DFBIT:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip4_ipsec_dfbit);
-       case IPSECCTL_ECN:
-               return sysctl_int(oldp, oldlenp, newp, newlen, &ip4_ipsec_ecn);
-       case IPSECCTL_DEBUG:
-               return sysctl_int(oldp, oldlenp, newp, newlen, &ipsec_debug);
-       default:
-               return EOPNOTSUPP;
+       /* if so == NULL, don't insist on getting the aux mbuf */
+       if (so) {
+               tag = ipsec_addaux(m);
+               if (!tag)
+                       return ENOBUFS;
+       } else
+               tag = ipsec_findaux(m);
+       if (tag) {
+               tag->socket = so;
+               ipsec_optaux(m, tag);
        }
-       /* NOTREACHED */
+       return 0;
 }
 
-#if INET6
-/*
- * System control for IPSEC6
- */
-u_char ipsec6ctlermap[PRC_NCMDS] = {
-       0,              0,              0,              0,
-       0,              EMSGSIZE,       EHOSTDOWN,      EHOSTUNREACH,
-       EHOSTUNREACH,   EHOSTUNREACH,   ECONNREFUSED,   ECONNREFUSED,
-       EMSGSIZE,       EHOSTUNREACH,   0,              0,
-       0,              0,              0,              0,
-       ENOPROTOOPT
-};
-
-int *ipsec6_sysvars[] = IPSEC6CTL_VARS;
+struct socket *
+ipsec_getsocket(
+       struct mbuf *m)
+{
+       struct ipsec_tag        *itag;
+       
+       itag = ipsec_findaux(m);
+       if (itag)
+               return itag->socket;
+       else
+               return NULL;
+}
 
 int
-ipsec6_sysctl(name, namelen, oldp, oldlenp, newp, newlen)
-       int *name;
-       u_int namelen;
-       void *oldp;
-       size_t *oldlenp;
-       void *newp;
-       size_t newlen;
+ipsec_addhist(
+       struct mbuf *m,
+       int proto,
+       u_int32_t spi)
 {
-       /* All sysctl names at this level are terminal. */
-       if (namelen != 1)
-               return ENOTDIR;
-
-       /* common sanity checks */
-       switch (name[0]) {
-       case IPSECCTL_DEF_ESP_TRANSLEV:
-       case IPSECCTL_DEF_ESP_NETLEV:
-       case IPSECCTL_DEF_AH_TRANSLEV:
-       case IPSECCTL_DEF_AH_NETLEV:
-               if (newp != NULL && newlen == sizeof(int)) {
-                       switch (*(int *)newp) {
-                       case IPSEC_LEVEL_USE:
-                       case IPSEC_LEVEL_REQUIRE:
-                               break;
-                       default:
-                               return EINVAL;
-                       }
-               }
-       }
+       struct ipsec_tag                *itag;
+       struct ipsec_history    *p;
+       itag = ipsec_addaux(m);
+       if (!itag)
+               return ENOBUFS;
+       if (itag->history_count == IPSEC_HISTORY_MAX)
+               return ENOSPC;  /* XXX */
+       
+       p = &itag->history[itag->history_count];
+       itag->history_count++;
+       
+       bzero(p, sizeof(*p));
+       p->ih_proto = proto;
+       p->ih_spi = spi;
+       
+       return 0;
+}
 
-       switch (name[0]) {
+struct ipsec_history *
+ipsec_gethist(
+       struct mbuf *m,
+       int *lenp)
+{
+       struct ipsec_tag        *itag;
+       
+       itag = ipsec_findaux(m);
+       if (!itag)
+               return NULL;
+       if (itag->history_count == 0)
+               return NULL;
+       if (lenp)
+               *lenp = (int)(itag->history_count * sizeof(struct ipsec_history));
+       return itag->history;
+}
 
-       case IPSECCTL_STATS:
-               return sysctl_struct(oldp, oldlenp, newp, newlen,
-                                    &ipsec6stat, sizeof(ipsec6stat));
-       case IPSECCTL_DEF_POLICY:
-               if (newp != NULL && newlen == sizeof(int)) {
-                       switch (*(int *)newp) {
-                       case IPSEC_POLICY_DISCARD:
-                       case IPSEC_POLICY_NONE:
-                               break;
-                       default:
-                               return EINVAL;
-                       }
-               }
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip6_def_policy.policy);
-       case IPSECCTL_DEF_ESP_TRANSLEV:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip6_esp_trans_deflev);
-       case IPSECCTL_DEF_ESP_NETLEV:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip6_esp_net_deflev);
-       case IPSECCTL_DEF_AH_TRANSLEV:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip6_ah_trans_deflev);
-       case IPSECCTL_DEF_AH_NETLEV:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip6_ah_net_deflev);
-       case IPSECCTL_INBOUND_CALL_IKE:
-               return sysctl_int(oldp, oldlenp, newp, newlen,
-                                 &ip6_inbound_call_ike);
-       case IPSECCTL_ECN:
-               return sysctl_int(oldp, oldlenp, newp, newlen, &ip6_ipsec_ecn);
-       case IPSECCTL_DEBUG:
-               return sysctl_int(oldp, oldlenp, newp, newlen, &ipsec_debug);
-       default:
-               return EOPNOTSUPP;
+void
+ipsec_clearhist(
+       struct mbuf *m)
+{
+       struct ipsec_tag        *itag;
+       
+       itag = ipsec_findaux(m);
+       if (itag) {
+               itag->history_count = 0;
        }
-       /* NOTREACHED */
+       ipsec_optaux(m, itag);
 }
-#endif /*INET6*/
 
-#endif /* __NetBSD__ */
+__private_extern__ void
+ipsec_send_natt_keepalive(
+       struct secasvar *sav)
+{
+       struct mbuf     *m;
+       struct udphdr *uh;
+       struct ip *ip;
+       int error;
+
+       lck_mtx_assert(sadb_mutex, LCK_MTX_ASSERT_NOTOWNED);
+       
+       if ((esp_udp_encap_port & 0xFFFF) == 0 || sav->remote_ike_port == 0) return;
+       
+       m = m_gethdr(M_NOWAIT, MT_DATA);
+       if (m == NULL) return;
+       
+       /*
+        * Create a UDP packet complete with IP header.
+        * We must do this because UDP output requires
+        * an inpcb which we don't have. UDP packet
+        * contains one byte payload. The byte is set
+        * to 0xFF.
+        */
+       ip = (struct ip*)m_mtod(m);
+       uh = (struct udphdr*)((char*)m_mtod(m) + sizeof(struct ip));
+       m->m_len = sizeof(struct udpiphdr) + 1;
+       bzero(m_mtod(m), 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 = 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;
+       
+       error = ip_output(m, NULL, &sav->sah->sa_route, IP_NOIPSEC, NULL, NULL);
+       if (error == 0)
+               sav->natt_last_activity = natt_now;
+
+}