sys/netgraph/atm/ngatmbase.c

   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
  49 static int ngatm_handler(module_t, int, void *);
  50
  51 static moduledata_t ngatm_data = {
  52         "ngatmbase",
  53         ngatm_handler,
  54         0
  55 };
  56
  57 MODULE_VERSION(ngatmbase, NGATMBASE_VERSION);
  58 DECLARE_MODULE(ngatmbase, ngatm_data, SI_SUB_EXEC, SI_ORDER_ANY);
  59
  60 /*********************************************************************/
  61 /*
  62  * UNI Stack message handling functions
  63  */
  64 static MALLOC_DEFINE(M_UNIMSG, "unimsg", "uni message buffers");
  65 static 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) */
  70 static struct mtx ngatm_unilist_mtx;
  71
  72 /*
  73  * Initialize UNI message subsystem
  74  */
  75 static void
  76 uni_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  */
  88 int
  89 uni_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  */
 117 int
 118 uni_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  */
 135 struct mbuf *
 136 uni_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                         if (!(MCLGET(m0, M_NOWAIT)))
 155                                 goto drop;
 156                         if (n > MCLBYTES)
 157                                 n = MCLBYTES;
 158                 }
 159
 160                 bcopy(msg->b_rptr, m0->m_data, n);
 161                 msg->b_rptr += n;
 162                 m0->m_len = n;
 163                 m0->m_pkthdr.len = n;
 164         }
 165
 166         last = m0;
 167         while (msg != NULL && (n = uni_msg_len(msg)) != 0) {
 168                 MGET(m, M_NOWAIT, MT_DATA);
 169                 if (m == NULL)
 170                         goto drop;
 171                 last->m_next = m;
 172                 last = m;
 173
 174                 if (n > MLEN) {
 175                         if (!(MCLGET(m, M_NOWAIT)))
 176                                 goto drop;
 177                         if (n > MCLBYTES)
 178                                 n = MCLBYTES;
 179                 }
 180
 181                 bcopy(msg->b_rptr, m->m_data, n);
 182                 msg->b_rptr += n;
 183                 m->m_len = n;
 184                 m0->m_pkthdr.len += n;
 185         }
 186
 187         return (m0);
 188
 189   drop:
 190         m_freem(m0);
 191         return (NULL);
 192 }
 193
 194 #ifdef NGATM_DEBUG
 195
 196 /*
 197  * Prepend a debugging header to each message
 198  */
 199 struct ngatm_msg {
 200         LIST_ENTRY(ngatm_msg) link;
 201         const char *file;
 202         int line;
 203         struct uni_msg msg;
 204 };
 205
 206 /*
 207  * These are the lists of free and used message headers.
 208  */
 209 static LIST_HEAD(, ngatm_msg) ngatm_freeuni =
 210     LIST_HEAD_INITIALIZER(ngatm_freeuni);
 211 static LIST_HEAD(, ngatm_msg) ngatm_useduni =
 212     LIST_HEAD_INITIALIZER(ngatm_useduni);
 213
 214 /*
 215  * Clean-up UNI message subsystem
 216  */
 217 static void
 218 uni_msg_fini(void)
 219 {
 220         struct ngatm_msg *h;
 221
 222         /* free all free message headers */
 223         while ((h = LIST_FIRST(&ngatm_freeuni)) != NULL) {
 224                 LIST_REMOVE(h, link);
 225                 free(h, M_UNIMSGHDR);
 226         }
 227
 228         /* forget about still used messages */
 229         LIST_FOREACH(h, &ngatm_useduni, link)
 230                 printf("unimsg header in use: %p (%s, %d)\n",
 231                     &h->msg, h->file, h->line);
 232
 233         mtx_destroy(&ngatm_unilist_mtx);
 234 }
 235
 236 /*
 237  * Allocate a message, that can hold at least s bytes.
 238  */
 239 struct uni_msg *
 240 _uni_msg_alloc(size_t s, const char *file, int line)
 241 {
 242         struct ngatm_msg *m;
 243
 244         mtx_lock(&ngatm_unilist_mtx);
 245         if ((m = LIST_FIRST(&ngatm_freeuni)) != NULL)
 246                 LIST_REMOVE(m, link);
 247         mtx_unlock(&ngatm_unilist_mtx);
 248
 249         if (m == NULL &&
 250             (m = malloc(sizeof(*m), M_UNIMSGHDR, M_NOWAIT)) == NULL)
 251                 return (NULL);
 252
 253         s += EXTRA;
 254         if((m->msg.b_buf = malloc(s, M_UNIMSG, M_NOWAIT | M_ZERO)) == NULL) {
 255                 mtx_lock(&ngatm_unilist_mtx);
 256                 LIST_INSERT_HEAD(&ngatm_freeuni, m, link);
 257                 mtx_unlock(&ngatm_unilist_mtx);
 258                 return (NULL);
 259         }
 260         m->msg.b_rptr = m->msg.b_wptr = m->msg.b_buf;
 261         m->msg.b_lim = m->msg.b_buf + s;
 262         m->file = file;
 263         m->line = line;
 264
 265         mtx_lock(&ngatm_unilist_mtx);
 266         LIST_INSERT_HEAD(&ngatm_useduni, m, link);
 267         mtx_unlock(&ngatm_unilist_mtx);
 268         return (&m->msg);
 269 }
 270
 271 /*
 272  * Destroy a UNI message.
 273  * The header is inserted into the free header list.
 274  */
 275 void
 276 _uni_msg_destroy(struct uni_msg *m, const char *file, int line)
 277 {
 278         struct ngatm_msg *h, *d;
 279
 280         d = (struct ngatm_msg *)((char *)m - offsetof(struct ngatm_msg, msg));
 281
 282         mtx_lock(&ngatm_unilist_mtx);
 283         LIST_FOREACH(h, &ngatm_useduni, link)
 284                 if (h == d)
 285                         break;
 286
 287         if (h == NULL) {
 288                 /*
 289                  * Not on used list. Ups.
 290                  */
 291                 LIST_FOREACH(h, &ngatm_freeuni, link)
 292                         if (h == d)
 293                                 break;
 294
 295                 if (h == NULL)
 296                         printf("uni_msg %p was never allocated; found "
 297                             "in %s:%u\n", m, file, line);
 298                 else
 299                         printf("uni_msg %p was already destroyed in %s,%d; "
 300                             "found in %s:%u\n", m, h->file, h->line,
 301                             file, line);
 302         } else {
 303                 free(m->b_buf, M_UNIMSG);
 304
 305                 LIST_REMOVE(d, link);
 306                 LIST_INSERT_HEAD(&ngatm_freeuni, d, link);
 307
 308                 d->file = file;
 309                 d->line = line;
 310         }
 311
 312         mtx_unlock(&ngatm_unilist_mtx);
 313 }
 314
 315 #else /* !NGATM_DEBUG */
 316
 317 /*
 318  * This assumes, that sizeof(struct uni_msg) >= sizeof(struct ngatm_msg)
 319  * and the alignment requirements of are the same.
 320  */
 321 struct ngatm_msg {
 322         LIST_ENTRY(ngatm_msg) link;
 323 };
 324
 325 /* Lists of free message headers.  */
 326 static LIST_HEAD(, ngatm_msg) ngatm_freeuni =
 327     LIST_HEAD_INITIALIZER(ngatm_freeuni);
 328
 329 /*
 330  * Clean-up UNI message subsystem
 331  */
 332 static void
 333 uni_msg_fini(void)
 334 {
 335         struct ngatm_msg *h;
 336
 337         /* free all free message headers */
 338         while ((h = LIST_FIRST(&ngatm_freeuni)) != NULL) {
 339                 LIST_REMOVE(h, link);
 340                 free(h, M_UNIMSGHDR);
 341         }
 342
 343         mtx_destroy(&ngatm_unilist_mtx);
 344 }
 345
 346 /*
 347  * Allocate a message, that can hold at least s bytes.
 348  */
 349 struct uni_msg *
 350 uni_msg_alloc(size_t s)
 351 {
 352         struct ngatm_msg *a;
 353         struct uni_msg *m;
 354
 355         mtx_lock(&ngatm_unilist_mtx);
 356         if ((a = LIST_FIRST(&ngatm_freeuni)) != NULL)
 357                 LIST_REMOVE(a, link);
 358         mtx_unlock(&ngatm_unilist_mtx);
 359
 360         if (a == NULL) {
 361                 if ((m = malloc(sizeof(*m), M_UNIMSGHDR, M_NOWAIT)) == NULL)
 362                         return (NULL);
 363                 a = (struct ngatm_msg *)m;
 364         } else
 365                 m = (struct uni_msg *)a;
 366
 367         s += EXTRA;
 368         if((m->b_buf = malloc(s, M_UNIMSG, M_NOWAIT | M_ZERO)) == NULL) {
 369                 mtx_lock(&ngatm_unilist_mtx);
 370                 LIST_INSERT_HEAD(&ngatm_freeuni, a, link);
 371                 mtx_unlock(&ngatm_unilist_mtx);
 372                 return (NULL);
 373         }
 374         m->b_rptr = m->b_wptr = m->b_buf;
 375         m->b_lim = m->b_buf + s;
 376
 377         return (m);
 378 }
 379
 380 /*
 381  * Destroy a UNI message.
 382  * The header is inserted into the free header list.
 383  */
 384 void
 385 uni_msg_destroy(struct uni_msg *m)
 386 {
 387         struct ngatm_msg *a;
 388
 389         a = (struct ngatm_msg *)m;
 390
 391         free(m->b_buf, M_UNIMSG);
 392
 393         mtx_lock(&ngatm_unilist_mtx);
 394         LIST_INSERT_HEAD(&ngatm_freeuni, a, link);
 395         mtx_unlock(&ngatm_unilist_mtx);
 396 }
 397
 398 #endif
 399
 400 /*
 401  * Build a message from a number of buffers. Arguments are pairs
 402  * of (void *, size_t) ending with a NULL pointer.
 403  */
 404 #ifdef NGATM_DEBUG
 405 struct uni_msg *
 406 _uni_msg_build(const char *file, int line, void *ptr, ...)
 407 #else
 408 struct uni_msg *
 409 uni_msg_build(void *ptr, ...)
 410 #endif
 411 {
 412         va_list ap;
 413         struct uni_msg *m;
 414         size_t len, n;
 415         void *p1;
 416
 417         len = 0;
 418         va_start(ap, ptr);
 419         p1 = ptr;
 420         while (p1 != NULL) {
 421                 n = va_arg(ap, size_t);
 422                 len += n;
 423                 p1 = va_arg(ap, void *);
 424         }
 425         va_end(ap);
 426
 427 #ifdef NGATM_DEBUG
 428         if ((m = _uni_msg_alloc(len, file, line)) == NULL)
 429 #else
 430         if ((m = uni_msg_alloc(len)) == NULL)
 431 #endif
 432                 return (NULL);
 433
 434         va_start(ap, ptr);
 435         p1 = ptr;
 436         while (p1 != NULL) {
 437                 n = va_arg(ap, size_t);
 438                 bcopy(p1, m->b_wptr, n);
 439                 m->b_wptr += n;
 440                 p1 = va_arg(ap, void *);
 441         }
 442         va_end(ap);
 443
 444         return (m);
 445 }
 446
 447 /*
 448  * Unpack an mbuf chain into a uni_msg buffer.
 449  */
 450 #ifdef NGATM_DEBUG
 451 int
 452 _uni_msg_unpack_mbuf(struct mbuf *m, struct uni_msg **pmsg, const char *file,
 453     int line)
 454 #else
 455 int
 456 uni_msg_unpack_mbuf(struct mbuf *m, struct uni_msg **pmsg)
 457 #endif
 458 {
 459         if (!(m->m_flags & M_PKTHDR)) {
 460                 printf("%s: bogus packet %p\n", __func__, m);
 461                 return (EINVAL);
 462         }
 463 #ifdef NGATM_DEBUG
 464         if ((*pmsg = _uni_msg_alloc(m->m_pkthdr.len, file, line)) == NULL)
 465 #else
 466         if ((*pmsg = uni_msg_alloc(m->m_pkthdr.len)) == NULL)
 467 #endif
 468                 return (ENOMEM);
 469
 470         m_copydata(m, 0, m->m_pkthdr.len, (*pmsg)->b_wptr);
 471         (*pmsg)->b_wptr += m->m_pkthdr.len;
 472
 473         return (0);
 474 }
 475
 476 /*********************************************************************/
 477
 478 static int
 479 ngatm_handler(module_t mod, int what, void *arg)
 480 {
 481         int error = 0;
 482
 483         switch (what) {
 484
 485           case MOD_LOAD:
 486                 uni_msg_init();
 487                 break;
 488
 489           case MOD_UNLOAD:
 490                 uni_msg_fini();
 491                 break;
 492
 493           default:
 494                 error = EOPNOTSUPP;
 495                 break;
 496         }
 497
 498         return (error);
 499 }