vpn: a better way to parse the packets using bitfields

4 files changed, 67 insertions, 136 deletions

diff --git a/src/vpn/packet.c b/src/vpn/packet.c
index 2d52dbc81..4e96fb5f1 100644
--- a/src/vpn/packet.c
+++ b/src/vpn/packet.c
@@ -8,31 +8,16 @@
 
 #include "debug.h"
 #include "packet.h"
+#include "arpa/inet.h"
 
-long payload(struct ip6_hdr* hdr) {{{
-        return (hdr->paylgth[0] << 8) + hdr->paylgth[1];
+short payload(struct ip6_hdr* hdr) {{{
+        return ntohs(hdr->paylgth);
 }}}
 
 void send_pkt(int fd, struct ip6_pkt* pkt) {{{
         int sz = payload(&(pkt->hdr));
         int w = 0;
-        char* buf = (char*)malloc(sz+40);
-
-        buf[0] = (6 << 4) | (pkt->hdr.tclass >> 4);
-        buf[1] = (pkt->hdr.tclass << 4) | (pkt->hdr.flowlbl[0] >> 4);
-        buf[2] = pkt->hdr.flowlbl[1];
-        buf[3] = pkt->hdr.flowlbl[2];
-        buf[4] = pkt->hdr.paylgth[0];
-        buf[5] = pkt->hdr.paylgth[1];
-        buf[6] = pkt->hdr.nxthdr;
-        buf[7] = pkt->hdr.hoplmt;
-
-        for (w = 0; w < 16; w++) {
-                buf[8+w] = pkt->hdr.sadr[w];
-                buf[24+w] = pkt->hdr.dadr[w];
-        }
-
-        memcpy(buf+40, pkt->data, sz);
+        char* buf = (char*)pkt;
 
         w = 0;
         while ( w > 0) {
@@ -46,48 +31,31 @@ void send_pkt(int fd, struct ip6_pkt* pkt) {{{
         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_ipv6pkt(int fd, struct pkt_tun** pkt) {{{
 }}}
 
 int recv_pkt(int fd, struct pkt_tun** pkt) {{{
-        struct pkt_tun* _pkt = (struct pkt_tun*)malloc(sizeof(struct pkt_tun));
-        *pkt = _pkt;
+        // TODO: länge lesen?
+        *pkt = (struct pkt_tun*)malloc(1504);
+        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;
+        unsigned char *data = (unsigned char*)_pkt;
 
         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];
+        int r = read(fd, data, 1504);
+        debug(1, 0, "read %d bytes\n", r);
 
-        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]);
+        debug(1, 0, "read the flags: %04x\n", ntohs(_pkt->flags));
+        debug(1, 0, "read the type: %04x\n", ntohs(_pkt->type));
 
-        switch((_pkt->type[0] << 8) + _pkt->type[1]) {
+        switch(ntohs(_pkt->type)) {
                 case 0x86dd:
-                        return recv_ipv6pkt(fd, pkt, data);
+                        debug(1, 0, "reading an ipv6-packet\n");
+                        struct ip6_pkt * pkt6 = (struct ip6_pkt*) *pkt;
+                        int size = payload(&(pkt6->hdr));
+                        debug(1, 0, "read the size: %d\n", size);
+                        return size;
                         break;
                 case 0x0800:
                         debug(1, 0, "unknown pkt-type: IPv4\n");
@@ -102,56 +70,13 @@ int recv_pkt(int fd, struct pkt_tun** pkt) {{{
 }}}
 
 struct ip6_pkt* parse_ip6(struct pkt_tun* pkt) {{{
-        struct ip6_pkt* pkt6 = (struct ip6_pkt*)malloc(sizeof(struct ip6_pkt));
-
-        pkt6->hdr.tclass = pkt->data[0] << 4 | pkt->data[1] >> 4;
-        pkt6->hdr.flowlbl[0] = pkt->data[1]>>4;
-        pkt6->hdr.flowlbl[1] = pkt->data[2];
-        pkt6->hdr.flowlbl[2] = pkt->data[3];
-
-        pkt6->hdr.paylgth[0] = pkt->data[4];
-        pkt6->hdr.paylgth[1] = pkt->data[5];
-
-        pkt6->hdr.nxthdr = pkt->data[6];
-        pkt6->hdr.hoplmt = pkt->data[7];
-
-        for (int w = 0; w < 16; w++) {
-                pkt6->hdr.sadr[w] = pkt->data[8+w];
-                pkt6->hdr.dadr[w] = pkt->data[24+w];
-        }
-
-        pkt6->data = (unsigned char*)malloc(payload(&(pkt6->hdr)));
-        memcpy(pkt6->data, pkt->data+40, payload(&(pkt6->hdr)));
+        struct ip6_pkt* pkt6 = (struct ip6_pkt*)pkt;
 
         return pkt6;
 }}}
 
 struct ip6_tcp* parse_ip6_tcp(struct ip6_pkt* pkt) {{{
-        struct ip6_tcp* res = (struct ip6_tcp*) malloc(sizeof(struct ip6_tcp));
-        memcpy(&(res->hdr), &(pkt->hdr), sizeof(struct ip6_hdr));
-        
-        res->data.spt = (pkt->data[0] << 8) | pkt->data[1];
-        res->data.dpt = (pkt->data[2] << 8) | pkt->data[3];
-
-        res->data.seq = (pkt->data[4] << 24) | (pkt->data[5] << 16) | (pkt->data[6] << 8) | pkt->data[7];
-        res->data.ack = (pkt->data[8] << 24) | (pkt->data[9] << 16) | (pkt->data[10] << 8) | pkt->data[11];
-
-        res->data.off = pkt->data[12] >> 4;
-        res->data.rsv = pkt->data[12] & 0xF;
-
-        res->data.flg = pkt->data[13];
-
-        res->data.wsz = (pkt->data[14] << 8) | pkt->data[15];
-
-        res->data.crc = (pkt->data[16] << 8) | pkt->data[17];
-
-        res->data.urg = (pkt->data[18] << 8) | pkt->data[19];
-
-        res->data.opt = (unsigned char*) malloc((res->data.off - 5)*4);
-        memcpy(res->data.opt, pkt->data+20, (res->data.off - 5)*4);
-
-        res->data.data = (unsigned char*) malloc(payload(&(pkt->hdr)) - 4*(res->data.off));
-        memcpy(res->data.data, pkt->data+4*(res->data.off), payload(&(pkt->hdr)) - 4*(res->data.off));
+        struct ip6_tcp* res = (struct ip6_tcp*) pkt;
 
         return res;
 }}}
diff --git a/src/vpn/packet.h b/src/vpn/packet.h
index 4044eecb0..fa3c8f38c 100644
--- a/src/vpn/packet.h
+++ b/src/vpn/packet.h
@@ -2,49 +2,54 @@
 #define _GNTUN_PACKET_H_
 
 struct pkt_tun {
-        unsigned char flags[2];
-        unsigned char type[2];
-
-        unsigned char* data;
+        unsigned flags:16;
+        unsigned type:16;
 };
 
 struct ip6_hdr {
-        unsigned char tclass;
-        unsigned char flowlbl[3];
-        unsigned char paylgth[2];
-        unsigned char nxthdr;
-        unsigned char hoplmt;
+        unsigned version:4;
+        unsigned tclass:8;
+        unsigned flowlbl:20;
+        unsigned paylgth:16;
+        unsigned nxthdr:8;
+        unsigned hoplmt:8;
         unsigned char sadr[16];
         unsigned char dadr[16];
 };
 
-struct ip6_pkt {
-        struct ip6_hdr hdr;
-        unsigned char* data;
+struct tcp_pkt {
+        unsigned spt:16;
+        unsigned dpt:16;
+        unsigned seq:32;
+        unsigned ack:32;
+        unsigned off:4;
+        unsigned rsv:4;
+        unsigned flg:8;
+        unsigned wsz:16;
+        unsigned crc:16;
+        unsigned urg:16;
+        unsigned char data[1];
 };
 
-struct tcp_pkt {
-        unsigned short spt, dpt;
-        unsigned int seq, ack;
-        unsigned char off, rsv, flg;
-        unsigned short wsz;
-        unsigned short crc, urg;
-        unsigned char* opt;
-        unsigned char* data;
+struct ip6_pkt {
+        struct pkt_tun tun;
+        struct ip6_hdr hdr;
+        unsigned char data[1];
 };
 
 struct ip6_tcp {
+        struct pkt_tun tun;
         struct ip6_hdr hdr;
         struct tcp_pkt 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_ipv6pkt(int fd, struct pkt_tun** pkt);
 extern int recv_pkt(int fd, struct pkt_tun** pkt);
 extern struct ip6_pkt* parse_ip6(struct pkt_tun* pkt);
 
-struct ip6_tcp* parse_ip6_tcp(struct ip6_pkt*);
+extern struct ip6_tcp* parse_ip6_tcp(struct ip6_pkt*);
 
-extern long payload(struct ip6_hdr* pkt);
+extern short payload(struct ip6_hdr* pkt);
 
 #endif
diff --git a/src/vpn/pretty-print.c b/src/vpn/pretty-print.c
index c9878461d..96e435d2d 100644
--- a/src/vpn/pretty-print.c
+++ b/src/vpn/pretty-print.c
@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <ctype.h>
+#include <arpa/inet.h>
 
 #include "packet.h"
 
@@ -112,13 +113,13 @@ void pkt_printf(struct ip6_pkt* pkt) {{{
         pp_ip6adr(pkt->hdr.sadr, buf+16);
         pp_ip6adr(pkt->hdr.dadr, buf+76);
 
-        int flow = (pkt->hdr.flowlbl[0] << 16) + (pkt->hdr.flowlbl[1] << 8) + (pkt->hdr.flowlbl[2]);
+        int flow = (ntohl(pkt->hdr.flowlbl));
         sprintf(tmp, "%03x", flow);
         memcpy(buf+138, tmp, 3);
         sprintf(tmp, "%-8d", flow);
         memcpy(buf+143, tmp, 8);
 
-        int length = (pkt->hdr.paylgth[0] << 8) + (pkt->hdr.paylgth[1]);
+        int length = ntohs(pkt->hdr.paylgth);
         sprintf(tmp, "%02x", length);
         memcpy(buf+198, tmp, 2);
         sprintf(tmp, "%-3d", length);
@@ -133,7 +134,7 @@ void pkt_printf(struct ip6_pkt* pkt) {{{
         sprintf(tmp, "%-3d", pkt->hdr.hoplmt);
         memcpy(buf+323, tmp, 3);
 
-        int size = payload(pkt);
+        int size = payload(&pkt->hdr);
         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);
@@ -144,14 +145,14 @@ void pkt_printf(struct ip6_pkt* pkt) {{{
 }}}
 
 void pkt_printf_ip6tcp(struct ip6_tcp* pkt) {{{
-        printf("spt: %u\n", pkt->data.spt);
-        printf("dpt: %u\n", pkt->data.dpt);
-        printf("seq: %u\n", pkt->data.seq);
-        printf("ack: %u\n", pkt->data.ack);
-        printf("off: %u\n", pkt->data.off);
-        printf("wsz: %u\n", pkt->data.wsz);
-        printf("crc: %u\n", pkt->data.crc);
-        printf("urg: %u\n", pkt->data.urg);
+        printf("spt: %u\n", ntohs(pkt->data.spt));
+        printf("dpt: %u\n", ntohs(pkt->data.dpt));
+        printf("seq: %u\n", ntohs(pkt->data.seq));
+        printf("ack: %u\n", ntohs(pkt->data.ack));
+        printf("off: %u\n", ntohs(pkt->data.off));
+        printf("wsz: %u\n", ntohs(pkt->data.wsz));
+        printf("crc: %u\n", ntohs(pkt->data.crc));
+        printf("urg: %u\n", ntohs(pkt->data.urg));
         printf("flags: %c%c%c%c%c%c%c%c\n",
                         pkt->data.flg & 0x80 ? 'C' : '.',
                         pkt->data.flg & 0x40 ? 'E' : '.',
diff --git a/src/vpn/test.c b/src/vpn/test.c
index 3672b9df2..87ad9b10d 100644
--- a/src/vpn/test.c
+++ b/src/vpn/test.c
@@ -10,6 +10,8 @@
 #include "debug.h"
 #include "pretty-print.h"
 #include "tcp.h"
+#include <arpa/inet.h>
+
 
 int main(int c, char** v) {
         char dev[IFNAMSIZ];
@@ -22,17 +24,15 @@ int main(int c, char** v) {
 
         for(;;) {
                 printf("read %d bytes from socket, ", recv_pkt(fd, &pkt));
-                switch (pkt->type[0] << 8 | pkt->type[1]) {
+                switch (ntohs(pkt->type)) {
                         case 0x86dd:
                                 printf("parsing ipv6:\n");
                                 struct ip6_pkt* pkt6 = parse_ip6(pkt);
+                                pkt_printf(pkt6);
+                                struct ip6_tcp* pkt6_tcp;
                                 switch(pkt6->hdr.nxthdr) {
-                                        case 0x3a:
-                                                pkt_printf(pkt6);
-                                                break;
                                         case 0x06:
-                                                pkt_printf(pkt6);
-                                                struct ip6_tcp* pkt6_tcp = parse_ip6_tcp(pkt6);
+                                                pkt6_tcp = parse_ip6_tcp(pkt6);
                                                 pkt_printf_ip6tcp(pkt6_tcp);
                                                 handle_tcp(pkt6_tcp);
                                                 break;