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/types.h>
  30 #include <sys/socket.h>
  31 #include <string.h>
  32 #include <netinet/in.h>
  33 #include <arpa/inet.h>
  34 #include <unistd.h>
  35 #include <stdlib.h>
  36 #include <stdio.h>
  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         if (ntohs(fhi->dtrans) & DUNDI_FLAG_RETRANS)
 478                 strcpy(retries, "Yes");
 479         else
 480                 strcpy(retries, "No");
 481         if ((ntohs(fhi->strans) & DUNDI_FLAG_RESERVED)) {
 482                 /* Ignore frames with high bit set to 1 */
 483                 return;
 484         }
 485         if ((fhi->cmdresp & 0x3f) > (int)sizeof(commands)/(int)sizeof(char *)) {
 486                 snprintf(class2, (int)sizeof(class2), "(%d?)", fhi->cmdresp);
 487                 class = class2;
 488         } else {
 489                 class = commands[(int)(fhi->cmdresp & 0x3f)];
 490         }
 491         snprintf(subclass2, (int)sizeof(subclass2), "%02x", fhi->cmdflags);
 492         subclass = subclass2;
 493         snprintf(tmp, (int)sizeof(tmp),
 494                 "%s-Frame Retry[%s] -- OSeqno: %3.3d ISeqno: %3.3d Type: %s (%s)\n",
 495                 pref[rx],
 496                 retries, fhi->oseqno, fhi->iseqno, class, fhi->cmdresp & 0x40 ? "Response" : "Command");
 497         outputf(tmp);
 498         snprintf(tmp, (int)sizeof(tmp),
 499                 "%s     Flags: %s STrans: %5.5d  DTrans: %5.5d [%s:%d]%s\n", (rx > 1) ? "     " : "",
 500                 subclass, ntohs(fhi->strans) & ~DUNDI_FLAG_RESERVED, ntohs(fhi->dtrans) & ~DUNDI_FLAG_RETRANS,
 501                 ast_inet_ntoa(sin->sin_addr), ntohs(sin->sin_port),
 502                 fhi->cmdresp & 0x80 ? " (Final)" : "");
 503         outputf(tmp);
 504         dump_ies(fhi->ies, rx > 1, datalen);
 505 }
 506
 507 int dundi_ie_append_raw(struct dundi_ie_data *ied, unsigned char ie, void *data, int datalen)
 508 {
 509         char tmp[256];
 510         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 511                 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);
 512                 errorf(tmp);
 513                 return -1;
 514         }
 515         ied->buf[ied->pos++] = ie;
 516         ied->buf[ied->pos++] = datalen;
 517         memcpy(ied->buf + ied->pos, data, datalen);
 518         ied->pos += datalen;
 519         return 0;
 520 }
 521
 522 int dundi_ie_append_cause(struct dundi_ie_data *ied, unsigned char ie, unsigned char cause, char *data)
 523 {
 524         char tmp[256];
 525         int datalen = data ? strlen(data) + 1 : 1;
 526         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 527                 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);
 528                 errorf(tmp);
 529                 return -1;
 530         }
 531         ied->buf[ied->pos++] = ie;
 532         ied->buf[ied->pos++] = datalen;
 533         ied->buf[ied->pos++] = cause;
 534         memcpy(ied->buf + ied->pos, data, datalen-1);
 535         ied->pos += datalen-1;
 536         return 0;
 537 }
 538
 539 int dundi_ie_append_hint(struct dundi_ie_data *ied, unsigned char ie, unsigned short flags, char *data)
 540 {
 541         char tmp[256];
 542         int datalen = data ? strlen(data) + 2 : 2;
 543         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 544                 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);
 545                 errorf(tmp);
 546                 return -1;
 547         }
 548         ied->buf[ied->pos++] = ie;
 549         ied->buf[ied->pos++] = datalen;
 550         flags = htons(flags);
 551         memcpy(ied->buf + ied->pos, &flags, sizeof(flags));
 552         ied->pos += 2;
 553         memcpy(ied->buf + ied->pos, data, datalen-1);
 554         ied->pos += datalen-2;
 555         return 0;
 556 }
 557
 558 int dundi_ie_append_encdata(struct dundi_ie_data *ied, unsigned char ie, unsigned char *iv, void *data, int datalen)
 559 {
 560         char tmp[256];
 561         datalen += 16;
 562         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 563                 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);
 564                 errorf(tmp);
 565                 return -1;
 566         }
 567         ied->buf[ied->pos++] = ie;
 568         ied->buf[ied->pos++] = datalen;
 569         memcpy(ied->buf + ied->pos, iv, 16);
 570         ied->pos += 16;
 571         if (data) {
 572                 memcpy(ied->buf + ied->pos, data, datalen-16);
 573                 ied->pos += datalen-16;
 574         }
 575         return 0;
 576 }
 577
 578 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)
 579 {
 580         char tmp[256];
 581         int datalen = data ? strlen(data) + 11 : 11;
 582         int x;
 583         unsigned short myw;
 584         if (datalen > ((int)sizeof(ied->buf) - ied->pos)) {
 585                 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);
 586                 errorf(tmp);
 587                 return -1;
 588         }
 589         ied->buf[ied->pos++] = ie;
 590         ied->buf[ied->pos++] = datalen;
 591         for (x=0;x<6;x++)
 592                 ied->buf[ied->pos++] = eid->eid[x];
 593         ied->buf[ied->pos++] = protocol;
 594         myw = htons(flags);
 595         memcpy(ied->buf + ied->pos, &myw, 2);
 596         ied->pos += 2;
 597         myw = htons(weight);
 598         memcpy(ied->buf + ied->pos, &myw, 2);
 599         ied->pos += 2;
 600         memcpy(ied->buf + ied->pos, data, datalen-11);
 601         ied->pos += datalen-11;
 602         return 0;
 603 }
 604
 605 int dundi_ie_append_addr(struct dundi_ie_data *ied, unsigned char ie, struct sockaddr_in *sin)
 606 {
 607         return dundi_ie_append_raw(ied, ie, sin, (int)sizeof(struct sockaddr_in));
 608 }
 609
 610 int dundi_ie_append_int(struct dundi_ie_data *ied, unsigned char ie, unsigned int value)
 611 {
 612         unsigned int newval;
 613         newval = htonl(value);
 614         return dundi_ie_append_raw(ied, ie, &newval, (int)sizeof(newval));
 615 }
 616
 617 int dundi_ie_append_short(struct dundi_ie_data *ied, unsigned char ie, unsigned short value)
 618 {
 619         unsigned short newval;
 620         newval = htons(value);
 621         return dundi_ie_append_raw(ied, ie, &newval, (int)sizeof(newval));
 622 }
 623
 624 int dundi_ie_append_str(struct dundi_ie_data *ied, unsigned char ie, char *str)
 625 {
 626         return dundi_ie_append_raw(ied, ie, str, strlen(str));
 627 }
 628
 629 int dundi_ie_append_eid(struct dundi_ie_data *ied, unsigned char ie, dundi_eid *eid)
 630 {
 631         return dundi_ie_append_raw(ied, ie, (unsigned char *)eid, sizeof(dundi_eid));
 632 }
 633
 634 int dundi_ie_append_byte(struct dundi_ie_data *ied, unsigned char ie, unsigned char dat)
 635 {
 636         return dundi_ie_append_raw(ied, ie, &dat, 1);
 637 }
 638
 639 int dundi_ie_append(struct dundi_ie_data *ied, unsigned char ie)
 640 {
 641         return dundi_ie_append_raw(ied, ie, NULL, 0);
 642 }
 643
 644 void dundi_set_output(void (*func)(const char *))
 645 {
 646         outputf = func;
 647 }
 648
 649 void dundi_set_error(void (*func)(const char *))
 650 {
 651         errorf = func;
 652 }
 653
 654 int dundi_parse_ies(struct dundi_ies *ies, unsigned char *data, int datalen)
 655 {
 656         /* Parse data into information elements */
 657         int len;
 658         int ie;
 659         char tmp[256];
 660         memset(ies, 0, (int)sizeof(struct dundi_ies));
 661         ies->ttl = -1;
 662         ies->expiration = -1;
 663         ies->unknowncmd = -1;
 664         ies->cause = -1;
 665         while(datalen >= 2) {
 666                 ie = data[0];
 667                 len = data[1];
 668                 if (len > datalen - 2) {
 669                         errorf("Information element length exceeds message size\n");
 670                         return -1;
 671                 }
 672                 switch(ie) {
 673                 case DUNDI_IE_EID:
 674                 case DUNDI_IE_EID_DIRECT:
 675                         if (len != (int)sizeof(dundi_eid)) {
 676                                 errorf("Improper entity identifer, expecting 6 bytes!\n");
 677                         } else if (ies->eidcount < DUNDI_MAX_STACK) {
 678                                 ies->eids[ies->eidcount] = (dundi_eid *)(data + 2);
 679                                 ies->eid_direct[ies->eidcount] = (ie == DUNDI_IE_EID_DIRECT);
 680                                 ies->eidcount++;
 681                         } else
 682                                 errorf("Too many entities in stack!\n");
 683                         break;
 684                 case DUNDI_IE_REQEID:
 685                         if (len != (int)sizeof(dundi_eid)) {
 686                                 errorf("Improper requested entity identifer, expecting 6 bytes!\n");
 687                         } else
 688                                 ies->reqeid = (dundi_eid *)(data + 2);
 689                         break;
 690                 case DUNDI_IE_CALLED_CONTEXT:
 691                         ies->called_context = (char *)data + 2;
 692                         break;
 693                 case DUNDI_IE_CALLED_NUMBER:
 694                         ies->called_number = (char *)data + 2;
 695                         break;
 696                 case DUNDI_IE_ANSWER:
 697                         if (len < sizeof(struct dundi_answer)) {
 698                                 snprintf(tmp, (int)sizeof(tmp), "Answer expected to be >=%d bytes long but was %d\n", (int)sizeof(struct dundi_answer), len);
 699                                 errorf(tmp);
 700                         } else {
 701                                 if (ies->anscount < DUNDI_MAX_ANSWERS)
 702                                         ies->answers[ies->anscount++]= (struct dundi_answer *)(data + 2);
 703                                 else
 704                                         errorf("Ignoring extra answers!\n");
 705                         }
 706                         break;
 707                 case DUNDI_IE_TTL:
 708                         if (len != (int)sizeof(unsigned short)) {
 709                                 snprintf(tmp, (int)sizeof(tmp), "Expecting ttl to be %d bytes long but was %d\n", (int)sizeof(unsigned short), len);
 710                                 errorf(tmp);
 711                         } else
 712                                 ies->ttl = ntohs(*((unsigned short *)(data + 2)));
 713                         break;
 714                 case DUNDI_IE_VERSION:
 715                         if (len != (int)sizeof(unsigned short)) {
 716                                 snprintf(tmp, (int)sizeof(tmp),  "Expecting version to be %d bytes long but was %d\n", (int)sizeof(unsigned short), len);
 717                                 errorf(tmp);
 718                         } else
 719                                 ies->version = ntohs(*((unsigned short *)(data + 2)));
 720                         break;
 721                 case DUNDI_IE_EXPIRATION:
 722                         if (len != (int)sizeof(unsigned short)) {
 723                                 snprintf(tmp, (int)sizeof(tmp),  "Expecting expiration to be %d bytes long but was %d\n", (int)sizeof(unsigned short), len);
 724                                 errorf(tmp);
 725                         } else
 726                                 ies->expiration = ntohs(*((unsigned short *)(data + 2)));
 727                         break;
 728                 case DUNDI_IE_KEYCRC32:
 729                         if (len != (int)sizeof(unsigned int)) {
 730                                 snprintf(tmp, (int)sizeof(tmp),  "Expecting expiration to be %d bytes long but was %d\n", (int)sizeof(unsigned int), len);
 731                                 errorf(tmp);
 732                         } else
 733                                 ies->keycrc32 = ntohl(*((unsigned int *)(data + 2)));
 734                         break;
 735                 case DUNDI_IE_UNKNOWN:
 736                         if (len == 1)
 737                                 ies->unknowncmd = data[2];
 738                         else {
 739                                 snprintf(tmp, (int)sizeof(tmp), "Expected single byte Unknown command, but was %d long\n", len);
 740                                 errorf(tmp);
 741                         }
 742                         break;
 743                 case DUNDI_IE_CAUSE:
 744                         if (len >= 1) {
 745                                 ies->cause = data[2];
 746                                 ies->causestr = (char *)data + 3;
 747                         } else {
 748                                 snprintf(tmp, (int)sizeof(tmp), "Expected at least one byte cause, but was %d long\n", len);
 749                                 errorf(tmp);
 750                         }
 751                         break;
 752                 case DUNDI_IE_HINT:
 753                         if (len >= 2) {
 754                                 ies->hint = (struct dundi_hint *)(data + 2);
 755                         } else {
 756                                 snprintf(tmp, (int)sizeof(tmp), "Expected at least two byte hint, but was %d long\n", len);
 757                                 errorf(tmp);
 758                         }
 759                         break;
 760                 case DUNDI_IE_DEPARTMENT:
 761                         ies->q_dept = (char *)data + 2;
 762                         break;
 763                 case DUNDI_IE_ORGANIZATION:
 764                         ies->q_org = (char *)data + 2;
 765                         break;
 766                 case DUNDI_IE_LOCALITY:
 767                         ies->q_locality = (char *)data + 2;
 768                         break;
 769                 case DUNDI_IE_STATE_PROV:
 770                         ies->q_stateprov = (char *)data + 2;
 771                         break;
 772                 case DUNDI_IE_COUNTRY:
 773                         ies->q_country = (char *)data + 2;
 774                         break;
 775                 case DUNDI_IE_EMAIL:
 776                         ies->q_email = (char *)data + 2;
 777                         break;
 778                 case DUNDI_IE_PHONE:
 779                         ies->q_phone = (char *)data + 2;
 780                         break;
 781                 case DUNDI_IE_IPADDR:
 782                         ies->q_ipaddr = (char *)data + 2;
 783                         break;
 784                 case DUNDI_IE_ENCDATA:
 785                         /* Recalculate len as the remainder of the message, regardless of
 786                            theoretical length */
 787                         len = datalen - 2;
 788                         if ((len > 16) && !(len % 16)) {
 789                                 ies->encblock = (struct dundi_encblock *)(data + 2);
 790                                 ies->enclen = len - 16;
 791                         } else {
 792                                 snprintf(tmp, (int)sizeof(tmp), "Invalid encrypted data length %d\n", len);
 793                                 errorf(tmp);
 794                         }
 795                         break;
 796                 case DUNDI_IE_SHAREDKEY:
 797                         if (len == 128) {
 798                                 ies->encsharedkey = (unsigned char *)(data + 2);
 799                         } else {
 800                                 snprintf(tmp, (int)sizeof(tmp), "Invalid encrypted shared key length %d\n", len);
 801                                 errorf(tmp);
 802                         }
 803                         break;
 804                 case DUNDI_IE_SIGNATURE:
 805                         if (len == 128) {
 806                                 ies->encsig = (unsigned char *)(data + 2);
 807                         } else {
 808                                 snprintf(tmp, (int)sizeof(tmp), "Invalid encrypted signature length %d\n", len);
 809                                 errorf(tmp);
 810                         }
 811                         break;
 812                 case DUNDI_IE_CACHEBYPASS:
 813                         ies->cbypass = 1;
 814                         break;
 815                 default:
 816                         snprintf(tmp, (int)sizeof(tmp), "Ignoring unknown information element '%s' (%d) of length %d\n", dundi_ie2str(ie), ie, len);
 817                         outputf(tmp);
 818                 }
 819                 /* Overwrite information element with 0, to null terminate previous portion */
 820                 data[0] = 0;
 821                 datalen -= (len + 2);
 822                 data += (len + 2);
 823         }
 824         /* Null-terminate last field */
 825         *data = '\0';
 826         if (datalen) {
 827                 errorf("Invalid information element contents, strange boundary\n");
 828                 return -1;
 829         }
 830         return 0;
 831 }