/* Implementation of draft-L2TP-16 + l2tp-flow-bcrl-1
 * TODO:
 *	- implement slow start/congestion avoidance for the control channel
 *
 * Copyright (C) 1997-2000 SpellCaster Telecommunications Inc.
 * $Id: l2tp_p.c,v 1.2 2004/07/04 20:57:20 bcrl Exp $
 * Released under the GNU Public License. See LICENSE file for details.
 */

#include "l2tp.h"

#define PULL16(skb, var)	(var = ntohs(*(u16 *)skb_pull(skb, 2)))

#define fPULL16(skb, fl, var)	if ((Flags & fl) == fl) {		\
					len -= 2;			\
					if (len < 0)			\
						goto out_discard;	\
					PULL16(skb, var);		\
				}

#define PUSH16(skb, var)	do { *(u16 *)skb_push(skb, 2) = htons(var); } while (0)

#define TUNNEL_HASH	64

static struct l2tp_tunnel *tunnels[TUNNEL_HASH];

static inline unsigned h(u16 t, u16 o)
{
	return (t + (o ^ (o/TUNNEL_HASH))) & (TUNNEL_HASH - 1);
}

static inline struct l2tp_tunnel *l2tp_find_tunnel(u16 Tunnel, u16 Session)
{
	struct l2tp_tunnel *tunnel = tunnels[h(Tunnel, Session)];

	while (tunnel && (tunnel->Tunnel != Tunnel || tunnel->Session != Session))
		tunnel = tunnel->next;
	return tunnel;
}

static struct l2tp_tunnel *l2tp_new_tunnel()
{
	struct l2tp_tunnel *t = kmalloc(sizeof(*t), GFP_KERNEL);
	if (t) {
		/* set things up */
		memset(t, 0, sizeof(*t));
	}

	return t;
}

void l2tp_ctrl_recv(struct l2tp_tunnel *s, struct sk_buff *skb)
{
	enum {
		m_SCCRQ = 1
	}	mask;
	u16	val;

	PULL16(skb, val);
	if (8 != val)
		goto out_discard;

	PULL16(skb, val);
	if (0 != val)
		goto out_discard;

	PULL16(skb, val);
	if (0 != val)
		goto out_discard;

	PULL16(skb, val);

	switch (val) {
	case L2TP_MT_SCCRQ:
	case L2TP_MT_SCCRP:
	case L2TP_MT_SCCCN:
	case L2TP_MT_StopCCN:
	case L2TP_MT_Hello:
	case L2TP_MT_OCRQ:
	case L2TP_MT_OCRP:
	case L2TP_MT_OCCN:
	case L2TP_MT_ICRQ:
	case L2TP_MT_ICRP:
	case L2TP_MT_ICCN:
	case L2TP_MT_CDN:
	case L2TP_MT_WEN:
	case L2TP_MT_SLI:

	default:
		break;
	}


	while (skb->len > 0) {
		u16 len, vendor, type;
		int M;	/* mandatory? */

		PULL16(skb, len);
		PULL16(skb, vendor);
		PULL16(skb, type);

		/* FIXME: implement hiding, but for now, just ignore it. */
		M = (len & 0x8000);
		len = (len >> 6) & 0xf;

		/* we don't do any vendor extensions */
		if (vendor)
			continue;

		switch (type) {
		case 1:	/* SCCRQ */
			mask |= m_SCCRQ;
			
			continue;
		case 2:	/* SCCRP */
			break;
		}
	}

	return;

out_discard:
	return;
}

#if 0
static int l2tp_ctrl_xmit(struct l2tp_tunnel *tunnel, struct sk_buff *skb)
{
	if (tunnel->data_win) {
		PUSH16(skb, tunnel->data_Nv);
		PUSH16(skb, tunnel->data_Nw);
		PUSH16(skb, tunnel->data_Nr);
		PUSH16(skb, tunnel->data_Ns);
	}

	PUSH16(skb, tunnel->Nr);
	PUSH16(skb, tunnel->Ns);
	PUSH16(skb, tunnel->Session);
	PUSH16(skb, tunnel->Tunnel);
	PUSH16(skb, L2TP_S);

	return l2tp_udp_xmit(tunnel, skb);
}

/*
 */
static int l2tp_data_xmit(struct l2tp_tunnel *tunnel, struct sk_buff *skb)
{
	u16 Flags;

	if (tunnel->data_win) {
		PUSH16(skb, tunnel->data_Nv);
		PUSH16(skb, tunnel->data_Nw);
		PUSH16(skb, tunnel->data_Nr);
		PUSH16(skb, tunnel->data_Ns);
		Flags = L2TP_D;
	} else {
		PUSH16(skb, 0);
		PUSH16(skb, tunnel->data_Ns);
		Flags = L2TP_S;
	}

	PUSH16(skb, tunnel->Session);
	PUSH16(skb, tunnel->Tunnel);

	PUSH16(skb, Flags);

	return l2tp_udp_xmit(tunnel, skb);
}
#endif


#define seq_dup(Ns, Nr)		((u16)(Ns - Nr) > 0x7fffU)	/* exactly half the sequence space is valid */


/* l2tp_recv
 *	Takes an incoming packet and passes it onto the appropriate tunnel handler.
 */
void l2tp_udp_recv(struct sk_buff *skb)
{
	struct l2tp_tunnel	*tunnel;
	int	len;
	u16	Flags;
	u16	Length = 0, Tunnel, Session, Ns = 0, Nr, Offset = 0;

	len = skb->len;

	fPULL16(skb, 0, Flags);
	if (L2TP_VER != (Flags & L2TP_VER_MASK)) {
		printk("bad version\n");
		goto out_discard;
	}
 
	fPULL16(skb, L2TP_L, Length);

	if ((Flags & L2TP_L) && Length > len) {	/* FIXME: repair for alignment */
		printk("bad length\n");
		goto out_discard;
	}

	fPULL16(skb, 0, Tunnel);
	fPULL16(skb, 0, Session);
	fPULL16(skb, L2TP_S, Ns);
	fPULL16(skb, L2TP_S, Nr);
	fPULL16(skb, L2TP_O, Offset);

	/* toss the header */
	if (Offset > len) {
		printk("bad offset\n");
		goto out_discard;
	}

	skb_pull(skb, Offset);
	len -= Offset;

	/* if we don't know about the tunnel, issue a stop */
	if (unlikely(!(tunnel = l2tp_find_tunnel(Tunnel, Session))))  {
#if 0
		l2tp_StopCCN(Tunnel, Session);
#endif
		printk("l2tp_udp_recv: discarding packet for unknown tunnel (%u %u)\n", Tunnel, Session);
		b_kfree_skb(skb);
		return;
	}

	if (!(L2TP_T & Flags)) {		/* this is a data packet */
		/* data packets are unreliably delivered -- we only need to make sure that it's in sequence */
		if (L2TP_S & Flags) {
			if (seq_dup(Ns, tunnel->data_Nr))
				goto out_dup;
			tunnel->data_Nr = Ns + 1;
		}

		l2tp_data_recv(tunnel, skb);
		return;
	}

	/* oohh, a control packet */
	/* The S bit MUST be set on control packets */
	if (!(L2TP_S & Flags))
		goto out_discard;

	/* if this packet is in sequence, process it and any other packets following
	 * it in our inbound queue.  This is split up into two parts so that any outgoing
	 * packets sent in the receive path can be sent with an updated Nr
	 */
	if (tunnel->Nr == Ns) {
		unsigned i, n = (Ns + 1) % L2TP_MAX_WINDOW;

		/* find out how many packets were queued up */
		for (i = 1; (i < L2TP_MAX_WINDOW) && tunnel->inPkts[n] ; i++)
			n = (n + 1) % L2TP_MAX_WINDOW;

		tunnel->Nr = Ns + i;
		tunnel->ack_pending = 1;

		/* now process them */
		while (i--) {
			l2tp_ctrl_recv(tunnel, skb);
			Ns++;
			skb = tunnel->inPkts[Ns % L2TP_MAX_WINDOW];
			tunnel->inPkts[Ns % L2TP_MAX_WINDOW] = NULL;
		}

		if (skb)
			tunnel->inPkts[Ns % L2TP_MAX_WINDOW] = skb;

		return;
	}

	/* for statistics, mark duplicate packets differently */
	if (seq_dup(Ns, tunnel->Nr))
		goto out_dup;

	/* don't queue it if it's beyond of our max window */
	if ((Ns - tunnel->Nr) >= L2TP_MAX_WINDOW)
		goto out_discard;

	/* is it a dup?  Argh! */
	if (tunnel->inPkts[Ns % L2TP_MAX_WINDOW])
		goto out_dup;

	tunnel->inPkts[Ns % L2TP_MAX_WINDOW] = skb;

	return;

out_dup:
#if 0
	stats.inDuplicates++;
#endif
	printk("l2tp_udp_recv: duplicate packet\n");
	b_kfree_skb(skb);
	return;

	/* silently discard a malformed packet */
out_discard:
#if 0
	stats.inDiscards++;
#endif
	printk("l2tp_udp_recv: discarding packet\n");
	b_kfree_skb(skb);
	return;
}