#ifdef USE_L2TP
/* l2tp_tunnel.cc
 */

#include "l2tpd.h"
#include "l2tp_tunnel.h"
#include "packet.h"

#include "l2tp_linux.h"
#include "aps_if.h"

#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>

#include "pretty_dump.h"
#include "md5.h"

unsigned num_est_tunnels;
unsigned num_tunnels;

unsigned num_est_sessions;
unsigned num_sessions;

l2tp_packet_t pkt;

l2tp_tunnel_t::l2tp_tunnel_t(l2tp_peer_t *_peer, u16 tunnel)
{
	fprintf(stderr, "new tunnel: %5d\n", tunnel);

	tunnel_state = L2TP_Tunnel_Idle;
	peer = _peer;
	memset(sessions, 0, sizeof(sessions));
	tunnel_id = peer_tunnel_id = 0;
	tunnel_Ns = tunnel_Nr = 0;
	call_serial_number = 0;
	need_zlb = 0;
	free_session_id = 0;
	l2tp_tunnel_hello::hello_interval = 60 * 100;
	if (!tunnel)
		tunnel = peer->alloc_tunnel_id(this);
	tunnel_id = tunnel;
}

l2tp_tunnel_t::~l2tp_tunnel_t()
{
	peer->remove_tunnel_id(tunnel_id, this);
}

void l2tp_tunnel_t::dump_sessions(ctrlfd_t *cfd, int verbose)
{
	static const char *states[] = {
		"idle", "wait-ctl-reply", "wait-ctl-conn", "established",
	};
	int i;
	cfd->printf("\ttunnel peer(%5d) local(%5d) state(%s)%c", peer_tunnel_id,
			tunnel_id, states[tunnel_state], verbose ? ' ' : '\n');
	if (verbose)
		cfd->printf("Ns(%u) Nr(%u)\n", tunnel_Ns, tunnel_Nr);

	for (i=0; i<65536; i++) {
		l2tp_session_t *session = sessions[i];
		if (session)
			session->dump_session(cfd);
	}
}

void l2tp_tunnel_t::clean_up(void)
{
	int i;

fprintf(stderr, "l2tp_tunnel_t: clean_up!\n");
	stop_hello_timer();

	for (i=0; i<65536; i++) {
		if (sessions[i])
			delete sessions[i];
		sessions[i] = NULL;
	}

	peer_tunnel_id = 0;
	tunnel_state = L2TP_Tunnel_Idle;
	tunnel_Ns = 0;
	tunnel_Nr = 0;
	need_zlb = 0;
	free_session_id = 1;
}

int l2tp_tunnel_t::alloc_session_id(void)
{
	u16 id = free_session_id;

	for (int i=0; i<65536; i++) {
		if (!id)
			id = 1;
		if (!sessions[id]) {
			free_session_id = id + 1;
			return id;
		}
		id++;
	}

	return -1;
}

void l2tp_tunnel_t::tx_timed_out(void)
{
	clean_up();
}


static const struct AVPInfo {
	signed char	min_len;	/* bytes or -1 if no min, 0 = unknown */
	unsigned char	name[31];
} RecognizedAVPs[MaxAVPs] = {
	{ 2, "MessageType" },
	{ 2, "ResultCode" },
	{ 2, "ProtocolVersion" },
	{ 4, "FramingCapabilities" },
	{ 4, "BearerCapabilities" },
	{ 8, "TieBreaker" },
	{ 2, "FirmwareRevision" },
	{ 1, "HostName" },
	{ -1, "VendorName" },
	{ 2, "AssignedTunnelID" },
	{ 2, "ReceiveWindowSize" },
	{ -1, "Challenge" },
	{ 3, "CauseCode" },
	{ 16, "ChallengeResponse" },
	{ 2, "AssignedSessionID" },
	{ 4, "CallSerialNumber" },
	{ 4, "MinimumBPS" },
	{ 4, "MaximumBPS" },
	{ 4, "BearerType" },
	{ 4, "FramingType" },
	{ 0, "Reserved20" },
	{ -1, "CalledNumber" },
	{ -1, "CallingNumber" },
	{ 0, "SubAddress" },
	{ 4, "ConnectSpeed" },
	{ 4, "PhysicalChannelID" },
	{ 0, "InitialReceivedLCPConfReq" },
	{ 0, "LastSentLCPConfReq" },
	{ 0, "LastReceivedLCPConfReq" },
	{ 2, "ProxyAuthenType" },
	{ -1, "ProxyAuthenName" },
	{ 0, "ProxyAuthenChallenge" },
	{ 2, "ProxyAuthenID" },
	{ 0, "ProxyResponse" },
	{ 0, "CallErrors" },
	{ 4, "ACCM" },
	{ 0, "RandomVector" },
	{ 0, "PrivateGroupID" },
	{ 4, "RxConnectSpeed" },
	{ 0, "SequencingRequired" },
};

int l2tp_packet_t::decode_avp(u16 vendor, u16 type, u16 length, u16 *ptr)
{
	/* we only support bog standard AVPs */
	if (vendor)
		return -1;

	if (type >= MaxAVPs)
		return -1;
	if (!RecognizedAVPs[type].min_len ||
	    length < RecognizedAVPs[type].min_len)
		return -1;
	AVPs[type] = ptr;
	nr_avps ++;
	return 0;
}

int l2tp_packet_t::parse_packet(u16 *data, int size)
{
	reset();
	//pretty_dump(size, (unsigned char *)data);
	this->raw_packet = data;
	this->raw_length = size;

	Flags = ntohs(*data++);
	if (Flags & L2TPF_L)
		Length = ntohs(*data++);
	else
		Length = size;

	if (Length > size)
		return -1;
	if (size < Length) {
		fprintf(stderr, "parse_packet: clipping size(%d) from Length(%d)\n", size, Length);
		size = Length;
	}

	Tunnel = ntohs(*data++);
	Session = ntohs(*data++);

	if (Flags & L2TPF_S) {
		Ns = ntohs(*data++);
		Nr = ntohs(*data++);
	} else {
		Ns = -1;
		Nr = -1;
	}

	if (Flags & L2TPF_O) {
		u16 offset = *data++;
		data = (u16 *)((char *)data + offset);
	}

	if (((char *)data - (char *)raw_packet) > (long)size) {
		fprintf(stderr, "packet too small (%ld) > size(%ld)",
			(long)((char *)data - (char *)raw_packet), (long)size);
		return -1;
	}

	size -= (char *)data - (char *)raw_packet;
	Length -= (char *)data - (char *)raw_packet;

	DataPtr = data;

	/* that's it for a data packet */
	if (!(Flags & L2TPF_T))
		return 0;

	/* now we get to parse all the avps */
	while (size >= 6) {
		u16 avp_lengthbits = ntohs(*data++);
		u16 avp_vendor = ntohs(*data++);
		u16 avp_type = ntohs(*data++);
		int avp_length = avp_lengthbits & L2TP_AVP_LENGTH;
		u16 *avp_value;

		if (avp_length < 6 || avp_length > size) {
			fprintf(stderr, "avp too short/long (%u vs %u).\n",
				avp_length, size);
			return -1;
		}
		size -= avp_length;
		if (avp_length > 6)
			avp_value = data;
		else
			avp_value = NULL;
		avp_length -= 6;
		data = (u16 *)((char *)data + avp_length);

		if (avp_lengthbits & L2TP_AVP_RESERVED) {
			fprintf(stderr, "reserved bits set, ignoring.\n");
			continue;	/* ignore as per RFC 2661 pg 13 */
		}
		if (avp_lengthbits & L2TP_AVP_H) {
			fprintf(stderr, "FIXME: hidden\n");
			return -1;
		}
		if (decode_avp(avp_vendor, avp_type, avp_length, avp_value)) {
			fprintf(stderr, "decode_avp failed [%d/%d/%s]\n",
				avp_vendor, avp_type,
				(avp_type >= MaxAVPs) ? NULL : 
					RecognizedAVPs[avp_type].name);
			if (avp_lengthbits & L2TP_AVP_M) {
				fprintf(stderr, "mandatory avp decode failed 0x%04x %d 0x%04x\n", avp_vendor, avp_type, avp_length);
				return -1;
			}
		}
	}

	if (size)
		fprintf(stderr, "parse_packet: %d bytes left\n", size);

	return 0;
}

int l2tp_packet_t::avp_length(int avp)
{
	if (AVPs[avp]) {
		u16 val = ntohs(AVPs[avp][-3]);
		val &= L2TP_AVP_LENGTH;
		if (val >= 6)
			val -= 6;
		else
			val = 0;
		return val;
	}
	return 0;
}

int l2tp_tunnel_t::build_challenge_rsp(u16 *where, unsigned char msgtype)
{
	int len = pkt.avp_length(AVP_Challenge);
	if (len < 1)
		return 0;

	fprintf(stderr, "ChallengeRsp: chal %d\n", len);
	MD5_CTX md5;

	MD5Init(&md5);
	MD5Update(&md5, &msgtype, 1);
	MD5Update(&md5, (unsigned char *)peer->m_secret, peer->m_secret_len);
	MD5Update(&md5, (unsigned char *)pkt.AVPs[AVP_Challenge], len);
	MD5Final(&md5);
	memcpy(where, md5.digest, 16);

	return 0x16;
}

int l2tp_packet_t::is_avp_mandatory(int avp)
{
	if (AVPs[avp]) {
		u16 val = ntohs(AVPs[avp][-3]);
		fprintf(stderr, "is_mandatory: 0x%04x\n", val);
		return !!(val & L2TP_AVP_M);
	}
	return 0;
}

void l2tp_tunnel_t::tunnel_abort(const char *msg)
{
	fprintf(stderr, "tunnel_abort(%s)\n", msg);
	/* FIXME */
}

void l2tp_tunnel_t::send_ZLB(void)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		0,	// session_id
		0,	// session_id
		0,	// Ns
		0,	// Nr
	};

	need_zlb = 0;
	send_pkt(data, sizeof(data), 0);
}

void l2tp_tunnel_t::send_SCCRQ(void)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		htons(peer_tunnel_id),
		0,	// session_id
		0,	// Ns
		htons(1),	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType), htons(L2TP_CMSG_TYPE_SCCRQ),

		// AssignedTunnelID AVP
		htons(0x8008), 0x0000, htons(AVP_AssignedTunnelID), htons(tunnel_id),
		// ProtocolVersion AVP -- 1.0
		htons(0x8008), 0x0000, htons(AVP_ProtocolVersion), htons(0x0100),
		// HostName AVP -- "bcrl"
		htons(0x800a), 0x0000, htons(AVP_HostName), htons(0x4243), htons(0x524c),

		// FramingCapabilities AVP
		htons(0x800a), 0x0000, htons(AVP_FramingCapabilities), 0, htons(3),
	};

	send_pkt(data, sizeof(data));
}

void l2tp_tunnel_t::send_SCCRP(void)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		0, //htons(peer_tunnel_id),
		0,	// session_id
		0,	// Ns
		htons(1),	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType), htons(L2TP_CMSG_TYPE_SCCRP),
		// AssignedTunnelID AVP
		htons(0x8008), 0x0000, htons(AVP_AssignedTunnelID), htons(tunnel_id),

		// FramingCapabilities AVP -- set len to c to make tcpdump crash
		htons(0x800a), 0x0000, htons(AVP_FramingCapabilities), htons(0x0000), htons(0x0003),

		// ProtocolVersion AVP
		htons(0x8008), 0x0000, htons(AVP_ProtocolVersion), htons(0x0100),
		// HostName AVP
		htons(0x800a), 0x0000, htons(AVP_HostName), htons(0x6263), htons(0x726c),
		// Optional
		htons(0x8016), 0x0000, htons(AVP_ChallengeResponse),
		0, 0, 0, 0, 0, 0, 0, 0,
	};

	int adj = build_challenge_rsp(&data[31], L2TP_CMSG_TYPE_SCCRP);
	send_pkt(data, sizeof(data) - 0x16 + adj);
}

void l2tp_tunnel_t::send_CDN(u16 session_id, u16 result)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		htons(peer_tunnel_id),
		htons(session_id),
		0,	// Ns
		0,	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType),
		htons(L2TP_CMSG_TYPE_CDN),

		// AssignedSessionID AVP
		htons(0x8008), 0x0000, htons(AVP_AssignedSessionID),
		htons(session_id),

		// ResultCode AVP
		htons(0x8008), 0x0000, htons(AVP_ResultCode),
		htons(result),

	};

	send_pkt(data, sizeof(data));
}

void l2tp_tunnel_t::send_SCCCN(void)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		htons(peer_tunnel_id),
		0,	// session_id
		0,	// Ns
		0,	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType),
		htons(L2TP_CMSG_TYPE_SCCCN),

		// Optional
		htons(0x8016), 0x0000, htons(AVP_ChallengeResponse),
		0, 0, 0, 0, 0, 0, 0, 0,
	};

	int adj = build_challenge_rsp(&data[13], L2TP_CMSG_TYPE_SCCCN);
	send_pkt(data, sizeof(data) - 0x16 + adj);
}

void l2tp_tunnel_t::send_StopCCN(void)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		htons(peer_tunnel_id),
		0,	// session_id
		0,	// Ns
		htons(1),	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType), htons(L2TP_CMSG_TYPE_StopCCN),
		// FramingCapabilities AVP -- set len to c to make tcpdump crash
		htons(0x800a), 0x0000, htons(AVP_FramingCapabilities), htons(0x0000), htons(0x0003),
		// AssignedTunnelID AVP
		htons(0x8008), 0x0000, htons(AVP_AssignedTunnelID), htons(tunnel_id),
		// HostName AVP
		htons(0x800a), 0x0000, htons(AVP_HostName), htons(0x6263), htons(0x726c),
		// ProtocolVersion AVP
		htons(0x8008), 0x0000, htons(AVP_ProtocolVersion), htons(0x0100),

	};

	send_pkt(data, sizeof(data));
}

void l2tp_tunnel_t::send_HELLO(void)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		htons(peer_tunnel_id),
		0,	// session_id
		0,	// Ns
		htons(1),	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType),
		htons(L2TP_CMSG_TYPE_HELLO),
	};

	send_pkt(data, sizeof(data));
}

void l2tp_tunnel_t::send_pkt(u16 *data, int len, int incr_seq)
{
	data[2] = htons(peer_tunnel_id);
	data[1] = htons(len);
	data[4] = htons(tunnel_Ns);
	data[5] = htons(tunnel_Nr);
	tunnel_Ns += incr_seq;
	need_zlb = 0;

	if (debug) {
		fprintf(stderr, "l2tp_tunnel_t::send_pkt\n");
		pretty_dump(len, (unsigned char *)data);
	}

	int ret = write(peer->udp_fd, data, len);
	if (ret <= 0) {
		fprintf(stderr, "send_pkt: write(%d) = %d, errno=%d\n", len, ret, errno);
	}

	if (incr_seq) {
		CPppPacket *pkt = new CPppPacket((u8 *)data, len);
		tx_append(pkt);
	}
}

void l2tp_tunnel_t::tx_retransmit(CPppPacket *pkt)
{
	u16 *data = (u16 *)pkt->m_start;
	data[5] = htons(tunnel_Nr); // update ack sequence number in the packet

	if (debug)
		fprintf(stderr, "tx_retransmit Ns=%d, Nr=%d\n",
			ntohs(data[4]), ntohs(data[5]));

	int ret = write(peer->udp_fd, pkt->m_start, pkt->GetLength());
	if (ret <= 0) {
		fprintf(stderr, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
		fprintf(stderr, "tx_retransmit: write(%d) = %d, errno=%d\n",
			pkt->GetLength(), ret, errno);
	}
}

void l2tp_tunnel_t::handle_SCCRQ(void)
{
	if (!pkt.AVPs[AVP_AssignedTunnelID]) {
		tunnel_abort("sccrq: no tunnel id");
		return;
	}

	u16 id = ntohs(*pkt.AVPs[AVP_AssignedTunnelID]);
	if (!id) {
		tunnel_abort("sccrq: assigned tunnel id is 0");
		return;
	}

	if (id && peer_tunnel_id != id) {
		fprintf(stderr, "peer_tunnel_id changed? %d vs %d\n", peer_tunnel_id, id);
		peer_tunnel_id = id;
		struct sockaddr_l2tp sl;
		memset(&sl, 0, sizeof(sl));
		sl.sl_tunnel = tunnel_id;
		sl.sl_peer_tunnel = peer_tunnel_id;
		if (connect(peer->l2tp_fd, (struct sockaddr *)&sl, sizeof(sl)))
			perror("SCCRQ: tunnel connect()");
	}

	switch (tunnel_state) {
	case L2TP_Tunnel_Idle:
		send_SCCRP();
		tunnel_state = L2TP_Tunnel_WaitCtlConn;
		break;
	case L2TP_Tunnel_Established:
		num_est_tunnels--;
	case L2TP_Tunnel_WaitCtlConn:
		send_StopCCN();
		tunnel_state = L2TP_Tunnel_Idle;
		clean_up();
		break;
	case L2TP_Tunnel_WaitCtlReply:
	default:
		fprintf(stderr, "handle_SCCRQ: unknown state\n");
	}
}

void l2tp_tunnel_t::handle_SCCRP(void)
{
	int id = -1;
	if (pkt.AVPs[AVP_AssignedTunnelID])
		id = ntohs(*pkt.AVPs[AVP_AssignedTunnelID]);

	switch (tunnel_state) {
	case L2TP_Tunnel_Established:
		num_est_tunnels--;
	case L2TP_Tunnel_Idle:
	case L2TP_Tunnel_WaitCtlConn:
		send_StopCCN();
		tunnel_state = L2TP_Tunnel_Idle;
		clean_up();
		break;
	case L2TP_Tunnel_WaitCtlReply:
		// If we've opened a tunnel and the other end has responded 
		// to our request, this is where we learn their tunnel id.
		if (!peer_tunnel_id) {
			if (id <= 0) {
				tunnel_abort("SCCRP: no tunnel id");
				return;
			}
			peer_tunnel_id = id;
		}
		num_est_tunnels++;
		tunnel_state = L2TP_Tunnel_Established;
		send_SCCCN();
		tunnel_up();
		break;
	default:
		fprintf(stderr, "handle_SCCRP: unknown state\n");
	}
}

void l2tp_tunnel_hello::TimerExpired(void)
{
	send_HELLO();
	Start(hello_interval);
}

void l2tp_tunnel_hello::start_hello_timer(void)
{
	Start(hello_interval);
}

void l2tp_tunnel_t::tunnel_up(void)
{
	start_hello_timer();

	// Bring up any waiting sessions.
	for (int i=0; i<65536; i++) {
		l2tp_session_t *session = sessions[i];
		if (session)
			session->tunnel_up();
	}
}

void l2tp_tunnel_t::handle_SCCCN(void)
{
	switch (tunnel_state) {
	case L2TP_Tunnel_Idle:
		clean_up();
		break;
	case L2TP_Tunnel_Established:
		num_est_tunnels--;
	case L2TP_Tunnel_WaitCtlReply:
		send_StopCCN();
		tunnel_state = L2TP_Tunnel_Idle;
		clean_up();
		break;
	case L2TP_Tunnel_WaitCtlConn:
		num_est_tunnels++;
		tunnel_state = L2TP_Tunnel_Established;
		tunnel_up();
		break;
	default:
		fprintf(stderr, "handle_SCCCN: unknown state\n");
	}
}

void l2tp_tunnel_t::handle_StopCCN(void)
{
	tunnel_state = L2TP_Tunnel_Idle;
	send_ZLB();
	clean_up();
}

void l2tp_tunnel_t::handle_HELLO(void)
{
	// implicit send_ZLB();	// or transmit another message
	need_zlb = 1;
}

void l2tp_tunnel_t::bring_up(void)
{
	switch (tunnel_state) {
	case L2TP_Tunnel_Idle:
		// Bring up the control connection.
		tunnel_state = L2TP_Tunnel_WaitCtlReply;
		send_SCCRQ();
		break;
	default:
		break;
	}
}

void l2tp_tunnel_t::open_session(ctrlfd_t *cfd, const char *site)
{
	bring_up();
	new l2tp_session_t(this, cfd, site);
}

//////////////////////////////////////////////////////////
//	Incoming call handling
//////////////////////////////////////////////////////////

void l2tp_tunnel_t::send_ICRQ(u16 session_id)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		htons(peer_tunnel_id),
		htons(0),	// session_id
		0,	// Ns
		0,	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType),
		htons(L2TP_CMSG_TYPE_ICRQ),

		// CallSerialNumber AVP
		htons(0x800a), 0x0000, htons(AVP_CallSerialNumber),
		htons(call_serial_number >> 16), htons(call_serial_number),

		// AssignedSessionID AVP
		htons(0x8008), 0x0000, htons(AVP_AssignedSessionID),
		htons(session_id),
	};

	call_serial_number ++;
	send_pkt(data, sizeof(data));
}

void l2tp_tunnel_t::send_ICRP(u16 peer_session_id, u16 session_id)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		htons(peer_tunnel_id),
		htons(peer_session_id),
		0,	// Ns
		0,	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType),
		htons(L2TP_CMSG_TYPE_ICRP),

		// AssignedSessionID AVP
		htons(0x8008), 0x0000, htons(AVP_AssignedSessionID),
		htons(session_id),
	};

	send_pkt(data, sizeof(data));
}

void l2tp_tunnel_t::send_ICCN(u16 session_id, u32 txspeed, u32 framing)
{
	u16	data[] = {
		htons(0xc802),
		0,	// len
		htons(peer_tunnel_id),
		htons(session_id),	// session_id
		0,	// Ns
		0,	// Nr

		// MessageType AVP
		htons(0x8008), 0x0000, htons(AVP_MessageType),
		htons(L2TP_CMSG_TYPE_ICCN),

		// FramingType AVP
		htons(0x800a), 0x0000, htons(AVP_FramingType),
		htons(framing >> 16), htons(framing),

		// ConnectSpeed AVP
		htons(0x800a), 0x0000, htons(AVP_ConnectSpeed),
		htons(txspeed >> 16), htons(txspeed),
	};

	send_pkt(data, sizeof(data));
}

void l2tp_tunnel_t::handle_ICRQ(void)
{
	if (!pkt.AVPs[AVP_AssignedSessionID]) {
		fprintf(stderr, "handle_ICRQ: no AssignedSessionID\n");
		return;
	}
	if (!pkt.AVPs[AVP_CallSerialNumber]) {
		fprintf(stderr, "handle_ICRQ: no CallSerialNumber\n");
		return;
	}

	u16 peer_id = ntohs(*pkt.AVPs[AVP_AssignedSessionID]);
	l2tp_session_t *session = sessions[peer_id];
	if (!session)
		session = new l2tp_session_t(this, peer_id, pkt.Session);

	session->handle_ICRQ(&pkt);
}

void l2tp_tunnel_t::handle_ICRP(void)
{
	if (!pkt.AVPs[AVP_AssignedSessionID]) {
		fprintf(stderr, "handle_ICRP: no AssignedSessionID\n");
		return;
	}

	u16 peer_id = ntohs(*pkt.AVPs[AVP_AssignedSessionID]);
	l2tp_session_t *session = sessions[pkt.Session];
	if (!session) {
		session = new l2tp_session_t(this, peer_id, pkt.Session);
		fprintf(stderr, "ICRP: made new session id %d peer %d\n", session->session_id, peer_id);
	}

	session->handle_ICRP(&pkt);
}

void l2tp_tunnel_t::handle_CDN(void)
{
	if (!pkt.AVPs[AVP_AssignedSessionID]) {
		fprintf(stderr, "handle_CDN: no AssignedSessionID\n");
		return;
	}

	u16 session_id = ntohs(*pkt.AVPs[AVP_AssignedSessionID]);
	l2tp_session_t *session = sessions[session_id];
	if (session)
		session->handle_CDN(&pkt);
}

void l2tp_tunnel_t::handle_ICCN(void)
{
	l2tp_session_t *session = sessions[pkt.Session];
	if (!session)
		session = new l2tp_session_t(this, 0, pkt.Session);

	session->handle_ICCN(&pkt);
}

//////////////////////////////////////////////////////////

typedef void (l2tp_tunnel_t::*l2tp_handle_packet_t)(void);
#define MAX_l2tp_handle_packet_t	17
static l2tp_handle_packet_t packet_t_array[MAX_l2tp_handle_packet_t] = {
	NULL,					// reserved
	&l2tp_tunnel_t::handle_SCCRQ,
	&l2tp_tunnel_t::handle_SCCRP,
	&l2tp_tunnel_t::handle_SCCCN,
	&l2tp_tunnel_t::handle_StopCCN,
	NULL,	// 5 - reserved
	&l2tp_tunnel_t::handle_HELLO,
	NULL,	// &l2tp_tunnel_t::handle_OCRQ,
	NULL,	// &l2tp_tunnel_t::handle_OCRP,
	NULL,	// &l2tp_tunnel_t::handle_OCCN,
	&l2tp_tunnel_t::handle_ICRQ,
	&l2tp_tunnel_t::handle_ICRP,
	&l2tp_tunnel_t::handle_ICCN,
	NULL,	// 13 - reserved
	&l2tp_tunnel_t::handle_CDN,
	NULL,	// &l2tp_tunnel_t::handle_WEN,
	NULL,	// &l2tp_tunnel_t::handle_SLI,
};

void l2tp_tunnel_t::handle_control_packet(void)
{
	l2tp_handle_packet_t func = NULL;
	int msgtype;

	if (!pkt.AVPs[AVP_MessageType]) {
		tunnel_abort("handle_control_packet: no MessageType AVP");
		pretty_dump(pkt.raw_length, (unsigned char *)pkt.raw_packet);
		return;
	}

	msgtype = ntohs(pkt.AVPs[AVP_MessageType][0]);
	if (msgtype < MAX_l2tp_handle_packet_t)
		func = packet_t_array[msgtype];

	if (func)
		(this->*func)();
	else if (pkt.is_avp_mandatory(AVP_MessageType)) {
		char tmp[256];
		sprintf(tmp, "handle_control_packet: unknown mandatory MessageType AVP %d\n", msgtype);
		tunnel_abort(tmp);
	}
}

void l2tp_tx_queue_t::TimerExpired(void)
{
	l2tp_tunnel_t *tunnel = (l2tp_tunnel_t *)this;

	if (debug)
		fprintf(stderr, "tx_queue: TimerExpired() Ns=%d, Nr=%d\n",
			tunnel->tunnel_Ns, tunnel->tunnel_Nr);

	if (!IsEmpty()) {
		CQueueItem *q = Peek();
		CPppPacket *pkt = (CPppPacket *)q;

		if (nr_retransmits > 10) {
			fprintf(stderr, "l2tp tx timeout\n");
			tx_timed_out();
			return;
		}

		nr_retransmits++;
		tx_retransmit(pkt);
		retransmit_interval *= 2;
		if (retransmit_interval > 800)
			retransmit_interval = 800;
		Start(retransmit_interval);
	} else
		retransmit_interval = 10;
}

void l2tp_tx_queue_t::discard_acked_packets(u16 seq)
{
	//if (IsEmpty())
	//	fprintf(stderr, "discard_acked_packets: IsEmpty\n");

	while (!IsEmpty()) {
		CQueueItem *q = Pull();
		CPppPacket *pkt = (CPppPacket *)q;

		u16 *data = (u16 *)pkt->m_start;
		u16 pkt_Ns = ntohs(data[4]);
		u16 delta = seq - pkt_Ns;
		// this packet is ack'd, destroy it
		if (delta > 0 && delta < 0x7000) {
			nr_retransmits = 0;
			if (debug)
				fprintf(stderr, "packet %d ack'd\n", pkt_Ns);
			delete pkt;
		} else {
			if (debug)
				fprintf(stderr, "packet %d unack'd\n", pkt_Ns);
			Insert(q);
			break;
		}
	}
}

void l2tp_tunnel_t::handle_packet(char *buf, unsigned size, struct sockaddr_in *sin)
{
	if (pkt.parse_packet((u16 *)buf, size) < 0) {
		fprintf(stderr, "malformed l2tp packet\n");
		return;
	}

	if (debug)
		fprintf(stderr, "my tunnel is %d peer %d, packet %d.%d\n",
			tunnel_id, peer_tunnel_id, pkt.Tunnel, pkt.Session);

	if (pkt.Flags & L2TPF_T) {
		u16 delta = tunnel_Nr - pkt.Ns;

		if (debug)
			fprintf(stderr, "incoming packet Ns=%d, Nr=%d,"
				" tunnel_Ns=%d, tunnel_Nr=%d\n",
				pkt.Ns, pkt.Nr, tunnel_Ns, tunnel_Nr);

		if (pkt.Ns == tunnel_Nr) {
			discard_acked_packets(pkt.Nr);
			if (pkt.nr_avps != 0) {
				need_zlb = 1;
				tunnel_Nr++;
				handle_control_packet();
			} // ZLBs only ack packets, nothing else.
		} else if (delta < 0x7fff) {
			fprintf(stderr, "sort of out of sequence %d vs %d\n",
				pkt.Ns, tunnel_Nr);
			discard_acked_packets(pkt.Nr);
		} else
			fprintf(stderr, "out of sequence %d vs %d\n",
				pkt.Ns, tunnel_Nr);
	} else {
		l2tp_session_t *session = sessions[pkt.Session];
		if (session) {
			session->handle_packet(&pkt);
		} else if (debug)
			fprintf(stderr, "data packet for unknown session %d?\n", pkt.Session);
	}

	if (need_zlb)
		send_ZLB();
}

//////////////////////////////////////////////////////////
//	Session handling
//////////////////////////////////////////////////////////

void l2tp_session_t::handle_ICRQ(l2tp_packet_t *pkt)
{
	switch (session_state) {
	case L2TP_Session_Idle:
		session_mode = Session_LNS;
		session_state = L2TP_Session_WaitConnect;
		tunnel->send_ICRP(peer_session_id, session_id);
		break;

	case L2TP_Session_Established:
	case L2TP_Session_WaitTunnel:	// FIXME: error?
	case L2TP_Session_WaitReply:
	case L2TP_Session_WaitConnect:
		disconnect_session();
		break;
	}
}

void l2tp_session_t::disconnect_session(void)
{
	if (session_state != L2TP_Session_Idle) {
		if (session_state == L2TP_Session_Established)
			num_est_sessions--;
		session_state = L2TP_Session_Idle;
		tunnel->send_CDN(session_id, 1);
		clean_up();
		if (multihop_session) {
			l2tp_session_t *other = multihop_session;
			multihop_session = NULL;
			other->multihop_session = NULL;
			other->disconnect_session();
		}
	}
	delete this;
}

void l2tp_session_t::handle_ICRP(l2tp_packet_t *pkt)
{
	int id = -1;
	if (pkt->AVPs[AVP_AssignedSessionID])
		id = ntohs(*pkt->AVPs[AVP_AssignedSessionID]);

	switch (session_state) {
	case L2TP_Session_WaitReply:
		if (!peer_session_id && id <= 0) {
			if (session_cfd)
				session_cfd->printf("icrp did not contain assigned session id avp\n");
			goto cancel_it;
		}
		if (!peer_session_id)
			peer_session_id = id;
		if (session_cfd)
			session_cfd->printf("connected on icrp[%d]\n", id);
		session_state = L2TP_Session_Established;
		num_est_sessions++;
		tunnel->send_ICCN(peer_session_id);

		if (multihop_session)
			break;
		//Open();
		//m_channel->state = CS_CONNECTED;
		//Up();
	{
		Call *call = new Call;
		call->ch = &ch;
		do_bdial(-1, session_cfd, session_site ? session_site : "l2tp",
			 "5551212", call);
		break;
	}

	case L2TP_Session_Established:
	case L2TP_Session_Idle:
	case L2TP_Session_WaitConnect:
	case L2TP_Session_WaitTunnel:
	cancel_it:
		disconnect_session();
		break;
	}
}

int l2tp_session_t::HardConnect(const char *number, u32 callType)
{
	m_channel->state = CS_CONNECTED;
	ConnectComplete(0);
	Up();
	return 0;
}

void l2tp_session_t::handle_CDN(l2tp_packet_t *pkt)
{
	disconnect_session();	// Is this right?  We send a CDN this way.
}

void l2tp_session_t::clean_up(void)
{
	Stop();		// kill hello timer
	Down();
	session_mode = Session_None;
}

void l2tp_session_t::handle_ICCN(l2tp_packet_t *pkt)
{
	switch (session_state) {
	case L2TP_Session_WaitTunnel:
	case L2TP_Session_WaitReply:
	case L2TP_Session_Idle:
		clean_up();
		break;
	case L2TP_Session_WaitConnect:
		num_est_sessions++;
		session_state = L2TP_Session_Established;
		m_channel->state = CS_CONNECTED;
		Up();
		break;

	case L2TP_Session_Established:
		disconnect_session();
		break;
	}
}

int l2tp_session_t::ch_ioctl(unsigned int cmd, unsigned long arg)
{
	struct l2tp_join_bundle {
		u16     tunnel, session;
		u16     peer_tunnel, peer_session;
		unsigned long arg;
	} join;

	join.tunnel = tunnel->tunnel_id;
	join.session = session_id;
	join.peer_tunnel = tunnel->peer_tunnel_id;
	join.peer_session = peer_session_id;
	join.arg = arg;
	int ret = ioctl(tunnel->peer->l2tp_fd, cmd, &join);
	if (-1 == ret) {
		const char *why = strerror(errno);
		fprintf(stderr, "ch_ioctl: ioctl(0x%x, 0x%x, %lu): %s\n",
			tunnel->peer->l2tp_fd, cmd, arg, why);
	}
	return ret;
}

void l2tp_session_t::HardHangup(void)
{
	ch.state = CS_DISCONNECTED;
	tunnel->send_CDN(session_id, 1);
	ConnectComplete(0x400);
	clean_up();

	delete this;
}

int l2tp_session_t::HardOutput(CPppPacket *pkt)
{
	pkt->Push16(peer_session_id);
	pkt->Push16(tunnel->peer_tunnel_id);
	pkt->Push16(0x0002);	/* data packet, no sequencing */

	int ret = write(tunnel->peer->udp_fd, pkt->m_start, pkt->GetLength());
	if (ret <= 0)
		perror(__FUNCTION__);
	return ret;
}

void l2tp_session_t::dump_session(ctrlfd_t *cfd)
{
	static const char *session_states[] = {
		"idle", "wait-connect", "established", "wait-tunnel",
		"wait-reply"
	};
	static const char *session_modes[] = {
		"", " LAC", " LNS",
	};
	cfd->printf("\t\tsession peer(%5d) local(%5d) state(%s)%s",
		peer_session_id, session_id, session_states[session_state],
		session_modes[session_mode]);

	if (multihop_session)
		cfd->printf(" multihop(%d/%d:%d)",
			    multihop_session->tunnel->tunnel_id,
			    multihop_session->peer_session_id, 
			    multihop_session->session_id);

	if (session_cfd) {
		cfd->putchar(' ');
		session_cfd->dump(cfd);
	}
	cfd->putchar('\n');
}

void l2tp_session_t::handle_packet(l2tp_packet_t *pkt)
{
	CLink *link = this;

	if (debug) {
		fprintf(stderr, "session data packet: Length=%d\n", pkt->Length);
		pretty_dump(pkt->Length, (unsigned char*)pkt->DataPtr);
	}

	if (multihop_session) {
		if (multihop_session->session_state == L2TP_Session_Established) {
			pkt->DataPtr--;
			*pkt->DataPtr = htons(multihop_session->peer_session_id);
			pkt->DataPtr--;
			*pkt->DataPtr = htons(multihop_session->tunnel->peer_tunnel_id);
			pkt->DataPtr--;
			*pkt->DataPtr = htons(0x0002);
			int ret = write(multihop_session->tunnel->peer->udp_fd,
				pkt->DataPtr, pkt->Length+6);
			if (-1 == ret)
				perror("multihop connection: write(udp)");
		} else
			fprintf(stderr, "data packet on multihop not up\n");
		return;
	}

	link->Input((u8 *)pkt->DataPtr, pkt->Length);
}

void l2tp_session_t::tunnel_up(void)
{
	if (session_cfd)
		session_cfd->printf("tunnel is up\n");
	if (L2TP_Session_WaitTunnel == session_state) {
		session_state = L2TP_Session_WaitReply;
		session_mode = Session_LAC;
		tunnel->send_ICRQ(session_id);
	}
}

void l2tp_session_t::GetConfig(void (*cbf)(void *, OptionSet_t *), void *obj,
				OptionSet_t *options)
{
	l2tp_tunnel_t *multihop;
	if (multihop_session) {
		fprintf(stderr, "l2tp_session_t::GetConfig -- already have multihop\n");
		return;
	}
	multihop = tunnel->peer->l2tpd->try_multihop(options);
	if (multihop) {
		// shutdown lcp
		Down();

		// make multihop go!
		multihop_session = new l2tp_session_t(multihop, this);
		return;
	}
	babd_GetConfig(cbf, obj, options);
}

l2tp_session_t::l2tp_session_t(l2tp_tunnel_t *tun, l2tp_session_t *in_session)
{
	init_session(tun, 0, 0);
	multihop_session = in_session;
	session_state = L2TP_Session_WaitTunnel;

	if (tun->is_established())
		tunnel_up();
	else
		tun->bring_up();
}


#endif