tuntest-icmp

tuntest.c

/*
iptables -t mangle -A PREROUTING -p icmp  -j MARK --set-mark 0x15
iptables -t mangle -A PREROUTING -p udp  -j MARK --set-mark 0x15
iptables -t mangle -A PREROUTING -p tcp  -j MARK --set-mark 0x15

echo 1 > /proc/sys/net/ipv4/ip_forward

ip rule add fwmark 0x15 lookup 100
ip route add 0.0.0.0/0 dev tun0 table 100


./tuntest
ip link set tun0 up

 */

#include <assert.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <getopt.h>
#include <string.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <malloc.h>
#include <net/ethernet.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <time.h>
#include <sys/ioctl.h>
#include <linux/if.h>
#include <linux/if_tun.h>
#include <signal.h>
#include <event.h>
#include <err.h>



struct event_base *base;
struct event *icmp_in_ev;

struct link_t {
	int link_fd;
	int icmp_fd;
	int raw_fd;
};

int set_nonblock(int fd)
{
	int flags;

	flags = fcntl(fd, F_GETFL);
	if (flags < 0) return flags;
	
	flags |= O_NONBLOCK;
	
	if (fcntl(fd, F_SETFL, flags) < 0) return -1;
        return 0;
}


unsigned short checksum(void *b, int len)
{	unsigned short *buf = b;
	unsigned int sum=0;
	unsigned short result;

	for ( sum = 0; len > 1; len -= 2 )
		sum += *buf++;
	if ( len == 1 )
		sum += *(unsigned char*)buf;
	sum = (sum >> 16) + (sum & 0xFFFF);
	sum += (sum >> 16);
	result = ~sum;
	return result;
}

void icmp_handler(int fd, short flags, void *arg)
{
	struct link_t *link = (struct link_t *)arg;
	uint8_t buf[2048];
	int ret;

	ret = read(fd, buf, 2048);

	/*
	for (int i = 0; i < ret; i++ ) {
		fprintf(stderr, "%02x ", (uint8_t)buf[i]);
		if (!((i+1) % 16)) printf("\n");
	}

	fprintf(stderr, "\noldlen %d\n", ret);
	*/
	
	uint32_t swap = *(uint32_t *)((void *)buf + 12);
	*(uint32_t *)((void *)buf + 12) = *(uint32_t *)((void *)buf + 16);

	*(uint32_t *)((void *)buf + 16) = swap;

	buf[10] = 0x00;
	buf[11] = 0x00;
	*(uint16_t *)((void *)buf + 10) = checksum((void *)buf, 20);
	
/*
	for (int i = 0; i < ret; i++ ) {
		fprintf(stderr, "%02x ", (uint8_t)buf[i]);
		if (!((i+1) % 16)) printf("\n");
	}

	fprintf(stderr, "\nnew len %d\n", ret);
*/
//	buf[20] = 0x11;
//	buf[21] = 0x00;
	buf[22] = 0x00;
	buf[23] = 0x00;

	*(uint16_t *)((void *)buf + 22) = checksum((void *)buf + 20, ret - 20);

	write(fd, buf, ret);
	
/*
	

	for (int i = 0; i < ret; i++ ) {
		fprintf(stderr, "%02x ", (uint8_t)buf[i]);
		if (!((i+1) % 16)) printf("\n");
	}

	fprintf(stderr, "\nnew len %d\n", ret);
*/
	/*
	struct sockaddr_in dst_addr;
	
	dst_addr.sin_family = AF_INET;
	dst_addr.sin_port = *(uint16_t *)(((void *)buf) + 5 * 4 + 2);
	dst_addr.sin_addr.s_addr = *(uint32_t *)(((void *)buf) + 4 * 4);
	fprintf(stderr, "s_addr 0x%08x\n", dst_addr.sin_addr.s_addr);

	ret = sendto(link->raw_fd, buf, ret, 0, (struct  sockaddr *)&dst_addr,sizeof(dst_addr));
	if (ret <= 0) {
		fprintf(stderr, "sendto failed\n");
	}
	*/
		
}
	
int tun_create(char *dev, int flags)
{
    struct ifreq ifr;
    int fd, err;

    assert(dev != NULL);

    if ((fd = open("/dev/net/tun", O_RDWR)) < 0)
        return fd;

    memset(&ifr, 0, sizeof(ifr));
    ifr.ifr_flags |= flags;
    if (*dev != '\0')
        strncpy(ifr.ifr_name, dev, IFNAMSIZ);
    if ((err = ioctl(fd, TUNSETIFF, (void *)&ifr)) < 0) {
        close(fd);
        return err;
    }
    strcpy(dev, ifr.ifr_name);

    return fd;
}


void accept_handler(int fd, short flags, void *arg)
{
	struct link_t *link = (struct link_t *)arg;
	int link_fd;

	struct sockaddr_in link_addr;
	socklen_t link_len;

	link_len = sizeof(link_addr);
	
	memset(link, 0, sizeof(struct link_t));

	memset(&link_addr, 0, link_len);

        link_fd = accept(fd, (struct sockaddr *)&link_addr, &link_len);

	if (link_fd < 0) {
		perror("accept");
	}

	if (set_nonblock(link_fd) < 0) {
		perror("set_nonblock");
	}

	link->link_fd = link_fd;
}

void create_service(struct event_base *base)
{
	struct sockaddr_in ser_addr;
        socklen_t ser_len;
        int ser_fd;
        struct event ser_ev;

        int on = 1;

        ser_len = sizeof(ser_addr);
        memset(&ser_addr, 0, ser_len);
        ser_addr.sin_family = AF_INET;
        ser_addr.sin_port = htons(8888);
        ser_addr.sin_addr.s_addr = INADDR_ANY;

	ser_fd = socket(AF_INET, SOCK_STREAM, 0);
	
	if (setsockopt(ser_fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) == -1)
                err(1, "setsockopt SO_REUSEADDR failed");

	if (bind(ser_fd, (struct sockaddr *)&ser_addr, ser_len) < 0)
                err(1, "bind failed");

        if (listen(ser_fd, 1024) < 0) err(1, "listen failed");

	if (set_nonblock(ser_fd) < 0)
                err(1, "failed to set server socket to non-blocking");

}

int main(int argc, char *argv[])
{
        int tun, ret, socksd;
	
        char tun_name[IFNAMSIZ];

	int opt = 1;
	
	struct link_t *link = malloc(sizeof(struct link_t));
	
	tun_name[0] = '\0';
	tun = tun_create(tun_name, IFF_TUN | IFF_NO_PI);
        if (tun < 0) {
                perror("tun_create");
                return 1;
        }
        printf("TUN name is %s\n", tun_name);

	if ((socksd = socket (PF_INET, SOCK_RAW, IPPROTO_RAW)) < 0) {
		perror ("socket() failed to get socket descriptor for using ioctl() ");
	}

	if(setsockopt(socksd, IPPROTO_IP, IP_HDRINCL, &opt, sizeof(opt)) < 0)
	{
		perror("setsockopt() for IP_HDRINCL error");
		exit(1);
	}

	link->icmp_fd = tun;
	link->raw_fd = socksd;
	

	base = event_base_new();
	
	icmp_in_ev = event_new(base, tun, EV_READ | EV_PERSIST,
			       icmp_handler, link);

	event_add(icmp_in_ev, NULL);

	event_base_loop(base, 0);

	event_free(icmp_in_ev);
	free(link);


        return 0;
}