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 <[email protected]>
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
uni_msg_init(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
uni_msg_extend(struct uni_msg * m,size_t s)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
uni_msg_append(struct uni_msg * m,void * buf,size_t size)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 *
uni_msg_pack_mbuf(struct uni_msg * msg,void * hdr,size_t hdrlen)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
uni_msg_fini(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 *
_uni_msg_alloc(size_t s,const char * file,int line)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
_uni_msg_destroy(struct uni_msg * m,const char * file,int line)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
uni_msg_fini(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 *
uni_msg_alloc(size_t s)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
uni_msg_destroy(struct uni_msg * m)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 *
_uni_msg_build(const char * file,int line,void * ptr,...)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
_uni_msg_unpack_mbuf(struct mbuf * m,struct uni_msg ** pmsg,const char * file,int line)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
ngatm_handler(module_t mod,int what,void * arg)481 ngatm_handler(module_t mod, int what, void *arg)
482 {
483 int error = 0;
484
485 switch (what) {
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 }
501