#include <stddef.h>
#include <stdint.h>

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

void die(void)
{
    while (1)
	;
}

#define SDRAM_ADDR 0x40000000
#define SDRAM_SIZE (32 << 20)

static void write_check(volatile uint32_t *p, uint32_t v)
{
    uint32_t rb;
    
    *p = v;
    rb = *p;

    if (v != rb)
	con_printf("\r%p : read %08x wrote %08x\r\n", p, rb, v);
}

static void test_sdram(void)
{
    uint32_t * const sdram_start = (uint32_t *)SDRAM_ADDR;
    uint32_t * const sdram_end   = (uint32_t *)(SDRAM_ADDR + SDRAM_SIZE);
    volatile uint32_t *p;
    
    con_printf("Testing SDRAM from %p to %p...\r\n", sdram_start, sdram_end);

    for (p = sdram_start ; p < sdram_end ; p++) {
	uint32_t a = (uint32_t)p;

	if (!(a & 0xffff))
	    con_printf("\r%p ", p);

	write_check(p, 0);
	write_check(p, ~0);
	write_check(p, ~a);
	write_check(p, a);
    }

    con_puts("\rReading back to check for aliases...\r\n");
	
    for (p = sdram_start ; p < sdram_end ; p++) {
	uint32_t a = (uint32_t)p;
	uint32_t v = *p;

	if (!(a & 0xffff))
	    con_printf("\r%p ", p);

#if 0
	if (v != a)
	  con_printf("\r%p : read %08x expected %08x\r\n", p, v, a);
#endif
    }

    con_puts("\rSDRAM test complete\r\n");
}

void main(void)
{
    static const char hello[] = "\r\n\nHello, World!\r\n";
    uint8_t led = 0;
    unsigned int loops = 0;

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

    con_puts(hello);

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

    die();
}