#include "fw.h"
#include "sdcard.h"
#include "io.h"
#include "sys.h"
#include "console.h"

static unsigned int errors = 0;
static unsigned int rate_limit;
static char err_char;

static void data_check(volatile uint32_t *p, uint32_t expect)
{
    uint32_t readback = *p;

    if (readback != expect) {
	if (rate_limit) {
	    con_printf("\n%p : read %08x expected %08x\n",
		       p, readback, expect);
	    rate_limit--;
	}
	err_char = 'X';
	errors++;
    }
}

static inline void write_check(volatile uint32_t *p, uint32_t v)
{
    *p = v;
    data_check(p, v);
}

#define ERROR_RATELIMIT 8

#define A 0x45c11ba1		/* Arbitrary odd constant */
#define B 0x78dacecb		/* Arbitrary constant */

static void test_sdram(void)
{
#if 0
    const unsigned int sdram_words = SDRAM_SIZE >> 2;
    static unsigned int stride = 4;
    uint32_t start_time = rdtime();
    uint32_t w = 0;
    uint32_t n;

    con_printf("Testing SDRAM from 0x%08x to 0x%08x, stride 0x%08x...\n",
	       SDRAM_ADDR, SDRAM_END, stride);

    err_char = '-';
    rate_limit = ERROR_RATELIMIT;
    for (n = 1, w = 0; n <= sdram_words; n++) {
	uint32_t a = w + SDRAM_ADDR;
	volatile uint32_t *p = (volatile uint32_t *)a;

	write_check(p, 0);
	write_check(p, ~0);
	write_check(p, ~w);
	write_check(p, w);
	write_check(p, A*w + B);

	if (!(n & 0x3ffff)) {
	    CON_DATA = err_char;
	    err_char = '-';
	}

	w = (w + stride) & SDRAM_MASK;
    }

    con_puts("\nReading back to check for aliases...\n");

    rate_limit = ERROR_RATELIMIT;
    for (n = 1, w = 0; n <= sdram_words; n++) {
	uint32_t a = w + SDRAM_ADDR;
	volatile uint32_t *p = (volatile uint32_t *)a;

	data_check(p, A*w + B);

	if (!(n & 0x3ffff)) {
	    CON_DATA = err_char;
	    err_char = '-';
	}

	w = (w - stride) & SDRAM_MASK;
    }

    con_printf("\nSDRAM test complete, time = %u ms\n",
	       (rdtime() - start_time)/(CPU_HZ/1000));

    stride *= 3;
    stride = (stride & SDRAM_MASK) ^ (stride >> (SDRAM_BITS-2));
    stride = (stride & ~3) | 4;
#endif
}

extern uint32_t __dram_bss_start[], __dram_bss_end[], __dram_bss_len[];

#define TESTDATA_WORDS (128*1024)
extern uint32_t testdata[TESTDATA_WORDS];

static void test_download(void)
{
    const unsigned int words = TESTDATA_WORDS;
    volatile uint32_t *p = testdata;
    unsigned int ok = words;
    unsigned int bsslen;
    uint32_t val = 0x00001111;
    unsigned int ratelimit = ERROR_RATELIMIT;

    for (unsigned int w = 0; w < words; w++) {
	uint32_t ram = *p;
	if (ram != val) {
	    ok--;
	    if (ratelimit) {
		ratelimit--;
		con_printf("%p : 0x%08x expected 0x%08x\n", p, ram, val);
	    }
	}
	p++;
	val = (val * 0x89abcdef) +
	    (uint32_t)((val * 0x89abcdefULL) >> 32) +
	    (w * 0x76543210);
    }
    con_printf("SDRAM download: %u/%u words OK\n", ok, words);

    if (ok != words) {
	for (unsigned int o = 0; o < (512*1024); o += (64*1024)) {
	    volatile uint16_t *hp = (uint16_t *)(SDRAM_ADDR + o);
	    p = (uint32_t *)(SDRAM_ADDR + o);
	    for (unsigned int w = 0; w < 8; w++) {
		uint16_t l = *hp++;
		uint16_t h = *hp++;
		con_printf(" %04x.%04x", l, h);
	    }
	    con_putc('\n');
	}
    }

    bsslen = (size_t)__dram_bss_len >> 2;
    ok = bsslen;
    for (const uint32_t *p = __dram_bss_start; p < __dram_bss_end; p++) {
	if (*p)
	    ok--;
    }

    con_printf("SDRAM clear: %u/%u words OK\n", ok, bsslen);
}

/* Make sure we don't leave anything in SDRAM that could be a false negative */
static void scrub_sdram(void)
{
    volatile uint32_t *p;

    for (p = (uint32_t *)SDRAM_ADDR; p < (uint32_t *)SDRAM_END; p++)
	*p = 0xdeadbeef;
}

void init_abc_memmap(void)
{
    volatile uint32_t *pg = &ABCMEMMAP_PAGE(0);

    /* Memory */
    for (unsigned int addr = 0; addr < 0x10000; addr += 512) {
	if (addr >= 0x5800 && addr < 0x6000) {
	    *pg++ = ABCMEMMAP_RD | addr;
	} else if (addr >= 0x8000 && addr < 0xc000) {
	    *pg++ = ABCMEMMAP_RD | ABCMEMMAP_WR | addr;
	} else {
	    *pg++ = addr;	/* Disabled */
	}
    }

    /* I/O */
    for (unsigned int sel = 0; sel < 64; sel++) {
	ABCMEMMAP_WRPORT(sel)  = 0;	/* Write DMA address/OUT enable */
	ABCMEMMAP_WRCOUNT(sel) = 0;	/* DMA write byte count */
	ABCMEMMAP_RDPORT(sel)  = 0;	/* Read DMA address/IN enable */
	ABCMEMMAP_RDCOUNT(sel) = 0;	/* DMA read byte count */
    }
}

static uint32_t get_null_ptr(void)
{
    uint32_t rv;

    /* Prevent gcc from removing a null pointer reference */
    asm volatile("lw %0,0(zero)" : "=r" (rv));
    return rv;
}

void main(void)
{
    /* The data section is not reinitialized on reset */
    static unsigned int loops = 1;

    uint32_t null_ptr;
    uint32_t irq_count;
    uint32_t abc_status;
    uint32_t done;
    uint32_t time_to_main;

    time_to_main = rdtime() - time_zero;
    null_ptr = get_null_ptr();

    CON_DATA = '1';
    unmask_irq(BUSERR_IRQ);

    CON_DATA = '2';
    unmask_irq(EBREAK_IRQ);

    CON_DATA = '3';
    unmask_irq(ROMCOPY_IRQ);

    CON_DATA = '4';
    init();

    con_printf("This is loop: %u\n", loops++);

    abc_status = ABC_STATUS;
    if (abc_status & ABC_STATUS_LIVE) {
	con_printf("I seem to be connected to ABC%d\n",
		   (ABC_STATUS & ABC_STATUS_800) ? 800 : 80);
    } else {
	con_puts("No ABC-bus host detected\n");
    }

    con_printf("Bootup code/clearing .bss took %u us\n",
	       time_to_main/(CPU_HZ/1000000));

    while (!(irqmask() & (1 << ROMCOPY_IRQ)))
	pause();

    con_printf("SDRAM download took %u us\n",
	       romcopy_time[0]/(CPU_HZ/1000000));
    con_printf("SDRAM bss clear took %u us\n",
	       romcopy_time[1]/(CPU_HZ/1000000));

    test_download();

    sdcard_reset();
    sdcard_init();
    test_sdram();
    scrub_sdram();

    irq_count = timer_count();
    done = rdtime() - time_zero; /* timer_irq_start */

    con_printf("%u timer interrupts received in %u ms, ~%u expected\n",
	       irq_count, done/(CPU_HZ/1000),
	       (done+(CPU_HZ/64))/(CPU_HZ/32));

    udelay(1000000);
    uint32_t null_ptr_now = get_null_ptr();
    if (null_ptr != null_ptr_now) {
	con_printf("*** NULL POINTER AREA CORRUPT: 0x%08x not 0x%08x\n",
		   null_ptr_now, null_ptr);
	con_flush();
	_die();
    }
    con_puts("*** Doing reset ***\n");
    con_flush();

    reset(SYS_RESET_SOFT);
}