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