MicrOBU/obu-firmware/test/host/test_util.c

#ifndef _WIN32
#define _DEFAULT_SOURCE // MAP_ANONYMOUS under -std=c11
#endif

#include "test_util.h"

#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef _WIN32
#include <windows.h>
#else
#include <signal.h>
#include <sys/mman.h>
#include <unistd.h>
#endif

// ---- Crash reporting ------------------------------------------------------------------------

static char s_context[200] = "startup";
static const uint8_t *s_crash_bytes;
static const int *s_crash_len;

void tu_set_context(const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    vsnprintf(s_context, sizeof s_context, fmt, ap);
    va_end(ap);
}

const char *tu_context(void)
{
    return s_context;
}

void tu_set_crash_input(const uint8_t *bytes, const int *len)
{
    s_crash_bytes = bytes;
    s_crash_len = len;
}

static void report_crash(const char *what)
{
    fprintf(stderr, "CRASH (%s) during: %s\n", what, s_context);
    if (s_crash_bytes && s_crash_len) {
        fprintf(stderr, "input (%d bytes): ", *s_crash_len);
        for (int i = 0; i < *s_crash_len; i++) {
            fprintf(stderr, "%02x", s_crash_bytes[i]);
        }
        fputc('\n', stderr);
    }
    fflush(stderr);
}

#ifdef _WIN32
static LONG WINAPI on_crash(EXCEPTION_POINTERS *info)
{
    char what[40];
    snprintf(what, sizeof what, "exception 0x%08lx",
             (unsigned long)info->ExceptionRecord->ExceptionCode);
    report_crash(what);
    return EXCEPTION_EXECUTE_HANDLER; // terminate, with the exception code as the exit status
}

void tu_install_crash_handler(void)
{
    SetUnhandledExceptionFilter(on_crash);
}
#else
static void on_crash(int sig)
{
    char what[40];
    snprintf(what, sizeof what, "signal %d", sig);
    report_crash(what); // not async-signal-safe, but the process is ending anyway
    _exit(2);
}

void tu_install_crash_handler(void)
{
    signal(SIGSEGV, on_crash);
    signal(SIGBUS, on_crash);
    signal(SIGILL, on_crash); // -fsanitize-undefined-trap-on-error traps with an illegal instruction
}
#endif

// ---- Guard page -----------------------------------------------------------------------------

static uint8_t *s_guard_end; // first byte of the no-access page
static size_t s_page_size;

void tu_guard_init(void)
{
#ifdef _WIN32
    SYSTEM_INFO si;
    GetSystemInfo(&si);
    s_page_size = si.dwPageSize;
    uint8_t *base = VirtualAlloc(NULL, 2 * s_page_size, MEM_RESERVE | MEM_COMMIT, PAGE_READWRITE);
    DWORD old;
    if (!base || !VirtualProtect(base + s_page_size, s_page_size, PAGE_NOACCESS, &old)) {
        fprintf(stderr, "guard page setup failed\n");
        exit(2);
    }
#else
    s_page_size = (size_t)sysconf(_SC_PAGESIZE);
    uint8_t *base = mmap(NULL, 2 * s_page_size, PROT_READ | PROT_WRITE,
                         MAP_PRIVATE | MAP_ANONYMOUS, -1, 0);
    if (base == MAP_FAILED || mprotect(base + s_page_size, s_page_size, PROT_NONE) != 0) {
        fprintf(stderr, "guard page setup failed\n");
        exit(2);
    }
#endif
    s_guard_end = base + s_page_size;
}

uint8_t *tu_guard_buf(int len)
{
    if (len < 0 || (size_t)len > s_page_size) {
        fprintf(stderr, "tu_guard_buf(%d) exceeds one page\n", len);
        exit(2);
    }
    return s_guard_end - len;
}

const uint8_t *tu_guarded(const uint8_t *frame, int len)
{
    uint8_t *dst = tu_guard_buf(len);
    if (len > 0) {
        memcpy(dst, frame, (size_t)len);
    }
    return dst;
}

// ---- pcap -----------------------------------------------------------------------------------

static uint32_t rd_le32(const uint8_t *p)
{
    return (uint32_t)p[0] | ((uint32_t)p[1] << 8) | ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
}

static uint32_t rd_be32(const uint8_t *p)
{
    return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | ((uint32_t)p[2] << 8) | (uint32_t)p[3];
}

int tu_pcap_foreach(const char *path, tu_frame_fn fn, void *ctx)
{
    FILE *f = fopen(path, "rb");
    if (!f) {
        return -1;
    }
    fseek(f, 0, SEEK_END);
    const long size = ftell(f);
    fseek(f, 0, SEEK_SET);
    if (size < 24) {
        fclose(f);
        return size == 0 ? 0 : -1; // the recordings include empty files
    }
    uint8_t *d = malloc((size_t)size);
    if (!d || fread(d, 1, (size_t)size, f) != (size_t)size) {
        fclose(f);
        free(d);
        return -1;
    }
    fclose(f);

    const uint32_t magic = rd_le32(d);
    const bool swapped = magic == 0xD4C3B2A1u || magic == 0x4D3CB2A1u;
    if (!swapped && magic != 0xA1B2C3D4u && magic != 0xA1B23C4Du) {
        free(d);
        return -1;
    }
    uint32_t (*u32)(const uint8_t *) = swapped ? rd_be32 : rd_le32;
    const uint32_t linktype = u32(d + 20);
    if (linktype != 127 && linktype != 105) {
        free(d);
        return -1;
    }

    long off = 24;
    int index = 0;
    while (off + 16 <= size) {
        const uint32_t incl = u32(d + off + 8);
        if (incl > (uint32_t)(size - off - 16)) {
            break; // last record cut off
        }
        const uint8_t *pkt = d + off + 16;
        int len = (int)incl;
        off += 16 + (long)incl;
        if (linktype == 127) {
            const int rt_len = len >= 4 ? (pkt[2] | (pkt[3] << 8)) : len + 1; // always little-endian
            if (rt_len > len) {
                index++;
                continue;
            }
            pkt += rt_len;
            len -= rt_len;
        }
        fn(pkt, len, index++, ctx);
    }
    free(d);
    return index;
}

static FILE *s_pcap_out;

static void put_le32(uint8_t *p, uint32_t v)
{
    p[0] = (uint8_t)v;
    p[1] = (uint8_t)(v >> 8);
    p[2] = (uint8_t)(v >> 16);
    p[3] = (uint8_t)(v >> 24);
}

bool tu_pcap_open(const char *path)
{
    // Global header: magic, version 2.4, zone 0, sigfigs 0, snaplen 65535, linktype 105.
    static const uint8_t hdr[24] = {
        0xD4, 0xC3, 0xB2, 0xA1, 0x02, 0x00, 0x04, 0x00, 0, 0, 0, 0, 0, 0, 0, 0,
        0xFF, 0xFF, 0x00, 0x00, 105, 0, 0, 0,
    };
    s_pcap_out = fopen(path, "wb");
    return s_pcap_out && fwrite(hdr, 1, sizeof hdr, s_pcap_out) == sizeof hdr;
}

void tu_pcap_add(const uint8_t *frame, int len)
{
    static uint32_t seq;
    if (!s_pcap_out || len <= 0) {
        return;
    }
    uint8_t rec[16];
    put_le32(rec + 0, seq++); // timestamp seconds: just the frame's sequence number
    put_le32(rec + 4, 0);
    put_le32(rec + 8, (uint32_t)len);
    put_le32(rec + 12, (uint32_t)len);
    fwrite(rec, 1, sizeof rec, s_pcap_out);
    fwrite(frame, 1, (size_t)len, s_pcap_out);
}

void tu_pcap_close(void)
{
    if (s_pcap_out) {
        fclose(s_pcap_out);
        s_pcap_out = NULL;
    }
}