/*
 * kido-topo.c — Kido CPU-topology registry.
 *
 * See kido-topo.h for the table law, slot law, identity
 * law, conflict law, absence law, primary law, lock law,
 * and init law. This file is the real logic in one
 * backend: the table is static BSS, the guard is
 * kido-sync's public lock API, comparison is
 * kido_mpidr_same_cpu(). No asm of its own exists in
 * this leaf — its ARM64 content is sync's exclusive
 * pair, already carried — so the hosted proof and the
 * bare-metal build differ only in which sync backend
 * the lock resolves to.
 *
 * TESTING: gcc -std=c11 -Wall -Wextra -Werror -pthread
 *        -DKIDO_HOSTED_TEST -DKIDO_TOPO_SELFTEST
 *        kido-topo.c kido-sync.c kido-mpidr.c -o topo-selftest
 */
#include "kido-topo.h"

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

/* One entry: the decoded identity whole, its smp slot,
 * and whether the slot is occupied. A free entry's aff
 * is never read. */
typedef struct kido_topo_entry {
    kido_mpidr_t aff;
    uint32_t    slot;
    uint32_t    occupied;
} kido_topo_entry_t;

static kido_topo_entry_t kido_topo_table[KIDO_TOPO_MAX_CPUS];
static kido_spinlock_t   kido_topo_lock;
static uint32_t          kido_topo_count_v = 0u;
static uint32_t          kido_topo_initialized = 0u;

/* ------------------------------------------------------------------ */
/* Init. Write-once flag: refused re-init means the flag never        */
/* transitions back, so later calls may read it without the lock.     */
/* ------------------------------------------------------------------ */
kido_status_t kido_topo_init(void)
{
    kido_status_t st;

    if (kido_topo_initialized != 0u)
        return KIDO_ERR_EARLY_ENV;
    st = kido_spin_init(&kido_topo_lock);
    if (st != KIDO_OK)
        return st;
    /* BSS already zeroes the table and the count; the
     * loop states the empty-table invariant instead of
     * relying on the reader to remember BSS semantics. */
    {
        uint32_t i;
        for (i = 0u; i < KIDO_TOPO_MAX_CPUS; i++) {
            kido_topo_table[i].occupied = 0u;
            kido_topo_table[i].slot = 0u;
        }
    }
    kido_topo_count_v = 0u;
    kido_topo_initialized = 1u;
    return KIDO_OK;
}

/* ------------------------------------------------------------------ */
/* Register. Both conflict arms are checked in the same scan, so      */
/* no ordering between "Aff known" and "slot known" can produce a      */
/* half-answer: an Aff conflict and a slot conflict both refuse       */
/* with the table untouched, and the exact pair is idempotent.        */
/* A full table refuses through the slot-conflict arm — every slot    */
/* is occupied, so any new Aff collides with an occupied slot —      */
/* which is why no separate FULL code exists.                         */
/* ------------------------------------------------------------------ */
kido_status_t kido_topo_register(const kido_mpidr_t *aff,
                                  uint32_t slot)
{
    kido_status_t st;
    uint32_t i;
    uint32_t free_idx = KIDO_TOPO_MAX_CPUS;
    uint32_t same = 0u;

    if (kido_topo_initialized == 0u)
        return KIDO_ERR_EARLY_ENV;
    if (aff == (const kido_mpidr_t *)0)
        return KIDO_ERR_EARLY_ENV;
    if (slot >= KIDO_TOPO_MAX_CPUS)
        return KIDO_ERR_EARLY_ENV;
    st = kido_spin_lock(&kido_topo_lock);
    if (st != KIDO_OK)
        return st;
    for (i = 0u; i < KIDO_TOPO_MAX_CPUS; i++) {
        if (kido_topo_table[i].occupied == 0u) {
            if (free_idx == KIDO_TOPO_MAX_CPUS)
                free_idx = i;
            continue;
        }
        st = kido_mpidr_same_cpu(aff, &kido_topo_table[i].aff,
                                  &same);
        if (st == KIDO_OK && same != 0u) {
            if (kido_topo_table[i].slot == slot) {
                /* Exact pair: idempotent, count unmoved. */
                (void)kido_spin_unlock(&kido_topo_lock);
                return KIDO_OK;
            }
            /* One PE, two slots: aliasing. Refuse. */
            (void)kido_spin_unlock(&kido_topo_lock);
            return KIDO_ERR_EARLY_ENV;
        }
        if (kido_topo_table[i].slot == slot) {
            /* One slot, two PEs: theft. Refuse. */
            (void)kido_spin_unlock(&kido_topo_lock);
            return KIDO_ERR_EARLY_ENV;
        }
    }
    if (free_idx == KIDO_TOPO_MAX_CPUS) {
        /* Unreachable by construction (a full table hits
         * the slot-conflict arm above), kept as the
         * fail-stop rather than a silent overwrite. */
        (void)kido_spin_unlock(&kido_topo_lock);
        return KIDO_ERR_EARLY_ENV;
    }
    kido_topo_table[free_idx].aff = *aff;
    kido_topo_table[free_idx].slot = slot;
    kido_topo_table[free_idx].occupied = 1u;
    kido_topo_count_v++;
    (void)kido_spin_unlock(&kido_topo_lock);
    return KIDO_OK;
}

/* ------------------------------------------------------------------ */
/* Lookups. Absence returns OK + found 0 with the out-pointer         */
/* untouched; only malformed calls refuse.                            */
/* ------------------------------------------------------------------ */
kido_status_t kido_topo_slot_for(const kido_mpidr_t *aff,
                                  uint32_t *slot_out,
                                  uint32_t *found)
{
    kido_status_t st;
    uint32_t i;
    uint32_t same = 0u;

    if (kido_topo_initialized == 0u)
        return KIDO_ERR_EARLY_ENV;
    if (aff == (const kido_mpidr_t *)0 ||
        slot_out == (uint32_t *)0 || found == (uint32_t *)0)
        return KIDO_ERR_EARLY_ENV;
    st = kido_spin_lock(&kido_topo_lock);
    if (st != KIDO_OK)
        return st;
    for (i = 0u; i < KIDO_TOPO_MAX_CPUS; i++) {
        if (kido_topo_table[i].occupied == 0u)
            continue;
        st = kido_mpidr_same_cpu(aff, &kido_topo_table[i].aff,
                                  &same);
        if (st == KIDO_OK && same != 0u) {
            *slot_out = kido_topo_table[i].slot;
            *found = 1u;
            (void)kido_spin_unlock(&kido_topo_lock);
            return KIDO_OK;
        }
    }
    *found = 0u;
    (void)kido_spin_unlock(&kido_topo_lock);
    return KIDO_OK;
}

kido_status_t kido_topo_aff_for(uint32_t slot,
                                 kido_mpidr_t *aff_out,
                                 uint32_t *found)
{
    kido_status_t st;
    uint32_t i;

    if (kido_topo_initialized == 0u)
        return KIDO_ERR_EARLY_ENV;
    if (aff_out == (kido_mpidr_t *)0 || found == (uint32_t *)0)
        return KIDO_ERR_EARLY_ENV;
    if (slot >= KIDO_TOPO_MAX_CPUS)
        return KIDO_ERR_EARLY_ENV;
    st = kido_spin_lock(&kido_topo_lock);
    if (st != KIDO_OK)
        return st;
    for (i = 0u; i < KIDO_TOPO_MAX_CPUS; i++) {
        if (kido_topo_table[i].occupied != 0u &&
            kido_topo_table[i].slot == slot) {
            *aff_out = kido_topo_table[i].aff;
            *found = 1u;
            (void)kido_spin_unlock(&kido_topo_lock);
            return KIDO_OK;
        }
    }
    *found = 0u;
    (void)kido_spin_unlock(&kido_topo_lock);
    return KIDO_OK;
}

kido_status_t kido_topo_count(uint32_t *count_out)
{
    kido_status_t st;

    if (kido_topo_initialized == 0u)
        return KIDO_ERR_EARLY_ENV;
    if (count_out == (uint32_t *)0)
        return KIDO_ERR_EARLY_ENV;
    st = kido_spin_lock(&kido_topo_lock);
    if (st != KIDO_OK)
        return st;
    *count_out = kido_topo_count_v;
    (void)kido_spin_unlock(&kido_topo_lock);
    return KIDO_OK;
}

#ifdef KIDO_TOPO_SELFTEST
static int kido_topo_checks = 0;
static int kido_topo_failures = 0;

#define TOPO_CHECK(cond) do { \
    kido_topo_checks++; \
    if (!(cond)) { \
        kido_topo_failures++; \
        printf("[kido-topo] FAIL line %d: %s\n", __LINE__, #cond); \
    } \
} while (0)

/* Decode helper: the test builds identities from raw
 * values through the real pure decoder, never by
 * hand-filling struct fields. */
static kido_mpidr_t topo_aff(uint64_t raw)
{
    kido_mpidr_t m;
    (void)kido_mpidr_decode(raw, &m);
    return m;
}

typedef struct topo_thread_arg {
    kido_mpidr_t aff;
    uint32_t    slot;
    kido_status_t st;
} topo_thread_arg_t;

static void *topo_thread_fn(void *arg)
{
    topo_thread_arg_t *a = (topo_thread_arg_t *)arg;
    a->st = kido_topo_register(&a->aff, a->slot);
    return (void *)0;
}

int kido_topo_selftest_main(void)
{
    kido_mpidr_t a, b;
    kido_mpidr_t out_aff;
    uint32_t slot = 99u;
    uint32_t found = 99u;
    uint32_t count = 99u;

    /* §1: pre-init refusals, out-pointers untouched. */
    a = topo_aff(0u);
    TOPO_CHECK(kido_topo_register(&a, 0u) == KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_slot_for(&a, &slot, &found) ==
               KIDO_ERR_EARLY_ENV && slot == 99u && found == 99u);
    TOPO_CHECK(kido_topo_aff_for(0u, &out_aff, &found) ==
               KIDO_ERR_EARLY_ENV && found == 99u);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_ERR_EARLY_ENV &&
               count == 99u);

    /* §2: init once; second init refuses even empty. */
    TOPO_CHECK(kido_topo_init() == KIDO_OK);
    TOPO_CHECK(kido_topo_init() == KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 0u);

    /* §3: the UP shape — the zero tuple at slot 0 is
     * registrable, and an unoccupied slot is absence,
     * not error. */
    a = topo_aff(0u);
    TOPO_CHECK(kido_topo_register(&a, 0u) == KIDO_OK);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 1u);
    slot = 99u; found = 99u;
    TOPO_CHECK(kido_topo_slot_for(&a, &slot, &found) == KIDO_OK &&
               found == 1u && slot == 0u);
    out_aff = topo_aff(0xdeadbeefu);
    found = 99u;
    TOPO_CHECK(kido_topo_aff_for(1u, &out_aff, &found) == KIDO_OK &&
               found == 0u && out_aff.raw == 0xdeadbeefu);
    b = topo_aff(7u);
    slot = 99u; found = 99u;
    TOPO_CHECK(kido_topo_slot_for(&b, &slot, &found) == KIDO_OK &&
               found == 0u && slot == 99u);

    /* §4: QEMU-virt-shaped secondaries, MT/U blindness,
     * and the multi-cluster alias guard. */
    a = topo_aff(1u); /* Aff0 1 */
    TOPO_CHECK(kido_topo_register(&a, 1u) == KIDO_OK);
    a = topo_aff(2u); /* Aff0 2 */
    TOPO_CHECK(kido_topo_register(&a, 2u) == KIDO_OK);
    /* MT sibling: same Aff0, MT set, at slot 3. A later
     * lookup with MT clear and U set must still find
     * slot 3 — MT/U are not identity. */
    a = topo_aff((1ull << 24) | 4u); /* MT=1, Aff0 4 */
    TOPO_CHECK(kido_topo_register(&a, 3u) == KIDO_OK);
    b = topo_aff((1ull << 30) | 4u); /* U=1, MT=0, Aff0 4 */
    slot = 99u; found = 99u;
    TOPO_CHECK(kido_topo_slot_for(&b, &slot, &found) == KIDO_OK &&
               found == 1u && slot == 3u);
    /* Multi-cluster: Aff1 1 / Aff0 0 is a DIFFERENT PE
     * from the zero tuple, though Aff0 matches. */
    a = topo_aff(1ull << 8); /* Aff1 1, Aff0 0 */
    TOPO_CHECK(kido_topo_register(&a, 4u) == KIDO_OK);
    b = topo_aff(0u);
    slot = 99u; found = 99u;
    TOPO_CHECK(kido_topo_slot_for(&b, &slot, &found) == KIDO_OK &&
               found == 1u && slot == 0u);
    slot = 99u; found = 99u;
    TOPO_CHECK(kido_topo_slot_for(&a, &slot, &found) == KIDO_OK &&
               found == 1u && slot == 4u);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 5u);

    /* §5: conflicts refuse with the table untouched;
     * exact-pair re-register is idempotent. */
    a = topo_aff(1u); /* already at slot 1 */
    TOPO_CHECK(kido_topo_register(&a, 1u) == KIDO_OK);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 5u);
    TOPO_CHECK(kido_topo_register(&a, 6u) == KIDO_ERR_EARLY_ENV);
    slot = 99u; found = 99u;
    TOPO_CHECK(kido_topo_slot_for(&a, &slot, &found) == KIDO_OK &&
               found == 1u && slot == 1u);
    b = topo_aff(9u); /* new Aff, occupied slot 1 */
    TOPO_CHECK(kido_topo_register(&b, 1u) == KIDO_ERR_EARLY_ENV);
    found = 99u;
    TOPO_CHECK(kido_topo_aff_for(1u, &out_aff, &found) == KIDO_OK &&
               found == 1u && out_aff.raw == 1u);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 5u);
    /* Malformed calls refuse. */
    TOPO_CHECK(kido_topo_register((const kido_mpidr_t *)0, 5u) ==
               KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_register(&b, 8u) == KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_slot_for((const kido_mpidr_t *)0, &slot,
                                   &found) == KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_slot_for(&b, (uint32_t *)0, &found) ==
               KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_slot_for(&b, &slot, (uint32_t *)0) ==
               KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_aff_for(8u, &out_aff, &found) ==
               KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_aff_for(5u, (kido_mpidr_t *)0, &found) ==
               KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_count((uint32_t *)0) ==
               KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 5u);

    /* §6: two threads registering distinct pairs at once
     * — the spinlock's hosted proof. Both succeed, the
     * count moves by exactly two, both lookups are exact.
     * The sync injector stays constant across the threads
     * (lock/unlock pair inside one call), so no stranger
     * refusal can fire spuriously here. */
    {
        topo_thread_arg_t t1, t2;
        pthread_t th1, th2;
        t1.aff = topo_aff(5u); t1.slot = 5u; t1.st = KIDO_OK;
        t2.aff = topo_aff(6u); t2.slot = 6u; t2.st = KIDO_OK;
        TOPO_CHECK(pthread_create(&th1, NULL, topo_thread_fn,
                                  &t1) == 0);
        TOPO_CHECK(pthread_create(&th2, NULL, topo_thread_fn,
                                  &t2) == 0);
        TOPO_CHECK(pthread_join(th1, NULL) == 0);
        TOPO_CHECK(pthread_join(th2, NULL) == 0);
        TOPO_CHECK(t1.st == KIDO_OK && t2.st == KIDO_OK);
        TOPO_CHECK(kido_topo_count(&count) == KIDO_OK &&
                   count == 7u);
        a = topo_aff(5u);
        slot = 99u; found = 99u;
        TOPO_CHECK(kido_topo_slot_for(&a, &slot, &found) ==
                   KIDO_OK && found == 1u && slot == 5u);
        a = topo_aff(6u);
        slot = 99u; found = 99u;
        TOPO_CHECK(kido_topo_slot_for(&a, &slot, &found) ==
                   KIDO_OK && found == 1u && slot == 6u);
    }

    /* §7: fill the table; a full table refuses a new PE
     * through the slot-conflict arm, while the exact
     * pair at the last slot stays idempotent. */
    a = topo_aff(7u);
    TOPO_CHECK(kido_topo_register(&a, 7u) == KIDO_OK);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 8u);
    b = topo_aff(0x1234u); /* new Aff, every slot occupied */
    TOPO_CHECK(kido_topo_register(&b, 0u) == KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_register(&b, 7u) == KIDO_ERR_EARLY_ENV);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 8u);
    TOPO_CHECK(kido_topo_register(&a, 7u) == KIDO_OK);
    TOPO_CHECK(kido_topo_count(&count) == KIDO_OK && count == 8u);
    found = 99u;
    TOPO_CHECK(kido_topo_aff_for(7u, &out_aff, &found) == KIDO_OK &&
               found == 1u && out_aff.raw == 7u);

    printf("[kido-topo] %d checks, %d failures\n",
           kido_topo_checks, kido_topo_failures);
    return kido_topo_failures;
}

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