/* pppoebr.c
 *
 * A program that will accept PPPoE packets on one network interface
 * and forward them to another interface/address.  Allows for both
 * bridging and [limited] routing of PPPoE packets.
 *
 * Copyright (C) 1997-2000 SpellCaster Telecommunications Inc.
 * $Id: pppoed.c,v 1.5 2004/08/17 18:01:53 bcrl Exp $
 * Released under the GNU Public License. See LICENSE file for details.
 *
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>

#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>

/* from the packet man page so that we compile under libc5 or glibc */
#include <sys/socket.h>
#include <features.h>    /* for the glibc version number */
#if __GLIBC__ >= 2 && __GLIBC_MINOR >= 1
#include <netpacket/packet.h>
#include <net/ethernet.h>     /* the L2 protocols */
#else
#include <asm/types.h>
#include <linux/if.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>   /* The L2 protocols */
#endif

#include <netinet/in.h>

#include "../../include/pretty_dump.h"

#define ETH_P_PPPoE_DISC	0x8863
#define ETH_P_PPPoE_SESN	0x8864

enum {
	stPPPOE_DISC_PADI,
	stPPPOE_DISC_PADR,
	stPPPOE_SESN,
} state = stPPPOE_DISC_PADI;

#define PPPoE_CODE_PADI		0x09
#define PPPoE_CODE_PADO		0x07
#define PPPoE_CODE_PADR		0x19
#define PPPoE_CODE_PADS		0x65
#define PPPoE_CODE_PADT		0xa7

unsigned char our_addr[6];
unsigned char dst_addr[6] = "\xff\xff\xff\xff\xff\xff";
unsigned char service_name[1500];
int service_name_len;
unsigned char ac_cookie[1500];
int ac_cookie_len;
unsigned char ac_name[1500];
int ac_name_len;

#define PACK	__attribute__((packed))

struct pppoehdr {
	unsigned short	ether_type;
	unsigned char	vertype;
	unsigned char	code;
	unsigned short	sesn_id;
	unsigned short	length;
} PACK;

#define PPPoE_TAG_END_OF_LIST		0x0000
#define PPPoE_TAG_SERVICE_NAME		0x0101
#define PPPoE_TAG_AC_NAME		0x0102
#define PPPoE_TAG_HOST_UNIQ		0x0103
#define PPPoE_TAG_AC_COOKIE		0x0104
#define PPPoE_TAG_VENDOR_SPECIFIC	0x0105
#define PPPoE_TAG_RELAY_SESSION_ID	0x0110
#define PPPoE_TAG_SERVICE_NAME_ERROR	0x0201
#define PPPoE_TAG_AC_SYSTEM_ERROR	0x0202
#define PPPoE_TAG_GENERIC_ERROR		0x0203

struct pppoetaghdr {
	unsigned short	tag_type;
	unsigned short	tag_length;
} PACK;

char *devname = "eth0";
struct sockaddr	sa;

void send_disc_pkt(int sk, unsigned char code, unsigned short sesn, int length, unsigned char *data)
{
	unsigned char buf[1500];
	struct pppoehdr *h = (struct pppoehdr *)(buf+12);

	memcpy(buf, dst_addr, 6);
	memcpy(buf+6, our_addr, 6);

	h->ether_type = htons(ETH_P_PPPoE_DISC);
	h->vertype = 0x11;
	h->code = code;
	h->sesn_id = htons(sesn);
	h->length = htons(length);

	if (length > 0)
		memcpy(h+1, data, length);

	fprintf(stderr, "sending:\n");
	length += sizeof(*h) + 12;
	pretty_dump(length, buf);

	if (0 >= sendto(sk, buf, length, 0, &sa, sizeof(sa)))
		perror("write");
}

void send_disc_padi(int sk)
{
	send_disc_pkt(sk, PPPoE_CODE_PADI, 0x0000, 0, NULL);
}

void send_disc_padr(int sk)
{
#define mktag(type, tagdata, taglen) \
		if (buf > data+sizeof(data)) {				\
			fprintf(stderr, "send_disc_padr: overflow\n");	\
			return;						\
		}							\
		((struct pppoetaghdr *)buf)->tag_type = htons(type);	\
		((struct pppoetaghdr *)buf)->tag_length = htons(taglen);\
		buf += sizeof (struct pppoetaghdr);			\
		memcpy(buf, tagdata, taglen);				\
		buf += taglen;

	unsigned char data[1500], *buf = data;

	mktag(PPPoE_TAG_AC_NAME, ac_name, ac_name_len);
	mktag(PPPoE_TAG_AC_COOKIE, ac_cookie, ac_cookie_len);

	/* MUST be exactly one Service-Name tag */
	mktag(PPPoE_TAG_SERVICE_NAME, service_name, service_name_len);

	send_disc_pkt(sk, PPPoE_CODE_PADR, 0x0000, buf-data, data);
}

void parse_pado(const struct pppoehdr *h, int len, const unsigned char *addr)
{
	const unsigned char *buf;
	int optlen;
	fprintf(stderr, "got pado len %d/%d/%d!\n", len, len - (int)sizeof(struct pppoehdr), ntohs(h->length));

	len -= sizeof(struct pppoehdr);
	if (ntohs(h->length) > len) {
		fprintf(stderr, "parse_pado: pppoe hdr length indicates short packet\n");
		return;
	}
	len = ntohs(h->length);
	buf = (void *)(h + 1);

	while (len > 0) {
		const struct pppoetaghdr *t = (void *)buf;
		if (len < sizeof(*t)) {
			fprintf(stderr, "parse_pado: short packet in option\n");
			return;
		}
		len -= sizeof(*t);
		buf += sizeof(*t);
		optlen = ntohs(t->tag_length);
		if (optlen < 0 || optlen > len) {
			fprintf(stderr, "parse_pado: bad option length(%d)\n", optlen);
			return;
		}
		switch (ntohs(t->tag_type)) {
		case PPPoE_TAG_AC_NAME:
			if (optlen > 0 && optlen < sizeof(ac_name)) {
				memcpy(ac_name, buf, optlen);
				ac_name_len = optlen;
				fprintf(stderr, "ac_name_len %d\n", optlen);
			} else
				fprintf(stderr, "bad AC-Name?\n");
			break;
		case PPPoE_TAG_SERVICE_NAME:
			if (!optlen)
				break;
			if (optlen < sizeof(service_name)) {
				memcpy(service_name, buf, optlen);
				service_name_len = optlen;
				fprintf(stderr, "service_name_len %d\n", optlen);
			} else
				fprintf(stderr, "bad servicename (%d)?\n", optlen);
			break;
		case PPPoE_TAG_AC_COOKIE:
			if (optlen > 0 && optlen < sizeof(ac_cookie)) {
				memcpy(ac_cookie, buf, optlen);
				ac_cookie_len = optlen;
				fprintf(stderr, "ac_cookie_len %d\n", optlen);
			} else
				fprintf(stderr, "bad AC-Cookie?\n");
			break;
		default:
			fprintf(stderr, "parse_pado: unknown pppoe option (%04x,%d)\n", ntohs(t->tag_type), optlen);
			break;
		}
		len -= optlen;
		buf += optlen;
	}

	/* packet turned out okay, use it */
	memcpy(dst_addr, addr, 6);
	state = stPPPOE_DISC_PADR;
	fprintf(stderr, "pado was good.\n");
}

void pppoe_recv(const unsigned char *buf, int len)
{
	struct pppoehdr *h = (struct pppoehdr *)(buf + 12);
	if (memcmp(buf, our_addr, 6)) {
		fprintf(stderr, "not our address.\n");
		return;
	}

	if (h->vertype != 0x11) {
		fprintf(stderr, "wrong version.\n");
		return;
	}

	switch (state) {
	case stPPPOE_DISC_PADI:
		if (h->code == PPPoE_CODE_PADO)
			parse_pado(h, len, buf+6);
		break;

	case stPPPOE_DISC_PADR:
		if (h->code == PPPoE_CODE_PADS) {
			state = stPPPOE_SESN;
			fprintf(stderr, "got pads!\n");
			sprintf((char *)buf, "%02x%02x%02x%02x%02x%02x%04x%s",
				dst_addr[0], dst_addr[1], dst_addr[2],
				dst_addr[3], dst_addr[4], dst_addr[5],
				ntohs(h->sesn_id),
				devname
				);
			execlp("bdial", "bdial", "hse", buf, NULL);
			exit(1);
		}
		break;

	case stPPPOE_SESN:
		break;
	}
}

void parse_args(int argc, char *argv[])
{
	if (argc > 1) {
		if (strcmp(argv[1], "-d"))
			goto usage;

		if (argc != 3)
			goto usage;

		devname = argv[2];
	}

	return;

usage:
	fprintf(stderr, "usage:  pppoed  <args>\n"
			"where args may be:\n"
			"	-d <devname>\n");
	exit(2);
}

int main(int argc, char *argv[])
{
	unsigned char buf[4096];
	struct ifreq ifreq;
	int sk;

	parse_args(argc, argv);

	sk = socket(PF_INET, SOCK_PACKET, htons(ETH_P_PPPoE_DISC));
	if (sk < 0) {
		perror("socket");
		return 1;
	}

	memset(&ifreq, 0, sizeof(ifreq));
	strncpy(ifreq.ifr_name, devname, sizeof(ifreq.ifr_name));
	if (ioctl(sk, SIOCGIFHWADDR, &ifreq) < 0) {
		perror("SIOCGIFHWADDR:");
		return 1;
	}

	memcpy(our_addr, &ifreq.ifr_ifru.ifru_hwaddr.sa_data, 6);

	sa.sa_family = 0;
	strncpy(sa.sa_data, devname, sizeof(sa.sa_data));

	if (bind(sk, &sa, sizeof(sa)) < 0) {
		perror("bind");
		return 1;
	}

	for (;;) {
		int len;

		switch (state) {
		case stPPPOE_DISC_PADI:
			send_disc_padi(sk);
			break;

		case stPPPOE_DISC_PADR:
			send_disc_padr(sk);
			break;

		case stPPPOE_SESN:
			break;
		}

		alarm(2);	/* timeout in 2 seconds */
		len = read(sk, buf, sizeof(buf));

		if (len <= 0) {
			perror("read");
			continue;
		}
		alarm(0);
		pretty_dump(len, buf); 

		pppoe_recv(buf, len);
	}
	return 1;
}