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