2 * Copyright (c) 2000, 2001 Boris Popov
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
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.
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
30 #include <sys/cdefs.h>
31 __FBSDID("$FreeBSD$");
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>
40 #include <sys/module.h>
43 #include <sys/mchain.h>
45 FEATURE(libmchain
, "mchain library");
47 MODULE_VERSION(libmchain
, 1);
49 #define MBERROR(format, ...) printf("%s(%d): "format, __func__ , \
50 __LINE__ , ## __VA_ARGS__)
52 #define MBPANIC(format, ...) printf("%s(%d): "format, __func__ , \
53 __LINE__ , ## __VA_ARGS__)
56 * Various helper functions
59 mb_init(struct mbchain
*mbp
)
63 m
= m_gethdr(M_WAITOK
, MT_DATA
);
70 mb_initm(struct mbchain
*mbp
, struct mbuf
*m
)
72 bzero(mbp
, sizeof(*mbp
));
73 mbp
->mb_top
= mbp
->mb_cur
= m
;
74 mbp
->mb_mleft
= M_TRAILINGSPACE(m
);
78 mb_done(struct mbchain
*mbp
)
87 mb_detach(struct mbchain
*mbp
)
97 mb_fixhdr(struct mbchain
*mbp
)
99 return (mbp
->mb_top
->m_pkthdr
.len
= m_fixhdr(mbp
->mb_top
));
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
109 mb_reserve(struct mbchain
*mbp
, int size
)
115 panic("mb_reserve: size = %d\n", size
);
117 if (mbp
->mb_mleft
< size
) {
118 mn
= m_get(M_WAITOK
, MT_DATA
);
119 mbp
->mb_cur
= m
->m_next
= mn
;
122 mbp
->mb_mleft
= M_TRAILINGSPACE(m
);
124 mbp
->mb_mleft
-= size
;
125 mbp
->mb_count
+= size
;
126 bpos
= mtod(m
, caddr_t
) + m
->m_len
;
132 mb_put_padbyte(struct mbchain
*mbp
)
137 dst
= mtod(mbp
->mb_cur
, caddr_t
) + mbp
->mb_cur
->m_len
;
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
));
147 mb_put_uint8(struct mbchain
*mbp
, uint8_t x
)
149 return (mb_put_mem(mbp
, (caddr_t
)&x
, sizeof(x
), MB_MSYSTEM
));
153 mb_put_uint16be(struct mbchain
*mbp
, uint16_t x
)
156 return (mb_put_mem(mbp
, (caddr_t
)&x
, sizeof(x
), MB_MSYSTEM
));
160 mb_put_uint16le(struct mbchain
*mbp
, uint16_t x
)
163 return (mb_put_mem(mbp
, (caddr_t
)&x
, sizeof(x
), MB_MSYSTEM
));
167 mb_put_uint32be(struct mbchain
*mbp
, uint32_t x
)
170 return (mb_put_mem(mbp
, (caddr_t
)&x
, sizeof(x
), MB_MSYSTEM
));
174 mb_put_uint32le(struct mbchain
*mbp
, uint32_t x
)
177 return (mb_put_mem(mbp
, (caddr_t
)&x
, sizeof(x
), MB_MSYSTEM
));
181 mb_put_int64be(struct mbchain
*mbp
, int64_t x
)
184 return (mb_put_mem(mbp
, (caddr_t
)&x
, sizeof(x
), MB_MSYSTEM
));
188 mb_put_int64le(struct mbchain
*mbp
, int64_t x
)
191 return (mb_put_mem(mbp
, (caddr_t
)&x
, sizeof(x
), MB_MSYSTEM
));
195 mb_put_mem(struct mbchain
*mbp
, c_caddr_t source
, int size
, int type
)
200 int cplen
, error
, mleft
, count
;
201 size_t srclen
, dstlen
;
204 mleft
= mbp
->mb_mleft
;
208 if (m
->m_next
== NULL
)
209 m
= m_getm(m
, size
, M_WAITOK
, MT_DATA
);
212 mleft
= M_TRAILINGSPACE(m
);
215 cplen
= mleft
> size
? size
: mleft
;
216 srclen
= dstlen
= cplen
;
217 dst
= mtod(m
, caddr_t
) + m
->m_len
;
222 error
= mbp
->mb_copy(mbp
, source
, dst
, &srclen
, &dstlen
);
227 for (src
= source
, count
= cplen
; count
; count
--)
231 bcopy(source
, dst
, cplen
);
234 error
= copyin(source
, dst
, cplen
);
246 mbp
->mb_count
+= dstlen
;
249 mbp
->mb_mleft
= mleft
;
254 mb_put_mbuf(struct mbchain
*mbp
, struct mbuf
*m
)
256 mbp
->mb_cur
->m_next
= m
;
258 mbp
->mb_count
+= m
->m_len
;
259 if (m
->m_next
== NULL
)
263 mbp
->mb_mleft
= M_TRAILINGSPACE(m
);
269 * copies a uio scatter/gather list to an mbuf chain.
272 mb_put_uio(struct mbchain
*mbp
, struct uio
*uiop
, int size
)
277 mtype
= (uiop
->uio_segflg
== UIO_SYSSPACE
) ? MB_MSYSTEM
: MB_MUSER
;
279 while (size
> 0 && uiop
->uio_resid
) {
280 if (uiop
->uio_iovcnt
<= 0 || uiop
->uio_iov
== NULL
)
282 left
= uiop
->uio_iov
->iov_len
;
290 error
= mb_put_mem(mbp
, uiop
->uio_iov
->iov_base
, left
, mtype
);
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
;
304 * Routines for fetching data from an mbuf chain
307 md_init(struct mdchain
*mdp
)
311 m
= m_gethdr(M_WAITOK
, MT_DATA
);
318 md_initm(struct mdchain
*mdp
, struct mbuf
*m
)
320 bzero(mdp
, sizeof(*mdp
));
321 mdp
->md_top
= mdp
->md_cur
= m
;
322 mdp
->md_pos
= mtod(m
, u_char
*);
326 md_done(struct mdchain
*mdp
)
329 m_freem(mdp
->md_top
);
335 * Append a separate mbuf chain. It is caller responsibility to prevent
336 * multiple calls to fetch/record routines.
339 md_append_record(struct mdchain
*mdp
, struct mbuf
*top
)
343 if (mdp
->md_top
== NULL
) {
351 top
->m_nextpkt
= NULL
;
356 * Put next record in place of existing
359 md_next_record(struct mdchain
*mdp
)
363 if (mdp
->md_top
== NULL
)
365 m
= mdp
->md_top
->m_nextpkt
;
374 md_get_uint8(struct mdchain
*mdp
, uint8_t *x
)
376 return (md_get_mem(mdp
, x
, 1, MB_MINLINE
));
380 md_get_uint16(struct mdchain
*mdp
, uint16_t *x
)
382 return (md_get_mem(mdp
, (caddr_t
)x
, 2, MB_MINLINE
));
386 md_get_uint16le(struct mdchain
*mdp
, uint16_t *x
)
389 int error
= md_get_uint16(mdp
, &v
);
397 md_get_uint16be(struct mdchain
*mdp
, uint16_t *x
)
400 int error
= md_get_uint16(mdp
, &v
);
408 md_get_uint32(struct mdchain
*mdp
, uint32_t *x
)
410 return (md_get_mem(mdp
, (caddr_t
)x
, 4, MB_MINLINE
));
414 md_get_uint32be(struct mdchain
*mdp
, uint32_t *x
)
419 error
= md_get_uint32(mdp
, &v
);
426 md_get_uint32le(struct mdchain
*mdp
, uint32_t *x
)
431 error
= md_get_uint32(mdp
, &v
);
438 md_get_int64(struct mdchain
*mdp
, int64_t *x
)
440 return (md_get_mem(mdp
, (caddr_t
)x
, 8, MB_MINLINE
));
444 md_get_int64be(struct mdchain
*mdp
, int64_t *x
)
449 error
= md_get_int64(mdp
, &v
);
456 md_get_int64le(struct mdchain
*mdp
, int64_t *x
)
461 error
= md_get_int64(mdp
, &v
);
468 md_get_mem(struct mdchain
*mdp
, caddr_t target
, int size
, int type
)
470 struct mbuf
*m
= mdp
->md_cur
;
477 MBERROR("incomplete copy\n");
481 count
= mtod(m
, u_char
*) + m
->m_len
- s
;
483 mdp
->md_cur
= m
= m
->m_next
;
485 s
= mdp
->md_pos
= mtod(m
, caddr_t
);
491 mdp
->md_pos
+= count
;
496 error
= copyout(s
, target
, count
);
501 bcopy(s
, target
, count
);
514 md_get_mbuf(struct mdchain
*mdp
, int size
, struct mbuf
**ret
)
516 struct mbuf
*m
= mdp
->md_cur
, *rm
;
518 rm
= m_copym(m
, mdp
->md_pos
- mtod(m
, u_char
*), size
, M_WAITOK
);
519 md_get_mem(mdp
, NULL
, size
, MB_MZERO
);
525 md_get_uio(struct mdchain
*mdp
, struct uio
*uiop
, int size
)
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
)
535 left
= uiop
->uio_iov
->iov_len
;
541 uiocp
= uiop
->uio_iov
->iov_base
;
544 error
= md_get_mem(mdp
, uiocp
, left
, mtype
);
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
;