add marsio_pkt_jump_to_innermost_layer

1 files changed, 82 insertions, 0 deletions

diff --git a/src/trace_output.c b/src/trace_output.c
index 1cdab09..aff6faa 100644
--- a/src/trace_output.c
+++ b/src/trace_output.c
@@ -11,9 +11,23 @@
 
 #include <errno.h>
 #include <fcntl.h>
+#include <netinet/ip.h>
+#include <netinet/ip6.h>
+#include <netinet/tcp.h>
+#include <netinet/udp.h>
 #include <pthread.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <arpa/inet.h>
+
+
+struct pkt_inner_ip_port
+{
+    char src_addr_str[INET6_ADDRSTRLEN];
+    char dst_addr_str[INET6_ADDRSTRLEN];
+    int32_t src_port;
+    int32_t dst_port;
+};
 
 #define BURST_MAX 64
 
@@ -31,6 +45,7 @@ int dp_trace_classification(struct mr_instance * instance, marsio_buff_t * mbufs
                             int nr_jobs_mbufs[DP_TRACE_JOB_NUM_MAX]);
 static void dp_trace_decode_to_message_pack(marsio_buff_t * mr_mbuf, char ** data, size_t * size, job_bitmap_t job_id);
 static int dp_trace_record_decode_to_str(marsio_buff_t * mr_mbuf, char * data, unsigned int size);
+int marsio_pkt_inner_ip_port_get(const marsio_buff_t * mbuf, struct pkt_inner_ip_port * info);
 
 struct dp_trace_output
 {
@@ -599,4 +614,71 @@ static int dp_trace_record_decode_to_str(marsio_buff_t * mr_mbuf, char * data, u
 #endif
 
     return 0;
+}
+
+int marsio_pkt_inner_ip_port_get(const marsio_buff_t * mbuf, struct pkt_inner_ip_port * info)
+{
+    memset(info, 0, sizeof(struct pkt_inner_ip_port));
+
+    // get inner ip
+    void * ipv4_ptr = NULL;
+    void * ipv6_ptr = NULL;
+    uint16_t l3_layer_type_id = 0;
+
+    ipv4_ptr = marsio_pkt_jump_to_innermost_layer(mbuf, LAYER_TYPE_ID_IPV4);
+    ipv6_ptr = marsio_pkt_jump_to_innermost_layer(mbuf, LAYER_TYPE_ID_IPV6);
+
+    if (ipv4_ptr > ipv6_ptr)
+    {
+        l3_layer_type_id = LAYER_TYPE_ID_IPV4;
+    }
+    else if (ipv6_ptr > ipv4_ptr)
+    {
+        l3_layer_type_id = LAYER_TYPE_ID_IPV6;
+    }
+
+    if (l3_layer_type_id == LAYER_TYPE_ID_IPV4)
+    {
+        struct iphdr * ipv4_hdr = (struct iphdr *)ipv4_ptr;
+        inet_ntop(AF_INET, &ipv4_hdr->saddr, info->src_addr_str, sizeof(info->src_addr_str));
+        inet_ntop(AF_INET, &ipv4_hdr->daddr, info->dst_addr_str, sizeof(info->dst_addr_str));
+    }
+    else if (l3_layer_type_id == LAYER_TYPE_ID_IPV6)
+    {
+        struct ip6_hdr * ipv6_hdr = (struct ip6_hdr *)ipv6_ptr;
+        inet_ntop(AF_INET6, &ipv6_hdr->ip6_src, info->src_addr_str, sizeof(info->src_addr_str));
+        inet_ntop(AF_INET6, &ipv6_hdr->ip6_dst, info->dst_addr_str, sizeof(info->dst_addr_str));
+    }
+
+    // get inner tcp or udp port
+    void * tcp_ptr = NULL;
+    void * udp_ptr = NULL;
+    uint16_t l4_layer_type_id = 0;
+
+    tcp_ptr = marsio_pkt_jump_to_innermost_layer(mbuf, LAYER_TYPE_ID_TCP);
+    udp_ptr = marsio_pkt_jump_to_innermost_layer(mbuf, LAYER_TYPE_ID_UDP);
+
+    if (tcp_ptr > udp_ptr)
+    {
+        l4_layer_type_id = LAYER_TYPE_ID_TCP;
+    }
+    else if (udp_ptr > tcp_ptr)
+    {
+        l4_layer_type_id = LAYER_TYPE_ID_UDP;
+    }
+
+    if (l4_layer_type_id == LAYER_TYPE_ID_TCP)
+    {
+        struct tcphdr * tcp_hdr = (struct tcphdr *)tcp_ptr;
+        info->src_port = tcp_hdr->th_sport;
+        info->dst_port = tcp_hdr->th_dport;
+    }
+    else if (l4_layer_type_id == LAYER_TYPE_ID_UDP)
+    {
+        struct udphdr * udp_hdr = (struct udphdr *)tcp_ptr;
+        info->src_port = udp_hdr->uh_sport;
+        info->dst_port = udp_hdr->uh_dport;
+    }
+
+    return 0;
 }
\ No newline at end of file