xref: /netbsd-src/sys/net/npf/npf_mbuf.c (revision c2f76ff004a2cb67efe5b12d97bd3ef7fe89e18d)
1 /*	$NetBSD: npf_mbuf.c,v 1.6 2011/01/18 20:33:46 rmind Exp $	*/
2 
3 /*-
4  * Copyright (c) 2009-2011 The NetBSD Foundation, Inc.
5  * All rights reserved.
6  *
7  * This material is based upon work partially supported by The
8  * NetBSD Foundation under a contract with Mindaugas Rasiukevicius.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  * 1. Redistributions of source code must retain the above copyright
14  *    notice, this list of conditions and the following disclaimer.
15  * 2. Redistributions in binary form must reproduce the above copyright
16  *    notice, this list of conditions and the following disclaimer in the
17  *    documentation and/or other materials provided with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
20  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
21  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
22  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
23  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 /*
33  * NPF network buffer management interface.
34  *
35  * Network buffer in NetBSD is mbuf.  Internal mbuf structures are
36  * abstracted within this source.
37  */
38 
39 #include <sys/cdefs.h>
40 __KERNEL_RCSID(0, "$NetBSD: npf_mbuf.c,v 1.6 2011/01/18 20:33:46 rmind Exp $");
41 
42 #include <sys/param.h>
43 #include <sys/mbuf.h>
44 
45 #include "npf_impl.h"
46 
47 /*
48  * nbuf_dataptr: return a pointer to data in nbuf.
49  */
50 void *
51 nbuf_dataptr(nbuf_t *nbuf)
52 {
53 	const struct mbuf *m = nbuf;
54 
55 	return mtod(m, void *);
56 }
57 
58 /*
59  * nbuf_advance: advance in mbuf or chain by specified amount of bytes.
60  *
61  * => Returns new pointer to data in mbuf and NULL if offset gets invalid.
62  * => Sets nbuf to current (after advance) mbuf in the chain.
63  */
64 void *
65 nbuf_advance(nbuf_t **nbuf, void *n_ptr, u_int n)
66 {
67 	struct mbuf *m = *nbuf;
68 	u_int off, wmark;
69 	uint8_t *d;
70 
71 	/* Offset with amount to advance. */
72 	off = (uintptr_t)n_ptr - mtod(m, uintptr_t) + n;
73 	wmark = m->m_len;
74 
75 	/* Find the mbuf according to offset. */
76 	while (__predict_false(wmark <= off)) {
77 		m = m->m_next;
78 		if (__predict_false(m == NULL)) {
79 			/*
80 			 * If out of chain, then offset is
81 			 * higher than packet length.
82 			 */
83 			return NULL;
84 		}
85 		wmark += m->m_len;
86 	}
87 
88 	/* Offset in mbuf data. */
89 	d = mtod(m, uint8_t *);
90 	KASSERT(off >= (wmark - m->m_len));
91 	d += (off - (wmark - m->m_len));
92 
93 	*nbuf = (void *)m;
94 	return d;
95 }
96 
97 /*
98  * nbuf_rw_datum: read or write a datum of specified length at current
99  * offset in the nbuf chain and copy datum into passed buffer.
100  *
101  * => Datum is allowed to overlap between two or more mbufs.
102  * => Note: all data in nbuf is in network byte order.
103  * => Returns 0 on success, error code on failure.
104  *
105  * Note: this function must be static inline with constant operation
106  * parameter - we expect constant propagation.
107  */
108 
109 #define	NBUF_DATA_READ		0
110 #define	NBUF_DATA_WRITE		1
111 
112 static inline int
113 nbuf_rw_datum(const int wr, struct mbuf *m, void *n_ptr, size_t len, void *buf)
114 {
115 	uint8_t *d = n_ptr, *b = buf;
116 	u_int off, wmark, end;
117 
118 	/* Current offset in mbuf. */
119 	off = (uintptr_t)n_ptr - mtod(m, uintptr_t);
120 	KASSERT(off < m->m_len);
121 	wmark = m->m_len;
122 
123 	/* Is datum overlapping? */
124 	end = off + len;
125 	while (__predict_false(end > wmark)) {
126 		u_int l;
127 
128 		/* Get the part of current mbuf. */
129 		l = m->m_len - off;
130 		KASSERT(l < len);
131 		len -= l;
132 		if (wr == NBUF_DATA_WRITE) {
133 			while (l--)
134 				*d++ = *b++;
135 		} else {
136 			KASSERT(wr == NBUF_DATA_READ);
137 			while (l--)
138 				*b++ = *d++;
139 		}
140 		KASSERT(len > 0);
141 
142 		/* Take next mbuf and continue. */
143 		m = m->m_next;
144 		if (__predict_false(m == NULL)) {
145 			/*
146 			 * If out of chain, then offset with datum
147 			 * length exceed the packet length.
148 			 */
149 			return EINVAL;
150 		}
151 		wmark += m->m_len;
152 		d = mtod(m, uint8_t *);
153 		off = 0;
154 	}
155 	KASSERT(n_ptr == d || mtod(m, uint8_t *) == d);
156 	KASSERT(len <= m->m_len);
157 
158 	/* Non-overlapping case: fetch the actual data. */
159 	if (wr == NBUF_DATA_WRITE) {
160 		while (len--)
161 			*d++ = *b++;
162 	} else {
163 		KASSERT(wr == NBUF_DATA_READ);
164 		while (len--)
165 			*b++ = *d++;
166 	}
167 	return 0;
168 }
169 
170 /*
171  * nbuf_{fetch|store}_datum: read/write absraction calls on nbuf_rw_datum().
172  */
173 int
174 nbuf_fetch_datum(nbuf_t *nbuf, void *n_ptr, size_t len, void *buf)
175 {
176 	struct mbuf *m = nbuf;
177 
178 	return nbuf_rw_datum(NBUF_DATA_READ, m, n_ptr, len, buf);
179 }
180 
181 int
182 nbuf_store_datum(nbuf_t *nbuf, void *n_ptr, size_t len, void *buf)
183 {
184 	struct mbuf *m = nbuf;
185 
186 	KASSERT((m->m_flags & M_PKTHDR) != 0 || !M_READONLY(m));
187 	return nbuf_rw_datum(NBUF_DATA_WRITE, m, n_ptr, len, buf);
188 }
189 
190 /*
191  * nbuf_advfetch: advance and fetch the datum.
192  */
193 int
194 nbuf_advfetch(nbuf_t **nbuf, void **n_ptr, u_int n, size_t len, void *buf)
195 {
196 	nbuf_t *orig_nbuf = *nbuf;
197 	void *orig_nptr = *n_ptr;
198 	int error;
199 
200 	*n_ptr = nbuf_advance(nbuf, *n_ptr, n);
201 	if (__predict_false(*n_ptr != NULL)) {
202 		error = nbuf_fetch_datum(*nbuf, *n_ptr, len, buf);
203 	} else {
204 		error = EINVAL;
205 	}
206 	if (__predict_false(error)) {
207 		*nbuf = orig_nbuf;
208 		*n_ptr = orig_nptr;
209 	}
210 	return error;
211 }
212 
213 /*
214  * nbuf_advstore: advance and store the datum.
215  */
216 int
217 nbuf_advstore(nbuf_t **nbuf, void **n_ptr, u_int n, size_t len, void *buf)
218 {
219 	nbuf_t *orig_nbuf = *nbuf;
220 	void *orig_nptr = *n_ptr;
221 	int error;
222 
223 	*n_ptr = nbuf_advance(nbuf, *n_ptr, n);
224 	if (__predict_false(*n_ptr != NULL)) {
225 		error = nbuf_store_datum(*nbuf, *n_ptr, len, buf);
226 	} else {
227 		error = EINVAL;
228 	}
229 	if (__predict_false(error)) {
230 		*nbuf = orig_nbuf;
231 		*n_ptr = orig_nptr;
232 	}
233 	return error;
234 }
235 
236 /*
237  * nbuf_add_tag: add a tag to specified network buffer.
238  *
239  * => Returns 0 on success, or errno on failure.
240  */
241 int
242 nbuf_add_tag(nbuf_t *nbuf, uint32_t key, uint32_t val)
243 {
244 	struct mbuf *m = nbuf;
245 	struct m_tag *mt;
246 	uint32_t *dat;
247 
248 	mt = m_tag_get(PACKET_TAG_NPF, sizeof(uint32_t), M_NOWAIT);
249 	if (__predict_false(mt == NULL)) {
250 		return ENOMEM;
251 	}
252 	dat = (uint32_t *)(mt + 1);
253 	*dat = val;
254 	m_tag_prepend(m, mt);
255 	return 0;
256 }
257 
258 /*
259  * nbuf_find_tag: find a tag in specified network buffer.
260  *
261  * => Returns 0 on success, or errno on failure.
262  */
263 int
264 nbuf_find_tag(nbuf_t *nbuf, uint32_t key, void **data)
265 {
266 	struct mbuf *m = nbuf;
267 	struct m_tag *mt;
268 
269 	mt = m_tag_find(m, PACKET_TAG_NPF, NULL);
270 	if (__predict_false(mt == NULL)) {
271 		return EINVAL;
272 	}
273 	*data = (void *)(mt + 1);
274 	return 0;
275 }
276