#include "hv/config.h"
#include "hv/gicv3.h"
#include "hv/log.h"
#include "hv/mmio.h"
#include "hv/uart.h"
#include "hv/vcpu.h"

#define GICD_CTLR 0x0000u
#define GICD_TYPER 0x0004u
#define GICR_CTLR 0x0000u
#define GICR_WAKER 0x0014u
#define GICR_WAKER_PROCESSOR_SLEEP (1u << 1)
#define GICR_WAKER_CHILDREN_ASLEEP (1u << 2)

static uintptr_t gicd(uint32_t off)
{
    return (uintptr_t)(CONFIG_GICD_BASE + (uint64_t)off);
}

static uintptr_t gicr(uint32_t off)
{
    return (uintptr_t)(CONFIG_GICR_BASE + (uint64_t)off);
}

void gicv3_init(void)
{
    uint32_t typer = mmio_read32(gicd(GICD_TYPER));
    mmio_write32(gicd(GICD_CTLR), 0u);

    uint32_t waker = mmio_read32(gicr(GICR_WAKER));
    waker &= ~GICR_WAKER_PROCESSOR_SLEEP;
    mmio_write32(gicr(GICR_WAKER), waker);
    for (uint32_t i = 0; i < 100000u; i++) {
        if ((mmio_read32(gicr(GICR_WAKER)) & GICR_WAKER_CHILDREN_ASLEEP) == 0u) {
            break;
        }
    }

    __asm__ volatile(
        "msr ICC_SRE_EL2, %0\n"
        "isb\n"
        "msr ICC_SRE_EL1, %0\n"
        "isb\n"
        "msr ICC_PMR_EL1, %1\n"
        "msr ICC_IGRPEN1_EL1, %0\n"
        "isb\n"
        :: "r"(1ull), "r"(0xffull)
        : "memory");

    mmio_write32(gicd(GICD_CTLR), 0x3u);
    log_simple(LOG_BOOT, 20u, typer, 0u);
    gicv3_vcpu_init();
}

void gicv3_dump(void)
{
    uart_puts("gicv3 gicd=");
    uart_put_hex64(CONFIG_GICD_BASE);
    uart_puts(" gicr=");
    uart_put_hex64(CONFIG_GICR_BASE);
    uart_puts(" typer=");
    uart_put_hex64(mmio_read32(gicd(GICD_TYPER)));
    uart_puts("\n");
}

uint32_t gicv3_ack_irq(void)
{
    uint64_t irq;
    __asm__ volatile("mrs %0, ICC_IAR1_EL1" : "=r"(irq));
    return (uint32_t)(irq & 0x3ffu);
}

void gicv3_eoi(uint32_t irq)
{
    __asm__ volatile("msr ICC_EOIR1_EL1, %0\nisb" :: "r"((uint64_t)irq) : "memory");
}

void gicv3_vcpu_init(void)
{
    __asm__ volatile(
        "msr ICH_HCR_EL2, %0\n"
        "msr ICH_VMCR_EL2, %1\n"
        "msr ICH_LR0_EL2, xzr\n"
        "isb\n"
        :: "r"(1ull), "r"(0xff0000ull)
        : "memory");
}

void gicv3_sync_vcpu_state(struct vcpu *vcpu)
{
    uint64_t lr;
    uint64_t state;

    if (vcpu == (struct vcpu *)0 || vcpu->active_virtual_irq == 0u) {
        return;
    }

    __asm__ volatile("mrs %0, ICH_LR0_EL2" : "=r"(lr));
    state = (lr >> 62u) & 0x3u;
    if (state == 0u) {
        vcpu_clear_pending_irq(vcpu, (uint32_t)vcpu->active_virtual_irq);
        vcpu->active_virtual_irq = 0u;
        vcpu->stats.virtual_irqs_completed++;
    }
}

void gicv3_flush_vcpu_state(struct vcpu *vcpu)
{
    if (vcpu == (struct vcpu *)0) {
        return;
    }

    if ((vcpu->pending_virtual_irq & (1ull << CONFIG_VTIMER_IRQ)) == 0u) {
        vcpu->active_virtual_irq = 0u;
        __asm__ volatile("msr ICH_LR0_EL2, xzr\nisb" ::: "memory");
        return;
    }

    uint64_t lr = (uint64_t)CONFIG_VTIMER_IRQ | (1ull << 62u) | (1ull << 60u);
    if (vcpu->active_virtual_irq != CONFIG_VTIMER_IRQ) {
        vcpu->stats.virtual_irqs_injected++;
    }
    vcpu->active_virtual_irq = CONFIG_VTIMER_IRQ;
    __asm__ volatile("msr ICH_LR0_EL2, %0\nisb" :: "r"(lr) : "memory");
}