From 7688209093fa47eb78e8323668bc4c6f04ffc020 Mon Sep 17 00:00:00 2001
From: angt <angt@users.noreply.github.com>
Date: Fri, 11 Dec 2015 11:32:22 +0100
Subject: [PATCH] Show tcp hdr in debug

---
 src/ip-static.h | 15 +++++++++++
 src/main.c      | 67 ++++++++++++++++++++++++++++++++++++++-----------
 2 files changed, 68 insertions(+), 14 deletions(-)

diff --git a/src/ip-static.h b/src/ip-static.h
index 074bcb3..d402ef3 100644
--- a/src/ip-static.h
+++ b/src/ip-static.h
@@ -40,3 +40,18 @@ static inline ssize_t ip_get_proto (const uint8_t *data, size_t size)
 
     return 0;
 }
+
+_pure_
+static inline ssize_t ip_get_hdr_size (const uint8_t *data, size_t size)
+{
+    switch (ip_get_version(data, size)) {
+    case 4:
+        return (data[0]&0xF)<<2;
+    case 6:
+        return 40;
+    case -1:
+        return -1;
+    }
+
+    return 0;
+}
diff --git a/src/main.c b/src/main.c
index f9b1323..bf49428 100644
--- a/src/main.c
+++ b/src/main.c
@@ -6,6 +6,7 @@
 #include <sys/time.h>
 #include <sys/fcntl.h>
 #include <sys/socket.h>
+#include <netinet/in.h>
 #include <netinet/tcp.h>
 #include <arpa/inet.h>
 #include <netdb.h>
@@ -482,22 +483,60 @@ static int gt_decrypt (struct crypto_ctx *ctx, buffer_t *dst, buffer_t *src)
     return 0;
 }
 
-static void dump_ip_header (uint8_t *data, size_t size)
+static void gt_dump (uint8_t *dst, size_t dst_size, uint8_t *src, size_t src_size)
 {
-    if (size<20)
+    if (dst_size<2*src_size+1)
         return;
 
     const char tbl[] = "0123456789ABCDEF";
-    char hex[41];
 
-    for (size_t i=0; i<20; i++) {
-        hex[(i<<1)+0] = tbl[0xF&(data[i]>>4)];
-        hex[(i<<1)+1] = tbl[0xF&(data[i])];
+    for (size_t i=0; i<src_size; i++) {
+        dst[(i<<1)+0] = tbl[0xF&(src[i]>>4)];
+        dst[(i<<1)+1] = tbl[0xF&(src[i])];
     }
 
-    hex[40] = 0;
+    dst[2*src_size] = 0;
+}
 
-    gt_log("DUMP(%zu): %s\n", size, hex);
+static void gt_print_hdr (uint8_t *data, size_t ip_size, const char *sockname)
+{
+    const int ip_version = ip_get_version(data, GT_MTU_MAX);
+    const ssize_t ip_proto = ip_get_proto(data, GT_MTU_MAX);
+    const ssize_t ip_hdr_size = ip_get_hdr_size(data, GT_MTU_MAX);
+
+    uint8_t ip_src[33];
+    uint8_t ip_dst[33];
+
+    switch (ip_version) {
+        case 4:
+            gt_dump(ip_src, sizeof(ip_src), &data[12], 4);
+            gt_dump(ip_dst, sizeof(ip_dst), &data[16], 4);
+            break;
+        case 6:
+            gt_dump(ip_src, sizeof(ip_src), &data[9], 16);
+            gt_dump(ip_dst, sizeof(ip_dst), &data[25], 16);
+            break;
+    }
+
+    gt_log("%s: version=%i size=%zi proto=%zi src=%s dst=%s\n", sockname, ip_version, ip_size, ip_proto, ip_src, ip_dst);
+
+    if (ip_hdr_size<=0 || ip_proto!=6)
+        return;
+
+    struct tcphdr tcp;
+
+    byte_cpy(&tcp, &data[ip_hdr_size], sizeof(tcp));
+
+    tcp.source = ntohs(tcp.source);
+    tcp.dest = ntohs(tcp.dest);
+    tcp.seq = ntohl(tcp.seq);
+    tcp.ack_seq = ntohl(tcp.ack_seq);
+    tcp.window = ntohs(tcp.window);
+
+    gt_log("%s: tcp src=%i dst=%i seq=%u ack=%u win=%u %s%s%s%s%s%s\n",
+            sockname, tcp.source, tcp.dest, tcp.seq, tcp.ack_seq, tcp.window,
+            tcp.fin?"FIN ":"", tcp.syn?"SYN ":"", tcp.rst?"RST ":"",
+            tcp.psh?"PSH ":"", tcp.ack?"ACK ":"", tcp.urg?"URG ":"");
 }
 
 static int gt_setup_secretkey (struct crypto_ctx *ctx, char *keyfile)
@@ -891,15 +930,15 @@ int main (int argc, char **argv)
                         continue;
 
                     if _0_(ip_size!=r) {
-                        dump_ip_header(data, r);
+                        uint8_t tmp[2*GT_MTU_MAX+1];
+                        gt_dump(tmp, sizeof(tmp), data, GT_MTU_MAX);
+                        gt_log("%s: DUMP %zi %s\n", sockname, r, tmp);
                         continue;
                     }
 
-                    if _0_(debug) {
-                        const int ip_version = ip_get_version(data, GT_MTU_MAX);
-                        const ssize_t ip_proto = ip_get_proto(data, GT_MTU_MAX);
-                        gt_log("%s: version=%i size=%zi proto=%zi\n", sockname, ip_version, ip_size, ip_proto);
-                    }
+                    if _0_(debug)
+                        gt_print_hdr(data, ip_size, sockname);
+
 
                     blks[blk_write++].size = r;
                     blk_count++;