ccore/src/Utility.c

#include "core/Utility.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <threads.h>
#include <time.h>

#include "ErrorSimulator.h"
#include "core/Logger.h"

extern inline size_t coreMaxSize(size_t a, size_t b);
extern inline size_t coreMinSize(size_t a, size_t b);
extern inline void coreSwapSize(size_t* a, size_t* b);
extern inline void coreSwapPointer(void** a, void** b);

static CoreExitHandler exitHandler = nullptr;
static void* exitData = nullptr;
static CoreOutOfMemoryHandler outOfMemoryHandler = nullptr;
static void* outOfMemoryData = nullptr;

size_t corePopCount(u64 u) {
    static const u64 map[16] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4};
    size_t sum = 0;
    for(size_t i = 0; i < sizeof(u64) * 8; i += 4) {
        sum += map[(u >> i) & 0xF];
    }
    return sum;
}

size_t coreToStringSize(const void* p, char* buffer, size_t n) {
    int w = snprintf(buffer, n, "%zu", *(const size_t*)p);
    return w < 0 ? 0 : (size_t)w;
}

void coreStringAdd(size_t* w, char** buffer, size_t* n, size_t shift) {
    *w += shift;
    if(*n > shift) {
        *buffer += shift;
        *n -= shift;
    } else {
        *n = 0;
    }
}

void coreStringAddI(size_t* w, char** buffer, size_t* n, int shift) {
    coreStringAdd(w, buffer, n, shift < 0 ? 0 : (size_t)shift);
}

[[noreturn]] void coreExitWithHandler(const char* file, int line, int value) {
    if(value != 0) {
        file = coreGetShortFileName(file);
        CORE_LOG_ERROR("Exit from %s:%d with value %d", file, line, value);
    }
    if(exitHandler != nullptr) {
        exitHandler(value, exitData);
    }
    exit(value);
}

void coreSetExitHandler(CoreExitHandler eh, void* data) {
    exitHandler = eh;
    exitData = data;
}

void coreSetOutOfMemoryHandler(CoreOutOfMemoryHandler h, void* data) {
    outOfMemoryHandler = h;
    outOfMemoryData = data;
}

static void* exitOnNull(void* p, size_t n) {
    if(p == nullptr) {
        CORE_LOG_ERROR("Out of memory, requested '%zu' bytes", n);
        CORE_EXIT(1);
    }
    return p;
}

static void* coreRealAllocate(size_t n) {
    void* p = malloc(n);
    while(p == nullptr && outOfMemoryHandler != nullptr) {
        outOfMemoryHandler(outOfMemoryData);
        p = malloc(n);
    }
    return exitOnNull(p, n);
}

static void* coreRealReallocate(void* oldP, size_t n) {
    if(n <= 0) {
        free(oldP);
        return nullptr;
    }
    void* p = realloc(oldP, n);
    if(p == nullptr) {
        while(p == nullptr && outOfMemoryHandler != nullptr) {
            outOfMemoryHandler(outOfMemoryData);
            p = realloc(oldP, n);
        }
    }
    return exitOnNull(p, n);
}

static void coreRealFree(void* p) {
    free(p);
}

#ifdef CORE_CHECK_MEMORY
static const u8 CANARY[16] = {0xDE, 0xAD, 0xBE, 0xEF, 0xDE, 0xAD, 0xBE, 0xEF,
                              0xDE, 0xAD, 0xBE, 0xEF, 0xDE, 0xAD, 0xBE, 0xEF};

struct CoreMemoryInfo;
typedef struct CoreMemoryInfo CoreMemoryInfo;
struct CoreMemoryInfo {
    CoreMemoryInfo* next;
    CoreMemoryInfo* previous;
    size_t size;
    int line;
    char buffer[64 - 2 * sizeof(void*) - sizeof(size_t) - sizeof(int)];
    char canary[sizeof(CANARY)];
};
static_assert(sizeof(CoreMemoryInfo) == 80, "memory info has invalid size");
static CoreMemoryInfo* headMemoryInfo = nullptr;

static void addMemoryInfo(CoreMemoryInfo* info, const char* file, int line,
                          size_t n) {
    info->next = nullptr;
    info->previous = nullptr;
    info->size = n;
    info->line = line;
    strncpy(info->buffer, coreGetShortFileName(file), sizeof(info->buffer));
    memcpy(info->canary, CANARY, sizeof(CANARY));
    memcpy((char*)info + n - sizeof(CANARY), CANARY, sizeof(CANARY));
    if(headMemoryInfo == nullptr) {
        headMemoryInfo = info;
    } else {
        headMemoryInfo->previous = info;
        info->next = headMemoryInfo;
        headMemoryInfo = info;
    }
}

static void removeMemoryInfo(CoreMemoryInfo* info) {
    if(info->previous == nullptr) {
        if(info->next == nullptr) {
            headMemoryInfo = nullptr;
        } else {
            headMemoryInfo = info->next;
            info->next->previous = nullptr;
        }
    } else {
        if(info->next == nullptr) {
            info->previous->next = nullptr;
        } else {
            info->previous->next = info->next;
            info->next->previous = info->previous;
        }
    }
}

void* coreDebugAllocate(const char* file, int line, size_t n) {
    n += sizeof(CoreMemoryInfo) + sizeof(CANARY);
    void* p = coreRealAllocate(n + sizeof(CANARY));
    addMemoryInfo(p, file, line, n);
    return (char*)p + sizeof(CoreMemoryInfo);
}

void* coreDebugReallocate(const char* file, int line, void* p, size_t n) {
    if(n > 0) {
        n += sizeof(CoreMemoryInfo) + sizeof(CANARY);
    }
    void* rp = (void*)p;
    if(rp != nullptr) {
        rp = (char*)rp - sizeof(CoreMemoryInfo);
        removeMemoryInfo(rp);
    }
    void* np = coreRealReallocate(rp, n);
    if(np == nullptr) {
        return nullptr;
    }
    addMemoryInfo(np, file, line, n);
    return (char*)np + sizeof(CoreMemoryInfo);
}

static bool checkCanary(void* p) {
    return memcmp(p, CANARY, sizeof(CANARY)) != 0;
}

void coreFreeDebug(const char* file, int line, void* p) {
    if(p == nullptr) {
        return;
    }
    CoreMemoryInfo* rp = nullptr;
    void* w = (char*)p - sizeof(CoreMemoryInfo);
    memcpy(&rp, &w, sizeof(rp));

    // CoreMemoryInfo* rp = (CoreMemoryInfo*)((char*)p -
    // sizeof(CoreMemoryInfo));
    if(checkCanary(rp->canary)) {
        file = coreGetShortFileName(file);
        CORE_LOG_ERROR("Free at %s:%d violated pre canary", file, line);
        CORE_EXIT(1);
    } else if(checkCanary((char*)rp + rp->size - sizeof(CANARY))) {
        file = coreGetShortFileName(file);
        CORE_LOG_ERROR("Free at %s:%d violated post canary", file, line);
        CORE_EXIT(1);
    }
    removeMemoryInfo(rp);
    coreRealFree(rp);
}

void corePrintMemoryReport() {
    for(CoreMemoryInfo* i = headMemoryInfo; i != nullptr; i = i->next) {
        CORE_LOG_ERROR("%s:%d was not freed", i->buffer, i->line);
    }
}

#else

void* coreAllocate(size_t n) {
    return coreRealAllocate(n);
}

void* coreReallocate(void* p, size_t n) {
    return coreRealReallocate(p, n);
}

void coreFree(void* p) {
    coreRealFree(p);
}

#endif

bool coreSleep(i64 nanos) {
    struct timespec t;
    t.tv_nsec = nanos % 1'000'000'000;
    t.tv_sec = nanos / 1'000'000'000;
    return thrd_sleep(&t, nullptr) != 0;
}

i64 coreNanos(void) {
    struct timespec ts;
    if(timespec_get(&ts, TIME_UTC) == 0 || CORE_TIME_GET_FAIL) {
        return -1;
    }
    return (i64)ts.tv_sec * 1'000'000'000L + (i64)ts.tv_nsec;
}