pbx/dundi-parser.c

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