pbx/dundi-parser.c

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