/*
 * kido-mpidr.c — Kido CPU-identity leaf: MPIDR_EL1 decode.
 *
 * See kido-mpidr.h for the field law, the no-linear-index
 * law, the same-CPU law, the reserved-bit law, and the
 * stated divergences. This file is the real logic in two
 * backends: ARM64 one-MRS read (WRITTEN, NOT ASSEMBLED,
 * NEVER EXECUTED — toolchain absent, 21st data point),
 * and a hosted path over the injector's value (the sync
 * precedent). The decode is shared across both backends:
 * read() calls decode() on the value it obtained, so the
 * two paths can never drift.
 *
 * TESTING: gcc -std=c11 -Wall -Wextra -Werror -DKIDO_HOSTED_TEST
 *        -DKIDO_MPIDR_SELFTEST kido-mpidr.c -o mpidr-selftest
 */
#include "kido-mpidr.h"

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

/* ------------------------------------------------------------------ */
/* Raw read. ARM64: one MRS, the sync spelling verbatim.              */
/* Hosted: the injector's value.                                      */
/* ------------------------------------------------------------------ */
#ifdef KIDO_HOSTED_TEST
static uint64_t kido_mpidr_host_raw = 0u;

kido_status_t kido_mpidr_test_set_raw(uint64_t raw)
{
    kido_mpidr_host_raw = raw;
    return KIDO_OK;
}

static uint64_t kido_mpidr_raw_read(void)
{
    return kido_mpidr_host_raw;
}
#else /* bare metal */
static uint64_t kido_mpidr_raw_read(void)
{
    uint64_t id = 0u;
    __asm__ volatile("mrs %[id], mpidr_el1" : [id] "=r" (id));
    return id;
}
#endif /* KIDO_HOSTED_TEST */

/* ------------------------------------------------------------------ */
/* Decode + identity. Pure bit slices; no firmware value makes       */
/* slicing undefined, so the only failure is a NULL out-pointer.     */
/* ------------------------------------------------------------------ */
kido_status_t kido_mpidr_decode(uint64_t raw, kido_mpidr_t *out)
{
    if (out == (kido_mpidr_t *)0)
        return KIDO_ERR_EARLY_ENV;
    out->raw  = raw;
    out->aff0 = (uint8_t)(raw & 0xffu);
    out->aff1 = (uint8_t)((raw >> 8) & 0xffu);
    out->aff2 = (uint8_t)((raw >> 16) & 0xffu);
    out->aff3 = (uint8_t)((raw >> 32) & 0xffu);
    out->mt   = (uint32_t)((raw >> 24) & 0x1u);
    out->u    = (uint32_t)((raw >> 30) & 0x1u);
    return KIDO_OK;
}

kido_status_t kido_mpidr_read(kido_mpidr_t *out)
{
    if (out == (kido_mpidr_t *)0)
        return KIDO_ERR_EARLY_ENV;
    return kido_mpidr_decode(kido_mpidr_raw_read(), out);
}

kido_status_t kido_mpidr_same_cpu(const kido_mpidr_t *a,
                                   const kido_mpidr_t *b,
                                   uint32_t *same)
{
    if (a == (const kido_mpidr_t *)0 ||
        b == (const kido_mpidr_t *)0 || same == (uint32_t *)0)
        return KIDO_ERR_EARLY_ENV;
    *same = (a->aff0 == b->aff0 && a->aff1 == b->aff1 &&
             a->aff2 == b->aff2 && a->aff3 == b->aff3) ? 1u : 0u;
    return KIDO_OK;
}

#ifdef KIDO_MPIDR_SELFTEST
static int kido_mpidr_checks = 0;
static int kido_mpidr_failures = 0;

#define MPIDR_CHECK(cond) do { \
    kido_mpidr_checks++; \
    if (!(cond)) { \
        kido_mpidr_failures++; \
        printf("[kido-mpidr] FAIL line %d: %s\n", __LINE__, #cond); \
    } \
} while (0)

int kido_mpidr_selftest_main(void)
{
    kido_mpidr_t m, n;
    uint32_t same = 99u;

    /* §1: NULL guards refuse, out-pointers untouched. */
    MPIDR_CHECK(kido_mpidr_decode(0u, (kido_mpidr_t *)0) ==
                KIDO_ERR_EARLY_ENV);
    MPIDR_CHECK(kido_mpidr_read((kido_mpidr_t *)0) ==
                KIDO_ERR_EARLY_ENV);
    MPIDR_CHECK(kido_mpidr_same_cpu((const kido_mpidr_t *)0, &m,
                                    &same) == KIDO_ERR_EARLY_ENV &&
                same == 99u);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, (const kido_mpidr_t *)0,
                                    &same) == KIDO_ERR_EARLY_ENV &&
                same == 99u);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, &m, (uint32_t *)0) ==
                KIDO_ERR_EARLY_ENV);

    /* §2: the zero tuple — a uniprocessor-shaped value. */
    MPIDR_CHECK(kido_mpidr_decode(0u, &m) == KIDO_OK);
    MPIDR_CHECK(m.raw == 0u && m.aff0 == 0u && m.aff1 == 0u &&
                m.aff2 == 0u && m.aff3 == 0u && m.mt == 0u &&
                m.u == 0u);

    /* §3: every field at once, distinct values per level. */
    MPIDR_CHECK(kido_mpidr_decode(
                    (2ull << 32) | (1ull << 16) | (3ull << 8) | 5ull,
                    &m) == KIDO_OK);
    MPIDR_CHECK(m.aff3 == 2u && m.aff2 == 1u && m.aff1 == 3u &&
                m.aff0 == 5u && m.mt == 0u && m.u == 0u);

    /* §4: MT and U flags, each alone and together. */
    MPIDR_CHECK(kido_mpidr_decode(1ull << 24, &m) == KIDO_OK &&
                m.mt == 1u && m.u == 0u);
    MPIDR_CHECK(kido_mpidr_decode(1ull << 30, &m) == KIDO_OK &&
                m.mt == 0u && m.u == 1u);
    MPIDR_CHECK(kido_mpidr_decode((1ull << 24) | (1ull << 30),
                                  &m) == KIDO_OK &&
                m.mt == 1u && m.u == 1u);

    /* §5: reserved bits are masked, never interpreted, never
     * policed — RES1 set (as real hardware always returns),
     * RES0 garbage in [63:40] and [29:25], all decode fine
     * and change no field. */
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull, &m) == KIDO_OK &&
                m.aff0 == 0u && m.mt == 0u && m.u == 0u &&
                m.raw == 0x80000000ull);
    MPIDR_CHECK(kido_mpidr_decode(0xabcdef0000000000ull |
                                  (1ull << 25) | (1ull << 29),
                                  &m) == KIDO_OK &&
                m.aff3 == 0u && m.mt == 0u && m.u == 0u);
    /* Bit 31 clear (a hosted injector value, never real
     * hardware) still decodes — the value is input. */
    MPIDR_CHECK(kido_mpidr_decode(5ull, &m) == KIDO_OK &&
                m.aff0 == 5u && m.raw == 5ull);

    /* §6: a realistic multiprocessor tuple — RES1 set, MT
     * clear, cluster-shaped Aff2/Aff1, two PEs apart only
     * in Aff0 (the QEMU virt -smp shape). */
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull | 0x00000000ull,
                                  &m) == KIDO_OK &&
                m.aff0 == 0u);
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull | 1ull, &n) ==
                KIDO_OK && n.aff0 == 1u);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, &n, &same) == KIDO_OK &&
                same == 0u);

    /* §7: same_cpu — true on identical tuples even when MT
     * or U differ (they are not identity), false on any
     * single Aff level differing. */
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull, &m) == KIDO_OK);
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull | (1ull << 24),
                                  &n) == KIDO_OK);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, &n, &same) == KIDO_OK &&
                same == 1u);
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull | (1ull << 30),
                                  &n) == KIDO_OK);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, &n, &same) == KIDO_OK &&
                same == 1u);
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull | (1ull << 8),
                                  &n) == KIDO_OK);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, &n, &same) == KIDO_OK &&
                same == 0u);
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull | (1ull << 16),
                                  &n) == KIDO_OK);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, &n, &same) == KIDO_OK &&
                same == 0u);
    MPIDR_CHECK(kido_mpidr_decode(0x80000000ull | (1ull << 32),
                                  &n) == KIDO_OK);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, &n, &same) == KIDO_OK &&
                same == 0u);

    /* §8: read() goes through decode() — the injector value
     * comes back field-exact, and re-decoding a decoded
     * raw is idempotent (read/decode can never drift). */
    MPIDR_CHECK(kido_mpidr_test_set_raw(
                    0x80000000ull | (1ull << 24) | (2ull << 32) |
                    (1ull << 16) | (3ull << 8) | 5ull) == KIDO_OK);
    MPIDR_CHECK(kido_mpidr_read(&m) == KIDO_OK);
    MPIDR_CHECK(m.aff3 == 2u && m.aff2 == 1u && m.aff1 == 3u &&
                m.aff0 == 5u && m.mt == 1u && m.u == 0u &&
                m.raw == (0x80000000ull | (1ull << 24) |
                          (2ull << 32) | (1ull << 16) |
                          (3ull << 8) | 5ull));
    MPIDR_CHECK(kido_mpidr_decode(m.raw, &n) == KIDO_OK);
    MPIDR_CHECK(n.aff3 == m.aff3 && n.aff2 == m.aff2 &&
                n.aff1 == m.aff1 && n.aff0 == m.aff0 &&
                n.mt == m.mt && n.u == m.u && n.raw == m.raw);

    /* §9: injector reset restores the default, and a
     * self-comparison is trivially same. */
    MPIDR_CHECK(kido_mpidr_test_set_raw(0u) == KIDO_OK);
    MPIDR_CHECK(kido_mpidr_read(&m) == KIDO_OK &&
                m.raw == 0u && m.aff0 == 0u);
    MPIDR_CHECK(kido_mpidr_same_cpu(&m, &m, &same) == KIDO_OK &&
                same == 1u);

    printf("[kido-mpidr] %d checks, %d failures\n",
           kido_mpidr_checks, kido_mpidr_failures);
    return kido_mpidr_failures;
}

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