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