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