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