#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <net/if.h>

#include <l2tp_linux.h>
#include <aps_if.h>

#include "timer.h"

int main(int argc, char *argv[])
{
	TimeVal start, end;
	long long delta;
	int i, count;
	int l2tp_fd;
	struct sockaddr_l2tp l2tp_sa;
	int udp_fd;
	struct l2tp_join_bundle ljb;
	struct sockaddr_in sin = {
		sin_family: AF_INET,
		sin_port: htons(1701),
		sin_addr: { htonl(INADDR_ANY) }
	};

	if (argc < 2)
		count = 1;
	else
		count = atoi(argv[1]);

	udp_fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
	if (-1 == udp_fd) {
		perror("socket -- udp");
		return 2;
	}

	if (bind(udp_fd, (struct sockaddr *)&sin, sizeof(sin))) {
		perror("bind -- udp");
		return 2;
	}

	l2tp_fd = socket(PF_L2TP, SOCK_DGRAM, 0);
	if (-1 == l2tp_fd) {
		perror("socket(PF_L2TP)\n");
		return 2;
	}

	l2tp_sa.sl_family = AF_L2TP;
	l2tp_sa.sl_rx_sfd = udp_fd;
	l2tp_sa.sl_tx_sfd = -1;
	l2tp_sa.sl_tunnel = htons(0);
	l2tp_sa.sl_session = htons(0);
	l2tp_sa.sl_peer_tunnel = htons(0);
	l2tp_sa.sl_peer_session = htons(0);

	if (bind(l2tp_fd, (struct sockaddr *)&l2tp_sa, sizeof(l2tp_sa))) {
		perror("bind -- l2tp");
		return 2;
	}

	memset(&l2tp_sa, 0, sizeof(l2tp_sa));
	l2tp_sa.sl_family = AF_L2TP;
	l2tp_sa.sl_rx_sfd = -1;
	l2tp_sa.sl_tx_sfd = udp_fd;
	l2tp_sa.sl_tunnel = htons(0);
	l2tp_sa.sl_session = htons(0);
	l2tp_sa.sl_peer_tunnel = htons(0);
	l2tp_sa.sl_peer_session = htons(0);
	socklen_t sin_len = sizeof(l2tp_sa);
	if (bind(l2tp_fd, (struct sockaddr *)&l2tp_sa, sizeof(l2tp_sa))) {
		perror("l2tp_peer_t::alloc_tunnel_id: bind(l2tp)");
		return -1;
	}

	if (getsockname(l2tp_fd, (struct sockaddr *)&l2tp_sa, &sin_len) < 0) {
		perror("getsockbyname(l2tp_tunnel_t:l2tp_fd)");
		exit(1);
	}

	unsigned tunnel = ntohs(l2tp_sa.sl_tunnel);

	memset(&l2tp_sa, 0, sizeof(l2tp_sa));
	l2tp_sa.sl_family = AF_L2TP;
	l2tp_sa.sl_rx_sfd = -1;
	l2tp_sa.sl_tx_sfd = -1;
	l2tp_sa.sl_tunnel = tunnel;
	l2tp_sa.sl_peer_tunnel = htons(1);

	if (connect(l2tp_fd, (struct sockaddr *)&l2tp_sa, sizeof(l2tp_sa))) {
		perror("connect -- l2tp");
		return 2;
	}

	start.GetTimeOfDay();

	for (i=1; i<=count; i++) {
		ljb.tunnel = tunnel;
		ljb.session = i;
		ljb.peer_tunnel = htons(1);
		ljb.peer_session = i;
		ljb.arg = ~0UL;

		if (0 != ioctl(l2tp_fd, BIOCCREATEBUNDLE, &ljb)) {
			perror("ioctl");
			return 2;
		}
		ljb.arg = ~0UL;
		int ndev_id = ioctl(l2tp_fd, BIOCGETDEVID, &ljb);
		if (ndev_id < 0) {
			perror("ioctl");
			return 2;
		}

		struct ifreq req;
		memset(&req, 0, sizeof(req));
		sprintf(req.ifr_name, "aps%d", ndev_id);

		req.ifr_mtu = 1500;
		if (0 != ioctl(udp_fd, SIOCSIFMTU, &req))
			perror("error setting interface mtu");

		struct sockaddr_in sin;

		sin.sin_family = AF_INET;
		sin.sin_addr.s_addr = htonl(0x0a000000 + i);
		memcpy(&req.ifr_addr, &sin, sizeof(sin));
		if (0 != ioctl(udp_fd, SIOCSIFADDR, &req))
			perror("error setting interface address");

		sin.sin_family = AF_INET;
		sin.sin_addr.s_addr = htonl(0x0a100000 + i);
		memcpy(&req.ifr_dstaddr, &sin, sizeof(sin));
		if (0 != ioctl(udp_fd, SIOCSIFDSTADDR, &req))
			perror("error setting interface dest addr");

		req.ifr_flags = IFF_UP | IFF_POINTOPOINT;
		if (0 != ioctl(udp_fd, SIOCSIFFLAGS, &req)) {
			perror("error setting interface flags");
			return 2;
		}

	}

	end.GetTimeOfDay();

	delta = end.scalar() - start.scalar();
	printf("done(%d): %Ld.%06d elapsed\n", i,
		delta / 1000000, (int)(delta % 1000000));
	return 0;
}