Index: sys/netipsec/ipsec.c
===================================================================
--- sys/netipsec/ipsec.c	(wersja 203490)
+++ sys/netipsec/ipsec.c	(kopia robocza)
@@ -124,6 +124,11 @@
  *  0	take anything
  */
 VNET_DEFINE(int, crypto_support) = CRYPTOCAP_F_HARDWARE | CRYPTOCAP_F_SOFTWARE;
+/*
+ * Use source/destination port for hash calculation when load-balancing
+ * forwarded packets.
+ */
+static int ipsec_useport = 0;
 
 SYSCTL_DECL(_net_inet_ipsec);
 
@@ -164,6 +169,9 @@
 SYSCTL_VNET_STRUCT(_net_inet_ipsec, OID_AUTO, ipsecstats,
 	CTLFLAG_RD, &VNET_NAME(ipsec4stat), ipsecstat,	
 	"IPsec IPv4 statistics.");
+SYSCTL_INT(_net_inet_ipsec, OID_AUTO,
+	useport,	CTLFLAG_RW,	&ipsec_useport,0,
+	"Use ports for load-balancing forwarded packets");
 
 #ifdef REGRESSION
 /*
@@ -417,9 +425,14 @@
 
 	sp = NULL;
 	if (key_havesp(dir)) {
+		int needport;
+
+		if (ipsec_useport)
+			needport = 1;
+		else
+			needport = (flag & IP_FORWARDING) ? 0 : 1;
 		/* Make an index to look for a policy. */
-		*error = ipsec_setspidx(m, &spidx,
-					(flag & IP_FORWARDING) ? 0 : 1);
+		*error = ipsec_setspidx(m, &spidx, needport);
 		if (*error != 0) {
 			DPRINTF(("%s: setpidx failed, dir %u flag %u\n",
 				__func__, dir, flag));
Index: sys/netipsec/key.c
===================================================================
--- sys/netipsec/key.c	(wersja 203490)
+++ sys/netipsec/key.c	(kopia robocza)
@@ -327,6 +327,10 @@
 SYSCTL_VNET_INT(_net_key, KEYCTL_PREFERED_OLDSA,
 	preferred_oldsa, CTLFLAG_RW, &VNET_NAME(key_preferred_oldsa),	0, "");
 
+static int key_spdchecksad = 0;
+SYSCTL_INT(_net_key, OID_AUTO, spdchecksad, CTLFLAG_RW, &key_spdchecksad, 0,
+    "When selecting SPD to send packet check if SAD exist");
+
 #define __LIST_CHAINED(elm) \
 	(!((elm)->chain.le_next == NULL && (elm)->chain.le_prev == NULL))
 #define LIST_INSERT_TAIL(head, elm, type, field) \
@@ -413,6 +417,8 @@
 static struct secasvar *key_do_allocsa_policy __P((struct secashead *, u_int));
 static void key_delsp __P((struct secpolicy *));
 static struct secpolicy *key_getsp __P((struct secpolicyindex *));
+static struct secpolicy *key_getsp2 __P((struct secpolicyindex *spidx,
+	struct secpolicy *csp));
 static void _key_delsp(struct secpolicy *sp);
 static struct secpolicy *key_getspbyid __P((u_int32_t));
 static u_int32_t key_newreqid __P((void));
@@ -603,6 +609,29 @@
 		LIST_FIRST(&V_sptree[dir]) != NULL : 1);
 }
 
+static int
+key_sphassa(struct secpolicy *sp)
+{
+	struct ipsecrequest *req;
+	struct secashead *sah;
+
+	/* We are interested in IPsec only. */
+	if (sp->policy != IPSEC_POLICY_IPSEC)
+		return (1);
+	req = sp->req;
+	if (req == NULL)
+		return (0);
+	/* Check if SPD has a matching SAD. */
+	sah = key_getsah(&req->saidx);
+	if (sah == NULL)
+		return (0);
+	if (LIST_EMPTY(&sah->savtree[SADB_SASTATE_LARVAL]) &&
+	    LIST_EMPTY(&sah->savtree[SADB_SASTATE_MATURE])) {
+		return (0);
+	}
+	return (1);
+}
+
 /* %%% IPsec policy management */
 /*
  * allocating a SP for OUTBOUND or INBOUND packet.
@@ -613,7 +642,10 @@
 struct secpolicy *
 key_allocsp(struct secpolicyindex *spidx, u_int dir, const char* where, int tag)
 {
-	struct secpolicy *sp;
+#define	MAX_SPS	64
+	struct secpolicy *sp, *sps[MAX_SPS];
+	u_int nsps;
+	int spdchecksad;
 
 	IPSEC_ASSERT(spidx != NULL, ("null spidx"));
 	IPSEC_ASSERT(dir == IPSEC_DIR_INBOUND || dir == IPSEC_DIR_OUTBOUND,
@@ -627,7 +659,11 @@
 		printf("*** objects\n");
 		kdebug_secpolicyindex(spidx));
 
+	nsps = 0;
+	spdchecksad = key_spdchecksad;
+
 	SPTREE_LOCK();
+again:
 	LIST_FOREACH(sp, &V_sptree[dir], chain) {
 		KEYDEBUG(KEYDEBUG_IPSEC_DATA,
 			printf("*** in SPD\n");
@@ -635,11 +671,53 @@
 
 		if (sp->state == IPSEC_SPSTATE_DEAD)
 			continue;
-		if (key_cmpspidx_withmask(&sp->spidx, spidx))
-			goto found;
+		if (!key_cmpspidx_withmask(&sp->spidx, spidx))
+			continue;
+		if (spdchecksad && !key_sphassa(sp))
+			continue;
+		sps[nsps++] = sp;
+		if (nsps >= MAX_SPS)
+			break;
+		if (sp->policy != IPSEC_POLICY_IPSEC)
+			break;
 	}
-	sp = NULL;
-found:
+	if (nsps == 0) {
+		if (spdchecksad) {
+			/*
+			 * We found nothing, retry without checking SAD.
+			 */
+			spdchecksad = 0;
+			goto again;
+		}
+		sp = NULL;
+	} else if (nsps == 1)
+		sp = sps[0];
+	else if (nsps > 1) {
+		uint32_t ip;
+		uint16_t port;
+		u_char hash;
+		int ii;
+
+		/*
+		 * We build hash based on source and destination IP and ports.
+		 * It is important that we generate the same hash for both
+		 * directions.
+		 */
+		hash = 0;
+		ip = (uint32_t)spidx->src.sin.sin_addr.s_addr;
+		port = (uint16_t)spidx->src.sin.sin_port;
+		for (ii = 0; ii <= 1; ii++) {
+			hash += (ip & 0xff); ip >>= 8;
+			hash += (ip & 0xff); ip >>= 8;
+			hash += (ip & 0xff); ip >>= 8;
+			hash += (ip & 0xff);
+			hash += (port & 0xff); port >>= 8;
+			hash += (port & 0xff);
+			ip = (uint32_t)spidx->dst.sin.sin_addr.s_addr;
+			port = (uint16_t)spidx->dst.sin.sin_port;
+		}
+		sp = sps[hash % nsps];
+	}
 	if (sp) {
 		/* sanity check */
 		KEY_CHKSPDIR(sp->spidx.dir, dir, __func__);
@@ -1321,7 +1399,37 @@
 
 	return sp;
 }
+ 
+static struct secpolicy *
+key_getsp2(struct secpolicyindex *spidx, struct secpolicy *csp)
+{
+	struct secpolicy *sp;
 
+	IPSEC_ASSERT(spidx != NULL, ("null spidx"));
+	IPSEC_ASSERT(csp != NULL, ("null csp"));
+
+	SPTREE_LOCK();
+	LIST_FOREACH(sp, &V_sptree[spidx->dir], chain) {
+		if (sp->state == IPSEC_SPSTATE_DEAD)
+			continue;
+		if (!key_cmpspidx_exactly(spidx, &sp->spidx))
+			continue;
+		if (sp->req == NULL || csp->req == NULL)
+			continue;
+		if (key_sockaddrcmp(&sp->req->saidx.src.sa,
+		    &csp->req->saidx.src.sa, 0) != 0)
+			continue;
+		if (key_sockaddrcmp(&sp->req->saidx.dst.sa,
+		    &csp->req->saidx.dst.sa, 0) != 0)
+			continue;
+		SP_ADDREF(sp);
+		break;
+	}
+	SPTREE_UNLOCK();
+
+	return sp;
+}
+
 /*
  * get SP by index.
  * OUT:	NULL	: not found
@@ -1794,7 +1902,7 @@
 	struct sadb_x_policy *xpl0, *xpl;
 	struct sadb_lifetime *lft = NULL;
 	struct secpolicyindex spidx;
-	struct secpolicy *newsp;
+	struct secpolicy *newsp, *chksp;
 	int error;
 
 	IPSEC_ASSERT(so != NULL, ("null socket"));
@@ -1871,6 +1979,11 @@
 			__func__));
 		return key_senderror(so, m, EINVAL);
 	}
+ 
+	/* allocation new SP entry */
+	newsp = key_msg2sp(xpl0, PFKEY_EXTLEN(xpl0), &error);
+	if (newsp == NULL)
+		return key_senderror(so, m, error);
 
 	/*
 	 * checking there is SP already or not.
@@ -1878,26 +1991,20 @@
 	 * If the type is either SPDADD or SPDSETIDX AND a SP is found,
 	 * then error.
 	 */
-	newsp = key_getsp(&spidx);
-	if (mhp->msg->sadb_msg_type == SADB_X_SPDUPDATE) {
-		if (newsp) {
-			newsp->state = IPSEC_SPSTATE_DEAD;
-			KEY_FREESP(&newsp);
-		}
-	} else {
-		if (newsp != NULL) {
-			KEY_FREESP(&newsp);
-			ipseclog((LOG_DEBUG, "%s: a SP entry exists already.\n",
-				__func__));
-			return key_senderror(so, m, EEXIST);
-		}
-	}
+	chksp = key_getsp2(&spidx, newsp);
+ 	if (mhp->msg->sadb_msg_type == SADB_X_SPDUPDATE) {
+		if (chksp != NULL) {
+			chksp->state = IPSEC_SPSTATE_DEAD;
+			KEY_FREESP(&chksp);
+ 		}
+	} else if (chksp != NULL) {
+		_key_delsp(newsp);
+		KEY_FREESP(&chksp);
+		ipseclog((LOG_DEBUG, "%s: a SP entry exists already.\n",
+		    __func__));
+		return key_senderror(so, m, EEXIST);
+ 	}
 
-	/* allocation new SP entry */
-	if ((newsp = key_msg2sp(xpl0, PFKEY_EXTLEN(xpl0), &error)) == NULL) {
-		return key_senderror(so, m, error);
-	}
-
 	if ((newsp->id = key_getnewspid()) == 0) {
 		_key_delsp(newsp);
 		return key_senderror(so, m, ENOBUFS);