[dragonfly.git] / sys / kern / libmchain / subr_mchain.c

/*
 * Copyright (c) 2000, 2001 Boris Popov
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *    This product includes software developed by Boris Popov.
 * 4. Neither the name of the author nor the names of any co-contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $FreeBSD: src/sys/kern/subr_mchain.c,v 1.2.2.2 2002/04/13 12:46:40 bp Exp $
 * $DragonFly: src/sys/kern/libmchain/subr_mchain.c,v 1.6 2007/10/20 09:38:01 sephe Exp $
 */


#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/errno.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/module.h>
#include <sys/uio.h>

#include <sys/mchain.h>

MODULE_VERSION(libmchain, 1);

#define MBERROR(format, args...) kprintf("%s(%d): "format, __func__ , \
				    __LINE__ ,## args)

#define MBPANIC(format, args...) kprintf("%s(%d): "format, __func__ , \
				    __LINE__ ,## args)

/*
 * Various helper functions
 */
int
m_fixhdr(struct mbuf *m0)
{
	struct mbuf *m = m0;
	int len = 0;

	while (m) {
		len += m->m_len;
		m = m->m_next;
	}
	m0->m_pkthdr.len = len;
	return len;
}

int
mb_init(struct mbchain *mbp)
{
	struct mbuf *m;

	m = m_gethdr(MB_WAIT, MT_DATA);
	if (m == NULL) 
		return ENOBUFS;
	m->m_pkthdr.rcvif = NULL;
	m->m_len = 0;
	mb_initm(mbp, m);
	return 0;
}

void
mb_initm(struct mbchain *mbp, struct mbuf *m)
{
	bzero(mbp, sizeof(*mbp));
	mbp->mb_top = mbp->mb_cur = m;
	mbp->mb_mleft = M_TRAILINGSPACE(m);
}

void
mb_done(struct mbchain *mbp)
{
	if (mbp->mb_top) {
		m_freem(mbp->mb_top);
		mbp->mb_top = NULL;
	}
}

struct mbuf *
mb_detach(struct mbchain *mbp)
{
	struct mbuf *m;

	m = mbp->mb_top;
	mbp->mb_top = NULL;
	return m;
}

int
mb_fixhdr(struct mbchain *mbp)
{
	return mbp->mb_top->m_pkthdr.len = m_fixhdr(mbp->mb_top);
}

/*
 * Check if object of size 'size' fit to the current position and
 * allocate new mbuf if not. Advance pointers and increase length of mbuf(s).
 * Return pointer to the object placeholder or NULL if any error occured.
 * Note: size should be <= MLEN 
 */
caddr_t
mb_reserve(struct mbchain *mbp, int size)
{
	struct mbuf *m, *mn;
	caddr_t bpos;

	if (size > MLEN)
		panic("mb_reserve: size = %d\n", size);
	m = mbp->mb_cur;
	if (mbp->mb_mleft < size) {
		mn = m_get(MB_WAIT, MT_DATA);
		if (mn == NULL)
			return NULL;
		mbp->mb_cur = m->m_next = mn;
		m = mn;
		m->m_len = 0;
		mbp->mb_mleft = M_TRAILINGSPACE(m);
	}
	mbp->mb_mleft -= size;
	mbp->mb_count += size;
	bpos = mtod(m, caddr_t) + m->m_len;
	m->m_len += size;
	return bpos;
}

int
mb_put_uint8(struct mbchain *mbp, u_int8_t x)
{
	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
}

int
mb_put_uint16be(struct mbchain *mbp, u_int16_t x)
{
	x = htobes(x);
	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
}

int
mb_put_uint16le(struct mbchain *mbp, u_int16_t x)
{
	x = htoles(x);
	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
}

int
mb_put_uint32be(struct mbchain *mbp, u_int32_t x)
{
	x = htobel(x);
	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
}

int
mb_put_uint32le(struct mbchain *mbp, u_int32_t x)
{
	x = htolel(x);
	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
}

int
mb_put_int64be(struct mbchain *mbp, int64_t x)
{
	x = htobeq(x);
	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
}

int
mb_put_int64le(struct mbchain *mbp, int64_t x)
{
	x = htoleq(x);
	return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM);
}

int
mb_put_mem(struct mbchain *mbp, c_caddr_t source, int size, int type)
{
	struct mbuf *m;
	caddr_t dst;
	c_caddr_t src;
	int error, mleft, count;
	size_t cplen;

	m = mbp->mb_cur;
	mleft = mbp->mb_mleft;

	while (size > 0) {
		if (mleft == 0) {
			if (m->m_next == NULL) {
				m->m_next = m_getc(size, MB_WAIT, MT_DATA);
				if (m == NULL)
					return ENOBUFS;
			}
			m = m->m_next;
			mleft = M_TRAILINGSPACE(m);
			continue;
		}
		cplen = mleft > size ? size : mleft;
		dst = mtod(m, caddr_t) + m->m_len;
		switch (type) {
		    case MB_MCUSTOM:
			error = mbp->mb_copy(mbp, source, dst, cplen);
			if (error)
				return error;
			break;
		    case MB_MINLINE:
			for (src = source, count = cplen; count; count--)
				*dst++ = *src++;
			break;
		    case MB_MSYSTEM:
			bcopy(source, dst, cplen);
			break;
		    case MB_MUSER:
			error = copyin(source, dst, cplen);
			if (error)
				return error;
			break;
		    case MB_MZERO:
			bzero(dst, cplen);
			break;
		}
		size -= cplen;
		source += cplen;
		m->m_len += cplen;
		mleft -= cplen;
		mbp->mb_count += cplen;
	}
	mbp->mb_cur = m;
	mbp->mb_mleft = mleft;
	return 0;
}

int
mb_put_mbuf(struct mbchain *mbp, struct mbuf *m)
{
	mbp->mb_cur->m_next = m;
	while (m) {
		mbp->mb_count += m->m_len;
		if (m->m_next == NULL)
			break;
		m = m->m_next;
	}
	mbp->mb_mleft = M_TRAILINGSPACE(m);
	mbp->mb_cur = m;
	return 0;
}

/*
 * copies a uio scatter/gather list to an mbuf chain.
 */
int
mb_put_uio(struct mbchain *mbp, struct uio *uiop, int size)
{
	long left;
	int mtype, error;

	mtype = (uiop->uio_segflg == UIO_SYSSPACE) ? MB_MSYSTEM : MB_MUSER;

	while (size > 0 && uiop->uio_resid) {
		if (uiop->uio_iovcnt <= 0 || uiop->uio_iov == NULL)
			return EFBIG;
		left = uiop->uio_iov->iov_len;
		if (left == 0) {
			uiop->uio_iov++;
			uiop->uio_iovcnt--;
			continue;
		}
		if (left > size)
			left = size;
		error = mb_put_mem(mbp, uiop->uio_iov->iov_base, left, mtype);
		if (error)
			return error;
		uiop->uio_offset += left;
		uiop->uio_resid -= left;
		uiop->uio_iov->iov_base = (char *)uiop->uio_iov->iov_base + left;
		uiop->uio_iov->iov_len -= left;
		size -= left;
	}
	return 0;
}

/*
 * Routines for fetching data from an mbuf chain
 */
int
md_init(struct mdchain *mdp)
{
	struct mbuf *m;

	m = m_gethdr(MB_WAIT, MT_DATA);
	if (m == NULL) 
		return ENOBUFS;
	m->m_pkthdr.rcvif = NULL;
	m->m_len = 0;
	md_initm(mdp, m);
	return 0;
}

void
md_initm(struct mdchain *mdp, struct mbuf *m)
{
	bzero(mdp, sizeof(*mdp));
	mdp->md_top = mdp->md_cur = m;
	mdp->md_pos = mtod(m, u_char*);
}

void
md_done(struct mdchain *mdp)
{
	if (mdp->md_top) {
		m_freem(mdp->md_top);
		mdp->md_top = NULL;
	}
}

/*
 * Append a separate mbuf chain. It is caller responsibility to prevent
 * multiple calls to fetch/record routines.
 */
void
md_append_record(struct mdchain *mdp, struct mbuf *top)
{
	struct mbuf *m;

	if (mdp->md_top == NULL) {
		md_initm(mdp, top);
		return;
	}
	m = mdp->md_top;
	while (m->m_nextpkt)
		m = m->m_nextpkt;
	m->m_nextpkt = top;
	top->m_nextpkt = NULL;
	return;
}

/*
 * Put next record in place of existing
 */
int
md_next_record(struct mdchain *mdp)
{
	struct mbuf *m;

	if (mdp->md_top == NULL)
		return ENOENT;
	m = mdp->md_top->m_nextpkt;
	md_done(mdp);
	if (m == NULL)
		return ENOENT;
	md_initm(mdp, m);
	return 0;
}

int
md_get_uint8(struct mdchain *mdp, u_int8_t *x)
{
	return md_get_mem(mdp, x, 1, MB_MINLINE);
}

int
md_get_uint16(struct mdchain *mdp, u_int16_t *x)
{
	return md_get_mem(mdp, (caddr_t)x, 2, MB_MINLINE);
}

int
md_get_uint16le(struct mdchain *mdp, u_int16_t *x)
{
	u_int16_t v;
	int error = md_get_uint16(mdp, &v);

	*x = letohs(v);
	return error;
}

int
md_get_uint16be(struct mdchain *mdp, u_int16_t *x) {
	u_int16_t v;
	int error = md_get_uint16(mdp, &v);

	*x = betohs(v);
	return error;
}

int
md_get_uint32(struct mdchain *mdp, u_int32_t *x)
{
	return md_get_mem(mdp, (caddr_t)x, 4, MB_MINLINE);
}

int
md_get_uint32be(struct mdchain *mdp, u_int32_t *x)
{
	u_int32_t v;
	int error;

	error = md_get_uint32(mdp, &v);
	*x = betohl(v);
	return error;
}

int
md_get_uint32le(struct mdchain *mdp, u_int32_t *x)
{
	u_int32_t v;
	int error;

	error = md_get_uint32(mdp, &v);
	*x = letohl(v);
	return error;
}

int
md_get_int64(struct mdchain *mdp, int64_t *x)
{
	return md_get_mem(mdp, (caddr_t)x, 8, MB_MINLINE);
}

int
md_get_int64be(struct mdchain *mdp, int64_t *x)
{
	int64_t v;
	int error;

	error = md_get_int64(mdp, &v);
	*x = betohq(v);
	return error;
}

int
md_get_int64le(struct mdchain *mdp, int64_t *x)
{
	int64_t v;
	int error;

	error = md_get_int64(mdp, &v);
	*x = letohq(v);
	return error;
}

int
md_get_mem(struct mdchain *mdp, caddr_t target, int size, int type)
{
	struct mbuf *m = mdp->md_cur;
	int error;
	u_int count;
	u_char *s;
	
	while (size > 0) {
		if (m == NULL) {
			MBERROR("incomplete copy\n");
			return EBADRPC;
		}
		s = mdp->md_pos;
		count = mtod(m, u_char*) + m->m_len - s;
		if (count == 0) {
			mdp->md_cur = m = m->m_next;
			if (m)
				s = mdp->md_pos = mtod(m, caddr_t);
			continue;
		}
		if (count > size)
			count = size;
		size -= count;
		mdp->md_pos += count;
		if (target == NULL)
			continue;
		switch (type) {
		    case MB_MUSER:
			error = copyout(s, target, count);
			if (error)
				return error;
			break;
		    case MB_MSYSTEM:
			bcopy(s, target, count);
			break;
		    case MB_MINLINE:
			while (count--)
				*target++ = *s++;
			continue;
		}
		target += count;
	}
	return 0;
}

int
md_get_mbuf(struct mdchain *mdp, int size, struct mbuf **ret)
{
	struct mbuf *m = mdp->md_cur, *rm;

	rm = m_copym(m, mdp->md_pos - mtod(m, u_char*), size, MB_WAIT);
	if (rm == NULL)
		return EBADRPC;
	md_get_mem(mdp, NULL, size, MB_MZERO);
	*ret = rm;
	return 0;
}

int
md_get_uio(struct mdchain *mdp, struct uio *uiop, int size)
{
	char *uiocp;
	long left;
	int mtype, error;

	mtype = (uiop->uio_segflg == UIO_SYSSPACE) ? MB_MSYSTEM : MB_MUSER;
	while (size > 0 && uiop->uio_resid) {
		if (uiop->uio_iovcnt <= 0 || uiop->uio_iov == NULL)
			return EFBIG;
		left = uiop->uio_iov->iov_len;
		if (left == 0) {
			uiop->uio_iov++;
			uiop->uio_iovcnt--;
			continue;
		}
		uiocp = uiop->uio_iov->iov_base;
		if (left > size)
			left = size;
		error = md_get_mem(mdp, uiocp, left, mtype);
		if (error)
			return error;
		uiop->uio_offset += left;
		uiop->uio_resid -= left;
		uiop->uio_iov->iov_base = (char *)uiop->uio_iov->iov_base + left;
		uiop->uio_iov->iov_len -= left;
		size -= left;
	}
	return 0;
}
Commit	Line	Data
984263bc MD	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 $
62230bcd	33	* $DragonFly: src/sys/kern/libmchain/subr_mchain.c,v 1.6 2007/10/20 09:38:01 sephe Exp $
984263bc MD	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
6ea70f76	50	#define MBERROR(format, args...) kprintf("%s(%d): "format, __func__ , \
984263bc MD	51	__LINE__ ,## args)
984263bc MD	52
6ea70f76	53	#define MBPANIC(format, args...) kprintf("%s(%d): "format, __func__ , \
984263bc MD	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
62230bcd	78	m = m_gethdr(MB_WAIT, MT_DATA);
984263bc MD	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) {
62230bcd	136	mn = m_get(MB_WAIT, MT_DATA);
984263bc MD	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;
973c11b9 MD	205	int error, mleft, count;
973c11b9 MD	206	size_t cplen;
984263bc MD	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) {
62230bcd	214	m->m_next = m_getc(size, MB_WAIT, MT_DATA);
984263bc MD	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;
656849c6	299	uiop->uio_iov->iov_base = (char *)uiop->uio_iov->iov_base + left;
984263bc MD	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
62230bcd	314	m = m_gethdr(MB_WAIT, MT_DATA);
984263bc MD	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
62230bcd	517	rm = m_copym(m, mdp->md_pos - mtod(m, u_char*), size, MB_WAIT);
984263bc MD	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;
656849c6	550	uiop->uio_iov->iov_base = (char *)uiop->uio_iov->iov_base + left;
984263bc MD	551	uiop->uio_iov->iov_len -= left;
	552	size -= left;
	553	}
	554	return 0;
	555	}