sys/netgraph/atm/ngatmbase.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   3  *
   4  * Copyright (c) 2001-2003
   5  *      Fraunhofer Institute for Open Communication Systems (FhG Fokus).
   6  *      All rights reserved.
   7  *
   8  * Author: Hartmut Brandt <harti@freebsd.org>
   9  *
  10  * Redistribution and use in source and binary forms, with or without
  11  * modification, are permitted provided that the following conditions
  12  * are met:
  13  * 1. Redistributions of source code must retain the above copyright
  14  *    notice, this list of conditions and the following disclaimer.
  15  * 2. Redistributions in binary form must reproduce the above copyright
  16  *    notice, this list of conditions and the following disclaimer in the
  17  *    documentation and/or other materials provided with the distribution.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  20  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  22  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  25  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  26  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  28  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  29  * SUCH DAMAGE.
  30  *
  31  * In-kernel UNI stack message functions.
  32  */
  33
  34 #include <sys/cdefs.h>
  35 __FBSDID("$FreeBSD$");
  36
  37 #include <sys/param.h>
  38 #include <sys/module.h>
  39 #include <sys/kernel.h>
  40 #include <sys/malloc.h>
  41 #include <sys/systm.h>
  42 #include <sys/lock.h>
  43 #include <sys/mutex.h>
  44 #include <sys/mbuf.h>
  45 #include <machine/stdarg.h>
  46 #include <netnatm/unimsg.h>
  47 #include <netgraph/atm/ngatmbase.h>
  48
  49 #define NGATMBASE_VERSION       1
  50
  51 static int ngatm_handler(module_t, int, void *);
  52
  53 static moduledata_t ngatm_data = {
  54         "ngatmbase",
  55         ngatm_handler,
  56         0
  57 };
  58
  59 MODULE_VERSION(ngatmbase, NGATMBASE_VERSION);
  60 DECLARE_MODULE(ngatmbase, ngatm_data, SI_SUB_EXEC, SI_ORDER_ANY);
  61
  62 /*********************************************************************/
  63 /*
  64  * UNI Stack message handling functions
  65  */
  66 static MALLOC_DEFINE(M_UNIMSG, "unimsg", "uni message buffers");
  67 static MALLOC_DEFINE(M_UNIMSGHDR, "unimsghdr", "uni message headers");
  68
  69 #define EXTRA   128
  70
  71 /* mutex to protect the free list (and the used list if debugging) */
  72 static struct mtx ngatm_unilist_mtx;
  73
  74 /*
  75  * Initialize UNI message subsystem
  76  */
  77 static void
  78 uni_msg_init(void)
  79 {
  80         mtx_init(&ngatm_unilist_mtx, "netgraph UNI msg header lists", NULL,
  81             MTX_DEF);
  82 }
  83
  84 /*
  85  * Ensure, that the message can be extended by at least s bytes.
  86  * Re-allocate the message (not the header). If that failes,
  87  * free the entire message and return ENOMEM. Free space at the start of
  88  * the message is retained.
  89  */
  90 int
  91 uni_msg_extend(struct uni_msg *m, size_t s)
  92 {
  93         u_char *b;
  94         size_t len, lead;
  95
  96         lead = uni_msg_leading(m);
  97         len = uni_msg_len(m);
  98         s += lead + len + EXTRA;
  99         if ((b = malloc(s, M_UNIMSG, M_NOWAIT)) == NULL) {
 100                 uni_msg_destroy(m);
 101                 return (ENOMEM);
 102         }
 103
 104         bcopy(m->b_rptr, b + lead, len);
 105         free(m->b_buf, M_UNIMSG);
 106
 107         m->b_buf = b;
 108         m->b_rptr = m->b_buf + lead;
 109         m->b_wptr = m->b_rptr + len;
 110         m->b_lim = m->b_buf + s;
 111
 112         return (0);
 113 }
 114
 115 /*
 116  * Append a buffer to the message, making space if needed.
 117  * If reallocation files, ENOMEM is returned and the message freed.
 118  */
 119 int
 120 uni_msg_append(struct uni_msg *m, void *buf, size_t size)
 121 {
 122         int error;
 123
 124         if ((error = uni_msg_ensure(m, size)))
 125                 return (error);
 126         bcopy(buf, m->b_wptr, size);
 127         m->b_wptr += size;
 128
 129         return (0);
 130 }
 131
 132 /*
 133  * Pack/unpack data from/into mbufs. Assume, that the (optional) header
 134  * fits into the first mbuf, ie. hdrlen < MHLEN. Note, that the message
 135  * can be NULL, but hdrlen should not be 0 in this case.
 136  */
 137 struct mbuf *
 138 uni_msg_pack_mbuf(struct uni_msg *msg, void *hdr, size_t hdrlen)
 139 {
 140         struct mbuf *m, *m0, *last;
 141         size_t n;
 142
 143         MGETHDR(m0, M_NOWAIT, MT_DATA);
 144         if (m0 == NULL)
 145                 return (NULL);
 146
 147         KASSERT(hdrlen <= MHLEN, ("uni_msg_pack_mbuf: hdrlen > MHLEN"));
 148
 149         if (hdrlen != 0) {
 150                 bcopy(hdr, m0->m_data, hdrlen);
 151                 m0->m_len = hdrlen;
 152                 m0->m_pkthdr.len = hdrlen;
 153
 154         } else {
 155                 if ((n = uni_msg_len(msg)) > MHLEN) {
 156                         if (!(MCLGET(m0, M_NOWAIT)))
 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, M_NOWAIT, MT_DATA);
 171                 if (m == NULL)
 172                         goto drop;
 173                 last->m_next = m;
 174                 last = m;
 175
 176                 if (n > MLEN) {
 177                         if (!(MCLGET(m, M_NOWAIT)))
 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 }