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

#define SDRAM_ADDR	0x40000000
#define SDRAM_ADDR_BITS	25
#define SDRAM_SIZE	(1U << SDRAM_ADDR_BITS)
#define SDRAM_MASK	(SDRAM_SIZE - 1)
#define SDRAM_END	(SDRAM_ADDR + SDRAM_SIZE)

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("\r\n%p : read %08x expected %08x\r\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...\r\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("\r\nReading back to check for aliases...\r\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("\rSDRAM test complete, time = %u ms\r\n",
	       (rdtime() - start_time)/(CPU_HZ/1000));

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

#define SDRAM_DONE IODEVRL(2,0)

void main(void)
{
    static const char hello[] = /* "\f\033[2J\033[H" */
	"\r\n\n*** Hello, World! ***\r\n"
	"Firmware compiled on: " __DATE__ " " __TIME__ "\r\n\n";

    /* The data section is not reinitialized on reset */
    static unsigned int loops = 1;

    uint8_t led = 0;
    uint32_t done;

    con_set_baudrate(115200);
    set_led(led = 0);

    while (!SDRAM_DONE)
	/* wait */;

    done = rdtime();

    con_puts(hello);

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

    volatile uint32_t *p = (uint32_t *)SDRAM_ADDR;
    const unsigned int words = 128*1024;
    unsigned int ok = words;
    uint32_t val = 0x00001111;
    for (unsigned int w = 0; w < words; w++) {
	if (*p++ != val)
	    ok--;
	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');
    }

    test_sdram();

    p = (uint32_t *)SDRAM_ADDR;
    for (unsigned int w = 0; w < words; w++)
	*p++ = 0xdeadbeef;

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