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