/*
 * kido-psci.c — Kido ARM PSCI conduit (DEN0022).
 *
 * See kido-psci.h for the function IDs, the return-code law,
 * and the stated divergences. This file is the real logic:
 * the hosted build runs the SAME functions against a scripted
 * firmware double (version, a 4-CPU state table, call
 * records) instead of the `smc` instruction. The bare-metal
 * path is WRITTEN, NOT ASSEMBLED, NEVER EXECUTED (toolchain
 * absent, 15th data point — see the deliberation record);
 * the QEMU virt PSCI proof is queued.
 *
 * TESTING: gcc -std=c11 -Wall -Wextra -Werror -DKIDO_HOSTED_TEST
 *        -DKIDO_PSCI_SELFTEST kido-psci.c -o psci-selftest
 */

#include "kido-psci.h"

#ifdef KIDO_PSCI_SELFTEST
#ifndef KIDO_HOSTED_TEST
#error "KIDO_PSCI_SELFTEST needs KIDO_HOSTED_TEST."
#endif
#endif

/* Driver state. Static BSS: exists before any allocator. */
static uint32_t kido_psci_version_state = 0u;
static int      kido_psci_ready = 0;

#if defined(KIDO_HOSTED_TEST)

/* ------------------------------------------------------------------ */
/* Hosted firmware double. Scripted PSCI monitor: a version, a */
/* 4-CPU state table (values 0..3 are a stand-in, NOT MPIDR */
/* format), and call records for the never-returning calls. */
/* The double enforces the firmware's own validations (entry */
/* != 0, known affinity) so the driver's error mapping is */
/* exercised, not assumed. SYSTEM_OFF / SYSTEM_RESET / */
/* CPU_OFF record and return SUCCESS here — on bare metal */
/* they never return, and the divergence is stated in the */
/* header, not hidden in the double. */
/* ------------------------------------------------------------------ */
#include <stdio.h>

static uint32_t kido_dbl_version = 0x00010000u; /* PSCI 1.0 */
static uint32_t kido_dbl_cpu_state[4] = {
    KIDO_PSCI_AFFINITY_ON, KIDO_PSCI_AFFINITY_OFF,
    KIDO_PSCI_AFFINITY_OFF, KIDO_PSCI_AFFINITY_OFF
};
static uint32_t kido_dbl_system_off = 0u;
static uint32_t kido_dbl_system_reset = 0u;
static uint32_t kido_dbl_suspend_count = 0u;
static uint32_t kido_dbl_last_suspend_state = 0u;
static uint64_t kido_dbl_last_suspend_entry = 0u;
static uint64_t kido_dbl_last_suspend_ctx = 0u;

static int kido_psci_is_known_fid(uint32_t fid)
{
    return fid == KIDO_PSCI_VERSION ||
           fid == KIDO_PSCI_CPU_SUSPEND ||
           fid == KIDO_PSCI_CPU_OFF ||
           fid == KIDO_PSCI_CPU_ON ||
           fid == KIDO_PSCI_AFFINITY_INFO ||
           fid == KIDO_PSCI_SYSTEM_OFF ||
           fid == KIDO_PSCI_SYSTEM_RESET ||
           fid == KIDO_PSCI_FEATURES;
}

static int64_t kido_smc(uint32_t fid, uint64_t a1, uint64_t a2,
                        uint64_t a3)
{
    (void)a2;
    switch (fid) {
    case KIDO_PSCI_VERSION:
        return (int64_t)kido_dbl_version;
    case KIDO_PSCI_FEATURES:
        /* FEATURES exists only from PSCI 1.0; the driver
         * pre-checks this, but the double enforces it too
         * so a direct double call cannot fake a 0.2 answer. */
        if (kido_dbl_version < 0x00010000u)
            return KIDO_PSCI_NOT_SUPPORTED;
        if (!kido_psci_is_known_fid((uint32_t)a1))
            return KIDO_PSCI_NOT_SUPPORTED;
        return 0; /* no extra feature bits for any known ID */
    case KIDO_PSCI_CPU_ON:
        if (a1 >= 4u)
            return KIDO_PSCI_NOT_PRESENT;
        if (a2 == 0u)
            return KIDO_PSCI_INVALID_ADDRESS;
        if (kido_dbl_cpu_state[a1] == KIDO_PSCI_AFFINITY_ON)
            return KIDO_PSCI_ALREADY_ON;
        if (kido_dbl_cpu_state[a1] == KIDO_PSCI_AFFINITY_ON_PENDING)
            return KIDO_PSCI_ON_PENDING;
        kido_dbl_cpu_state[a1] = KIDO_PSCI_AFFINITY_ON;
        (void)a3;
        return KIDO_PSCI_SUCCESS;
    case KIDO_PSCI_CPU_OFF:
        /* Hosted divergence, stated in the header: the host
         * CPU cannot power down, so the call is recorded
         * only by returning SUCCESS. No state is changed:
         * CPU_OFF has no target CPU to mark off. */
        return KIDO_PSCI_SUCCESS;
    case KIDO_PSCI_CPU_SUSPEND:
        kido_dbl_suspend_count++;
        kido_dbl_last_suspend_state = (uint32_t)a1;
        kido_dbl_last_suspend_entry = a2;
        kido_dbl_last_suspend_ctx = a3;
        return KIDO_PSCI_SUCCESS;
    case KIDO_PSCI_AFFINITY_INFO:
        if (a1 >= 4u)
            return KIDO_PSCI_NOT_PRESENT;
        return (int64_t)kido_dbl_cpu_state[a1];
    case KIDO_PSCI_SYSTEM_OFF:
        kido_dbl_system_off = 1u;
        return KIDO_PSCI_SUCCESS;
    case KIDO_PSCI_SYSTEM_RESET:
        kido_dbl_system_reset = 1u;
        return KIDO_PSCI_SUCCESS;
    default:
        return KIDO_PSCI_NOT_SUPPORTED;
    }
}

#else /* bare metal: the real SMC conduit (SMCCC). */

static int64_t kido_smc(uint32_t fid, uint64_t a1, uint64_t a2,
                        uint64_t a3)
{
    uint64_t r0 = (uint64_t)fid, r1 = a1, r2 = a2, r3 = a3;
    __asm__ volatile("smc #0"
                     : "+r" (r0), "+r" (r1), "+r" (r2), "+r" (r3)
                     :
                     : "memory");
    return (int64_t)r0;
}

#endif /* KIDO_HOSTED_TEST */

/* Map a firmware return to a kido status. SUCCESS is OK;
 * NOT_SUPPORTED is STUB_REACHED (the firmware does not
 * implement the function); every other negative is
 * EARLY_ENV. No new header code, per the header's law. */
static kido_status_t kido_psci_map(int64_t r)
{
    if (r == KIDO_PSCI_SUCCESS)
        return KIDO_OK;
    if (r == KIDO_PSCI_NOT_SUPPORTED)
        return KIDO_ERR_STUB_REACHED;
    return KIDO_ERR_EARLY_ENV;
}

kido_status_t kido_psci_init(void)
{
    int64_t r = kido_smc(KIDO_PSCI_VERSION, 0u, 0u, 0u);
    uint32_t major, minor;

    if (r < 0) {
        kido_psci_ready = 0;
        kido_psci_version_state = 0u;
        return KIDO_ERR_EARLY_ENV;
    }
    major = ((uint32_t)r >> 16) & 0xFFFFu;
    minor = (uint32_t)r & 0xFFFFu;
    if (major == 0u && minor < 2u) {
        /* Pre-0.2 is not a PSCI monitor this driver speaks. */
        kido_psci_ready = 0;
        kido_psci_version_state = 0u;
        return KIDO_ERR_EARLY_ENV;
    }
    kido_psci_version_state = (uint32_t)r;
    kido_psci_ready = 1;
    return KIDO_OK;
}

kido_status_t kido_psci_version(uint32_t *out)
{
    if (out == NULL)
        return KIDO_ERR_EARLY_ENV;
    if (!kido_psci_ready)
        return KIDO_ERR_EARLY_ENV;
    *out = kido_psci_version_state;
    return KIDO_OK;
}

kido_status_t kido_psci_features(uint32_t fid, uint32_t *out)
{
    int64_t r;

    if (out == NULL)
        return KIDO_ERR_EARLY_ENV;
    if (!kido_psci_ready)
        return KIDO_ERR_EARLY_ENV;
    if (kido_psci_version_state < 0x00010000u)
        return KIDO_ERR_STUB_REACHED;
    r = kido_smc(KIDO_PSCI_FEATURES, (uint64_t)fid, 0u, 0u);
    if (r < 0)
        return kido_psci_map(r);
    *out = (uint32_t)r;
    return KIDO_OK;
}

kido_status_t kido_psci_cpu_on(uint64_t affinity, uint64_t entry,
                                uint64_t context_id)
{
    int64_t r;

    if (!kido_psci_ready)
        return KIDO_ERR_EARLY_ENV;
    /* No driver-side validation: entry == 0 and unknown
     * affinity are firmware validations (INVALID_ADDRESS /
     * NOT_PRESENT), and the mapping above carries them.
     * Second-guessing the firmware here would split the
     * contract between two validators. */
    r = kido_smc(KIDO_PSCI_CPU_ON, affinity, entry, context_id);
    return kido_psci_map(r);
}

kido_status_t kido_psci_cpu_off(void)
{
    int64_t r;

    if (!kido_psci_ready)
        return KIDO_ERR_EARLY_ENV;
    r = kido_smc(KIDO_PSCI_CPU_OFF, 0u, 0u, 0u);
    return kido_psci_map(r);
}

kido_status_t kido_psci_cpu_suspend(uint32_t power_state,
                                     uint64_t entry,
                                     uint64_t context_id)
{
    int64_t r;

    if (!kido_psci_ready)
        return KIDO_ERR_EARLY_ENV;
    r = kido_smc(KIDO_PSCI_CPU_SUSPEND, (uint64_t)power_state,
                 entry, context_id);
    return kido_psci_map(r);
}

kido_status_t kido_psci_affinity_info(uint64_t affinity,
                                       uint32_t lowest_level,
                                       uint32_t *out)
{
    int64_t r;

    if (out == NULL)
        return KIDO_ERR_EARLY_ENV;
    if (!kido_psci_ready)
        return KIDO_ERR_EARLY_ENV;
    r = kido_smc(KIDO_PSCI_AFFINITY_INFO, affinity,
                 (uint64_t)lowest_level, 0u);
    if (r < 0)
        return kido_psci_map(r);
    if (r > 2)
        return KIDO_ERR_EARLY_ENV;
    *out = (uint32_t)r;
    return KIDO_OK;
}

kido_status_t kido_psci_system_off(void)
{
    int64_t r;

    if (!kido_psci_ready)
        return KIDO_ERR_EARLY_ENV;
    r = kido_smc(KIDO_PSCI_SYSTEM_OFF, 0u, 0u, 0u);
    return kido_psci_map(r);
}

kido_status_t kido_psci_system_reset(void)
{
    int64_t r;

    if (!kido_psci_ready)
        return KIDO_ERR_EARLY_ENV;
    r = kido_smc(KIDO_PSCI_SYSTEM_RESET, 0u, 0u, 0u);
    return kido_psci_map(r);
}

#ifdef KIDO_PSCI_SELFTEST

/* Hosted-double setters (test only). */
void kido_psci_test_set_version(uint32_t version)
{
    kido_dbl_version = version;
}

void kido_psci_test_set_cpu_state(uint64_t affinity, uint32_t state)
{
    if (affinity < 4u)
        kido_dbl_cpu_state[affinity] = state;
}

void kido_psci_test_get_records(uint32_t *off_called,
                                uint32_t *reset_called,
                                uint32_t *suspend_count)
{
    if (off_called != NULL)
        *off_called = kido_dbl_system_off;
    if (reset_called != NULL)
        *reset_called = kido_dbl_system_reset;
    if (suspend_count != NULL)
        *suspend_count = kido_dbl_suspend_count;
}

static unsigned kido_psci_checks = 0u;
static unsigned kido_psci_failures = 0u;

#define PSCI_CHECK(cond) do { \
    kido_psci_checks++; \
    if (!(cond)) { \
        kido_psci_failures++; \
        printf("[FAIL] line %d: %s\n", __LINE__, #cond); \
    } \
} while (0)

int kido_psci_selftest_main(void)
{
    uint32_t ver = 0u, feats = 0u, state = 0u;
    uint32_t off = 0u, reset = 0u, susp = 0u;

    /* §1: pre-init guards — every call refuses. */
    PSCI_CHECK(kido_psci_version(&ver) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_features(KIDO_PSCI_CPU_ON, &feats) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_cpu_on(1u, 0x40080000u, 0u) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_cpu_off() == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_cpu_suspend(0u, 0x40080000u, 0u) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_affinity_info(0u, 0u, &state) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_system_off() == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_system_reset() == KIDO_ERR_EARLY_ENV);

    /* §2: init at the default (PSCI 1.0); version recorded. */
    PSCI_CHECK(kido_psci_init() == KIDO_OK);
    PSCI_CHECK(kido_psci_version(&ver) == KIDO_OK && ver == 0x00010000u);

    /* §3: FEATURES at 1.0 — known IDs answer 0 bits,
     * unknown IDs are NOT_SUPPORTED -> STUB_REACHED. */
    PSCI_CHECK(kido_psci_features(KIDO_PSCI_CPU_ON, &feats) == KIDO_OK && feats == 0u);
    PSCI_CHECK(kido_psci_features(KIDO_PSCI_SYSTEM_RESET, &feats) == KIDO_OK && feats == 0u);
    PSCI_CHECK(kido_psci_features(0x8400FFFFu, &feats) == KIDO_ERR_STUB_REACHED);

    /* §4: affinity states from the default table. */
    PSCI_CHECK(kido_psci_affinity_info(0u, 0u, &state) == KIDO_OK && state == KIDO_PSCI_AFFINITY_ON);
    PSCI_CHECK(kido_psci_affinity_info(1u, 0u, &state) == KIDO_OK && state == KIDO_PSCI_AFFINITY_OFF);
    PSCI_CHECK(kido_psci_affinity_info(2u, 0u, &state) == KIDO_OK && state == KIDO_PSCI_AFFINITY_OFF);
    PSCI_CHECK(kido_psci_affinity_info(9u, 0u, &state) == KIDO_ERR_EARLY_ENV);

    /* §5: CPU_ON — OFF -> ON, then ALREADY_ON is loud. */
    PSCI_CHECK(kido_psci_cpu_on(1u, 0x40080000u, 42u) == KIDO_OK);
    PSCI_CHECK(kido_psci_affinity_info(1u, 0u, &state) == KIDO_OK && state == KIDO_PSCI_AFFINITY_ON);
    PSCI_CHECK(kido_psci_cpu_on(1u, 0x40080000u, 42u) == KIDO_ERR_EARLY_ENV);
    /* ON_PENDING is loud the same way. */
    kido_psci_test_set_cpu_state(2u, KIDO_PSCI_AFFINITY_ON_PENDING);
    PSCI_CHECK(kido_psci_affinity_info(2u, 0u, &state) == KIDO_OK && state == KIDO_PSCI_AFFINITY_ON_PENDING);
    PSCI_CHECK(kido_psci_cpu_on(2u, 0x40080000u, 0u) == KIDO_ERR_EARLY_ENV);
    /* Unknown affinity and zero entry are firmware errors. */
    PSCI_CHECK(kido_psci_cpu_on(9u, 0x40080000u, 0u) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_cpu_on(3u, 0u, 0u) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_affinity_info(3u, 0u, &state) == KIDO_OK && state == KIDO_PSCI_AFFINITY_OFF);

    /* §6: CPU_SUSPEND records power_state, entry, context. */
    PSCI_CHECK(kido_psci_cpu_suspend(0x00010001u, 0x40081000u, 7u) == KIDO_OK);
    PSCI_CHECK(kido_psci_cpu_suspend(0x00000000u, 0x40081000u, 8u) == KIDO_OK);
    kido_psci_test_get_records(NULL, NULL, &susp);
    PSCI_CHECK(susp == 2u);
    PSCI_CHECK(kido_dbl_last_suspend_state == 0x00000000u);
    PSCI_CHECK(kido_dbl_last_suspend_entry == 0x40081000u);
    PSCI_CHECK(kido_dbl_last_suspend_ctx == 8u);

    /* §7: SYSTEM_OFF / SYSTEM_RESET record on hosted
     * (bare metal they never return — stated divergence). */
    PSCI_CHECK(kido_psci_system_off() == KIDO_OK);
    PSCI_CHECK(kido_psci_system_reset() == KIDO_OK);
    kido_psci_test_get_records(&off, &reset, NULL);
    PSCI_CHECK(off == 1u);
    PSCI_CHECK(reset == 1u);

    /* §8: CPU_OFF returns OK on hosted (same divergence:
     * the host CPU cannot power down). */
    PSCI_CHECK(kido_psci_cpu_off() == KIDO_OK);

    /* §9: NULL guards after init. */
    PSCI_CHECK(kido_psci_version(NULL) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_features(KIDO_PSCI_CPU_ON, NULL) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_affinity_info(0u, 0u, NULL) == KIDO_ERR_EARLY_ENV);

    /* §10: version gating — 0.2 init OK, FEATURES refused;
     * pre-0.2 init refused, driver left unready. */
    kido_psci_test_set_version(0x00000002u);
    PSCI_CHECK(kido_psci_init() == KIDO_OK);
    PSCI_CHECK(kido_psci_version(&ver) == KIDO_OK && ver == 0x00000002u);
    PSCI_CHECK(kido_psci_features(KIDO_PSCI_CPU_ON, &feats) == KIDO_ERR_STUB_REACHED);
    /* CPU_ON still works at 0.2 (it is a 0.2 function). */
    kido_psci_test_set_cpu_state(3u, KIDO_PSCI_AFFINITY_OFF);
    PSCI_CHECK(kido_psci_cpu_on(3u, 0x40080000u, 0u) == KIDO_OK);
    kido_psci_test_set_version(0x00000001u);
    PSCI_CHECK(kido_psci_init() == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_version(&ver) == KIDO_ERR_EARLY_ENV);
    PSCI_CHECK(kido_psci_cpu_on(3u, 0x40080000u, 0u) == KIDO_ERR_EARLY_ENV);

    /* §11: re-init at 1.0 restores the conduit. */
    kido_psci_test_set_version(0x00010000u);
    PSCI_CHECK(kido_psci_init() == KIDO_OK);
    PSCI_CHECK(kido_psci_features(KIDO_PSCI_CPU_ON, &feats) == KIDO_OK);

    printf("[kido-psci] %u checks, %u failures\n",
           kido_psci_checks, kido_psci_failures);
    return (int)kido_psci_failures;
}

int main(void)
{
    return kido_psci_selftest_main() != 0;
}

#endif /* KIDO_PSCI_SELFTEST */
