test/generateload.c

   1 #include <arpa/inet.h>
   2 #include <time.h>
   3 #include <errno.h>
   4 #include <fcntl.h>
   5 #include <linux/if.h>
   6 #include <netdb.h>
   7 #include <netinet/in.h>
   8 #include <stdarg.h>
   9 #include <stdio.h>
  10 #include <stdlib.h>
  11 #include <string.h>
  12 #include <sys/socket.h>
  13 #include <sys/types.h>
  14 #include <linux/ip.h>
  15 #include <linux/udp.h>
  16 #include <unistd.h>
  17 #include <signal.h>
  18 #include <getopt.h>
  19 #include <sys/mman.h>
  20
  21 #define PPPLCP  0xc021
  22 #define PPPPAP  0xc023
  23 #define PPPCHAP 0xc223
  24 #define PPPIPCP 0x8021
  25 #define PPPIP   0x0021
  26 #define PPPCCP  0x80fd
  27
  28 #define CONFREQ 1
  29 #define CONFACK 2
  30 #define CONFNAK 3
  31 #define CONFREJ 4
  32 #define TERMREQ 5
  33 #define TERMACK 6
  34 #define CODEREJ 7
  35 #define PROTREJ 8
  36 #define ECHOREQ 9
  37 #define ECHOREP 10
  38 #define DISCREQ 11
  39
  40 #define PACKET_LENGTH   1000
  41 #define TARGET_PPS      5000
  42 #define TARGET          "211.29.131.33"
  43 #define GWADDR          "211.29.131.30"
  44 #define NUM_SESSIONS    1
  45 #define MAX_PACKETS     0
  46 #define AVG_SIZE        5
  47
  48 typedef unsigned short u16;
  49 typedef unsigned int u32;
  50 typedef unsigned char u8;
  51
  52 char *lcp_codes[] = {
  53         "reserved",
  54         "CONFREQ",
  55         "CONFACK",
  56         "CONFNAK",
  57         "CONFREJ",
  58         "TERMREQ",
  59         "TERMACK",
  60         "CODEREJ",
  61         "PROTREJ",
  62         "ECHOREQ",
  63         "ECHOREP",
  64         "DISCREQ",
  65 };
  66
  67 char *mtypes[] = {
  68         "reserved",
  69         "SCCRQ",
  70         "SCCRP",
  71         "SCCCN",
  72         "StopCCN", // 4
  73         "reserved",
  74         "HELLO",
  75         "OCRQ",
  76         "OCRP",
  77         "OCCN",
  78         "ICRQ", // 10
  79         "ICRP",
  80         "ICCN",
  81         "reserved",
  82         "CDN",
  83         "WEN", // 15
  84         "SLI",
  85 };
  86
  87 char *attributes[] = {
  88         "Message Type", // 0
  89         "Result Code", // 1
  90         "Protocol Version", // 2
  91         "Framing Capabilities", // 3
  92         "Bearer Capabilities", // 4
  93         "Tie Breaker", // 5
  94         "Firmware Revision", // 6
  95         "Host Name", // 7
  96         "Vendor Name", // 8
  97         "Assigned Tunnel ID", // 9
  98         "Receive Window Size", // 10
  99         "Challenge", // 11
 100         "Q.931 Cause Code", // 12
 101         "Challenge Response", // 13
 102         "Assigned Session ID", // 14
 103         "Call Serial Number", // 15
 104         "Minimum BPS", // 16
 105         "Maximum BPS", // 17
 106         "Bearer Type", // 18 (2 = Analog, 1 = Digital)
 107         "Framing Type", // 19 (2 = Async, 1 = Sync)
 108         "Reserved 20", // 20
 109         "Called Number", // 21
 110         "Calling Number", // 22
 111         "Sub Address", // 23
 112         "Tx Connect Speed", // 24
 113         "Physical Channel ID", // 25
 114         "Initial Received LCP CONFREQ", // 26
 115         "Last Sent LCP CONFREQ", // 27
 116         "Last Received LCP CONFREQ", // 28
 117         "Proxy Authen Type", // 29
 118         "Proxy Authen Name", // 30
 119         "Proxy Authen Challenge", // 31
 120         "Proxy Authen ID", // 32
 121         "Proxy Authen Response", // 33
 122         "Call Errors", // 34
 123         "ACCM", // 35
 124         "Random Vector", // 36
 125         "Private Group ID", // 37
 126         "Rx Connect Speed", // 38
 127         "Sequencing Required", // 39
 128 };
 129
 130 char *result_codes[] = {
 131         "Reserved",
 132         "General request to clear control connection",
 133         "General error--Error Code indicates the problem",
 134         "Control channel already exists",
 135         "Requester is not authorized to establish a control channel",
 136         "The protocol version of the requester is not supported",
 137         "Requester is being shut down",
 138         "Finite State Machine error",
 139 };
 140
 141 char *error_codes[] = {
 142         "No general error",
 143         "No control connection exists yet for this LAC-LNS pair",
 144         "Length is wrong",
 145         "One of the field values was out of range or reserved field was non-zero",
 146         "Insufficient resources to handle this operation now",
 147         "The Session ID is invalid in this context",
 148         "A generic vendor-specific error occurred in the LAC",
 149         "Try another LNS",
 150         "Session or tunnel was shutdown due to receipt of an unknown AVP with the M-bit set",
 151 };
 152
 153
 154 typedef struct
 155 {
 156         char buf[4096];
 157         int length;
 158 } controlt;
 159
 160 typedef struct avp_s
 161 {
 162         int length;
 163         int type;
 164         struct avp_s *next;
 165         char value[1024];
 166 } avp;
 167
 168 typedef struct
 169 {
 170         int length;
 171         u16 session;
 172         u16 tunnel;
 173         u16 ns;
 174         u16 nr;
 175         u16 mtype;
 176         char *buf;
 177         avp *first;
 178         avp *last;
 179 } control_message;
 180
 181 typedef struct {
 182 unsigned long long send_count , recv_count ;
 183 unsigned long long spkt , rpkt ;
 184 unsigned int dropped;
 185 unsigned long sbytes , rbytes ;
 186 int quitit;
 187 struct sessiont
 188 {
 189         short remote_session;
 190         char open;
 191         int ppp_state;
 192         unsigned char ppp_identifier;
 193         int addr;
 194 } sessions[65536];
 195
 196 int active_sessions ;
 197 } sharedt;
 198
 199 sharedt * ss;
 200
 201 void controlsend(controlt * c, short t, short s);
 202 void controlnull(short t);
 203 controlt *controlnew(u16 mtype);
 204 void controls(controlt * c, u16 avp, char *val, u8 m);
 205 void control16(controlt * c, u16 avp, u16 val, u8 m);
 206 void control32(controlt * c, u16 avp, u32 val, u8 m);
 207 void controlfree(controlt *c);
 208 control_message *parsecontrol(char *buf, int length);
 209 void dump_control_message(control_message *c);
 210 u32 avp_get_32(control_message *c, int id);
 211 u16 avp_get_16(control_message *c, int id);
 212 char *avp_get_s(control_message *c, int id);
 213 void reader_thread(int udpfd);
 214 void skip_zlb();
 215 void cm_free(control_message *m);
 216 controlt *ppp_new(u16 session, int protocol);
 217 void ppp_free(controlt *packet);
 218 controlt *ppp_lcp(u16 s, unsigned char type, char identifier);
 219 controlt *ppp_ipcp(u16 s, unsigned char type, char identifier);
 220 void ppp_send(controlt *c);
 221 void ppp_add_16(controlt * c, u16 val);
 222 void ppp_add_32(controlt * c, u32 val);
 223 void ppp_add_s(controlt * c, char *val);
 224 void ppp_lcp_add_option(controlt *c, unsigned char option, unsigned char length, int data);
 225 void dump_ppp_packet(char *packet, int l);
 226 controlt *ppp_pap(u16 s, unsigned char type, char identifier, char *username, char *password);
 227 char *inet_toa(unsigned long addr);
 228 __u16 checksum(unsigned char *addr, int count);
 229 void sigalarm(int junk);
 230 void sigint(int signal);
 231 void clean_shutdown();
 232 void print_report();
 233
 234 int ns = 0, nr = 0;
 235 int udpfd;
 236 int t = 0;
 237 struct sockaddr_in gatewayaddr = {0};
 238 int numsessions = NUM_SESSIONS;
 239 int packet_length = PACKET_LENGTH;
 240 int target_pps = TARGET_PPS;
 241 char *target = TARGET;
 242 char *gwaddr = GWADDR;
 243 int max_packets = MAX_PACKETS;
 244 int ppsend;
 245 int do_init = 1;
 246 char **session_usernames;
 247 char *base_username = "dslloadtest";
 248 char *base_password = "testing";
 249 char *suffix = "@optusnet.com.au";
 250
 251 int main(int argc, char *argv[])
 252 {
 253         int s;
 254         char *packet;
 255
 256         ss = (sharedt*) mmap(NULL, sizeof(*ss), PROT_READ | PROT_WRITE, MAP_SHARED | MAP_ANONYMOUS, 0, 0);
 257
 258         // Process Arguments {{{
 259         while ((s = getopt(argc, argv, "?hs:g:l:p:m:t:nU:P:")) > 0)
 260         {
 261                 switch (s)
 262                 {
 263                         case 's' :
 264                                 numsessions = atoi(optarg);
 265                                 if (numsessions <= 0)
 266                                 {
 267                                         printf("You must have at least 1 session\n");
 268                                         return -1;
 269                                 }
 270                                 break;
 271                         case 'l' :
 272                                 packet_length = atoi(optarg);
 273                                 if (packet_length < 64)
 274                                 {
 275                                         printf("You must have at least 64 byte packets\n");
 276                                         return -1;
 277                                 }
 278                                 break;
 279                         case 'n' :
 280                                 do_init = 0;
 281                                 break;
 282                         case 'p' :
 283                                 target_pps = atoi(optarg);
 284                                 break;
 285                         case 'm' :
 286                                 max_packets = atoi(optarg);
 287                                 if (packet_length < 64)
 288                                 {
 289                                         printf("You must send at least 50 packets.\n");
 290                                         return -1;
 291                                 }
 292                                 break;
 293                         case 't' :
 294                                 target = strdup(optarg);
 295                                 break;
 296                         case 'g' :
 297                                 gwaddr = strdup(optarg);
 298                                 break;
 299                         case 'U' :
 300                                 base_username = strdup(optarg);
 301                                 break;
 302                         case 'P' :
 303                                 base_password = strdup(optarg);
 304                                 break;
 305                         case 'h' :
 306                         case '?' :
 307                                 printf("Options:\n");
 308                                 printf("\t-s number of ss->sessions\n");
 309                                 printf("\t-l packet length\n");
 310                                 printf("\t-p target pps\n");
 311                                 printf("\t-m maximum number of packets\n");
 312                                 printf("\t-t target IP address\n");
 313                                 printf("\t-g gateway IP address\n");
 314                                 printf("\t-U username (or base if multiple)\n");
 315                                 printf("\t-P password\n");
 316                                 return(0);
 317                                 break;
 318                 }
 319         }
 320         if (target_pps)
 321                 ppsend = target_pps / 50;
 322         else
 323                 ppsend = 0;
 324
 325         packet = calloc(4096, 1);
 326
 327         memset(ss->sessions, 0, sizeof(ss->sessions));
 328
 329         if (do_init)
 330                 printf("Creating %d ss->sessions to %s\n", numsessions, gwaddr);
 331         printf("Targeting %d packets per second\n", target_pps);
 332         if (max_packets) printf("Sending a maximum of %d packets\n", max_packets);
 333         printf("Sending packets to %s\n", target);
 334         printf("Sending %d byte packets\n", packet_length);
 335
 336         session_usernames = (char **)calloc(sizeof(char *), numsessions);
 337         if (numsessions > 1)
 338         {
 339                 int sul = strlen(base_username) + 10;
 340                 int i;
 341
 342                 for (i = 0; i < numsessions; i++)
 343                 {
 344                         session_usernames[i] = (char *)calloc(sul, 1);
 345                         snprintf(session_usernames[i], sul, "%s%d", base_username, i+1);
 346                 }
 347         }
 348         else
 349         {
 350                 session_usernames[0] = strdup(base_username);
 351         }
 352         // }}}
 353
 354         // Create socket/*{{{*/
 355         {
 356                 int on = 1;
 357                 struct sockaddr_in addr;
 358
 359                 memset(&addr, 0, sizeof(addr));
 360                 addr.sin_family = AF_INET;
 361                 addr.sin_port = htons(38001);
 362
 363                 udpfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
 364                 if (udpfd <= 0)
 365                 {
 366                         perror("socket");
 367                         return -1;
 368                 }
 369
 370                 setsockopt(udpfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
 371                 if (bind(udpfd, (void *) &addr, sizeof(addr)) < 0)
 372                 {
 373                         perror("bind");
 374                         return -1;
 375                 }
 376
 377                 printf("Bound to port %d\n", htons(addr.sin_port));
 378         }/*}}}*/
 379
 380         gatewayaddr.sin_family = AF_INET;
 381         gatewayaddr.sin_port = htons(1701);
 382         inet_aton(gwaddr, &gatewayaddr.sin_addr);
 383
 384         // Create tunnel/*{{{*/
 385         if (do_init) {
 386                 controlt *c;
 387                 control_message *r;
 388
 389                 c = controlnew(1); // SCCRQ
 390                 controls(c, 7, "loadtest", 0); // Tunnel Hostname
 391                 controls(c, 8, "OIE", 0); // Vendor Name
 392                 control16(c, 9, 1, 0); // Assigned Tunnel ID
 393                 control16(c, 2, 256, 0); // Version 1.0
 394                 control16(c, 3, 1, 0); // Framing (Async)
 395                 control16(c, 4, 1, 0); // Bearer (Digital)
 396                 control16(c, 10, 20, 0); // Receive Window Size
 397                 controlsend(c, 0, 0);
 398                 controlfree(c);
 399
 400                 // Receive reply/*{{{*/
 401                 {
 402                         struct sockaddr_in addr;
 403                         int alen = sizeof(addr), l;
 404
 405                         l = recvfrom(udpfd, packet, 4096, 0, (void *) &addr, &alen);
 406                         if (l < 0)
 407                         {
 408                                 printf("Error creating tunnel: %s\n", strerror(errno));
 409                                 return -1;
 410                         }
 411                         printf("Received ");
 412                         r = parsecontrol(packet, l);
 413                         if (!r->first)
 414                         {
 415                                 printf("Invalid packet.. no first avp\n");
 416                                 return -1;
 417                         }
 418
 419                         printf("Assigned tunnel: %d\n", t = avp_get_16(r, 9));
 420                         cm_free(r);
 421
 422                         c = controlnew(3); // SCCCN
 423                         controlsend(c, t, 0);
 424                         controlfree(c);
 425                         skip_zlb();
 426                 }/*}}}*/
 427         }/*}}}*/
 428
 429
 430         // Create ss->sessions/*{{{*/
 431         if (do_init)
 432         {
 433                 for (s = 1; s <= numsessions; s++)
 434                 {
 435                         controlt *c;
 436
 437                         c = controlnew(10); // ICRQ
 438                         controls(c, 21, "12356", 0); // Called Number
 439                         controls(c, 22, "000", 0); // Calling Number
 440                         control16(c, 14, s, 0); // Assigned Session ID
 441                         controlsend(c, t, 0);
 442                         controlfree(c);
 443                         usleep(15000); // 15 ms
 444                 }
 445         }
 446         printf("All session create requests sent...\n");/*}}}*/
 447
 448         if ( fork() == 0) {
 449                 reader_thread(udpfd);
 450                 exit(0);
 451         }
 452
 453         {
 454                 char tmp[512];
 455                 fprintf(stderr, "Press enter to begin sending traffic\n");
 456                 fgets(tmp, 512, stdin);
 457         }
 458
 459         fprintf(stderr, "Beginning sending traffic through %d ss->sessions\n", ss->active_sessions);
 460         printf("        TS: Total Packets Sent\n");
 461         printf("        TL: Total Packets Lost\n");
 462         printf("        PL: Packet Loss\n");
 463         printf("        SS: Send Speed\n");
 464         printf("        RS: Receive Speed\n");
 465         printf("        SP: Packets/Second Sent\n");
 466         printf("        RP: Packets/Second Received\n");
 467         printf("        NS: Number of active ss->sessions\n");
 468
 469         signal(SIGALRM, sigalarm);
 470         signal(SIGINT, sigint);
 471         alarm(1);
 472
 473         // Traffic generation loop {{{
 474         {
 475                 struct sockaddr_in to;
 476                 struct iphdr *iph;
 477                 struct udphdr *udph;
 478                 char *data;
 479                 int len = 0;
 480                 unsigned int seq = 0;
 481                 controlt *c;
 482
 483                 // Get address
 484                 memset(&to, 0, sizeof(struct sockaddr_in));
 485                 to.sin_family = AF_INET;
 486                 inet_aton(target, &to.sin_addr);
 487
 488                 c = ppp_new(1, PPPIP);
 489
 490                 iph = (struct iphdr *)(c->buf + c->length);
 491                 udph = (struct udphdr *)(c->buf + c->length + sizeof(struct iphdr));
 492                 data = (char *)(c->buf + c->length + sizeof(struct iphdr) + sizeof(struct udphdr));
 493                 len = sizeof(struct iphdr) + sizeof(struct udphdr);
 494                 c->length += len;
 495
 496                 //IP
 497                 c->length += sizeof(struct iphdr);
 498                 iph->tos = 0;
 499                 iph->id = ntohs(1);
 500                 iph->frag_off = ntohs(1 << 14);
 501                 iph->ttl = 30;
 502                 iph->check = 0;
 503                 iph->version = 4;
 504                 iph->ihl = 5;
 505                 iph->protocol = 17;
 506                 memcpy(&iph->daddr, &to.sin_addr, sizeof(iph->daddr));
 507
 508                 // UDP
 509                 udph->source = ntohs(39999);
 510                 udph->dest = ntohs(39000);
 511                 udph->check = 0;
 512
 513                 // Data
 514                 memset(data, 64, 1500);
 515
 516                 udph->len = ntohs(sizeof(struct udphdr) + packet_length);
 517                 iph->tot_len = ntohs(len + packet_length);
 518                 c->length += packet_length;
 519
 520                 while (!ss->quitit && ss->active_sessions)
 521                 {
 522                         int i;
 523                         for (i = 1; i <= numsessions && !ss->quitit; i++)
 524                         {
 525                                 // Skip ss->sessions that aren't active yet
 526                                 if (!ss->sessions[i].open || ss->sessions[i].ppp_state != 2)
 527                                         continue;
 528
 529                                 *(u16 *)(c->buf + 4) = htons(ss->sessions[i].remote_session); // Session ID
 530                                 iph->saddr = ss->sessions[i].addr;
 531                                 iph->check = 0;
 532                                 iph->check = ntohs(checksum((char *)iph, sizeof(struct iphdr)));
 533
 534                                 *((unsigned int *) data) = seq++;
 535                                 ppp_send(c);
 536
 537                                 ss->send_count++;
 538                                 ss->spkt++;
 539                                 ss->sbytes += c->length;
 540
 541                                 if (ppsend && ss->send_count % ppsend == 0)
 542                                 {
 543                                         struct timespec req;
 544                                         req.tv_sec = 0;
 545                                         req.tv_nsec = 5 * 1000 * 1000;
 546                                         nanosleep(&req, NULL);
 547                                 }
 548
 549                                 if (max_packets && ss->send_count >= max_packets) ss->quitit++;
 550                         }
 551                 }
 552
 553                 c->length -= packet_length;
 554
 555         }/*}}}*/
 556
 557         clean_shutdown();
 558         print_report();
 559
 560         close(udpfd);
 561         return 0;
 562 }
 563
 564 void print_report()
 565 {
 566         float loss;
 567
 568         loss = 100 - (((ss->recv_count * 1.0) / (ss->send_count * 1.0)) * 100.0);
 569
 570         printf("\n");
 571         printf("Total Packets Sent:     %llu\n", ss->send_count);
 572         printf("Total Packets Received: %llu\n", ss->recv_count);
 573         printf("Overall Packet Loss:    %0.2f%%", loss);
 574         printf("\n");
 575 }
 576
 577 void clean_shutdown()/*{{{*/
 578 {
 579         int i;
 580         for (i = 0; i < numsessions; i++)
 581         {
 582                 // Close Session
 583                 controlt *c;
 584
 585                 if (!ss->sessions[i].open) continue;
 586                 c = controlnew(14); // CDN
 587                 control16(c, 14, i, 0); // Assigned Session ID
 588                 control16(c, 1, 1, 0); // Result Code
 589                 controlsend(c, t, ss->sessions[i].remote_session);
 590                 controlfree(c);
 591         }
 592
 593         // Close Tunnel
 594         {
 595                 controlt *c;
 596
 597                 c = controlnew(4); // StopCCN
 598                 control16(c, 9, 1, 0); // Assigned Tunnel ID
 599                 control16(c, 1, 1, 0); // Result Code
 600                 controlsend(c, t, 0);
 601                 controlfree(c);
 602         }
 603 }/*}}}*/
 604
 605 void sigint(int signal)
 606 {
 607         ss->quitit++;
 608 }
 609
 610 void sigalarm(int junk)
 611 {
 612         static unsigned long long last_rpkts[AVG_SIZE], last_spkts[AVG_SIZE];
 613         static int last = 0, avg_count = 0;
 614         register unsigned int avg_s = 0, avg_r = 0, i;
 615         float loss;
 616
 617         last_rpkts[last] = ss->rpkt;
 618         last_spkts[last] = ss->spkt;
 619         last = (last + 1) % AVG_SIZE;
 620         if (avg_count < AVG_SIZE) avg_count++;
 621
 622         for (i = 0; i < avg_count; i++)
 623         {
 624                 avg_s += last_spkts[i];
 625                 avg_r += last_rpkts[i];
 626         }
 627         avg_s /= avg_count;
 628         avg_r /= avg_count;
 629
 630         loss = 100 - (((avg_r * 1.0) / (avg_s * 1.0)) * 100.0);
 631         fprintf(stderr, "TS:%llu TL:%lld DR:%4d PL:%-3.2f%% SS:%0.1fMbits/s RS:%0.1fMbits/s NS:%u SP:%u RP:%u\n",
 632                         ss->send_count, ss->send_count-ss->recv_count, ss->dropped, loss,
 633                         (ss->sbytes/1024.0/1024.0*8), (ss->rbytes/1024.0/1024.0*8),
 634                         ss->active_sessions,
 635                         avg_s, avg_r);
 636
 637         ss->spkt = ss->rpkt = 0;
 638         ss->sbytes = ss->rbytes = 0;
 639         alarm(1);
 640 }
 641
 642 __u16 checksum(unsigned char *addr, int count)
 643 {
 644         register long sum = 0;
 645
 646         for (; count > 1; count -= 2)
 647         {
 648                 sum += ntohs(*(u32 *)addr);
 649                 addr += 2;
 650         }
 651
 652         if (count > 1) sum += *(unsigned char *)addr;
 653
 654         // take only 16 bits out of the 32 bit sum and add up the carries
 655         while (sum >> 16)
 656                 sum = (sum & 0xFFFF) + (sum >> 16);
 657
 658         // one's complement the result
 659         sum = ~sum;
 660
 661         return ((u16) sum);
 662 }
 663
 664 // Control Stuff {{{
 665 void control16(controlt * c, u16 avp, u16 val, u8 m)
 666 {
 667         u16 l = (m ? 0x8008 : 0x0008);
 668         *(u16 *) (c->buf + c->length + 0) = htons(l);
 669         *(u16 *) (c->buf + c->length + 2) = htons(0);
 670         *(u16 *) (c->buf + c->length + 4) = htons(avp);
 671         *(u16 *) (c->buf + c->length + 6) = htons(val);
 672         c->length += 8;
 673 }
 674
 675 // add an AVP (32 bit)
 676 void control32(controlt * c, u16 avp, u32 val, u8 m)
 677 {
 678         u16 l = (m ? 0x800A : 0x000A);
 679         *(u16 *) (c->buf + c->length + 0) = htons(l);
 680         *(u16 *) (c->buf + c->length + 2) = htons(0);
 681         *(u16 *) (c->buf + c->length + 4) = htons(avp);
 682         *(u32 *) (c->buf + c->length + 6) = htonl(val);
 683         c->length += 10;
 684 }
 685
 686 // add an AVP (32 bit)
 687 void controls(controlt * c, u16 avp, char *val, u8 m)
 688 {
 689         u16 l = ((m ? 0x8000 : 0) + strlen(val) + 6);
 690         *(u16 *) (c->buf + c->length + 0) = htons(l);
 691         *(u16 *) (c->buf + c->length + 2) = htons(0);
 692         *(u16 *) (c->buf + c->length + 4) = htons(avp);
 693         memcpy(c->buf + c->length + 6, val, strlen(val));
 694         c->length += 6 + strlen(val);
 695 }
 696
 697 // new control connection
 698 controlt *controlnew(u16 mtype)
 699 {
 700         controlt *c;
 701         c = calloc(sizeof(controlt), 1);
 702         c->length = 12;
 703         control16(c, 0, mtype, 1);
 704         return c;
 705 }
 706
 707 void controlnull(short t)
 708 {
 709         controlt *c;
 710         c = calloc(sizeof(controlt), 1);
 711         c->length = 12;
 712         controlsend(c, t, 0);
 713         controlfree(c);
 714         ns--;
 715 }
 716
 717 // add a control message to a tunnel, and send if within window
 718 void controlsend(controlt * c, short t, short s)
 719 {
 720         *(u16 *) (c->buf + 0) = htons(0xC802); // flags/ver
 721         *(u16 *) (c->buf + 2) = htons(c->length); // length
 722         *(u16 *) (c->buf + 4) = htons(t); // tunnel
 723         *(u16 *) (c->buf + 6) = htons(s); // session
 724         *(u16 *) (c->buf + 8) = htons(ns++); // sequence
 725         *(u16 *) (c->buf + 10) = htons(nr); // sequence
 726 //      printf("Sending ");
 727 //      cm_free(parsecontrol(c->buf, c->length));
 728         sendto(udpfd, c->buf, c->length, 0, (struct sockaddr *)&gatewayaddr, sizeof(gatewayaddr));
 729 }
 730
 731 void controlfree(controlt *c)
 732 {
 733         if (!c) return;
 734         free(c);
 735 }
 736
 737 control_message *parsecontrol(char *buf, int length)
 738 {
 739         char *p = buf;
 740         control_message *c;
 741
 742         c = calloc(sizeof(control_message), 1);
 743         c->buf = buf;
 744         c->length = length;
 745
 746         c->tunnel = ntohs(*(u16 *)(buf + 4));
 747         c->session = ntohs(*(u16 *)(buf + 6));
 748         c->ns = ntohs(*(u16 *)(buf + 8));
 749         c->nr = nr = ntohs(*(u16 *)(buf + 10));
 750         p += 12;
 751         while ((p - buf) < length)
 752         {
 753                 avp *a = calloc(sizeof(avp), 1);
 754                 a->length = ntohs(*(short *)(p)) & 0x3FF;
 755                 a->type = ntohs(*(short *)(p + 4));
 756                 memcpy(a->value, p + 6, a->length - 6);
 757                 if (a->type == 0) c->mtype = ntohs(*(short *)a->value);
 758                 p += a->length;
 759                 if (c->last)
 760                         c->last->next = a;
 761                 else
 762                         c->first = a;
 763                 c->last = a;
 764         }
 765         if (c->first)
 766                 dump_control_message(c);
 767         return c;
 768 }
 769
 770 void dump_control_message(control_message *c)
 771 {
 772         avp *a;
 773         printf("Control Message (type=%u s=%u t=%d ns=%d nr=%d)\n", c->mtype, c->session, c->tunnel, c->ns, c->nr);
 774         for (a = c->first; a; a = a->next)
 775         {
 776                 printf("        avp: %s, len: %d", attributes[a->type], a->length - 6);
 777                 switch (a->type)
 778                 {
 779                         // Short
 780                         case 6 :
 781                         case 9 :
 782                         case 10 :
 783                         case 39 :
 784                         case 14 : printf(", value: %u\n", ntohs(*(short *)a->value));
 785                                  break;
 786
 787                         // Integer
 788                         case 16 :
 789                         case 17 :
 790                         case 24 :
 791                         case 25 :
 792                         case 38 :
 793                         case 15 : printf(", value: %u\n", ntohl(*(u32 *)a->value));
 794                                   break;
 795
 796                         // String
 797                         case 7 :
 798                         case 21 :
 799                         case 22 :
 800                         case 23 :
 801                         case 37 :
 802                         case 8 : printf(", value: \"%s\"\n", a->value);
 803                                  break;
 804
 805                         case 2 : printf(", value: %d.%d\n", *(char *)a->value, *(char *)a->value + 1);
 806                                  break;
 807                         case 0 : printf(", value: %s\n", mtypes[ntohs(*(short *)a->value)]);
 808                                  break;
 809                         case 19 :
 810                         case 3 : printf(", value: (%d) %s %s\n", ntohl(*(u32 *)a->value),
 811                                                  (ntohl(*(u32 *)a->value) & 0x01) ? "synchronous" : "",
 812                                                  (ntohl(*(u32 *)a->value) & 0x02) ? "asynchronous" : "");
 813                                  break;
 814                         case 18 :
 815                         case 4 : printf(", value: (%d) %s %s\n", ntohl(*(u32 *)a->value),
 816                                                  (ntohl(*(u32 *)a->value) & 0x01) ? "digital" : "",
 817                                                  (ntohl(*(u32 *)a->value) & 0x02) ? "analog" : "");
 818                                  break;
 819
 820                         default : printf("\n");
 821                                   break;
 822                 }
 823         }
 824         printf("\n");
 825 }
 826
 827 u16 avp_get_16(control_message *c, int id)
 828 {
 829         avp *a;
 830
 831         for (a = c->first; a; a = a->next)
 832                 if (a->type == id) return ntohs(*(short *)a->value);
 833         return 0;
 834 }
 835
 836 u32 avp_get_32(control_message *c, int id)
 837 {
 838         avp *a;
 839
 840         for (a = c->first; a; a = a->next)
 841                 if (a->type == id) return ntohl(*(u32 *)a->value);
 842         return 0;
 843 }
 844
 845 char *avp_get_s(control_message *c, int id)
 846 {
 847         avp *a;
 848
 849         for (a = c->first; a; a = a->next)
 850                 if (a->type == id) return (char *)a->value;
 851         return 0;
 852 }
 853
 854 void cm_free(control_message *m)
 855 {
 856         avp *a, *n;
 857
 858         for (a = m->first; a; )
 859         {
 860                 n = a->next;
 861                 free(a);
 862                 a = n;
 863         }
 864
 865         free(m);
 866 }
 867
 868 // }}}
 869
 870 void reader_thread(int updfd)/*{{{*/
 871 {
 872         unsigned char *packet;
 873         unsigned int seq = 0;
 874
 875         printf("Starting reader thread\n");
 876         packet = malloc(4096);
 877         while (!ss->quitit)
 878         {
 879                 struct sockaddr_in addr;
 880                 int alen = sizeof(addr);
 881                 control_message *m;
 882                 int l;
 883                 int s;
 884                 int pfc = 0;
 885
 886 //              memset(packet, 0, 4096);
 887                 if ((l = recvfrom(udpfd, packet, 4096, 0, (void *) &addr, &alen)) < 0) break;
 888                 ss->rbytes += l;
 889                 if (!do_init)
 890                 {
 891                         ss->recv_count++;
 892                         ss->rpkt++;
 893                         continue;
 894                 }
 895                 if (l < 12)
 896                 {
 897                         printf("Short packet received: %d bytes\n", l);
 898                 }
 899                 s = ntohs(*(u16 *)(packet + 4));
 900                 if (!s)
 901                 {
 902                         printf("Invalid session ID\n");
 903                         continue;
 904                 }
 905                 if (packet[0] == 0xc8)
 906                 {
 907                         // Control Packet
 908                         printf("Reader Received ");
 909                         m = parsecontrol(packet, l);
 910                         printf("\n");
 911                         s = m->session;
 912
 913                         switch (m->mtype)
 914                         {
 915                                 case 4  : printf("StopCCN\n");
 916                                           printf("Killing tunnel %d\n", avp_get_16(m, 9));
 917                                           ss->quitit++;
 918                                           break;
 919                                 case 6  : printf("HELLO, sending ZLB ACK\n");
 920                                           controlnull(t);
 921                                           break;
 922                                 case 11 :
 923                                         {
 924                                                 controlt *c;
 925
 926                                                 printf("Received ICRP. Responding with CONFREQ\n");
 927
 928                                                 ss->sessions[s].remote_session = avp_get_16(m, 14);
 929                                                 ss->sessions[s].open = 1;
 930                                                 ss->sessions[s].ppp_state = 1;
 931
 932                                                 c = controlnew(12); // ICCN
 933                                                 controlsend(c, t, ss->sessions[s].remote_session);
 934                                                 controlfree(c);
 935
 936                                                 c = ppp_lcp(s, CONFREQ, 0);
 937                                                 ppp_lcp_add_option(c, 1, 2, htons(1500)); // MRU = 1400
 938                                                 ppp_lcp_add_option(c, 3, 2, htons(0xC023)); // Authentication Protocol - PAP
 939                                                 ppp_send(c);
 940                                                 controlfree(c);
 941                                                 break;
 942                                         }
 943                                 case 14 : {
 944                                                 int s;
 945                                                 printf("CDN\n");
 946                                                 s = avp_get_16(m, 14);
 947                                                 printf("Killing session %d\n", s);
 948                                                 ss->sessions[s].open = 0;
 949                                                 ss->sessions[s].ppp_state = 0;
 950                                                 ss->active_sessions--;
 951                                                 controlnull(t);
 952                                                 break;
 953                                           }
 954
 955                         }
 956                         if (m->mtype == 4)
 957                         {
 958                                 printf("StopCCN Received.. Dieing\n");
 959                                 ss->quitit++;
 960                                 break;
 961                         }
 962                         cm_free(m);
 963                 }
 964                 else
 965                 {
 966                         // Data Packet
 967                         unsigned short protocol = ntohs(*(u16 *)(packet + 6));
 968
 969                         if (protocol == 0xff03)
 970                         {
 971                                 pfc = 2;
 972                                 packet += 2;
 973                                 protocol = ntohs(*(u16 *)(packet + 6));
 974                         }
 975                         if (protocol != PPPIP)
 976                         {
 977                                 printf("Received ");
 978                                 dump_ppp_packet(packet + 6, l - 6);
 979                         }
 980
 981                         if (protocol == PPPLCP)
 982                         {
 983                                 controlt *r;
 984                                 unsigned char ppp_id = *(char *)(packet + 9);
 985
 986                                 switch (*(char *)(packet + 8))
 987                                 {
 988                                         case CONFREQ :
 989                                                 r = ppp_lcp(s, CONFACK, ppp_id);
 990                                                 ppp_send(r);
 991                                                 break;
 992                                         case CONFACK :
 993                                                 r = ppp_pap(s, CONFREQ, 0, session_usernames[s-1], base_password);
 994                                                 ppp_send(r);
 995                                                 break;
 996                                         case TERMREQ :
 997                                                 r = ppp_lcp(s, TERMACK, ppp_id);
 998                                                 ppp_send(r);
 999                                                 break;
1000                                         case ECHOREQ :
1001                                                 r = ppp_lcp(s, ECHOREP, ppp_id);
1002                                                 ppp_add_32(r, 0);
1003                                                 ppp_send(r);
1004                                                 break;
1005                                 }
1006                         }
1007                         else if (protocol == PPPIPCP)
1008                         {
1009                                 controlt *r;
1010                                 int taddr = 0;
1011                                 u32 address = *(u32 *)(packet + 14);
1012
1013                                 switch (*(char *)(packet + 8))
1014                                 {
1015                                         case CONFREQ :
1016                                                 r = ppp_ipcp(s, CONFREQ, time(NULL) % 255);
1017                                                 ppp_lcp_add_option(r, 3, 4, htonl(taddr)); // Request 0.0.0.0
1018                                                 ppp_send(r);
1019                                                 controlfree(r);
1020                                                 r = ppp_ipcp(s, CONFACK, time(NULL) % 255);
1021                                                 ppp_lcp_add_option(r, 3, 4, address); // ACK gateway IP
1022                                                 ppp_send(r);
1023                                                 controlfree(r);
1024                                                 break;
1025                                         case CONFNAK :
1026                                                 // Ack whatever address we are given - it's ours
1027                                                 r = ppp_ipcp(s, CONFACK, time(NULL) % 255);
1028                                                 ppp_lcp_add_option(r, 3, 4, address); // Request 0.0.0.0
1029                                                 ppp_send(r);
1030                                                 controlfree(r);
1031                                                 printf("Session %d: %s\n", s, inet_toa(address));
1032                                                 ss->sessions[s].ppp_state = 2;
1033                                                 ss->sessions[s].addr = address;
1034                                                 ss->active_sessions++;
1035                                                 break;
1036                                         case CONFACK :
1037                                                 printf("Conf-Ack Received\n");
1038                                                 break;
1039                                         case TERMREQ :
1040                                                 printf("Term-Req Received\n");
1041                                                 break;
1042                                         case ECHOREQ :
1043                                                 printf("Echo-Req Received\n");
1044                                                 break;
1045                                         case ECHOREP :
1046                                                 printf("Echo-Rep Received\n");
1047                                                 break;
1048                                 }
1049                         }
1050                         else if (protocol == PPPPAP)
1051                         {
1052                                 if (*(u16 *)(packet + 8) == 3)
1053                                 {
1054                                         controlt *c;
1055                                         printf("Closing Connection\n");
1056
1057                                         c = controlnew(14); // CDN
1058                                         control16(c, 14, ss->sessions[s].remote_session, 0); // Assigned Session ID
1059                                         controlsend(c, t, 0);
1060                                         controlfree(c);
1061                                         ss->sessions[s].open = 0;
1062                                 }
1063                         }
1064                         else if (protocol == PPPIP)
1065                         {
1066                                 struct iphdr *iph = (struct iphdr *)(packet + 8);
1067                                 char * data = (char*) (packet + 8 + sizeof(struct iphdr) + sizeof(struct udphdr));
1068                                 if (!ss->sessions[s].open)
1069                                 {
1070                                         printf("Packet for closed session %d\n", s);
1071                                         continue;
1072                                 }
1073
1074                                 if (iph->protocol == 17)
1075                                 {
1076                                         int iseq;
1077                                         ss->recv_count++;
1078                                         ss->rpkt++;
1079                                         iseq = *((unsigned int *) data);
1080                                         if (seq != iseq) {
1081                                                 ss->dropped += (iseq - seq) ;
1082                                         }
1083                                         seq = iseq + 1; // Next sequence number to expect.
1084                                 }
1085                         }
1086                 }
1087                 packet -= pfc;
1088         }
1089         free(packet);
1090
1091         printf("Closing reader thread\n");
1092
1093 }/*}}}*/
1094
1095 void skip_zlb() /*{{{*/
1096 {
1097         struct sockaddr_in addr;
1098         int alen = sizeof(addr);
1099         char buf[1024];
1100         int l;
1101         l = recvfrom(udpfd, buf, 1024, MSG_PEEK, (void *) &addr, &alen);
1102         if (l < 0)
1103         {
1104                 printf("recvfrom: %s\n", strerror(errno));
1105                 return;
1106         }
1107         if (l <= 12)
1108         {
1109                 printf("Skipping ZLB (l=%d)\n", l);
1110                 recvfrom(udpfd, buf, 1024, 0, (void *) &addr, &alen);
1111         }
1112 }
1113 /*}}}*/
1114
1115 // PPP Stuff {{{
1116 controlt *ppp_new(u16 session, int protocol)
1117 {
1118         controlt *c = calloc(sizeof(controlt), 1);
1119         *(u16 *)(c->buf + 4) = htons(ss->sessions[session].remote_session); // Tunnel
1120         *(u16 *)(c->buf + 6) = htons(protocol);
1121         c->length += 8;
1122
1123         return c;
1124 }
1125
1126 void ppp_free(controlt *c)
1127 {
1128         free(c);
1129 }
1130
1131 controlt *ppp_lcp(u16 s, unsigned char type, char identifier)
1132 {
1133         controlt *c;
1134
1135         if (!identifier) identifier = ss->sessions[s].ppp_identifier++;
1136         c = ppp_new(s, PPPLCP);
1137         *(char *)(c->buf + c->length + 0) = type;
1138         *(char *)(c->buf + c->length + 1) = identifier;
1139         *(u16 *)(c->buf + c->length + 2) = ntohs(4);
1140         c->length += 4;
1141
1142         return c;
1143 }
1144
1145 controlt *ppp_ipcp(u16 s, unsigned char type, char identifier)
1146 {
1147         controlt *c;
1148
1149         if (!identifier) identifier = ss->sessions[s].ppp_identifier++;
1150         c = ppp_new(s, PPPIPCP);
1151         *(char *)(c->buf + c->length + 0) = type;
1152         *(char *)(c->buf + c->length + 1) = identifier;
1153         *(u16 *)(c->buf + c->length + 2) = ntohs(4);
1154         c->length += 4;
1155
1156         return c;
1157 }
1158
1159 controlt *ppp_pap(u16 s, unsigned char type, char identifier, char *username, char *password)
1160 {
1161         controlt *c;
1162
1163         if (!identifier) identifier = ss->sessions[s].ppp_identifier++;
1164         c = ppp_new(s, PPPPAP);
1165         *(char *)(c->buf + c->length + 0) = type;
1166         *(char *)(c->buf + c->length + 1) = identifier;
1167         *(u16 *)(c->buf + c->length + 2) = ntohs(4);
1168         c->length += 4;
1169
1170         *(char *)(c->buf + c->length) = strlen(username) + strlen(suffix);
1171         memcpy((c->buf + c->length + 1), username, strlen(username));
1172         memcpy((c->buf + c->length + 1 + strlen(username)), suffix, strlen(suffix));
1173         c->length += strlen(username) + 1 + strlen(suffix);
1174
1175         *(char *)(c->buf + c->length) = strlen(password);
1176         memcpy((c->buf + c->length + 1), password, strlen(password));
1177         c->length += strlen(password) + 1;
1178
1179         return c;
1180 }
1181
1182 void ppp_send(controlt *c)
1183 {
1184         *(u16 *)(c->buf + 0) = htons(0x0002); // flags/ver
1185         *(u16 *)(c->buf + 2) = htons(t); // tunnel
1186         *(u16 *)(c->buf + 10) = ntohs(c->length - 8);
1187         if (sendto(udpfd, c->buf, c->length, 0, (struct sockaddr *)&gatewayaddr, sizeof(gatewayaddr)) < 0)
1188                 perror("sendto");
1189         if (htons(*(u16 *)(c->buf + 6)) != PPPIP)
1190         {
1191                 printf("PPP Sending ");
1192                 dump_ppp_packet(c->buf + 6, c->length - 6);
1193         }
1194 }
1195
1196 void ppp_add_16(controlt *c, u16 val)
1197 {
1198         *(u16 *) (c->buf + c->length) = htons(val);
1199         c->length += 2;
1200 }
1201
1202 void ppp_add_32(controlt *c, u32 val)
1203 {
1204         *(u32 *) (c->buf + c->length) = htons(val);
1205         c->length += 4;
1206 }
1207
1208 void ppp_add_s(controlt *c, char *val)
1209 {
1210         memcpy(c->buf + c->length, val, strlen(val));
1211         c->length += strlen(val);
1212 }
1213
1214 void ppp_lcp_add_option(controlt *c, unsigned char option, unsigned char length, int data)
1215 {
1216         *(char *)(c->buf + c->length + 0) = option;
1217         *(char *)(c->buf + c->length + 1) = length + 2;
1218         memcpy(c->buf + c->length + 2, &data, length);
1219         c->length += 2 + length;
1220 }
1221
1222 void dump_ppp_packet(char *packet, int l)
1223 {
1224         char *p = packet;
1225         int protocol ;
1226         if (*(unsigned char *)p == 0xff) p += 2;
1227         protocol = ntohs(*(u16 *)(p));
1228         printf("PPP Packet\n");
1229         switch (protocol)
1230         {
1231                 case PPPCCP : printf("  Protocol: PPPCCP\n"); break;
1232         }
1233         if (protocol == PPPLCP)
1234         {
1235                 printf("        Protocol: PPPLCP\n");
1236                 printf("        LCP Code: %s\n", lcp_codes[*(u8 *)(p + 2)]);
1237         }
1238         else if (protocol == PPPPAP)
1239         {
1240                 printf("        Protocol: PPPPAP\n");
1241                 if (*(char *)(p + 2) == 2)
1242                 {
1243                                 printf("        Authentication accepted\n");
1244                 }
1245                 else if (*(char *)(p + 2) == 3)
1246                 {
1247                                 printf("        Authentication denied\n");
1248                 }
1249         }
1250         else if (protocol == PPPIPCP)
1251         {
1252                 printf("        Protocol: PPPIPCP\n");
1253                 printf("        IPCP Code: %s\n", lcp_codes[*(u8 *)(p + 2)]);
1254                 printf("        Address: %s\n", inet_toa(*(u32 *)(p + 8)));
1255         }
1256         else if (protocol == PPPIP)
1257         {
1258                 struct iphdr *iph;
1259                 struct protoent *pr;
1260
1261                 iph = (struct iphdr *)(p + 2);
1262
1263                 printf("        Protocol: PPPIP\n");
1264                 printf("        Length: %d\n", l);
1265                 printf("        IP Version: %d\n", iph->version);
1266                 if (iph->version != 4) return;
1267                 pr = getprotobynumber(iph->protocol);
1268                 printf("        IP Header Length: %d\n", iph->ihl);
1269                 printf("        IP TTL: %d\n", iph->ttl);
1270                 printf("        IP Protocol: %s (%d)\n", (pr ? pr->p_name : "unknown"), iph->protocol);
1271                 printf("        IP Checksum: %x\n", ntohs(iph->check));
1272         }
1273         else
1274         {
1275                 printf("        Protocol: unknown 0x%x\n", protocol);
1276         }
1277         printf("\n");
1278 }
1279
1280 char *inet_toa(unsigned long addr)
1281 {
1282         struct in_addr in;
1283         memcpy(&in, &addr, sizeof(unsigned long));
1284         return inet_ntoa(in);
1285 }
1286
1287 // }}}
1288