#include "arm_boot.h"

#define HIGH_PRIORITY	0x0001
#define LOW_PRIORITY	0x0002

// IRQ lines masks
#define IRQ_MSK_TIMER0      0x0001
#define IRQ_MSK_TIMER1      0x0002
#define IRQ_MSK_TIMER2      0x0004
#define IRQ_MSK_GPIO_0      0x0008
#define IRQ_MSK_EIM         0x0010
#define IRQ_MSK_KBGPIO_7_0  0x0020
#define IRQ_MSK_UART        0x0040
#define IRQ_MSK_UART_IRDA   0x0080
#define IRQ_MSK_KBGPIO_15_8 0x0100
#define IRQ_MSK_GPIO_3      0x0200
#define IRQ_MSK_GPIO_2      0x0400
#define IRQ_MSK_I2C         0x0800
#define IRQ_MSK_GPIO_1      0x1000
#define IRQ_MSK_SPI         0x2000
#define IRQ_MSK_GPIO_19_4   0x4000
#define IRQ_MSK_API         0x8000
#define IRQ_MSK_ALL         0xFFFF


// IRQ lines
#define IRQ_TIMER0      0
#define IRQ_TIMER1      1
#define IRQ_TIMER2      2
#define IRQ_GPIO_0      3
#define IRQ_EIM         4
#define IRQ_KBGPIO_7_0  5
#define IRQ_UART        6
#define IRQ_UART_IRDA   7
#define IRQ_KBGPIO_15_8 8
#define IRQ_GPIO_3      9
#define IRQ_GPIO_2      10
#define IRQ_I2C         11
#define IRQ_GPIO_1      12
#define IRQ_SPI         13
#define IRQ_GPIO_19_4   14
#define IRQ_API         15


// int_ctrl_reg register values
#define INT_CTRL_IRQ_AGR   0x0001
#define INT_CTRL_FIQ_AGR   0x0002



// ---------------------- Interrupt Setup --------------------------------
#define SWI_CMD_SETSTK         1
#define SWI_CMD_SETCPSR        2

#define SWI_MODE_USER          0x10
#define SWI_MODE_FIQ           0x11
#define SWI_MODE_IRQ           0x12
#define SWI_MODE_SVC           0x13

#define SWI_MASK_IRQ           0x80
#define SWI_MASK_FIQ           0x40

#define SWI_IRQENABLE          0x00
#define SWI_IRQDISABLE         0x80
#define SWI_FIQENABLE          0x00
#define SWI_FIQDISABLE         0x40

#define ENABLE_IRQ_INTERRUPTS  SWI_Command(SWI_CMD_SETCPSR, \
                                           SWI_MASK_IRQ,    \
                                           SWI_IRQENABLE)
#define DISABLE_IRQ_INTERRUPTS SWI_Command(SWI_CMD_SETCPSR, \
                                           SWI_MASK_IRQ,    \
                                           SWI_IRQDISABLE)
#define ENABLE_FIQ_INTERRUPTS  SWI_Command(SWI_CMD_SETCPSR, \
                                           SWI_MASK_FIQ,    \
                                           SWI_FIQENABLE)
#define DISABLE_FIQ_INTERRUPTS SWI_Command(SWI_CMD_SETCPSR, \
                                           SWI_MASK_FIQ,    \
                                           SWI_FIQDISABLE)

int SWI_cmd, SWI_arg1, SWI_arg2;

static void SWI_Command(int cmd, unsigned long arg1, unsigned long arg2)
{
    SWI_cmd = cmd;
    SWI_arg1 = arg1;
    SWI_arg2 = arg2;
    asm(" SWIAL #0");
}

// ---------------------- Stack Setup ---------------------------
#define SVC_STACK_SIZE  128      
#define IRQ_STACK_SIZE  128      
#define FIQ_STACK_SIZE  128      

static unsigned long _svc_stack[SVC_STACK_SIZE];
static unsigned long _irq_stack[IRQ_STACK_SIZE];
static unsigned long _fiq_stack[FIQ_STACK_SIZE];

void setupstacks()
{
    SWI_Command(SWI_CMD_SETSTK, SWI_MODE_SVC, 
                (unsigned long) _svc_stack + SVC_STACK_SIZE);
    SWI_Command(SWI_CMD_SETSTK, SWI_MODE_IRQ, 
                (unsigned long) _irq_stack + IRQ_STACK_SIZE);
    SWI_Command(SWI_CMD_SETSTK, SWI_MODE_FIQ, 
                (unsigned long) _fiq_stack + FIQ_STACK_SIZE);
} 
// --------------------------------------------------------------


unsigned long factorial = 0xFFFFFFFF;


asm("ISR: .sect \"isr\" ");

volatile unsigned long* const it_reg            	= (unsigned long*)0xffff2d00;
volatile unsigned long* const mask_it_reg           = (unsigned long*)0xffff2d04;
volatile unsigned long* const src_irq_reg           = (unsigned long*)0xffff2d08;
volatile unsigned long* const src_fiq_reg           = (unsigned long*)0xffff2d0c;
volatile unsigned long* const src_irq_bin_reg       = (unsigned long*)0xffff2d10;
volatile unsigned long* const src_fiq_bin_reg       = (unsigned long*)0xffff2d14;
volatile unsigned long* const int_ctrl_reg          = (unsigned long*)0xffff2d18;
volatile unsigned long* const ilr_irq[] = {
    (unsigned long*)0xffff2d1c, /*IRQ0  Watchdog TIMER */
    (unsigned long*)0xffff2d20, /*IRQ1  TIMER1*/
    (unsigned long*)0xffff2d24, /*IRQ2  TIMER2*/
    (unsigned long*)0xffff2d28, /*IRQ3  GPIO0*/
    (unsigned long*)0xffff2d2c, /*IRQ4  Ethernet interface*/
    (unsigned long*)0xffff2d30, /*IRQ5  KBGPIO(7:0) */
    (unsigned long*)0xffff2d34, /*IRQ6  UART*/
    (unsigned long*)0xffff2d38, /*IRQ7  UART_IRDA*/
    (unsigned long*)0xffff2d3c, /*IRQ8  KBGPIO(15:8)*/
    (unsigned long*)0xffff2d40, /*IRQ9  GPIO3*/
    (unsigned long*)0xffff2d44, /*IRQ10 GPIO2*/
    (unsigned long*)0xffff2d48, /*IRQ11 I2C*/
    (unsigned long*)0xffff2d4c, /*IRQ12 GPIO1*/
    (unsigned long*)0xffff2d50, /*IRQ13 SPI*/
    (unsigned long*)0xffff2d54, /*IRQ14 GPIO(19:4)*/
    (unsigned long*)0xffff2d58  /*IRQ15 API*/
};
	  
interrupt
void c_spin( void )
{
	while (1);  /*interrupted in bad place*/
}


void timer1_handler(void)
{
}

void timer2_handler(void)
{
}

// Enable the API interrupt on the MCU
void setup_api_interrupt(void)
{
#define API_IRQ      15

    // set the 'I' bit in CPSR to enable IRQ interrupts
    ENABLE_IRQ_INTERRUPTS;

    // configure the API interrupt level register:
    //      SENSE_EDGE = 1
    //      PRIORITY   = 0
    //      FIQ        = 0
	*ilr_irq[API_IRQ] = 0x20;

    // clear the API interrupt bit in the Interrupt Register
    // this clears all pending API interrupts
    *it_reg &= ~(1 << API_IRQ); 

    // clear the API interrupt bit in the Mask Interrupt Register
    // (enables the interrupt)
  	*mask_it_reg &= ~(1 << API_IRQ); 
}

// ISR for the API interrupt
void api_handler(void)
{
    volatile unsigned long  *dsp_output  = (unsigned long *)DSP_OUTPUT;
    factorial = Read_Long_API(dsp_output);
}


void it_handler(unsigned short priority)
{
    int it_source;

	if (priority == HIGH_PRIORITY) {
   		it_source = *src_fiq_reg;
   	}
  	else {
    	it_source = *src_irq_reg;
    }
    
  	switch (it_source) {

    case 0x0001 : 	break;
    case 0x0002 : 
        timer1_handler(); 
        break;
    case 0x0004 : 
        timer2_handler();
        break;
    case 0x0008 : 	break;
    case 0x0010 : 	break;
    case 0x0020 : 	break;
    case 0x0040 : 	break;
    case 0x0080 : 	break;
    case 0x0100 : 	break;
    case 0x0200 : 	break;
    case 0x0400 : 	break;
    case 0x0800 : 	break;
    case 0x1000 : 	break;
    case 0x2000 : 	break;
    case 0x4000 : 	break;
    case 0x8000 : 	
        api_handler();
        break;
        
    default	 	:   asm(" .long 0xDEFED0FE");
  	}

    /* re-enable interrupt on the interrupt controller */
  	if (priority == HIGH_PRIORITY) {
        *int_ctrl_reg |= INT_CTRL_FIQ_AGR;
    }
  	else {
        *int_ctrl_reg |= INT_CTRL_IRQ_AGR;
    }
}  

interrupt
void c_irq( void )
{
	it_handler(LOW_PRIORITY);
}

interrupt
void c_fiq( void )
{
	it_handler(HIGH_PRIORITY);
}

void resetinterrupts()
{
    // Disable all interrupts
    *mask_it_reg = IRQ_MSK_ALL;      
}