#include <stdint.h>
#include <memory.h>
#include <debug.h>

#define FLAGS_MASK (PAGE_SIZE-1)
#define MASK_FLAGS(addr) ((uint64_t)addr & ~FLAGS_MASK)

#define PRESENT(p) (p & PAGE_PRESENT)

#define PT(ptr) ((uint64_t *)P2V(MASK_FLAGS(ptr)))

#define P4E(P4, addr) (PT(P4)[P4_OFFSET(addr)])
#define P3E(P4, addr) (PT(P4E(P4, addr))[P3_OFFSET(addr)])
#define P2E(P4, addr) (PT(P3E(P4, addr))[P2_OFFSET(addr)])
#define P1E(P4, addr) (PT(P2E(P4, addr))[P1_OFFSET(addr)])


static int P1_exists(uint64_t P4, uint64_t addr)
{

    if (P4 && PRESENT(P4E(P4, addr)) && PRESENT(P3E(P4, addr)) && PRESENT(P2E(P4, addr)))
        return 1;
    return 0;
}

static int touch_P1(uint64_t P4, uint64_t addr, uint16_t flags)
{
    if (!P4) return -1;

    if(!PRESENT(P4E(P4, addr)) && (!(P4E(P4,addr) = pmm_calloc())))
        return -1;
    P4E(P4, addr) |= flags | PAGE_PRESENT;

    if(!PRESENT(P3E(P4, addr)) && (!(P3E(P4, addr) = pmm_calloc())))
        return -1;
    P3E(P4, addr) |= flags | PAGE_PRESENT;

    if(!PRESENT(P2E(P4, addr)) && (!(P2E(P4, addr) = pmm_calloc())))
        return -1;
    P2E(P4, addr) |= flags | PAGE_PRESENT;

    return 0;
}

uint64_t vmm_get_page(uint64_t P4, uint64_t addr)
{
    if(P1_exists(P4, addr))
        return P1E(P4, addr);
    return -1;
}

int vmm_set_page(uint64_t P4, uint64_t addr, uint64_t page, uint16_t flags)
{
    if(!P1_exists(P4, addr))
        if(touch_P1(P4, addr, flags))
            return -1;

    P1E(P4, addr) = page | flags;
    return 0;
}

void vmm_clear_page(uint64_t P4, uint64_t addr, int free)
{
    if(!P1_exists(P4, addr))
        return;

    uint64_t *pt;

    P1E(P4, addr) = 0;

    if(!free)
        return;

    pt = PT(P2E(P4, addr));
    for(int i = 0; i < ENTRIES_PER_PT; i++)
        if(pt[i])
            return;
    pmm_free(MASK_FLAGS(P2E(P4, addr)));
    P2E(P4, addr) = 0;

    pt = PT(P3E(P4, addr));
    for(int i = 0; i < ENTRIES_PER_PT; i++)
        if(pt[i])
            return;
    pmm_free(MASK_FLAGS(P3E(P4, addr)));
    P3E(P4, addr) = 0;

    pt = PT(P4E(P4, addr));
    for(int i = 0; i < ENTRIES_PER_PT; i++)
        if(pt[i])
            return;
    pmm_free(MASK_FLAGS(P4E(P4, addr)));
    P4E(P4, addr) = 0;
}