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