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