1 files changed, 53 insertions, 52 deletions

diff --git a/sys/netipsec/ipsec_mbuf.c b/sys/netipsec/ipsec_mbuf.c
index 322df11a1efb..d813c923a101 100644
--- a/sys/netipsec/ipsec_mbuf.c
+++ b/sys/netipsec/ipsec_mbuf.c
@@ -75,66 +75,67 @@ m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
 	 */
 	remain = m->m_len - skip;		/* data to move */
 	if (hlen > M_TRAILINGSPACE(m)) {
-		struct mbuf *n;
+		struct mbuf *n0, *n, **np;
+		int todo, len, done, alloc;
+
+		n0 = NULL;
+		np = &n0;
+		alloc = 0;
+		done = 0;
+		todo = remain;
+		while (todo > 0) {
+			if (todo > MHLEN) {
+				n = m_getcl(M_DONTWAIT, m->m_type, 0);
+				len = MCLBYTES;
+			}
+			else {
+				n = m_get(M_DONTWAIT, m->m_type);
+				len = MHLEN;
+			}
+			if (n == NULL) {
+				m_freem(n0);
+				return NULL;
+			}
+			*np = n;
+			np = &n->m_next;
+			alloc++;
+			len = min(todo, len);
+			memcpy(n->m_data, mtod(m, char *) + skip + done, len);
+			n->m_len = len;
+			done += len;
+			todo -= len;
+		}
 
-		/* XXX code doesn't handle clusters XXX */
-		IPSEC_ASSERT(remain < MLEN, ("remainder too big: %u", remain));
-		/*
-		 * Not enough space in m, split the contents
-		 * of m, inserting new mbufs as required.
-		 *
-		 * NB: this ignores mbuf types.
-		 */
-		MGET(n, M_DONTWAIT, MT_DATA);
-		if (n == NULL)
-			return (NULL);
-		n->m_next = m->m_next;		/* splice new mbuf */
-		m->m_next = n;
-		V_ipsec4stat.ips_mbinserted++;
 		if (hlen <= M_TRAILINGSPACE(m) + remain) {
-			/*
-			 * New header fits in the old mbuf if we copy
-			 * the remainder; just do the copy to the new
-			 * mbuf and we're good to go.
-			 */
-			memcpy(mtod(n, caddr_t),
-			       mtod(m, caddr_t) + skip, remain);
-			n->m_len = remain;
 			m->m_len = skip + hlen;
 			*off = skip;
-		} else {
-			/*
-			 * No space in the old mbuf for the new header.
-			 * Make space in the new mbuf and check the
-			 * remainder'd data fits too.  If not then we
-			 * must allocate an additional mbuf (yech).
-			 */
-			n->m_len = 0;
-			if (remain + hlen > M_TRAILINGSPACE(n)) {
-				struct mbuf *n2;
-
-				MGET(n2, M_DONTWAIT, MT_DATA);
-				/* NB: new mbuf is on chain, let caller free */
-				if (n2 == NULL)
-					return (NULL);
-				n2->m_len = 0;
-				memcpy(mtod(n2, caddr_t),
-				       mtod(m, caddr_t) + skip, remain);
-				n2->m_len = remain;
-				/* splice in second mbuf */
-				n2->m_next = n->m_next;
-				n->m_next = n2;
-				V_ipsec4stat.ips_mbinserted++;
-			} else {
-				memcpy(mtod(n, caddr_t) + hlen,
-				       mtod(m, caddr_t) + skip, remain);
-				n->m_len += remain;
+			if (n0 != NULL) {
+				*np = m->m_next;
+				m->m_next = n0;
 			}
-			m->m_len -= remain;
-			n->m_len += hlen;
+		}
+		else {
+			n = m_get(M_DONTWAIT, m->m_type);
+			if (n == NULL) {
+				m_freem(n0);
+				return NULL;
+			}
+			alloc++;
+
+			if ((n->m_next = n0) == NULL)
+				np = &n->m_next;
+			n0 = n;
+
+			*np = m->m_next;
+			m->m_next = n0;
+
+			n->m_len = hlen;
+			m->m_len = skip;
+
 			m = n;			/* header is at front ... */
 			*off = 0;		/* ... of new mbuf */
 		}
+		V_ipsec4stat.ips_mbinserted++;
 	} else {
 		/*
 		 * Copy the remainder to the back of the mbuf