/*
 * Copyright (c) 2001 The University of Waikato, Hamilton, New Zealand.
 * All rights reserved.
 *
 * This code has been developed by the University of Waikato WAND
 * research group along with the DAG PCI network capture cards. For
 * further information please visit http://dag.cs.waikato.ac.nz.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *      This product includes software developed by the University of
 *      Waikato, Hamilton, NZ, and its contributors.
 * 4. Neither the name of the University nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE UNIVERSITY AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE UNIVERSITY OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $Id: dagpppoe.c,v 1.1.2.2 2002/02/23 01:57:26 nevil Exp $
 *
 * See also:
 *	RFC 1483 Multiprotocol encapsulation over AAL5
 *	RFC 2516 A Method for Transmitting PPP Over Ethernet (PPPoE)
 *
 * This program currently only supports EtherType 8864.
 */
#include	<stdio.h>
#include	<fcntl.h>
#include	<stdlib.h>
#include	<string.h>
#include	<unistd.h>
#include	<regex.h>
#include	<errno.h>
#include	<stdarg.h>
#include	<sys/types.h>
#include	<sys/stat.h>
#include	<sys/time.h>
#include	<netinet/in.h>

#include	<pcap.h>
#include	<pcap-int.h>

#include	<dagtools.h>

/*
 * Rudimentary definition of an ATM cell and other structures.
 */
typedef	long long	ll_t;
typedef struct cell {
	ll_t		ts;
	unsigned	crc;
	unsigned	atm;
	unsigned char	pload[48];
} cell_t;

typedef struct ethpkt {
	unsigned char	pad[2];
	unsigned char	dst[6];
	unsigned char	src[6];
	unsigned short	etype;
	unsigned char	pload[64];	/* for 8864 packets */
} ethpkt_t;

typedef struct inet {
	unsigned	ip_hl:4;	/* header length */
	unsigned	ip_v:4;		/* version */
	unsigned char	ip_tos;		/* type of service */
	unsigned short	ip_len;		/* total length */
	unsigned short	ip_id;		/* identification */
	unsigned short	ip_off;		/* fragment offset field */
	unsigned char	ip_ttl;		/* time to live */
	unsigned char	ip_p;		/* protocol */
	unsigned short	ip_sum;		/* checksum */
	unsigned	ip_src;		/* source address */
	unsigned	ip_dst;		/* destination address */
} ip_t;

typedef struct epacket {
	ll_t		ts;
	ethpkt_t	eth;
} epacket_t;

typedef struct apacket {
	ll_t		ts;
	unsigned	atm;
	u_char		pload[2*48];
} apacket_t;

typedef union packet {
	epacket_t e;
	apacket_t a;
} packet_t;

static char     *prog;
static void     set_argv0(char *argv0);
void     	warning(char *fmt, ...) __attribute__((format (printf, 1, 2)));
void     	error(char *fmt, ...) __attribute__((format (printf, 1, 2)));
void     	panic(char *fmt, ...) __attribute__((noreturn, format (printf, 1, 2)));
static void     usage(void)	      __attribute__((noreturn));;

static void	loop(void);
static void	reass(int s, cell_t *cp);
static int	ispppoe(void *p);
static void	prts(ll_t ts);
static void	prip(unsigned char *p, int len);

static void	dump_pcap(void *);
static void	dump_ascii(void *);
static void	dump_adsl(void *);
static void	(*dump)(void *p) = dump_pcap;

/*
 * NSTREAMS must be <= 256 for this implementation to work.
 */
# define	NSTREAMS	2

FILE		*is[NSTREAMS];
char		*isname[NSTREAMS];
cell_t		icell[NSTREAMS];
static int 	nstreams = 0;

int
main(int argc, char *argv[])
{
	int     opt;
	int	ifd;
	int	flags;

	if(sizeof(cell_t) != 64)
		panic("sizeof(cell_t)==%d\n", sizeof(cell_t));
	if(sizeof(ethpkt_t) != (2+14+64))
		panic("sizeof(ethpkt_t)==%d\n", sizeof(ethpkt_t));
	if(sizeof(apacket_t) != 108)
		panic("sizeof(apacket_t)==%d\n", sizeof(apacket_t));

	set_argv0(argv[0]);
	opterr = 0;
	while((opt = getopt(argc, argv, "hvi:p:an")) != EOF)
		switch(opt) {
		  case 'h':
			usage();
		  case 'v':
			/* XXX not implemented */
			break;
		  case 'a':
			dump = dump_ascii;
			break;
		  case 'n':
			dump = dump_adsl;	/* for NeTraMet */
			break;
		  case 'i':
			if(nstreams >= NSTREAMS)
				panic("too many input streams (max %i): %s\n", nstreams, optarg);
			if((is[nstreams] = fopen(optarg, "r")) < 0)
				panic("fopen %s: %s\n", optarg, strerror(errno));
			/*
			 * Don't give stdin an isname[], it's used as a signal below
			 */
			nstreams++;
			break;
		  case 'p':
			if(nstreams >= NSTREAMS)
				panic("too many input streams (max %i): %s\n", nstreams, optarg);
			if(mkfifo(optarg, S_IRUSR|S_IWUSR|S_IRGRP) < 0)
				panic("mkfifo %s: %s\n", optarg, strerror(errno));
			/*
			 * Would like to do a straight fopen(3) here, but need
			 * nonblocking to go on and open other fifo(s).
			 */
			if((ifd = open(optarg, O_RDWR|O_NONBLOCK,
					S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP)) < 0)
				panic("open %s: %s\n", optarg, strerror(errno));
			if((flags = fcntl(ifd, F_GETFL)) < 0)
				panic("fcntl F_GETFL %s: %s\n", optarg, strerror(errno));
			flags &= ~O_NONBLOCK;
			if(fcntl(ifd, F_SETFL, flags) < 0)
				panic("fcntl F_SETFL %s: %s\n", optarg, strerror(errno));
			if((is[nstreams] = fdopen(ifd, "r")) < 0)
				panic("fdopen %s: %s\n", optarg, strerror(errno));
			isname[nstreams] = optarg;
			nstreams++;
			break;
		  default:
			panic("unknown option or argument to -%c\n", optopt);
		}
	argc -= optind;
	argv += optind;

	/*
	 * If no file name has been specified, use stdin.
	 */
	if(nstreams == 0) {
		/*
		 * Don't give stdin an isname[], it's used as a signal below
		 */
		is[nstreams] = stdin;
		nstreams++;
	}

	hash_create(10*1024);

	loop();

	return 0;
}


# if 0
ts=0x3a94c97f2708f900 crc=0x00000000 atm=0x00a00660
0xaaaa0300 0x80c20007 0x00000090 0x3935403f
0x00d0590d 0x325b8864 0x11000066 0x05ae0021
0x450005ac 0x42964000 0x4006d73e 0x0a64034a
ts=0x3a94c97f27dad700 crc=0x00000000 atm=0x00a00660
0x0ac80302 0x08830014 0x0abf8a22 0x00545b09
0x50107edc 0xb8ce0000 0xecfe1d19 0xa5da1b3c
0x3c334000 0x0001121a 0x8be23cfa 0x3683523c
# endif

/*
 * Implementing partial reassembly with VC hashing. The resulting
 * stream consists of IP headers for which the IP checksum has seen
 * a match.
 *
 * This algorithm has problems identifying packets if the second cell
 * looks identical to a PPPoE header, which is *highly* unlikely. For
 * one thing the destination IP address must be 170.170.3.0, and a lot
 * of further coincidences. We ignore this case.
 *
 * Algorithm hashes for all interfaces in parallel.
 */

static void
loop(void)
{
	int	s, latest;

	/*
	 * Buffer one cell per each input stream
	 */
	for( s = 0 ; nstreams && (s < NSTREAMS) ; s++ ) {
		if(is[s] == NULL)
			continue;	/* stream not active (anymore) */
		if(icell[s].ts == 0LL) {
			if(fread(&icell[s], 1, sizeof(cell_t), is[s]) != sizeof(cell_t)) {
				warning("stream %d closed\n", s);
				fclose(is[s]);
				is[s] = NULL;
				nstreams--;
				continue; /* for(;;) */
			}
		}
		/*
		 * Wrap up fifos
		 */
		if(isname[s] != NULL) {
			if(unlink(isname[s]) < 0)
				error("unlink %s: %s\n", isname[s], strerror(errno));
			isname[s] = NULL;
		}
	}
	/*
	 * Main data multiplex loop
	 */
	while(nstreams) {
		for( s = 0, latest = -1 ; s < NSTREAMS ; s++ ) {
			if(is[s] == NULL)
				continue;
			if((latest == -1) || (icell[s].ts < icell[latest].ts))
				latest = s;
		}
		if(latest == -1)
			panic("latest undefined, should never happen %s %d\n", __FILE__, __LINE__);
		reass(latest, &icell[latest]);
		if(fread(&icell[latest], 1, sizeof(cell_t), is[latest]) != sizeof(cell_t)) {
			warning("stream %d closed\n", latest);
			fclose(is[latest]);
			is[latest] = NULL;
			nstreams--;
		}
	}
}

/*
 * Passing the timestamp of the second cell as the "packet timestamp"
 * of the combined record to ensure the ordering at the output.
 * Previously, we took the timestamp of the first, which sounds "more
 * correct", but due to the second cell possibly arring late we might
 * see timestamps warping backwards at the output.
 */
static void
reass(int s, cell_t *cp)
{
	hash_t	*hp;
	unsigned vpvcmask = 0x0ffffff0;		/* VPI 8 bits, VCI 16 bits */
	epacket_t *ep;
	apacket_t *ap;
	ip_t	*ip;

	cp->atm = ntohl(cp->atm);
	cp->atm &= vpvcmask;	/* mask PT and CLP fields */
	cp->atm >>= 4;		/* free up topmost 8 bits */
	cp->atm |= (s << 24);	/* topmost bits == interface */

	if(ispppoe(cp->pload)) {
		/*
		 * Consider this a new first cell, PPPoE
		 */
		if((hp = hash_find(cp->atm)) != NULL) {
			error("stream %d reassembly error, dropping old cell\n", s);
			/* release data */
			(void)hash_delete(cp->atm);
		}
		if((hp = hash_enter(cp->atm)) == NULL)
			panic("hash_enter returns NULL\n");
		if((hp->data = malloc(sizeof(packet_t))) == NULL)
			panic("malloc packet_t: %s\n", strerror(errno));
		if(dump == dump_adsl) {
			/*
			 * Pasting together the two cells with ATM header and
			 * timestamp of the first, making 96 bytes of ATM payload.
			 */
			ap = hp->data;
			ap->atm = cp->atm;
			(void)memcpy(ap->pload, cp->pload, 48);
		} else {
			/*
			 * We are constructing a genering IP-over-Ethernet packet
			 * from this weird encapsulation, for better consumation
			 * by pcap and applications.
			 */
			ep = hp->data;
			ep->ts = cp->ts;
			ep->eth.pad[0] = ep->eth.pad[1] = 0;
			(void)memcpy(ep->eth.dst, &cp->pload[10], 2*6);	/* src and dst MAC */
			ep->eth.etype = htons(0x0800);			/* fake that */
			(void)memcpy(ep->eth.pload, &cp->pload[32], 16);/* first 4 words IP */
		}
	} else {
		/*
		 * Could be a first cell of a non-PPPoE packet
		 * or a second packet of a PPPoE frame
		 */
		if((hp = hash_find(cp->atm)) != NULL) {
			/*
			 * PPPoE reassembly complete, check IP checksum
			 */
			if(dump == dump_adsl) {
				ap = hp->data;
				ap->ts = cp->ts;
				(void)memcpy(&ap->pload[48], cp->pload, 48);
				ip = (ip_t *)&ap->pload[32];
				if(in_chksum((unsigned short *)ip, ip->ip_hl*4))
					error("stream %d IP checksum failed, packet dropped\n", s);
				else
					(*dump)(ap);
			} else {
				ep = hp->data;
				ep->ts = cp->ts;
				(void)memcpy(&ep->eth.pload[16], &cp->pload[0], 48);
				ip = (ip_t *)ep->eth.pload;
				if(in_chksum((unsigned short *)ip, ip->ip_hl*4))
					error("stream %d IP checksum failed, packet dropped\n", s);
				else
					(*dump)(ep);
			}
			free(hp->data);
			(void)hash_delete(cp->atm);
		}
	}
}

/*
 * Backend functions: pcap and ascii dump
 */
void			*dumper;
pcap_t			pcap;
struct pcap_pkthdr	pkthdr;

void
dump_pcap(void *vp)
{
	epacket_t	*p = vp;
	ip_t		*ip = (ip_t *)p->eth.pload;

	if(pcap.linktype == 0) {
		pcap.linktype = DLT_EN10MB;
		pcap.offset   = 2;
		pcap.snapshot = 2*6+2+64;
		pcap.tzoff    = 43200;	/* its a lie, who cares */
		dumper = pcap_dump_open(&pcap, "-");	/* stdout */
	};
	pkthdr.ts.tv_sec = (p->ts >> 32);
	p->ts = ((p->ts &  0xffffffffULL) * 1000 * 1000);
	p->ts += (p->ts & 0x80000000ULL) << 1;	/* rounding */
	pkthdr.ts.tv_usec = (p->ts >> 32);
	pkthdr.len    = ntohs(ip->ip_len) + 2*6+2;
	pkthdr.caplen = min(pkthdr.len,2*6+2+64);
	pcap_dump(dumper, &pkthdr, p->eth.dst);
}

void
dump_ascii(void  *vp)
{
	epacket_t	*p = vp;

	prts(p->ts);
	printf("\n");
	/* printf("0x%.8x\n", cp->atm);*/	/* no ATM cell context here */
	prip(p->eth.pload, 40);
	fflush(stdout);
}

/*
 * Backend function: adsl dump, for NeTraMET
 */

void
dump_adsl(void *vp)
{
	apacket_t	*p = vp;

	if(fwrite(p, 1, sizeof(apacket_t), stdout) != sizeof(apacket_t))
		panic("fwrite apacket stdout: %s\n", strerror(errno));
}

static int
ispppoe(void *p)
{
	unsigned	*pload = p;

	return ((pload[0] == htonl(0xaaaa0300)) &&
	   	(pload[1] == htonl(0x80c20007)) &&
	   	((ntohl(pload[5]) & 0xffff) == 0x8864) &&
	   	((ntohl(pload[7]) & 0xffff) == 0x0021));
}

static void
prts(ll_t ts)
{
	struct timeval	tv;
	static int simplets = 0;	/* could be a command line option */

# if (BYTE_ORDER ==  BIG_ENDIAN)
	ts =	((0xff00000000000000ULL & ts) >> 56) |
		((0x00ff000000000000ULL & ts) >> 40) |
		((0x0000ff0000000000ULL & ts) >> 24) |
		((0x000000ff00000000ULL & ts) >>  8) |
		((0x00000000ff000000ULL & ts) <<  8) |
		((0x0000000000ff0000ULL & ts) << 24) |
		((0x000000000000ff00ULL & ts) << 40) |
		((0x00000000000000ffULL & ts) << 56) ;
# endif
	if(simplets) {
		printf("ts=%lld\t", ts);
	} else {
		tv.tv_sec = (long)(ts >> 32);
		tv.tv_usec = ((ts & 0xffffffffLL) * 1000000) >> 32;
		printf("%.24s.%.6ld\t", ctime(&tv.tv_sec), tv.tv_usec);
	}
}

/*
 * Just for the sake of printing, this is slightly incorrect.
 */
#define IN_CHKSUM(X)   in_chksum((unsigned short *)(X),20)

static void
prip(unsigned char *p, int len)
{
	unsigned short  us;

	printf("\tIP  version=%d hlength=%d(%dBytes) tos=0x%.2x totlength=%d\n",
		p[0] >> 4, p[0] & 0xf, 4*(p[0]&0xf),
		p[1], ntohs(*((unsigned short *)&p[2])));
	printf("\t    ident=0x%.4x ", ntohs(*((unsigned short *)&p[4])));
	us = ntohs(*((unsigned short *)&p[6]));
	printf("flags=<%s,%s,%s> fragoff=%u\n",
		(us & (1<<15)) ? "reserved!" : "",
		(us & (1<<14)) ? "df" : "",
		(us & (1<<13)) ? "more": "",
		(us & ((1<<13)-1)));
	printf("\t    ttl=%u proto=%u", p[8], p[9]);
	switch(p[9]) {
	  case 1:
		printf("(ICMP) ");
		break;
	  case 6:
		printf("(TCP) ");
		break;
	  case 17:
		printf("(UDP) ");
		break;
	  default:
		printf(" ");
		break;
	}
	printf("chksum=0x%.4x%s\n", ntohs(*((unsigned short *)&p[10])),
		IN_CHKSUM(p) ? "(failed)" : "(correct)");
	printf("\t    src=%u.%u.%u.%u dst=%u.%u.%u.%u\n",
			p[12], p[13], p[14], p[15],
			p[16], p[17], p[18], p[19]);
	/*
	 * Print port numbers for packets with no IP options and which are
	 * not fragments, TCP or UDP
	 */
	if(((p[0]&0xf)==5) && ((us & ((1<<13)-1))==0) && ((p[9]==6) || (p[9]==17)))
		printf("\t%s sport=%u dport=%u\n",
			(p[9]==6) ? "TCP" : "UDP",
			ntohs(*((unsigned short *)&p[20])),
			ntohs(*((unsigned short *)&p[22])));
	/*
	 * XXX Could print remaining payload here
	 */
}
/*
 * Helper functions
 */
static void
set_argv0(char *argv0)
{
	if((prog = strrchr(argv0, '/')) == NULL)
		prog = argv0;
	else
		prog++;
}

void
warning(char *fmt, ...)
{
	va_list ap;

	fprintf(stderr, "%s: warning: ", prog);
	va_start(ap, fmt);
	vfprintf(stderr, fmt, ap);
	va_end(ap);
}

void
error(char *fmt, ...)
{
	va_list ap;

	fprintf(stderr, "%s: error: ", prog);
	va_start(ap, fmt);
	vfprintf(stderr, fmt, ap);
	va_end(ap);
}

void
panic(char *fmt, ...)
{
	va_list ap;

	fprintf(stderr, "%s: panic: ", prog);
	va_start(ap, fmt);
	vfprintf(stderr, fmt, ap);
	va_end(ap);
	exit(1);
}

static char usgtxt[] = "\
%s: filter PPPoE packets from DAG ATM traces\n\
Usage: %s [-hvan] [-i <in>] [-p <fifo>] <dag-atm-trace-in >pcap-out\n\
	-h      this page\n\
	-v      increase verbosity (currently unsupported)\n\
	-t	print timestamps as integer\n\
	-i <in>	take input from <in>, can be specified multiple times\n\
	-p <fifo> like -i <in>, but create named fifo <fifo>\n\
	-a	dump ASCII (default is pcap)\n\
	-n	dump ADSL (NeTraMet, default is pcap\n\
";

static void
usage()
{
	fprintf(stderr, usgtxt, prog, prog);
	exit(1);
}

