/* l2tp_test.c - a simple test program for L2TP on Linux using pppd.
 *
 * Copyright 2012 Benjamin LaHaise <bcrl@kvack.org>.  All Rights Reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <netinet/in.h>
#include <net/if.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
//#include <linux/if_ether.h>
//#include <linux/if_pppox.h>

#ifndef AF_PPPOX
#define AF_PPPOX		24
#define PF_PPPOX		AF_PPPOX
#endif
#ifndef PX_PROTO_OL2TP
#define PX_PROTO_OL2TP		1
#endif

typedef unsigned short __kernel_sa_family_t;
typedef	int __kernel_pid_t;
typedef unsigned short __u16;
typedef unsigned int __u32;

struct pppol2tp_addr {
	__kernel_pid_t		pid;
	int			fd;
	struct sockaddr_in	addr;
	__u16			s_tunnel, s_session;
	__u16			d_tunnel, d_session;
};

struct sockaddr_pppol2tp {
	__kernel_sa_family_t	sa_family;
	unsigned int		sa_protocol;
	struct pppol2tp_addr	pppol2tp;
} __attribute__((packed));

struct pppol2tpv3_addr {
	__kernel_pid_t		pid;
	int			fd;
	struct sockaddr_in	addr;
	__u32			s_tunnel, s_session;
	__u32			d_tunnel, d_session;
} __attribute__((packed));

struct sockaddr_pppol2tpv3 {
        __kernel_sa_family_t	sa_family;
        unsigned int		sa_protocol;
        struct pppol2tpv3_addr	pppol2tp;
} __attribute__((packed));

struct pppol2tpin6_addr {
	__kernel_pid_t		pid;
	int			fd;

	__u16			s_tunnel, s_session;
	__u16			d_tunnel, d_session;

	struct sockaddr_in6	addr;
};

struct sockaddr_pppol2tpin6 {
	__kernel_sa_family_t	sa_family;
	unsigned int		sa_protocol;
	struct pppol2tpin6_addr	pppol2tp;
} __attribute__((packed));

struct pppol2tpv3in6_addr {
	__kernel_pid_t		pid;
	int			fd;

	__u32			s_tunnel, s_session;
	__u32			d_tunnel, d_session;

	struct sockaddr_in6	addr;
};

struct sockaddr_pppol2tpv3in6 {
	__kernel_sa_family_t		sa_family;
	unsigned int			sa_protocol;
	struct pppol2tpv3in6_addr	pppol2tp;
};

union sockaddru {
	struct sockaddr		sa;
	struct sockaddr_in	sin;
	struct sockaddr_in6	sin6;
};

union pppol2tp_sa_union {
	struct sockaddr_pppol2tp	sa4;
	struct sockaddr_pppol2tpin6	sa6;
	struct sockaddr_pppol2tpv3	sa4_v3;
	struct sockaddr_pppol2tpv3in6	sa6_v3;
};

unsigned parse_scope(char *str)
{
	int sockfd = socket(AF_INET, SOCK_STREAM, 0);
	struct ifreq ifr;

	memset(&ifr, 0, sizeof(ifr));
	strncpy(ifr.ifr_name, str, sizeof(ifr.ifr_name));
	if (0 == ioctl(sockfd, SIOCGIFINDEX, &ifr)) {
		close(sockfd);
		return ifr.ifr_ifindex;
	}
	printf("ioctl(sockfd, SIOCGIFINDEX, '%s'): %s\n", str, strerror(errno));
	exit(2);
}

union sockaddru *parse_sockaddru(char *str)
{
	union sockaddru *sau = calloc(1, sizeof(*sau));
	char *percent = strchr(str, '%');
	char *at = strchr(str, '@');
	int port = 1701;

	if (at) {
		port = atoi(at + 1);
		if ((port < 0) || (port > 65535)) {
			printf("Invalid port number: %d\n", port);
			exit(1);
		}
		*at = 0;
	}

	if (strchr(str, ':')) {
		sau->sin6.sin6_family = AF_INET6;
		sau->sin6.sin6_port = htons(port);
		if (percent)
			*percent = 0;
		if (!inet_pton(AF_INET6, str, &sau->sin6.sin6_addr)) {
			printf("Could not parse '%s' as IPv6\n", str);
			exit(1);
		}
		if (percent)
			sau->sin6.sin6_scope_id = parse_scope(percent + 1);
printf("port = %u\n", (unsigned)(ntohs(sau->sin6.sin6_port)));
	} else {
		if (percent) {
			printf("scope not supported in IPv4 address\n");
			exit(1);
		}
		sau->sin.sin_family = AF_INET;
		sau->sin.sin_port = htons(port);
printf("port = %u\n", (unsigned)(ntohs(sau->sin.sin_port)));
		if (!inet_pton(AF_INET, str, &sau->sin.sin_addr)) {
			printf("Could not parse '%s' as IPv4\n", str);
			exit(1);
		}
	}

	return sau;
}

char *format_su(union sockaddru *su, char *tmp, size_t tmp_len)
{
	const char *scope = "";
	const char *a;
	char t2[64], t3[16];
	unsigned short port;

	if (su->sa.sa_family == AF_INET) {
		a = inet_ntop(AF_INET, &su->sin.sin_addr, t2, sizeof t2);
		port = ntohs(su->sin.sin_port);
	} else if (su->sa.sa_family == AF_INET6) {
		a = inet_ntop(AF_INET6, &su->sin6.sin6_addr, t2, sizeof t2);
		port = ntohs(su->sin6.sin6_port);
		if (su->sin6.sin6_scope_id) {
			snprintf(t3, sizeof(t3), "%%%u",
				su->sin6.sin6_scope_id);
			scope = t3;
		}
	} else
		return NULL;

	snprintf(tmp, tmp_len, "%s%s %d", a, scope, port);
	return tmp;
}

void print_p_sa_u(union pppol2tp_sa_union *p_sa_u, int len)
{
	char tmp[64];

	if (len == sizeof(p_sa_u->sa4))
		printf( "sa4 {\n"
			"	sa_family   = %u\n"
			"	sa_protocol = %u\n"
			"	.pid        = %d\n"
			"	.fd         = %d\n"
			"	.addr       = %s\n"
			"	.s_tunnel   = %u\n"
			"	.s_session  = %u\n"
			"	.d_tunnel   = %u\n"
			"	.d_session  = %u\n}\n",
			(unsigned)p_sa_u->sa4.sa_family,
			(unsigned)p_sa_u->sa4.sa_protocol,
			(int)p_sa_u->sa4.pppol2tp.pid,
			(int)p_sa_u->sa4.pppol2tp.fd,
			format_su((void *)&p_sa_u->sa4.pppol2tp.addr, tmp, sizeof(tmp)),
			(unsigned)p_sa_u->sa4.pppol2tp.s_tunnel,
			(unsigned)p_sa_u->sa4.pppol2tp.s_session,
			(unsigned)p_sa_u->sa4.pppol2tp.d_tunnel,
			(unsigned)p_sa_u->sa4.pppol2tp.d_session);
	else if (len == sizeof(p_sa_u->sa6))
		printf( "sa6 {\n"
			"	sa_family   = %u\n"
			"	sa_protocol = %u\n"
			"	.pid        = %d\n"
			"	.fd         = %d\n"
			"	.addr       = %s\n"
			"	.s_tunnel   = %u\n"
			"	.s_session  = %u\n"
			"	.d_tunnel   = %u\n"
			"	.d_session  = %u\n}\n",
			(unsigned)p_sa_u->sa6.sa_family,
			(unsigned)p_sa_u->sa6.sa_protocol,
			(int)p_sa_u->sa6.pppol2tp.pid,
			(int)p_sa_u->sa6.pppol2tp.fd,
			format_su((void *)&p_sa_u->sa6.pppol2tp.addr, tmp, sizeof(tmp)),
			(unsigned)p_sa_u->sa6.pppol2tp.s_tunnel,
			(unsigned)p_sa_u->sa6.pppol2tp.s_session,
			(unsigned)p_sa_u->sa6.pppol2tp.d_tunnel,
			(unsigned)p_sa_u->sa6.pppol2tp.d_session);
	else if (len == sizeof(p_sa_u->sa4_v3))
		;
	else if (len == sizeof(p_sa_u->sa6_v3))
		;
	else {
		fprintf(stderr, "print_p_sa_u: unknown len(%d)!\n", len);
		exit(2);
	}
}

int main(int argc, char *argv[])
{
	union sockaddru	*local = NULL, *remote = NULL;
	char tmp[256];
	int argi = 1;
	int udp_fd;
	int tunnel_fd;
	int session_fd;
	char *lnsmode = "";
	int our_tunnel_id = 1;
	int our_session_id = 2;
	int peer_tunnel_id = 1;
	int peer_session_id = 2;

	for (argi=1; argi < argc; argi++) {
		if (!strncmp(argv[argi], "--local=", 8))
			local = parse_sockaddru(argv[argi] + 8);
		else if (!strncmp(argv[argi], "--our-tunnel-id=", 16))
			our_tunnel_id = atoi(argv[argi] + 16);
		else if (!strncmp(argv[argi], "--peer-tunnel-id=", 17))
			peer_tunnel_id = atoi(argv[argi] + 17);
		else if (!strncmp(argv[argi], "--remote=", 9))
			remote = parse_sockaddru(argv[argi] + 9);
		else if (strstr(argv[argi], "pppd"))
			break;
		else {
			printf("unused argument '%s'\n", argv[argi]);
			return 1;
		}
	}
	if (!local) {
		printf("Usage: l2tp_test --local=<local ip> [--remote=<remote ip>] /usr/sbin/pppd <pppd options>\n");
		return 1;
	}

	if (remote && (local->sa.sa_family != remote->sa.sa_family)) {
		printf("local and remote family does not match!\n");
		return 1;
	}

	printf("sizeof(struct sockaddr_pppol2tp) = %lu\n",
		(unsigned long)sizeof(struct sockaddr_pppol2tp));
	printf("sizeof(struct sockaddr_pppol2tpin6) = %lu\n",
		(unsigned long)sizeof(struct sockaddr_pppol2tpin6));
	printf("sizeof(struct sockaddr_pppol2tpv3) = %lu\n",
		(unsigned long)sizeof(struct sockaddr_pppol2tpv3));
	printf("sizeof(struct sockaddr_pppol2tpv3in6) = %lu\n",
		(unsigned long)sizeof(struct sockaddr_pppol2tpv3in6));
	printf("sizeof(union pppol2tp_sa_union) = %lu\n",
		(unsigned long)sizeof(union pppol2tp_sa_union));

	udp_fd = socket(local->sin.sin_family, SOCK_DGRAM, IPPROTO_UDP);
	if (udp_fd < 0) {
		perror("socket(UDP)");
		return 1;
	}

	if (bind(udp_fd, &local->sa, sizeof(*local))) {
		perror("bind(UDP)");
		return 1;
	}

	if (remote && connect(udp_fd, &remote->sa, sizeof(*remote))) {
		perror("connect(UDP)");
		return 1;
	}

	printf("Local: %s\n", format_su(local, tmp, sizeof tmp));
	if (remote)
		printf("Remote: %s\n", format_su(remote, tmp, sizeof tmp));

	tunnel_fd = socket(AF_PPPOX, SOCK_DGRAM, PX_PROTO_OL2TP);
	if (tunnel_fd < 0) {
		perror("socket(AF_PPPOX, SOCK_DGRAM, PX_PROTO_OL2TP)");
		return 1;
	}

	struct sockaddr_pppol2tp tunnel_sa = {
		.sa_family = AF_PPPOX,
		.sa_protocol = PX_PROTO_OL2TP,
		.pppol2tp.fd = udp_fd,
		.pppol2tp.s_tunnel = our_tunnel_id,
		.pppol2tp.s_session = 0,
		.pppol2tp.d_tunnel = peer_tunnel_id,
		.pppol2tp.d_session = 0,
	};

	if (connect(tunnel_fd, (struct sockaddr *)&tunnel_sa,
                    sizeof(tunnel_sa))) {
		perror("connect(tunnel_fd)");
		return 1;
	}

	session_fd = socket(AF_PPPOX, SOCK_DGRAM, PX_PROTO_OL2TP);
	if (session_fd < 0) {
		perror("socket(AF_PPPOX, SOCK_DGRAM, PX_PROTO_OL2TP)");
		return 1;
	}

	struct sockaddr_pppol2tp session_sa = {
		.sa_family = AF_PPPOX,
		.sa_protocol = PX_PROTO_OL2TP,
		.pppol2tp.fd = udp_fd,
		.pppol2tp.s_tunnel = our_tunnel_id,
		.pppol2tp.s_session = our_session_id,
		.pppol2tp.d_tunnel = peer_tunnel_id,
		.pppol2tp.d_session = peer_session_id,
	};

	if (connect(session_fd, (struct sockaddr *)&session_sa,
                    sizeof(tunnel_sa))) {
		perror("connect(session_fd)");
		return 1;
	}

	printf("udp_fd=%d tunnel_fd=%d session_fd=%d\n", udp_fd, tunnel_fd,
	       session_fd);

	union pppol2tp_sa_union p_sa_u;
	socklen_t len = sizeof(p_sa_u);
	if (getsockname(session_fd, (struct sockaddr *)&p_sa_u, &len)) {
		perror("getsockname(session_fd, p_sa_u)");
		return 1;
	}

	printf("getsockname(session_fd, p_sa_u): len=%d\n", (int)len);
	print_p_sa_u(&p_sa_u, len);

	if (argc <= argi)
		return 0;

	char *out = tmp;
	*out = 0;
	for (; argi < argc; argi++) {
		int len = strlen(argv[argi]);
		if ((out - tmp + len + 2) >= sizeof(tmp)) {
			fprintf(stderr, "pppd command string too long\n");
			return 1;
		}
		if (out != tmp)
			*out++ = ' ';
		memcpy(out, argv[argi], len + 1);
		out += len;
	}
	snprintf(out, sizeof(tmp) - (out - tmp),
		 " plugin pppol2tp.so pppol2tp %d pppol2tp_tunnel_id %d"
		 " pppol2tp_session_id %d%s",
		 session_fd, peer_tunnel_id, peer_session_id, lnsmode);

	printf("command line: '%s'\n", tmp);
	system(tmp);

	return 0;
}