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$");
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 static MALLOC_DEFINE(M_UNIMSG, "unimsg", "uni message buffers");
65 static 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
uni_msg_init(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
uni_msg_extend(struct uni_msg * m,size_t s)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
uni_msg_append(struct uni_msg * m,void * buf,size_t size)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 *
uni_msg_pack_mbuf(struct uni_msg * msg,void * hdr,size_t hdrlen)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 if (!(MCLGET(m0, M_NOWAIT)))
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, M_NOWAIT, MT_DATA);
169 if (m == NULL)
170 goto drop;
171 last->m_next = m;
172 last = m;
173
174 if (n > MLEN) {
175 if (!(MCLGET(m, M_NOWAIT)))
176 goto drop;
177 if (n > MCLBYTES)
178 n = MCLBYTES;
179 }
180
181 bcopy(msg->b_rptr, m->m_data, n);
182 msg->b_rptr += n;
183 m->m_len = n;
184 m0->m_pkthdr.len += n;
185 }
186
187 return (m0);
188
189 drop:
190 m_freem(m0);
191 return (NULL);
192 }
193
194 #ifdef NGATM_DEBUG
195
196 /*
197 * Prepend a debugging header to each message
198 */
199 struct ngatm_msg {
200 LIST_ENTRY(ngatm_msg) link;
201 const char *file;
202 int line;
203 struct uni_msg msg;
204 };
205
206 /*
207 * These are the lists of free and used message headers.
208 */
209 static LIST_HEAD(, ngatm_msg) ngatm_freeuni =
210 LIST_HEAD_INITIALIZER(ngatm_freeuni);
211 static LIST_HEAD(, ngatm_msg) ngatm_useduni =
212 LIST_HEAD_INITIALIZER(ngatm_useduni);
213
214 /*
215 * Clean-up UNI message subsystem
216 */
217 static void
uni_msg_fini(void)218 uni_msg_fini(void)
219 {
220 struct ngatm_msg *h;
221
222 /* free all free message headers */
223 while ((h = LIST_FIRST(&ngatm_freeuni)) != NULL) {
224 LIST_REMOVE(h, link);
225 free(h, M_UNIMSGHDR);
226 }
227
228 /* forget about still used messages */
229 LIST_FOREACH(h, &ngatm_useduni, link)
230 printf("unimsg header in use: %p (%s, %d)\n",
231 &h->msg, h->file, h->line);
232
233 mtx_destroy(&ngatm_unilist_mtx);
234 }
235
236 /*
237 * Allocate a message, that can hold at least s bytes.
238 */
239 struct uni_msg *
_uni_msg_alloc(size_t s,const char * file,int line)240 _uni_msg_alloc(size_t s, const char *file, int line)
241 {
242 struct ngatm_msg *m;
243
244 mtx_lock(&ngatm_unilist_mtx);
245 if ((m = LIST_FIRST(&ngatm_freeuni)) != NULL)
246 LIST_REMOVE(m, link);
247 mtx_unlock(&ngatm_unilist_mtx);
248
249 if (m == NULL &&
250 (m = malloc(sizeof(*m), M_UNIMSGHDR, M_NOWAIT)) == NULL)
251 return (NULL);
252
253 s += EXTRA;
254 if((m->msg.b_buf = malloc(s, M_UNIMSG, M_NOWAIT | M_ZERO)) == NULL) {
255 mtx_lock(&ngatm_unilist_mtx);
256 LIST_INSERT_HEAD(&ngatm_freeuni, m, link);
257 mtx_unlock(&ngatm_unilist_mtx);
258 return (NULL);
259 }
260 m->msg.b_rptr = m->msg.b_wptr = m->msg.b_buf;
261 m->msg.b_lim = m->msg.b_buf + s;
262 m->file = file;
263 m->line = line;
264
265 mtx_lock(&ngatm_unilist_mtx);
266 LIST_INSERT_HEAD(&ngatm_useduni, m, link);
267 mtx_unlock(&ngatm_unilist_mtx);
268 return (&m->msg);
269 }
270
271 /*
272 * Destroy a UNI message.
273 * The header is inserted into the free header list.
274 */
275 void
_uni_msg_destroy(struct uni_msg * m,const char * file,int line)276 _uni_msg_destroy(struct uni_msg *m, const char *file, int line)
277 {
278 struct ngatm_msg *h, *d;
279
280 d = (struct ngatm_msg *)((char *)m - offsetof(struct ngatm_msg, msg));
281
282 mtx_lock(&ngatm_unilist_mtx);
283 LIST_FOREACH(h, &ngatm_useduni, link)
284 if (h == d)
285 break;
286
287 if (h == NULL) {
288 /*
289 * Not on used list. Ups.
290 */
291 LIST_FOREACH(h, &ngatm_freeuni, link)
292 if (h == d)
293 break;
294
295 if (h == NULL)
296 printf("uni_msg %p was never allocated; found "
297 "in %s:%u\n", m, file, line);
298 else
299 printf("uni_msg %p was already destroyed in %s,%d; "
300 "found in %s:%u\n", m, h->file, h->line,
301 file, line);
302 } else {
303 free(m->b_buf, M_UNIMSG);
304
305 LIST_REMOVE(d, link);
306 LIST_INSERT_HEAD(&ngatm_freeuni, d, link);
307
308 d->file = file;
309 d->line = line;
310 }
311
312 mtx_unlock(&ngatm_unilist_mtx);
313 }
314
315 #else /* !NGATM_DEBUG */
316
317 /*
318 * This assumes, that sizeof(struct uni_msg) >= sizeof(struct ngatm_msg)
319 * and the alignment requirements of are the same.
320 */
321 struct ngatm_msg {
322 LIST_ENTRY(ngatm_msg) link;
323 };
324
325 /* Lists of free message headers. */
326 static LIST_HEAD(, ngatm_msg) ngatm_freeuni =
327 LIST_HEAD_INITIALIZER(ngatm_freeuni);
328
329 /*
330 * Clean-up UNI message subsystem
331 */
332 static void
uni_msg_fini(void)333 uni_msg_fini(void)
334 {
335 struct ngatm_msg *h;
336
337 /* free all free message headers */
338 while ((h = LIST_FIRST(&ngatm_freeuni)) != NULL) {
339 LIST_REMOVE(h, link);
340 free(h, M_UNIMSGHDR);
341 }
342
343 mtx_destroy(&ngatm_unilist_mtx);
344 }
345
346 /*
347 * Allocate a message, that can hold at least s bytes.
348 */
349 struct uni_msg *
uni_msg_alloc(size_t s)350 uni_msg_alloc(size_t s)
351 {
352 struct ngatm_msg *a;
353 struct uni_msg *m;
354
355 mtx_lock(&ngatm_unilist_mtx);
356 if ((a = LIST_FIRST(&ngatm_freeuni)) != NULL)
357 LIST_REMOVE(a, link);
358 mtx_unlock(&ngatm_unilist_mtx);
359
360 if (a == NULL) {
361 if ((m = malloc(sizeof(*m), M_UNIMSGHDR, M_NOWAIT)) == NULL)
362 return (NULL);
363 a = (struct ngatm_msg *)m;
364 } else
365 m = (struct uni_msg *)a;
366
367 s += EXTRA;
368 if((m->b_buf = malloc(s, M_UNIMSG, M_NOWAIT | M_ZERO)) == NULL) {
369 mtx_lock(&ngatm_unilist_mtx);
370 LIST_INSERT_HEAD(&ngatm_freeuni, a, link);
371 mtx_unlock(&ngatm_unilist_mtx);
372 return (NULL);
373 }
374 m->b_rptr = m->b_wptr = m->b_buf;
375 m->b_lim = m->b_buf + s;
376
377 return (m);
378 }
379
380 /*
381 * Destroy a UNI message.
382 * The header is inserted into the free header list.
383 */
384 void
uni_msg_destroy(struct uni_msg * m)385 uni_msg_destroy(struct uni_msg *m)
386 {
387 struct ngatm_msg *a;
388
389 a = (struct ngatm_msg *)m;
390
391 free(m->b_buf, M_UNIMSG);
392
393 mtx_lock(&ngatm_unilist_mtx);
394 LIST_INSERT_HEAD(&ngatm_freeuni, a, link);
395 mtx_unlock(&ngatm_unilist_mtx);
396 }
397
398 #endif
399
400 /*
401 * Build a message from a number of buffers. Arguments are pairs
402 * of (void *, size_t) ending with a NULL pointer.
403 */
404 #ifdef NGATM_DEBUG
405 struct uni_msg *
_uni_msg_build(const char * file,int line,void * ptr,...)406 _uni_msg_build(const char *file, int line, void *ptr, ...)
407 #else
408 struct uni_msg *
409 uni_msg_build(void *ptr, ...)
410 #endif
411 {
412 va_list ap;
413 struct uni_msg *m;
414 size_t len, n;
415 void *p1;
416
417 len = 0;
418 va_start(ap, ptr);
419 p1 = ptr;
420 while (p1 != NULL) {
421 n = va_arg(ap, size_t);
422 len += n;
423 p1 = va_arg(ap, void *);
424 }
425 va_end(ap);
426
427 #ifdef NGATM_DEBUG
428 if ((m = _uni_msg_alloc(len, file, line)) == NULL)
429 #else
430 if ((m = uni_msg_alloc(len)) == NULL)
431 #endif
432 return (NULL);
433
434 va_start(ap, ptr);
435 p1 = ptr;
436 while (p1 != NULL) {
437 n = va_arg(ap, size_t);
438 bcopy(p1, m->b_wptr, n);
439 m->b_wptr += n;
440 p1 = va_arg(ap, void *);
441 }
442 va_end(ap);
443
444 return (m);
445 }
446
447 /*
448 * Unpack an mbuf chain into a uni_msg buffer.
449 */
450 #ifdef NGATM_DEBUG
451 int
_uni_msg_unpack_mbuf(struct mbuf * m,struct uni_msg ** pmsg,const char * file,int line)452 _uni_msg_unpack_mbuf(struct mbuf *m, struct uni_msg **pmsg, const char *file,
453 int line)
454 #else
455 int
456 uni_msg_unpack_mbuf(struct mbuf *m, struct uni_msg **pmsg)
457 #endif
458 {
459 if (!(m->m_flags & M_PKTHDR)) {
460 printf("%s: bogus packet %p\n", __func__, m);
461 return (EINVAL);
462 }
463 #ifdef NGATM_DEBUG
464 if ((*pmsg = _uni_msg_alloc(m->m_pkthdr.len, file, line)) == NULL)
465 #else
466 if ((*pmsg = uni_msg_alloc(m->m_pkthdr.len)) == NULL)
467 #endif
468 return (ENOMEM);
469
470 m_copydata(m, 0, m->m_pkthdr.len, (*pmsg)->b_wptr);
471 (*pmsg)->b_wptr += m->m_pkthdr.len;
472
473 return (0);
474 }
475
476 /*********************************************************************/
477
478 static int
ngatm_handler(module_t mod,int what,void * arg)479 ngatm_handler(module_t mod, int what, void *arg)
480 {
481 int error = 0;
482
483 switch (what) {
484
485 case MOD_LOAD:
486 uni_msg_init();
487 break;
488
489 case MOD_UNLOAD:
490 uni_msg_fini();
491 break;
492
493 default:
494 error = EOPNOTSUPP;
495 break;
496 }
497
498 return (error);
499 }
500