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