Divert6.c

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip6.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <string.h>
#include <err.h>

#define DIVERT_PORT 700

int
main(int argc, char *argv[])
{
	int fd, s;
	struct sockaddr_in6 sin;
	socklen_t sin_len;

	fd = socket(AF_INET6, SOCK_RAW, IPPROTO_DIVERT);
	if (fd == -1)
		err(1, "socket");

	memset(&sin, 0, sizeof(sin));
	sin.sin6_family = AF_INET6;
	sin.sin6_port = htons(DIVERT_PORT);
	sin.sin6_addr = in6addr_any;

	sin_len = sizeof(struct sockaddr_in6);

	s = bind(fd, (struct sockaddr *) &sin, sin_len);
	if (s == -1)
		err(1, "bind");

	for (;;) {
		ssize_t n;
		char packet[65535];
		struct ip6_hdr *ip;
		struct tcphdr *th;
		int hlen;
		char src[48], dst[48];

		memset(packet, 0, sizeof(packet));
		n = recvfrom(fd, packet, sizeof(packet), 0,
		    (struct sockaddr *) &sin, &sin_len);
		if (n == -1) {
			warn("recvfrom");
			continue;
		}
		if (n < sizeof(struct ip6_hdr)) {
			warnx("packet is too short");
			continue;
		}

		ip = (struct ip6_hdr *) packet;
		hlen = sizeof(struct ip6_hdr);
//		if (hlen < sizeof(struct ip6_hdr) || ntohs(ip->ip_len) < hlen ||
//		    n < ntohs(ip->ip_len)) {
//			warnx("invalid IPv6 packet");
//			continue;
//		}

		th = (struct tcphdr *) (packet + hlen);

		if (inet_ntop(AF_INET6, &ip->ip6_src, src,
		    sizeof(src)) == NULL)
			(void)strlcpy(src, "?", sizeof(src));

		if (inet_ntop(AF_INET6, &ip->ip6_dst, dst,
		    sizeof(dst)) == NULL)
			(void)strlcpy(dst, "?", sizeof(dst));

		printf("%s:%u -> %s:%u\n",
		    src,
		    ntohs(th->th_sport),
		    dst,
		    ntohs(th->th_dport)
		);

		n = sendto(fd, packet, n, 0, (struct sockaddr *) &sin,
		    sin_len);
		if (n == -1)
			warn("sendto");
	}

	return 0;
}