test/generateload.c

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