contrib/ngatm/libngatm/unimsg.c

   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 *
  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
  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
  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
 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 *
 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
 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
 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
 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
 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
 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 *
 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 }