 @@ -1,14 +1,14 @@
-/*	$NetBSD: if_ipsec.c,v 1.6 2018/03/09 11:03:26 knakahara Exp $  */
+/*	$NetBSD: if_ipsec.c,v 1.7 2018/03/13 02:12:05 knakahara Exp $  */
 /*
  * Copyright (c) 2017 Internet Initiative Japan Inc.
  * All rights reserved.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  * 1. Redistributions of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright
  *    notice, this list of conditions and the following disclaimer in the
  *    documentation and/or other materials provided with the distribution.
 @@ -17,27 +17,27 @@
  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: if_ipsec.c,v 1.6 2018/03/09 11:03:26 knakahara Exp $");
+__KERNEL_RCSID(0, "$NetBSD: if_ipsec.c,v 1.7 2018/03/13 02:12:05 knakahara Exp $");
 #ifdef _KERNEL_OPT
 #include "opt_inet.h"
 #endif
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/kernel.h>
 #include <sys/mbuf.h>
 #include <sys/socket.h>
 #include <sys/sockio.h>
 #include <sys/errno.h>
 #include <sys/ioctl.h>
 @@ -1300,47 +1300,57 @@ if_ipsec_unshare_sp(struct ipsec_variant
 	mutex_exit(&ipsec_softcs.lock);
 	if (sc2 == NULL)
 		return 0; /* not shared */
 	IV_SP_IN(var) = NULL;
 	IV_SP_IN6(var) = NULL;
 	IV_SP_OUT(var) = NULL;
 	IV_SP_OUT6(var) = NULL;
 	if_ipsec_putref_variant(var2, &psref);
 	return 1; /* shared */
+}
 static inline void
-if_ipsec_add_mbuf(struct mbuf *m0, void *data, size_t len)
+if_ipsec_add_mbuf_optalign(struct mbuf *m0, void *data, size_t len, bool align)
+{
 	struct mbuf *m;
 	MGET(m, M_WAITOK | M_ZERO, MT_DATA);
-	m->m_len = PFKEY_ALIGN8(len);
+	if (align)
 		m->m_len = PFKEY_ALIGN8(len);
 	else
 		m->m_len = len;
 	m_copyback(m, 0, len, data);
 	m_cat(m0, m);
+}
 static inline void
-if_ipsec_add_mbuf_addr_port(struct mbuf *m0, struct sockaddr *addr, in_port_t port)
+if_ipsec_add_mbuf(struct mbuf *m0, void *data, size_t len)
+{
 	if_ipsec_add_mbuf_optalign(m0, data, len, true);
+}
 static inline void
 if_ipsec_add_mbuf_addr_port(struct mbuf *m0, struct sockaddr *addr, in_port_t port, bool align)
+{
 	if (port == 0) {
-		if_ipsec_add_mbuf(m0, addr, addr->sa_len);
+		if_ipsec_add_mbuf_optalign(m0, addr, addr->sa_len, align);
 	} else {
 		struct sockaddr addrport;
 		if_ipsec_set_addr_port(&addrport, addr, port);
-		if_ipsec_add_mbuf(m0, &addrport, addrport.sa_len);
+		if_ipsec_add_mbuf_optalign(m0, &addrport, addrport.sa_len, align);
+	}
+}
 static inline void
 if_ipsec_add_pad(struct mbuf *m0, size_t len)
+{
 	struct mbuf *m;
 	if (len == 0)
 		return;
 	MGET(m, M_WAITOK | M_ZERO, MT_DATA);
 	m->m_len = len;
 @@ -1402,45 +1412,42 @@ if_ipsec_set_sadb_dst(struct sadb_addres
 static inline size_t
 if_ipsec_set_sadb_x_policy(struct sadb_x_policy *xpl,
     struct sadb_x_ipsecrequest *xisr, uint16_t policy, uint8_t dir, uint32_t id,
     uint8_t level, struct sockaddr *src, struct sockaddr *dst)
+{
 	size_t size;
 	KASSERT(policy != IPSEC_POLICY_IPSEC || xisr != NULL);
 	size = sizeof(*xpl);
 	if (policy == IPSEC_POLICY_IPSEC) {
 		size += PFKEY_ALIGN8(sizeof(*xisr));
-		if (src != NULL)
+		if (src != NULL && dst != NULL)
-			size += PFKEY_ALIGN8(src->sa_len);
+			size += PFKEY_ALIGN8(src->sa_len + dst->sa_len);
 		if (dst != NULL)
 			size += PFKEY_ALIGN8(dst->sa_len);
+	}
 	xpl->sadb_x_policy_len = PFKEY_UNIT64(size);
 	xpl->sadb_x_policy_exttype = SADB_X_EXT_POLICY;
 	xpl->sadb_x_policy_type = policy;
 	xpl->sadb_x_policy_dir = dir;
 	xpl->sadb_x_policy_reserved = 0;
 	xpl->sadb_x_policy_id = id;
 	xpl->sadb_x_policy_reserved2 = 0;
 	if (policy == IPSEC_POLICY_IPSEC) {
 		xisr->sadb_x_ipsecrequest_len = PFKEY_ALIGN8(sizeof(*xisr));
-		if (src != NULL)
+		if (src != NULL && dst != NULL)
-			xisr->sadb_x_ipsecrequest_len += PFKEY_ALIGN8(src->sa_len);
+			xisr->sadb_x_ipsecrequest_len +=
-		if (dst != NULL)
+				PFKEY_ALIGN8(src->sa_len + dst->sa_len);
 			xisr->sadb_x_ipsecrequest_len += PFKEY_ALIGN8(dst->sa_len);
 		xisr->sadb_x_ipsecrequest_proto = IPPROTO_ESP;
 		xisr->sadb_x_ipsecrequest_mode = IPSEC_MODE_TRANSPORT;
 		xisr->sadb_x_ipsecrequest_level = level;
 		xisr->sadb_x_ipsecrequest_reqid = key_newreqid();
+	}
 	return size;
+}
 static inline void
 if_ipsec_set_sadb_msg(struct sadb_msg *msg, uint16_t extlen, uint8_t msgtype)
+{
 @@ -1529,48 +1536,46 @@ if_ipsec_add_sp0(struct sockaddr *src, i
 	ext_msg_len += PFKEY_UNIT64(size);
 	size = if_ipsec_set_sadb_dst(&xdst, dst, proto);
 	ext_msg_len += PFKEY_UNIT64(size);
 	size = if_ipsec_set_sadb_x_policy(&xpl, &xisr, policy, dir, 0, level, src, dst);
 	ext_msg_len += PFKEY_UNIT64(size);
 	if_ipsec_set_sadb_msg_add(&msg, ext_msg_len);
 	/* build PF_KEY message */
 	m->m_len = sizeof(msg);
 	m_copyback(m, 0, sizeof(msg), &msg);
 	if_ipsec_add_mbuf(m, &xsrc, sizeof(xsrc));
-	if_ipsec_add_mbuf_addr_port(m, src, sport);
+	if_ipsec_add_mbuf_addr_port(m, src, sport, true);
 	padlen = PFKEY_UNUNIT64(xsrc.sadb_address_len)
 		- (sizeof(xsrc) + PFKEY_ALIGN8(src->sa_len));
 	if_ipsec_add_pad(m, padlen);
 	if_ipsec_add_mbuf(m, &xdst, sizeof(xdst));
-	if_ipsec_add_mbuf_addr_port(m, dst, dport);
+	if_ipsec_add_mbuf_addr_port(m, dst, dport, true);
 	padlen = PFKEY_UNUNIT64(xdst.sadb_address_len)
 		- (sizeof(xdst) + PFKEY_ALIGN8(dst->sa_len));
 	if_ipsec_add_pad(m, padlen);
 	if_ipsec_add_mbuf(m, &xpl, sizeof(xpl));
 	if (policy == IPSEC_POLICY_IPSEC) {
 		if_ipsec_add_mbuf(m, &xisr, sizeof(xisr));
-		if_ipsec_add_mbuf_addr_port(m, src, sport);
+		if_ipsec_add_mbuf_addr_port(m, src, sport, false);
-		if_ipsec_add_mbuf_addr_port(m, dst, dport);
+		if_ipsec_add_mbuf_addr_port(m, dst, dport, false);
+	}
 	padlen = PFKEY_UNUNIT64(xpl.sadb_x_policy_len) - sizeof(xpl);
-	if (src != NULL)
+	if (src != NULL && dst != NULL)
-		padlen -= PFKEY_ALIGN8(src->sa_len);
+		padlen -= PFKEY_ALIGN8(src->sa_len + dst->sa_len);
 	if (dst != NULL)
 		padlen -= PFKEY_ALIGN8(dst->sa_len);
 	if_ipsec_add_pad(m, padlen);
 	/* key_kpi_spdadd() has already done KEY_SP_REF(). */
 	return key_kpi_spdadd(m);
+}
 static int
 if_ipsec_add_sp(struct ipsec_variant *var,
     struct sockaddr *src, in_port_t sport,
     struct sockaddr *dst, in_port_t dport)
+{
 	struct ipsec_softc *sc = var->iv_softc;
 	int level;