projects / l2tpns.git / blob
? search:


c3a40b61206b73e385ef6d90c5db48516703cfd1
[l2tpns.git] / 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
for cloning use: "git clone git://git.sameswireless.fr/l2tpns.git" ; For add to your sources.list: "deb http://package.sameswifi.fr/ jessie main"