xref: /dragonfly/sys/kern/libmchain/subr_mchain.c (revision 36a3d1d6) 
1 /*
2  * Copyright (c) 2000, 2001 Boris Popov
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  * 3. All advertising materials mentioning features or use of this software
14  *    must display the following acknowledgement:
15  *    This product includes software developed by Boris Popov.
16  * 4. Neither the name of the author nor the names of any co-contributors
17  *    may be used to endorse or promote products derived from this software
18  *    without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
21  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
26  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
29  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
30  * SUCH DAMAGE.
31  *
32  * $FreeBSD: src/sys/kern/subr_mchain.c,v 1.2.2.2 2002/04/13 12:46:40 bp Exp $
33  * $DragonFly: src/sys/kern/libmchain/subr_mchain.c,v 1.6 2007/10/20 09:38:01 sephe Exp $
34  */
35 
36 
37 #include <sys/param.h>
38 #include <sys/systm.h>
39 #include <sys/kernel.h>
40 #include <sys/errno.h>
41 #include <sys/malloc.h>
42 #include <sys/mbuf.h>
43 #include <sys/module.h>
44 #include <sys/uio.h>
45 
46 #include <sys/mchain.h>
47 
48 MODULE_VERSION(libmchain, 1);
49 
50 #define MBERROR(format, args...) kprintf("%s(%d): "format, __func__ , \
51 				    __LINE__ ,## args)
52 
53 #define MBPANIC(format, args...) kprintf("%s(%d): "format, __func__ , \
54 				    __LINE__ ,## args)
55 
56 /*
57  * Various helper functions
58  */
59 int
60 m_fixhdr(struct mbuf *m0)
61 {
62 	struct mbuf *m = m0;
63 	int len = 0;
64 
65 	while (m) {
66 		len += m->m_len;
67 		m = m->m_next;
68 	}
69 	m0->m_pkthdr.len = len;
70 	return len;
71 }
72 
73 int
74 mb_init(struct mbchain *mbp)
75 {
76 	struct mbuf *m;
77 
78 	m = m_gethdr(MB_WAIT, MT_DATA);
79 	if (m == NULL)
80 		return ENOBUFS;
81 	m->m_pkthdr.rcvif = NULL;
82 	m->m_len = 0;
83 	mb_initm(mbp, m);
84 	return 0;
85 }
86 
87 void
88 mb_initm(struct mbchain *mbp, struct mbuf *m)
89 {
90 	bzero(mbp, sizeof(*mbp));
91 	mbp->mb_top = mbp->mb_cur = m;
92 	mbp->mb_mleft = M_TRAILINGSPACE(m);
93 }
94 
95 void
96 mb_done(struct mbchain *mbp)
97 {
98 	if (mbp->mb_top) {
99 		m_freem(mbp->mb_top);
100 		mbp->mb_top = NULL;
101 	}
102 }
103 
104 struct mbuf *
105 mb_detach(struct mbchain *mbp)
106 {
107 	struct mbuf *m;
108 
109 	m = mbp->mb_top;
110 	mbp->mb_top = NULL;
111 	return m;
112 }
113 
114 int
115 mb_fixhdr(struct mbchain *mbp)
116 {
117 	return mbp->mb_top->m_pkthdr.len = m_fixhdr(mbp->mb_top);
118 }
119 
120 /*
121  * Check if object of size 'size' fit to the current position and
122  * allocate new mbuf if not. Advance pointers and increase length of mbuf(s).
123  * Return pointer to the object placeholder or NULL if any error occured.
124  * Note: size should be <= MLEN
125  */
126 caddr_t
127 mb_reserve(struct mbchain *mbp, int size)
128 {
129 	struct mbuf *m, *mn;
130 	caddr_t bpos;
131 
132 	if (size > MLEN)
133 		panic("mb_reserve: size = %d\n", size);
134 	m = mbp->mb_cur;
135 	if (mbp->mb_mleft < size) {
136 		mn = m_get(MB_WAIT, MT_DATA);
137 		if (mn == NULL)
138 			return NULL;
139 		mbp->mb_cur = m->m_next = mn;
140 		m = mn;
141 		m->m_len = 0;
142 		mbp->mb_mleft = M_TRAILINGSPACE(m);
143 	}
144 	mbp->mb_mleft -= size;
145 	mbp->mb_count += size;
146 	bpos = mtod(m, caddr_t) + m->m_len;
147 	m->m_len += size;
148 	return bpos;
149 }
150 
151 int
152 mb_put_uint8(struct mbchain *mbp, u_int8_t x)
153 {
154 	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
155 }
156 
157 int
158 mb_put_uint16be(struct mbchain *mbp, u_int16_t x)
159 {
160 	x = htobes(x);
161 	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
162 }
163 
164 int
165 mb_put_uint16le(struct mbchain *mbp, u_int16_t x)
166 {
167 	x = htoles(x);
168 	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
169 }
170 
171 int
172 mb_put_uint32be(struct mbchain *mbp, u_int32_t x)
173 {
174 	x = htobel(x);
175 	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
176 }
177 
178 int
179 mb_put_uint32le(struct mbchain *mbp, u_int32_t x)
180 {
181 	x = htolel(x);
182 	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
183 }
184 
185 int
186 mb_put_int64be(struct mbchain *mbp, int64_t x)
187 {
188 	x = htobeq(x);
189 	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
190 }
191 
192 int
193 mb_put_int64le(struct mbchain *mbp, int64_t x)
194 {
195 	x = htoleq(x);
196 	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
197 }
198 
199 int
200 mb_put_mem(struct mbchain *mbp, c_caddr_t source, int size, int type)
201 {
202 	struct mbuf *m;
203 	caddr_t dst;
204 	c_caddr_t src;
205 	int error, mleft, count;
206 	size_t cplen;
207 
208 	m = mbp->mb_cur;
209 	mleft = mbp->mb_mleft;
210 
211 	while (size > 0) {
212 		if (mleft == 0) {
213 			if (m->m_next == NULL) {
214 				m->m_next = m_getc(size, MB_WAIT, MT_DATA);
215 				if (m == NULL)
216 					return ENOBUFS;
217 			}
218 			m = m->m_next;
219 			mleft = M_TRAILINGSPACE(m);
220 			continue;
221 		}
222 		cplen = mleft > size ? size : mleft;
223 		dst = mtod(m, caddr_t) + m->m_len;
224 		switch (type) {
225 		    case MB_MCUSTOM:
226 			error = mbp->mb_copy(mbp, source, dst, cplen);
227 			if (error)
228 				return error;
229 			break;
230 		    case MB_MINLINE:
231 			for (src = source, count = cplen; count; count--)
232 				*dst++ = *src++;
233 			break;
234 		    case MB_MSYSTEM:
235 			bcopy(source, dst, cplen);
236 			break;
237 		    case MB_MUSER:
238 			error = copyin(source, dst, cplen);
239 			if (error)
240 				return error;
241 			break;
242 		    case MB_MZERO:
243 			bzero(dst, cplen);
244 			break;
245 		}
246 		size -= cplen;
247 		source += cplen;
248 		m->m_len += cplen;
249 		mleft -= cplen;
250 		mbp->mb_count += cplen;
251 	}
252 	mbp->mb_cur = m;
253 	mbp->mb_mleft = mleft;
254 	return 0;
255 }
256 
257 int
258 mb_put_mbuf(struct mbchain *mbp, struct mbuf *m)
259 {
260 	mbp->mb_cur->m_next = m;
261 	while (m) {
262 		mbp->mb_count += m->m_len;
263 		if (m->m_next == NULL)
264 			break;
265 		m = m->m_next;
266 	}
267 	mbp->mb_mleft = M_TRAILINGSPACE(m);
268 	mbp->mb_cur = m;
269 	return 0;
270 }
271 
272 /*
273  * copies a uio scatter/gather list to an mbuf chain.
274  */
275 int
276 mb_put_uio(struct mbchain *mbp, struct uio *uiop, int size)
277 {
278 	long left;
279 	int mtype, error;
280 
281 	mtype = (uiop->uio_segflg == UIO_SYSSPACE) ? MB_MSYSTEM : MB_MUSER;
282 
283 	while (size > 0 && uiop->uio_resid) {
284 		if (uiop->uio_iovcnt <= 0 || uiop->uio_iov == NULL)
285 			return EFBIG;
286 		left = uiop->uio_iov->iov_len;
287 		if (left == 0) {
288 			uiop->uio_iov++;
289 			uiop->uio_iovcnt--;
290 			continue;
291 		}
292 		if (left > size)
293 			left = size;
294 		error = mb_put_mem(mbp, uiop->uio_iov->iov_base, left, mtype);
295 		if (error)
296 			return error;
297 		uiop->uio_offset += left;
298 		uiop->uio_resid -= left;
299 		uiop->uio_iov->iov_base = (char *)uiop->uio_iov->iov_base + left;
300 		uiop->uio_iov->iov_len -= left;
301 		size -= left;
302 	}
303 	return 0;
304 }
305 
306 /*
307  * Routines for fetching data from an mbuf chain
308  */
309 int
310 md_init(struct mdchain *mdp)
311 {
312 	struct mbuf *m;
313 
314 	m = m_gethdr(MB_WAIT, MT_DATA);
315 	if (m == NULL)
316 		return ENOBUFS;
317 	m->m_pkthdr.rcvif = NULL;
318 	m->m_len = 0;
319 	md_initm(mdp, m);
320 	return 0;
321 }
322 
323 void
324 md_initm(struct mdchain *mdp, struct mbuf *m)
325 {
326 	bzero(mdp, sizeof(*mdp));
327 	mdp->md_top = mdp->md_cur = m;
328 	mdp->md_pos = mtod(m, u_char*);
329 }
330 
331 void
332 md_done(struct mdchain *mdp)
333 {
334 	if (mdp->md_top) {
335 		m_freem(mdp->md_top);
336 		mdp->md_top = NULL;
337 	}
338 }
339 
340 /*
341  * Append a separate mbuf chain. It is caller responsibility to prevent
342  * multiple calls to fetch/record routines.
343  */
344 void
345 md_append_record(struct mdchain *mdp, struct mbuf *top)
346 {
347 	struct mbuf *m;
348 
349 	if (mdp->md_top == NULL) {
350 		md_initm(mdp, top);
351 		return;
352 	}
353 	m = mdp->md_top;
354 	while (m->m_nextpkt)
355 		m = m->m_nextpkt;
356 	m->m_nextpkt = top;
357 	top->m_nextpkt = NULL;
358 	return;
359 }
360 
361 /*
362  * Put next record in place of existing
363  */
364 int
365 md_next_record(struct mdchain *mdp)
366 {
367 	struct mbuf *m;
368 
369 	if (mdp->md_top == NULL)
370 		return ENOENT;
371 	m = mdp->md_top->m_nextpkt;
372 	md_done(mdp);
373 	if (m == NULL)
374 		return ENOENT;
375 	md_initm(mdp, m);
376 	return 0;
377 }
378 
379 int
380 md_get_uint8(struct mdchain *mdp, u_int8_t *x)
381 {
382 	return md_get_mem(mdp, x, 1, MB_MINLINE);
383 }
384 
385 int
386 md_get_uint16(struct mdchain *mdp, u_int16_t *x)
387 {
388 	return md_get_mem(mdp, (caddr_t)x, 2, MB_MINLINE);
389 }
390 
391 int
392 md_get_uint16le(struct mdchain *mdp, u_int16_t *x)
393 {
394 	u_int16_t v;
395 	int error = md_get_uint16(mdp, &v);
396 
397 	*x = letohs(v);
398 	return error;
399 }
400 
401 int
402 md_get_uint16be(struct mdchain *mdp, u_int16_t *x) {
403 	u_int16_t v;
404 	int error = md_get_uint16(mdp, &v);
405 
406 	*x = betohs(v);
407 	return error;
408 }
409 
410 int
411 md_get_uint32(struct mdchain *mdp, u_int32_t *x)
412 {
413 	return md_get_mem(mdp, (caddr_t)x, 4, MB_MINLINE);
414 }
415 
416 int
417 md_get_uint32be(struct mdchain *mdp, u_int32_t *x)
418 {
419 	u_int32_t v;
420 	int error;
421 
422 	error = md_get_uint32(mdp, &v);
423 	*x = betohl(v);
424 	return error;
425 }
426 
427 int
428 md_get_uint32le(struct mdchain *mdp, u_int32_t *x)
429 {
430 	u_int32_t v;
431 	int error;
432 
433 	error = md_get_uint32(mdp, &v);
434 	*x = letohl(v);
435 	return error;
436 }
437 
438 int
439 md_get_int64(struct mdchain *mdp, int64_t *x)
440 {
441 	return md_get_mem(mdp, (caddr_t)x, 8, MB_MINLINE);
442 }
443 
444 int
445 md_get_int64be(struct mdchain *mdp, int64_t *x)
446 {
447 	int64_t v;
448 	int error;
449 
450 	error = md_get_int64(mdp, &v);
451 	*x = betohq(v);
452 	return error;
453 }
454 
455 int
456 md_get_int64le(struct mdchain *mdp, int64_t *x)
457 {
458 	int64_t v;
459 	int error;
460 
461 	error = md_get_int64(mdp, &v);
462 	*x = letohq(v);
463 	return error;
464 }
465 
466 int
467 md_get_mem(struct mdchain *mdp, caddr_t target, int size, int type)
468 {
469 	struct mbuf *m = mdp->md_cur;
470 	int error;
471 	u_int count;
472 	u_char *s;
473 
474 	while (size > 0) {
475 		if (m == NULL) {
476 			MBERROR("incomplete copy\n");
477 			return EBADRPC;
478 		}
479 		s = mdp->md_pos;
480 		count = mtod(m, u_char*) + m->m_len - s;
481 		if (count == 0) {
482 			mdp->md_cur = m = m->m_next;
483 			if (m)
484 				s = mdp->md_pos = mtod(m, caddr_t);
485 			continue;
486 		}
487 		if (count > size)
488 			count = size;
489 		size -= count;
490 		mdp->md_pos += count;
491 		if (target == NULL)
492 			continue;
493 		switch (type) {
494 		    case MB_MUSER:
495 			error = copyout(s, target, count);
496 			if (error)
497 				return error;
498 			break;
499 		    case MB_MSYSTEM:
500 			bcopy(s, target, count);
501 			break;
502 		    case MB_MINLINE:
503 			while (count--)
504 				*target++ = *s++;
505 			continue;
506 		}
507 		target += count;
508 	}
509 	return 0;
510 }
511 
512 int
513 md_get_mbuf(struct mdchain *mdp, int size, struct mbuf **ret)
514 {
515 	struct mbuf *m = mdp->md_cur, *rm;
516 
517 	rm = m_copym(m, mdp->md_pos - mtod(m, u_char*), size, MB_WAIT);
518 	if (rm == NULL)
519 		return EBADRPC;
520 	md_get_mem(mdp, NULL, size, MB_MZERO);
521 	*ret = rm;
522 	return 0;
523 }
524 
525 int
526 md_get_uio(struct mdchain *mdp, struct uio *uiop, int size)
527 {
528 	char *uiocp;
529 	long left;
530 	int mtype, error;
531 
532 	mtype = (uiop->uio_segflg == UIO_SYSSPACE) ? MB_MSYSTEM : MB_MUSER;
533 	while (size > 0 && uiop->uio_resid) {
534 		if (uiop->uio_iovcnt <= 0 || uiop->uio_iov == NULL)
535 			return EFBIG;
536 		left = uiop->uio_iov->iov_len;
537 		if (left == 0) {
538 			uiop->uio_iov++;
539 			uiop->uio_iovcnt--;
540 			continue;
541 		}
542 		uiocp = uiop->uio_iov->iov_base;
543 		if (left > size)
544 			left = size;
545 		error = md_get_mem(mdp, uiocp, left, mtype);
546 		if (error)
547 			return error;
548 		uiop->uio_offset += left;
549 		uiop->uio_resid -= left;
550 		uiop->uio_iov->iov_base = (char *)uiop->uio_iov->iov_base + left;
551 		uiop->uio_iov->iov_len -= left;
552 		size -= left;
553 	}
554 	return 0;
555 }
556