#include "bsp/plic.h"
#include "bsp/soc.h"

#include <stdint.h>
#include <stdio.h>
#include <string.h>

#define PLIC_WORDS ((0x200008u / sizeof(uint32_t)) + 1u)
#define PLIC_ENABLE_OFFSET 0x2000u

static uint32_t plic_words[PLIC_WORDS];
static int failures;

#define CHECK(condition, message) do { \
	if(!(condition)) { \
		fprintf(stderr, "FAIL: %s\n", message); \
		failures++; \
	} \
} while(0)

uint32_t test_bsp_read32(uintptr_t address)
{
	if((address < BSP_PLIC_BASE) ||
	   (address >= BSP_PLIC_BASE + sizeof(plic_words)))
	{
		failures++;
		return 0u;
	}
	return plic_words[(address - BSP_PLIC_BASE) / sizeof(uint32_t)];
}

void test_bsp_write32(uintptr_t address, uint32_t value)
{
	if((address < BSP_PLIC_BASE) ||
	   (address >= BSP_PLIC_BASE + sizeof(plic_words)))
	{
		failures++;
		return;
	}
	plic_words[(address - BSP_PLIC_BASE) / sizeof(uint32_t)] = value;
}

static uint32_t enabled_sources(void)
{
	return plic_words[PLIC_ENABLE_OFFSET / sizeof(uint32_t)];
}

static void initialize_gps_owned_sources(void)
{
	bsp_plic_disable_all();
	bsp_plic_set_threshold(0u);
	for(uint32_t source = 1u ; source <= 10u ; source++)
	{
		bsp_plic_set_priority(source, 1u);
		bsp_plic_enable(source);
	}
}

int main(void)
{
	memset(plic_words, 0, sizeof(plic_words));

	/* Model the previous fused image: GPS sources 1..10 plus PMOD/IMU 11. */
	for(uint32_t source = 1u ; source <= 11u ; source++)
	{
		bsp_plic_enable(source);
	}
	CHECK(enabled_sources() == 0x0ffeu,
	      "fused-image source mask was not established");

	/* CPU-only warm reset must remove every source not owned by GPS. */
	initialize_gps_owned_sources();
	CHECK(enabled_sources() == 0x07feu,
	      "warm GPS initialization retained the previous IMU source");
	for(uint32_t source = 1u ; source <= 10u ; source++)
	{
		CHECK(plic_words[source] == 1u,
		      "GPS source priority was not re-established");
	}

	/* A later IMU start may explicitly reclaim source 11. */
	bsp_plic_set_priority(11u, 1u);
	bsp_plic_enable(11u);
	CHECK(enabled_sources() == 0x0ffeu,
	      "explicit IMU start did not add source 11");

	/* A second warm GPS-only initialization must be idempotent. */
	initialize_gps_owned_sources();
	CHECK(enabled_sources() == 0x07feu,
	      "repeated warm initialization was not idempotent");

	if(failures == 0)
	{
		puts("plic warm-reset tests passed");
	}
	return failures == 0 ? 0 : 1;
}