1121330Sharti/*
2121330Sharti * Copyright (c) 1996-2003
3121330Sharti *	Fraunhofer Institute for Open Communication Systems (FhG Fokus).
4121330Sharti * 	All rights reserved.
5121330Sharti *
6121330Sharti * Redistribution and use in source and binary forms, with or without
7121330Sharti * modification, are permitted provided that the following conditions
8121330Sharti * are met:
9121330Sharti * 1. Redistributions of source code must retain the above copyright
10121330Sharti *    notice, this list of conditions and the following disclaimer.
11121330Sharti * 2. Redistributions in binary form must reproduce the above copyright
12121330Sharti *    notice, this list of conditions and the following disclaimer in the
13121330Sharti *    documentation and/or other materials provided with the distribution.
14121330Sharti *
15121330Sharti * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16121330Sharti * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17121330Sharti * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18121330Sharti * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19121330Sharti * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20121330Sharti * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21121330Sharti * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22121330Sharti * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23121330Sharti * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24121330Sharti * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25121330Sharti * SUCH DAMAGE.
26121330Sharti *
27121330Sharti * Author: Hartmut Brandt <harti@freebsd.org>
28121330Sharti *
29131823Sharti * $Begemot: libunimsg/libngatm/unimsg.c,v 1.4 2004/07/08 08:21:41 brandt Exp $
30121330Sharti *
31121330Sharti * User space message structure.
32121330Sharti */
33121330Sharti
34121330Sharti#include <stdio.h>
35121330Sharti#include <stdlib.h>
36121330Sharti#include <stdarg.h>
37121330Sharti#include <string.h>
38121330Sharti#include <errno.h>
39121330Sharti#include <arpa/inet.h>
40121330Sharti#include <netnatm/unimsg.h>
41121330Sharti
42121330Sharti/* the amount of extra bytes to allocate */
43121330Sharti#define EXTRA	128
44121330Sharti
45121330Sharti/*
46121330Sharti * Allocate a message that can hold at least 's' bytes. Return NULL if
47121330Sharti * allocation fails.
48121330Sharti */
49121330Shartistruct uni_msg *
50121330Shartiuni_msg_alloc(size_t s)
51121330Sharti{
52121330Sharti	struct uni_msg *m;
53121330Sharti
54121330Sharti	s += EXTRA;
55121330Sharti
56121330Sharti	if ((m = malloc(sizeof(struct uni_msg))) == NULL)
57121330Sharti		return NULL;
58121330Sharti	if ((m->b_buf = malloc(s)) == NULL) {
59121330Sharti		free(m);
60121330Sharti		return (NULL);
61121330Sharti	}
62121330Sharti	m->b_rptr = m->b_wptr = m->b_buf;
63121330Sharti	m->b_lim = m->b_buf + s;
64121330Sharti	return (m);
65121330Sharti}
66121330Sharti
67121330Sharti/*
68121330Sharti * Destroy the message and free memory
69121330Sharti */
70121330Shartivoid
71121330Shartiuni_msg_destroy(struct uni_msg *m)
72121330Sharti{
73121330Sharti	free(m->b_buf);
74121330Sharti	free(m);
75121330Sharti}
76121330Sharti
77121330Sharti/*
78121330Sharti * Extend message by at least 's' additional bytes.
79121330Sharti * May reallocate the message buffer. Return -1 on errors, 0 if ok.
80121330Sharti * If an error occurs the message is destroyed.
81121330Sharti */
82121330Shartiint
83121330Shartiuni_msg_extend(struct uni_msg *m, size_t s)
84121330Sharti{
85121330Sharti	u_char *b;
86121330Sharti	size_t len, leading, newsize;
87121330Sharti
88121330Sharti	len = uni_msg_len(m);
89121330Sharti	newsize = m->b_wptr - m->b_buf + s + EXTRA;
90121330Sharti	leading = m->b_rptr - m->b_buf;
91121330Sharti	if ((b = realloc(m->b_buf, newsize)) == NULL) {
92121330Sharti		free(m->b_buf);
93121330Sharti		free(m);
94121330Sharti		return (-1);
95121330Sharti	}
96121330Sharti	m->b_buf = b;
97121330Sharti	m->b_rptr = m->b_buf + leading;
98121330Sharti	m->b_wptr = m->b_rptr + len;
99121330Sharti	m->b_lim = m->b_buf + newsize;
100121330Sharti
101121330Sharti	return (0);
102121330Sharti}
103121330Sharti
104121330Sharti/*
105121330Sharti * Append the given buffer to the message. May reallocate the message
106121330Sharti * buffer. Return 0 if ok, -1 on errors.
107121330Sharti */
108121330Shartiint
109121330Shartiuni_msg_append(struct uni_msg *m, void *buf, size_t size)
110121330Sharti{
111121330Sharti	int error;
112121330Sharti
113121330Sharti	if ((error = uni_msg_ensure(m, size)))
114121330Sharti		return (error);
115121330Sharti	memcpy(m->b_wptr, buf, size);
116121330Sharti	m->b_wptr += size;
117121330Sharti
118121330Sharti	return (0);
119121330Sharti}
120121330Sharti
121121330Sharti/*
122121330Sharti * Construct a message from a number of pieces. The list of pieces must end
123121330Sharti * with a NULL pointer.
124121330Sharti */
125121330Shartistruct uni_msg *
126121330Shartiuni_msg_build(void *ptr, ...)
127121330Sharti{
128121330Sharti	va_list ap;
129121330Sharti	struct uni_msg *m;
130121330Sharti	size_t len, n;
131121330Sharti	void *p1;
132121330Sharti
133121330Sharti	len = 0;
134121330Sharti	va_start(ap, ptr);
135121330Sharti	p1 = ptr;
136121330Sharti	while (p1 != NULL) {
137121330Sharti		n = va_arg(ap, size_t);
138121330Sharti		len += n;
139121330Sharti		p1 = va_arg(ap, void *);
140121330Sharti	}
141121330Sharti	va_end(ap);
142121330Sharti
143121330Sharti	if ((m = uni_msg_alloc(len)) == NULL)
144121330Sharti		return (NULL);
145121330Sharti
146121330Sharti	va_start(ap, ptr);
147121330Sharti	p1 = ptr;
148121330Sharti	while (p1 != NULL) {
149121330Sharti		n = va_arg(ap, size_t);
150121330Sharti		memcpy(m->b_wptr, p1, n);
151121330Sharti		m->b_wptr += n;
152121330Sharti		p1 = va_arg(ap, void *);
153121330Sharti	}
154121330Sharti	va_end(ap);
155121330Sharti
156121330Sharti	return (m);
157121330Sharti}
158121330Sharti
159121330Sharti/*
160121330Sharti * Strip the last 32 bit word from the buffer.
161121330Sharti * Barf if there is no word left.
162121330Sharti */
163121330Shartiu_int
164121330Shartiuni_msg_strip32(struct uni_msg *msg)
165121330Sharti{
166131823Sharti	uint32_t w;
167121330Sharti
168121330Sharti	msg->b_wptr -= 4;
169121330Sharti	bcopy(msg->b_wptr, &w, 4);
170121330Sharti	return (ntohl(w));
171121330Sharti}
172121330Sharti
173121330Sharti/*
174121330Sharti * Strip the first four bytes of the buffer.
175121330Sharti */
176121330Shartiu_int
177121330Shartiuni_msg_get32(struct uni_msg *msg)
178121330Sharti{
179131823Sharti	uint32_t w;
180121330Sharti
181121330Sharti	bcopy(msg->b_rptr, &w, 4);
182121330Sharti	msg->b_rptr += 4;
183121330Sharti	return (ntohl(w));
184121330Sharti}
185121330Sharti
186121330Sharti/*
187121330Sharti * Append a 32 bit word to the buffer.
188121330Sharti */
189121330Shartiint
190121330Shartiuni_msg_append32(struct uni_msg *msg, u_int u)
191121330Sharti{
192121330Sharti	if (uni_msg_ensure(msg, 4) == -1)
193121330Sharti		return (-1);
194121330Sharti	u = htonl(u);
195121330Sharti	bcopy(&u, msg->b_wptr, 4);
196121330Sharti	msg->b_wptr += 4;
197121330Sharti	return (0);
198121330Sharti}
199121330Sharti
200121330Sharti/*
201121330Sharti * Append a byte to the buffer.
202121330Sharti */
203121330Shartiint
204121330Shartiuni_msg_append8(struct uni_msg *msg, u_int u)
205121330Sharti{
206121330Sharti	if (uni_msg_ensure(msg, 1) == -1)
207121330Sharti		return (-1);
208121330Sharti	*msg->b_wptr++ = u;
209121330Sharti	return (0);
210121330Sharti}
211121330Sharti
212121330Sharti/*
213121330Sharti * Return the i-th word counted from the end of the buffer.
214121330Sharti * i=-1 will return the last 32bit word, i=-2 the 2nd last.
215121330Sharti * Assumes that the word is in the buffer.
216121330Sharti */
217121330Shartiu_int
218121330Shartiuni_msg_trail32(const struct uni_msg *msg, int i)
219121330Sharti{
220121330Sharti	u_int w;
221121330Sharti
222121330Sharti	bcopy(msg->b_wptr + 4 * i, &w, 4);
223121330Sharti	return (ntohl(w));
224121330Sharti}
225121330Sharti
226121330Sharti
227121330Sharti/*
228121330Sharti * Make a duplicate.
229121330Sharti */
230121330Shartistruct uni_msg *
231121330Shartiuni_msg_dup(const struct uni_msg *inp)
232121330Sharti{
233121330Sharti	struct uni_msg *msg;
234121330Sharti	u_int len, off;
235121330Sharti
236121330Sharti	len = inp->b_wptr - inp->b_rptr;
237121330Sharti	off = inp->b_rptr - inp->b_buf;
238121330Sharti	if ((msg = uni_msg_alloc(inp->b_lim - inp->b_buf)) == NULL)
239121330Sharti		return (NULL);
240121330Sharti	msg->b_rptr = msg->b_buf + off;
241121330Sharti	msg->b_wptr = msg->b_rptr + len;
242121330Sharti	(void)memcpy(msg->b_rptr, inp->b_rptr, len);
243121330Sharti
244121330Sharti	return (msg);
245121330Sharti}
246