1/*
2 * Copyright (c) 1996-2003
3 *	Fraunhofer Institute for Open Communication Systems (FhG Fokus).
4 * 	All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 *    notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 *    notice, this list of conditions and the following disclaimer in the
13 *    documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 *
27 * Author: Hartmut Brandt <harti@freebsd.org>
28 *
29 * $Begemot: libunimsg/libngatm/unimsg.c,v 1.4 2004/07/08 08:21:41 brandt Exp $
30 *
31 * User space message structure.
32 */
33
34#include <stdio.h>
35#include <stdlib.h>
36#include <stdarg.h>
37#include <string.h>
38#include <errno.h>
39#include <arpa/inet.h>
40#include <netnatm/unimsg.h>
41
42/* the amount of extra bytes to allocate */
43#define EXTRA	128
44
45/*
46 * Allocate a message that can hold at least 's' bytes. Return NULL if
47 * allocation fails.
48 */
49struct uni_msg *
50uni_msg_alloc(size_t s)
51{
52	struct uni_msg *m;
53
54	s += EXTRA;
55
56	if ((m = malloc(sizeof(struct uni_msg))) == NULL)
57		return NULL;
58	if ((m->b_buf = malloc(s)) == NULL) {
59		free(m);
60		return (NULL);
61	}
62	m->b_rptr = m->b_wptr = m->b_buf;
63	m->b_lim = m->b_buf + s;
64	return (m);
65}
66
67/*
68 * Destroy the message and free memory
69 */
70void
71uni_msg_destroy(struct uni_msg *m)
72{
73	free(m->b_buf);
74	free(m);
75}
76
77/*
78 * Extend message by at least 's' additional bytes.
79 * May reallocate the message buffer. Return -1 on errors, 0 if ok.
80 * If an error occurs the message is destroyed.
81 */
82int
83uni_msg_extend(struct uni_msg *m, size_t s)
84{
85	u_char *b;
86	size_t len, leading, newsize;
87
88	len = uni_msg_len(m);
89	newsize = m->b_wptr - m->b_buf + s + EXTRA;
90	leading = m->b_rptr - m->b_buf;
91	if ((b = realloc(m->b_buf, newsize)) == NULL) {
92		free(m->b_buf);
93		free(m);
94		return (-1);
95	}
96	m->b_buf = b;
97	m->b_rptr = m->b_buf + leading;
98	m->b_wptr = m->b_rptr + len;
99	m->b_lim = m->b_buf + newsize;
100
101	return (0);
102}
103
104/*
105 * Append the given buffer to the message. May reallocate the message
106 * buffer. Return 0 if ok, -1 on errors.
107 */
108int
109uni_msg_append(struct uni_msg *m, void *buf, size_t size)
110{
111	int error;
112
113	if ((error = uni_msg_ensure(m, size)))
114		return (error);
115	memcpy(m->b_wptr, buf, size);
116	m->b_wptr += size;
117
118	return (0);
119}
120
121/*
122 * Construct a message from a number of pieces. The list of pieces must end
123 * with a NULL pointer.
124 */
125struct uni_msg *
126uni_msg_build(void *ptr, ...)
127{
128	va_list ap;
129	struct uni_msg *m;
130	size_t len, n;
131	void *p1;
132
133	len = 0;
134	va_start(ap, ptr);
135	p1 = ptr;
136	while (p1 != NULL) {
137		n = va_arg(ap, size_t);
138		len += n;
139		p1 = va_arg(ap, void *);
140	}
141	va_end(ap);
142
143	if ((m = uni_msg_alloc(len)) == NULL)
144		return (NULL);
145
146	va_start(ap, ptr);
147	p1 = ptr;
148	while (p1 != NULL) {
149		n = va_arg(ap, size_t);
150		memcpy(m->b_wptr, p1, n);
151		m->b_wptr += n;
152		p1 = va_arg(ap, void *);
153	}
154	va_end(ap);
155
156	return (m);
157}
158
159/*
160 * Strip the last 32 bit word from the buffer.
161 * Barf if there is no word left.
162 */
163u_int
164uni_msg_strip32(struct uni_msg *msg)
165{
166	uint32_t w;
167
168	msg->b_wptr -= 4;
169	bcopy(msg->b_wptr, &w, 4);
170	return (ntohl(w));
171}
172
173/*
174 * Strip the first four bytes of the buffer.
175 */
176u_int
177uni_msg_get32(struct uni_msg *msg)
178{
179	uint32_t w;
180
181	bcopy(msg->b_rptr, &w, 4);
182	msg->b_rptr += 4;
183	return (ntohl(w));
184}
185
186/*
187 * Append a 32 bit word to the buffer.
188 */
189int
190uni_msg_append32(struct uni_msg *msg, u_int u)
191{
192	if (uni_msg_ensure(msg, 4) == -1)
193		return (-1);
194	u = htonl(u);
195	bcopy(&u, msg->b_wptr, 4);
196	msg->b_wptr += 4;
197	return (0);
198}
199
200/*
201 * Append a byte to the buffer.
202 */
203int
204uni_msg_append8(struct uni_msg *msg, u_int u)
205{
206	if (uni_msg_ensure(msg, 1) == -1)
207		return (-1);
208	*msg->b_wptr++ = u;
209	return (0);
210}
211
212/*
213 * Return the i-th word counted from the end of the buffer.
214 * i=-1 will return the last 32bit word, i=-2 the 2nd last.
215 * Assumes that the word is in the buffer.
216 */
217u_int
218uni_msg_trail32(const struct uni_msg *msg, int i)
219{
220	u_int w;
221
222	bcopy(msg->b_wptr + 4 * i, &w, 4);
223	return (ntohl(w));
224}
225
226
227/*
228 * Make a duplicate.
229 */
230struct uni_msg *
231uni_msg_dup(const struct uni_msg *inp)
232{
233	struct uni_msg *msg;
234	u_int len, off;
235
236	len = inp->b_wptr - inp->b_rptr;
237	off = inp->b_rptr - inp->b_buf;
238	if ((msg = uni_msg_alloc(inp->b_lim - inp->b_buf)) == NULL)
239		return (NULL);
240	msg->b_rptr = msg->b_buf + off;
241	msg->b_wptr = msg->b_rptr + len;
242	(void)memcpy(msg->b_rptr, inp->b_rptr, len);
243
244	return (msg);
245}
246