#include <signal.h>
#include <stddef.h>
#include <errno.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>
#include <netinet/udp.h>
#include <netinet/tcp.h>
#include <net/if.h>
#include <net/ethernet.h>
#include <stdio.h>
#include <string.h>
#include <err.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/uio.h>
#include <linux/sockios.h>
#include <linux/if_tun.h>
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>

typedef uint32_t u32;
typedef uint16_t u16;
typedef uint8_t u8;

#ifndef TUNGETFEATURES
#define TUNGETFEATURES  _IOR('T', 207, unsigned int)
#endif
#ifndef IFF_GSO_HDR
#define IFF_GSO_HDR	0x4000
#endif

static bool use_gso = true;

static bool write_all(int fd, const void *data, unsigned long size)
{
	while (size) {
		int done;

		done = write(fd, data, size);
		if (done < 0 && errno == EINTR)
			continue;
		if (done <= 0)
			return false;
		data += done;
		size -= done;
	}

	return true;
}

static bool read_all(int fd, void *data, unsigned long size)
{
	while (size) {
		int done;

		done = read(fd, data, size);
		if (done < 0 && errno == EINTR)
			continue;
		if (done <= 0)
			return false;
		data += done;
		size -= done;
	}

	return true;
}

static uint32_t str2ip(const char *ipaddr)
{
	unsigned int byte[4];

	sscanf(ipaddr, "%u.%u.%u.%u", &byte[0], &byte[1], &byte[2], &byte[3]);
	return (byte[0] << 24) | (byte[1] << 16) | (byte[2] << 8) | byte[3];
}

static void configure_device(int fd, const char *devname, uint32_t ipaddr)
{
	struct ifreq ifr;
	struct sockaddr_in *sin = (struct sockaddr_in *)&ifr.ifr_addr;

	/* Don't read these incantations.  Just cut & paste them like I did! */
	memset(&ifr, 0, sizeof(ifr));
	strcpy(ifr.ifr_name, devname);
	sin->sin_family = AF_INET;
	sin->sin_addr.s_addr = htonl(ipaddr);
	if (ioctl(fd, SIOCSIFADDR, &ifr) != 0)
		err(1, "Setting %s interface address", devname);
	ifr.ifr_flags = IFF_UP;
	if (ioctl(fd, SIOCSIFFLAGS, &ifr) != 0)
		err(1, "Bringing interface %s up", devname);
}

static int setup_tun_net(uint32_t ip)
{
	struct ifreq ifr;
	int netfd, ipfd;
	unsigned int features;

	/* We open the /dev/net/tun device and tell it we want a tap device.  A
	 * tap device is like a tun device, only somehow different.  To tell
	 * the truth, I completely blundered my way through this code, but it
	 * works now! */
	netfd = open("/dev/net/tun", O_RDWR);
	if (netfd < 0)
		err(1, "Opening /dev/net/tun");

	if (use_gso &&
	    (ioctl(netfd, TUNGETFEATURES, &features) != 0
	     || !(features & IFF_GSO_HDR))) {
		fprintf(stderr, "No GSO support!\n");
		use_gso = false;
	}

	memset(&ifr, 0, sizeof(ifr));
	ifr.ifr_flags = IFF_TAP | IFF_NO_PI | (use_gso ? IFF_GSO_HDR : 0);
	strcpy(ifr.ifr_name, "tap%d");
	if (ioctl(netfd, TUNSETIFF, &ifr) != 0)
		err(1, "configuring /dev/net/tun");

	/* We need a socket to perform the magic network ioctls to bring up the
	 * tap interface, connect to the bridge etc.  Any socket will do! */
	ipfd = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP);
	if (ipfd < 0)
		err(1, "opening IP socket");

	/* We are peer 0, ie. first slot, so we hand dev->mem to this routine
	 * to write the MAC address at the start of the device memory.  */
	configure_device(ipfd, ifr.ifr_name, ip);
	close(ipfd);

	return netfd;
}

static void two_way_popen(char *const argv[])
{
	int pid;
	int pipe1[2], pipe2[2];

	if (pipe(pipe1) != 0 || pipe(pipe2) != 0)
		err(1, "creating pipe");

	pid = fork();
	if (pid == -1)
		err(1, "forking");

	if (pid == 0) {
		/* We are the child. */
		close(pipe1[1]);
		close(pipe2[0]);
		dup2(pipe1[0], STDIN_FILENO);
		dup2(pipe2[1], STDOUT_FILENO);

		execvp(argv[0], argv);
		fprintf(stderr, "Failed to exec '%s': %m\n", argv[0]);
		kill(getppid(), SIGKILL);
	}

	/* We are parent. */
	close(pipe1[0]);
	close(pipe2[1]);
	dup2(pipe1[1], STDOUT_FILENO);
	dup2(pipe2[0], STDIN_FILENO);
}

struct virtio_net_hdr
{
#define VIRTIO_NET_HDR_F_NEEDS_CSUM	1	// Use csum_start, csum_offset
	__u8 flags;
#define VIRTIO_NET_HDR_GSO_NONE		0	// Not a GSO frame
#define VIRTIO_NET_HDR_GSO_TCPV4	1	// GSO frame, IPv4 TCP (TSO)
/* FIXME: Do we need this?  If they said they can handle ECN, do they care? */
#define VIRTIO_NET_HDR_GSO_TCPV4_ECN	2	// GSO frame, IPv4 TCP w/ ECN
#define VIRTIO_NET_HDR_GSO_UDP		3	// GSO frame, IPv4 UDP (UFO)
#define VIRTIO_NET_HDR_GSO_TCPV6	4	// GSO frame, IPv6 TCP
	__u8 gso_type;
	__u16 gso_hdr_len;	/* Ethernet + IP + tcp/udp hdrs */
	__u16 gso_size;		/* Bytes to append to gso_hdr_len per frame */
	__u16 csum_start;	/* Position to start checksumming from */
	__u16 csum_offset;	/* Offset after that to place checksum */
};

struct packet
{
	struct virtio_net_hdr gso;
	struct ether_header mac;
	struct iphdr ip;
	union {
		struct icmphdr icmp;
		struct tcphdr tcp;
		struct udphdr udp;
		char pad[65535 - 34];
	};
} __attribute__((packed));

static inline unsigned short from32to16(unsigned long x)
{
	/* add up 16-bit and 16-bit for 16+c bit */
	x = (x & 0xffff) + (x >> 16);
	/* add up carry.. */
	x = (x & 0xffff) + (x >> 16);
	return x;
}

static unsigned int csum_fold(unsigned int sum)
{
	return ~from32to16(sum);
}

static unsigned long do_csum(const unsigned char * buff, int len)
{
	int odd, count;
	unsigned long result = 0;

	if (len <= 0)
		return 0;

	odd = 1 & (unsigned long) buff;
	if (odd) {
		result = *buff;
		len--;
		buff++;
	}
	count = len >> 1;		/* nr of 16-bit words.. */
	if (count) {
		if (2 & (unsigned long) buff) {
			result += *(unsigned short *) buff;
			count--;
			len -= 2;
			buff += 2;
		}
		count >>= 1;		/* nr of 32-bit words.. */
		if (count) {
		        unsigned long carry = 0;
			do {
				unsigned int w = *(unsigned int *) buff;
				count--;
				buff += 4;
				result += carry;
				result += w;
				carry = (w > result);
			} while (count);
			result += carry;
			result = (result & 0xffff) + (result >> 16);
		}
		if (len & 2) {
			result += *(unsigned short *) buff;
			buff += 2;
		}
	}
	if (len & 1)
		result += (*buff << 8);
	result = from32to16(result);
	if (odd)
		result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);

	return result;
}

static unsigned int csum_partial(const void * buff, int len, unsigned int sum)
{
	unsigned int result = do_csum(buff, len);

	/* add in old sum, and carry.. */
	result += sum;
	if (sum > result)
		result += 1;
	return result;
}

static void csum_replace(__u16 *sum, u32 from, u32 to)
{
	u32 diff[] = { ~from, to };
	*sum = csum_fold(csum_partial(diff, sizeof(diff), *sum ^ 0xFFFF));
}

#define NIPQUAD(addr) \
	((unsigned char *)&addr)[0], \
	((unsigned char *)&addr)[1], \
	((unsigned char *)&addr)[2], \
	((unsigned char *)&addr)[3]

/* Change destination IP address */
static void nat_packet(struct packet *packet, u32 src, u32 dst)
{
	u32 oldsrc, olddst;

	if (packet->mac.ether_type != htons(ETHERTYPE_IP))
		return;

	oldsrc = packet->ip.saddr;
	olddst = packet->ip.daddr;
	packet->ip.saddr = src;
	packet->ip.daddr = dst;
	csum_replace(&packet->ip.check, oldsrc, src);
	csum_replace(&packet->ip.check, olddst, dst);

	switch (packet->ip.protocol) {
	case IPPROTO_TCP:
		csum_replace(&packet->tcp.check, oldsrc, src);
		csum_replace(&packet->tcp.check, olddst, dst);
		break;
	case IPPROTO_UDP:
		csum_replace(&packet->udp.check, oldsrc, src);
		csum_replace(&packet->udp.check, olddst, dst);
		break;
	}
}

int main(int argc, char *argv[])
{
	int netfd;
	__u32 natdst, natsrc;
	int size;
	struct packet packet;
	void *buf;

	if (argv[1] && strcmp(argv[1], "--no-gso") == 0) {
		argv++;
		argc--;
		use_gso = false;
	}

	if (argc < 4)
		errx(1, "Usage: %s [--no-gso] ip-addr src-nat-addr dst-nat-addr [command-to-open...]", argv[0]);

	netfd = setup_tun_net(str2ip(argv[1]));
	natsrc = htonl(str2ip(argv[2]));
	natdst = htonl(str2ip(argv[3]));

	/* Eg. ssh othermachine /root/tun_gso_pipe 192.168.1.2 192.168.5.2 192.158.5.1 */
	if (argc > 4)
		two_way_popen(argv+4);

	if (use_gso)
		buf = &packet;
	else
		buf = &packet.mac;

	for (;;) {
		fd_set fds;

		FD_ZERO(&fds);
		FD_SET(netfd, &fds);
		FD_SET(STDIN_FILENO, &fds);
		select(netfd+1, &fds, NULL, NULL, NULL);
		if (FD_ISSET(netfd, &fds)) {
			size = read(netfd, buf, sizeof(packet));
			if (size <= 0)
				err(1, "Reading netfd");
			if (use_gso)
				fprintf(stderr, "Read %u, gso = %u/%u\n", size,
					packet.gso.gso_type,
					packet.gso.gso_size);
			nat_packet(&packet, natsrc, natdst);
			if (!write_all(STDOUT_FILENO, &size, sizeof(size))
			    || !write_all(STDOUT_FILENO, buf, size))
				err(1, "Writing data to stdout");
		}
		if (FD_ISSET(STDIN_FILENO, &fds)) {
			int ret;
			if (!read_all(STDIN_FILENO, &size, sizeof(size)))
				err(1, "Reading stdin");
			if (!read_all(STDIN_FILENO, buf, size))
				err(1, "Reading %u byte packet", size);
			fprintf(stderr, "Writing %u, gso = %u/%u\n", size,
				packet.gso.gso_type,
				packet.gso.gso_size);
			ret = write(netfd, buf, size);
			if (ret != size)
				err(1, "Writing data to netfd gave %i/%i",
				    ret, size);
		}
	}
}