sys/netgraph7/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: src/sys/netgraph/atm/ngatmbase.c,v 1.3 2005/01/07 01:45:40 imp Exp $");
  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 MALLOC_DEFINE(M_UNIMSG, "unimsg", "uni message buffers");
  65 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                         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  */
 201 struct 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  */
 211 static LIST_HEAD(, ngatm_msg) ngatm_freeuni =
 212     LIST_HEAD_INITIALIZER(ngatm_freeuni);
 213 static LIST_HEAD(, ngatm_msg) ngatm_useduni =
 214     LIST_HEAD_INITIALIZER(ngatm_useduni);
 215
 216 /*
 217  * Clean-up UNI message subsystem
 218  */
 219 static void
 220 uni_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  */
 241 struct 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  */
 277 void
 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  */
 323 struct ngatm_msg {
 324         LIST_ENTRY(ngatm_msg) link;
 325 };
 326
 327 /* Lists of free message headers.  */
 328 static LIST_HEAD(, ngatm_msg) ngatm_freeuni =
 329     LIST_HEAD_INITIALIZER(ngatm_freeuni);
 330
 331 /*
 332  * Clean-up UNI message subsystem
 333  */
 334 static void
 335 uni_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  */
 351 struct uni_msg *
 352 uni_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  */
 386 void
 387 uni_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
 407 struct uni_msg *
 408 _uni_msg_build(const char *file, int line, void *ptr, ...)
 409 #else
 410 struct uni_msg *
 411 uni_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
 453 int
 454 _uni_msg_unpack_mbuf(struct mbuf *m, struct uni_msg **pmsg, const char *file,
 455     int line)
 456 #else
 457 int
 458 uni_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
 480 static int
 481 ngatm_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 }