#include "kernel/aplic.h"

static volatile char* APLIC_M = (volatile char*)0x0c00'0000;
static volatile char* APLIC_S = (volatile char*)0x0d00'0000;

static volatile char* IMSIC_M = (volatile char*)0x2400'0000;
static volatile char* IMSIC_S = (volatile char*)0x2800'0000;


static inline uint32_t read(volatile char* base, uint32_t offset)
{
    return *(volatile uint32_t*)(base + offset);
}

static inline void write(volatile char* base, uint32_t offset, uint32_t v)
{
    *(volatile uint32_t*)(base + offset) = v;
}

enum
{
    DOMAINCFG_BASE      = 0x8000'0000,
    DOMAINCFG_DM        = 0x0000'0004,
    DOMAINCFG_DM_MSI    = 0x0000'0004,
    DOMAINCFG_IE        = 0x0000'0100,
};

bool aplic_w_domaincfg(volatile char* base, uint32_t value)
{
    write(base, 0, value);
    return (read(base, 0) & DOMAINCFG_DM) == (value & DOMAINCFG_DM);
}

void aplic_w_sourcecfg(volatile char* base, uint32_t source, uint32_t v)
{
    write(base, 4 + 4 * (source - 1), v);
}

void aplic_w_sourcecfg_delegate(volatile char* base, uint32_t source, uint32_t child)
{
    aplic_w_sourcecfg(base, source, (1U << 10) | (child & 0x3ff));
}

void aplic_w_sourcecfg_active_high(volatile char* base, uint32_t source)
{
    aplic_w_sourcecfg(base, source, 6);
}

void aplic_w_smsiaddr(volatile char* base, void* addr)
{
    write(base, 0x1BC8, (uint32_t)addr >> 12);
    write(base, 0x1BCC, 0);
}

void aplic_enable_intr(volatile char* base, uint32_t source)
{
    write(base, 0x1EDC, source);
}

void aplic_w_target_msi(volatile char* base, uint32_t source, uint32_t hart_index, uint32_t guest_index, uint32_t eiid)
{
    uint32_t v = ((hart_index & 0x3fff) << 18) | ((guest_index & 0x3f) << 12) | (eiid & 0x3ff);
    write(base, 0x3000 + 4 * source, v);
}

enum
{
    EIDELIVERY  = 0x70,
    EITHRESHOLD = 0x72,
    EIE0        = 0xC0,
};

enum
{
    EIDELIVERY_DISABLED = 0,
    EIDELIVERY_ENABLED  = 1,
};

void imsic_w_seidelivery(uint32_t v)
{
    __asm__ volatile(
        "csrw siselect, %0;"
        "csrw sireg, %1;"
        :: "r"(EIDELIVERY), "r"(v));
}

void imsic_w_seithreshold(uint32_t v)
{
    __asm__ volatile(
        "csrw siselect, %0;"
        "csrw sireg, %1;"
        :: "r"(EITHRESHOLD), "r"(v));
}

void imsic_s_seie(uint32_t id)
{
    __asm__ volatile(
        "csrw siselect, %0;"
        "csrs sireg, %1;"
        :: "r"(EIE0 + id / 32), "r"(1U << (id % 32)));
}

void aplic_init()
{
    uint32_t domaincfg = DOMAINCFG_BASE | DOMAINCFG_DM_MSI | DOMAINCFG_IE;
    if (!aplic_w_domaincfg(APLIC_M, domaincfg)) panic("aplic_init: enable APLIC M");
    if (!aplic_w_domaincfg(APLIC_S, domaincfg)) panic("aplic_init: enable APLIC S");
    
    aplic_w_smsiaddr(APLIC_M, (void*)IMSIC_S);

    imsic_w_seithreshold(0);
    imsic_w_seidelivery(EIDELIVERY_ENABLED);
}

void aplic_enable_intr_s(uint32_t source, uint32_t msg)
{
    aplic_w_sourcecfg_delegate(APLIC_M, source, 0);
    aplic_w_sourcecfg_active_high(APLIC_S, source);
    aplic_w_target_msi(APLIC_S, source, 0, 0, msg);
    aplic_enable_intr(APLIC_S, source);
    imsic_s_seie(msg);
}

uint32_t aplic_claim_msi_s()
{
    uint32_t intr;
    __asm__ volatile("csrrw %0, stopei, zero" : "=r"(intr));
    return intr >> 16;
}