pbx/dundi-parser.c

   1 /*
   2  * Asterisk -- An open source telephony toolkit.
   3  *
   4  * Copyright (C) 1999 - 2005, Digium, Inc.
   5  *
   6  * Mark Spencer <markster@digium.com>
   7  *
   8  * See http://www.asterisk.org for more information about
   9  * the Asterisk project. Please do not directly contact
  10  * any of the maintainers of this project for assistance;
  11  * the project provides a web site, mailing lists and IRC
  12  * channels for your use.
  13  *
  14  * This program is free software, distributed under the terms of
  15  * the GNU General Public License Version 2. See the LICENSE file
  16  * at the top of the source tree.
  17  */
  18
  19 /*! \file
  20  *
  21  * \brief Distributed Universal Number Discovery (DUNDi)
  22  *
  23  */
  24
  25 #include <sys/types.h>
  26 #include <sys/socket.h>
  27 #include <string.h>
  28 #include <netinet/in.h>
  29 #include <arpa/inet.h>
  30 #include <unistd.h>
  31 #include <stdlib.h>
  32 #include <stdio.h>
  33
  34 #include "asterisk.h"
  35
  36 ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
  37
  38 #include "asterisk/frame.h"
  39 #include "asterisk/utils.h"
  40 #include "asterisk/dundi.h"
  41 #include "dundi-parser.h"
  42 #include "asterisk/dundi.h"
  43
  44 static void internaloutput(const char *str)
  45 {
  46         fputs(str, stdout);
  47 }
  48
  49 static void internalerror(const char *str)
  50 {
  51         fprintf(stderr, "WARNING: %s", str);
  52 }
  53
  54 static void (*outputf)(const char *str) = internaloutput;
  55 static void (*errorf)(const char *str) = internalerror;
  56
  57 char *dundi_eid_to_str(char *s, int maxlen, dundi_eid *eid)
  58 {
  59         int x;
  60         char *os = s;
  61         if (maxlen < 18) {
  62                 if (s && (maxlen > 0))
  63                         *s = '\0';
  64         } else {
  65                 for (x=0;x<5;x++) {
  66                         sprintf(s, "%02x:", eid->eid[x]);
  67                         s += 3;
  68                 }
  69                 sprintf(s, "%02x", eid->eid[5]);
  70         }
  71         return os;
  72 }
  73
  74 char *dundi_eid_to_str_short(char *s, int maxlen, dundi_eid *eid)
  75 {
  76         int x;
  77         char *os = s;
  78         if (maxlen < 13) {
  79                 if (s && (maxlen > 0))
  80                         *s = '\0';
  81         } else {
  82                 for (x=0;x<6;x++) {
  83                         sprintf(s, "%02X", eid->eid[x]);
  84                         s += 2;
  85                 }
  86         }
  87         return os;
  88 }
  89
  90 int dundi_str_to_eid(dundi_eid *eid, char *s)
  91 {
  92         unsigned int eid_int[6];
  93         int x;
  94         if (sscanf(s, "%x:%x:%x:%x:%x:%x", &eid_int[0], &eid_int[1], &eid_int[2],
  95                  &eid_int[3], &eid_int[4], &eid_int[5]) != 6)
  96                         return -1;
  97         for (x=0;x<6;x++)
  98                 eid->eid[x] = eid_int[x];
  99         return 0;
 100 }
 101
 102 int dundi_str_short_to_eid(dundi_eid *eid, char *s)
 103 {
 104         unsigned int eid_int[6];
 105         int x;
 106         if (sscanf(s, "%2x%2x%2x%2x%2x%2x", &eid_int[0], &eid_int[1], &eid_int[2],
 107                  &eid_int[3], &eid_int[4], &eid_int[5]) != 6)
 108                         return -1;
 109         for (x=0;x<6;x++)
 110                 eid->eid[x] = eid_int[x];
 111         return 0;
 112 }
 113
 114 int dundi_eid_zero(dundi_eid *eid)
 115 {
 116         int x;
 117         for (x=0;x<sizeof(eid->eid) / sizeof(eid->eid[0]);x++)
 118                 if (eid->eid[x]) return 0;
 119         return 1;
 120 }
 121
 122 int dundi_eid_cmp(dundi_eid *eid1, dundi_eid *eid2)
 123 {
 124         return memcmp(eid1, eid2, sizeof(dundi_eid));
 125 }
 126
 127 static void dump_string(char *output, int maxlen, void *value, int len)
 128 {
 129         maxlen--;
 130         if (maxlen > len)
 131                 maxlen = len;
 132         strncpy(output,value, maxlen);
 133         output[maxlen] = '\0';
 134 }
 135
 136 static void dump_cbypass(char *output, int maxlen, void *value, int len)
 137 {
 138         maxlen--;
 139         strncpy(output, "Bypass Caches", maxlen);
 140         output[maxlen] = '\0';
 141 }
 142
 143 static void dump_eid(char *output, int maxlen, void *value, int len)
 144 {
 145         if (len == 6)
 146                 dundi_eid_to_str(output, maxlen, (dundi_eid *)value);
 147         else
 148                 snprintf(output, maxlen, "Invalid EID len %d", len);
 149 }
 150
 151 char *dundi_hint2str(char *buf, int bufsiz, int flags)
 152 {
 153         strcpy(buf, "");
 154         buf[bufsiz-1] = '\0';
 155         if (flags & DUNDI_HINT_TTL_EXPIRED) {
 156                 strncat(buf, "TTLEXPIRED|", bufsiz - strlen(buf) - 1);
 157         }
 158         if (flags & DUNDI_HINT_DONT_ASK) {
 159                 strncat(buf, "DONTASK|", bufsiz - strlen(buf) - 1);
 160         }
 161         if (flags & DUNDI_HINT_UNAFFECTED) {
 162                 strncat(buf, "UNAFFECTED|", bufsiz - strlen(buf) - 1);
 163         }
 164         /* Get rid of trailing | */
 165         if (ast_strlen_zero(buf))
 166                 strcpy(buf, "NONE|");
 167         buf[strlen(buf)-1] = '\0';
 168         return buf;
 169 }
 170
 171 static void dump_hint(char *output, int maxlen, void *value, int len)
 172 {
 173         unsigned short flags;
 174         char tmp[512];
 175         char tmp2[256];
 176         if (len < 2) {
 177                 strncpy(output, "<invalid contents>", maxlen);
 178                 return;
 179         }
 180         memcpy(&flags, value, sizeof(flags));
 181         flags = ntohs(flags);
 182         memset(tmp, 0, sizeof(tmp));
 183         dundi_hint2str(tmp2, sizeof(tmp2), flags);
 184         snprintf(tmp, sizeof(tmp), "[%s] ", tmp2);
 185         memcpy(tmp + strlen(tmp), value + 2, len - 2);
 186         strncpy(output, tmp, maxlen - 1);
 187 }
 188
 189 static void dump_cause(char *output, int maxlen, void *value, int len)
 190 {
 191         static char *causes[] = {
 192                 "SUCCESS",
 193                 "GENERAL",
 194                 "DYNAMIC",
 195                 "NOAUTH" ,
 196                 };
 197         char tmp[256];
 198         char tmp2[256];
 199         int mlen;
 200         unsigned char cause;
 201         if (len < 1) {
 202                 strncpy(output, "<invalid contents>", maxlen);
 203                 return;
 204         }
 205         cause = *((unsigned char *)value);
 206         memset(tmp2, 0, sizeof(tmp2));
 207         mlen = len - 1;
 208         if (mlen > 255)
 209                 mlen = 255;
 210         memcpy(tmp2, value + 1, mlen);
 211         if (cause < sizeof(causes) / sizeof(causes[0])) {
 212                 if (len > 1)
 213                         snprintf(tmp, sizeof(tmp), "%s: %s", causes[cause], tmp2);
 214                 else
 215                         snprintf(tmp, sizeof(tmp), "%s", causes[cause]);
 216         } else {
 217                 if (len > 1)
 218                         snprintf(tmp, sizeof(tmp), "%d: %s", cause, tmp2);
 219                 else
 220                         snprintf(tmp, sizeof(tmp), "%d", cause);
 221         }
 222
 223         strncpy(output,tmp, maxlen);
 224         output[maxlen] = '\0';
 225 }
 226
 227 static void dump_int(char *output, int maxlen, void *value, int len)
 228 {
 229         if (len == (int)sizeof(unsigned int))
 230                 snprintf(output, maxlen, "%lu", (unsigned long)ntohl(*((unsigned int *)value)));
 231         else
 232                 snprintf(output, maxlen, "Invalid INT");
 233 }
 234
 235 static void dump_short(char *output, int maxlen, void *value, int len)
 236 {
 237         if (len == (int)sizeof(unsigned short))
 238                 snprintf(output, maxlen, "%d", ntohs(*((unsigned short *)value)));
 239         else
 240                 snprintf(output, maxlen, "Invalid SHORT");
 241 }
 242
 243 static void dump_byte(char *output, int maxlen, void *value, int len)
 244 {
 245         if (len == (int)sizeof(unsigned char))
 246                 snprintf(output, maxlen, "%d", *((unsigned char *)value));
 247         else
 248                 snprintf(output, maxlen, "Invalid BYTE");
 249 }
 250
 251 static char *proto2str(int proto, char *buf, int bufsiz)
 252 {
 253         switch(proto) {
 254         case DUNDI_PROTO_NONE:
 255                 strncpy(buf, "None", bufsiz - 1);
 256                 break;
 257         case DUNDI_PROTO_IAX:
 258                 strncpy(buf, "IAX", bufsiz - 1);
 259                 break;
 260         case DUNDI_PROTO_SIP:
 261                 strncpy(buf, "SIP", bufsiz - 1);
 262                 break;
 263         case DUNDI_PROTO_H323:
 264                 strncpy(buf, "H.323", bufsiz - 1);
 265                 break;
 266         default:
 267                 snprintf(buf, bufsiz, "Unknown Proto(%d)", proto);
 268         }
 269         buf[bufsiz-1] = '\0';
 270         return buf;
 271 }
 272
 273 char *dundi_flags2str(char *buf, int bufsiz, int flags)
 274 {
 275         strcpy(buf, "");
 276         buf[bufsiz-1] = '\0';
 277         if (flags & DUNDI_FLAG_EXISTS) {
 278                 strncat(buf, "EXISTS|", bufsiz - strlen(buf) - 1);
 279         }
 280         if (flags & DUNDI_FLAG_MATCHMORE) {
 281                 strncat(buf, "MATCHMORE|", bufsiz - strlen(buf) - 1);
 282         }
 283         if (flags & DUNDI_FLAG_CANMATCH) {
 284                 strncat(buf, "CANMATCH|", bufsiz - strlen(buf) - 1);
 285         }
 286         if (flags & DUNDI_FLAG_IGNOREPAT) {
 287                 strncat(buf, "IGNOREPAT|", bufsiz - strlen(buf) - 1);
 288         }
 289         if (flags & DUNDI_FLAG_RESIDENTIAL) {
 290                 strncat(buf, "RESIDENCE|", bufsiz - strlen(buf) - 1);
 291         }
 292         if (flags & DUNDI_FLAG_COMMERCIAL) {
 293                 strncat(buf, "COMMERCIAL|", bufsiz - strlen(buf) - 1);
 294         }
 295         if (flags & DUNDI_FLAG_MOBILE) {
 296                 strncat(buf, "MOBILE", bufsiz - strlen(buf) - 1);
 297         }
 298         if (flags & DUNDI_FLAG_NOUNSOLICITED) {
 299                 strncat(buf, "NOUNSLCTD|", bufsiz - strlen(buf) - 1);
 300         }
 301         if (flags & DUNDI_FLAG_NOCOMUNSOLICIT) {
 302                 strncat(buf, "NOCOMUNSLTD|", bufsiz - strlen(buf) - 1);
 303         }
 304         /* Get rid of trailing | */
 305         if (ast_strlen_zero(buf))
 306                 strcpy(buf, "NONE|");
 307         buf[strlen(buf)-1] = '\0';
 308         return buf;
 309 }
 310
 311 static void dump_answer(char *output, int maxlen, void *value, int len)
 312 {
 313         struct dundi_answer *answer;
 314         char proto[40];
 315         char flags[40];
 316         char eid_str[40];
 317         char tmp[512]="";
 318         if (len >= 10) {
 319                 answer = (struct dundi_answer *)(value);
 320                 memcpy(tmp, answer->data, (len >= 500) ? 500 : len - 10);
 321                 dundi_eid_to_str(eid_str, sizeof(eid_str), &answer->eid);
 322                 snprintf(output, maxlen, "[%s] %d <%s/%s> from [%s]",
 323                         dundi_flags2str(flags, sizeof(flags), ntohs(answer->flags)),
 324                         ntohs(answer->weight),
 325                         proto2str(answer->protocol, proto, sizeof(proto)),
 326                                 tmp, eid_str);
 327         } else
 328                 strncpy(output, "Invalid Answer", maxlen - 1);
 329 }
 330
 331 static void dump_encrypted(char *output, int maxlen, void *value, int len)
 332 {
 333         char iv[33];
 334         int x;
 335         if ((len > 16) && !(len % 16)) {
 336                 /* Build up IV */
 337                 for (x=0;x<16;x++) {
 338                         snprintf(iv + (x << 1), 3, "%02x", ((unsigned char *)value)[x]);
 339                 }
 340                 snprintf(output, maxlen, "[IV %s] %d encrypted blocks\n", iv, len / 16);
 341         } else
 342                 snprintf(output, maxlen, "Invalid Encrypted Datalen %d", len);
 343 }
 344
 345 static void dump_raw(char *output, int maxlen, void *value, int len)
 346 {
 347         int x;
 348         unsigned char *u = value;
 349         output[maxlen - 1] = '\0';
 350         strcpy(output, "[ ");
 351         for (x=0;x<len;x++) {
 352                 snprintf(output + strlen(output), maxlen - strlen(output) - 1, "%02x ", u[x]);
 353         }
 354         strncat(output + strlen(output), "]", maxlen - strlen(output) - 1);
 355 }
 356
 357 static struct dundi_ie {
 358         int ie;
 359         char *name;
 360         void (*dump)(char *output, int maxlen, void *value, int len);
 361 } ies[] = {
 362         { DUNDI_IE_EID, "ENTITY IDENT", dump_eid },
 363         { DUNDI_IE_CALLED_CONTEXT, "CALLED CONTEXT", dump_string },
 364         { DUNDI_IE_CALLED_NUMBER, "CALLED NUMBER", dump_string },
 365         { DUNDI_IE_EID_DIRECT, "DIRECT EID", dump_eid },
 366         { DUNDI_IE_ANSWER, "ANSWER", dump_answer },
 367         { DUNDI_IE_TTL, "TTL", dump_short },
 368         { DUNDI_IE_VERSION, "VERSION", dump_short },
 369         { DUNDI_IE_EXPIRATION, "EXPIRATION", dump_short },
 370         { DUNDI_IE_UNKNOWN, "UKWN DUNDI CMD", dump_byte },
 371         { DUNDI_IE_CAUSE, "CAUSE", dump_cause },
 372         { DUNDI_IE_REQEID, "REQUEST EID", dump_eid },
 373         { DUNDI_IE_ENCDATA, "ENCDATA", dump_encrypted },
 374         { DUNDI_IE_SHAREDKEY, "SHAREDKEY", dump_raw },
 375         { DUNDI_IE_SIGNATURE, "SIGNATURE", dump_raw },
 376         { DUNDI_IE_KEYCRC32, "KEYCRC32", dump_int },
 377         { DUNDI_IE_HINT, "HINT", dump_hint },
 378         { DUNDI_IE_DEPARTMENT, "DEPARTMENT", dump_string },
 379         { DUNDI_IE_ORGANIZATION, "ORGANIZTN", dump_string },
 380         { DUNDI_IE_LOCALITY, "LOCALITY", dump_string },
 381         { DUNDI_IE_STATE_PROV, "STATEPROV", dump_string },
 382         { DUNDI_IE_COUNTRY, "COUNTRY", dump_string },
 383         { DUNDI_IE_EMAIL, "EMAIL", dump_string },
 384         { DUNDI_IE_PHONE, "PHONE", dump_string },
 385         { DUNDI_IE_IPADDR, "ADDRESS", dump_string },
 386         { DUNDI_IE_CACHEBYPASS, "CBYPASS", dump_cbypass },
 387 };
 388
 389 const char *dundi_ie2str(int ie)
 390 {
 391         int x;
 392         for (x=0;x<(int)sizeof(ies) / (int)sizeof(ies[0]); x++) {
 393                 if (ies[x].ie == ie)
 394                         return ies[x].name;
 395         }
 396         return "Unknown IE";
 397 }
 398
 399 static void dump_ies(unsigned char *iedata, int spaces, int len)
 400 {
 401         int ielen;
 402         int ie;
 403         int x;
 404         int found;
 405         char interp[1024];
 406         char tmp[1024];
 407         if (len < 2)
 408                 return;
 409         while(len >= 2) {
 410                 ie = iedata[0];
 411                 ielen = iedata[1];
 412                 /* Encrypted data is the remainder */
 413                 if (ie == DUNDI_IE_ENCDATA)
 414                         ielen = len - 2;
 415                 if (ielen + 2> len) {
 416                         snprintf(tmp, (int)sizeof(tmp), "Total IE length of %d bytes exceeds remaining frame length of %d bytes\n", ielen + 2, len);
 417                         outputf(tmp);
 418                         return;
 419                 }
 420                 found = 0;
 421                 for (x=0;x<(int)sizeof(ies) / (int)sizeof(ies[0]); x++) {
 422                         if (ies[x].ie == ie) {
 423                                 if (ies[x].dump) {
 424                                         ies[x].dump(interp, (int)sizeof(interp), iedata + 2, ielen);
 425                                         snprintf(tmp, (int)sizeof(tmp), "   %s%-15.15s : %s\n", (spaces ? "     " : "" ), ies[x].name, interp);
 426                                         outputf(tmp);
 427                                 } else {
 428                                         if (ielen)
 429                                                 snprintf(interp, (int)sizeof(interp), "%d bytes", ielen);
 430                                         else
 431                                                 strcpy(interp, "Present");
 432                                         snprintf(tmp, (int)sizeof(tmp), "   %s%-15.15s : %s\n", (spaces ? "     " : "" ), ies[x].name, interp);
 433                                         outputf(tmp);
 434                                 }
 435                                 found++;
 436                         }
 437                 }
 438                 if (!found) {
 439                         snprintf(tmp, (int)sizeof(tmp), "   %sUnknown IE %03d  : Present\n", (spaces ? "     " : "" ), ie);
 440                         outputf(tmp);
 441                 }
 442                 iedata += (2 + ielen);
 443                 len -= (2 + ielen);
 444         }
 445         outputf("\n");
 446 }
 447
 448 void dundi_showframe(struct dundi_hdr *fhi, int rx, struct sockaddr_in *sin, int datalen)
 449 {
 450         char *pref[] = {
 451                 "Tx",
 452                 "Rx",
 453                 "    ETx",
 454                 "    Erx" };
 455         char *commands[] = {
 456                 "ACK         ",
 457                 "DPDISCOVER  ",
 458                 "DPRESPONSE  ",
 459                 "EIDQUERY    ",
 460                 "EIDRESPONSE ",
 461                 "PRECACHERQ  ",
 462                 "PRECACHERP  ",
 463                 "INVALID     ",
 464                 "UNKNOWN CMD ",
 465                 "NULL        ",
 466                 "REQREQ      ",
 467                 "REGRESPONSE ",
 468                 "CANCEL      ",
 469                 "ENCRYPT     ",
 470                 "ENCREJ      " };
 471         char class2[20];
 472         char *class;
 473         char subclass2[20];
 474         char *subclass;
 475         char tmp[256];
 476         char retries[20];
 477         char iabuf[INET_ADDRSTRLEN];
 478         if (ntohs(fhi->dtrans) & DUNDI_FLAG_RETRANS)
 479                 strcpy(retries, "Yes");
 480         else
 481                 strcpy(retries, "No");
 482         if ((ntohs(fhi->strans) & DUNDI_FLAG_RESERVED)) {
 483                 /* Ignore frames with high bit set to 1 */
 484                 return;
 485         }
 486         if ((fhi->cmdresp & 0x3f) > (int)sizeof(commands)/(int)sizeof(char *)) {
 487                 snprintf(class2, (int)sizeof(class2), "(%d?)", fhi->cmdresp);
 488                 class = class2;
 489         } else {
 490                 class = commands[(int)(fhi->cmdresp & 0x3f)];
 491         }
 492         snprintf(subclass2, (int)sizeof(subclass2), "%02x", fhi->cmdflags);
 493         subclass = subclass2;
 494         snprintf(tmp, (int)sizeof(tmp),
 495                 "%s-Frame Retry[%s] -- OSeqno: %3.3d ISeqno: %3.3d Type: %s (%s)\n",
 496                 pref[rx],
 497                 retries, fhi->oseqno, fhi->iseqno, class, fhi->cmdresp & 0x40 ? "Response" : "Command");
 498         outputf(tmp);
 499         snprintf(tmp, (int)sizeof(tmp),
 500                 "%s     Flags: %s STrans: %5.5d  DTrans: %5.5d [%s:%d]%s\n", (rx > 1) ? "     " : "",
 501                 subclass, ntohs(fhi->strans) & ~DUNDI_FLAG_RESERVED, ntohs(fhi->dtrans) & ~DUNDI_FLAG_RETRANS,
 502                 ast_inet_ntoa(iabuf, sizeof(iabuf), sin->sin_addr), ntohs(sin->sin_port),
 503                 fhi->cmdresp & 0x80 ? " (Final)" : "");
 504         outputf(tmp);
 505         dump_ies(fhi->ies, rx > 1, datalen);
 506 }
 507
 508 int dundi_ie_append_raw(struct dundi_ie_data *ied, unsigned char ie, void *data, int datalen)
 509 {
 510         char tmp[256];
 511         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 512                 snprintf(tmp, (int)sizeof(tmp), "Out of space for ie '%s' (%d), need %d have %d\n", dundi_ie2str(ie), ie, datalen, (int)sizeof(ied->buf) - ied->pos);
 513                 errorf(tmp);
 514                 return -1;
 515         }
 516         ied->buf[ied->pos++] = ie;
 517         ied->buf[ied->pos++] = datalen;
 518         memcpy(ied->buf + ied->pos, data, datalen);
 519         ied->pos += datalen;
 520         return 0;
 521 }
 522
 523 int dundi_ie_append_cause(struct dundi_ie_data *ied, unsigned char ie, unsigned char cause, char *data)
 524 {
 525         char tmp[256];
 526         int datalen = data ? strlen(data) + 1 : 1;
 527         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 528                 snprintf(tmp, (int)sizeof(tmp), "Out of space for ie '%s' (%d), need %d have %d\n", dundi_ie2str(ie), ie, datalen, (int)sizeof(ied->buf) - ied->pos);
 529                 errorf(tmp);
 530                 return -1;
 531         }
 532         ied->buf[ied->pos++] = ie;
 533         ied->buf[ied->pos++] = datalen;
 534         ied->buf[ied->pos++] = cause;
 535         memcpy(ied->buf + ied->pos, data, datalen-1);
 536         ied->pos += datalen-1;
 537         return 0;
 538 }
 539
 540 int dundi_ie_append_hint(struct dundi_ie_data *ied, unsigned char ie, unsigned short flags, char *data)
 541 {
 542         char tmp[256];
 543         int datalen = data ? strlen(data) + 2 : 2;
 544         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 545                 snprintf(tmp, (int)sizeof(tmp), "Out of space for ie '%s' (%d), need %d have %d\n", dundi_ie2str(ie), ie, datalen, (int)sizeof(ied->buf) - ied->pos);
 546                 errorf(tmp);
 547                 return -1;
 548         }
 549         ied->buf[ied->pos++] = ie;
 550         ied->buf[ied->pos++] = datalen;
 551         flags = htons(flags);
 552         memcpy(ied->buf + ied->pos, &flags, sizeof(flags));
 553         ied->pos += 2;
 554         memcpy(ied->buf + ied->pos, data, datalen-1);
 555         ied->pos += datalen-2;
 556         return 0;
 557 }
 558
 559 int dundi_ie_append_encdata(struct dundi_ie_data *ied, unsigned char ie, unsigned char *iv, void *data, int datalen)
 560 {
 561         char tmp[256];
 562         datalen += 16;
 563         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 564                 snprintf(tmp, (int)sizeof(tmp), "Out of space for ie '%s' (%d), need %d have %d\n", dundi_ie2str(ie), ie, datalen, (int)sizeof(ied->buf) - ied->pos);
 565                 errorf(tmp);
 566                 return -1;
 567         }
 568         ied->buf[ied->pos++] = ie;
 569         ied->buf[ied->pos++] = datalen;
 570         memcpy(ied->buf + ied->pos, iv, 16);
 571         ied->pos += 16;
 572         if (data) {
 573                 memcpy(ied->buf + ied->pos, data, datalen-16);
 574                 ied->pos += datalen-16;
 575         }
 576         return 0;
 577 }
 578
 579 int dundi_ie_append_answer(struct dundi_ie_data *ied, unsigned char ie, dundi_eid *eid, unsigned char protocol, unsigned short flags, unsigned short weight, char *data)
 580 {
 581         char tmp[256];
 582         int datalen = data ? strlen(data) + 11 : 11;
 583         int x;
 584         unsigned short myw;
 585         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 586                 snprintf(tmp, (int)sizeof(tmp), "Out of space for ie '%s' (%d), need %d have %d\n", dundi_ie2str(ie), ie, datalen, (int)sizeof(ied->buf) - ied->pos);
 587                 errorf(tmp);
 588                 return -1;
 589         }
 590         ied->buf[ied->pos++] = ie;
 591         ied->buf[ied->pos++] = datalen;
 592         for (x=0;x<6;x++)
 593                 ied->buf[ied->pos++] = eid->eid[x];
 594         ied->buf[ied->pos++] = protocol;
 595         myw = htons(flags);
 596         memcpy(ied->buf + ied->pos, &myw, 2);
 597         ied->pos += 2;
 598         myw = htons(weight);
 599         memcpy(ied->buf + ied->pos, &myw, 2);
 600         ied->pos += 2;
 601         memcpy(ied->buf + ied->pos, data, datalen-11);
 602         ied->pos += datalen-11;
 603         return 0;
 604 }
 605
 606 int dundi_ie_append_addr(struct dundi_ie_data *ied, unsigned char ie, struct sockaddr_in *sin)
 607 {
 608         return dundi_ie_append_raw(ied, ie, sin, (int)sizeof(struct sockaddr_in));
 609 }
 610
 611 int dundi_ie_append_int(struct dundi_ie_data *ied, unsigned char ie, unsigned int value)
 612 {
 613         unsigned int newval;
 614         newval = htonl(value);
 615         return dundi_ie_append_raw(ied, ie, &newval, (int)sizeof(newval));
 616 }
 617
 618 int dundi_ie_append_short(struct dundi_ie_data *ied, unsigned char ie, unsigned short value)
 619 {
 620         unsigned short newval;
 621         newval = htons(value);
 622         return dundi_ie_append_raw(ied, ie, &newval, (int)sizeof(newval));
 623 }
 624
 625 int dundi_ie_append_str(struct dundi_ie_data *ied, unsigned char ie, char *str)
 626 {
 627         return dundi_ie_append_raw(ied, ie, str, strlen(str));
 628 }
 629
 630 int dundi_ie_append_eid(struct dundi_ie_data *ied, unsigned char ie, dundi_eid *eid)
 631 {
 632         return dundi_ie_append_raw(ied, ie, (unsigned char *)eid, sizeof(dundi_eid));
 633 }
 634
 635 int dundi_ie_append_byte(struct dundi_ie_data *ied, unsigned char ie, unsigned char dat)
 636 {
 637         return dundi_ie_append_raw(ied, ie, &dat, 1);
 638 }
 639
 640 int dundi_ie_append(struct dundi_ie_data *ied, unsigned char ie)
 641 {
 642         return dundi_ie_append_raw(ied, ie, NULL, 0);
 643 }
 644
 645 void dundi_set_output(void (*func)(const char *))
 646 {
 647         outputf = func;
 648 }
 649
 650 void dundi_set_error(void (*func)(const char *))
 651 {
 652         errorf = func;
 653 }
 654
 655 int dundi_parse_ies(struct dundi_ies *ies, unsigned char *data, int datalen)
 656 {
 657         /* Parse data into information elements */
 658         int len;
 659         int ie;
 660         char tmp[256];
 661         memset(ies, 0, (int)sizeof(struct dundi_ies));
 662         ies->ttl = -1;
 663         ies->expiration = -1;
 664         ies->unknowncmd = -1;
 665         ies->cause = -1;
 666         while(datalen >= 2) {
 667                 ie = data[0];
 668                 len = data[1];
 669                 if (len > datalen - 2) {
 670                         errorf("Information element length exceeds message size\n");
 671                         return -1;
 672                 }
 673                 switch(ie) {
 674                 case DUNDI_IE_EID:
 675                 case DUNDI_IE_EID_DIRECT:
 676                         if (len != (int)sizeof(dundi_eid)) {
 677                                 errorf("Improper entity identifer, expecting 6 bytes!\n");
 678                         } else if (ies->eidcount < DUNDI_MAX_STACK) {
 679                                 ies->eids[ies->eidcount] = (dundi_eid *)(data + 2);
 680                                 ies->eid_direct[ies->eidcount] = (ie == DUNDI_IE_EID_DIRECT);
 681                                 ies->eidcount++;
 682                         } else
 683                                 errorf("Too many entities in stack!\n");
 684                         break;
 685                 case DUNDI_IE_REQEID:
 686                         if (len != (int)sizeof(dundi_eid)) {
 687                                 errorf("Improper requested entity identifer, expecting 6 bytes!\n");
 688                         } else
 689                                 ies->reqeid = (dundi_eid *)(data + 2);
 690                         break;
 691                 case DUNDI_IE_CALLED_CONTEXT:
 692                         ies->called_context = (char *)data + 2;
 693                         break;
 694                 case DUNDI_IE_CALLED_NUMBER:
 695                         ies->called_number = (char *)data + 2;
 696                         break;
 697                 case DUNDI_IE_ANSWER:
 698                         if (len < sizeof(struct dundi_answer)) {
 699                                 snprintf(tmp, (int)sizeof(tmp), "Answer expected to be >=%d bytes long but was %d\n", (int)sizeof(struct dundi_answer), len);
 700                                 errorf(tmp);
 701                         } else {
 702                                 if (ies->anscount < DUNDI_MAX_ANSWERS)
 703                                         ies->answers[ies->anscount++]= (struct dundi_answer *)(data + 2);
 704                                 else
 705                                         errorf("Ignoring extra answers!\n");
 706                         }
 707                         break;
 708                 case DUNDI_IE_TTL:
 709                         if (len != (int)sizeof(unsigned short)) {
 710                                 snprintf(tmp, (int)sizeof(tmp), "Expecting ttl to be %d bytes long but was %d\n", (int)sizeof(unsigned short), len);
 711                                 errorf(tmp);
 712                         } else
 713                                 ies->ttl = ntohs(*((unsigned short *)(data + 2)));
 714                         break;
 715                 case DUNDI_IE_VERSION:
 716                         if (len != (int)sizeof(unsigned short)) {
 717                                 snprintf(tmp, (int)sizeof(tmp),  "Expecting version to be %d bytes long but was %d\n", (int)sizeof(unsigned short), len);
 718                                 errorf(tmp);
 719                         } else
 720                                 ies->version = ntohs(*((unsigned short *)(data + 2)));
 721                         break;
 722                 case DUNDI_IE_EXPIRATION:
 723                         if (len != (int)sizeof(unsigned short)) {
 724                                 snprintf(tmp, (int)sizeof(tmp),  "Expecting expiration to be %d bytes long but was %d\n", (int)sizeof(unsigned short), len);
 725                                 errorf(tmp);
 726                         } else
 727                                 ies->expiration = ntohs(*((unsigned short *)(data + 2)));
 728                         break;
 729                 case DUNDI_IE_KEYCRC32:
 730                         if (len != (int)sizeof(unsigned int)) {
 731                                 snprintf(tmp, (int)sizeof(tmp),  "Expecting expiration to be %d bytes long but was %d\n", (int)sizeof(unsigned int), len);
 732                                 errorf(tmp);
 733                         } else
 734                                 ies->keycrc32 = ntohl(*((unsigned int *)(data + 2)));
 735                         break;
 736                 case DUNDI_IE_UNKNOWN:
 737                         if (len == 1)
 738                                 ies->unknowncmd = data[2];
 739                         else {
 740                                 snprintf(tmp, (int)sizeof(tmp), "Expected single byte Unknown command, but was %d long\n", len);
 741                                 errorf(tmp);
 742                         }
 743                         break;
 744                 case DUNDI_IE_CAUSE:
 745                         if (len >= 1) {
 746                                 ies->cause = data[2];
 747                                 ies->causestr = (char *)data + 3;
 748                         } else {
 749                                 snprintf(tmp, (int)sizeof(tmp), "Expected at least one byte cause, but was %d long\n", len);
 750                                 errorf(tmp);
 751                         }
 752                         break;
 753                 case DUNDI_IE_HINT:
 754                         if (len >= 2) {
 755                                 ies->hint = (struct dundi_hint *)(data + 2);
 756                         } else {
 757                                 snprintf(tmp, (int)sizeof(tmp), "Expected at least two byte hint, but was %d long\n", len);
 758                                 errorf(tmp);
 759                         }
 760                         break;
 761                 case DUNDI_IE_DEPARTMENT:
 762                         ies->q_dept = (char *)data + 2;
 763                         break;
 764                 case DUNDI_IE_ORGANIZATION:
 765                         ies->q_org = (char *)data + 2;
 766                         break;
 767                 case DUNDI_IE_LOCALITY:
 768                         ies->q_locality = (char *)data + 2;
 769                         break;
 770                 case DUNDI_IE_STATE_PROV:
 771                         ies->q_stateprov = (char *)data + 2;
 772                         break;
 773                 case DUNDI_IE_COUNTRY:
 774                         ies->q_country = (char *)data + 2;
 775                         break;
 776                 case DUNDI_IE_EMAIL:
 777                         ies->q_email = (char *)data + 2;
 778                         break;
 779                 case DUNDI_IE_PHONE:
 780                         ies->q_phone = (char *)data + 2;
 781                         break;
 782                 case DUNDI_IE_IPADDR:
 783                         ies->q_ipaddr = (char *)data + 2;
 784                         break;
 785                 case DUNDI_IE_ENCDATA:
 786                         /* Recalculate len as the remainder of the message, regardless of
 787                            theoretical length */
 788                         len = datalen - 2;
 789                         if ((len > 16) && !(len % 16)) {
 790                                 ies->encblock = (struct dundi_encblock *)(data + 2);
 791                                 ies->enclen = len - 16;
 792                         } else {
 793                                 snprintf(tmp, (int)sizeof(tmp), "Invalid encrypted data length %d\n", len);
 794                                 errorf(tmp);
 795                         }
 796                         break;
 797                 case DUNDI_IE_SHAREDKEY:
 798                         if (len == 128) {
 799                                 ies->encsharedkey = (unsigned char *)(data + 2);
 800                         } else {
 801                                 snprintf(tmp, (int)sizeof(tmp), "Invalid encrypted shared key length %d\n", len);
 802                                 errorf(tmp);
 803                         }
 804                         break;
 805                 case DUNDI_IE_SIGNATURE:
 806                         if (len == 128) {
 807                                 ies->encsig = (unsigned char *)(data + 2);
 808                         } else {
 809                                 snprintf(tmp, (int)sizeof(tmp), "Invalid encrypted signature length %d\n", len);
 810                                 errorf(tmp);
 811                         }
 812                         break;
 813                 case DUNDI_IE_CACHEBYPASS:
 814                         ies->cbypass = 1;
 815                         break;
 816                 default:
 817                         snprintf(tmp, (int)sizeof(tmp), "Ignoring unknown information element '%s' (%d) of length %d\n", dundi_ie2str(ie), ie, len);
 818                         outputf(tmp);
 819                 }
 820                 /* Overwrite information element with 0, to null terminate previous portion */
 821                 data[0] = 0;
 822                 datalen -= (len + 2);
 823                 data += (len + 2);
 824         }
 825         /* Null-terminate last field */
 826         *data = '\0';
 827         if (datalen) {
 828                 errorf("Invalid information element contents, strange boundary\n");
 829                 return -1;
 830         }
 831         return 0;
 832 }