#include <sys/socket.h>
#include <sys/types.h>

#include <arpa/inet.h>
#include <ifaddrs.h>
#include <net/ethernet.h>
#include <net/if_dl.h>
#include <net/if_types.h>
#include <netinet/in.h>
#include <netinet/ip6.h>
#include <netinet/icmp6.h>
#include <pcap/pcap.h>

#include <err.h>
#include <stdarg.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <syslog.h>
#include <unistd.h>

void usage(void)
{
	fprintf(stderr, "ndp6-pcap [-d] -i interface -p network/prefixlen...\n");
	exit(1);
}

/*
 * list of networks proxied by this programme
 */
struct network {
	struct in6_addr prefix;
	int             plen;
} *proxiednets;
int nbnet = 0;

/*
 * NA payload, always including the ether address of the proxying interface
 */
struct adv {
	struct nd_neighbor_advert	  na;
	struct nd_opt_hdr		  lladdr_opt;
	struct ether_addr		  lladdr;
	/* no padding: sizeof(nd_opt_hdr + ether_addr) % 8 == 0 */
} __packed adv;

struct ether_addr mylladdr;	/* link-layer address of the outside interface  */
uint32_t myscope;		/* link-local scope of the outside interface */
int sender;			/* socket for sending NA */

int debug = 0;

void trace(int level, char *message, ...);
uint16_t getscope(struct sockaddr_in6 *sin6);
int samenet(struct in6_addr *a, struct in6_addr *b, int plen);
int proxied(struct in6_addr *a);
void reply_ns(u_char *args, const struct pcap_pkthdr *pkt,
	      const u_char *frame);

void reply_ns(u_char *args,  const struct pcap_pkthdr *pkt,
	      const u_char *frame)
{
	int minsize = sizeof(struct ether_header) +
		sizeof(struct ip6_hdr) +
		sizeof(struct nd_neighbor_solicit);
	if (pkt->caplen < minsize) {
		trace(LOG_NOTICE, "received small packet: %d < %d\n",
		    pkt->caplen, minsize);
		return;
	}

	struct ip6_hdr *ns_packet;
	ns_packet = (struct ip6_hdr *)(frame +
				       sizeof(struct ether_header));

	if (! IN6_IS_ADDR_LINKLOCAL(&ns_packet->ip6_src)) {
		trace(LOG_NOTICE, "received packet from non link-local sender");
		return;
	}

	struct nd_neighbor_solicit *ns;
	ns = (struct nd_neighbor_solicit *)(frame + 
					    sizeof(struct ether_header) +
					    sizeof(struct ip6_hdr));

	if (debug) {
		char src[INET6_ADDRSTRLEN], tgt[INET6_ADDRSTRLEN];
		inet_ntop(AF_INET6, &ns_packet->ip6_src, src, INET6_ADDRSTRLEN);
		inet_ntop(AF_INET6, &ns->nd_ns_target, tgt, INET6_ADDRSTRLEN);
		trace(LOG_DEBUG, "received packet from %s, target %s", src, tgt);
	}

	if (! proxied(&ns->nd_ns_target)) {
		trace(LOG_DEBUG, "target address is not to be proxied");
		return;
	}

	/* our reply */
	bzero(&adv, sizeof(struct adv));
	adv.na.nd_na_type		= ND_NEIGHBOR_ADVERT;
	adv.na.nd_na_flags_reserved	= ND_NA_FLAG_SOLICITED;
	adv.na.nd_na_target		= ns->nd_ns_target;
	adv.lladdr_opt.nd_opt_type	= ND_OPT_TARGET_LINKADDR;
	adv.lladdr_opt.nd_opt_len	= 1;	/* sizeof lladdr in octets */
	adv.lladdr			= mylladdr;

	struct sockaddr_in6 dstsaddr;
	bzero(&dstsaddr, sizeof(struct sockaddr_in6));
	dstsaddr.sin6_family		= AF_INET6;
	dstsaddr.sin6_addr		= ns_packet->ip6_src;
	dstsaddr.sin6_scope_id		= myscope;

	if (sendto(sender, &adv, sizeof(struct adv), 0,
		   (struct sockaddr *)&dstsaddr, sizeof(struct sockaddr_in6)) < 0)
		trace(LOG_ERR, "error sending reply: %m");
	else
		trace(LOG_DEBUG, "reply sent");
}

uint8_t pl2m[8] = {
	0x00, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe
};
int samenet(struct in6_addr *a, struct in6_addr *b, int plen)
{
	if (memcmp(a, b, plen / 8))
		return 0;
	uint8_t m = pl2m[plen % 8];
	return (a->s6_addr[plen / 8] & m) == (b->s6_addr[plen / 8] & m);
}

/* returns 1 if the address given is one for which we proxy */
int proxied(struct in6_addr *a)
{
	for (int i = 0; ! IN6_IS_ADDR_UNSPECIFIED(&proxiednets[i].prefix); i++)
		if (samenet(a, &proxiednets[i].prefix, proxiednets[i].plen))
			return 1;
	return 0;
}

int main(int argc, char **argv)
{
	pcap_t *pcap;
	struct bpf_program filter;
	char errbuf[PCAP_ERRBUF_SIZE];
	char *intfname = NULL;

	int ch;
	while ((ch = getopt(argc, argv, "di:p:")) != -1) {
		switch (ch) {
		case 'd': {
			debug = 1;
			break;
		}
		case 'i': {
			intfname = optarg;
			break;
		}
		case 'p': {
			char *netname = strdup(optarg);
			char *plena = strchr(netname, '/');
			if (! plena)
				errx(1, "can't parse %s as a network name",
				     netname);
			*plena = '\0';
			plena++;
			nbnet++;
			proxiednets = realloc(proxiednets,
					      (nbnet+1) * sizeof(struct network));
			if (! proxiednets)
				err(1, "realloc(proxiednets, %d)", nbnet);
			trace(LOG_INFO, "will proxy for %s/%s (%d)",
			    netname, plena, nbnet);

			struct network *net = &proxiednets[nbnet - 1];
			if (inet_pton(AF_INET6, netname, &net->prefix) <= 0)
				err(1, "could not parse %s", netname);
			net->plen = atoi(plena);
			if (net->plen < 0 || net->plen > 128)
				err(1, "bad prefix length: %d", net->plen);

			free(netname);
			break;
		}
		default:
			warnx("unkown option -- %c", ch);
			usage();
			break;
		}
	}
	proxiednets[nbnet].prefix = in6addr_any;
	proxiednets[nbnet].plen   = 0;

	if (! intfname)
		errx(1, "no outside interface given");
	if (nbnet == 0)
		errx(1, "no network to be proxied given");

	if (! debug)
		setlogmask(LOG_UPTO(LOG_NOTICE));

	/* get scope id for LL address, and the link address */
	struct ifaddrs *headifa;
	int scopefound = 0;
	int lladdrfound = 0;
	if (getifaddrs(&headifa) < 0)
		err(1, "getifaddrs");
	for (struct ifaddrs *ifa = headifa; ifa; ifa = ifa->ifa_next) {
		if (strcmp(ifa->ifa_name, intfname))
			continue;
		if (! ifa->ifa_addr)
			continue;
		if (ifa->ifa_addr->sa_family == AF_INET6) {
			struct sockaddr_in6 *sin6 =
				(struct sockaddr_in6 *)ifa->ifa_addr;
			if (! IN6_IS_ADDR_LINKLOCAL(&sin6->sin6_addr))
				continue;
			myscope = getscope(sin6);
			scopefound = 1;
			trace(LOG_DEBUG, "scope for %s is %d", intfname, myscope);
		} else if (ifa->ifa_addr->sa_family == AF_LINK) {
			struct sockaddr_dl *sdl =
				(struct sockaddr_dl *)ifa->ifa_addr;
			if (sdl->sdl_type != IFT_ETHER)
				continue;
			mylladdr = *((struct ether_addr *) LLADDR(sdl));
			lladdrfound = 1;
			trace(LOG_DEBUG, "ethernet address for %s is %s",
			    intfname, ether_ntoa(&mylladdr));
		}
	}
	freeifaddrs(headifa);

	if (! lladdrfound || ! scopefound)
		errx(1, "could not find scope and link address for %s",
			intfname);

	/* setup the sending socket for the NA replies */
	if ((sender = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6)) < 0)
		err(1, "creating socket");
	if (shutdown(sender, SHUT_RD) < 0)
		err(1, "half-closing the sending socket");
	/* the NA we send are link-local */
	int  hoplimit = 255;
	if (setsockopt(sender, IPPROTO_IPV6, IPV6_UNICAST_HOPS,
		       (char *) &hoplimit, sizeof(hoplimit)) == -1)
		err(1, "set hoplimit");

	/* setup pcap */
	if (! (pcap = pcap_open_live(intfname, BUFSIZ, 1, 1, errbuf)))
		errx(1, "pcap_open_live: %s", errbuf);

	/* pcap filter. we're looking at an offset of the ip6 header because
	 * pcap doesn't support looking at the icmp6 type */
	char *filter_exp;
	asprintf(&filter_exp, "icmp6 and ip6[%zd] = %d",
		 sizeof(struct ip6_hdr) + offsetof(struct icmp6_hdr, icmp6_type),
		 ND_NEIGHBOR_SOLICIT);
	if (! filter_exp)
		err(1, "creating filter");
	if (pcap_compile(pcap, &filter, filter_exp, 1, 0) < 0)
		errx(1, "pcap_compile: %s", pcap_geterr(pcap));
	if (pcap_setfilter(pcap, &filter) < 0)
		errx(1, "pcap_setfilter: %s", pcap_geterr(pcap));

	/* fork and start pcap's event loop */
	if (!debug && daemon(0, 0) < 0)
			err(1, "daemon");
	pcap_loop(pcap, -1, reply_ns, NULL);

	/* not reached */
	return 0;
}

/*
 * XXX: taken from ifconfig: FreeBSD doesn't return the scope id in the
 * scope_id field, but embedded in the address itself 
 *
 * fixed in http://svnweb.freebsd.org/base?view=revision&revision=243187
 */
uint16_t getscope(struct sockaddr_in6 *sin6)
{
	if (IN6_IS_ADDR_LINKLOCAL(&sin6->sin6_addr) &&
	    sin6->sin6_scope_id == 0) {
		sin6->sin6_scope_id =
			ntohs(*(uint16_t *)&sin6->sin6_addr.s6_addr[2]);
		sin6->sin6_addr.s6_addr[2] = sin6->sin6_addr.s6_addr[3] = 0;
	}
	return sin6->sin6_scope_id;
}

void trace(int priority, char *message, ...)
{
	va_list argp;
	va_start(argp, message);
	if (debug) {
		vfprintf(stderr, message, argp);
		fprintf(stderr, "\n");
	} else {
		vsyslog(priority, message, argp);
	}
	va_end(argp);
}