libdnet-1.11/src/blob.c Source File

00001 /*
00002  * blob.c
00003  *
00004  * Copyright (c) 2002 Dug Song <dugsong@monkey.org>
00005  *
00006  * $Id: blob.c,v 1.5 2006/01/08 16:06:49 dugsong Exp $
00007  */
00008 
00009 #include "config.h"
00010 
00011 #include <ctype.h>
00012 #include <stdarg.h>
00013 #include <stdio.h>
00014 #include <stdlib.h>
00015 #include <string.h>
00016 
00017 #include "dnet.h"
00018 
00019 static void     *(*bl_malloc)(size_t) = malloc;
00020 static void     *(*bl_realloc)(void *, size_t) = realloc;
00021 static void      (*bl_free)(void *) = free;
00022 static int         bl_size = BUFSIZ;
00023 
00024 static int         fmt_D(int, int, blob_t *, va_list *);
00025 static int         fmt_H(int, int, blob_t *, va_list *);
00026 static int         fmt_b(int, int, blob_t *, va_list *);
00027 static int         fmt_c(int, int, blob_t *, va_list *);
00028 static int         fmt_d(int, int, blob_t *, va_list *);
00029 static int         fmt_h(int, int, blob_t *, va_list *);
00030 static int         fmt_s(int, int, blob_t *, va_list *);
00031 
00032 static void        print_hexl(blob_t *);
00033 
00034 static blob_fmt_cb blob_ascii_fmt[] = {
00035         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00036         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00037         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00038         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00039         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00040         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00041         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00042         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00043         NULL,   NULL,   NULL,   NULL,   fmt_D,  NULL,   NULL,   NULL,
00044         fmt_H,  NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00045         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00046         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00047         NULL,   NULL,   fmt_b,  fmt_c,  fmt_d,  NULL,   NULL,   NULL,
00048         fmt_h,  NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,
00049         NULL,   NULL,   NULL,   fmt_s,  NULL,   NULL,   NULL,   NULL,
00050         NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL,   NULL
00051 };
00052 
00053 struct blob_printer {
00054         char      *name;
00055         void     (*print)(blob_t *);
00056 } blob_printers[] = {
00057         { "hexl",       print_hexl },
00058         { NULL,         NULL },
00059 };
00060 
00061 blob_t *
00062 blob_new(void)
00063 {
00064         blob_t *b;
00065 
00066         if ((b = bl_malloc(sizeof(*b))) != NULL) {
00067                 b->off = b->end = 0;
00068                 b->size = bl_size;
00069                 if ((b->base = bl_malloc(b->size)) == NULL) {
00070                         bl_free(b);
00071                         b = NULL;
00072                 }
00073         }
00074         return (b);
00075 }
00076 
00077 static int
00078 blob_reserve(blob_t *b, int len)
00079 {
00080         void *p;
00081         int nsize;
00082 
00083         if (b->size < b->end + len) {
00084                 if (b->size == 0)
00085                         return (-1);
00086 
00087                 if ((nsize = b->end + len) > bl_size)
00088                         nsize = ((nsize / bl_size) + 1) * bl_size;
00089                 
00090                 if ((p = bl_realloc(b->base, nsize)) == NULL)
00091                         return (-1);
00092                 
00093                 b->base = p;
00094                 b->size = nsize;
00095         }
00096         b->end += len;
00097         
00098         return (0);
00099 }
00100 
00101 int
00102 blob_read(blob_t *b, void *buf, int len)
00103 {
00104         if (b->end - b->off < len)
00105                 len = b->end - b->off;
00106         
00107         memcpy(buf, b->base + b->off, len);
00108         b->off += len;
00109         
00110         return (len);
00111 }
00112 
00113 int
00114 blob_write(blob_t *b, const void *buf, int len)
00115 {
00116         if (b->off + len <= b->end ||
00117             blob_reserve(b, b->off + len - b->end) == 0) {
00118                 memcpy(b->base + b->off, (u_char *)buf, len);
00119                 b->off += len;
00120                 return (len);
00121         }
00122         return (-1);
00123 }
00124 
00125 int
00126 blob_insert(blob_t *b, const void *buf, int len)
00127 {
00128         if (blob_reserve(b, len) == 0 && b->size) {
00129                 if (b->end - b->off > 0)
00130                         memmove( b->base + b->off + len, b->base + b->off, b->end - b->off);
00131                 memcpy(b->base + b->off, buf, len);
00132                 b->off += len;
00133                 return (len);
00134         }
00135         return (-1);
00136 }
00137 
00138 int
00139 blob_delete(blob_t *b, void *buf, int len)
00140 {
00141         if (b->off + len <= b->end && b->size) {
00142                 if (buf != NULL)
00143                         memcpy(buf, b->base + b->off, len);
00144                 memmove(b->base + b->off, b->base + b->off + len, b->end - (b->off + len));
00145                 b->end -= len;
00146                 return (len);
00147         }
00148         return (-1);
00149 }
00150 
00151 static int
00152 blob_fmt(blob_t *b, int pack, const char *fmt, va_list *ap)
00153 {
00154         blob_fmt_cb fmt_cb;
00155         char *p;
00156         int len;
00157 
00158         for (p = (char *)fmt; *p != '\0'; p++) {
00159                 if (*p == '%') {
00160                         p++;
00161                         if (isdigit((int)*p)) {
00162                                 len = strtol(p, &p, 10);
00163                         } else if (*p == '*') {
00164                                 len = va_arg(*ap, int);
00165                                 p++;
00166                         } else
00167                                 len = 0;
00168                         
00169                         if ((fmt_cb = blob_ascii_fmt[(int)*p]) == NULL)
00170                                 return (-1);
00171 
00172                         if ((*fmt_cb)(pack, len, b, ap) < 0)
00173                                 return (-1);
00174                 } else {
00175                         if (pack) {
00176                                 if (b->off + 1 < b->end ||
00177                                     blob_reserve(b, b->off + 1 - b->end) == 0)
00178                                         b->base[b->off++] = *p;
00179                                 else
00180                                         return (-1);
00181                         } else {
00182                                 if (b->base[b->off++] != *p)
00183                                         return (-1);
00184                         }
00185                 }
00186         }
00187         return (0);
00188 }
00189 
00190 int
00191 blob_pack(blob_t *b, const char *fmt, ...)
00192 {
00193         va_list ap;
00194         va_start(ap, fmt);
00195         return (blob_fmt(b, 1, fmt, &ap));
00196 }
00197 
00198 int
00199 blob_unpack(blob_t *b, const char *fmt, ...)
00200 {
00201         va_list ap;
00202         va_start(ap, fmt);
00203         return (blob_fmt(b, 0, fmt, &ap));
00204 }
00205 
00206 int
00207 blob_seek(blob_t *b, int off, int whence)
00208 {
00209         if (whence == SEEK_CUR)
00210                 off += b->off;
00211         else if (whence == SEEK_END)
00212                 off += b->end;
00213 
00214         if (off < 0 || off > b->end)
00215                 return (-1);
00216         
00217         return ((b->off = off));
00218 }
00219 
00220 int
00221 blob_index(blob_t *b, const void *buf, int len)
00222 {
00223         int i;
00224 
00225         for (i = b->off; i <= b->end - len; i++) {
00226                 if (memcmp(b->base + i, buf, len) == 0)
00227                         return (i);
00228         }
00229         return (-1);
00230 }
00231 
00232 int
00233 blob_rindex(blob_t *b, const void *buf, int len)
00234 {
00235         int i;
00236 
00237         for (i = b->end - len; i >= 0; i--) {
00238                 if (memcmp(b->base + i, buf, len) == 0)
00239                         return (i);
00240         }
00241         return (-1);
00242 }
00243 
00244 int
00245 blob_print(blob_t *b, char *style, int len)
00246 {
00247         struct blob_printer *bp;
00248 
00249         for (bp = blob_printers; bp->name != NULL; bp++) {
00250                 if (strcmp(bp->name, style) == 0)
00251                         bp->print(b);
00252         }
00253         return (0);
00254 }
00255 
00256 int
00257 blob_sprint(blob_t *b, char *style, int len, char *dst, int size)
00258 {
00259         return (0);
00260 }
00261 
00262 blob_t *
00263 blob_free(blob_t *b)
00264 {
00265         if (b->size)
00266                 bl_free(b->base);
00267         bl_free(b);
00268         return (NULL);
00269 }
00270 
00271 int
00272 blob_register_alloc(size_t size, void *(bmalloc)(size_t),
00273     void (*bfree)(void *), void *(*brealloc)(void *, size_t))
00274 {
00275         bl_size = size;
00276         if (bmalloc != NULL)
00277                 bl_malloc = bmalloc;
00278         if (bfree != NULL)
00279                 bl_free = bfree;
00280         if (brealloc != NULL)
00281                 bl_realloc = brealloc;
00282         return (0);
00283 }
00284 
00285 int
00286 blob_register_pack(char c, blob_fmt_cb fmt_cb)
00287 {
00288         if (blob_ascii_fmt[(int)c] == NULL) {
00289                 blob_ascii_fmt[(int)c] = fmt_cb;
00290                 return (0);
00291         }
00292         return (-1);
00293 }
00294 
00295 static int
00296 fmt_D(int pack, int len, blob_t *b, va_list *ap)
00297 {
00298         if (len) return (-1);
00299         
00300         if (pack) {
00301                 uint32_t n = va_arg(*ap, uint32_t);
00302                 n = htonl(n);
00303                 if (blob_write(b, &n, sizeof(n)) < 0)
00304                         return (-1);
00305         } else {
00306                 uint32_t *n = va_arg(*ap, uint32_t *);
00307                 if (blob_read(b, n, sizeof(*n)) != sizeof(*n))
00308                         return (-1);
00309                 *n = ntohl(*n);
00310         }
00311         return (0);
00312 }
00313 
00314 static int
00315 fmt_H(int pack, int len, blob_t *b, va_list *ap)
00316 {
00317         if (len) return (-1);
00318         
00319         if (pack) {
00320                 uint16_t n = va_arg(*ap, int);
00321                 n = htons(n);
00322                 if (blob_write(b, &n, sizeof(n)) < 0)
00323                         return (-1);
00324         } else {
00325                 uint16_t *n = va_arg(*ap, uint16_t *);
00326                 if (blob_read(b, n, sizeof(*n)) != sizeof(*n))
00327                         return (-1);
00328                 *n = ntohs(*n);
00329         }
00330         return (0);
00331 }
00332 
00333 static int
00334 fmt_b(int pack, int len, blob_t *b, va_list *ap)
00335 {
00336         void *p = va_arg(*ap, void *);
00337         
00338         if (len <= 0) return (-1);
00339         
00340         if (pack)
00341                 return (blob_write(b, p, len));
00342         else
00343                 return (blob_read(b, p, len));
00344 }
00345 
00346 static int
00347 fmt_c(int pack, int len, blob_t *b, va_list *ap)
00348 {
00349         if (len) return (-1);
00350         
00351         if (pack) {
00352                 uint8_t n = va_arg(*ap, int);
00353                 return (blob_write(b, &n, sizeof(n)));
00354         } else {
00355                 uint8_t *n = va_arg(*ap, uint8_t *);
00356                 return (blob_read(b, n, sizeof(*n)));
00357         }
00358 }
00359 
00360 static int
00361 fmt_d(int pack, int len, blob_t *b, va_list *ap)
00362 {
00363         if (len) return (-1);
00364         
00365         if (pack) {
00366                 uint32_t n = va_arg(*ap, uint32_t);
00367                 return (blob_write(b, &n, sizeof(n)));
00368         } else {
00369                 uint32_t *n = va_arg(*ap, uint32_t *);
00370                 return (blob_read(b, n, sizeof(*n)));
00371         }
00372 }
00373 
00374 static int
00375 fmt_h(int pack, int len, blob_t *b, va_list *ap)
00376 {
00377         if (len) return (-1);
00378         
00379         if (pack) {
00380                 uint16_t n = va_arg(*ap, int);
00381                 return (blob_write(b, &n, sizeof(n)));
00382         } else {
00383                 uint16_t *n = va_arg(*ap, uint16_t *);
00384                 return (blob_read(b, n, sizeof(*n)));
00385         }
00386 }
00387 
00388 static int
00389 fmt_s(int pack, int len, blob_t *b, va_list *ap)
00390 {
00391         char *p = va_arg(*ap, char *);
00392         char c = '\0';
00393         int i, end;
00394         
00395         if (pack) {
00396                 if (len > 0) {
00397                         if ((c = p[len - 1]) != '\0')
00398                                 p[len - 1] = '\0';
00399                 } else
00400                         len = strlen(p) + 1;
00401                 
00402                 if (blob_write(b, p, len) > 0) {
00403                         if (c != '\0')
00404                                 p[len - 1] = c;
00405                         return (len);
00406                 }
00407         } else {
00408                 if (len <= 0) return (-1);
00409 
00410                 if ((end = b->end - b->off) < len)
00411                         end = len;
00412                 
00413                 for (i = 0; i < end; i++) {
00414                         if ((p[i] = b->base[b->off + i]) == '\0') {
00415                                 b->off += i + 1;
00416                                 return (i);
00417                         }
00418                 }
00419         }
00420         return (-1);
00421 }
00422 
00423 static void
00424 print_hexl(blob_t *b)
00425 {
00426         u_int i, j, jm, len;
00427         u_char *p;
00428         int c;
00429 
00430         p = b->base + b->off;
00431         len = b->end - b->off;
00432         
00433         printf("\n");
00434         
00435         for (i = 0; i < len; i += 0x10) {
00436                 printf("  %04x: ", (u_int)(i + b->off));
00437                 jm = len - i;
00438                 jm = jm > 16 ? 16 : jm;
00439                 
00440                 for (j = 0; j < jm; j++) {
00441                         printf((j % 2) ? "%02x " : "%02x", (u_int)p[i + j]);
00442                 }
00443                 for (; j < 16; j++) {
00444                         printf((j % 2) ? "   " : "  ");
00445                 }
00446                 printf(" ");
00447                 
00448                 for (j = 0; j < jm; j++) {
00449                         c = p[i + j];
00450                         printf("%c", isprint(c) ? c : '.');
00451                 }
00452                 printf("\n");
00453         }
00454 }