1 files changed, 145 insertions, 0 deletions
diff --git a/platform/src/packet_handle.cpp b/platform/src/packet_handle.cpp
new file mode 100644
index 0000000..07f736d
--- /dev/null
+++ b/platform/src/packet_handle.cpp
@@ -0,0 +1,145 @@
+#include <stddef.h>
+
+#include "packet_parser.h"
+#include "packet_inject.h"
+#include "packet_handle.h"
+
+static void packet_inject(int next_proto, const char *data, int len, struct metrics *metrics)
+{
+    if (next_proto == 4)
+    {
+        struct ip *hdr = (struct ip *)data;
+        if (packet_inject_ipv4(&hdr->ip_dst, data, len) == 0)
+        {
+            ATOMIC_ADD(&metrics->succ_tx_v4_pkts, 1);
+            ATOMIC_ADD(&metrics->succ_tx_v4_bytes, len);
+        }
+        else
+        {
+            ATOMIC_ADD(&metrics->err_tx_v4_pkts, 1);
+            ATOMIC_ADD(&metrics->err_tx_v4_bytes, len);
+        }
+    }
+
+    if (next_proto == 6)
+    {
+        struct ip6_hdr *hdr = (struct ip6_hdr *)data;
+        if (packet_inject_ipv6(&hdr->ip6_dst, data, len) == 0)
+        {
+            ATOMIC_ADD(&metrics->succ_tx_v6_pkts, 1);
+            ATOMIC_ADD(&metrics->succ_tx_v6_bytes, len);
+        }
+        else
+        {
+            ATOMIC_ADD(&metrics->err_tx_v6_pkts, 1);
+            ATOMIC_ADD(&metrics->err_tx_v6_bytes, len);
+        }
+    }
+}
+
+static void packet_handle_error(struct metrics *metrics, int n_pkts, int n_bytes)
+{
+    ATOMIC_ADD(&metrics->rx_err_pkts, n_pkts);
+    ATOMIC_ADD(&metrics->rx_err_bytes, n_bytes);
+}
+
+// return 1: is  gtp
+// return 0: not gtp
+static int packet_handle_gtp(struct packet_parser *handler, struct metrics *metrics)
+{
+    const struct layer_record *gtp_layer = packet_parser_get_most_outer(handler, LAYER_TYPE_GTPV1_U);
+    if (gtp_layer == NULL)
+    {
+        return 0;
+    }
+
+    if (gtp_layer->hdr_offset + gtp_layer->hdr_len >= handler->packet_len)
+    {
+        packet_handle_error(metrics, 1, handler->packet_len);
+        return 1;
+    }
+
+    const char *inject_data = (const char *)handler->packet_data + gtp_layer->hdr_offset + gtp_layer->hdr_len;
+    int inject_len = gtp_layer->pld_len;
+    uint8_t next_proto = gtp_next_proto((const char *)handler->packet_data + gtp_layer->hdr_offset);
+
+    if (next_proto != 4 && next_proto != 6)
+    {
+        packet_handle_error(metrics, 1, handler->packet_len);
+        return 1;
+    }
+    else
+    {
+        packet_inject(next_proto, inject_data, inject_len, metrics);
+        return 1;
+    }
+}
+
+// return 1: is  l3
+// return 0: not l3
+static int packet_handle_l3(struct packet_parser *handler, struct metrics *metrics)
+{
+    const struct layer_record *l3_layer = packet_parser_get_most_outer(handler, LAYER_TYPE_L3);
+    if (l3_layer == NULL)
+    {
+        return 0;
+    }
+
+    if (l3_layer->hdr_offset >= handler->packet_len)
+    {
+        packet_handle_error(metrics, 1, handler->packet_len);
+        return 1;
+    }
+
+    const char *inject_data = (const char *)handler->packet_data + l3_layer->hdr_offset;
+    int inject_len = l3_layer->hdr_len + l3_layer->pld_len;
+    uint8_t next_proto = 0;
+
+    if (l3_layer->type == LAYER_TYPE_IPV4)
+    {
+        next_proto = 4;
+    }
+    else if (l3_layer->type == LAYER_TYPE_IPV6)
+    {
+        next_proto = 6;
+    }
+    else
+    {
+        packet_handle_error(metrics, 1, handler->packet_len);
+        return 1;
+    }
+
+    packet_inject(next_proto, inject_data, inject_len, metrics);
+    return 1;
+}
+
+void packet_handle(const char *data, int len, struct metrics *metrics)
+{
+    ATOMIC_ADD(&metrics->rx_pkts, 1);
+    ATOMIC_ADD(&metrics->rx_bytes, len);
+
+    if (data == NULL || len <= 0)
+    {
+        packet_handle_error(metrics, 1, len);
+        return;
+    }
+
+    struct packet_parser handler;
+    uint64_t packet_id = ATOMIC_READ(&metrics->rx_pkts);
+    packet_parser_init(&handler);
+    packet_parser_parse(&handler, data, len, packet_id);
+
+    // Handle GTP
+    if (packet_handle_gtp(&handler, metrics) == 1)
+    {
+        return;
+    }
+
+    // Handle L3
+    if (packet_handle_l3(&handler, metrics) == 1)
+    {
+        return;
+    }
+
+    packet_handle_error(metrics, 1, len);
+}