pbx/dundi-parser.c

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