From 09c43b4598f40f2bf3ebfe7593f9ed10502b3574 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Philipp=20T=C3=B6lke?= Date: Mon, 28 Jun 2010 12:37:36 +0000 Subject: [PATCH] parse ipv6-packets for the vpn-functionality --- src/vpn/Makefile | 10 +- src/vpn/debug.c | 13 +++ src/vpn/debug.h | 7 ++ src/vpn/packet.c | 235 +++++++++++++++++++++++++++++++++++++++++++++++ src/vpn/packet.h | 29 ++++++ src/vpn/test.c | 25 +++++ src/vpn/tun.c | 39 +++----- src/vpn/tun.h | 11 +++ 8 files changed, 339 insertions(+), 30 deletions(-) create mode 100644 src/vpn/debug.c create mode 100644 src/vpn/debug.h create mode 100644 src/vpn/packet.c create mode 100644 src/vpn/packet.h create mode 100644 src/vpn/test.c create mode 100644 src/vpn/tun.h diff --git a/src/vpn/Makefile b/src/vpn/Makefile index ac8633c65..848faf24d 100644 --- a/src/vpn/Makefile +++ b/src/vpn/Makefile @@ -1,16 +1,14 @@ -CFLAGS=-Wall -pedantic +CFLAGS=-Wall -pedantic --std=c99 -g CXXFLAGS = ${CFLAGS} LDFLAGS = all:default -default: tun +default: test -tun.o: tun.c - -tun: tun.o +test: test.o tun.o packet.o debug.o clean: rm -f *.o - rm -f tun + rm -f test diff --git a/src/vpn/debug.c b/src/vpn/debug.c new file mode 100644 index 000000000..900a4bd00 --- /dev/null +++ b/src/vpn/debug.c @@ -0,0 +1,13 @@ +#include +#include +#include + +#include "debug.h" + +void debug(int lvl, int es, char* msg, ...) { + va_list ap; + va_start(ap, msg); + vprintf(msg, ap); + va_end(ap); + if (es != 0) exit(es); +} diff --git a/src/vpn/debug.h b/src/vpn/debug.h new file mode 100644 index 000000000..8f3e71813 --- /dev/null +++ b/src/vpn/debug.h @@ -0,0 +1,7 @@ +#ifndef _GNTUN_DEBUG_H_ +#define _GNTUN_DEBUG_H_ + +/* exits with status exit if != 0; */ +extern void debug(int lvl, int exit, char* msg, ...); + +#endif diff --git a/src/vpn/packet.c b/src/vpn/packet.c new file mode 100644 index 000000000..c4c7dc290 --- /dev/null +++ b/src/vpn/packet.c @@ -0,0 +1,235 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include "debug.h" +#include "packet.h" + +static long payload(struct ip6_pkt* pkt) { + return (pkt->paylgth[0] << 8) + pkt->paylgth[1]; +} + +static char* pretty = /*{{{*/ +/* 0 1 2 3 4 5 6 + 0123456789012345678901234567890123456789012345678901234567890123456789 */ +"IPv6-Paket from xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx \n" //60 +" to xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx \n" //120 +" flow 0xXXX ( ) \n" //180 +" length 0xXX ( ) \n" //240 +" nexthdr 0xXX ( \n" //300 +" hoplmt 0xXX ( ) \n" //360 +"first 128 bytes of payload: \n" //420 +/* 0 1 2 3 4 5 6 + 0123456789012345678901234567890123456789012345678901234567890123456789 */ +"XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX | ................ \n" //490 +"XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX | ................ \n" //560 +"XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX | ................ \n" //630 +"XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX | ................ \n" //700 +"XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX | ................ \n" //770 +"XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX | ................ \n" //840 +"XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX | ................ \n" //910 +"XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX XX | ................ \n";//980 +/*}}}*/ + +void send_pkt(int fd, struct ip6_pkt* pkt) {{{ + int sz = payload(pkt); + int w = 0; + char* buf = (char*)malloc(sz+40); + + buf[0] = (6 << 4) | (pkt->tclass >> 4); + buf[1] = (pkt->tclass << 4) | (pkt->flowlbl[0] >> 4); + buf[2] = pkt->flowlbl[1]; + buf[3] = pkt->flowlbl[2]; + buf[4] = pkt->paylgth[0]; + buf[5] = pkt->paylgth[1]; + buf[6] = pkt->nxthdr; + buf[7] = pkt->hoplmt; + + for (w = 0; w < 16; w++) { + buf[8+w] = pkt->sadr[w]; + buf[24+w] = pkt->dadr[w]; + } + + memcpy(buf+40, pkt->data, sz); + + w = 0; + while ( w > 0) { + int t = write(fd, buf+w, (sz + 40) - w); + if (t < 0) + debug(1, 0, "packet: write : %s\n", strerror(errno)); + else + w+=t; + } + + free(buf); +}}} + +int recv_ipv6pkt(int fd, struct pkt_tun** pkt, unsigned char* data) {{{ + int size = (data[4] << 8) + data[5] + 40; + + debug(1, 0, "read the size: %d\n", size); + + (*pkt)->data = (unsigned char*)malloc(size); + + memcpy((*pkt)->data, data, size); + + return size; +}}} + +int recv_pkt(int fd, struct pkt_tun** pkt) {{{ + struct pkt_tun* _pkt = (struct pkt_tun*)malloc(sizeof(struct pkt_tun)); + *pkt = _pkt; + + unsigned char data[1500]; + unsigned char buf[4]; + + struct iovec vect[2]; + vect[0].iov_len = sizeof(struct tun_pi); + vect[0].iov_base = &buf; + vect[1].iov_len = 1500; + vect[1].iov_base = data; + + int r = 0; + + debug(1, 0, "beginning to read...\n"); + + r = readv(fd, vect, 2); + + _pkt->flags[0] = buf[0]; + _pkt->flags[1] = buf[1]; + _pkt->type[0] = buf[2]; + _pkt->type[1] = buf[3]; + + debug(1, 0, "read the flags: %02x%02x\n", _pkt->flags[0], _pkt->flags[1]); + debug(1, 0, "read the type: %02x%02x\n", _pkt->type[0], _pkt->type[1]); + + switch((_pkt->type[0] << 8) + _pkt->type[1]) { + case 0x86dd: + return recv_ipv6pkt(fd, pkt, data); + break; + case 0x0800: + debug(1, 0, "unknown pkt-type: IPv4\n"); + //IPv4 TODO + break; + default: + debug(1, 0, "unknown pkt-type: 0x%02x\n", 0x800); + //Whatever TODO + break; + } + return -1; +}}} + +struct ip6_pkt* parse_ip6(struct pkt_tun* pkt) {{{ + struct ip6_pkt* pkt6 = (struct ip6_pkt*)malloc(sizeof(struct ip6_pkt)); + + pkt6->tclass = pkt->data[0] << 4 | pkt->data[1] >> 4; + pkt6->flowlbl[0] = pkt->data[1]>>4; + pkt6->flowlbl[1] = pkt->data[2]; + pkt6->flowlbl[2] = pkt->data[3]; + + pkt6->paylgth[0] = pkt->data[4]; + pkt6->paylgth[1] = pkt->data[5]; + + pkt6->nxthdr = pkt->data[6]; + pkt6->hoplmt = pkt->data[7]; + + for (int w = 0; w < 16; w++) { + pkt6->sadr[w] = pkt->data[8+w]; + pkt6->dadr[w] = pkt->data[24+w]; + } + + pkt6->data = (unsigned char*)malloc(payload(pkt6)); + memcpy(pkt6->data, pkt->data+40, payload(pkt6)); + + return pkt6; +}}} + +static void pp_ip6adr(unsigned char* adr, char* dest) {{{ + char tmp[3]; + + sprintf(tmp, "%02X", adr[0]); + memcpy(dest+0, tmp, 2); + sprintf(tmp, "%02X", adr[1]); + memcpy(dest+2, tmp, 2); + + sprintf(tmp, "%02X", adr[2]); + memcpy(dest+5, tmp, 2); + sprintf(tmp, "%02X", adr[3]); + memcpy(dest+7, tmp, 2); + + sprintf(tmp, "%02X", adr[4]); + memcpy(dest+10, tmp, 2); + sprintf(tmp, "%02X", adr[5]); + memcpy(dest+12, tmp, 2); + + sprintf(tmp, "%02X", adr[6]); + memcpy(dest+15, tmp, 2); + sprintf(tmp, "%02X", adr[7]); + memcpy(dest+17, tmp, 2); + + sprintf(tmp, "%02X", adr[8]); + memcpy(dest+20, tmp, 2); + sprintf(tmp, "%02X", adr[9]); + memcpy(dest+22, tmp, 2); + + sprintf(tmp, "%02X", adr[10]); + memcpy(dest+25, tmp, 2); + sprintf(tmp, "%02X", adr[11]); + memcpy(dest+27, tmp, 2); + + sprintf(tmp, "%02X", adr[12]); + memcpy(dest+30, tmp, 2); + sprintf(tmp, "%02X", adr[13]); + memcpy(dest+32, tmp, 2); + + sprintf(tmp, "%02X", adr[14]); + memcpy(dest+35, tmp, 2); + sprintf(tmp, "%02X", adr[15]); + memcpy(dest+37, tmp, 2); +}}} + +void pp_hexdump(unsigned char* data, char* dest, int max) { + char tmp[3]; + int to = max > 8 ? 8 : max; + for (int i = 0; i < to; i++) { + sprintf(tmp, "%02x", data[i]); + memcpy(dest+(3*i), tmp, 2); + } +} + +void pkt_printf(struct ip6_pkt* pkt) { + char* buf = (char*)malloc(strlen(pretty)+1); + char tmp[4]; + + memcpy(buf, pretty, strlen(pretty)+1); + + pp_ip6adr(pkt->sadr, buf+16); + pp_ip6adr(pkt->dadr, buf+76); + + sprintf(tmp, "%03x", (pkt->flowlbl[0] << 16) + (pkt->flowlbl[1] << 8) + (pkt->flowlbl[2])); + memcpy(buf+138, tmp, 3); + + sprintf(tmp, "%02x", (pkt->paylgth[0] << 8) + (pkt->paylgth[1])); + memcpy(buf+198, tmp, 2); + + sprintf(tmp, "%02x", pkt->nxthdr); + memcpy(buf+258, tmp, 2); + + sprintf(tmp, "%02x", pkt->hoplmt); + memcpy(buf+318, tmp, 2); + + int size = payload(pkt); + for(int i = 0; i < 8; i++) { + if (16*i > size) break; + pp_hexdump(pkt->data + (16*i), buf + 420 + (i*70), size - 16*i); + pp_hexdump(pkt->data + (16*i) + 8, buf + 445 + (i*70), size - (16*i + 8)); + } + + printf(buf); + free(buf); +} diff --git a/src/vpn/packet.h b/src/vpn/packet.h new file mode 100644 index 000000000..9d5153f13 --- /dev/null +++ b/src/vpn/packet.h @@ -0,0 +1,29 @@ +#ifndef _GNTUN_PACKET_H_ +#define _GNTUN_PACKET_H_ + +struct pkt_tun { + unsigned char flags[2]; + unsigned char type[2]; + + unsigned char* data; +}; + +struct ip6_pkt { + unsigned char tclass; + unsigned char flowlbl[3]; + unsigned char paylgth[2]; + unsigned char nxthdr; + unsigned char hoplmt; + unsigned char sadr[16]; + unsigned char dadr[16]; + + unsigned char* data; +}; + +extern void send_pkt(int fd, struct ip6_pkt* pkt); +extern int recv_ipv6pkt(int fd, struct pkt_tun** pkt, unsigned char*); +extern int recv_pkt(int fd, struct pkt_tun** pkt); +extern struct ip6_pkt* parse_ip6(struct pkt_tun* pkt); +extern void pkt_printf(struct ip6_pkt* pkt); + +#endif diff --git a/src/vpn/test.c b/src/vpn/test.c new file mode 100644 index 000000000..7b1182126 --- /dev/null +++ b/src/vpn/test.c @@ -0,0 +1,25 @@ +#include +#include +#include + +#include + +#include "packet.h" +#include "tun.h" +#include "debug.h" + +int main(int c, char** v) { + //char* dev = (char*) malloc(IFNAMSIZ); + char dev[IFNAMSIZ]; + int fd = init_tun(dev); + + debug(1, 0, "Initialized the interface %s.\n", dev); + + struct pkt_tun* pkt; + + printf("read %d bytes from socket, now to parse'em\n", recv_pkt(fd, &pkt)); + + struct ip6_pkt* pkt6 = parse_ip6(pkt); + + pkt_printf(pkt6); +} diff --git a/src/vpn/tun.c b/src/vpn/tun.c index 6e184b079..d3c38bb4d 100644 --- a/src/vpn/tun.c +++ b/src/vpn/tun.c @@ -11,32 +11,40 @@ #include #include #include +#include -int tun_alloc(char *dev) { +#include "debug.h" + +/** + * Creates a tun-interface called dev; + * if *dev == 0, uses the name supplied by the kernel + * returns the fd to the tun or -1 + */ +int init_tun(char *dev) { /*{{{*/ struct ifreq ifr; int fd, err; if( (fd = open("/dev/net/tun", O_RDWR)) < 0 ) { - fprintf(stderr, "open: %s\n", strerror(errno)); + debug(1, 0, "opening /dev/net/tun: %s\n", strerror(errno)); return -1; } memset(&ifr, 0, sizeof(ifr)); ifr.ifr_flags = IFF_TUN; - if(*dev) + if(dev) strncpy(ifr.ifr_name, dev, IFNAMSIZ); if ((err = ioctl(fd, TUNSETIFF, (void *) &ifr)) < 0 ){ close(fd); - fprintf(stderr, "ioctl: %s\n", strerror(errno)); + debug(1, 0, "ioctl'ing /dev/net/tun: %s\n", strerror(errno)); return err; } strcpy(dev, ifr.ifr_name); return fd; -} +} /*}}}*/ -void n2o(fd) { +void n2o(int fd) { char buf[1024]; int r, w; for(;;) { @@ -61,7 +69,7 @@ void n2o(fd) { } } -void o2n(fd) { +void o2n(int fd) { char buf[1024]; int r, w; for(;;) { @@ -85,20 +93,3 @@ void o2n(fd) { } } } - -int main(int argc, char** argv) { - char name[IFNAMSIZ]; - int fd; - - memset(name, 0, IFNAMSIZ); - - strncpy(name, "mynet", IFNAMSIZ); - fprintf(stderr, "fd = %d, name = %s\n", fd = tun_alloc(name), name); - - if (fork() == 0) - n2o(fd); - - o2n(fd); - - return 0; -} diff --git a/src/vpn/tun.h b/src/vpn/tun.h new file mode 100644 index 000000000..ebd92199a --- /dev/null +++ b/src/vpn/tun.h @@ -0,0 +1,11 @@ +#ifndef _GNTUN_TUN_H_ +#define _GNTUN_TUN_H_ + +/** + * Creates a tun-interface called dev; + * if *dev == 0, uses the name supplied by the kernel + * returns the fd to the tun or -1 + */ +extern int init_tun(char *dev); + +#endif -- 2.25.1