Index: net/pfkeyv2.h
===================================================================
--- net/pfkeyv2.h
+++ net/pfkeyv2.h
@@ -97,10 +97,11 @@
   u_int16_t sadb_sa_len;
   u_int16_t sadb_sa_exttype;
   u_int32_t sadb_sa_spi;
-  u_int8_t sadb_sa_replay;
+  u_int32_t sadb_sa_replay;
   u_int8_t sadb_sa_state;
   u_int8_t sadb_sa_auth;
   u_int8_t sadb_sa_encrypt;
+  u_int8_t sadb_sa_reserved[5];
   u_int32_t sadb_sa_flags;
 };
 
@@ -150,8 +151,7 @@
 struct sadb_prop {
   u_int16_t sadb_prop_len;
   u_int16_t sadb_prop_exttype;
-  u_int8_t sadb_prop_replay;
-  u_int8_t sadb_prop_reserved[3];
+  u_int32_t sadb_prop_replay;
 };
 
 struct sadb_comb {
Index: netipsec/ipsec.c
===================================================================
--- netipsec/ipsec.c
+++ netipsec/ipsec.c
@@ -251,7 +251,6 @@
 #endif
 static void ipsec_delpcbpolicy(struct inpcbpolicy *);
 static struct secpolicy *ipsec_deepcopy_policy(struct secpolicy *src);
-static void vshiftl(unsigned char *, int, int);
 
 MALLOC_DEFINE(M_IPSEC_INPCB, "inpcbpolicy", "inpcb-resident ipsec policy");
 
@@ -1476,57 +1475,70 @@
  * beforehand).
  * 0 (zero) is returned if packet disallowed, 1 if packet permitted.
  *
- * Based on RFC 2401.
+ * Based on RFC 6479. Blocks are 32 bits unsigned integers
  */
+
+#define IPSEC_BITMAP_INDEX_MASK(w)	(w - 1)
+#define IPSEC_REDUNDANT_BIT_SHIFTS	5
+#define IPSEC_REDUNDANT_BITS		(1 << IPSEC_REDUNDANT_BIT_SHIFTS)
+#define IPSEC_BITMAP_LOC_MASK		(IPSEC_REDUNDANT_BITS - 1)
+
 int
 ipsec_chkreplay(u_int32_t seq, struct secasvar *sav)
 {
 	const struct secreplay *replay;
-	u_int32_t diff;
-	int fr;
-	u_int32_t wsizeb;	/* Constant: bits of window size. */
-	int frlast;		/* Constant: last frame. */
+	u_int32_t wsizeb;		/* Constant: window size. */
+	int ret, index, bit_location;
 
 	IPSEC_ASSERT(sav != NULL, ("Null SA"));
 	IPSEC_ASSERT(sav->replay != NULL, ("Null replay state"));
 
+	SECASVAR_LOCK(sav);
+
+	ret = 0;
 	replay = sav->replay;
 
+	/* No need to check replay if disabled. */
 	if (replay->wsize == 0)
-		return (1);	/* No need to check replay. */
+		goto allowed;
 
 	/* Constant. */
-	frlast = replay->wsize - 1;
 	wsizeb = replay->wsize << 3;
 
 	/* Sequence number of 0 is invalid. */
 	if (seq == 0)
-		return (0);
+		goto end;
 
 	/* First time is always okay. */
 	if (replay->count == 0)
-		return (1);
-
-	if (seq > replay->lastseq) {
-		/* Larger sequences are okay. */
-		return (1);
-	} else {
-		/* seq is equal or less than lastseq. */
-		diff = replay->lastseq - seq;
+		goto allowed;
 
-		/* Over range to check, i.e. too old or wrapped. */
-		if (diff >= wsizeb)
-			return (0);
-
-		fr = frlast - diff / 8;
-
-		/* This packet already seen? */
-		if ((replay->bitmap)[fr] & (1 << (diff % 8)))
-			return (0);
+	/* Larger sequences are okay. */
+	if (seq > replay->lastseq)
+		goto allowed;
+
+	/* Over range to check, i.e. too old or wrapped. */
+	if (replay->lastseq - seq >= wsizeb)
+		goto end;
+
+	/* The sequence is inside the sliding window
+	 * now check the bit in the bitmap
+	 * bit location only depends on the sequence number
+	 */
+	bit_location = seq & IPSEC_BITMAP_LOC_MASK;
+	index = (seq >> IPSEC_REDUNDANT_BIT_SHIFTS)
+		& IPSEC_BITMAP_INDEX_MASK(replay->bitmap_size);
+
+	/* This packet already seen? */
+	if ((replay->bitmap)[index] & (1 << bit_location))
+		goto end;
+
+allowed:
+	ret = 1;
+end:
+	SECASVAR_UNLOCK(sav);
 
-		/* Out of order but good. */
-		return (1);
-	}
+	return (ret);
 }
 
 /*
@@ -1539,72 +1551,61 @@
 {
 	char buf[128];
 	struct secreplay *replay;
-	u_int32_t diff;
-	int fr;
-	u_int32_t wsizeb;	/* Constant: bits of window size. */
-	int frlast;		/* Constant: last frame. */
+	u_int32_t wsizeb;		/* Constant: window size. */
+	int ret, diff, index, bit_location;
 
 	IPSEC_ASSERT(sav != NULL, ("Null SA"));
 	IPSEC_ASSERT(sav->replay != NULL, ("Null replay state"));
 
+	SECASVAR_LOCK(sav);
+
+	ret = 1;
 	replay = sav->replay;
 
 	if (replay->wsize == 0)
 		goto ok;	/* No need to check replay. */
 
 	/* Constant. */
-	frlast = replay->wsize - 1;
 	wsizeb = replay->wsize << 3;
 
 	/* Sequence number of 0 is invalid. */
 	if (seq == 0)
-		return (1);
+		goto end;
 
-	/* First time. */
-	if (replay->count == 0) {
-		replay->lastseq = seq;
-		bzero(replay->bitmap, replay->wsize);
-		(replay->bitmap)[frlast] = 1;
+	/* The packet is too old, no need to update */
+	if (wsizeb + seq < replay->lastseq)
 		goto ok;
-	}
 
+	/* Now update the bit */
+	index = (seq >> IPSEC_REDUNDANT_BIT_SHIFTS);
+
+	/* First check if the sequence number is in the range */
 	if (seq > replay->lastseq) {
-		/* seq is larger than lastseq. */
-		diff = seq - replay->lastseq;
+		int id;
+		int index_cur = replay->lastseq >> IPSEC_REDUNDANT_BIT_SHIFTS;
 
-		/* New larger sequence number. */
-		if (diff < wsizeb) {
-			/* In window. */
-			/* Set bit for this packet. */
-			vshiftl(replay->bitmap, diff, replay->wsize);
-			(replay->bitmap)[frlast] |= 1;
-		} else {
-			/* This packet has a "way larger". */
-			bzero(replay->bitmap, replay->wsize);
-			(replay->bitmap)[frlast] = 1;
+		diff = index - index_cur;
+		if (diff > replay->bitmap_size) {
+			/* something unusual in this case */
+			diff = replay->bitmap_size;
 		}
-		replay->lastseq = seq;
 
-		/* Larger is good. */
-	} 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)
-			return (1);
+		for (id = 0; id < diff; ++id) {
+			replay->bitmap[(id + index_cur + 1)
+			& IPSEC_BITMAP_INDEX_MASK(replay->bitmap_size)] = 0;
+		}
 
-		fr = frlast - diff / 8;
+		replay->lastseq = seq;
+	}
 
-		/* This packet already seen? */
-		if ((replay->bitmap)[fr] & (1 << (diff % 8)))
-			return (1);
+	index &= IPSEC_BITMAP_INDEX_MASK(replay->bitmap_size);
+	bit_location = seq & IPSEC_BITMAP_LOC_MASK;
 
-		/* Mark as seen. */
-		(replay->bitmap)[fr] |= (1 << (diff % 8));
+	/* this packet has already been received */
+	if (replay->bitmap[index] & (1 << bit_location))
+		goto end;
 
-		/* Out of order but good. */
-	}
+	replay->bitmap[index] |= (1 << bit_location);
 
 ok:
 	if (replay->count == ~0) {
@@ -1614,39 +1615,18 @@
 
 		/* Don't increment, no more packets accepted. */
 		if ((sav->flags & SADB_X_EXT_CYCSEQ) == 0)
-			return (1);
+			goto end;
 
 		ipseclog((LOG_WARNING, "%s: replay counter made %d cycle. %s\n",
 		    __func__, replay->overflow,
 		    ipsec_logsastr(sav, buf, sizeof(buf))));
 	}
 
-	replay->count++;
-
-	return (0);
-}
+	ret = 0;
 
-/*
- * Shift variable length buffer to left.
- * IN:	bitmap: pointer to the buffer
- * 	nbit:	the number of to shift.
- *	wsize:	buffer size (bytes).
- */
-static void
-vshiftl(unsigned char *bitmap, int nbit, int wsize)
-{
-	int s, j, i;
-	unsigned char over;
-
-	for (j = 0; j < nbit; j += 8) {
-		s = (nbit - j < 8) ? (nbit - j): 8;
-		bitmap[0] <<= s;
-		for (i = 1; i < wsize; i++) {
-			over = (bitmap[i] >> (8 - s));
-			bitmap[i] <<= s;
-			bitmap[i-1] |= over;
-		}
-	}
+end:
+	SECASVAR_UNLOCK(sav);
+	return (ret);
 }
 
 /* Return a printable string for the address. */
Index: netipsec/key.c
===================================================================
--- netipsec/key.c
+++ netipsec/key.c
@@ -2940,6 +2940,8 @@
 		sav->sched = NULL;
 	}
 	if (sav->replay != NULL) {
+		if (sav->replay->bitmap != NULL)
+			free(sav->replay->bitmap, M_IPSEC_MISC);
 		free(sav->replay, M_IPSEC_MISC);
 		sav->replay = NULL;
 	}
@@ -3121,16 +3123,42 @@
 
 		/* replay window */
 		if ((sa0->sadb_sa_flags & SADB_X_EXT_OLD) == 0) {
+
 			sav->replay = (struct secreplay *)
-				malloc(sizeof(struct secreplay)+sa0->sadb_sa_replay, M_IPSEC_MISC, M_NOWAIT|M_ZERO);
+				malloc(sizeof(struct secreplay), M_IPSEC_MISC, M_NOWAIT|M_ZERO);
 			if (sav->replay == NULL) {
 				ipseclog((LOG_DEBUG, "%s: No more memory.\n",
 					__func__));
 				error = ENOBUFS;
 				goto fail;
 			}
-			if (sa0->sadb_sa_replay != 0)
-				sav->replay->bitmap = (caddr_t)(sav->replay+1);
+
+			if (sa0->sadb_sa_replay != 0) {
+				u_int32_t bitmap_size; /* number of 32b blocks to be allocated */
+
+				if (sa0->sadb_sa_replay > IPSEC_MAX_REPLAY_WSIZE) {
+					error = EINVAL;
+					goto fail;
+				}
+
+				/* RFC 6479:
+				 * - the allocated replay window size must be a power of two
+				 * - use an extra 32b block as a redundant window
+				 */
+				bitmap_size = 1;
+				while (sa0->sadb_sa_replay + 4 > bitmap_size)
+					bitmap_size <<= 1;
+				bitmap_size = bitmap_size/4;
+
+				sav->replay->bitmap = malloc(bitmap_size*sizeof(u_int32_t), M_IPSEC_MISC, M_NOWAIT|M_ZERO);
+				if (sav->replay->bitmap == NULL) {
+					ipseclog((LOG_DEBUG, "%s: No more memory.\n",
+						__func__));
+					error = ENOBUFS;
+					goto fail;
+				}
+				sav->replay->bitmap_size = bitmap_size;
+			}
 			sav->replay->wsize = sa0->sadb_sa_replay;
 		}
 	}
@@ -3405,6 +3433,7 @@
 {
 	struct mbuf *result = NULL, *tres = NULL, *m;
 	int i;
+	u_int32_t replay_count;
 	int dumporder[] = {
 		SADB_EXT_SA, SADB_X_EXT_SA2,
 		SADB_EXT_LIFETIME_HARD, SADB_EXT_LIFETIME_SOFT,
@@ -3435,8 +3464,10 @@
 			break;
 
 		case SADB_X_EXT_SA2:
-			m = key_setsadbxsa2(sav->sah->saidx.mode,
-					sav->replay ? sav->replay->count : 0,
+			SECASVAR_LOCK(sav);
+			replay_count = sav->replay ? sav->replay->count : 0;
+			SECASVAR_UNLOCK(sav);
+			m = key_setsadbxsa2(sav->sah->saidx.mode, replay_count,
 					sav->sah->saidx.reqid);
 			if (!m)
 				goto fail;
@@ -3634,7 +3665,9 @@
 	p->sadb_sa_len = PFKEY_UNIT64(len);
 	p->sadb_sa_exttype = SADB_EXT_SA;
 	p->sadb_sa_spi = sav->spi;
+	SECASVAR_LOCK(sav);
 	p->sadb_sa_replay = (sav->replay != NULL ? sav->replay->wsize : 0);
+	SECASVAR_UNLOCK(sav);
 	p->sadb_sa_state = sav->state;
 	p->sadb_sa_auth = sav->alg_auth;
 	p->sadb_sa_encrypt = sav->alg_enc;
@@ -6853,6 +6886,7 @@
 	int len;
 	int error = -1;
 	struct sadb_lifetime *lt;
+	u_int32_t replay_count;
 
 	IPSEC_ASSERT (sav != NULL, ("null sav"));
 	IPSEC_ASSERT (sav->sah != NULL, ("null sa header"));
@@ -6876,8 +6910,10 @@
 	m_cat(result, m);
 
 	/* create SA extension */
-	m = key_setsadbxsa2(sav->sah->saidx.mode,
-			sav->replay ? sav->replay->count : 0,
+	SECASVAR_LOCK(sav);
+	replay_count = sav->replay ? sav->replay->count : 0;
+	SECASVAR_UNLOCK(sav);
+	m = key_setsadbxsa2(sav->sah->saidx.mode, replay_count,
 			sav->sah->saidx.reqid);
 	if (!m) {
 		error = ENOBUFS;
Index: netipsec/key_debug.c
===================================================================
--- netipsec/key_debug.c
+++ netipsec/key_debug.c
@@ -570,8 +570,11 @@
 	if (sav->key_enc != NULL)
 		kdebug_sadb_key((struct sadb_ext *)sav->key_enc);
 
-	if (sav->replay != NULL)
+	if (sav->replay != NULL) {
+		SECASVAR_LOCK(sav);
 		kdebug_secreplay(sav->replay);
+		SECASVAR_UNLOCK(sav);
+	}
 	if (sav->lft_c != NULL)
 		kdebug_sec_lifetime(sav->lft_c);
 	if (sav->lft_h != NULL)
@@ -595,8 +598,8 @@
 	if (rpl == NULL)
 		panic("%s: NULL pointer was passed.\n", __func__);
 
-	printf(" secreplay{ count=%u wsize=%u seq=%u lastseq=%u",
-	    rpl->count, rpl->wsize, rpl->seq, rpl->lastseq);
+	printf(" secreplay{ count=%u bitmap_size=%u wsize=%u seq=%u lastseq=%u",
+	    rpl->count, rpl->bitmap_size, rpl->wsize, rpl->seq, rpl->lastseq);
 
 	if (rpl->bitmap == NULL) {
 		printf(" }\n");
@@ -605,7 +608,7 @@
 
 	printf("\n   bitmap { ");
 
-	for (len = 0; len < rpl->wsize; len++) {
+	for (len = 0; len < rpl->bitmap_size*4; len++) {
 		for (l = 7; l >= 0; l--)
 			printf("%u", (((rpl->bitmap)[len] >> l) & 1) ? 1 : 0);
 	}
Index: netipsec/keydb.h
===================================================================
--- netipsec/keydb.h
+++ netipsec/keydb.h
@@ -35,6 +35,8 @@
 
 #ifdef _KERNEL
 
+#include <sys/mutex.h>
+
 #include <netipsec/key_var.h>
 
 #ifndef _SOCKADDR_UNION_DEFINED
@@ -106,7 +108,10 @@
 struct auth_hash;
 struct comp_algo;
 
-/* Security Association */
+/* Security Association
+ *  (m) locked by mtx
+ *  (c) read only except during creation / free
+ */
 struct secasvar {
 	LIST_ENTRY(secasvar) chain;
 	struct mtx lock;		/* update/access lock */
@@ -127,7 +132,7 @@
 	size_t schedlen;
 	uint64_t cntr;			/* counter for GCM and CTR */
 
-	struct secreplay *replay;	/* replay prevention */
+	struct secreplay *replay;	/* (m) replay prevention */
 	time_t created;			/* for lifetime */
 
 	struct seclifetime *lft_c;	/* CURRENT lifetime, it's constant. */
@@ -171,12 +176,14 @@
 #define SAV_ISCTRORGCM(_sav)	(SAV_ISCTR((_sav)) || SAV_ISGCM((_sav)))
 
 /* replay prevention */
+#define IPSEC_MAX_REPLAY_WSIZE	536870908 /* (UINT32_MAX+1 - 32)/8 */
 struct secreplay {
 	u_int32_t count;
 	u_int wsize;		/* window size, i.g. 4 bytes */
 	u_int32_t seq;		/* used by sender */
 	u_int32_t lastseq;	/* used by receiver */
-	caddr_t bitmap;		/* used by receiver */
+	u_int32_t *bitmap;	/* used by receiver */
+	u_int bitmap_size;	/* size of the bitmap array */
 	int overflow;		/* overflow flag */
 };
 
Index: netipsec/xform_ah.c
===================================================================
--- netipsec/xform_ah.c
+++ netipsec/xform_ah.c
@@ -961,8 +961,11 @@
 
 	/* Insert packet replay counter, as requested.  */
 	if (sav->replay) {
+		SECASVAR_LOCK(sav);
+
 		if (sav->replay->count == ~0 &&
 		    (sav->flags & SADB_X_EXT_CYCSEQ) == 0) {
+			SECASVAR_UNLOCK(sav);
 			DPRINTF(("%s: replay counter wrapped for SA %s/%08lx\n",
 			    __func__, ipsec_address(&sav->sah->saidx.dst, buf,
 			    sizeof(buf)), (u_long) ntohl(sav->spi)));
@@ -976,6 +979,8 @@
 #endif
 			sav->replay->count++;
 		ah->ah_seq = htonl(sav->replay->count);
+
+		SECASVAR_UNLOCK(sav);
 	}
 
 	/* Get crypto descriptors. */
Index: netipsec/xform_esp.c
===================================================================
--- netipsec/xform_esp.c
+++ netipsec/xform_esp.c
@@ -762,12 +762,14 @@
 	if (sav->replay) {
 		u_int32_t replay;
 
+		SECASVAR_LOCK(sav);
 #ifdef REGRESSION
 		/* Emulate replay attack when ipsec_replay is TRUE. */
 		if (!V_ipsec_replay)
 #endif
 			sav->replay->count++;
 		replay = htonl(sav->replay->count);
+		SECASVAR_UNLOCK(sav);
 		bcopy((caddr_t) &replay,
 		    mtod(mo, caddr_t) + roff + sizeof(u_int32_t),
 		    sizeof(u_int32_t));