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