1/*- 2 * Copyright (c) 2001-2003 3 * Fraunhofer Institute for Open Communication Systems (FhG Fokus). 4 * All rights reserved. 5 * 6 * Author: Hartmut Brandt <harti@freebsd.org> 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 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 * In-kernel UNI stack message functions. 30 */ 31 32#include <sys/cdefs.h> 33__FBSDID("$FreeBSD$"); 34 35#include <sys/param.h> 36#include <sys/module.h> 37#include <sys/kernel.h> 38#include <sys/malloc.h> 39#include <sys/systm.h> 40#include <sys/lock.h> 41#include <sys/mutex.h> 42#include <sys/mbuf.h> 43#include <machine/stdarg.h> 44#include <netnatm/unimsg.h> 45#include <netgraph/atm/ngatmbase.h> 46 47#define NGATMBASE_VERSION 1 48 49static int ngatm_handler(module_t, int, void *); 50 51static moduledata_t ngatm_data = { 52 "ngatmbase", 53 ngatm_handler, 54 0 55}; 56 57MODULE_VERSION(ngatmbase, NGATMBASE_VERSION); 58DECLARE_MODULE(ngatmbase, ngatm_data, SI_SUB_EXEC, SI_ORDER_ANY); 59 60/*********************************************************************/ 61/* 62 * UNI Stack message handling functions 63 */ 64static MALLOC_DEFINE(M_UNIMSG, "unimsg", "uni message buffers"); 65static MALLOC_DEFINE(M_UNIMSGHDR, "unimsghdr", "uni message headers"); 66 67#define EXTRA 128 68 69/* mutex to protect the free list (and the used list if debugging) */ 70static struct mtx ngatm_unilist_mtx; 71 72/* 73 * Initialize UNI message subsystem 74 */ 75static void 76uni_msg_init(void) 77{ 78 mtx_init(&ngatm_unilist_mtx, "netgraph UNI msg header lists", NULL, 79 MTX_DEF); 80} 81 82/* 83 * Ensure, that the message can be extended by at least s bytes. 84 * Re-allocate the message (not the header). If that failes, 85 * free the entire message and return ENOMEM. Free space at the start of 86 * the message is retained. 87 */ 88int 89uni_msg_extend(struct uni_msg *m, size_t s) 90{ 91 u_char *b; 92 size_t len, lead; 93 94 lead = uni_msg_leading(m); 95 len = uni_msg_len(m); 96 s += lead + len + EXTRA; 97 if ((b = malloc(s, M_UNIMSG, M_NOWAIT)) == NULL) { 98 uni_msg_destroy(m); 99 return (ENOMEM); 100 } 101 102 bcopy(m->b_rptr, b + lead, len); 103 free(m->b_buf, M_UNIMSG); 104 105 m->b_buf = b; 106 m->b_rptr = m->b_buf + lead; 107 m->b_wptr = m->b_rptr + len; 108 m->b_lim = m->b_buf + s; 109 110 return (0); 111} 112 113/* 114 * Append a buffer to the message, making space if needed. 115 * If reallocation files, ENOMEM is returned and the message freed. 116 */ 117int 118uni_msg_append(struct uni_msg *m, void *buf, size_t size) 119{ 120 int error; 121 122 if ((error = uni_msg_ensure(m, size))) 123 return (error); 124 bcopy(buf, m->b_wptr, size); 125 m->b_wptr += size; 126 127 return (0); 128} 129 130/* 131 * Pack/unpack data from/into mbufs. Assume, that the (optional) header 132 * fits into the first mbuf, ie. hdrlen < MHLEN. Note, that the message 133 * can be NULL, but hdrlen should not be 0 in this case. 134 */ 135struct mbuf * 136uni_msg_pack_mbuf(struct uni_msg *msg, void *hdr, size_t hdrlen) 137{ 138 struct mbuf *m, *m0, *last; 139 size_t n; 140 141 MGETHDR(m0, M_NOWAIT, MT_DATA); 142 if (m0 == NULL) 143 return (NULL); 144 145 KASSERT(hdrlen <= MHLEN, ("uni_msg_pack_mbuf: hdrlen > MHLEN")); 146 147 if (hdrlen != 0) { 148 bcopy(hdr, m0->m_data, hdrlen); 149 m0->m_len = hdrlen; 150 m0->m_pkthdr.len = hdrlen; 151 152 } else { 153 if ((n = uni_msg_len(msg)) > MHLEN) { 154 MCLGET(m0, M_NOWAIT); 155 if (!(m0->m_flags & M_EXT)) 156 goto drop; 157 if (n > MCLBYTES) 158 n = MCLBYTES; 159 } 160 161 bcopy(msg->b_rptr, m0->m_data, n); 162 msg->b_rptr += n; 163 m0->m_len = n; 164 m0->m_pkthdr.len = n; 165 } 166 167 last = m0; 168 while (msg != NULL && (n = uni_msg_len(msg)) != 0) { 169 MGET(m, M_NOWAIT, MT_DATA); 170 if (m == NULL) 171 goto drop; 172 last->m_next = m; 173 last = m; 174 175 if (n > MLEN) { 176 MCLGET(m, M_NOWAIT); 177 if (!(m->m_flags & M_EXT)) 178 goto drop; 179 if (n > MCLBYTES) 180 n = MCLBYTES; 181 } 182 183 bcopy(msg->b_rptr, m->m_data, n); 184 msg->b_rptr += n; 185 m->m_len = n; 186 m0->m_pkthdr.len += n; 187 } 188 189 return (m0); 190 191 drop: 192 m_freem(m0); 193 return (NULL); 194} 195 196#ifdef NGATM_DEBUG 197 198/* 199 * Prepend a debugging header to each message 200 */ 201struct ngatm_msg { 202 LIST_ENTRY(ngatm_msg) link; 203 const char *file; 204 int line; 205 struct uni_msg msg; 206}; 207 208/* 209 * These are the lists of free and used message headers. 210 */ 211static LIST_HEAD(, ngatm_msg) ngatm_freeuni = 212 LIST_HEAD_INITIALIZER(ngatm_freeuni); 213static LIST_HEAD(, ngatm_msg) ngatm_useduni = 214 LIST_HEAD_INITIALIZER(ngatm_useduni); 215 216/* 217 * Clean-up UNI message subsystem 218 */ 219static void 220uni_msg_fini(void) 221{ 222 struct ngatm_msg *h; 223 224 /* free all free message headers */ 225 while ((h = LIST_FIRST(&ngatm_freeuni)) != NULL) { 226 LIST_REMOVE(h, link); 227 free(h, M_UNIMSGHDR); 228 } 229 230 /* forget about still used messages */ 231 LIST_FOREACH(h, &ngatm_useduni, link) 232 printf("unimsg header in use: %p (%s, %d)\n", 233 &h->msg, h->file, h->line); 234 235 mtx_destroy(&ngatm_unilist_mtx); 236} 237 238/* 239 * Allocate a message, that can hold at least s bytes. 240 */ 241struct uni_msg * 242_uni_msg_alloc(size_t s, const char *file, int line) 243{ 244 struct ngatm_msg *m; 245 246 mtx_lock(&ngatm_unilist_mtx); 247 if ((m = LIST_FIRST(&ngatm_freeuni)) != NULL) 248 LIST_REMOVE(m, link); 249 mtx_unlock(&ngatm_unilist_mtx); 250 251 if (m == NULL && 252 (m = malloc(sizeof(*m), M_UNIMSGHDR, M_NOWAIT)) == NULL) 253 return (NULL); 254 255 s += EXTRA; 256 if((m->msg.b_buf = malloc(s, M_UNIMSG, M_NOWAIT | M_ZERO)) == NULL) { 257 mtx_lock(&ngatm_unilist_mtx); 258 LIST_INSERT_HEAD(&ngatm_freeuni, m, link); 259 mtx_unlock(&ngatm_unilist_mtx); 260 return (NULL); 261 } 262 m->msg.b_rptr = m->msg.b_wptr = m->msg.b_buf; 263 m->msg.b_lim = m->msg.b_buf + s; 264 m->file = file; 265 m->line = line; 266 267 mtx_lock(&ngatm_unilist_mtx); 268 LIST_INSERT_HEAD(&ngatm_useduni, m, link); 269 mtx_unlock(&ngatm_unilist_mtx); 270 return (&m->msg); 271} 272 273/* 274 * Destroy a UNI message. 275 * The header is inserted into the free header list. 276 */ 277void 278_uni_msg_destroy(struct uni_msg *m, const char *file, int line) 279{ 280 struct ngatm_msg *h, *d; 281 282 d = (struct ngatm_msg *)((char *)m - offsetof(struct ngatm_msg, msg)); 283 284 mtx_lock(&ngatm_unilist_mtx); 285 LIST_FOREACH(h, &ngatm_useduni, link) 286 if (h == d) 287 break; 288 289 if (h == NULL) { 290 /* 291 * Not on used list. Ups. 292 */ 293 LIST_FOREACH(h, &ngatm_freeuni, link) 294 if (h == d) 295 break; 296 297 if (h == NULL) 298 printf("uni_msg %p was never allocated; found " 299 "in %s:%u\n", m, file, line); 300 else 301 printf("uni_msg %p was already destroyed in %s,%d; " 302 "found in %s:%u\n", m, h->file, h->line, 303 file, line); 304 } else { 305 free(m->b_buf, M_UNIMSG); 306 307 LIST_REMOVE(d, link); 308 LIST_INSERT_HEAD(&ngatm_freeuni, d, link); 309 310 d->file = file; 311 d->line = line; 312 } 313 314 mtx_unlock(&ngatm_unilist_mtx); 315} 316 317#else /* !NGATM_DEBUG */ 318 319/* 320 * This assumes, that sizeof(struct uni_msg) >= sizeof(struct ngatm_msg) 321 * and the alignment requirements of are the same. 322 */ 323struct ngatm_msg { 324 LIST_ENTRY(ngatm_msg) link; 325}; 326 327/* Lists of free message headers. */ 328static LIST_HEAD(, ngatm_msg) ngatm_freeuni = 329 LIST_HEAD_INITIALIZER(ngatm_freeuni); 330 331/* 332 * Clean-up UNI message subsystem 333 */ 334static void 335uni_msg_fini(void) 336{ 337 struct ngatm_msg *h; 338 339 /* free all free message headers */ 340 while ((h = LIST_FIRST(&ngatm_freeuni)) != NULL) { 341 LIST_REMOVE(h, link); 342 free(h, M_UNIMSGHDR); 343 } 344 345 mtx_destroy(&ngatm_unilist_mtx); 346} 347 348/* 349 * Allocate a message, that can hold at least s bytes. 350 */ 351struct uni_msg * 352uni_msg_alloc(size_t s) 353{ 354 struct ngatm_msg *a; 355 struct uni_msg *m; 356 357 mtx_lock(&ngatm_unilist_mtx); 358 if ((a = LIST_FIRST(&ngatm_freeuni)) != NULL) 359 LIST_REMOVE(a, link); 360 mtx_unlock(&ngatm_unilist_mtx); 361 362 if (a == NULL) { 363 if ((m = malloc(sizeof(*m), M_UNIMSGHDR, M_NOWAIT)) == NULL) 364 return (NULL); 365 a = (struct ngatm_msg *)m; 366 } else 367 m = (struct uni_msg *)a; 368 369 s += EXTRA; 370 if((m->b_buf = malloc(s, M_UNIMSG, M_NOWAIT | M_ZERO)) == NULL) { 371 mtx_lock(&ngatm_unilist_mtx); 372 LIST_INSERT_HEAD(&ngatm_freeuni, a, link); 373 mtx_unlock(&ngatm_unilist_mtx); 374 return (NULL); 375 } 376 m->b_rptr = m->b_wptr = m->b_buf; 377 m->b_lim = m->b_buf + s; 378 379 return (m); 380} 381 382/* 383 * Destroy a UNI message. 384 * The header is inserted into the free header list. 385 */ 386void 387uni_msg_destroy(struct uni_msg *m) 388{ 389 struct ngatm_msg *a; 390 391 a = (struct ngatm_msg *)m; 392 393 free(m->b_buf, M_UNIMSG); 394 395 mtx_lock(&ngatm_unilist_mtx); 396 LIST_INSERT_HEAD(&ngatm_freeuni, a, link); 397 mtx_unlock(&ngatm_unilist_mtx); 398} 399 400#endif 401 402/* 403 * Build a message from a number of buffers. Arguments are pairs 404 * of (void *, size_t) ending with a NULL pointer. 405 */ 406#ifdef NGATM_DEBUG 407struct uni_msg * 408_uni_msg_build(const char *file, int line, void *ptr, ...) 409#else 410struct uni_msg * 411uni_msg_build(void *ptr, ...) 412#endif 413{ 414 va_list ap; 415 struct uni_msg *m; 416 size_t len, n; 417 void *p1; 418 419 len = 0; 420 va_start(ap, ptr); 421 p1 = ptr; 422 while (p1 != NULL) { 423 n = va_arg(ap, size_t); 424 len += n; 425 p1 = va_arg(ap, void *); 426 } 427 va_end(ap); 428 429#ifdef NGATM_DEBUG 430 if ((m = _uni_msg_alloc(len, file, line)) == NULL) 431#else 432 if ((m = uni_msg_alloc(len)) == NULL) 433#endif 434 return (NULL); 435 436 va_start(ap, ptr); 437 p1 = ptr; 438 while (p1 != NULL) { 439 n = va_arg(ap, size_t); 440 bcopy(p1, m->b_wptr, n); 441 m->b_wptr += n; 442 p1 = va_arg(ap, void *); 443 } 444 va_end(ap); 445 446 return (m); 447} 448 449/* 450 * Unpack an mbuf chain into a uni_msg buffer. 451 */ 452#ifdef NGATM_DEBUG 453int 454_uni_msg_unpack_mbuf(struct mbuf *m, struct uni_msg **pmsg, const char *file, 455 int line) 456#else 457int 458uni_msg_unpack_mbuf(struct mbuf *m, struct uni_msg **pmsg) 459#endif 460{ 461 if (!(m->m_flags & M_PKTHDR)) { 462 printf("%s: bogus packet %p\n", __func__, m); 463 return (EINVAL); 464 } 465#ifdef NGATM_DEBUG 466 if ((*pmsg = _uni_msg_alloc(m->m_pkthdr.len, file, line)) == NULL) 467#else 468 if ((*pmsg = uni_msg_alloc(m->m_pkthdr.len)) == NULL) 469#endif 470 return (ENOMEM); 471 472 m_copydata(m, 0, m->m_pkthdr.len, (*pmsg)->b_wptr); 473 (*pmsg)->b_wptr += m->m_pkthdr.len; 474 475 return (0); 476} 477 478/*********************************************************************/ 479 480static int 481ngatm_handler(module_t mod, int what, void *arg) 482{ 483 int error = 0; 484 485 switch (what) { 486 487 case MOD_LOAD: 488 uni_msg_init(); 489 break; 490 491 case MOD_UNLOAD: 492 uni_msg_fini(); 493 break; 494 495 default: 496 error = EOPNOTSUPP; 497 break; 498 } 499 500 return (error); 501} 502