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

#define DEBUG     0
#define MINITESTS 1
#define DELAY     1

static void __hot con_print_hex(unsigned int n)
{
    for (int i = 0; i < 8; i++) {
	unsigned int c = n >> 28;
	n <<= 4;
	con_putc(c + ((c >= 10) ? 'a'-10 : '0'));
    }
}

/* Don't mark no_return or gcc moves it to SDRAM */
static void __hot __text_hot killed(const char *how, size_t pc)
{
    /* Cannot use con_printf() here */
    const uint16_t *pcp;
    size_t mtval;

    asm volatile("csrr %0,mtval" : "=r" (mtval));

    /* Try to move back to the previous instruction (if not a jump...) */
    pc += -4 + (pc & 1);
    pcp = (const uint16_t *)pc;

    con_puts(hotstr("ERROR: "));
    con_puts(how);
    con_puts(hotstr(" at 0x"));
    con_print_hex(pc);
    con_puts(hotstr(" (0x"));
    con_print_hex((pcp[1] << 16) + pcp[0]);
    con_puts(hotstr(")\nBad address: 0x"));
    con_print_hex(mtval);
    con_putc('\n');

    for (int i = 0; i < 32; i += 8) {
	for (int j = 0; j < 8; j++) {
	    uint32_t v = rdxreg(i+j);
	    con_print_hex(v);
	    con_putc((j == 7) ? '\n' : ' ');
	}
    }

    con_flush();
    udelay(5000000);
    reset(SYS_RESET_SOFT);
}

IRQHANDLER(buserr,0)
{
    killed(hotstr("misaligned"), pc);
}

IRQHANDLER(ebreak,0)
{
    killed(hotstr("invalid instruction"), pc);
}

volatile __sbss uint32_t timer_irq_count;
IRQHANDLER(sysclock,0)
{
    uint32_t count = timer_irq_count;
    count++;
    timer_irq_count = count;
    if ( MINITESTS ) {
	static const char spinner[4] = "/-\\|";
	if (!(count & (TIMER_HZ-1))) {
	    uint32_t seconds = count >> TIMER_SHIFT;
	    CON_DATA = spinner[seconds & 3];
	    CON_DATA = '\b';
	}
    }
}

static void late_init(void);

uint32_t __sbss timer_irq_start;
void __hot init(void)
{
    static __string_hot const char hello[] =
	"\n\n*** Hello, World! ***\n"
	"MAX80 "
#ifdef TEST
	"testing"
#endif
	"firmware compiled on: ";

    timer_irq_start = rdtime();

    /* Start ROM copy engine, unmask timer and fatal exceptions */
    unmask_irqs((1U << ROMCOPY_IRQ)|(1U << EBREAK_IRQ)|
		(1U << BUSERR_IRQ)|(1U << SYSCLOCK_IRQ));

    con_puts(hello);
    con_puts(__datestamp);
    con_putc('\n');

    set_leds(7);
    wait_romcopy_done();

    if ( MINITESTS ) {
	extern uint32_t __dram_init_start[], __dram_init_end[];
	extern uint32_t __dram_bss_start[], __dram_bss_end[];
	volatile uint32_t *dp;
	uint32_t v;
	uint32_t wrerr;
	uint32_t not_zero, all_ones;
	uint32_t rx = 0x193ac604;

	v = wrerr = 0;
	all_ones  = -1;
	not_zero  = 0;

	for (dp = __dram_init_start; dp < __dram_init_end; dp++) {
	    uint32_t v1, v2;

	    v1 = *dp;
	    v += v1;

	    v2 = v1 ^ rx;
	    *dp = v2;

	    wrerr |= *dp ^ v2;

	    *dp = v1;

	    not_zero |= v1;
	    all_ones &= v1;
	    
	    rx *= 0xf4d5725f;
	}

	con_puts(hotstr("SDRAM data checksum: "));
	con_print_hex(v);
	con_puts(hotstr(" expected "));
	con_print_hex(__dram_checksum);
	con_putc('\n');

	con_puts(hotstr("Bits always set, clear: "));
	con_print_hex(all_ones);
	con_putc(' ');
	con_print_hex(~not_zero);
	con_putc('\n');
	
	if (wrerr)
	    con_puts(hotstr("SDRAM read/write error\n"));

	v = 0;
	for (dp = __dram_bss_start; dp < __dram_bss_end; dp++) {
	    uint32_t x = *dp;
	    v |= x;
	}

	if (v)
	    con_puts(hotstr("SDRAM .bss is not zero!\n"));
    }
    set_leds(6);

    con_flush();

    set_leds(5);
#if DELAY
    con_puts(hotstr("Waiting 5 s for testing..."));
    udelay(5000000);
    con_putc('\n');
    con_flush();
#endif

    late_init();
}

volatile uint32_t __dram_bss test_dram[8];

static void __hot late_init(void)
{
    /* This needs to be done as early as possible!!! */
    con_puts("Running abc_init_memmap: ");
    con_flush();
    abc_init_memmap();
    con_puts("ok\n");

    if (SYS_MAGIC != SYS_MAGIC_MAX80) {
	con_puts("Not a MAX80 board?!?!\n\n");
	_die();
    } else {
	con_printf("MAX80 ver %u.%u rework flags %02x fpga %u\n",
		   SYS_BOARDMAJOR, SYS_BOARDMINOR,
		   SYS_BOARDFIX, SYS_BOARDFPGA);
	if (SYS_BOARDMAJOR != SYS_BOARDFPGA) {
	    con_puts("Invalid FPGA firmware for this board revision\n");
	    _die();
	}
    }
    con_putc('\n');

    if ( MINITESTS ) {
	con_puts("Quick DRAM test:\n");
	for (int i = 0; i < 8; i++) {
	    uint32_t v = (i*0x11111111) + 0x44332211;
	    test_dram[i] = v;
	    (void)test_dram[i];	/* Force immediate readback */
	    con_printf("%08x ", v);
	}
	con_putc('\n');

	for (int i = 0; i < 8; i++) {
	    con_printf("%08x ", test_dram[i]);
	}
	con_puts("\n\nRandom number generator test:\n");
	for (int i = 0; i < 8; i++)
	    con_printf("%08x ", rdrand());
	con_puts("\n\n");
    }

    rom_get_serial();

    set_leds(4);

    read_rtc();
    rtc_abc_init();

    set_leds(3);

    sdcard_reset();
    disk_cache_init();
    abcdisk_init();

    set_leds(2);

    pun80_init();

    set_leds(1);


    abc_init();

    /* Release WAIT# if asserted */
    ABC_BUSCTL = 0;

    set_leds(0);
}