ipdivert/mirror.c

/*
 * Copyright 2015-2018 Rivoreo
 *
 * 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.
 */

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/tcp.h>
#include <netinet/udp.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>

static void print_from_to(FILE *f, const struct ip *iph, int iph_len) {
	char buffer[16];
	uint16_t src_port = 0, dst_port = 0;
	switch(iph->ip_p) {
		case IPPROTO_TCP:
			{
				struct tcphdr *tcph = (struct tcphdr *)((char *)iph + iph_len);
				src_port = tcph->th_sport;
				dst_port = tcph->th_dport;
			}
			break;
		case IPPROTO_UDP:
			{
				struct udphdr *udph = (struct udphdr *)((char *)iph + iph_len);
				src_port = udph->uh_sport;
				dst_port = udph->uh_dport;
			}
	}
	if(!inet_ntop(AF_INET, &iph->ip_src, buffer, sizeof buffer)) strcpy(buffer, "INVALID");
	fputs(buffer, f);
	if(src_port) fprintf(f, ":%u", (unsigned int)ntohs(src_port));
	fputs(" -> ", stderr);
	if(!inet_ntop(AF_INET, &iph->ip_dst, buffer, sizeof buffer)) strcpy(buffer, "INVALID");
	fputs(buffer, f);
	if(dst_port) fprintf(f, ":%u", (unsigned int)ntohs(dst_port));
	fputc('\n', stderr);

}

int main(int argc, char **argv) {
	if(argc < 2) {
		fprintf(stderr, "Usage: %s <divert-port>\n", argv[0]);
		return -1;
	}
	int divert_port = atoi(argv[1]);
	if(divert_port < 1) {
		fprintf(stderr, "%s: Port number must be greater than 0\n", argv[0]);
		return -1;
	}
	unsigned char buffer[16384];
	struct sigaction act = { .sa_handler = SIG_IGN };
	if(sigaction(SIGPIPE, &act, NULL) < 0) {
		perror("sigaction");
		return 1;
	}
	setvbuf(stdout, NULL, _IOLBF, 0);
	setvbuf(stderr, NULL, _IONBF, 0);
	int fd = socket(AF_INET, SOCK_RAW, IPPROTO_DIVERT);
	if(fd == -1) {
		perror("socket: AF_INET,SOCK_RAW,IPPROTO_DIVERT");
		return 1;
	}
	struct sockaddr_in listen_addr = {
		.sin_family = AF_INET,
		.sin_port = htons(divert_port)
	};
	if(bind(fd, (struct sockaddr *)&listen_addr, sizeof listen_addr) < 0) {
		perror("bind");
		return 1;
	}
	struct sockaddr source_addr;
	socklen_t source_addr_len;
	int s;
	while(1) {
		source_addr_len = sizeof source_addr;
		s = recvfrom(fd, buffer, sizeof buffer, 0, &source_addr, &source_addr_len);
		if(s < 0) {
			if(errno == EINTR) continue;
			perror("recvfrom");
			usleep(10000);
			continue;
		}
		struct ip *iph = (struct ip *)buffer;
		int iph_len = iph->ip_hl << 2;
		if(iph_len < 0 || iph_len > s) continue;
		if((iph->ip_src.s_addr & 0xffffff) == 0x7f || (iph->ip_dst.s_addr & 0xffffff) == 0x7f) goto put_back;
		print_from_to(stderr, iph, iph_len);

		struct in_addr tmp_addr = iph->ip_src;
		iph->ip_src = iph->ip_dst;
		iph->ip_dst = tmp_addr;
put_back:
		if(sendto(fd, buffer, s, 0, &source_addr, source_addr_len) < 0) {
			perror("sendto");
		}
	}
}