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
49static int ngatm_handler(module_t, int, void *);
50
51static moduledata_t ngatm_data = {
52	"ngatmbase",
53	ngatm_handler,
54	0
55};
56
57MODULE_VERSION(ngatmbase, NGATMBASE_VERSION);
58DECLARE_MODULE(ngatmbase, ngatm_data, SI_SUB_EXEC, SI_ORDER_ANY);
59
60/*********************************************************************/
61/*
62 * UNI Stack message handling functions
63 */
64static MALLOC_DEFINE(M_UNIMSG, "unimsg", "uni message buffers");
65static 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) */
70static struct mtx ngatm_unilist_mtx;
71
72/*
73 * Initialize UNI message subsystem
74 */
75static void
76uni_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 */
88int
89uni_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 */
117int
118uni_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 */
135struct mbuf *
136uni_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			MCLGET(m0, M_NOWAIT);
155			if (!(m0->m_flags & M_EXT))
156				goto drop;
157			if (n > MCLBYTES)
158				n = MCLBYTES;
159		}
160
161		bcopy(msg->b_rptr, m0->m_data, n);
162		msg->b_rptr += n;
163		m0->m_len = n;
164		m0->m_pkthdr.len = n;
165	}
166
167	last = m0;
168	while (msg != NULL && (n = uni_msg_len(msg)) != 0) {
169		MGET(m, M_NOWAIT, MT_DATA);
170		if (m == NULL)
171			goto drop;
172		last->m_next = m;
173		last = m;
174
175		if (n > MLEN) {
176			MCLGET(m, M_NOWAIT);
177			if (!(m->m_flags & M_EXT))
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 */
201struct 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 */
211static LIST_HEAD(, ngatm_msg) ngatm_freeuni =
212    LIST_HEAD_INITIALIZER(ngatm_freeuni);
213static LIST_HEAD(, ngatm_msg) ngatm_useduni =
214    LIST_HEAD_INITIALIZER(ngatm_useduni);
215
216/*
217 * Clean-up UNI message subsystem
218 */
219static void
220uni_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 */
241struct 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 */
277void
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 */
323struct ngatm_msg {
324	LIST_ENTRY(ngatm_msg) link;
325};
326
327/* Lists of free message headers.  */
328static LIST_HEAD(, ngatm_msg) ngatm_freeuni =
329    LIST_HEAD_INITIALIZER(ngatm_freeuni);
330
331/*
332 * Clean-up UNI message subsystem
333 */
334static void
335uni_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 */
351struct uni_msg *
352uni_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 */
386void
387uni_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
407struct uni_msg *
408_uni_msg_build(const char *file, int line, void *ptr, ...)
409#else
410struct uni_msg *
411uni_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
453int
454_uni_msg_unpack_mbuf(struct mbuf *m, struct uni_msg **pmsg, const char *file,
455    int line)
456#else
457int
458uni_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
480static int
481ngatm_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}
502