/*
 * kido-smp.c — Kido secondary-CPU bring-up policy, on top of PSCI.
 *
 * See kido-smp.h for the two-phase law (STARTED vs ARRIVED),
 * the secondary stub contract (mailbox + context_id), and the
 * stated divergences. This file is the real logic: the hosted
 * build runs the SAME functions against the real psci API
 * (psci.c's scripted firmware double), not a second double.
 * The bare-metal path is WRITTEN, NOT ASSEMBLED, NEVER
 * EXECUTED (toolchain absent, 16th data point); the QEMU
 * proof waits on the future stub as well as the toolchain.
 *
 * TESTING: gcc -std=c11 -Wall -Wextra -Werror -DKIDO_HOSTED_TEST
 *        -DKIDO_SMP_SELFTEST kido-smp.c kido-psci.c -o smp-selftest
 */
#include "kido-smp.h"

#ifdef KIDO_SMP_SELFTEST
#ifndef KIDO_HOSTED_TEST
#error "KIDO_SMP_SELFTEST needs KIDO_HOSTED_TEST."
#endif
#include <stdio.h>
#endif /* KIDO_SMP_SELFTEST */

/* Per-slot record. Static BSS: exists before any allocator. */
typedef struct kido_smp_slot {
    uint64_t         affinity;
    uint64_t         entry;
    uint64_t         stack_base;
    uint64_t         stack_size;
    kido_smp_state_t state;
    uint32_t         released;
} kido_smp_slot_t;

static kido_smp_slot_t kido_smp_slots[KIDO_SMP_MAX_CPUS];
static int             kido_smp_ready = 0;

/* Stack-top mailbox (header contract): written by bring_up
 * before cpu_on, read by the future secondary stub. */
uint64_t kido_smp_secondary_stack_top[KIDO_SMP_MAX_CPUS];

static void kido_smp_slot_clear(uint32_t slot)
{
    kido_smp_slots[slot].affinity = 0u;
    kido_smp_slots[slot].entry = 0u;
    kido_smp_slots[slot].stack_base = 0u;
    kido_smp_slots[slot].stack_size = 0u;
    kido_smp_slots[slot].state = KIDO_SMP_EMPTY;
    kido_smp_slots[slot].released = 0u;
    kido_smp_secondary_stack_top[slot] = 0u;
}

kido_status_t kido_smp_init(void)
{
    uint32_t ver = 0u;
    uint32_t i;

    /* The conduit must be ready; init itself is caller-gated
     * (Gate policy), so smp checks readiness, never calls it. */
    if (kido_psci_version(&ver) != KIDO_OK)
        return KIDO_ERR_EARLY_ENV;
    for (i = 0u; i < KIDO_SMP_MAX_CPUS; i++) {
        if (kido_smp_slots[i].state == KIDO_SMP_STARTED ||
            kido_smp_slots[i].state == KIDO_SMP_ARRIVED)
            return KIDO_ERR_EARLY_ENV; /* never orphan a live CPU */
    }
    for (i = 0u; i < KIDO_SMP_MAX_CPUS; i++)
        kido_smp_slot_clear(i);
    kido_smp_ready = 1;
    return KIDO_OK;
}

kido_status_t kido_smp_bring_up(uint64_t affinity, uint64_t entry,
                                 uint64_t stack_base,
                                 uint64_t stack_size,
                                 uint32_t *slot_out)
{
    uint32_t slot = 0u;
    uint32_t i;
    int      found = 0;
    kido_status_t st;
    uint32_t fw = 0u;
    uint32_t poll;

    if (!kido_smp_ready)
        return KIDO_ERR_EARLY_ENV;
    /* Guards precede reservation: a refused guard leaves the
     * table empty (pinned by test). */
    if (slot_out == NULL)
        return KIDO_ERR_EARLY_ENV;
    if (stack_base == 0u)
        return KIDO_ERR_EARLY_ENV;
    if (stack_size < KIDO_SMP_MIN_STACK_SIZE)
        return KIDO_ERR_EARLY_ENV;
    if ((stack_base & 15u) != 0u || (stack_size & 15u) != 0u)
        return KIDO_ERR_EARLY_ENV;
    if (stack_base + stack_size < stack_base)
        return KIDO_ERR_EARLY_ENV; /* wrap */
    /* Duplicate affinity in a live or failed slot refuses. An
     * OFF slot with this affinity is reused in place below. */
    for (i = 0u; i < KIDO_SMP_MAX_CPUS; i++) {
        if (kido_smp_slots[i].affinity == affinity &&
            (kido_smp_slots[i].state == KIDO_SMP_STARTED ||
             kido_smp_slots[i].state == KIDO_SMP_ARRIVED ||
             kido_smp_slots[i].state == KIDO_SMP_FAILED))
            return KIDO_ERR_EARLY_ENV;
        if (!found && kido_smp_slots[i].state == KIDO_SMP_OFF &&
            kido_smp_slots[i].affinity == affinity) {
            slot = i;
            found = 1;
        }
    }
    if (!found) {
        for (i = 0u; i < KIDO_SMP_MAX_CPUS; i++) {
            if (kido_smp_slots[i].state == KIDO_SMP_EMPTY) {
                slot = i;
                found = 1;
                break;
            }
        }
    }
    if (!found)
        return KIDO_ERR_NO_MEMORY; /* table full (sched precedent) */

    /* Reserve, then write the mailbox, then call: the slot
     * index IS the context_id, so it must exist first. */
    kido_smp_slots[slot].affinity = affinity;
    kido_smp_slots[slot].entry = entry;
    kido_smp_slots[slot].stack_base = stack_base;
    kido_smp_slots[slot].stack_size = stack_size;
    kido_smp_slots[slot].state = KIDO_SMP_STARTED; /* provisional */
    kido_smp_slots[slot].released = 0u;
    kido_smp_secondary_stack_top[slot] = stack_base + stack_size;

    st = kido_psci_cpu_on(affinity, entry, (uint64_t)slot);
    if (st != KIDO_OK) {
        /* Refusal changes no firmware state: free the slot
         * so retry with corrected arguments is allowed. */
        kido_smp_slot_clear(slot);
        return st;
    }

    /* Firmware-truth poll: STARTED means the firmware
     * reports ON, nothing more (deliberation C1/C5). */
    for (poll = 0u; poll < KIDO_SMP_AFFINITY_POLLS; poll++) {
        st = kido_psci_affinity_info(affinity, 0u, &fw);
        if (st != KIDO_OK) {
            kido_smp_slots[slot].state = KIDO_SMP_FAILED;
            return st;
        }
        if (fw == KIDO_PSCI_AFFINITY_ON) {
            kido_smp_slots[slot].state = KIDO_SMP_STARTED;
            *slot_out = slot;
            return KIDO_OK;
        }
        /* OFF / ON_PENDING: keep polling. */
    }
    kido_smp_slots[slot].state = KIDO_SMP_FAILED;
    return KIDO_ERR_EARLY_ENV;
}

kido_status_t kido_smp_secondary_arrived(uint32_t context_id)
{
    if (!kido_smp_ready)
        return KIDO_ERR_EARLY_ENV;
    if (context_id >= KIDO_SMP_MAX_CPUS)
        return KIDO_ERR_EARLY_ENV;
    if (kido_smp_slots[context_id].state != KIDO_SMP_STARTED)
        return KIDO_ERR_EARLY_ENV;
    kido_smp_slots[context_id].state = KIDO_SMP_ARRIVED;
    return KIDO_OK;
}

kido_status_t kido_smp_release(uint32_t slot)
{
    if (!kido_smp_ready)
        return KIDO_ERR_EARLY_ENV;
    if (slot >= KIDO_SMP_MAX_CPUS)
        return KIDO_ERR_EARLY_ENV;
    if (kido_smp_slots[slot].state != KIDO_SMP_ARRIVED)
        return KIDO_ERR_EARLY_ENV;
    kido_smp_slots[slot].released = 1u;
    return KIDO_OK;
}

kido_status_t kido_smp_secondary_wait_release(uint32_t context_id,
                                               uint32_t max_spins)
{
    uint32_t spin;

    if (!kido_smp_ready)
        return KIDO_ERR_EARLY_ENV;
    if (context_id >= KIDO_SMP_MAX_CPUS)
        return KIDO_ERR_EARLY_ENV;
    if (kido_smp_slots[context_id].state != KIDO_SMP_ARRIVED)
        return KIDO_ERR_EARLY_ENV;
    for (spin = 0u; spin < max_spins; spin++) {
        if (kido_smp_slots[context_id].released != 0u)
            return KIDO_OK;
    }
    return KIDO_ERR_EARLY_ENV;
}

kido_status_t kido_smp_secondary_off(uint32_t context_id)
{
    if (!kido_smp_ready)
        return KIDO_ERR_EARLY_ENV;
    if (context_id >= KIDO_SMP_MAX_CPUS)
        return KIDO_ERR_EARLY_ENV;
    if (kido_smp_slots[context_id].state != KIDO_SMP_STARTED &&
        kido_smp_slots[context_id].state != KIDO_SMP_ARRIVED)
        return KIDO_ERR_EARLY_ENV;
    /* Marks nothing: on bare metal success never returns.
     * The status propagates; the primary learns via query. */
    return kido_psci_cpu_off();
}

kido_status_t kido_smp_slot_state(uint32_t slot, kido_smp_state_t *out)
{
    if (!kido_smp_ready)
        return KIDO_ERR_EARLY_ENV;
    if (slot >= KIDO_SMP_MAX_CPUS || out == NULL)
        return KIDO_ERR_EARLY_ENV;
    *out = kido_smp_slots[slot].state;
    return KIDO_OK;
}

kido_status_t kido_smp_query(uint32_t slot, kido_smp_state_t *out)
{
    kido_status_t st;
    uint32_t fw = 0u;

    if (!kido_smp_ready)
        return KIDO_ERR_EARLY_ENV;
    if (slot >= KIDO_SMP_MAX_CPUS || out == NULL)
        return KIDO_ERR_EARLY_ENV;
    if (kido_smp_slots[slot].state == KIDO_SMP_EMPTY ||
        kido_smp_slots[slot].state == KIDO_SMP_FAILED)
        return KIDO_ERR_EARLY_ENV;
    st = kido_psci_affinity_info(kido_smp_slots[slot].affinity,
                                  0u, &fw);
    if (st != KIDO_OK)
        return st;
    if (fw == KIDO_PSCI_AFFINITY_OFF)
        kido_smp_slots[slot].state = KIDO_SMP_OFF;
    /* Firmware ON / ON_PENDING leaves smp state alone:
     * arrival is smp-level, not firmware-level. */
    *out = kido_smp_slots[slot].state;
    return KIDO_OK;
}

kido_status_t kido_smp_stats(kido_smp_stats_t *out)
{
    uint32_t i;

    if (!kido_smp_ready || out == NULL)
        return KIDO_ERR_EARLY_ENV;
    out->brought = 0u;
    out->arrived = 0u;
    out->failed = 0u;
    out->live = 0u;
    for (i = 0u; i < KIDO_SMP_MAX_CPUS; i++) {
        switch (kido_smp_slots[i].state) {
        case KIDO_SMP_STARTED:
            out->brought++;
            out->live++;
            break;
        case KIDO_SMP_ARRIVED:
            out->brought++;
            out->arrived++;
            out->live++;
            break;
        case KIDO_SMP_OFF:
            out->brought++;
            break;
        case KIDO_SMP_FAILED:
            out->failed++;
            break;
        default:
            break;
        }
    }
    return KIDO_OK;
}

#ifdef KIDO_SMP_SELFTEST
static unsigned kido_smp_checks = 0u;
static unsigned kido_smp_failures = 0u;

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

int kido_smp_selftest_main(void)
{
    uint32_t slot = 99u;
    kido_smp_state_t st = KIDO_SMP_EMPTY;
    kido_smp_stats_t stats;

    /* §1: pre-init guards — every call refuses. */
    SMP_CHECK(kido_smp_bring_up(3u, 0x40080000u, 0x41000000u,
                                16384u, &slot) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_secondary_arrived(0u) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_release(0u) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_secondary_wait_release(0u, 16u) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_secondary_off(0u) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_slot_state(0u, &st) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_query(0u, &st) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_stats(&stats) == KIDO_ERR_EARLY_ENV);

    /* §2: init requires the conduit; re-init at rest allowed. */
    SMP_CHECK(kido_smp_init() == KIDO_ERR_EARLY_ENV); /* psci not ready */
    SMP_CHECK(kido_psci_init() == KIDO_OK);
    SMP_CHECK(kido_smp_init() == KIDO_OK);
    SMP_CHECK(kido_smp_init() == KIDO_OK);
    SMP_CHECK(kido_smp_stats(&stats) == KIDO_OK &&
              stats.brought == 0u && stats.arrived == 0u &&
              stats.failed == 0u && stats.live == 0u);

    /* §3: guards refuse BEFORE reservation — table stays empty. */
    SMP_CHECK(kido_smp_bring_up(3u, 0u, 0x41000000u, 16384u,
                                &slot) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_bring_up(3u, 0x40080000u, 0u, 16384u,
                                &slot) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_bring_up(3u, 0x40080000u, 0x41000000u,
                                4096u, &slot) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_bring_up(3u, 0x40080000u, 0x41000001u,
                                16384u, &slot) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_bring_up(3u, 0x40080000u, 0x41000000u,
                                16385u, &slot) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_bring_up(3u, 0x40080000u, 0x41000000u,
                                16384u, NULL) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_stats(&stats) == KIDO_OK &&
              stats.brought == 0u && stats.live == 0u);
    SMP_CHECK(kido_smp_secondary_stack_top[0] == 0u);

    /* §4: retry after refusal, then success (deliberation C4).
     * The entry-0 refusal above freed its slot; affinity 3 now
     * brings up cleanly into slot 0 with the mailbox written. */
    SMP_CHECK(kido_smp_bring_up(3u, 0x40080000u, 0x41000000u,
                                16384u, &slot) == KIDO_OK && slot == 0u);
    SMP_CHECK(kido_smp_secondary_stack_top[0] == 0x41004000u);
    SMP_CHECK(kido_smp_slot_state(0u, &st) == KIDO_OK &&
              st == KIDO_SMP_STARTED);
    SMP_CHECK(kido_smp_stats(&stats) == KIDO_OK &&
              stats.brought == 1u && stats.live == 1u);

    /* §5: duplicate affinity refuses. */
    SMP_CHECK(kido_smp_bring_up(3u, 0x40080000u, 0x41200000u,
                                16384u, &slot) == KIDO_ERR_EARLY_ENV);

    /* §6: release/wait before arrival refuse. */
    SMP_CHECK(kido_smp_release(0u) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_secondary_wait_release(0u, 16u) == KIDO_ERR_EARLY_ENV);

    /* §7: arrival, and only arrival, moves STARTED -> ARRIVED. */
    SMP_CHECK(kido_smp_secondary_arrived(0u) == KIDO_OK);
    SMP_CHECK(kido_smp_slot_state(0u, &st) == KIDO_OK &&
              st == KIDO_SMP_ARRIVED);
    SMP_CHECK(kido_smp_secondary_arrived(0u) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_secondary_arrived(9u) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_stats(&stats) == KIDO_OK &&
              stats.arrived == 1u && stats.live == 1u);

    /* §8: release then wait. */
    SMP_CHECK(kido_smp_release(0u) == KIDO_OK);
    SMP_CHECK(kido_smp_secondary_wait_release(0u, 16u) == KIDO_OK);
    SMP_CHECK(kido_smp_release(9u) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_secondary_wait_release(9u, 16u) == KIDO_ERR_EARLY_ENV);

    /* §9: second and third CPUs through the same path. */
    SMP_CHECK(kido_smp_bring_up(2u, 0x40080000u, 0x41100000u,
                                16384u, &slot) == KIDO_OK && slot == 1u);
    SMP_CHECK(kido_smp_secondary_stack_top[1] == 0x41104000u);
    SMP_CHECK(kido_smp_secondary_arrived(1u) == KIDO_OK);
    SMP_CHECK(kido_smp_release(1u) == KIDO_OK);
    SMP_CHECK(kido_smp_secondary_wait_release(1u, 16u) == KIDO_OK);
    SMP_CHECK(kido_smp_bring_up(1u, 0x40080000u, 0x41200000u,
                                16384u, &slot) == KIDO_OK && slot == 2u);
    SMP_CHECK(kido_smp_stats(&stats) == KIDO_OK &&
              stats.brought == 3u && stats.arrived == 2u &&
              stats.live == 3u);

    /* §10: secondary_off propagates the hosted OK and marks
     * nothing (the never-returns law's hosted divergence). */
    SMP_CHECK(kido_smp_secondary_off(1u) == KIDO_OK);
    SMP_CHECK(kido_smp_slot_state(1u, &st) == KIDO_OK &&
              st == KIDO_SMP_ARRIVED);
    SMP_CHECK(kido_smp_secondary_off(9u) == KIDO_ERR_EARLY_ENV);

    /* §11: query reads firmware truth without disturbing
     * smp-level state; guards refuse. */
    SMP_CHECK(kido_smp_query(1u, &st) == KIDO_OK &&
              st == KIDO_SMP_ARRIVED);
    SMP_CHECK(kido_smp_query(9u, &st) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_query(1u, NULL) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_slot_state(9u, &st) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_slot_state(1u, NULL) == KIDO_ERR_EARLY_ENV);

    /* §12: firmware refusal (unknown affinity) frees its
     * slot — mailbox word 3 stays zero, no FAILED recorded. */
    SMP_CHECK(kido_smp_bring_up(9u, 0x40080000u, 0x41300000u,
                                16384u, &slot) == KIDO_ERR_EARLY_ENV);
    SMP_CHECK(kido_smp_secondary_stack_top[3] == 0u);
    SMP_CHECK(kido_smp_stats(&stats) == KIDO_OK &&
              stats.brought == 3u && stats.failed == 0u);

    /* §13 (LAST): re-init refuses while secondaries are live. */
    SMP_CHECK(kido_smp_init() == KIDO_ERR_EARLY_ENV);

    printf("[kido-smp] %u checks, %u failures\n",
           kido_smp_checks, kido_smp_failures);
    return (int)kido_smp_failures;
}

int main(void)
{
    return kido_smp_selftest_main() != 0;
}
#endif /* KIDO_SMP_SELFTEST */
