/* 
 * Copyright (c) 2010 Peter J. Philipp
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * 
 */

#include <sys/param.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/stat.h>

#include <net/if.h>
#include <net/if_dl.h>
#include <net/if_types.h>
#include <net/bpf.h>
#include <net/ethertypes.h>

#include <netinet/in.h>
#define _KERNEL 1
#include <netinet/ip6.h>
#undef _KERNEL
#include <netinet/icmp6.h>
#include <netinet/if_ether.h>

#include <arpa/inet.h>

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <ifaddrs.h>
#include <errno.h>
#include <syslog.h>
#include <time.h>

void reply_icmp(char *ip6, int len, char *source, int);
int open_filter(char *interface);
int ip_cksum(u_int16_t *p, int len);

static struct ether_addr *src, *dst;
char *interface = "em0";

int
main(int argc, char *argv[])
{
	time_t now;
	int len, fd;
	int ds, sslen = sizeof(struct sockaddr_in6);
	struct sockaddr_in6 sin6;
	char buf[2048];
	char clock[512];
	char abuf[INET6_ADDRSTRLEN];
	u_int8_t version;
	struct ip6_hdr *ip6;


	if (argc != 4) {
		/* as in faketrace6 em0 00:0c:29:18:78:88 00:0c:29:6d:84:5c */
		fprintf(stderr, "usage: faketrace6 [interface] [mymac] [mygatewaymac]\n");
		exit(1);
	}
		
	interface = argv[1];
	src = ether_aton(argv[2]);
	dst = ether_aton(argv[3]);

	openlog("faketrace6", LOG_PID | LOG_NDELAY, LOG_DAEMON);

	syslog(LOG_INFO, "faketrace6 starting up");
	
	ds = socket(AF_INET6, SOCK_RAW, IPPROTO_DIVERT);
	if (ds < 0) {
		perror("socket");
		exit(1);
	}

	memset(&sin6, 0, sizeof(sin6));	
	sin6.sin6_family = AF_INET6;
	sin6.sin6_port = htons(9999);

	if (bind(ds, (struct sockaddr *)&sin6, sizeof(sin6)) < 0) {
		perror("bind");
		exit(1);
	}

	fd = open_filter(interface);
	if (fd < 0) {
		perror("open_filter");
		exit(1);	
	}

#if 1
	daemon(0,0);
#endif

	for (;;) {
		len = recvfrom(ds, buf, sizeof(buf), 0, (struct sockaddr *)&sin6, &sslen);		
	
		if (len < 0) {
			perror("read");
			continue;
		}	

		if (len < sizeof(struct ip6_hdr))
			continue;

		ip6 = (struct ip6_hdr *)&buf[0];
		
		version = (ip6->ip6_vfc >> 4);

		if (version != 6)
			continue;
	
		if (ip6->ip6_nxt != IPPROTO_UDP)
			goto skip;

		switch (ip6->ip6_hops) {
		case 1:
			/* hello */
			reply_icmp((char *)ip6, len, "2001:a60:f074:ff::1", fd);
			break;
		case 2:
			reply_icmp((char *)ip6, len, "2001:a60:f074:ff::2", fd);
			/* why */
			break;
		case 3:
			inet_ntop(AF_INET6, (char *)&ip6->ip6_src, (char *)&abuf, sizeof(abuf));
			syslog(LOG_INFO, "possible traceroute6 from %s", abuf);
			reply_icmp((char *)ip6, len, "2001:a60:f074:ff::3", fd);
			/* are */
			break;
		case 4:
			reply_icmp((char *)ip6, len, "2001:a60:f074:ff::4", fd);
			/* you */
			break;
		case 5:
			reply_icmp((char *)ip6, len, "2001:a60:f074:ff::5", fd);
			/* tracerouting */
			break;
		case 6:

			now = time(NULL);	
			now %= 86400;

			snprintf(clock, sizeof(clock), "2001:a60:f074:fe::%x:%x", (now >> 16) & 0xffff, now & 0xffff);
			
			/* clock */
			reply_icmp((char *)ip6, len, clock , fd);
			break;
		default:
			break;
		}

#if 0

		printf("--------------------------------------------------------------------\n");
		printf("VERSION: %d\n", version);
		inet_ntop(AF_INET6, (char *)&ip6->ip6_src, (char *)&abuf, sizeof(abuf));
		printf("SOURCE: %s\n", abuf);
		inet_ntop(AF_INET6, (char *)&ip6->ip6_dst, (char *)&abuf, sizeof(abuf));
		printf("DEST: %s\n", abuf);	
		printf("HOPS: %d\n", ip6->ip6_hops);
		printf("NEXT HEADER: %s\n", (ip6->ip6_nxt == IPPROTO_ICMPV6) ? "ICMP" : (ip6->ip6_nxt == IPPROTO_TCP) ? "TCP" : (ip6->ip6_nxt == IPPROTO_UDP) ? "UDP" : "unknown");

		for (i = 0; i < len ; i++) {
			if (i && i % 16 == 0)
				printf("\n");

			c = buf[i];

			printf("%02X", c);
		}	
			
		printf("\n");
		printf("--------------------------------------------------------------------\n");

#endif
		if (ip6->ip6_hops < 6)
			continue;
skip:
		if (sendto(ds, buf, len, 0, (struct sockaddr*)&sin6, sslen) < 0) {
			perror("write");
		}
	} /* for(;;) */
}

void 
reply_icmp(char *ip6h, int len, char *source, int fd)
{
	struct ether_header *eh;
	struct in6_addr ia6;
	struct ip6_hdr *rip6, *ip6;
	struct icmp6_hdr *icmp6;
	struct ip6_hdr_pseudo *pseudo;
	char buf[2048];
	char csum[2048];
	char *payload;

	inet_pton(AF_INET6, source, &ia6);	
	printf("replying from %s\n", source);

	memset(&buf, 0, sizeof(buf));
	eh = (struct ether_header *)&buf[0];
	memcpy(eh->ether_shost, (char *)src, 6);
	memcpy(eh->ether_dhost, (char *)dst, 6);


	eh->ether_type = htons(ETHERTYPE_IPV6); /* IPv6 protocol */

	rip6 = (struct ip6_hdr *)&buf[sizeof(struct ether_header)];
	ip6 = (struct ip6_hdr *)ip6h;
		
	rip6->ip6_vfc = (6 << 4);
	memcpy((char *)&rip6->ip6_src, (char *)&ia6, sizeof(struct in6_addr));
	memcpy((char *)&rip6->ip6_dst, (char *)&ip6->ip6_src, sizeof(struct in6_addr));
	rip6->ip6_plen = htons(sizeof(struct icmp6_hdr) + len);
	rip6->ip6_hops = 0xff;
	rip6->ip6_nxt = IPPROTO_ICMPV6;
	
	icmp6 = (struct icmp6_hdr *)&buf[sizeof(struct ether_header) + sizeof(struct ip6_hdr)];

	icmp6->icmp6_type = ICMP6_TIME_EXCEEDED;
	icmp6->icmp6_code = 0;
	icmp6->icmp6_cksum = 0; 	/* later */
	
	payload = (char *)&buf[sizeof(struct ether_header) + sizeof(struct ip6_hdr) + sizeof(struct icmp6_hdr)];
	memcpy(payload, ip6h, len);

	/* now we checksum */

	pseudo = (struct ip6_hdr_pseudo *)&csum[0];
	pseudo->ip6ph_nxt = IPPROTO_ICMPV6;
	pseudo->ip6ph_len = htons(sizeof(struct icmp6_hdr) + len);
	memcpy((char *)&pseudo->ip6ph_src, (char *)&rip6->ip6_src, sizeof(struct in6_addr));
	memcpy((char *)&pseudo->ip6ph_dst, (char *)&rip6->ip6_dst, sizeof(struct in6_addr));

	memcpy((char *)&csum[sizeof(struct ip6_hdr_pseudo)], 
		icmp6, sizeof(struct icmp6_hdr) + len);

	icmp6->icmp6_cksum = ip_cksum((u_int16_t *)&csum[0], sizeof(struct ip6_hdr_pseudo) + 
						sizeof(struct icmp6_hdr) + 
						len);
	write(fd, buf, sizeof(struct ether_header) + len + sizeof(struct ip6_hdr) + sizeof(struct icmp6_hdr));

	return;
}

int
open_filter(char *interface)
{
        struct ifreq ifr;
        char buf[PATH_MAX];
        int i = 0, fd;
        u_int hdrcomplete, dltype;

        do {
                snprintf(buf, sizeof(buf), "/dev/bpf%d", i++);
                fd = open(buf, O_RDWR, 0);
        } while (fd < 0 && errno == EBUSY);

        if (fd < 0) {
                perror("open");
                return -1;
        }
#if 0
        /* set maximum buffer length to something high */
        blen = 32768;
        if (ioctl(fd, BIOCSBLEN, (u_int)&blen) < 0) {
                perror("ioctl 1");
                close (fd);
                return -1;
        }
#endif
        /* set interface on bpf */
        memset(&ifr, 0, sizeof(ifr));
        strncpy(ifr.ifr_name, interface, sizeof(ifr.ifr_name) - 1);
        if (ioctl(fd, BIOCSETIF, &ifr) < 0) {
                perror("ioctl 2");
                close (fd);
                return -1;
        }
        /* write complete frame headers */
        hdrcomplete=1;
        if (ioctl(fd, BIOCSHDRCMPLT, &hdrcomplete) < 0) {
                perror("ioctl 3");
                return -1;
        }
        /* 
         * If we're not ethernet return with -1 as there is no point opening
         * bpf for a utility that is a ethernet spoofer
         */
        if (ioctl(fd, BIOCGDLT, &dltype) < 0) {
                perror("ioctl 4");
                return -1;
        }
        if (dltype == DLT_EN10MB) 
                return (fd);

        fprintf(stderr, "dltype != DLT_EN10MB, missing -l flag?\n");

        errno = ENOSYS;
        close (fd);
        return -1;
}

/*
 * IP_CKSUM - compute the ones complement sum of the ones complement of 16 bit 
 *                        numbers
 */

int
ip_cksum(u_int16_t *p, int len)
{
        register int nleft = len;
        register u_int16_t *w = p;
        register int sum = 0;
        u_int16_t answer;
        
        while (nleft > 1) {
                sum += *w++;
                nleft -= 2;
        }

        if (nleft == 1) {
                *(u_char *)(&answer) =  *(u_char *)w;
                sum += answer;
        }

        sum = (sum >> 16) + (sum & 0xffff);
        sum += (sum >> 16);

        answer = ~sum;
        return (answer);
}