l2tpns/test/generateload.c

#define _POSIX_C_SOURCE 200112L
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#define __USE_XOPEN_EXTENDED
#define __USE_MISC
#include <arpa/inet.h>
#include <time.h>
#include <errno.h>
#include <fcntl.h>
#include <linux/if.h>
#include <netdb.h>
#include <netinet/in.h>
#include <stdarg.h>
#include <string.h>
#include <sys/socket.h>
#include <linux/ip.h>
#include <linux/udp.h>
#include <unistd.h>
#include <signal.h>
#include <getopt.h>
#include <sys/mman.h>

#define PPPLCP	0xc021
#define PPPPAP	0xc023
#define PPPCHAP	0xc223
#define PPPIPCP	0x8021
#define PPPIP	0x0021
#define PPPCCP	0x80fd

#define CONFREQ	1
#define CONFACK	2
#define CONFNAK	3
#define CONFREJ	4
#define TERMREQ	5
#define TERMACK	6
#define CODEREJ	7
#define PROTREJ	8
#define ECHOREQ	9
#define ECHOREP	10
#define DISCREQ	11

#define PACKET_LENGTH	1000
#define TARGET_PPS	5000
#define TARGET		"211.29.131.33"
#define GWADDR		"211.29.131.30"
#define NUM_SESSIONS	1
#define MAX_PACKETS	0
#define AVG_SIZE	5

typedef unsigned short u16;
typedef unsigned int u32;
typedef unsigned char u8;

char *lcp_codes[] = {
	"reserved",
	"CONFREQ",
	"CONFACK",
	"CONFNAK",
	"CONFREJ",
	"TERMREQ",
	"TERMACK",
	"CODEREJ",
	"PROTREJ",
	"ECHOREQ",
	"ECHOREP",
	"DISCREQ",
};

char *mtypes[] = {
	"reserved",
	"SCCRQ",
	"SCCRP",
	"SCCCN",
	"StopCCN", // 4
	"reserved",
	"HELLO",
	"OCRQ",
	"OCRP",
	"OCCN",
	"ICRQ", // 10
	"ICRP",
	"ICCN",
	"reserved",
	"CDN",
	"WEN", // 15
	"SLI",
};

char *attributes[] = {
	"Message Type", // 0
	"Result Code", // 1
	"Protocol Version", // 2
	"Framing Capabilities", // 3
	"Bearer Capabilities", // 4
	"Tie Breaker", // 5
	"Firmware Revision", // 6
	"Host Name", // 7
	"Vendor Name", // 8
	"Assigned Tunnel ID", // 9
	"Receive Window Size", // 10
	"Challenge", // 11
	"Q.931 Cause Code", // 12
	"Challenge Response", // 13
	"Assigned Session ID", // 14
	"Call Serial Number", // 15
	"Minimum BPS", // 16
	"Maximum BPS", // 17
	"Bearer Type", // 18 (2 = Analog, 1 = Digital)
	"Framing Type", // 19 (2 = Async, 1 = Sync)
	"Reserved 20", // 20
	"Called Number", // 21
	"Calling Number", // 22
	"Sub Address", // 23
	"Tx Connect Speed", // 24
	"Physical Channel ID", // 25
	"Initial Received LCP CONFREQ", // 26
	"Last Sent LCP CONFREQ", // 27
	"Last Received LCP CONFREQ", // 28
	"Proxy Authen Type", // 29
	"Proxy Authen Name", // 30
	"Proxy Authen Challenge", // 31
	"Proxy Authen ID", // 32
	"Proxy Authen Response", // 33
	"Call Errors", // 34
	"ACCM", // 35
	"Random Vector", // 36
	"Private Group ID", // 37
	"Rx Connect Speed", // 38
	"Sequencing Required", // 39
};

char *result_codes[] = {
	"Reserved",
	"General request to clear control connection",
	"General error--Error Code indicates the problem",
	"Control channel already exists",
	"Requester is not authorized to establish a control channel",
	"The protocol version of the requester is not supported",
	"Requester is being shut down",
	"Finite State Machine error",
};

char *error_codes[] = {
	"No general error",
	"No control connection exists yet for this LAC-LNS pair",
	"Length is wrong",
	"One of the field values was out of range or reserved field was non-zero",
	"Insufficient resources to handle this operation now",
	"The Session ID is invalid in this context",
	"A generic vendor-specific error occurred in the LAC",
	"Try another LNS",
	"Session or tunnel was shutdown due to receipt of an unknown AVP with the M-bit set",
};


typedef struct
{
	char buf[4096];
	int length;
} controlt;

typedef struct avp_s
{
	int length;
	int type;
	struct avp_s *next;
	char value[1024];
} avp;

typedef struct
{
	int length;
	u16 session;
	u16 tunnel;
	u16 ns;
	u16 nr;
	u16 mtype;
	char *buf;
	avp *first;
	avp *last;
} control_message;

typedef struct {
long long send_count, recv_count;
long long spkt, rpkt ;
int dropped;
long sbytes, rbytes ;
int quitit;
struct sessiont
{
	short remote_session;
	char open;
	int ppp_state;
	unsigned char ppp_identifier;
	int addr;
} sessions[65536];

int active_sessions ;
} sharedt;

sharedt * ss;

void controlsend(controlt * c, short t, short s);
void controlnull(short t);
controlt *controlnew(u16 mtype);
void controls(controlt * c, u16 avp, char *val, u8 m);
void control16(controlt * c, u16 avp, u16 val, u8 m);
void control32(controlt * c, u16 avp, u32 val, u8 m);
void controlfree(controlt *c);
control_message *parsecontrol(char *buf, int length);
void dump_control_message(control_message *c);
u32 avp_get_32(control_message *c, int id);
u16 avp_get_16(control_message *c, int id);
char *avp_get_s(control_message *c, int id);
void reader_thread(void);
void skip_zlb();
void cm_free(control_message *m);
controlt *ppp_new(u16 session, int protocol);
void ppp_free(controlt *packet);
controlt *ppp_lcp(u16 s, unsigned char type, char identifier);
controlt *ppp_ipcp(u16 s, unsigned char type, char identifier);
void ppp_send(controlt *c);
void ppp_add_16(controlt * c, u16 val);
void ppp_add_32(controlt * c, u32 val);
void ppp_add_s(controlt * c, char *val);
void ppp_lcp_add_option(controlt *c, unsigned char option, unsigned char length, int data);
void dump_ppp_packet(char *packet, int l);
controlt *ppp_pap(u16 s, unsigned char type, char identifier, char *username, char *password);
char *inet_toa(unsigned long addr);
__u16 checksum(unsigned char *addr, int count);
void sigalarm(int junk);
void sigint(int signal);
void clean_shutdown();
void print_report();

int ns = 0, nr = 0;
int udpfd;
int t = 0;
struct sockaddr_in gatewayaddr;
int numsessions = NUM_SESSIONS;
int packet_length = PACKET_LENGTH;
int target_pps = TARGET_PPS;
char *target = TARGET;
char *gwaddr = GWADDR;
int max_packets = MAX_PACKETS;
int ppsend;
int do_init = 1;
char **session_usernames;
char *base_username = "dslloadtest";
char *base_password = "testing";
char *suffix = "@optusnet.com.au";

int main(int argc, char *argv[])
{
	int s;
	unsigned char *packet;

	ss = (sharedt*) mmap(NULL, sizeof(*ss), PROT_READ | PROT_WRITE, MAP_SHARED | MAP_ANONYMOUS, 0, 0);

	// Process Arguments {{{
	while ((s = getopt(argc, argv, "?hs:g:l:p:m:t:nU:P:")) > 0)
	{
		switch (s)
		{
			case 's' :
				numsessions = atoi(optarg);
				if (numsessions <= 0)
				{
					printf("You must have at least 1 session\n");
					return -1;
				}
				break;
			case 'l' :
				packet_length = atoi(optarg);
				if (packet_length < 64)
				{
					printf("You must have at least 64 byte packets\n");
					return -1;
				}
				break;
			case 'n' :
				do_init = 0;
				break;
			case 'p' :
				target_pps = atoi(optarg);
				break;
			case 'm' :
				max_packets = atoi(optarg);
				if (max_packets < 50)
				{
					printf("You must send at least 50 packets.\n");
					return -1;
				}
				break;
			case 't' :
				target = strdup(optarg);
				break;
			case 'g' :
				gwaddr = strdup(optarg);
				break;
			case 'U' :
				base_username = strdup(optarg);
				break;
			case 'P' :
				base_password = strdup(optarg);
				break;
			case 'h' :
			case '?' :
				printf("Options:\n");
				printf("\t-s number of ss->sessions\n");
				printf("\t-l packet length\n");
				printf("\t-p target pps\n");
				printf("\t-m maximum number of packets\n");
				printf("\t-t target IP address\n");
				printf("\t-g gateway IP address\n");
				printf("\t-U username (or base if multiple)\n");
				printf("\t-P password\n");
				return(0);
				break;
		}
	}
	if (target_pps)
		ppsend = target_pps / 50;
	else
		ppsend = 0;

	packet = calloc(4096, 1);

	memset(ss->sessions, 0, sizeof(ss->sessions));

	if (do_init)
		printf("Creating %d ss->sessions to %s\n", numsessions, gwaddr);
	printf("Targeting %d packets per second\n", target_pps);
	if (max_packets) printf("Sending a maximum of %d packets\n", max_packets);
	printf("Sending packets to %s\n", target);
	printf("Sending %d byte packets\n", packet_length);

	session_usernames = (char **)calloc(sizeof(char *), numsessions);
	if (numsessions > 1)
	{
		int sul = strlen(base_username) + 10;
		int i;

		for (i = 0; i < numsessions; i++)
		{
			session_usernames[i] = (char *)calloc(sul, 1);
			snprintf(session_usernames[i], sul, "%s%d", base_username, i+1);
		}
	}
	else
	{
		session_usernames[0] = strdup(base_username);
	}
	// }}}

	// Create socket/*{{{*/
	{
		int on = 1;
		struct sockaddr_in addr;

		memset(&addr, 0, sizeof(addr));
		addr.sin_family = AF_INET;
		addr.sin_port = htons(38001);

		udpfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
		if (udpfd <= 0)
		{
			perror("socket");
			return -1;
		}

		setsockopt(udpfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
		if (bind(udpfd, (void *) &addr, sizeof(addr)) < 0)
		{
			perror("bind");
			return -1;
		}

		printf("Bound to port %d\n", htons(addr.sin_port));
	}/*}}}*/

	memset(&gatewayaddr, 0, sizeof(gatewayaddr));
	gatewayaddr.sin_family = AF_INET;
	gatewayaddr.sin_port = htons(1701);
	inet_aton(gwaddr, &gatewayaddr.sin_addr);

	// Create tunnel/*{{{*/
	if (do_init) {
		controlt *c;
		control_message *r;

		c = controlnew(1); // SCCRQ
		controls(c, 7, "loadtest", 0); // Tunnel Hostname
		controls(c, 8, "OIE", 0); // Vendor Name
		control16(c, 9, 1, 0); // Assigned Tunnel ID
		control16(c, 2, 256, 0); // Version 1.0
		control16(c, 3, 1, 0); // Framing (Async)
		control16(c, 4, 1, 0); // Bearer (Digital)
		control16(c, 10, 20, 0); // Receive Window Size
		controlsend(c, 0, 0);
		controlfree(c);

		// Receive reply/*{{{*/
		{
			struct sockaddr_in addr;
			int l;
			socklen_t alen = sizeof(addr);

			l = recvfrom(udpfd, packet, 4096, 0, (void *) &addr, &alen);
			if (l < 0)
			{
				printf("Error creating tunnel: %s\n", strerror(errno));
				return -1;
			}
			printf("Received ");
			r = parsecontrol((char *) packet, l);
			if (!r->first)
			{
				printf("Invalid packet.. no first avp\n");
				return -1;
			}

			printf("Assigned tunnel: %d\n", t = avp_get_16(r, 9));
			cm_free(r);

			c = controlnew(3); // SCCCN
			controlsend(c, t, 0);
			controlfree(c);
			skip_zlb();
		}/*}}}*/
	}/*}}}*/


	// Create ss->sessions/*{{{*/
	if (do_init)
	{
		for (s = 1; s <= numsessions; s++)
		{
			controlt *c;

			c = controlnew(10); // ICRQ
			controls(c, 21, "12356", 0); // Called Number
			controls(c, 22, "000", 0); // Calling Number
			control16(c, 14, s, 0); // Assigned Session ID
			controlsend(c, t, 0);
			controlfree(c);
			usleep(15000); // 15 ms
		}
	}
	printf("All session create requests sent...\n");/*}}}*/

	if ( fork() == 0) {
		reader_thread();
		exit(0);
	}

	{
		char tmp[512];
		fprintf(stderr, "Press enter to begin sending traffic\n");
		fgets(tmp, 512, stdin);
	}

	fprintf(stderr, "Beginning sending traffic through %d ss->sessions\n", ss->active_sessions);
	printf("	TS: Total Packets Sent\n");
	printf("	TL: Total Packets Lost\n");
	printf("	PL: Packet Loss\n");
	printf("	SS: Send Speed\n");
	printf("	RS: Receive Speed\n");
	printf("	SP: Packets/Second Sent\n");
	printf("	RP: Packets/Second Received\n");
	printf("	NS: Number of active ss->sessions\n");

	signal(SIGALRM, sigalarm);
	signal(SIGINT, sigint);
	alarm(1);

	// Traffic generation loop {{{
	{
		struct sockaddr_in to;
		struct iphdr *iph;
		struct udphdr *udph;
		char *data;
		int len = 0;
		unsigned int seq = 0;
		controlt *c;

		// Get address
		memset(&to, 0, sizeof(struct sockaddr_in));
		to.sin_family = AF_INET;
		inet_aton(target, &to.sin_addr);

		c = ppp_new(1, PPPIP);

		iph = (struct iphdr *)(c->buf + c->length);
		udph = (struct udphdr *)(c->buf + c->length + sizeof(struct iphdr));
		data = (char *)(c->buf + c->length + sizeof(struct iphdr) + sizeof(struct udphdr));
		len = sizeof(struct iphdr) + sizeof(struct udphdr);
		c->length += len;

		//IP
		c->length += sizeof(struct iphdr);
		iph->tos = 0;
		iph->id = ntohs(1);
		iph->frag_off = ntohs(1 << 14);
		iph->ttl = 30;
		iph->check = 0;
		iph->version = 4;
		iph->ihl = 5;
		iph->protocol = 17;
		memcpy(&iph->daddr, &to.sin_addr, sizeof(iph->daddr));

		// UDP
		udph->source = ntohs(39999);
		udph->dest = ntohs(39000);
		udph->check = 0;

		// Data
		memset(data, 64, 1500);

		udph->len = ntohs(sizeof(struct udphdr) + packet_length);
		iph->tot_len = ntohs(len + packet_length);
		c->length += packet_length;

		while (!ss->quitit && ss->active_sessions)
		{
			int i;
			for (i = 1; i <= numsessions && !ss->quitit; i++)
			{
				// Skip ss->sessions that aren't active yet
				if (!ss->sessions[i].open || ss->sessions[i].ppp_state != 2)
					continue;

				*(u16 *)(c->buf + 4) = htons(ss->sessions[i].remote_session); // Session ID
				iph->saddr = ss->sessions[i].addr;
				iph->check = 0;
				iph->check = ntohs(checksum((unsigned char *)iph, sizeof(struct iphdr)));

				*((unsigned int *) data) = seq++;
				ppp_send(c);

				ss->send_count++;
				ss->spkt++;
				ss->sbytes += c->length;

				if (ppsend && ss->send_count % ppsend == 0)
				{
					struct timespec req;
					req.tv_sec = 0;
					req.tv_nsec = 5 * 1000 * 1000;
					nanosleep(&req, NULL);
				}

				if (max_packets && ss->send_count >= max_packets)
					ss->quitit++;
			}
		}

		c->length -= packet_length;

	}/*}}}*/

	clean_shutdown();
	print_report();

	close(udpfd);
	return 0;
}

void print_report()
{
	float loss;

	loss = 100 - (((ss->recv_count * 1.0) / (ss->send_count * 1.0)) * 100.0);

	printf("\n");
	printf("Total Packets Sent:	%llu\n", ss->send_count);
	printf("Total Packets Received:	%llu\n", ss->recv_count);
	printf("Overall Packet Loss:	%0.2f%%", loss);
	printf("\n");
}

void clean_shutdown()/*{{{*/
{
	int i;
	for (i = 0; i < numsessions; i++)
	{
		// Close Session
		controlt *c;

		if (!ss->sessions[i].open) continue;
		c = controlnew(14); // CDN
		control16(c, 14, i, 0); // Assigned Session ID
		control16(c, 1, 1, 0); // Result Code
		controlsend(c, t, ss->sessions[i].remote_session);
		controlfree(c);
	}

	// Close Tunnel
	{
		controlt *c;

		c = controlnew(4); // StopCCN
		control16(c, 9, 1, 0); // Assigned Tunnel ID
		control16(c, 1, 1, 0); // Result Code
		controlsend(c, t, 0);
		controlfree(c);
	}
}/*}}}*/

void sigint(int unused __attribute__ ((unused)))
{
	ss->quitit++;
}

void sigalarm(int unused __attribute__ ((unused)))
{
	static unsigned long long last_rpkts[AVG_SIZE], last_spkts[AVG_SIZE];
	static int last = 0, avg_count = 0;
	unsigned int avg_s = 0, avg_r = 0;
	int i;
	float loss;

	last_rpkts[last] = ss->rpkt;
	last_spkts[last] = ss->spkt;
	last = (last + 1) % AVG_SIZE;
	if (avg_count < AVG_SIZE)
		avg_count++;

	for (i = 0; i < avg_count; i++)
	{
		avg_s += last_spkts[i];
		avg_r += last_rpkts[i];
	}
	avg_s /= avg_count;
	avg_r /= avg_count;

	loss = 100 - (((avg_r * 1.0) / (avg_s * 1.0)) * 100.0);
	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",
			ss->send_count, ss->send_count-ss->recv_count, ss->dropped, loss,
			(ss->sbytes/1024.0/1024.0*8), (ss->rbytes/1024.0/1024.0*8),
			ss->active_sessions,
			avg_s, avg_r);

	ss->spkt = ss->rpkt = 0;
	ss->sbytes = ss->rbytes = 0;
	alarm(1);
}

__u16 checksum(unsigned char *addr, int count)
{
	register long sum = 0;

	for (; count > 1; count -= 2)
	{
		sum += ntohs(*(u16 *)addr);
		addr += 2;
	}

	if (count > 0) sum += *(unsigned char *)addr;

	// take only 16 bits out of the 32 bit sum and add up the carries
	if (sum >> 16)
		sum = (sum & 0xFFFF) + (sum >> 16);

	// one's complement the result
	sum = ~sum;

	return ((u16) sum);
}

// Control Stuff {{{
void control16(controlt * c, u16 avp, u16 val, u8 m)
{
	u16 l = (m ? 0x8008 : 0x0008);
	*(u16 *) (c->buf + c->length + 0) = htons(l);
	*(u16 *) (c->buf + c->length + 2) = htons(0);
	*(u16 *) (c->buf + c->length + 4) = htons(avp);
	*(u16 *) (c->buf + c->length + 6) = htons(val);
	c->length += 8;
}

// add an AVP (32 bit)
void control32(controlt * c, u16 avp, u32 val, u8 m)
{
	u16 l = (m ? 0x800A : 0x000A);
	*(u16 *) (c->buf + c->length + 0) = htons(l);
	*(u16 *) (c->buf + c->length + 2) = htons(0);
	*(u16 *) (c->buf + c->length + 4) = htons(avp);
	*(u32 *) (c->buf + c->length + 6) = htonl(val);
	c->length += 10;
}

// add an AVP (32 bit)
void controls(controlt * c, u16 avp, char *val, u8 m)
{
	u16 l = ((m ? 0x8000 : 0) + strlen(val) + 6);
	*(u16 *) (c->buf + c->length + 0) = htons(l);
	*(u16 *) (c->buf + c->length + 2) = htons(0);
	*(u16 *) (c->buf + c->length + 4) = htons(avp);
	memcpy(c->buf + c->length + 6, val, strlen(val));
	c->length += 6 + strlen(val);
}

// new control connection
controlt *controlnew(u16 mtype)
{
	controlt *c;
	c = calloc(sizeof(controlt), 1);
	c->length = 12;
	control16(c, 0, mtype, 1);
	return c;
}

void controlnull(short t)
{
	controlt *c;
	c = calloc(sizeof(controlt), 1);
	c->length = 12;
	controlsend(c, t, 0);
	controlfree(c);
	ns--;
}

// add a control message to a tunnel, and send if within window
void controlsend(controlt * c, short t, short s)
{
	*(u16 *) (c->buf + 0) = htons(0xC802); // flags/ver
	*(u16 *) (c->buf + 2) = htons(c->length); // length
	*(u16 *) (c->buf + 4) = htons(t); // tunnel
	*(u16 *) (c->buf + 6) = htons(s); // session
	*(u16 *) (c->buf + 8) = htons(ns++); // sequence
	*(u16 *) (c->buf + 10) = htons(nr); // sequence
//	printf("Sending ");
//	cm_free(parsecontrol(c->buf, c->length));
	sendto(udpfd, c->buf, c->length, 0, (struct sockaddr *)&gatewayaddr, sizeof(gatewayaddr));
}

void controlfree(controlt *c)
{
	if (!c) return;
	free(c);
}

control_message *parsecontrol(char *buf, int length)
{
	char *p = buf;
	control_message *c;

	c = calloc(sizeof(control_message), 1);
	c->buf = buf;
	c->length = length;

	c->tunnel = ntohs(*(u16 *)(buf + 4));
	c->session = ntohs(*(u16 *)(buf + 6));
	c->ns = ntohs(*(u16 *)(buf + 8));
	c->nr = nr = ntohs(*(u16 *)(buf + 10));
	p += 12;
	while ((p - buf) < length)
	{
		avp *a = calloc(sizeof(avp), 1);
		a->length = ntohs(*(short *)(p)) & 0x3FF;
		a->type = ntohs(*(short *)(p + 4));
		memcpy(a->value, p + 6, a->length - 6);
		if (a->type == 0) c->mtype = ntohs(*(short *)a->value);
		p += a->length;
		if (c->last)
			c->last->next = a;
		else
			c->first = a;
		c->last = a;
	}
	if (c->first)
		dump_control_message(c);
	return c;
}

void dump_control_message(control_message *c)
{
	avp *a;
	printf("Control Message (type=%u s=%u t=%d ns=%d nr=%d)\n", c->mtype, c->session, c->tunnel, c->ns, c->nr);
	for (a = c->first; a; a = a->next)
	{
		printf("	avp: %s, len: %d", attributes[a->type], a->length - 6);
		switch (a->type)
		{
			// Short
			case 6 :
			case 9 :
			case 10 :
			case 39 :
			case 14 : printf(", value: %u\n", ntohs(*(short *)a->value));
				 break;

			// Integer
			case 16 :
			case 17 :
			case 24 :
			case 25 :
			case 38 :
			case 15 : printf(", value: %u\n", ntohl(*(u32 *)a->value));
				  break;

			// String
			case 7 :
			case 21 :
			case 22 :
			case 23 :
			case 37 :
			case 8 : printf(", value: \"%s\"\n", a->value);
				 break;

			case 2 : printf(", value: %d.%d\n", *(char *)a->value, *(char *)a->value + 1);
				 break;
			case 0 : printf(", value: %s\n", mtypes[ntohs(*(short *)a->value)]);
				 break;
			case 19 :
			case 3 : printf(", value: (%d) %s %s\n", ntohl(*(u32 *)a->value),
						 (ntohl(*(u32 *)a->value) & 0x01) ? "synchronous" : "",
						 (ntohl(*(u32 *)a->value) & 0x02) ? "asynchronous" : "");
				 break;
			case 18 :
			case 4 : printf(", value: (%d) %s %s\n", ntohl(*(u32 *)a->value),
						 (ntohl(*(u32 *)a->value) & 0x01) ? "digital" : "",
						 (ntohl(*(u32 *)a->value) & 0x02) ? "analog" : "");
				 break;

			default : printf("\n");
				  break;
		}
	}
	printf("\n");
}

u16 avp_get_16(control_message *c, int id)
{
	avp *a;

	for (a = c->first; a; a = a->next)
		if (a->type == id) return ntohs(*(short *)a->value);
	return 0;
}

u32 avp_get_32(control_message *c, int id)
{
	avp *a;

	for (a = c->first; a; a = a->next)
		if (a->type == id) return ntohl(*(u32 *)a->value);
	return 0;
}

char *avp_get_s(control_message *c, int id)
{
	avp *a;

	for (a = c->first; a; a = a->next)
		if (a->type == id) return (char *)a->value;
	return 0;
}

void cm_free(control_message *m)
{
	avp *a, *n;

	for (a = m->first; a; )
	{
		n = a->next;
		free(a);
		a = n;
	}

	free(m);
}

// }}}

void reader_thread()/*{{{*/
{
	unsigned char *packet;
	unsigned int seq = 0;

	printf("Starting reader thread\n");
	packet = malloc(4096);
	while (!ss->quitit)
	{
		struct sockaddr_in addr;
		socklen_t alen = sizeof(addr);
		control_message *m;
		int l;
		int s;
		int pfc = 0;

//		memset(packet, 0, 4096);
		if ((l = recvfrom(udpfd, packet, 4096, 0, (void *) &addr, &alen)) < 0) break;
		ss->rbytes += l;
		if (!do_init)
		{
			ss->recv_count++;
			ss->rpkt++;
			continue;
		}
		if (l < 12)
		{
			printf("Short packet received: %d bytes\n", l);
		}
		s = ntohs(*(u16 *)(packet + 4));
		if (!s)
		{
			printf("Invalid session ID\n");
			continue;
		}
		if (packet[0] == 0xc8)
		{
			// Control Packet
			printf("Reader Received ");
			m = parsecontrol((char *) packet, l);
			printf("\n");
			s = m->session;

			switch (m->mtype)
			{
				case 4  : printf("StopCCN\n");
					  printf("Killing tunnel %d\n", avp_get_16(m, 9));
					  ss->quitit++;
					  break;
				case 6  : printf("HELLO, sending ZLB ACK\n");
					  controlnull(t);
					  break;
				case 11 :
					{
						controlt *c;

						printf("Received ICRP. Responding with CONFREQ\n");

						ss->sessions[s].remote_session = avp_get_16(m, 14);
						ss->sessions[s].open = 1;
						ss->sessions[s].ppp_state = 1;

						c = controlnew(12); // ICCN
						controlsend(c, t, ss->sessions[s].remote_session);
						controlfree(c);

						c = ppp_lcp(s, CONFREQ, 0);
						ppp_lcp_add_option(c, 1, 2, htons(1500)); // MRU = 1400
						ppp_lcp_add_option(c, 3, 2, htons(0xC023)); // Authentication Protocol - PAP
						ppp_send(c);
						controlfree(c);
						break;
					}
				case 14 : {
						int s;
						printf("CDN\n");
						s = avp_get_16(m, 14);
						printf("Killing session %d\n", s);
						ss->sessions[s].open = 0;
						ss->sessions[s].ppp_state = 0;
						ss->active_sessions--;
					        controlnull(t);
					        break;
					  }

			}
			if (m->mtype == 4)
			{
				printf("StopCCN Received.. Dieing\n");
				ss->quitit++;
				break;
			}
			cm_free(m);
		}
		else
		{
			// Data Packet
			unsigned short protocol = ntohs(*(u16 *)(packet + 6));

			if (protocol == 0xff03)
			{
				pfc = 2;
				packet += 2;
				protocol = ntohs(*(u16 *)(packet + 6));
			}
			if (protocol != PPPIP)
			{
				printf("Received ");
				dump_ppp_packet((char *) (packet + 6), l - 6);
			}

			if (protocol == PPPLCP)
			{
				controlt *r;
				unsigned char ppp_id = *(char *)(packet + 9);

				switch (*(char *)(packet + 8))
				{
					case CONFREQ :
						r = ppp_lcp(s, CONFACK, ppp_id);
						ppp_send(r);
						break;
					case CONFACK :
						r = ppp_pap(s, CONFREQ, 0, session_usernames[s-1], base_password);
						ppp_send(r);
						break;
					case TERMREQ :
						r = ppp_lcp(s, TERMACK, ppp_id);
						ppp_send(r);
						break;
					case ECHOREQ :
						r = ppp_lcp(s, ECHOREP, ppp_id);
						ppp_add_32(r, 0);
						ppp_send(r);
						break;
				}
			}
			else if (protocol == PPPIPCP)
			{
				controlt *r;
				int taddr = 0;
				u32 address = *(u32 *)(packet + 14);

				switch (*(char *)(packet + 8))
				{
					case CONFREQ :
						r = ppp_ipcp(s, CONFREQ, time(NULL) % 255);
						ppp_lcp_add_option(r, 3, 4, htonl(taddr)); // Request 0.0.0.0
						ppp_send(r);
						controlfree(r);
						r = ppp_ipcp(s, CONFACK, time(NULL) % 255);
						ppp_lcp_add_option(r, 3, 4, address); // ACK gateway IP
						ppp_send(r);
						controlfree(r);
						break;
					case CONFNAK :
						// Request whatever address we are given - it's ours
						r = ppp_ipcp(s, CONFREQ, time(NULL) % 255);
						ppp_lcp_add_option(r, 3, 4, address);
						ppp_send(r);
						controlfree(r);
						printf("Session %d: %s\n", s, inet_toa(address));
						ss->sessions[s].ppp_state = 2;
						ss->sessions[s].addr = address;
						ss->active_sessions++;
						break;
					case CONFACK :
						printf("Conf-Ack Received\n");
						break;
					case TERMREQ :
						printf("Term-Req Received\n");
						break;
					case ECHOREQ :
						printf("Echo-Req Received\n");
						break;
					case ECHOREP :
						printf("Echo-Rep Received\n");
						break;
				}
			}
			else if (protocol == PPPPAP)
			{
				if (*(u16 *)(packet + 8) == 3)
				{
					controlt *c;
					printf("Closing Connection\n");

					c = controlnew(14); // CDN
					control16(c, 14, ss->sessions[s].remote_session, 0); // Assigned Session ID
					controlsend(c, t, 0);
					controlfree(c);
					ss->sessions[s].open = 0;
				}
			}
			else if (protocol == PPPIP)
			{
				struct iphdr *iph = (struct iphdr *)(packet + 8);
				char * data = (char*) (packet + 8 + sizeof(struct iphdr) + sizeof(struct udphdr));
				if (!ss->sessions[s].open)
				{
					printf("Packet for closed session %d\n", s);
					continue;
				}

				if (iph->protocol == 17)
				{
					unsigned int iseq;
					ss->recv_count++;
					ss->rpkt++;
					iseq = *((unsigned int *) data);
					if (seq != iseq)
						ss->dropped += (iseq - seq) ;

					seq = iseq + 1; // Next sequence number to expect.
				}
			}
		}
		packet -= pfc;
	}
	free(packet);

	printf("Closing reader thread\n");

}/*}}}*/

void skip_zlb() /*{{{*/
{
	struct sockaddr_in addr;
	socklen_t alen = sizeof(addr);
	char buf[1024];
	int l;
	l = recvfrom(udpfd, buf, 1024, MSG_PEEK, (void *) &addr, &alen);
	if (l < 0)
	{
		printf("recvfrom: %s\n", strerror(errno));
		return;
	}
	if (l <= 12)
	{
		printf("Skipping ZLB (l=%d)\n", l);
		recvfrom(udpfd, buf, 1024, 0, (void *) &addr, &alen);
	}
}
/*}}}*/

// PPP Stuff {{{
controlt *ppp_new(u16 session, int protocol)
{
	controlt *c = calloc(sizeof(controlt), 1);
	*(u16 *)(c->buf + 4) = htons(ss->sessions[session].remote_session); // Tunnel
	*(u16 *)(c->buf + 6) = htons(protocol);
	c->length += 8;

	return c;
}

void ppp_free(controlt *c)
{
	free(c);
}

controlt *ppp_lcp(u16 s, unsigned char type, char identifier)
{
	controlt *c;

	if (!identifier) identifier = ss->sessions[s].ppp_identifier++;
	c = ppp_new(s, PPPLCP);
	*(char *)(c->buf + c->length + 0) = type;
	*(char *)(c->buf + c->length + 1) = identifier;
	*(u16 *)(c->buf + c->length + 2) = ntohs(4);
	c->length += 4;

	return c;
}

controlt *ppp_ipcp(u16 s, unsigned char type, char identifier)
{
	controlt *c;

	if (!identifier) identifier = ss->sessions[s].ppp_identifier++;
	c = ppp_new(s, PPPIPCP);
	*(char *)(c->buf + c->length + 0) = type;
	*(char *)(c->buf + c->length + 1) = identifier;
	*(u16 *)(c->buf + c->length + 2) = ntohs(4);
	c->length += 4;

	return c;
}

controlt *ppp_pap(u16 s, unsigned char type, char identifier, char *username, char *password)
{
	controlt *c;

	if (!identifier) identifier = ss->sessions[s].ppp_identifier++;
	c = ppp_new(s, PPPPAP);
	*(char *)(c->buf + c->length + 0) = type;
	*(char *)(c->buf + c->length + 1) = identifier;
	*(u16 *)(c->buf + c->length + 2) = ntohs(4);
	c->length += 4;

	*(char *)(c->buf + c->length) = strlen(username) + strlen(suffix);
	memcpy((c->buf + c->length + 1), username, strlen(username));
	memcpy((c->buf + c->length + 1 + strlen(username)), suffix, strlen(suffix));
	c->length += strlen(username) + 1 + strlen(suffix);

	*(char *)(c->buf + c->length) = strlen(password);
	memcpy((c->buf + c->length + 1), password, strlen(password));
	c->length += strlen(password) + 1;

	return c;
}

void ppp_send(controlt *c)
{
	*(u16 *)(c->buf + 0) = htons(0x0002); // flags/ver
	*(u16 *)(c->buf + 2) = htons(t); // tunnel
	*(u16 *)(c->buf + 10) = ntohs(c->length - 8);
	if (sendto(udpfd, c->buf, c->length, 0, (struct sockaddr *)&gatewayaddr, sizeof(gatewayaddr)) < 0)
		perror("sendto");
	if (htons(*(u16 *)(c->buf + 6)) != PPPIP)
	{
		printf("PPP Sending ");
		dump_ppp_packet(c->buf + 6, c->length - 6);
	}
}

void ppp_add_16(controlt *c, u16 val)
{
	*(u16 *) (c->buf + c->length) = htons(val);
	c->length += 2;
}

void ppp_add_32(controlt *c, u32 val)
{
	*(u32 *) (c->buf + c->length) = htons(val);
	c->length += 4;
}

void ppp_add_s(controlt *c, char *val)
{
	memcpy(c->buf + c->length, val, strlen(val));
	c->length += strlen(val);
}

void ppp_lcp_add_option(controlt *c, unsigned char option, unsigned char length, int data)
{
	*(char *)(c->buf + c->length + 0) = option;
	*(char *)(c->buf + c->length + 1) = length + 2;
	memcpy(c->buf + c->length + 2, &data, length);
	c->length += 2 + length;
}

void dump_ppp_packet(char *packet, int l)
{
	char *p = packet;
	int protocol ;
	if (*(unsigned char *)p == 0xff) p += 2;
	protocol = ntohs(*(u16 *)(p));
	printf("PPP Packet\n");
	switch (protocol)
	{
		case PPPCCP : printf("	Protocol: PPPCCP\n"); break;
	}
	if (protocol == PPPLCP)
	{
		printf("	Protocol: PPPLCP\n");
		printf("	LCP Code: %s\n", lcp_codes[*(u8 *)(p + 2)]);
	}
	else if (protocol == PPPPAP)
	{
		printf("	Protocol: PPPPAP\n");
		if (*(char *)(p + 2) == 2)
		{
				printf("	Authentication accepted\n");
		}
		else if (*(char *)(p + 2) == 3)
		{
				printf("	Authentication denied\n");
		}
	}
	else if (protocol == PPPIPCP)
	{
		printf("	Protocol: PPPIPCP\n");
		printf("	IPCP Code: %s\n", lcp_codes[*(u8 *)(p + 2)]);
		printf("	Address: %s\n", inet_toa(*(u32 *)(p + 8)));
	}
	else if (protocol == PPPIP)
	{
		struct iphdr *iph;
		struct protoent *pr;

		iph = (struct iphdr *)(p + 2);

		printf("	Protocol: PPPIP\n");
		printf("	Length: %d\n", l);
		printf("	IP Version: %d\n", iph->version);
		if (iph->version != 4) return;
		pr = getprotobynumber(iph->protocol);
		printf("	IP Header Length: %d\n", iph->ihl);
		printf("	IP TTL: %d\n", iph->ttl);
		printf("	IP Protocol: %s (%d)\n", (pr ? pr->p_name : "unknown"), iph->protocol);
		printf("	IP Checksum: %x\n", ntohs(iph->check));
	}
	else
	{
		printf("	Protocol: unknown 0x%x\n", protocol);
	}
	printf("\n");
}

char *inet_toa(unsigned long addr)
{
	struct in_addr in;
	memcpy(&in, &addr, sizeof(unsigned long));
	return inet_ntoa(in);
}

// }}}