#include "common.h"
#include "sysinit.h"
#include "dual_core_ds5.h"
#include "lpt.h"
#include "leds.h"
#include "startup.h"
#include "ecc_tests.h"
#include "benchinit.h"

/* Scatter file symbol to tell us the M4 start address */
extern unsigned int Image$$M4_CODE$$Base[];
void setup_interrupts(void);

int main_entry(void)
{

	sysinit();




	ENABLE_LEDS;

	//printf("Hello World from A5 Core!\n");

	/* This will give the M4 core the starting address and start clocking for execution */
	//start_m4();
	
//	setup_interrupts();


	
	/* Stay here */
	while (1)
	{
		led_blink_tower(1);
  
		/* Say Hello */
		//hello_A5();
	}

	return 0;
}

/* Blink LEDs on TWR-VF600 */
void led_blink_tower(uint32_t blinks)
{
	uint32_t i;
  
	/* Flash on and off LED's */
	for (i = 0; i < blinks; i++)
	{
		/* Turn on first two LEDs - these are next to each other on the board */
		LED1_ON;
		LED3_ON;

		/* Delay */
		//time_delay_ms(125);
		time_delay_local(0xFFFF00);

		/* Turn off first two LEDs */
		LED1_OFF;
		LED3_OFF;

		/* Delay */
		//time_delay_ms(200);			/* If this is used, it will pause the timer when M4 is halted */
		time_delay_local(0xFFFF00);
	}
}

void hello_A5()
{
	/* Lock gate to write UART, then unlock */
	SEMA4->GATE[1] = CORE0_LOCK;
	if(SEMA4->GATE[1] == CORE0_LOCK)
	{
		/* Output message from M4 */
		printf("Hello A5!\n");

		/* Unlock Semaphore */
		SEMA4->GATE[1] = UNLOCK;
	}
}

void start_m4()
{
	int address;

	/* Set starting point for M4 code using scatter symbol */
	/* Refer to the M4 scatter file for detail on the memory map layout */
	//address = (((unsigned int)(Image$$M4_CODE$$Base)) | 1);
	address = (((unsigned int)(Image$$M4_CODE$$Base)) | 0x401UL) & ~0x20000000UL;		//0x400 offset for vector table, +1 for Thumb mode

	/* Print start address with message */
	//printf("Starting M4 core at address 0x%x\n", address);

	SRC->GPR[2] = address;

	/* Enable M4 core */
	CCM->CCOWR = 0x15a5a;
}

void time_delay_local(uint32_t delay_cnt)
{
	do
	{
		__asm("nop");
		__asm("nop");
		__asm("nop");

	} while(delay_cnt--);
}

/********************************************************************
*   Function:     setup_interrupts
*   Purpose:      Route vectors to specific core
*   Input:        None
*   Return:       None
*
*   Notes:        Must define ARMCC_M4 if compiling for M4 core
*   				If not it will default to A5
*********************************************************************/
void setup_interrupts(void)
{
    /* install specific interrupt vectors based on core we are running on */
#ifdef ARMCC_M4

	/* Make sure vectors_m4.h has address of interrupt handler function setup */
	NVIC->IP[PDB_IRQn - 32] = (uint8_t)0x80U;
	NVIC->IP[ADC0_IRQn - 32] = (uint8_t)0x80U;
	NVIC->IP[ADC1_IRQn - 32] = (uint8_t)0x80U;
	NVIC->IP[DAC0_IRQn - 32] = (uint8_t)0x80U;
	NVIC->IP[DAC1_IRQn - 32] = (uint8_t)0x80U;
	NVIC->IP[DMA0_Transfer_Complete_IRQn - 32] = (uint8_t)0x80U;
	NVIC->IP[DMA0_Error_IRQn - 32] = (uint8_t)0x80U;
	NVIC->IP[DMA1_Transfer_Complete_IRQn - 32] = (uint8_t)0x80U;
	NVIC->IP[DMA1_Error_IRQn - 32] = (uint8_t)0x80U;
	//NVIC->IP[SNVS_Consolidated_IRQn - 32] = (uint8_t)0x80U;

	enable_irq(PDB_IRQn - 32);
	enable_irq(ADC0_IRQn - 32);
	enable_irq(ADC1_IRQn - 32);
	enable_irq(DAC0_IRQn - 32);
	enable_irq(DAC1_IRQn - 32);
	enable_irq(DMA0_Transfer_Complete_IRQn - 32);
	enable_irq(DMA0_Error_IRQn - 32);
	enable_irq(DMA1_Transfer_Complete_IRQn - 32);
	enable_irq(DMA1_Error_IRQn - 32);
	//enable_irq(SNVS_Consolidated_IRQn - 32);       //SRTC

	/* Route appropriate interrupts to M4 Core */
	for(i = 0; i < 112; i++)
	{
	//MSCM.IRSPRC[i].B.CP1 = 1;     /* route all to M4 */
	//MSCM.IRSPRC[i].B.CP1 = 0;     /* do not route any to M4 except for select few, see below */
	}

	/* route select interrupts to M4 using CP1 */
	MSCM.IRSPRC[PDB_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[ADC0_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[ADC1_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[DAC0_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[DAC1_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[DMA0_Transfer_Complete_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[DMA0_Error_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[DMA1_Transfer_Complete_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[DMA1_Error_IRQn - 32].B.CP1 = 1;
	MSCM.IRSPRC[SNVS_Consolidated_IRQn - 32].B.CP1 = 1;    //SRTC

	/* Turn OFF interrupts to A5 core using CP0 */
	MSCM.IRSPRC[PDB_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[ADC0_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[ADC1_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[DAC0_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[DAC1_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[DMA0_Transfer_Complete_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[DMA0_Error_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[DMA1_Transfer_Complete_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[DMA1_Error_IRQn - 32].B.CP0 = 0;
	MSCM.IRSPRC[SNVS_Consolidated_IRQn - 32].B.CP0 = 0;

#else  /* ARMCC_A5 */

	/* Turn OFF these interrupts to M4 using CP1 */
	MSCM->IRSPRC[PDB_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[ADC0_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[ADC1_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[DAC0_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[DAC1_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[DMA0_Transfer_Complete_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[DMA0_Error_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[DMA1_Transfer_Complete_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[DMA1_Error_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[DRAM_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[MSCM_ECC0_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	MSCM->IRSPRC[MSCM_ECC1_IRQn - 32] &= ~MSCM_IRSPRC_CP1En_MASK;
	//MSCM.IRSPRC[SNVS_Consolidated_IRQn - 32].B.CP1 = 0;    //SRTC


	/* Setup A5 for interrupts */
	/* This will enable interrupt, setup handler and setup IRSPRC */
	enable_interrupt(PDB_IRQn, pdb_isr_routine);
	enable_interrupt(ADC0_IRQn, adc0_isr_routine);
	enable_interrupt(ADC1_IRQn, adc1_isr_routine);
	enable_interrupt(DAC0_IRQn, dac0_isr_routine);
	enable_interrupt(DAC1_IRQn, dac1_isr_routine);
	enable_interrupt(DMA0_Transfer_Complete_IRQn, dma0_isr_routine);
	enable_interrupt(DMA0_Error_IRQn, dma0_isr_routine);
	enable_interrupt(DMA1_Transfer_Complete_IRQn, dma1_isr_routine);
	enable_interrupt(DMA1_Error_IRQn, dma1_isr_routine);
	enable_interrupt(MSCM_ECC0_IRQn, default_isr);
	enable_interrupt(MSCM_ECC1_IRQn, default_isr);
	enable_interrupt(DRAM_IRQn, default_isr);

	
#endif

}
