vpn: parse and pretty-print tcp-packets

5 files changed, 81 insertions, 4 deletions

diff --git a/src/vpn/packet.c b/src/vpn/packet.c
index fc9eb7ba4..79841dc64 100644
--- a/src/vpn/packet.c
+++ b/src/vpn/packet.c
@@ -125,3 +125,33 @@ struct ip6_pkt* parse_ip6(struct pkt_tun* 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) - 4*(res->data.off));
+        memcpy(res->data.data, pkt->data+4*(res->data.off), payload(pkt) - 4*(res->data.off));
+
+        return res;
+}}}
diff --git a/src/vpn/packet.h b/src/vpn/packet.h
index 7857bb283..504690936 100644
--- a/src/vpn/packet.h
+++ b/src/vpn/packet.h
@@ -23,14 +23,27 @@ struct ip6_pkt {
         unsigned char* data;
 };
 
+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_tcp {
+        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_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);
+
+struct ip6_tcp* parse_ip6_tcp(struct ip6_pkt*);
 
 extern long payload(struct ip6_pkt* pkt);
 
diff --git a/src/vpn/pretty-print.c b/src/vpn/pretty-print.c
index 945ac92a7..c9878461d 100644
--- a/src/vpn/pretty-print.c
+++ b/src/vpn/pretty-print.c
@@ -94,6 +94,9 @@ void pp_write_header(char* dest, struct ip6_pkt* pkt) {{{
                 case 0x3a:
                         memcpy(dest, "ICMPv6)", 7);
                         break;
+                case 0x06:
+                        memcpy(dest, "TCP)", 4);
+                        break;
                 default:
                         memcpy(dest, "unknown)", 8);
                         break;
@@ -139,3 +142,24 @@ void pkt_printf(struct ip6_pkt* pkt) {{{
         printf(buf);
         free(buf);
 }}}
+
+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("flags: %c%c%c%c%c%c%c%c\n",
+                        pkt->data.flg & 0x80 ? 'C' : '.',
+                        pkt->data.flg & 0x40 ? 'E' : '.',
+                        pkt->data.flg & 0x20 ? 'U' : '.',
+                        pkt->data.flg & 0x10 ? 'A' : '.',
+                        pkt->data.flg & 0x08 ? 'P' : '.',
+                        pkt->data.flg & 0x04 ? 'R' : '.',
+                        pkt->data.flg & 0x02 ? 'S' : '.',
+                        pkt->data.flg & 0x01 ? 'F' : '.'
+                        );
+}}}
diff --git a/src/vpn/pretty-print.h b/src/vpn/pretty-print.h
index 191e2e2b5..2ab32b9c2 100644
--- a/src/vpn/pretty-print.h
+++ b/src/vpn/pretty-print.h
@@ -1,12 +1,12 @@
 #ifndef _GNTUN_PP_H_
 #define _GNTUN_PP_H_
 
-extern static void pp_ip6adr(unsigned char* adr, char* dest);
-
 extern void pp_hexdump(unsigned char* data, char* dest, int max);
 
 extern void pp_write_header(char* dest, struct ip6_pkt* pkt);
 
 extern void pkt_printf(struct ip6_pkt* pkt);
 
+void pkt_printf_ip6tcp(struct ip6_tcp* pkt);
+
 #endif
diff --git a/src/vpn/test.c b/src/vpn/test.c
index 15aad30d6..6d63d4bb1 100644
--- a/src/vpn/test.c
+++ b/src/vpn/test.c
@@ -7,6 +7,7 @@
 #include "packet.h"
 #include "tun.h"
 #include "debug.h"
+#include "pretty-print.h"
 
 int main(int c, char** v) {
         //char* dev = (char*) malloc(IFNAMSIZ);
@@ -23,7 +24,16 @@ int main(int c, char** v) {
                         case 0x86dd:
                                 printf("parsing ipv6:\n");
                                 struct ip6_pkt* pkt6 = parse_ip6(pkt);
-                                pkt_printf(pkt6);
+                                switch(pkt6->hdr.nxthdr) {
+                                        case 0x3a:
+                                                pkt_printf(pkt6);
+                                                break;
+                                        case 0x06:
+                                                pkt_printf(pkt6);
+                                                struct ip6_tcp* pkt6_tcp = parse_ip6_tcp(pkt6);
+                                                pkt_printf_ip6tcp(pkt6_tcp);
+                                                break;
+                                }
                                 break;
                         default:
                                 printf("unknown/unimplemented packet-type\n");