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