parse ipv6-packets for the vpn-functionality

diff --git a/src/vpn/Makefile b/src/vpn/Makefile
index ac8633c65fb2ca07c2b02e2eadfff3dd37d9f8c6..848faf24dcae4ba8075fb2ff3f242a35fe76caf2 100644 (file)
--- 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 (file)
index 0000000..900a4bd
--- /dev/null
+++ b/src/vpn/debug.c
@@ -0,0 +1,13 @@
+#include <stdio.h>
+#include <stdarg.h>
+#include <stdlib.h>
+
+#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 (file)
index 0000000..8f3e718
--- /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 (file)
index 0000000..c4c7dc2
--- /dev/null
+++ b/src/vpn/packet.c
@@ -0,0 +1,235 @@
+#include <errno.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/uio.h>
+
+#include <linux/if_tun.h>
+
+#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 (file)
index 0000000..9d5153f
--- /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 (file)
index 0000000..7b11821
--- /dev/null
+++ b/src/vpn/test.c
@@ -0,0 +1,25 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/socket.h>
+
+#include <linux/if.h>
+
+#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 6e184b079a2727b66af551bf9fd1339fac6e7ffc..d3c38bb4d266c5d13e87311e7614857a69d73c62 100644 (file)
--- a/src/vpn/tun.c
+++ b/src/vpn/tun.c
@@ -11,32 +11,40 @@
 #include <stdio.h>
 #include <string.h>
 #include <errno.h>
+#include <stdlib.h>
 
-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 (file)
index 0000000..ebd9219
--- /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