projects / l2tpns.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
history | raw | HEAD
Merge from master
[l2tpns.git] / 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
for cloning use: "git clone git://git.sameswireless.fr/l2tpns.git" ; For add to your sources.list: "deb http://package.sameswifi.fr/ jessie main"