#include <net/if.h>
#include <linux/if_tun.h>
#include <sys/ioctl.h>
#include <sys/fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <netinet/in.h>
#include <sys/time.h>
#include <errno.h>
#include <netpacket/packet.h>
#include <poll.h>

#define ETH_P_PPPoE_DISC       0x8863
#define ETH_P_PPPoE_SESN       0x8864

#ifdef USE_TUN
#define DEVICE "tun0"
#else
#define DEVICE "eth4"
#endif

struct sockaddr_pkt {
	unsigned short spkt_family;
	unsigned char  spkt_device[14];
	unsigned short spkt_protocol;
};

void dump_buf(unsigned char *buf, int len)
{
	int i;
	for (i=0; i<len; i++) {
		if (!(i & 15))
			printf("%04x:", i);
		printf(" %02x", buf[i]);
		if ((i & 15) == 15)
			putchar('\n');
	}
	if (len & 15)
		putchar('\n');
}

void make_nonblock(int fd)
{
	int fl = fcntl(fd, F_GETFL);
	if (-1 == fl)
		perror("l2tp_tunnel_t: fcntl(F_GETFL)\n");
	if (-1 == fcntl(fd, F_SETFL, fl | O_NONBLOCK))
		perror("l2tp_tunnel_t: fcntl(F_SETFL)\n");
}

int main (int argc, char *argv[])
{
	struct packet_raw {
		unsigned char	dst_addr[6];
		unsigned char	src_addr[6];
		unsigned short	proto;
		unsigned char	data[50];
	} packet_raw;
	struct packet {
		unsigned short	zero;
		unsigned short	proto;
		unsigned char	dst_addr[6];
		unsigned char	src_addr[6];
		unsigned short	proto2;
		unsigned char	data[1536];
	} packet;
	long count = 1;
	unsigned char our_addr[6];
	unsigned char rand_addr[6] = { 0x54, 0x11, 0x22, 0x33, 0x44, 0x55 };
	struct ifreq ifr;
	int len;
	int pack_fd;
	struct sockaddr_pkt spkt;
	struct sockaddr sa;
	long sent = 0;
	struct timeval start_tv, end_tv;
	long pkt_size = 64;

	if (argc > 1 && !strncmp(argv[1], "--count=", 8)) {
		count = atol(argv[1]+8);
		printf("using a count of %ld\n", count);
	}

	if (argc > 2 && !strncmp(argv[2], "--size=", 7)) {
		pkt_size = atol(argv[2]+7);
		printf("using a pkt_size of %ld\n", pkt_size);
	}

#ifdef USE_TUN
	unsigned char	tmp[131072];
	int tun_fd;

	tun_fd = open("/dev/net/tun", O_RDWR);
	if (-1 == tun_fd) {
		perror("open(/dev/net/tun)");
		return 2;
	}

	memset(&ifr, 0, sizeof(ifr));
	strcpy(ifr.ifr_name, "tun0");
	ifr.ifr_flags = TUN_TAP_DEV;

	if (-1 == ioctl(tun_fd, TUNSETIFF, &ifr)) {
		perror("ioctl(TUNSETIFF)");
		return 2;
	}

	if (-1 == system("/sbin/ifconfig tun0 up")) {
		perror("system /sbin/ifconfig tun0 up");
		return 2;
	}
#endif

	pack_fd = socket(PF_PACKET, SOCK_PACKET, htons(ETH_P_PPPoE_SESN));
	if (pack_fd < 0) {
		perror("socket(ETH_P_PPPoE_SESN)");
		return 2;
	}

	memset(&ifr, 0, sizeof ifr);
	strcpy(ifr.ifr_name, DEVICE);
	if (ioctl(pack_fd, SIOCGIFHWADDR, &ifr) < 0) {
		perror("ioctl(pack_fd, SIOCGIFHWADDR)");
		return 2;
	}

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

	memset(&spkt, 0, sizeof(spkt));
	spkt.spkt_family = AF_PACKET;
	spkt.spkt_protocol = htons(ETH_P_PPPoE_SESN);
	strcpy((char *)spkt.spkt_device, DEVICE);

	memset(&sa, 0, sizeof(sa));
	strcpy(sa.sa_data, DEVICE);

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

#if 0
	if (connect(pack_fd, &sa, sizeof(sa)) < 0) {
		perror("connect(pack_fd)");
		return 2;
	}
#endif

	//make_nonblock(tun_fd);
	//make_nonblock(pack_fd);

	memset(&packet, 0, sizeof(packet));
	packet.proto = htons(ETH_P_PPPoE_SESN);
	memcpy(&packet.dst_addr, our_addr, 6);
	memcpy(&packet.src_addr, rand_addr, 6);
	packet.proto2 = htons(ETH_P_PPPoE_SESN);

	memset(&packet_raw, 0, sizeof(packet_raw));
	packet_raw.proto = htons(ETH_P_PPPoE_SESN);
	packet_raw.dst_addr[0] = 0x00;
	packet_raw.dst_addr[1] = 0x1a;
	packet_raw.dst_addr[2] = 0x92;
	packet_raw.dst_addr[3] = 0xd7;
	packet_raw.dst_addr[4] = 0xe9;
	packet_raw.dst_addr[5] = 0x81;
	//memcpy(&packet_raw.dst_addr, "\x00\x0D\xB9\x0C\xD6\x78", 6);
	memcpy(&packet_raw.dst_addr, "\x00\x0D\xB9\x12\x9A\xBA", 6);
	memcpy(&packet_raw.src_addr, our_addr, 6);
	packet_raw.proto = htons(ETH_P_PPPoE_SESN);
	packet_raw.data[0] = 0x11;
	packet_raw.data[1] = 0x00;
	packet_raw.data[2] = 0x00;	// session >> 8
	packet_raw.data[3] = 0x01;	// session & 0xff
	packet_raw.data[4] = (pkt_size - 20) >> 8;	// length >> 8
	packet_raw.data[5] = (pkt_size - 20);	// length & 0xff

	dump_buf((unsigned char *)&packet_raw, sizeof(packet_raw));

	gettimeofday(&start_tv, NULL);

	do {
		len = sendto(pack_fd, &packet_raw, pkt_size, 0, (struct sockaddr *)&spkt, sizeof(spkt));
		if (len == -1) {
			if (errno == ENOBUFS) {
				struct pollfd pfd = {
					.fd = pack_fd, 
					.events = POLLOUT, 
					.revents = 0,
				};
				if (-1 == poll(&pfd, 1, 0)) {
					perror("poll()");
					exit(2);
				}
				continue;
			} else
				break;
		}
#if 0
		printf("got len=%d\n", len);
		dump_buf(tmp, len);
#endif
		if (++sent >= count)
			break;
	} while (1);
	if (len < 0)
		printf("got len=%d: %d -> %s\n", len, errno, strerror(errno));

#ifdef USE_TUN
	while ((len = read(tun_fd, tmp, sizeof tmp)) > 0) {
		printf("got len=%d\n", len);
		dump_buf(tmp, len);
	}
#endif
	gettimeofday(&end_tv, NULL);

	printf("sent=%ld\n", sent);
	end_tv.tv_sec -= start_tv.tv_sec;
	end_tv.tv_usec -= start_tv.tv_usec;
	if (end_tv.tv_usec < 0) {
		end_tv.tv_usec += 1000000;
		end_tv.tv_sec -= 1;
	}
	printf("elapsed=%ld.%06ld\n", end_tv.tv_sec, end_tv.tv_usec);
	return 0;
}