#include <iolpc2146.h>
#include <intrinsics.h>

#define BIT_(x) (1 << (x))

namespace
{
    unsigned const DBG_PIN = 24;
}

enum vec_id_t
{
    VEC_RESET = 0,
    VEC_UNDEF = 1,
    VEC_SWI = 2,
    VEC_PREFETCH = 3,
    VEC_DATA_ABORT = 4,
    VEC_RESERVED__ = 5,
    VEC_IRQ = 6,
    VEC_FIQ = 7,
    
    VEC_TOTAL_NUM = 8
};

#define MEMMAP_OPTION (1)

#if 1 == MEMMAP_OPTION
    #define VIC_ISR __irq __arm
#else
    #pragma location=0x40000000
    __no_init unsigned intvec[VEC_TOTAL_NUM * 2];
    
    #define VIC_ISR
#endif

VIC_ISR void irq_timer(void);
VIC_ISR void non_vectored_handler(void);

extern "C" __irq __arm void reset_handler(void) {}
extern "C" __irq __arm void undef_handler(void) {}
extern "C" __irq __arm void swi_handler(void) {}
extern "C" __irq __arm void prefetch_handler(void) {}
extern "C" __irq __arm void data_handler(void) {}    
extern "C" __irq __arm void fiq_handler(void) {}

#if 2 == MEMMAP_OPTION
    __irq __arm void irq_handler(void)
    {
        typedef void (VIC_ISR * handler_t)(void);
        handler_t h = reinterpret_cast<handler_t>(VICVectAddr);
        (*h)();
    }
#endif

void hw_init(void)
{
    PCON = 0;
    PCONP = BIT_(1);

    VPBDIV = 1;
    
    MAMCR = 0;
    MAMTIM = 2;
    MAMCR = 2;
    
    
    MEMMAP = MEMMAP_OPTION;
    
#if 2 == MEMMAP_OPTION
    for (unsigned i = 0; i < VEC_TOTAL_NUM; ++i)
    {
        intvec[i] = 0x18F09FE5; // ldr pc, [pc, #24];
    }
    
    intvec[VEC_TOTAL_NUM + VEC_RESET] = reinterpret_cast<unsigned>(&reset_handler);
    intvec[VEC_TOTAL_NUM + VEC_UNDEF] = reinterpret_cast<unsigned>(&undef_handler);
    intvec[VEC_TOTAL_NUM + VEC_SWI] = reinterpret_cast<unsigned>(&swi_handler);
    intvec[VEC_TOTAL_NUM + VEC_PREFETCH] = reinterpret_cast<unsigned>(&prefetch_handler);
    intvec[VEC_TOTAL_NUM + VEC_DATA_ABORT] = reinterpret_cast<unsigned>(&data_handler);
    intvec[VEC_TOTAL_NUM + VEC_IRQ] = reinterpret_cast<unsigned>(&irq_handler);
    intvec[VEC_TOTAL_NUM + VEC_FIQ] = reinterpret_cast<unsigned>(&fiq_handler);
#endif

    VICSoftIntClear = ~0;
    VICIntEnClear = ~0;
    VICIntSelect = 0;

    VICVectAddr0 = reinterpret_cast<unsigned long>(&irq_timer);
    VICVectCntl0 = 4 + BIT_(5);
    VICIntSelect &= ~BIT_(4);    
    VICDefVectAddr = reinterpret_cast<unsigned long>(&non_vectored_handler);

    VICIntEnable = BIT_(4);

    VICProtection = 1;
    VICVectAddr = 0;    

    T0IR = 0xFF;
    T0TCR = BIT_(0) + BIT_(1);
    T0CTCR = 0;
    T0PR = 0;
    T0PC = 0;
    T0MCR = 0;
    T0CCR = 0;
    T0EMR = 0;
    T0TCR = BIT_(0);
    
    SCS = BIT_(1);
    PINSEL2 &= ~BIT_(3);
    FIO1DIR |= BIT_(DBG_PIN);
}

namespace
{
    unsigned const TIMER_TICK = 20000000 / 1000;
}

void start(void)
{
    T0MR0 = T0TC + TIMER_TICK;
    T0MCR_bit.MR0INT = 1;
    __enable_interrupt();
}

int main(void)
{
    hw_init();
    start();
    while (true) {}
}

VIC_ISR void irq_timer(void)
{
    T0MR0 += TIMER_TICK;
    FIO1PIN ^= BIT_(DBG_PIN);
    T0IR = T0IR;
    VICVectAddr = 0;
}

VIC_ISR void non_vectored_handler(void)
{
    VICVectAddr = 0;
}

intvec[i] = 0x18F09FE5; // ldr pc, [pc, #24];

        for( i = 0; i < 0x40; i += 4 )
        {
            tmp = __readMemory32(0x40000000 + i, "Memory");
            __writeMemory32(tmp,0x00000000 + i, "Memory");
        }

    org    0x00
__program_start:        LDR pc, [pc, #24]
        undefvec:        B           undef_handler
        swivec:            B           swi_handler
        pabtvec:        B           prefetch_handler
        dabtvec:        B           data_handler

    org 0x18
        irqvec:            B            irq_handler
        fiqvec:         B           fiq_handler
        
    org 0x20
        DC32                        ?cstartup

intvec[VEC_TOTAL_NUM + VEC_RESET] = ARM7TDMI_MEM_32(0x00000020);

MEMMAP = MEMMAP_OPTION;

    MODULE  BOOTLOADER_UTIL;
    PUBLIC  bootloader_enable_interrupt;
    PUBLIC  bootloader_disable_interrupt;
    RSEG    BOOTLOADERCODE
    
    CODE32    

bootloader_enable_interrupt:
    PUSH    {r0}
    MRS     r0, cpsr
    ORR     r0, r0, #192
    MSR     cpsr_c, r0
    POP     {r0}
    BX      lr;

bootloader_disable_interrupt:
    PUSH    {r0}
    MRS     r0, cpsr
    bic     r0, r0, #192
    MSR     cpsr_c, r0
    POP     {r0}
    BX      lr;

    LTORG;
    ENDMOD;