#include "hv/sysreg.h"
#include "hv/string.h"

uint64_t sysreg_key_make(uint32_t op0, uint32_t op1, uint32_t crn, uint32_t crm, uint32_t op2)
{
    return ((uint64_t)(op0 & 0x3u) << 14u) |
           ((uint64_t)(op1 & 0x7u) << 11u) |
           ((uint64_t)(crn & 0xfu) << 7u) |
           ((uint64_t)(crm & 0xfu) << 3u) |
           (uint64_t)(op2 & 0x7u);
}

bool sysreg_decode_from_esr_iss(uint64_t iss, struct sysreg_access *out)
{
    if (out == (struct sysreg_access *)0) {
        return false;
    }
    out->op0 = (uint32_t)((iss >> 20u) & 0x3u);
    out->op2 = (uint32_t)((iss >> 17u) & 0x7u);
    out->op1 = (uint32_t)((iss >> 14u) & 0x7u);
    out->crn = (uint32_t)((iss >> 10u) & 0xfu);
    out->rt = (uint32_t)((iss >> 5u) & 0x1fu);
    out->crm = (uint32_t)((iss >> 1u) & 0xfu);
    out->is_read = ((iss & 1u) == ESR_SYSREG_DIR_READ);
    out->key = sysreg_key_make(out->op0, out->op1, out->crn, out->crm, out->op2);
    return true;
}

struct sysreg_name {
    uint64_t key;
    const char *name;
};

#define KEY(op0, op1, crn, crm, op2) SYSREG_KEY_CONST((op0), (op1), (crn), (crm), (op2))

static const struct sysreg_name g_names[] = {
    { KEY(3, 0, 0, 0, 0), "midr_el1" },
    { KEY(3, 0, 0, 0, 5), "mpidr_el1" },
    { KEY(3, 0, 0, 4, 0), "id_aa64pfr0_el1" },
    { KEY(3, 0, 0, 4, 1), "id_aa64pfr1_el1" },
    { KEY(3, 0, 0, 6, 0), "id_aa64isar0_el1" },
    { KEY(3, 0, 0, 6, 1), "id_aa64isar1_el1" },
    { KEY(3, 0, 0, 7, 0), "id_aa64mmfr0_el1" },
    { KEY(3, 0, 0, 7, 1), "id_aa64mmfr1_el1" },
    { KEY(3, 1, 0, 0, 1), "clidr_el1" },
    { KEY(3, 2, 0, 0, 0), "csselr_el1" },
    { KEY(3, 3, 0, 0, 1), "ctr_el0" },
    { KEY(3, 3, 0, 0, 7), "dczid_el0" },
    { KEY(3, 0, 1, 0, 0), "sctlr_el1" },
    { KEY(3, 0, 1, 0, 1), "actlr_el1" },
    { KEY(3, 0, 1, 0, 2), "cpacr_el1" },
    { KEY(3, 4, 1, 1, 0), "hcr_el2" },
    { KEY(3, 6, 1, 1, 0), "scr_el3" },
    { KEY(3, 0, 2, 0, 0), "ttbr0_el1" },
    { KEY(3, 0, 2, 0, 1), "ttbr1_el1" },
    { KEY(3, 0, 2, 0, 2), "tcr_el1" },
    { KEY(3, 4, 2, 1, 0), "vttbr_el2" },
    { KEY(3, 3, 9, 12, 0), "pmcr_el0" },
    { KEY(3, 0, 10, 2, 0), "mair_el1" },
    { KEY(3, 0, 12, 0, 0), "vbar_el1" },
    { KEY(3, 0, 12, 0, 1), "rvbar_el1" },
    { KEY(3, 0, 13, 0, 1), "contextidr_el1" },
    { KEY(3, 0, 13, 0, 4), "tpidr_el1" },
    { KEY(3, 3, 13, 0, 3), "tpidrro_el0" },
    { KEY(3, 0, 14, 1, 0), "cntkctl_el1" },
    { KEY(3, 3, 14, 3, 0), "cntv_tval_el0" },
    { KEY(3, 3, 14, 3, 1), "cntv_ctl_el0" },
    { KEY(3, 3, 14, 3, 2), "cntv_cval_el0" },
    { KEY(2, 0, 0, 2, 2), "mdscr_el1" },
};

const char *sysreg_key_name(uint64_t key)
{
    for (uint32_t i = 0; i < HV_ARRAY_SIZE(g_names); i++) {
        if (g_names[i].key == key) {
            return g_names[i].name;
        }
    }
    return "unknown";
}

const char *esr_ec_name(uint32_t ec)
{
    switch (ec) {
    case ESR_EC_UNKNOWN: return "unknown";
    case ESR_EC_WFI_WFE: return "wfi-wfe";
    case ESR_EC_HVC64: return "hvc64";
    case ESR_EC_SMC64: return "smc64";
    case ESR_EC_SYSREG: return "sysreg";
    case ESR_EC_IABT_LOWER: return "iabt-lower";
    case ESR_EC_DABT_LOWER: return "dabt-lower";
    case ESR_EC_BRK64: return "brk64";
    default: return "reserved";
    }
}

bool sysreg_selftest(void)
{
    struct sysreg_access a;
    uint64_t iss = (3ull << 20u) | (0ull << 17u) | (0ull << 14u) |
                   (0ull << 10u) | (0ull << 5u) | (0ull << 1u) | 1ull;
    if (!sysreg_decode_from_esr_iss(iss, &a)) {
        return false;
    }
    return a.is_read && a.rt == 0u && a.key == KEY(3, 0, 0, 0, 0) &&
           hv_strcmp(esr_ec_name(ESR_EC_SYSREG), "sysreg") == 0;
}