subr_mchain.c revision 219028
1169689Skan/*- 2169689Skan * Copyright (c) 2000, 2001 Boris Popov 3169689Skan * All rights reserved. 4169689Skan * 5169689Skan * Redistribution and use in source and binary forms, with or without 6169689Skan * modification, are permitted provided that the following conditions 7169689Skan * are met: 8169689Skan * 1. Redistributions of source code must retain the above copyright 9169689Skan * notice, this list of conditions and the following disclaimer. 10169689Skan * 2. Redistributions in binary form must reproduce the above copyright 11169689Skan * notice, this list of conditions and the following disclaimer in the 12169689Skan * documentation and/or other materials provided with the distribution. 13169689Skan * 4. Neither the name of the author nor the names of any co-contributors 14169689Skan * may be used to endorse or promote products derived from this software 15169689Skan * without specific prior written permission. 16169689Skan * 17169689Skan * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 18169689Skan * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19169689Skan * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20169689Skan * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 21169689Skan * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22169689Skan * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 23169689Skan * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 24169689Skan * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25169689Skan * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 26169689Skan * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 27169689Skan * SUCH DAMAGE. 28169689Skan */ 29169689Skan 30169689Skan#include <sys/cdefs.h> 31169689Skan__FBSDID("$FreeBSD: head/sys/kern/subr_mchain.c 219028 2011-02-25 10:11:01Z netchild $"); 32169689Skan 33169689Skan#include <sys/param.h> 34169689Skan#include <sys/systm.h> 35169689Skan#include <sys/sysctl.h> 36169689Skan#include <sys/endian.h> 37169689Skan#include <sys/errno.h> 38169689Skan#include <sys/mbuf.h> 39169689Skan#include <sys/module.h> 40169689Skan#include <sys/uio.h> 41169689Skan 42169689Skan#include <sys/mchain.h> 43169689Skan 44169689SkanFEATURE(libmchain, "mchain library"); 45169689Skan 46169689SkanMODULE_VERSION(libmchain, 1); 47169689Skan 48169689Skan#define MBERROR(format, ...) printf("%s(%d): "format, __func__ , \ 49169689Skan __LINE__ , ## __VA_ARGS__) 50169689Skan 51169689Skan#define MBPANIC(format, ...) printf("%s(%d): "format, __func__ , \ 52169689Skan __LINE__ , ## __VA_ARGS__) 53169689Skan 54169689Skan/* 55169689Skan * Various helper functions 56169689Skan */ 57169689Skanint 58169689Skanmb_init(struct mbchain *mbp) 59169689Skan{ 60169689Skan struct mbuf *m; 61169689Skan 62169689Skan m = m_gethdr(M_WAIT, MT_DATA); 63169689Skan m->m_len = 0; 64169689Skan mb_initm(mbp, m); 65169689Skan return 0; 66169689Skan} 67169689Skan 68169689Skanvoid 69169689Skanmb_initm(struct mbchain *mbp, struct mbuf *m) 70169689Skan{ 71169689Skan bzero(mbp, sizeof(*mbp)); 72169689Skan mbp->mb_top = mbp->mb_cur = m; 73169689Skan mbp->mb_mleft = M_TRAILINGSPACE(m); 74169689Skan} 75169689Skan 76169689Skanvoid 77169689Skanmb_done(struct mbchain *mbp) 78169689Skan{ 79169689Skan if (mbp->mb_top) { 80169689Skan m_freem(mbp->mb_top); 81169689Skan mbp->mb_top = NULL; 82169689Skan } 83169689Skan} 84169689Skan 85169689Skanstruct mbuf * 86169689Skanmb_detach(struct mbchain *mbp) 87169689Skan{ 88169689Skan struct mbuf *m; 89169689Skan 90169689Skan m = mbp->mb_top; 91169689Skan mbp->mb_top = NULL; 92169689Skan return m; 93169689Skan} 94169689Skan 95169689Skanint 96169689Skanmb_fixhdr(struct mbchain *mbp) 97169689Skan{ 98169689Skan return mbp->mb_top->m_pkthdr.len = m_fixhdr(mbp->mb_top); 99169689Skan} 100169689Skan 101169689Skan/* 102169689Skan * Check if object of size 'size' fit to the current position and 103169689Skan * allocate new mbuf if not. Advance pointers and increase length of mbuf(s). 104169689Skan * Return pointer to the object placeholder or NULL if any error occured. 105169689Skan * Note: size should be <= MLEN 106169689Skan */ 107169689Skancaddr_t 108169689Skanmb_reserve(struct mbchain *mbp, int size) 109169689Skan{ 110169689Skan struct mbuf *m, *mn; 111169689Skan caddr_t bpos; 112169689Skan 113169689Skan if (size > MLEN) 114169689Skan panic("mb_reserve: size = %d\n", size); 115169689Skan m = mbp->mb_cur; 116169689Skan if (mbp->mb_mleft < size) { 117169689Skan mn = m_get(M_WAIT, MT_DATA); 118169689Skan mbp->mb_cur = m->m_next = mn; 119169689Skan m = mn; 120169689Skan m->m_len = 0; 121169689Skan mbp->mb_mleft = M_TRAILINGSPACE(m); 122169689Skan } 123169689Skan mbp->mb_mleft -= size; 124169689Skan mbp->mb_count += size; 125169689Skan bpos = mtod(m, caddr_t) + m->m_len; 126169689Skan m->m_len += size; 127169689Skan return bpos; 128169689Skan} 129169689Skan 130169689Skanint 131169689Skanmb_put_uint8(struct mbchain *mbp, uint8_t x) 132169689Skan{ 133169689Skan return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM); 134169689Skan} 135169689Skan 136169689Skanint 137169689Skanmb_put_uint16be(struct mbchain *mbp, uint16_t x) 138169689Skan{ 139169689Skan x = htobe16(x); 140169689Skan return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM); 141169689Skan} 142169689Skan 143169689Skanint 144169689Skanmb_put_uint16le(struct mbchain *mbp, uint16_t x) 145169689Skan{ 146169689Skan x = htole16(x); 147169689Skan return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM); 148169689Skan} 149169689Skan 150169689Skanint 151169689Skanmb_put_uint32be(struct mbchain *mbp, uint32_t x) 152169689Skan{ 153169689Skan x = htobe32(x); 154169689Skan return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM); 155169689Skan} 156169689Skan 157169689Skanint 158169689Skanmb_put_uint32le(struct mbchain *mbp, uint32_t x) 159169689Skan{ 160169689Skan x = htole32(x); 161169689Skan return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM); 162169689Skan} 163169689Skan 164169689Skanint 165169689Skanmb_put_int64be(struct mbchain *mbp, int64_t x) 166169689Skan{ 167169689Skan x = htobe64(x); 168169689Skan return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM); 169169689Skan} 170169689Skan 171169689Skanint 172169689Skanmb_put_int64le(struct mbchain *mbp, int64_t x) 173169689Skan{ 174169689Skan x = htole64(x); 175169689Skan return mb_put_mem(mbp, (caddr_t)&x, sizeof(x), MB_MSYSTEM); 176169689Skan} 177169689Skan 178169689Skanint 179169689Skanmb_put_mem(struct mbchain *mbp, c_caddr_t source, int size, int type) 180169689Skan{ 181169689Skan struct mbuf *m; 182169689Skan caddr_t dst; 183169689Skan c_caddr_t src; 184169689Skan int cplen, error, mleft, count; 185169689Skan size_t srclen, dstlen; 186169689Skan 187169689Skan m = mbp->mb_cur; 188169689Skan mleft = mbp->mb_mleft; 189169689Skan 190169689Skan while (size > 0) { 191169689Skan if (mleft == 0) { 192169689Skan if (m->m_next == NULL) 193169689Skan m = m_getm(m, size, M_WAIT, MT_DATA); 194169689Skan else 195169689Skan m = m->m_next; 196169689Skan mleft = M_TRAILINGSPACE(m); 197169689Skan continue; 198169689Skan } 199169689Skan cplen = mleft > size ? size : mleft; 200169689Skan srclen = dstlen = cplen; 201169689Skan dst = mtod(m, caddr_t) + m->m_len; 202169689Skan switch (type) { 203169689Skan case MB_MCUSTOM: 204169689Skan srclen = size; 205169689Skan dstlen = mleft; 206169689Skan error = mbp->mb_copy(mbp, source, dst, &srclen, &dstlen); 207169689Skan if (error) 208169689Skan return error; 209169689Skan break; 210169689Skan case MB_MINLINE: 211169689Skan for (src = source, count = cplen; count; count--) 212169689Skan *dst++ = *src++; 213169689Skan break; 214169689Skan case MB_MSYSTEM: 215169689Skan bcopy(source, dst, cplen); 216169689Skan break; 217169689Skan case MB_MUSER: 218169689Skan error = copyin(source, dst, cplen); 219169689Skan if (error) 220169689Skan return error; 221169689Skan break; 222169689Skan case MB_MZERO: 223169689Skan bzero(dst, cplen); 224169689Skan break; 225169689Skan } 226169689Skan size -= srclen; 227169689Skan source += srclen; 228169689Skan m->m_len += dstlen; 229169689Skan mleft -= dstlen; 230169689Skan mbp->mb_count += dstlen; 231169689Skan } 232169689Skan mbp->mb_cur = m; 233169689Skan mbp->mb_mleft = mleft; 234169689Skan return 0; 235169689Skan} 236169689Skan 237169689Skanint 238169689Skanmb_put_mbuf(struct mbchain *mbp, struct mbuf *m) 239169689Skan{ 240169689Skan mbp->mb_cur->m_next = m; 241169689Skan while (m) { 242169689Skan mbp->mb_count += m->m_len; 243169689Skan if (m->m_next == NULL) 244169689Skan break; 245169689Skan m = m->m_next; 246169689Skan } 247169689Skan mbp->mb_mleft = M_TRAILINGSPACE(m); 248169689Skan mbp->mb_cur = m; 249169689Skan return 0; 250169689Skan} 251169689Skan 252169689Skan/* 253169689Skan * copies a uio scatter/gather list to an mbuf chain. 254169689Skan */ 255169689Skanint 256169689Skanmb_put_uio(struct mbchain *mbp, struct uio *uiop, int size) 257169689Skan{ 258169689Skan long left; 259169689Skan int mtype, error; 260169689Skan 261169689Skan mtype = (uiop->uio_segflg == UIO_SYSSPACE) ? MB_MSYSTEM : MB_MUSER; 262169689Skan 263169689Skan while (size > 0 && uiop->uio_resid) { 264169689Skan if (uiop->uio_iovcnt <= 0 || uiop->uio_iov == NULL) 265169689Skan return EFBIG; 266169689Skan left = uiop->uio_iov->iov_len; 267169689Skan if (left == 0) { 268169689Skan uiop->uio_iov++; 269169689Skan uiop->uio_iovcnt--; 270169689Skan continue; 271169689Skan } 272169689Skan if (left > size) 273169689Skan left = size; 274169689Skan error = mb_put_mem(mbp, uiop->uio_iov->iov_base, left, mtype); 275169689Skan if (error) 276169689Skan return error; 277169689Skan uiop->uio_offset += left; 278169689Skan uiop->uio_resid -= left; 279169689Skan uiop->uio_iov->iov_base = 280169689Skan (char *)uiop->uio_iov->iov_base + left; 281169689Skan uiop->uio_iov->iov_len -= left; 282169689Skan size -= left; 283169689Skan } 284169689Skan return 0; 285169689Skan} 286169689Skan 287169689Skan/* 288169689Skan * Routines for fetching data from an mbuf chain 289169689Skan */ 290169689Skanint 291169689Skanmd_init(struct mdchain *mdp) 292169689Skan{ 293169689Skan struct mbuf *m; 294169689Skan 295169689Skan m = m_gethdr(M_WAIT, MT_DATA); 296169689Skan m->m_len = 0; 297169689Skan md_initm(mdp, m); 298169689Skan return 0; 299169689Skan} 300169689Skan 301169689Skanvoid 302169689Skanmd_initm(struct mdchain *mdp, struct mbuf *m) 303169689Skan{ 304169689Skan bzero(mdp, sizeof(*mdp)); 305169689Skan mdp->md_top = mdp->md_cur = m; 306169689Skan mdp->md_pos = mtod(m, u_char*); 307169689Skan} 308169689Skan 309169689Skanvoid 310169689Skanmd_done(struct mdchain *mdp) 311169689Skan{ 312169689Skan if (mdp->md_top) { 313169689Skan m_freem(mdp->md_top); 314169689Skan mdp->md_top = NULL; 315169689Skan } 316169689Skan} 317169689Skan 318169689Skan/* 319169689Skan * Append a separate mbuf chain. It is caller responsibility to prevent 320169689Skan * multiple calls to fetch/record routines. 321169689Skan */ 322169689Skanvoid 323169689Skanmd_append_record(struct mdchain *mdp, struct mbuf *top) 324169689Skan{ 325169689Skan struct mbuf *m; 326169689Skan 327169689Skan if (mdp->md_top == NULL) { 328169689Skan md_initm(mdp, top); 329169689Skan return; 330169689Skan } 331169689Skan m = mdp->md_top; 332169689Skan while (m->m_nextpkt) 333169689Skan m = m->m_nextpkt; 334169689Skan m->m_nextpkt = top; 335169689Skan top->m_nextpkt = NULL; 336169689Skan return; 337169689Skan} 338169689Skan 339169689Skan/* 340169689Skan * Put next record in place of existing 341169689Skan */ 342169689Skanint 343169689Skanmd_next_record(struct mdchain *mdp) 344169689Skan{ 345169689Skan struct mbuf *m; 346169689Skan 347169689Skan if (mdp->md_top == NULL) 348169689Skan return ENOENT; 349169689Skan m = mdp->md_top->m_nextpkt; 350169689Skan md_done(mdp); 351169689Skan if (m == NULL) 352169689Skan return ENOENT; 353169689Skan md_initm(mdp, m); 354169689Skan return 0; 355169689Skan} 356169689Skan 357169689Skanint 358169689Skanmd_get_uint8(struct mdchain *mdp, uint8_t *x) 359169689Skan{ 360169689Skan return md_get_mem(mdp, x, 1, MB_MINLINE); 361169689Skan} 362169689Skan 363169689Skanint 364169689Skanmd_get_uint16(struct mdchain *mdp, uint16_t *x) 365169689Skan{ 366169689Skan return md_get_mem(mdp, (caddr_t)x, 2, MB_MINLINE); 367169689Skan} 368169689Skan 369169689Skanint 370169689Skanmd_get_uint16le(struct mdchain *mdp, uint16_t *x) 371169689Skan{ 372169689Skan uint16_t v; 373169689Skan int error = md_get_uint16(mdp, &v); 374169689Skan 375169689Skan if (x != NULL) 376169689Skan *x = le16toh(v); 377169689Skan return error; 378169689Skan} 379169689Skan 380169689Skanint 381169689Skanmd_get_uint16be(struct mdchain *mdp, uint16_t *x) 382169689Skan{ 383169689Skan uint16_t v; 384169689Skan int error = md_get_uint16(mdp, &v); 385169689Skan 386169689Skan if (x != NULL) 387169689Skan *x = be16toh(v); 388169689Skan return error; 389169689Skan} 390169689Skan 391169689Skanint 392169689Skanmd_get_uint32(struct mdchain *mdp, uint32_t *x) 393169689Skan{ 394169689Skan return md_get_mem(mdp, (caddr_t)x, 4, MB_MINLINE); 395169689Skan} 396169689Skan 397169689Skanint 398169689Skanmd_get_uint32be(struct mdchain *mdp, uint32_t *x) 399169689Skan{ 400169689Skan uint32_t v; 401169689Skan int error; 402169689Skan 403169689Skan error = md_get_uint32(mdp, &v); 404169689Skan if (x != NULL) 405169689Skan *x = be32toh(v); 406169689Skan return error; 407169689Skan} 408169689Skan 409169689Skanint 410169689Skanmd_get_uint32le(struct mdchain *mdp, uint32_t *x) 411169689Skan{ 412169689Skan uint32_t v; 413169689Skan int error; 414169689Skan 415169689Skan error = md_get_uint32(mdp, &v); 416169689Skan if (x != NULL) 417169689Skan *x = le32toh(v); 418169689Skan return error; 419169689Skan} 420169689Skan 421169689Skanint 422169689Skanmd_get_int64(struct mdchain *mdp, int64_t *x) 423169689Skan{ 424169689Skan return md_get_mem(mdp, (caddr_t)x, 8, MB_MINLINE); 425169689Skan} 426169689Skan 427169689Skanint 428169689Skanmd_get_int64be(struct mdchain *mdp, int64_t *x) 429169689Skan{ 430169689Skan int64_t v; 431169689Skan int error; 432169689Skan 433169689Skan error = md_get_int64(mdp, &v); 434169689Skan if (x != NULL) 435169689Skan *x = be64toh(v); 436169689Skan return error; 437169689Skan} 438169689Skan 439169689Skanint 440169689Skanmd_get_int64le(struct mdchain *mdp, int64_t *x) 441169689Skan{ 442169689Skan int64_t v; 443169689Skan int error; 444169689Skan 445169689Skan error = md_get_int64(mdp, &v); 446169689Skan if (x != NULL) 447169689Skan *x = le64toh(v); 448169689Skan return error; 449169689Skan} 450169689Skan 451169689Skanint 452169689Skanmd_get_mem(struct mdchain *mdp, caddr_t target, int size, int type) 453169689Skan{ 454169689Skan struct mbuf *m = mdp->md_cur; 455169689Skan int error; 456169689Skan u_int count; 457169689Skan u_char *s; 458169689Skan 459169689Skan while (size > 0) { 460169689Skan if (m == NULL) { 461169689Skan MBERROR("incomplete copy\n"); 462169689Skan return EBADRPC; 463169689Skan } 464169689Skan s = mdp->md_pos; 465169689Skan count = mtod(m, u_char*) + m->m_len - s; 466169689Skan if (count == 0) { 467169689Skan mdp->md_cur = m = m->m_next; 468169689Skan if (m) 469169689Skan s = mdp->md_pos = mtod(m, caddr_t); 470169689Skan continue; 471169689Skan } 472169689Skan if (count > size) 473169689Skan count = size; 474169689Skan size -= count; 475169689Skan mdp->md_pos += count; 476169689Skan if (target == NULL) 477169689Skan continue; 478169689Skan switch (type) { 479169689Skan case MB_MUSER: 480169689Skan error = copyout(s, target, count); 481169689Skan if (error) 482169689Skan return error; 483169689Skan break; 484169689Skan case MB_MSYSTEM: 485169689Skan bcopy(s, target, count); 486169689Skan break; 487169689Skan case MB_MINLINE: 488169689Skan while (count--) 489169689Skan *target++ = *s++; 490169689Skan continue; 491169689Skan } 492169689Skan target += count; 493169689Skan } 494169689Skan return 0; 495169689Skan} 496169689Skan 497169689Skanint 498169689Skanmd_get_mbuf(struct mdchain *mdp, int size, struct mbuf **ret) 499169689Skan{ 500169689Skan struct mbuf *m = mdp->md_cur, *rm; 501169689Skan 502169689Skan rm = m_copym(m, mdp->md_pos - mtod(m, u_char*), size, M_WAIT); 503169689Skan md_get_mem(mdp, NULL, size, MB_MZERO); 504169689Skan *ret = rm; 505169689Skan return 0; 506169689Skan} 507169689Skan 508169689Skanint 509169689Skanmd_get_uio(struct mdchain *mdp, struct uio *uiop, int size) 510169689Skan{ 511169689Skan char *uiocp; 512169689Skan long left; 513169689Skan int mtype, error; 514169689Skan 515169689Skan mtype = (uiop->uio_segflg == UIO_SYSSPACE) ? MB_MSYSTEM : MB_MUSER; 516169689Skan while (size > 0 && uiop->uio_resid) { 517169689Skan if (uiop->uio_iovcnt <= 0 || uiop->uio_iov == NULL) 518169689Skan return EFBIG; 519169689Skan left = uiop->uio_iov->iov_len; 520169689Skan if (left == 0) { 521169689Skan uiop->uio_iov++; 522169689Skan uiop->uio_iovcnt--; 523169689Skan continue; 524169689Skan } 525169689Skan uiocp = uiop->uio_iov->iov_base; 526169689Skan if (left > size) 527169689Skan left = size; 528169689Skan error = md_get_mem(mdp, uiocp, left, mtype); 529169689Skan if (error) 530169689Skan return error; 531169689Skan uiop->uio_offset += left; 532169689Skan uiop->uio_resid -= left; 533169689Skan uiop->uio_iov->iov_base = 534169689Skan (char *)uiop->uio_iov->iov_base + left; 535169689Skan uiop->uio_iov->iov_len -= left; 536169689Skan size -= left; 537169689Skan } 538169689Skan return 0; 539169689Skan} 540169689Skan