Port pcapng

5 files changed, 445 insertions, 1 deletions

diff --git a/support/CMakeLists.txt b/support/CMakeLists.txt
index c26370f..b4bbd78 100644
--- a/support/CMakeLists.txt
+++ b/support/CMakeLists.txt
@@ -116,4 +116,7 @@ file(MAKE_DIRECTORY ${INSTALL_DIR}/include)
 add_library(libcjson SHARED IMPORTED GLOBAL)
 add_dependencies(libcjson cJSON)
 set_property(TARGET libcjson PROPERTY IMPORTED_LOCATION ${INSTALL_DIR}/lib64/libcjson.a)
-set_property(TARGET libcjson PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${INSTALL_DIR}/include)
\ No newline at end of file
+set_property(TARGET libcjson PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${INSTALL_DIR}/include)
+
+### pcapng
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/pcapng)
\ No newline at end of file
diff --git a/support/pcapng/CMakeLists.txt b/support/pcapng/CMakeLists.txt
new file mode 100644
index 0000000..13012e5
--- /dev/null
+++ b/support/pcapng/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.0)
+
+project(pcapng)
+
+add_library(libpcapng STATIC
+    pcapng.c
+)
+
+target_include_directories(libpcapng PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+target_link_libraries(libpcapng PRIVATE libmarsio)
\ No newline at end of file
diff --git a/support/pcapng/pcapng.c b/support/pcapng/pcapng.c
new file mode 100644
index 0000000..b0bf050
--- /dev/null
+++ b/support/pcapng/pcapng.c
@@ -0,0 +1,269 @@
+#include "pcapng.h"
+
+#include <alloca.h>
+#include <fcntl.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+
+/**
+ * Macro to align a value to a given power-of-two. The resultant value
+ * will be of the same type as the first parameter, and will be no
+ * bigger than the first parameter. Second parameter must be a
+ * power-of-two value.
+ */
+#define ALIGN_FLOOR(val, align) (typeof(val))((val) & (~((typeof(val))((align)-1))))
+
+/**
+ * Macro to align a value to a given power-of-two. The resultant
+ * value will be of the same type as the first parameter, and
+ * will be no lower than the first parameter. Second parameter
+ * must be a power-of-two value.
+ */
+#define ALIGN_CEIL(val, align) ALIGN_FLOOR(((val) + ((typeof(val))(align)-1)), align)
+#define ALIGN(val, align) ALIGN_CEIL(val, align)
+
+static inline uint16_t pcapng_optlen(uint16_t len)
+{
+    return ALIGN(sizeof(struct pcapng_option) + len, sizeof(uint32_t));
+}
+
+/* build TLV option and return location of next */
+static inline struct pcapng_option * pcapng_add_option(struct pcapng_option * popt, uint16_t code, const void * data,
+                                                       uint16_t len)
+{
+    popt->code = code;
+    popt->length = len;
+    memcpy(popt->data, data, len);
+
+    return (struct pcapng_option *)((uint8_t *)popt + pcapng_optlen(len));
+}
+
+/*
+ * Write required initial section header describing the capture
+ */
+static inline int pcapng_section_block(struct pcapng_t * self, const char * os, const char * hw, const char * app,
+                                       const char * comment)
+{
+    struct pcapng_section_header * hdr;
+    struct pcapng_option * opt;
+    void * buf;
+    uint32_t len;
+    ssize_t cc;
+
+    len = sizeof(*hdr);
+    if (hw)
+        len += pcapng_optlen(strlen(hw));
+    if (os)
+        len += pcapng_optlen(strlen(os));
+    if (app)
+        len += pcapng_optlen(strlen(app));
+    if (comment)
+        len += pcapng_optlen(strlen(comment));
+
+    /* reserve space for OPT_END */
+    len += pcapng_optlen(0);
+    len += sizeof(uint32_t);
+
+    buf = calloc(1, len);
+    if (!buf)
+        return -1;
+
+    hdr = (struct pcapng_section_header *)buf;
+    *hdr = (struct pcapng_section_header){
+        .block_type = PCAPNG_SECTION_BLOCK,
+        .block_length = len,
+        .byte_order_magic = PCAPNG_BYTE_ORDER_MAGIC,
+        .major_version = PCAPNG_MAJOR_VERS,
+        .minor_version = PCAPNG_MINOR_VERS,
+        .section_length = UINT64_MAX,
+    };
+
+    /* After the section header insert variable length options. */
+    opt = (struct pcapng_option *)(hdr + 1);
+    if (comment)
+        opt = pcapng_add_option(opt, PCAPNG_OPT_COMMENT, comment, strlen(comment));
+    if (hw)
+        opt = pcapng_add_option(opt, PCAPNG_SHB_HARDWARE, hw, strlen(hw));
+    if (os)
+        opt = pcapng_add_option(opt, PCAPNG_SHB_OS, os, strlen(os));
+    if (app)
+        opt = pcapng_add_option(opt, PCAPNG_SHB_USERAPPL, app, strlen(app));
+
+    /* The standard requires last option to be OPT_END */
+    opt = pcapng_add_option(opt, PCAPNG_OPT_END, NULL, 0);
+
+    /* clone block_length after option */
+    memcpy(opt, &hdr->block_length, sizeof(uint32_t));
+
+    cc = write(self->outfd, buf, len);
+    free(buf);
+
+    return cc;
+}
+
+/* Write an interface block */
+static inline int pcapng_interface_block(struct pcapng_t * self)
+{
+    struct pcapng_interface_block * hdr;
+    uint32_t len;
+    const uint8_t tsresol = 9; /* nanosecond resolution */
+    void * buf;
+    struct pcapng_option * opt;
+
+    /* Compute length of interface block options */
+    len = sizeof(*hdr);
+
+    len += pcapng_optlen(sizeof(tsresol)); /* timestamp */
+    len += pcapng_optlen(0);               /* for OPT_END */
+    len += sizeof(uint32_t);               /* Block length */
+
+    buf = alloca(len);
+    if (!buf)
+        return -1;
+
+    hdr = (struct pcapng_interface_block *)buf;
+    *hdr = (struct pcapng_interface_block){
+        .block_type = PCAPNG_INTERFACE_BLOCK,
+        .link_type = 1, /* DLT_EN10MB - Ethernet */
+        .block_length = len,
+    };
+
+    opt = (struct pcapng_option *)(hdr + 1);
+    opt = pcapng_add_option(opt, PCAPNG_IFB_TSRESOL, &tsresol, sizeof(tsresol));
+    opt = pcapng_add_option(opt, PCAPNG_OPT_END, NULL, 0);
+
+    /* clone block_length after optionsa */
+    memcpy(opt, &hdr->block_length, sizeof(uint32_t));
+
+    return write(self->outfd, buf, len);
+}
+
+/* Create new pcapng writer handle */
+struct pcapng_t * pcapng_fdopen(int fd, const char * osname, const char * hardware, const char * appname,
+                                const char * comment)
+{
+    struct pcapng_t * self;
+
+    self = malloc(sizeof(*self));
+    if (!self)
+    {
+        return NULL;
+    }
+
+    self->outfd = fd;
+
+    if (pcapng_section_block(self, osname, hardware, appname, comment) < 0)
+        goto fail;
+
+    if (pcapng_interface_block(self) < 0)
+        goto fail;
+
+    return self;
+fail:
+    free(self);
+    return NULL;
+}
+
+struct pcapng_t * pcapng_open(const char * path)
+{
+    int pcapng_fd = open(path, O_WRONLY | O_CREAT, 0640);
+    if (pcapng_fd < 0)
+    {
+        return NULL;
+    }
+
+    return pcapng_fdopen(pcapng_fd, NULL, NULL, NULL, NULL);
+}
+
+void pcapng_close(struct pcapng_t * self)
+{
+    close(self->outfd);
+    free(self);
+}
+
+/* Make a copy of original mbuf with pcapng header and options */
+int pcapng_copy(marsio_buff_t * mbuf, uint32_t snaplen, const char * comment,
+                struct pcapng_enhance_packet_block ** obj_p)
+{
+    uint32_t orig_len = 0;
+    uint32_t data_len = 0;
+    uint64_t timestamp;
+    uint32_t len = 0;
+    struct pcapng_option * opt = NULL;
+
+    orig_len = marsio_buff_buflen(mbuf);
+    data_len = marsio_buff_datalen(mbuf);
+    data_len = snaplen < data_len ? snaplen : data_len;
+
+    len += sizeof(struct pcapng_enhance_packet_block);
+    len += data_len;
+
+    if (comment)
+    {
+        len += pcapng_optlen(strlen(comment));
+    }
+    len += sizeof(uint32_t); /* Block length */
+
+    /* Note: END_OPT necessary here. Wireshark doesn't do it. */
+
+    struct pcapng_enhance_packet_block * epb = calloc(1, len);
+    if (epb == NULL)
+    {
+        return -1;
+    }
+
+    epb->block_type = PCAPNG_ENHANCED_PACKET_BLOCK;
+    epb->block_length = len;
+
+    struct timespec current_time;
+    clock_gettime(CLOCK_REALTIME, &current_time);
+    timestamp = (uint64_t)current_time.tv_sec * 1000000000 + current_time.tv_nsec;
+
+    epb->timestamp_hi = timestamp >> 32;
+    epb->timestamp_lo = (uint32_t)timestamp;
+    epb->capture_length = data_len;
+    epb->original_length = orig_len;
+
+    memcpy((char *)epb + sizeof(*epb), marsio_buff_mtod(mbuf), data_len);
+
+    opt = (struct pcapng_option *)((char *)epb + sizeof(*epb) + data_len);
+
+    if (comment)
+    {
+        opt = pcapng_add_option(opt, PCAPNG_OPT_COMMENT, comment, strlen(comment));
+    }
+
+    /* set trailer of block length */
+    memcpy(opt, &epb->block_length, sizeof(uint32_t));
+
+    *obj_p = epb;
+    return 0;
+}
+
+/* Write pre-formatted packets to file. */
+int pcapng_write_packets(struct pcapng_t * self, struct pcapng_enhance_packet_block * epbs[], uint16_t nb_epb)
+{
+    unsigned int write_packet_cnt = 0;
+    int ret = 0;
+
+    for (unsigned int i = 0; i < nb_epb; i++)
+    {
+        struct pcapng_enhance_packet_block * epb = epbs[i];
+
+        if (epb->block_type != PCAPNG_ENHANCED_PACKET_BLOCK)
+        {
+            continue;
+        }
+
+        ret = write(self->outfd, epb, epb->block_length);
+        if (ret == epb->block_length)
+        {
+            write_packet_cnt++;
+        }
+    }
+
+    return write_packet_cnt;
+}
\ No newline at end of file
diff --git a/support/pcapng/pcapng.h b/support/pcapng/pcapng.h
new file mode 100644
index 0000000..c585a84
--- /dev/null
+++ b/support/pcapng/pcapng.h
@@ -0,0 +1,16 @@
+#pragma once
+#include "marsio.h"
+#include "pcapng_proto.h"
+
+struct pcapng_t
+{
+    int outfd; /* output file */
+};
+
+struct pcapng_t * pcapng_fdopen(int fd, const char * osname, const char * hardware, const char * appname,
+                                const char * comment);
+struct pcapng_t * pcapng_open(const char * path);
+void pcapng_close(struct pcapng_t * self);
+int pcapng_copy(marsio_buff_t * mbuf, uint32_t snaplen, const char * comment,
+                struct pcapng_enhance_packet_block ** obj_p);
+int pcapng_write_packets(struct pcapng_t * self, struct pcapng_enhance_packet_block * epbs[], uint16_t nb_epb);
\ No newline at end of file
diff --git a/support/pcapng/pcapng_proto.h b/support/pcapng/pcapng_proto.h
new file mode 100644
index 0000000..6bcdbb5
--- /dev/null
+++ b/support/pcapng/pcapng_proto.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: BSD-3-Clause
+ * Copyright(c) 2019-2020 Microsoft Corporation
+ *
+ * PCAP Next Generation Capture File writer
+ *
+ * See: https://github.com/pcapng/pcapng/ for the file format.
+ */
+#pragma once
+#include <stdint.h>
+
+enum pcapng_block_types
+{
+    PCAPNG_INTERFACE_BLOCK = 1,
+    PCAPNG_PACKET_BLOCK, /* Obsolete */
+    PCAPNG_SIMPLE_PACKET_BLOCK,
+    PCAPNG_NAME_RESOLUTION_BLOCK,
+    PCAPNG_INTERFACE_STATS_BLOCK,
+    PCAPNG_ENHANCED_PACKET_BLOCK,
+
+    PCAPNG_SECTION_BLOCK = 0x0A0D0D0A,
+};
+
+struct pcapng_option
+{
+    uint16_t code;
+    uint16_t length;
+    uint8_t data[];
+};
+
+#define PCAPNG_BYTE_ORDER_MAGIC 0x1A2B3C4D
+#define PCAPNG_MAJOR_VERS 1
+#define PCAPNG_MINOR_VERS 0
+
+enum pcapng_opt
+{
+    PCAPNG_OPT_END = 0,
+    PCAPNG_OPT_COMMENT = 1,
+};
+
+struct pcapng_section_header
+{
+    uint32_t block_type;
+    uint32_t block_length;
+    uint32_t byte_order_magic;
+    uint16_t major_version;
+    uint16_t minor_version;
+    uint64_t section_length;
+};
+
+enum pcapng_section_opt
+{
+    PCAPNG_SHB_HARDWARE = 2,
+    PCAPNG_SHB_OS = 3,
+    PCAPNG_SHB_USERAPPL = 4,
+};
+
+struct pcapng_interface_block
+{
+    uint32_t block_type; /* 1 */
+    uint32_t block_length;
+    uint16_t link_type;
+    uint16_t reserved;
+    uint32_t snap_len;
+};
+
+enum pcapng_interface_options
+{
+    PCAPNG_IFB_NAME = 2,
+    PCAPNG_IFB_DESCRIPTION,
+    PCAPNG_IFB_IPV4ADDR,
+    PCAPNG_IFB_IPV6ADDR,
+    PCAPNG_IFB_MACADDR,
+    PCAPNG_IFB_EUIADDR,
+    PCAPNG_IFB_SPEED,
+    PCAPNG_IFB_TSRESOL,
+    PCAPNG_IFB_TZONE,
+    PCAPNG_IFB_FILTER,
+    PCAPNG_IFB_OS,
+    PCAPNG_IFB_FCSLEN,
+    PCAPNG_IFB_TSOFFSET,
+    PCAPNG_IFB_HARDWARE,
+};
+
+struct pcapng_enhance_packet_block
+{
+    uint32_t block_type; /* 6 */
+    uint32_t block_length;
+    uint32_t interface_id;
+    uint32_t timestamp_hi;
+    uint32_t timestamp_lo;
+    uint32_t capture_length;
+    uint32_t original_length;
+};
+
+/* Flags values */
+#define PCAPNG_IFB_INBOUND 0b01
+#define PCAPNG_IFB_OUTBOUND 0b10
+
+enum pcapng_epb_options
+{
+    PCAPNG_EPB_FLAGS = 2,
+    PCAPNG_EPB_HASH,
+    PCAPNG_EPB_DROPCOUNT,
+    PCAPNG_EPB_PACKETID,
+    PCAPNG_EPB_QUEUE,
+    PCAPNG_EPB_VERDICT,
+};
+
+enum pcapng_epb_hash
+{
+    PCAPNG_HASH_2COMP = 0,
+    PCAPNG_HASH_XOR,
+    PCAPNG_HASH_CRC32,
+    PCAPNG_HASH_MD5,
+    PCAPNG_HASH_SHA1,
+    PCAPNG_HASH_TOEPLITZ,
+};
+
+struct pcapng_simple_packet
+{
+    uint32_t block_type; /* 3 */
+    uint32_t block_length;
+    uint32_t packet_length;
+};
+
+struct pcapng_statistics
+{
+    uint32_t block_type; /* 5 */
+    uint32_t block_length;
+    uint32_t interface_id;
+    uint32_t timestamp_hi;
+    uint32_t timestamp_lo;
+};
+
+enum pcapng_isb_options
+{
+    PCAPNG_ISB_STARTTIME = 2,
+    PCAPNG_ISB_ENDTIME,
+    PCAPNG_ISB_IFRECV,
+    PCAPNG_ISB_IFDROP,
+    PCAPNG_ISB_FILTERACCEPT,
+    PCAPNG_ISB_OSDROP,
+    PCAPNG_ISB_USRDELIV,
+};