src/libstrongswan/chunk.c

   1 /**
   2  * @file chunk.c
   3  *
   4  * @brief Pointer/lenght abstraction and its functions.
   5  *
   6  */
   7
   8 /*
   9  * Copyright (C) 2005-2006 Martin Willi
  10  * Copyright (C) 2005 Jan Hutter
  11  * Hochschule fuer Technik Rapperswil
  12  *
  13  * This program is free software; you can redistribute it and/or modify it
  14  * under the terms of the GNU General Public License as published by the
  15  * Free Software Foundation; either version 2 of the License, or (at your
  16  * option) any later version.  See <http://www.fsf.org/copyleft/gpl.txt>.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  21  * for more details.
  22  */
  23
  24 #include <stdio.h>
  25
  26 #include "chunk.h"
  27
  28 #include <printf_hook.h>
  29
  30 /**
  31  * Empty chunk.
  32  */
  33 chunk_t chunk_empty = { NULL, 0 };
  34
  35 /**
  36  * Described in header.
  37  */
  38 chunk_t chunk_create(u_char *ptr, size_t len)
  39 {
  40         chunk_t chunk = {ptr, len};
  41         return chunk;
  42 }
  43
  44 /**
  45  * Described in header.
  46  */
  47 chunk_t chunk_create_clone(u_char *ptr, chunk_t chunk)
  48 {
  49         chunk_t clone = chunk_empty;
  50
  51         if (chunk.ptr && chunk.len > 0)
  52         {
  53                 clone.ptr = ptr;
  54                 clone.len = chunk.len;
  55                 memcpy(clone.ptr, chunk.ptr, chunk.len);
  56         }
  57
  58         return clone;
  59 }
  60
  61 /**
  62  * Decribed in header.
  63  */
  64 size_t chunk_length(const char* mode, ...)
  65 {
  66         va_list chunks;
  67         size_t length = 0;
  68
  69         va_start(chunks, mode);
  70         while (TRUE)
  71         {
  72                 switch (*mode++)
  73                 {
  74                         case 'm':
  75                         case 'c':
  76                         {
  77                                 chunk_t ch = va_arg(chunks, chunk_t);
  78                                 length += ch.len;
  79                                 continue;
  80                         }
  81                         default:
  82                                 break;
  83                 }
  84                 break;
  85         }
  86         va_end(chunks);
  87         return length;
  88 }
  89
  90 /**
  91  * Decribed in header.
  92  */
  93 chunk_t chunk_create_cat(u_char *ptr, const char* mode, ...)
  94 {
  95         va_list chunks;
  96         chunk_t construct = chunk_create(ptr, 0);
  97
  98         va_start(chunks, mode);
  99         while (TRUE)
 100         {
 101                 bool free_chunk = FALSE;
 102                 switch (*mode++)
 103                 {
 104                         case 'm':
 105                         {
 106                                 free_chunk = TRUE;
 107                         }
 108                         case 'c':
 109                         {
 110                                 chunk_t ch = va_arg(chunks, chunk_t);
 111                                 memcpy(ptr, ch.ptr, ch.len);
 112                                 ptr += ch.len;
 113                                 construct.len += ch.len;
 114                                 if (free_chunk)
 115                                 {
 116                                         free(ch.ptr);
 117                                 }
 118                                 continue;
 119                         }
 120                         default:
 121                                 break;
 122                 }
 123                 break;
 124         }
 125         va_end(chunks);
 126
 127         return construct;
 128 }
 129
 130 /**
 131  * Decribed in header.
 132  */
 133 void chunk_split(chunk_t chunk, const char *mode, ...)
 134 {
 135         va_list chunks;
 136         size_t len;
 137         chunk_t *ch;
 138
 139         va_start(chunks, mode);
 140         while (TRUE)
 141         {
 142                 if (*mode == '\0')
 143                 {
 144                         break;
 145                 }
 146                 len = va_arg(chunks, size_t);
 147                 ch = va_arg(chunks, chunk_t*);
 148                 /* a null chunk means skip len bytes */
 149                 if (ch == NULL)
 150                 {
 151                         chunk = chunk_skip(chunk, len);
 152                         continue;
 153                 }
 154                 switch (*mode++)
 155                 {
 156                         case 'm':
 157                         {
 158                                 ch->len = min(chunk.len, len);
 159                                 if (ch->len)
 160                                 {
 161                                         ch->ptr = chunk.ptr;
 162                                 }
 163                                 else
 164                                 {
 165                                         ch->ptr = NULL;
 166                                 }
 167                                 chunk = chunk_skip(chunk, ch->len);
 168                                 continue;
 169                         }
 170                         case 'a':
 171                         {
 172                                 ch->len = min(chunk.len, len);
 173                                 if (ch->len)
 174                                 {
 175                                         ch->ptr = malloc(ch->len);
 176                                         memcpy(ch->ptr, chunk.ptr, ch->len);
 177                                 }
 178                                 else
 179                                 {
 180                                         ch->ptr = NULL;
 181                                 }
 182                                 chunk = chunk_skip(chunk, ch->len);
 183                                 continue;
 184                         }
 185                         case 'c':
 186                         {
 187                                 ch->len = min(ch->len, chunk.len);
 188                                 ch->len = min(ch->len, len);
 189                                 if (ch->len)
 190                                 {
 191                                         memcpy(ch->ptr, chunk.ptr, ch->len);
 192                                 }
 193                                 else
 194                                 {
 195                                         ch->ptr = NULL;
 196                                 }
 197                                 chunk = chunk_skip(chunk, ch->len);
 198                                 continue;
 199                         }
 200                         default:
 201                                 break;
 202                 }
 203                 break;
 204         }
 205         va_end(chunks);
 206 }
 207
 208
 209 /**
 210  * Described in header.
 211  */
 212 void chunk_free(chunk_t *chunk)
 213 {
 214         free(chunk->ptr);
 215         chunk->ptr = NULL;
 216         chunk->len = 0;
 217 }
 218
 219 /**
 220  * Described in header.
 221  */
 222 chunk_t chunk_skip(chunk_t chunk, size_t bytes)
 223 {
 224         if (chunk.len > bytes)
 225         {
 226                 chunk.ptr += bytes;
 227                 chunk.len -= bytes;
 228                 return chunk;
 229         }
 230         return chunk_empty;
 231 }
 232
 233 /**
 234  * Described in header.
 235  */
 236 bool chunk_equals(chunk_t a, chunk_t b)
 237 {
 238         return a.ptr != NULL  && b.ptr != NULL &&
 239                         a.len == b.len && memeq(a.ptr, b.ptr, a.len);
 240 }
 241
 242 /**
 243  * Described in header.
 244  */
 245 bool chunk_equals_or_null(chunk_t a, chunk_t b)
 246 {
 247         if (a.ptr == NULL || b.ptr == NULL)
 248                 return TRUE;
 249         return a.len == b.len && memeq(a.ptr, b.ptr, a.len);
 250 }
 251
 252 /**
 253  * Number of bytes per line to dump raw data
 254  */
 255 #define BYTES_PER_LINE 16
 256
 257 /**
 258  * output handler in printf() for byte ranges
 259  */
 260 static int print_bytes(FILE *stream, const struct printf_info *info,
 261                                            const void *const *args)
 262 {
 263         char *bytes = *((void**)(args[0]));
 264         int len = *((size_t*)(args[1]));
 265
 266         char buffer[BYTES_PER_LINE * 3];
 267         char ascii_buffer[BYTES_PER_LINE + 1];
 268         char *buffer_pos = buffer;
 269         char *bytes_pos  = bytes;
 270         char *bytes_roof = bytes + len;
 271         int line_start = 0;
 272         int i = 0;
 273         int written = 0;
 274
 275         written += fprintf(stream, "=> %d bytes @ %p", len, bytes);
 276
 277         while (bytes_pos < bytes_roof)
 278         {
 279                 static char hexdig[] = "0123456789ABCDEF";
 280
 281                 *buffer_pos++ = hexdig[(*bytes_pos >> 4) & 0xF];
 282                 *buffer_pos++ = hexdig[ *bytes_pos       & 0xF];
 283
 284                 ascii_buffer[i++] =
 285                                 (*bytes_pos > 31 && *bytes_pos < 127) ? *bytes_pos : '.';
 286
 287                 if (++bytes_pos == bytes_roof || i == BYTES_PER_LINE)
 288                 {
 289                         int padding = 3 * (BYTES_PER_LINE - i);
 290                         int written;
 291
 292                         while (padding--)
 293                         {
 294                                 *buffer_pos++ = ' ';
 295                         }
 296                         *buffer_pos++ = '\0';
 297                         ascii_buffer[i] = '\0';
 298
 299                         written += fprintf(stream, "\n%4d: %s  %s",
 300                                                            line_start, buffer, ascii_buffer);
 301
 302
 303                         buffer_pos = buffer;
 304                         line_start += BYTES_PER_LINE;
 305                         i = 0;
 306                 }
 307                 else
 308                 {
 309                         *buffer_pos++ = ' ';
 310                 }
 311         }
 312         return written;
 313 }
 314
 315 /**
 316  * output handler in printf() for chunks
 317  */
 318 static int print_chunk(FILE *stream, const struct printf_info *info,
 319                                            const void *const *args)
 320 {
 321         chunk_t *chunk = *((chunk_t**)(args[0]));
 322         bool first = TRUE;
 323         chunk_t copy = *chunk;
 324         int written = 0;
 325
 326         if (!info->alt)
 327         {
 328                 const void *new_args[] = {&chunk->ptr, &chunk->len};
 329                 return print_bytes(stream, info, new_args);
 330         }
 331
 332         while (copy.len > 0)
 333         {
 334                 if (first)
 335                 {
 336                         first = FALSE;
 337                 }
 338                 else
 339                 {
 340                         written += fprintf(stream, ":");
 341                 }
 342                 written += fprintf(stream, "%02x", *copy.ptr++);
 343                 copy.len--;
 344         }
 345         return written;
 346 }
 347
 348 /**
 349  * register printf() handlers
 350  */
 351 static void __attribute__ ((constructor))print_register()
 352 {
 353         register_printf_function(PRINTF_CHUNK, print_chunk, arginfo_ptr);
 354         register_printf_function(PRINTF_BYTES, print_bytes, arginfo_ptr_int);
 355 }