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