xref: /freebsd/sys/netipsec/ipsec_mbuf.c (revision fcf596178b5f2be36424ecbc1b6a3224b29c91d2)
1aaea26efSSam Leffler /*-
2aaea26efSSam Leffler  * Copyright (c) 2002, 2003 Sam Leffler, Errno Consulting
3aaea26efSSam Leffler  * All rights reserved.
4aaea26efSSam Leffler  *
5aaea26efSSam Leffler  * Redistribution and use in source and binary forms, with or without
6aaea26efSSam Leffler  * modification, are permitted provided that the following conditions
7aaea26efSSam Leffler  * are met:
8aaea26efSSam Leffler  * 1. Redistributions of source code must retain the above copyright
9aaea26efSSam Leffler  *    notice, this list of conditions and the following disclaimer.
10aaea26efSSam Leffler  * 2. Redistributions in binary form must reproduce the above copyright
11aaea26efSSam Leffler  *    notice, this list of conditions and the following disclaimer in the
12aaea26efSSam Leffler  *    documentation and/or other materials provided with the distribution.
13aaea26efSSam Leffler  *
14aaea26efSSam Leffler  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15aaea26efSSam Leffler  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16aaea26efSSam Leffler  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17aaea26efSSam Leffler  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18aaea26efSSam Leffler  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19aaea26efSSam Leffler  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20aaea26efSSam Leffler  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21aaea26efSSam Leffler  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22aaea26efSSam Leffler  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23aaea26efSSam Leffler  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24aaea26efSSam Leffler  * SUCH DAMAGE.
25aaea26efSSam Leffler  *
26aaea26efSSam Leffler  * $FreeBSD$
27aaea26efSSam Leffler  */
2888768458SSam Leffler 
2988768458SSam Leffler /*
3088768458SSam Leffler  * IPsec-specific mbuf routines.
3188768458SSam Leffler  */
3288768458SSam Leffler 
3388768458SSam Leffler #include <sys/param.h>
3488768458SSam Leffler #include <sys/systm.h>
358ec07310SGleb Smirnoff #include <sys/malloc.h>
3688768458SSam Leffler #include <sys/mbuf.h>
3788768458SSam Leffler #include <sys/socket.h>
3888768458SSam Leffler 
39eddfbb76SRobert Watson #include <net/vnet.h>
4088768458SSam Leffler #include <netinet/in.h>
4188768458SSam Leffler #include <netipsec/ipsec.h>
4288768458SSam Leffler 
4388768458SSam Leffler /*
44972136faSSam Leffler  * Make space for a new header of length hlen at skip bytes
45972136faSSam Leffler  * into the packet.  When doing this we allocate new mbufs only
4688768458SSam Leffler  * when absolutely necessary.  The mbuf where the new header
4788768458SSam Leffler  * is to go is returned together with an offset into the mbuf.
4888768458SSam Leffler  * If NULL is returned then the mbuf chain may have been modified;
4988768458SSam Leffler  * the caller is assumed to always free the chain.
5088768458SSam Leffler  */
5188768458SSam Leffler struct mbuf *
5288768458SSam Leffler m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
5388768458SSam Leffler {
5488768458SSam Leffler 	struct mbuf *m;
5588768458SSam Leffler 	unsigned remain;
5688768458SSam Leffler 
579ffa9677SSam Leffler 	IPSEC_ASSERT(m0 != NULL, ("null mbuf"));
589ffa9677SSam Leffler 	IPSEC_ASSERT(hlen < MHLEN, ("hlen too big: %u", hlen));
5988768458SSam Leffler 
6088768458SSam Leffler 	for (m = m0; m && skip > m->m_len; m = m->m_next)
6188768458SSam Leffler 		skip -= m->m_len;
6288768458SSam Leffler 	if (m == NULL)
6388768458SSam Leffler 		return (NULL);
6488768458SSam Leffler 	/*
6588768458SSam Leffler 	 * At this point skip is the offset into the mbuf m
6688768458SSam Leffler 	 * where the new header should be placed.  Figure out
6788768458SSam Leffler 	 * if there's space to insert the new header.  If so,
68023795f0SBjoern A. Zeeb 	 * and copying the remainder makes sense then do so.
6988768458SSam Leffler 	 * Otherwise insert a new mbuf in the chain, splitting
7088768458SSam Leffler 	 * the contents of m as needed.
7188768458SSam Leffler 	 */
7288768458SSam Leffler 	remain = m->m_len - skip;		/* data to move */
73*fcf59617SAndrey V. Elsukov 	if (remain > skip &&
74*fcf59617SAndrey V. Elsukov 	    hlen + max_linkhdr < M_LEADINGSPACE(m)) {
75*fcf59617SAndrey V. Elsukov 		/*
76*fcf59617SAndrey V. Elsukov 		 * mbuf has enough free space at the beginning.
77*fcf59617SAndrey V. Elsukov 		 * XXX: which operation is the most heavy - copying of
78*fcf59617SAndrey V. Elsukov 		 *	possible several hundred of bytes or allocation
79*fcf59617SAndrey V. Elsukov 		 *	of new mbuf? We can remove max_linkhdr check
80*fcf59617SAndrey V. Elsukov 		 *	here, but it is possible that this will lead
81*fcf59617SAndrey V. Elsukov 		 *	to allocation of new mbuf in Layer 2 code.
82*fcf59617SAndrey V. Elsukov 		 */
83*fcf59617SAndrey V. Elsukov 		m->m_data -= hlen;
84*fcf59617SAndrey V. Elsukov 		bcopy(mtodo(m, hlen), mtod(m, caddr_t), skip);
85*fcf59617SAndrey V. Elsukov 		m->m_len += hlen;
86*fcf59617SAndrey V. Elsukov 		*off = skip;
87*fcf59617SAndrey V. Elsukov 	} else if (hlen > M_TRAILINGSPACE(m)) {
88357c11c9SVANHULLEBUS Yvan 		struct mbuf *n0, *n, **np;
89357c11c9SVANHULLEBUS Yvan 		int todo, len, done, alloc;
9088768458SSam Leffler 
91357c11c9SVANHULLEBUS Yvan 		n0 = NULL;
92357c11c9SVANHULLEBUS Yvan 		np = &n0;
93357c11c9SVANHULLEBUS Yvan 		alloc = 0;
94357c11c9SVANHULLEBUS Yvan 		done = 0;
95357c11c9SVANHULLEBUS Yvan 		todo = remain;
96357c11c9SVANHULLEBUS Yvan 		while (todo > 0) {
97357c11c9SVANHULLEBUS Yvan 			if (todo > MHLEN) {
98eb1b1807SGleb Smirnoff 				n = m_getcl(M_NOWAIT, m->m_type, 0);
99357c11c9SVANHULLEBUS Yvan 				len = MCLBYTES;
100357c11c9SVANHULLEBUS Yvan 			}
101357c11c9SVANHULLEBUS Yvan 			else {
102eb1b1807SGleb Smirnoff 				n = m_get(M_NOWAIT, m->m_type);
103357c11c9SVANHULLEBUS Yvan 				len = MHLEN;
104357c11c9SVANHULLEBUS Yvan 			}
105357c11c9SVANHULLEBUS Yvan 			if (n == NULL) {
106357c11c9SVANHULLEBUS Yvan 				m_freem(n0);
107357c11c9SVANHULLEBUS Yvan 				return NULL;
108357c11c9SVANHULLEBUS Yvan 			}
109357c11c9SVANHULLEBUS Yvan 			*np = n;
110357c11c9SVANHULLEBUS Yvan 			np = &n->m_next;
111357c11c9SVANHULLEBUS Yvan 			alloc++;
112357c11c9SVANHULLEBUS Yvan 			len = min(todo, len);
113357c11c9SVANHULLEBUS Yvan 			memcpy(n->m_data, mtod(m, char *) + skip + done, len);
114357c11c9SVANHULLEBUS Yvan 			n->m_len = len;
115357c11c9SVANHULLEBUS Yvan 			done += len;
116357c11c9SVANHULLEBUS Yvan 			todo -= len;
117357c11c9SVANHULLEBUS Yvan 		}
118357c11c9SVANHULLEBUS Yvan 
11988768458SSam Leffler 		if (hlen <= M_TRAILINGSPACE(m) + remain) {
12088768458SSam Leffler 			m->m_len = skip + hlen;
12188768458SSam Leffler 			*off = skip;
122357c11c9SVANHULLEBUS Yvan 			if (n0 != NULL) {
123357c11c9SVANHULLEBUS Yvan 				*np = m->m_next;
124357c11c9SVANHULLEBUS Yvan 				m->m_next = n0;
12588768458SSam Leffler 			}
126357c11c9SVANHULLEBUS Yvan 		}
127357c11c9SVANHULLEBUS Yvan 		else {
128eb1b1807SGleb Smirnoff 			n = m_get(M_NOWAIT, m->m_type);
129357c11c9SVANHULLEBUS Yvan 			if (n == NULL) {
130357c11c9SVANHULLEBUS Yvan 				m_freem(n0);
131357c11c9SVANHULLEBUS Yvan 				return NULL;
132357c11c9SVANHULLEBUS Yvan 			}
133357c11c9SVANHULLEBUS Yvan 			alloc++;
134357c11c9SVANHULLEBUS Yvan 
135357c11c9SVANHULLEBUS Yvan 			if ((n->m_next = n0) == NULL)
136357c11c9SVANHULLEBUS Yvan 				np = &n->m_next;
137357c11c9SVANHULLEBUS Yvan 			n0 = n;
138357c11c9SVANHULLEBUS Yvan 
139357c11c9SVANHULLEBUS Yvan 			*np = m->m_next;
140357c11c9SVANHULLEBUS Yvan 			m->m_next = n0;
141357c11c9SVANHULLEBUS Yvan 
142357c11c9SVANHULLEBUS Yvan 			n->m_len = hlen;
143357c11c9SVANHULLEBUS Yvan 			m->m_len = skip;
144357c11c9SVANHULLEBUS Yvan 
14588768458SSam Leffler 			m = n;			/* header is at front ... */
14688768458SSam Leffler 			*off = 0;		/* ... of new mbuf */
14788768458SSam Leffler 		}
1486659296cSAndrey V. Elsukov 		IPSECSTAT_INC(ips_mbinserted);
14988768458SSam Leffler 	} else {
15088768458SSam Leffler 		/*
15188768458SSam Leffler 		 * Copy the remainder to the back of the mbuf
15288768458SSam Leffler 		 * so there's space to write the new header.
15388768458SSam Leffler 		 */
1542aebee88SDag-Erling Smørgrav 		bcopy(mtod(m, caddr_t) + skip,
15588768458SSam Leffler 		    mtod(m, caddr_t) + skip + hlen, remain);
15688768458SSam Leffler 		m->m_len += hlen;
15788768458SSam Leffler 		*off = skip;
15888768458SSam Leffler 	}
15988768458SSam Leffler 	m0->m_pkthdr.len += hlen;		/* adjust packet length */
16088768458SSam Leffler 	return m;
16188768458SSam Leffler }
16288768458SSam Leffler 
16388768458SSam Leffler /*
16488768458SSam Leffler  * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header
16588768458SSam Leffler  * length is updated, and a pointer to the first byte of the padding
16688768458SSam Leffler  * (which is guaranteed to be all in one mbuf) is returned.
16788768458SSam Leffler  */
16888768458SSam Leffler caddr_t
16988768458SSam Leffler m_pad(struct mbuf *m, int n)
17088768458SSam Leffler {
17188768458SSam Leffler 	register struct mbuf *m0, *m1;
17288768458SSam Leffler 	register int len, pad;
17388768458SSam Leffler 	caddr_t retval;
17488768458SSam Leffler 
17588768458SSam Leffler 	if (n <= 0) {  /* No stupid arguments. */
1769ffa9677SSam Leffler 		DPRINTF(("%s: pad length invalid (%d)\n", __func__, n));
17788768458SSam Leffler 		m_freem(m);
17888768458SSam Leffler 		return NULL;
17988768458SSam Leffler 	}
18088768458SSam Leffler 
18188768458SSam Leffler 	len = m->m_pkthdr.len;
18288768458SSam Leffler 	pad = n;
18388768458SSam Leffler 	m0 = m;
18488768458SSam Leffler 
18588768458SSam Leffler 	while (m0->m_len < len) {
18688768458SSam Leffler 		len -= m0->m_len;
18788768458SSam Leffler 		m0 = m0->m_next;
18888768458SSam Leffler 	}
18988768458SSam Leffler 
19088768458SSam Leffler 	if (m0->m_len != len) {
1919ffa9677SSam Leffler 		DPRINTF(("%s: length mismatch (should be %d instead of %d)\n",
1929ffa9677SSam Leffler 			__func__, m->m_pkthdr.len,
1939ffa9677SSam Leffler 			m->m_pkthdr.len + m0->m_len - len));
19488768458SSam Leffler 
19588768458SSam Leffler 		m_freem(m);
19688768458SSam Leffler 		return NULL;
19788768458SSam Leffler 	}
19888768458SSam Leffler 
19988768458SSam Leffler 	/* Check for zero-length trailing mbufs, and find the last one. */
20088768458SSam Leffler 	for (m1 = m0; m1->m_next; m1 = m1->m_next) {
20188768458SSam Leffler 		if (m1->m_next->m_len != 0) {
2029ffa9677SSam Leffler 			DPRINTF(("%s: length mismatch (should be %d instead "
2039ffa9677SSam Leffler 				"of %d)\n", __func__,
20488768458SSam Leffler 				m->m_pkthdr.len,
20588768458SSam Leffler 				m->m_pkthdr.len + m1->m_next->m_len));
20688768458SSam Leffler 
20788768458SSam Leffler 			m_freem(m);
20888768458SSam Leffler 			return NULL;
20988768458SSam Leffler 		}
21088768458SSam Leffler 
21188768458SSam Leffler 		m0 = m1->m_next;
21288768458SSam Leffler 	}
21388768458SSam Leffler 
21488768458SSam Leffler 	if (pad > M_TRAILINGSPACE(m0)) {
21588768458SSam Leffler 		/* Add an mbuf to the chain. */
216eb1b1807SGleb Smirnoff 		MGET(m1, M_NOWAIT, MT_DATA);
217155d72c4SPedro F. Giffuni 		if (m1 == NULL) {
21888768458SSam Leffler 			m_freem(m0);
2199ffa9677SSam Leffler 			DPRINTF(("%s: unable to get extra mbuf\n", __func__));
22088768458SSam Leffler 			return NULL;
22188768458SSam Leffler 		}
22288768458SSam Leffler 
22388768458SSam Leffler 		m0->m_next = m1;
22488768458SSam Leffler 		m0 = m1;
22588768458SSam Leffler 		m0->m_len = 0;
22688768458SSam Leffler 	}
22788768458SSam Leffler 
22888768458SSam Leffler 	retval = m0->m_data + m0->m_len;
22988768458SSam Leffler 	m0->m_len += pad;
23088768458SSam Leffler 	m->m_pkthdr.len += pad;
23188768458SSam Leffler 
23288768458SSam Leffler 	return retval;
23388768458SSam Leffler }
23488768458SSam Leffler 
23588768458SSam Leffler /*
23688768458SSam Leffler  * Remove hlen data at offset skip in the packet.  This is used by
23788768458SSam Leffler  * the protocols strip protocol headers and associated data (e.g. IV,
23888768458SSam Leffler  * authenticator) on input.
23988768458SSam Leffler  */
24088768458SSam Leffler int
24188768458SSam Leffler m_striphdr(struct mbuf *m, int skip, int hlen)
24288768458SSam Leffler {
24388768458SSam Leffler 	struct mbuf *m1;
24488768458SSam Leffler 	int roff;
24588768458SSam Leffler 
24688768458SSam Leffler 	/* Find beginning of header */
24788768458SSam Leffler 	m1 = m_getptr(m, skip, &roff);
24888768458SSam Leffler 	if (m1 == NULL)
24988768458SSam Leffler 		return (EINVAL);
25088768458SSam Leffler 
25188768458SSam Leffler 	/* Remove the header and associated data from the mbuf. */
25288768458SSam Leffler 	if (roff == 0) {
25388768458SSam Leffler 		/* The header was at the beginning of the mbuf */
2546659296cSAndrey V. Elsukov 		IPSECSTAT_INC(ips_input_front);
25588768458SSam Leffler 		m_adj(m1, hlen);
25688768458SSam Leffler 		if ((m1->m_flags & M_PKTHDR) == 0)
25788768458SSam Leffler 			m->m_pkthdr.len -= hlen;
25888768458SSam Leffler 	} else if (roff + hlen >= m1->m_len) {
25988768458SSam Leffler 		struct mbuf *mo;
26088768458SSam Leffler 
26188768458SSam Leffler 		/*
26288768458SSam Leffler 		 * Part or all of the header is at the end of this mbuf,
26388768458SSam Leffler 		 * so first let's remove the remainder of the header from
26488768458SSam Leffler 		 * the beginning of the remainder of the mbuf chain, if any.
26588768458SSam Leffler 		 */
2666659296cSAndrey V. Elsukov 		IPSECSTAT_INC(ips_input_end);
26788768458SSam Leffler 		if (roff + hlen > m1->m_len) {
26888768458SSam Leffler 			/* Adjust the next mbuf by the remainder */
26988768458SSam Leffler 			m_adj(m1->m_next, roff + hlen - m1->m_len);
27088768458SSam Leffler 
27188768458SSam Leffler 			/* The second mbuf is guaranteed not to have a pkthdr... */
27288768458SSam Leffler 			m->m_pkthdr.len -= (roff + hlen - m1->m_len);
27388768458SSam Leffler 		}
27488768458SSam Leffler 
27588768458SSam Leffler 		/* Now, let's unlink the mbuf chain for a second...*/
27688768458SSam Leffler 		mo = m1->m_next;
27788768458SSam Leffler 		m1->m_next = NULL;
27888768458SSam Leffler 
27988768458SSam Leffler 		/* ...and trim the end of the first part of the chain...sick */
28088768458SSam Leffler 		m_adj(m1, -(m1->m_len - roff));
28188768458SSam Leffler 		if ((m1->m_flags & M_PKTHDR) == 0)
28288768458SSam Leffler 			m->m_pkthdr.len -= (m1->m_len - roff);
28388768458SSam Leffler 
28488768458SSam Leffler 		/* Finally, let's relink */
28588768458SSam Leffler 		m1->m_next = mo;
28688768458SSam Leffler 	} else {
28788768458SSam Leffler 		/*
28888768458SSam Leffler 		 * The header lies in the "middle" of the mbuf; copy
28988768458SSam Leffler 		 * the remainder of the mbuf down over the header.
29088768458SSam Leffler 		 */
2916659296cSAndrey V. Elsukov 		IPSECSTAT_INC(ips_input_middle);
29288768458SSam Leffler 		bcopy(mtod(m1, u_char *) + roff + hlen,
29388768458SSam Leffler 		      mtod(m1, u_char *) + roff,
29488768458SSam Leffler 		      m1->m_len - (roff + hlen));
29588768458SSam Leffler 		m1->m_len -= hlen;
29688768458SSam Leffler 		m->m_pkthdr.len -= hlen;
29788768458SSam Leffler 	}
29888768458SSam Leffler 	return (0);
29988768458SSam Leffler }
30088768458SSam Leffler 
30188768458SSam Leffler /*
30288768458SSam Leffler  * Diagnostic routine to check mbuf alignment as required by the
30388768458SSam Leffler  * crypto device drivers (that use DMA).
30488768458SSam Leffler  */
30588768458SSam Leffler void
30688768458SSam Leffler m_checkalignment(const char* where, struct mbuf *m0, int off, int len)
30788768458SSam Leffler {
30888768458SSam Leffler 	int roff;
30988768458SSam Leffler 	struct mbuf *m = m_getptr(m0, off, &roff);
31088768458SSam Leffler 	caddr_t addr;
31188768458SSam Leffler 
31288768458SSam Leffler 	if (m == NULL)
31388768458SSam Leffler 		return;
31488768458SSam Leffler 	printf("%s (off %u len %u): ", where, off, len);
31588768458SSam Leffler 	addr = mtod(m, caddr_t) + roff;
31688768458SSam Leffler 	do {
31788768458SSam Leffler 		int mlen;
31888768458SSam Leffler 
31988768458SSam Leffler 		if (((uintptr_t) addr) & 3) {
32088768458SSam Leffler 			printf("addr misaligned %p,", addr);
32188768458SSam Leffler 			break;
32288768458SSam Leffler 		}
32388768458SSam Leffler 		mlen = m->m_len;
32488768458SSam Leffler 		if (mlen > len)
32588768458SSam Leffler 			mlen = len;
32688768458SSam Leffler 		len -= mlen;
32788768458SSam Leffler 		if (len && (mlen & 3)) {
32888768458SSam Leffler 			printf("len mismatch %u,", mlen);
32988768458SSam Leffler 			break;
33088768458SSam Leffler 		}
33188768458SSam Leffler 		m = m->m_next;
33288768458SSam Leffler 		addr = m ? mtod(m, caddr_t) : NULL;
33388768458SSam Leffler 	} while (m && len > 0);
33488768458SSam Leffler 	for (m = m0; m; m = m->m_next)
33588768458SSam Leffler 		printf(" [%p:%u]", mtod(m, caddr_t), m->m_len);
33688768458SSam Leffler 	printf("\n");
33788768458SSam Leffler }
338