/* ***************************************************************************************************************
   crt.s                  STARTUP  ASSEMBLY  CODE
                        -----------------------

   Module includes the interrupt vectors and start-up code.
  *************************************************************************************************************** */

/* Stack Sizes */
.set  UND_STACK_SIZE, 0x00000004      /* stack for "undefined instruction" interrupts is 4 bytes  */
.set  ABT_STACK_SIZE, 0x00000004      /* stack for "abort" interrupts is 4 bytes                  */
.set  FIQ_STACK_SIZE, 0x00000004      /* stack for "FIQ" interrupts  is 4 bytes                  */
.set  IRQ_STACK_SIZE, 0X00000004      /* stack for "IRQ" normal interrupts is 4 bytes             */
.set  SVC_STACK_SIZE, 0x00000004      /* stack for "SVC" supervisor mode is 4 bytes              */

#.set _MEMMAP,      0xE01FC040

/* Standard definitions of Mode bits and Interrupt (I & F) flags in PSRs (program status registers) */
.set  MODE_USR, 0x10                  /* Normal User Mode                               */
.set  MODE_FIQ, 0x11                  /* FIQ Processing Fast Interrupts Mode                   */
.set  MODE_IRQ, 0x12                  /* IRQ Processing Standard Interrupts Mode                */
.set  MODE_SVC, 0x13                  /* Supervisor Processing Software Interrupts Mode          */
.set  MODE_ABT, 0x17                  /* Abort Processing memory Faults Mode                   */
.set  MODE_UND, 0x1B                  /* Undefined Processing Undefined Instructions Mode       */
.set  MODE_SYS, 0x1F                  /* System Running Priviledged Operating System Tasks  Mode   */

.set  I_BIT, 0x80                     /* when I bit is set, IRQ is disabled (program status registers) */
.set  F_BIT, 0x40                     /* when F bit is set, FIQ is disabled (program status registers) */


.text
.arm

.global   Reset_Handler
.global _startup
.func   _startup

_startup:

# Exception Vectors
_vectors:       ldr     PC, Reset_Addr         
                ldr     PC, Undef_Addr
                ldr     PC, SWI_Addr
                ldr     PC, PAbt_Addr
                ldr     PC, DAbt_Addr
                NOP                            ; Reserved Vector
                LDR     PC, IRQ_Addr           ; I added this
                LDR     PC, [PC, #-0x0FF0]     ; Vector from VicVectAddr
                LDR     PC, FIQ_Addr

Reset_Addr:     .word   Reset_Handler      /* defined in this module below  */
Undef_Addr:     .word   UNDEF_Routine      /* defined in main.c  */
SWI_Addr:       .word   SWI_Routine         /* defined in main.c  */
PAbt_Addr:      .word   UNDEF_Routine      /* defined in main.c  */
DAbt_Addr:      .word   UNDEF_Routine      /* defined in main.c  */
IRQ_Addr:       .word   IRQ_Routine         /* defined in main.c  */
FIQ_Addr:       .word   FIQ_Routine         /* defined in main.c  */
                .word   0               /* rounds the vectors and ISR addresses to 64 bytes total  */

Reset_Handler: 
            /* Setup a stack for each mode - note that this only sets up a usable stack
            for User mode.   Also each mode is setup with interrupts initially disabled. */
              
             ldr   r0, =_stack_end
             msr   CPSR_c, #MODE_UND|I_BIT|F_BIT    /* Undefined Instruction Mode  */
             mov   sp, r0
             sub   r0, r0, #UND_STACK_SIZE
             msr   CPSR_c, #MODE_ABT|I_BIT|F_BIT    /* Abort Mode */
             mov   sp, r0
             sub   r0, r0, #ABT_STACK_SIZE
             msr   CPSR_c, #MODE_FIQ|I_BIT|F_BIT    /* FIQ Mode */
             mov   sp, r0   
               sub   r0, r0, #FIQ_STACK_SIZE
             msr   CPSR_c, #MODE_IRQ|I_BIT|F_BIT    /* IRQ Mode */
             mov   sp, r0
             sub   r0, r0, #IRQ_STACK_SIZE
             msr   CPSR_c, #MODE_SVC|I_BIT|F_BIT    /* Supervisor Mode */
             mov   sp, r0
             sub   r0, r0, #SVC_STACK_SIZE
             msr   CPSR_c, #MODE_SYS|I_BIT|F_BIT    /* User Mode */
             mov   sp, r0

            /* copy .data section (Copy from ROM to RAM) */
                ldr     R1, =_etext
                ldr     R2, =_data
                ldr     R3, =_edata
1:              cmp     R2, R3
                ldrlo   R0, [R1], #4
                strlo   R0, [R2], #4
                blo     1b

            /* Clear .bss section (Zero init)  */
                mov     R0, #0
                ldr     R1, =_bss_start
                ldr     R2, =_bss_end
2:            cmp     R1, R2
                strlo   R0, [R1], #4
                blo     2b

            /* Enter the C code  */
                b       main

.endfunc
.end

/* Enable and disable functions "ripped" from a sample by R O Software.
* Copyright 2004, R O SoftWare
* No guarantees, warrantees, or promises, implied or otherwise.
* May be used for hobby or commercial purposes provided copyright
* notice remains intact. */

#include "VIClowlevel.h"

#define IRQ_MASK 0x00000080


static inline unsigned asm_get_cpsr(void)
{
unsigned long retval;

asm volatile (" mrs %0, cpsr" : "=r" (retval) : /* no inputs */ );
return retval;
}


static inline void asm_set_cpsr(unsigned val)
{
asm volatile (" msr cpsr, %0" : /* no outputs */ : "r" (val) );
}


unsigned enableIRQ(void)
{
unsigned _cpsr;

_cpsr = asm_get_cpsr();
asm_set_cpsr(_cpsr & ~IRQ_MASK);
return _cpsr;
}


unsigned disableIRQ(void)
{
unsigned _cpsr;

_cpsr = asm_get_cpsr();
asm_set_cpsr(_cpsr | IRQ_MASK);
return _cpsr;
}


unsigned restoreIRQ(unsigned oldCPSR)
{
unsigned _cpsr;

_cpsr = asm_get_cpsr();
asm_set_cpsr((_cpsr & ~IRQ_MASK) | (oldCPSR & IRQ_MASK));
return _cpsr;
}
/* end of R O code */

void IRQ_Routine(void)
{
    int i;
    IOSET0 = 0x30600000;        //4 LEDs blink
    for(i=0;i<0x0000ffff;i++);
    IOCLR0 = 0x30600000;
    T0IR = 0x01;                //clear interrupt
    VICVectAddr = 0;            //end of interrupt - dummy write
}

void FIQ_Routine(void){
    while (1) ; 
}
void SWI_Routine(void){
    while (1) ; 
}
void UNDEF_Routine(void) {
	while (1) ;	
}

.set  I_BIT, 0x80		/* when I bit is set, IRQ is disabled (program status registers) */
.set  F_BIT, 0x40		/* when F bit is set, FIQ is disabled (program status registers) */

...

	msr   CPSR_c, #MODE_FIQ|I_BIT|F_BIT 	/* FIQ Mode */
	mov   sp, r0	
	sub   r0, r0, #FIQ_STACK_SIZE
	msr   CPSR_c, #MODE_IRQ|I_BIT|F_BIT 	/* IRQ Mode */
	mov   sp, r0
	sub   r0, r0, #IRQ_STACK_SIZE

.set  I_BIT, 0x80		/* when I bit is set, IRQ is disabled (program status registers) */

    			msr   CPSR_c, #MODE_IRQ|I_BIT|F_BIT 	/* IRQ Mode */
    			mov   sp, r0
    			sub   r0, r0, #IRQ_STACK_SIZE

    			msr   CPSR_c, #MODE_IRQ 	/* IRQ Mode */
    			mov   sp, r0
    			sub   r0, r0, #IRQ_STACK_SIZE

#include "LPC214x.h"
#define PLOCK 0x400
#define IRQ_MASK 0x00000080
void init(void);
void IRQ_Routine (void)   __attribute__ ((interrupt("IRQ")));
void FIQ_Routine (void)   __attribute__ ((interrupt("FIQ")));
void SWI_Routine (void)   __attribute__ ((interrupt("SWI")));
void UNDEF_Routine (void) __attribute__ ((interrupt("UNDEF")));
unsigned enableIRQ(void);
unsigned disableIRQ(void);
unsigned restoreIRQ(unsigned oldCPSR);

unsigned int cp;

int main(void)
{
    int i;
    IODIR0 = 0x30600000;
    IOCLR0 = 0x30600000;                    //LEDs off
    init();
    T0TCR = 0x02;                           //reset counter
    T0IR = 0xff;
    T0MCR = 0x0003;                         //interrupt and reset on MR0
    T0MR0 = 0x0000ffff;                     //compare-hit count
    VICVectCntl0 = 0x00000024;              //use it for Timer 0 Interrupt:
    VICVectAddr0 = (unsigned)&IRQ_Routine;   //set interrupt vector in 0
    VICIntEnable = 0x00000010;              //enable TIMER0 interrupt
    T0TCR = 0x01;                           //enable Timer0
    cp = enableIRQ();
    
    while(1){
        i=0;
        i=1;
    }
}

void IRQ_Routine(void)
{
    int i;
    IOSET0 = 0x30600000;        //4 LEDs blink
    for(i=0;i<0x0000ffff;i++);
    IOCLR0 = 0x30600000;
    T0IR = 0x01;                //clear interrupt
    VICVectAddr0 = 0;           //end of interrupt - dummy write
}

void init(void)
{
    PLLCFG=0x24;                //set multiplier/divider values
    PLLFEED=0xaa;
    PLLFEED=0x55;
    PLLCON=0x01;                //enable PLL
    PLLFEED=0xaa;
    PLLFEED=0x55;
    while(!(PLLSTAT & PLOCK));  //wait for the PLL to lock to set frequency
    PLLCON=0x3;                 //connect the PLL as the clock source
    PLLFEED=0xaa;
    PLLFEED=0x55;
    MAMCR=0x02;                 //enable MAM
    MAMTIM=0x04;                //set number of clocks for flash memory fetch
    VPBDIV=0x01;                //set peripheral clock(pclk) to system clock(cclk)
}

void FIQ_Routine(void){
    while (1) ; 
}
void SWI_Routine(void){
    while (1) ; 
}
void UNDEF_Routine(void) {
	while (1) ;	
}
static inline unsigned asm_get_cpsr(void)
{
  unsigned long retval;
  asm volatile (" mrs  %0, cpsr" : "=r" (retval) : /* no inputs */  );
  return retval;
}

static inline void asm_set_cpsr(unsigned val)
{
  asm volatile (" msr  cpsr, %0" : /* no outputs */ : "r" (val)  );
}

unsigned enableIRQ(void)
{
  unsigned _cpsr;

  _cpsr = asm_get_cpsr();
  asm_set_cpsr(_cpsr & ~IRQ_MASK);
  return _cpsr;
}

unsigned disableIRQ(void)
{
  unsigned _cpsr;

  _cpsr = asm_get_cpsr();
  asm_set_cpsr(_cpsr | IRQ_MASK);
  return _cpsr;
}

unsigned restoreIRQ(unsigned oldCPSR)
{
  unsigned _cpsr;

  _cpsr = asm_get_cpsr();
  asm_set_cpsr((_cpsr & ~IRQ_MASK) | (oldCPSR & IRQ_MASK));
  return _cpsr;
}

void IRQ_Routine(void)
{
    int i;
    IOSET0 = 0x30600000;        //4 LEDs blink
    for(i=0;i<0x0000ffff;i++);
    IOCLR0 = 0x30600000;
    T0IR = 0x01;                //clear interrupt
    VICVectAddr0 = 0;           //end of interrupt - dummy write
//The above bold changes the address again
}

void IRQ_Routine(void)
{
    int i;
    IOSET0 = 0x30600000;        //4 LEDs blink
    for(i=0;i<0x0000ffff;i++);
    IOCLR0 = 0x30600000;
    T0IR = 0x01;                //clear interrupt
    VICVectAddr = 0x0000;           //end of interrupt - dummy write
}