1/*-
2 * Copyright (c) 2002, 2003 Sam Leffler, Errno Consulting
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 *    notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 *    notice, this list of conditions and the following disclaimer in the
12 *    documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 * $FreeBSD$
27 */
28
29/*
30 * IPsec-specific mbuf routines.
31 */
32
33#include "opt_param.h"
34
35#include <sys/param.h>
36#include <sys/systm.h>
37#include <sys/mbuf.h>
38#include <sys/socket.h>
39
40#include <net/route.h>
41#include <net/vnet.h>
42
43#include <netinet/in.h>
44
45#include <netipsec/ipsec.h>
46
47/*
48 * Make space for a new header of length hlen at skip bytes
49 * into the packet.  When doing this we allocate new mbufs only
50 * when absolutely necessary.  The mbuf where the new header
51 * is to go is returned together with an offset into the mbuf.
52 * If NULL is returned then the mbuf chain may have been modified;
53 * the caller is assumed to always free the chain.
54 */
55struct mbuf *
56m_makespace(struct mbuf *m0, int skip, int hlen, int *off)
57{
58	struct mbuf *m;
59	unsigned remain;
60
61	IPSEC_ASSERT(m0 != NULL, ("null mbuf"));
62	IPSEC_ASSERT(hlen < MHLEN, ("hlen too big: %u", hlen));
63
64	for (m = m0; m && skip > m->m_len; m = m->m_next)
65		skip -= m->m_len;
66	if (m == NULL)
67		return (NULL);
68	/*
69	 * At this point skip is the offset into the mbuf m
70	 * where the new header should be placed.  Figure out
71	 * if there's space to insert the new header.  If so,
72	 * and copying the remainder makes sense then do so.
73	 * Otherwise insert a new mbuf in the chain, splitting
74	 * the contents of m as needed.
75	 */
76	remain = m->m_len - skip;		/* data to move */
77	if (hlen > M_TRAILINGSPACE(m)) {
78		struct mbuf *n0, *n, **np;
79		int todo, len, done, alloc;
80
81		n0 = NULL;
82		np = &n0;
83		alloc = 0;
84		done = 0;
85		todo = remain;
86		while (todo > 0) {
87			if (todo > MHLEN) {
88				n = m_getcl(M_NOWAIT, m->m_type, 0);
89				len = MCLBYTES;
90			}
91			else {
92				n = m_get(M_NOWAIT, m->m_type);
93				len = MHLEN;
94			}
95			if (n == NULL) {
96				m_freem(n0);
97				return NULL;
98			}
99			*np = n;
100			np = &n->m_next;
101			alloc++;
102			len = min(todo, len);
103			memcpy(n->m_data, mtod(m, char *) + skip + done, len);
104			n->m_len = len;
105			done += len;
106			todo -= len;
107		}
108
109		if (hlen <= M_TRAILINGSPACE(m) + remain) {
110			m->m_len = skip + hlen;
111			*off = skip;
112			if (n0 != NULL) {
113				*np = m->m_next;
114				m->m_next = n0;
115			}
116		}
117		else {
118			n = m_get(M_NOWAIT, m->m_type);
119			if (n == NULL) {
120				m_freem(n0);
121				return NULL;
122			}
123			alloc++;
124
125			if ((n->m_next = n0) == NULL)
126				np = &n->m_next;
127			n0 = n;
128
129			*np = m->m_next;
130			m->m_next = n0;
131
132			n->m_len = hlen;
133			m->m_len = skip;
134
135			m = n;			/* header is at front ... */
136			*off = 0;		/* ... of new mbuf */
137		}
138		IPSECSTAT_INC(ips_mbinserted);
139	} else {
140		/*
141		 * Copy the remainder to the back of the mbuf
142		 * so there's space to write the new header.
143		 */
144		bcopy(mtod(m, caddr_t) + skip,
145		      mtod(m, caddr_t) + skip + hlen, remain);
146		m->m_len += hlen;
147		*off = skip;
148	}
149	m0->m_pkthdr.len += hlen;		/* adjust packet length */
150	return m;
151}
152
153/*
154 * m_pad(m, n) pads <m> with <n> bytes at the end. The packet header
155 * length is updated, and a pointer to the first byte of the padding
156 * (which is guaranteed to be all in one mbuf) is returned.
157 */
158caddr_t
159m_pad(struct mbuf *m, int n)
160{
161	register struct mbuf *m0, *m1;
162	register int len, pad;
163	caddr_t retval;
164
165	if (n <= 0) {  /* No stupid arguments. */
166		DPRINTF(("%s: pad length invalid (%d)\n", __func__, n));
167		m_freem(m);
168		return NULL;
169	}
170
171	len = m->m_pkthdr.len;
172	pad = n;
173	m0 = m;
174
175	while (m0->m_len < len) {
176		len -= m0->m_len;
177		m0 = m0->m_next;
178	}
179
180	if (m0->m_len != len) {
181		DPRINTF(("%s: length mismatch (should be %d instead of %d)\n",
182			__func__, m->m_pkthdr.len,
183			m->m_pkthdr.len + m0->m_len - len));
184
185		m_freem(m);
186		return NULL;
187	}
188
189	/* Check for zero-length trailing mbufs, and find the last one. */
190	for (m1 = m0; m1->m_next; m1 = m1->m_next) {
191		if (m1->m_next->m_len != 0) {
192			DPRINTF(("%s: length mismatch (should be %d instead "
193				"of %d)\n", __func__,
194				m->m_pkthdr.len,
195				m->m_pkthdr.len + m1->m_next->m_len));
196
197			m_freem(m);
198			return NULL;
199		}
200
201		m0 = m1->m_next;
202	}
203
204	if (pad > M_TRAILINGSPACE(m0)) {
205		/* Add an mbuf to the chain. */
206		MGET(m1, M_NOWAIT, MT_DATA);
207		if (m1 == 0) {
208			m_freem(m0);
209			DPRINTF(("%s: unable to get extra mbuf\n", __func__));
210			return NULL;
211		}
212
213		m0->m_next = m1;
214		m0 = m1;
215		m0->m_len = 0;
216	}
217
218	retval = m0->m_data + m0->m_len;
219	m0->m_len += pad;
220	m->m_pkthdr.len += pad;
221
222	return retval;
223}
224
225/*
226 * Remove hlen data at offset skip in the packet.  This is used by
227 * the protocols strip protocol headers and associated data (e.g. IV,
228 * authenticator) on input.
229 */
230int
231m_striphdr(struct mbuf *m, int skip, int hlen)
232{
233	struct mbuf *m1;
234	int roff;
235
236	/* Find beginning of header */
237	m1 = m_getptr(m, skip, &roff);
238	if (m1 == NULL)
239		return (EINVAL);
240
241	/* Remove the header and associated data from the mbuf. */
242	if (roff == 0) {
243		/* The header was at the beginning of the mbuf */
244		IPSECSTAT_INC(ips_input_front);
245		m_adj(m1, hlen);
246		if ((m1->m_flags & M_PKTHDR) == 0)
247			m->m_pkthdr.len -= hlen;
248	} else if (roff + hlen >= m1->m_len) {
249		struct mbuf *mo;
250
251		/*
252		 * Part or all of the header is at the end of this mbuf,
253		 * so first let's remove the remainder of the header from
254		 * the beginning of the remainder of the mbuf chain, if any.
255		 */
256		IPSECSTAT_INC(ips_input_end);
257		if (roff + hlen > m1->m_len) {
258			/* Adjust the next mbuf by the remainder */
259			m_adj(m1->m_next, roff + hlen - m1->m_len);
260
261			/* The second mbuf is guaranteed not to have a pkthdr... */
262			m->m_pkthdr.len -= (roff + hlen - m1->m_len);
263		}
264
265		/* Now, let's unlink the mbuf chain for a second...*/
266		mo = m1->m_next;
267		m1->m_next = NULL;
268
269		/* ...and trim the end of the first part of the chain...sick */
270		m_adj(m1, -(m1->m_len - roff));
271		if ((m1->m_flags & M_PKTHDR) == 0)
272			m->m_pkthdr.len -= (m1->m_len - roff);
273
274		/* Finally, let's relink */
275		m1->m_next = mo;
276	} else {
277		/*
278		 * The header lies in the "middle" of the mbuf; copy
279		 * the remainder of the mbuf down over the header.
280		 */
281		IPSECSTAT_INC(ips_input_middle);
282		bcopy(mtod(m1, u_char *) + roff + hlen,
283		      mtod(m1, u_char *) + roff,
284		      m1->m_len - (roff + hlen));
285		m1->m_len -= hlen;
286		m->m_pkthdr.len -= hlen;
287	}
288	return (0);
289}
290
291/*
292 * Diagnostic routine to check mbuf alignment as required by the
293 * crypto device drivers (that use DMA).
294 */
295void
296m_checkalignment(const char* where, struct mbuf *m0, int off, int len)
297{
298	int roff;
299	struct mbuf *m = m_getptr(m0, off, &roff);
300	caddr_t addr;
301
302	if (m == NULL)
303		return;
304	printf("%s (off %u len %u): ", where, off, len);
305	addr = mtod(m, caddr_t) + roff;
306	do {
307		int mlen;
308
309		if (((uintptr_t) addr) & 3) {
310			printf("addr misaligned %p,", addr);
311			break;
312		}
313		mlen = m->m_len;
314		if (mlen > len)
315			mlen = len;
316		len -= mlen;
317		if (len && (mlen & 3)) {
318			printf("len mismatch %u,", mlen);
319			break;
320		}
321		m = m->m_next;
322		addr = m ? mtod(m, caddr_t) : NULL;
323	} while (m && len > 0);
324	for (m = m0; m; m = m->m_next)
325		printf(" [%p:%u]", mtod(m, caddr_t), m->m_len);
326	printf("\n");
327}
328