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 */
49 struct uni_msg *
uni_msg_alloc(size_t s)50 uni_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 */
70 void
uni_msg_destroy(struct uni_msg * m)71 uni_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 */
82 int
uni_msg_extend(struct uni_msg * m,size_t s)83 uni_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 */
108 int
uni_msg_append(struct uni_msg * m,void * buf,size_t size)109 uni_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 */
125 struct uni_msg *
uni_msg_build(void * ptr,...)126 uni_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 */
163 u_int
uni_msg_strip32(struct uni_msg * msg)164 uni_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 */
176 u_int
uni_msg_get32(struct uni_msg * msg)177 uni_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 */
189 int
uni_msg_append32(struct uni_msg * msg,u_int u)190 uni_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 */
203 int
uni_msg_append8(struct uni_msg * msg,u_int u)204 uni_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 */
217 u_int
uni_msg_trail32(const struct uni_msg * msg,int i)218 uni_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 */
230 struct uni_msg *
uni_msg_dup(const struct uni_msg * inp)231 uni_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