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, __FUNCTION__ , \ 48 __LINE__ ,## args) 49 50 #define MBPANIC(format, args...) printf("%s(%d): "format, __FUNCTION__ , \ 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 * NOTE: can ony handle iovcnt == 1 270 */ 271 int 272 mb_put_uio(struct mbchain *mbp, struct uio *uiop, int size) 273 { 274 long left; 275 int mtype, error; 276 277 #ifdef DIAGNOSTIC 278 if (uiop->uio_iovcnt != 1) 279 MBPANIC("iovcnt != 1"); 280 #endif 281 mtype = (uiop->uio_segflg == UIO_SYSSPACE) ? MB_MSYSTEM : MB_MUSER; 282 283 while (size > 0) { 284 left = uiop->uio_iov->iov_len; 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; 292 uiop->uio_iov->iov_base += left; 293 uiop->uio_iov->iov_len -= left; 294 size -= left; 295 } 296 return 0; 297 } 298 299 /* 300 * Routines for fetching data from an mbuf chain 301 */ 302 int 303 md_init(struct mdchain *mdp) 304 { 305 struct mbuf *m; 306 307 m = m_gethdr(M_TRYWAIT, MT_DATA); 308 if (m == NULL) 309 return ENOBUFS; 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 389 *x = letohs(v); 390 return error; 391 } 392 393 int 394 md_get_uint16be(struct mdchain *mdp, u_int16_t *x) { 395 u_int16_t v; 396 int error = md_get_uint16(mdp, &v); 397 398 *x = betohs(v); 399 return error; 400 } 401 402 int 403 md_get_uint32(struct mdchain *mdp, u_int32_t *x) 404 { 405 return md_get_mem(mdp, (caddr_t)x, 4, MB_MINLINE); 406 } 407 408 int 409 md_get_uint32be(struct mdchain *mdp, u_int32_t *x) 410 { 411 u_int32_t v; 412 int error; 413 414 error = md_get_uint32(mdp, &v); 415 *x = betohl(v); 416 return error; 417 } 418 419 int 420 md_get_uint32le(struct mdchain *mdp, u_int32_t *x) 421 { 422 u_int32_t v; 423 int error; 424 425 error = md_get_uint32(mdp, &v); 426 *x = letohl(v); 427 return error; 428 } 429 430 int 431 md_get_int64(struct mdchain *mdp, int64_t *x) 432 { 433 return md_get_mem(mdp, (caddr_t)x, 8, MB_MINLINE); 434 } 435 436 int 437 md_get_int64be(struct mdchain *mdp, int64_t *x) 438 { 439 int64_t v; 440 int error; 441 442 error = md_get_int64(mdp, &v); 443 *x = betohq(v); 444 return error; 445 } 446 447 int 448 md_get_int64le(struct mdchain *mdp, int64_t *x) 449 { 450 int64_t v; 451 int error; 452 453 error = md_get_int64(mdp, &v); 454 *x = letohq(v); 455 return error; 456 } 457 458 int 459 md_get_mem(struct mdchain *mdp, caddr_t target, int size, int type) 460 { 461 struct mbuf *m = mdp->md_cur; 462 int error; 463 u_int count; 464 u_char *s; 465 466 while (size > 0) { 467 if (m == NULL) { 468 MBERROR("incomplete copy\n"); 469 return EBADRPC; 470 } 471 s = mdp->md_pos; 472 count = mtod(m, u_char*) + m->m_len - s; 473 if (count == 0) { 474 mdp->md_cur = m = m->m_next; 475 if (m) 476 s = mdp->md_pos = mtod(m, caddr_t); 477 continue; 478 } 479 if (count > size) 480 count = size; 481 size -= count; 482 mdp->md_pos += count; 483 if (target == NULL) 484 continue; 485 switch (type) { 486 case MB_MUSER: 487 error = copyout(s, target, count); 488 if (error) 489 return error; 490 break; 491 case MB_MSYSTEM: 492 bcopy(s, target, count); 493 break; 494 case MB_MINLINE: 495 while (count--) 496 *target++ = *s++; 497 continue; 498 } 499 target += count; 500 } 501 return 0; 502 } 503 504 int 505 md_get_mbuf(struct mdchain *mdp, int size, struct mbuf **ret) 506 { 507 struct mbuf *m = mdp->md_cur, *rm; 508 509 rm = m_copym(m, mdp->md_pos - mtod(m, u_char*), size, M_TRYWAIT); 510 if (rm == NULL) 511 return EBADRPC; 512 md_get_mem(mdp, NULL, size, MB_MZERO); 513 *ret = rm; 514 return 0; 515 } 516 517 int 518 md_get_uio(struct mdchain *mdp, struct uio *uiop, int size) 519 { 520 char *uiocp; 521 long left; 522 int mtype, error; 523 524 mtype = (uiop->uio_segflg == UIO_SYSSPACE) ? MB_MSYSTEM : MB_MUSER; 525 while (size > 0) { 526 if (uiop->uio_iovcnt <= 0 || uiop->uio_iov == NULL) 527 return EFBIG; 528 left = uiop->uio_iov->iov_len; 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 if (uiop->uio_iov->iov_len <= size) { 538 uiop->uio_iovcnt--; 539 uiop->uio_iov++; 540 } else { 541 uiop->uio_iov->iov_base += left; 542 uiop->uio_iov->iov_len -= left; 543 } 544 size -= left; 545 } 546 return 0; 547 } 548