#include <stddef.h>
#include <stdint.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%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 SOME_NUMBER 0x78dacecb

static void test_sdram(void)
{
    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);

    n = 0;
    err_char = '-';
    rate_limit = ERROR_RATELIMIT;
    do {
	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, w + SOME_NUMBER);

	if (!(++n & 0x3ffff)) {
	    CONSOLE = err_char;
	    err_char = '-';
	}
	
	w = (w + stride) & SDRAM_MASK;
    } while (w);

    con_puts("\r\nReading back to check for aliases...\r\n");
	
    rate_limit = ERROR_RATELIMIT;
    do {
	uint32_t a = w + SDRAM_ADDR;
	volatile uint32_t *p = (volatile uint32_t *)a;

	data_check(p, w + SOME_NUMBER);

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

	w = (w - stride) & SDRAM_MASK;
    } while (w);

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

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

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

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

    while ( 1 ) {
	con_puts(hello);
	con_printf("Loops: %u, errors = %u\r\n\n", loops++, errors);
	test_sdram();
	led = (led << 1) | ((~led >> 2) & 1);
	set_led(led);
    }
}