1

1 month ago · 136a3642c0
1 changed files with 65 additions and 0 deletions
--- a/src/packet_dump.c
+++ b/src/packet_dump.c
@ -0,0 +1,65 @@
+#include "packet_dump.h"
+#include "../lib/debug_config.h"
+#include <stdio.h>
+#include <string.h>
+
+void dump_ip_packet(const char* direction, const uint8_t* data, size_t len) {
+    if (len < 20) {
+        DEBUG_DEBUG(DEBUG_CATEGORY_TUN, "[%s] Packet too small: %zd bytes", direction, len);
+        return;
+    }
+
+    uint8_t version = (data[0] >> 4) & 0x0F;
+    if (version != 4) {
+        DEBUG_DEBUG(DEBUG_CATEGORY_TUN, "[%s] Non-IPv4 packet: %zd bytes", direction, len);
+        return;
+    }
+
+    uint8_t ihl = (data[0] & 0x0F) * 4;
+    if (len < ihl) {
+        DEBUG_DEBUG(DEBUG_CATEGORY_TUN, "[%s] Truncated IP header: %zd bytes", direction, len);
+        return;
+    }
+
+    uint8_t protocol = data[9];
+    uint32_t src_ip = *(uint32_t*)&data[12];
+    uint32_t dst_ip = *(uint32_t*)&data[16];
+
+    char src_str[16], dst_str[16];
+    snprintf(src_str, sizeof(src_str), "%u.%u.%u.%u",
+             (src_ip) & 0xFF, (src_ip >> 8) & 0xFF,
+             (src_ip >> 16) & 0xFF, (src_ip >> 24) & 0xFF);
+    snprintf(dst_str, sizeof(dst_str), "%u.%u.%u.%u",
+             (dst_ip) & 0xFF, (dst_ip >> 8) & 0xFF,
+             (dst_ip >> 16) & 0xFF, (dst_ip >> 24) & 0xFF);
+
+    const char* proto_name = "UNKNOWN";
+    uint16_t src_port = 0, dst_port = 0;
+
+    if (protocol == 6 && len > ihl + 4) {
+        proto_name = "TCP";
+        src_port = (data[ihl] << 8) | data[ihl + 1];
+        dst_port = (data[ihl + 2] << 8) | data[ihl + 3];
+    } else if (protocol == 17 && len > ihl + 4) {
+        proto_name = "UDP";
+        src_port = (data[ihl] << 8) | data[ihl + 1];
+        dst_port = (data[ihl + 2] << 8) | data[ihl + 3];
+    } else if (protocol == 1) {
+        proto_name = "ICMP";
+    }
+
+    DEBUG_DEBUG(DEBUG_CATEGORY_TUN, "[%s] IPv4 %s %s:%d -> %s:%d (%zd bytes) data: "
+                "%02x%02x%02x%02x%02x%02x%02x%02x "
+                "%02x%02x%02x%02x%02x%02x%02x%02x "
+                "%02x%02x%02x%02x%02x%02x%02x%02x "
+                "%02x%02x%02x%02x%02x%02x%02x%02x",
+                direction, proto_name, src_str, src_port, dst_str, dst_port, len,
+                data[ihl + 0], data[ihl + 1], data[ihl + 2], data[ihl + 3],
+                data[ihl + 4], data[ihl + 5], data[ihl + 6], data[ihl + 7],
+                data[ihl + 8], data[ihl + 9], data[ihl + 10], data[ihl + 11],
+                data[ihl + 12], data[ihl + 13], data[ihl + 14], data[ihl + 15],
+                data[ihl + 16], data[ihl + 17], data[ihl + 18], data[ihl + 19],
+                data[ihl + 20], data[ihl + 21], data[ihl + 22], data[ihl + 23],
+                data[ihl + 24], data[ihl + 25], data[ihl + 26], data[ihl + 27],
+                data[ihl + 28], data[ihl + 29], data[ihl + 30], data[ihl + 31]);
+}