#include "fw.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)
{
    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)) {
	    CONSOLE = 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)) {
	    CONSOLE = 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_ADDR_BITS-2));
    stride = (stride & ~3) | 4;
}

static void test_download(void)
{
    volatile uint32_t *p = (uint32_t *)SDRAM_ADDR;
    const unsigned int words = 128*1024;
    unsigned int ok = words;
    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("%u/%u words OK\n\n", 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');
    }
}

/* 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;
}

static volatile uint32_t timer_irq_count;
static void periodic_irq(unsigned int vector)
{
    uint32_t count = timer_irq_count;
    (void)vector;

    count++;
    timer_irq_count = count;
    set_led(count >> 3); /* 4 Hz */
}

static void init(void)
{
    static const char hello[] =
	"\n*** Hello, World! ***\n"
	"Firmware compiled on: " __DATE__ " " __TIME__ "\n\n";

    con_set_baudrate(115000);
    set_led(0);

    con_puts(hello);

    register_irq(3, periodic_irq, true);
}

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

    uint32_t done;
    uint32_t irq_count;

    while (!ROMCOPY_DONE)
	pause();

    done = rdtime() - time_zero;

    init();

    con_printf("This is loop: %u\n", loops++);
    con_printf("SDRAM download took %u us\n", done/(CPU_HZ/1000000));

    test_download();

    disk_init();
    test_sdram();
    scrub_sdram();

    irq_count = timer_irq_count;
    done = rdtime() - time_zero;

    con_printf("%u timer interrupts received in %u us\n",
	       irq_count, done/(CPU_HZ/1000000));

    udelay(1000000);
    con_puts("*** Doing reset ***\n");
    con_flush();
    while ( 1 )
      RESET_CMD = 1;
}