#include "system_defines.h"
#include "cyclone_device.h"
#include "pmbus_commands.h"
#include "pmbus.h"
#include "variables.h"
#include "function_definitions.h"
#include "software_interrupts.h"

#pragma INTERRUPT(undefined_instruction_exception,UDEF)
void undefined_instruction_exception(void)
{
}

#pragma INTERRUPT(software_interrupt,SWI)
void software_interrupt(Uint32 arg1, Uint32 arg2, Uint32 arg3, Uint8 swi_number)
//void software_interrupt(Uint32 *address, Uint32 data, Uint32 more_data, Uint8 swi_number)
{
	//make sure interrupts are disabled
	asm(" MRS     r3, cpsr "); 		// get psr
	asm(" ORR     r3, r3, #0xc0 "); // set interrupt disables
	asm(" MSR     cpsr, r3"); 		// restore psr

	asm("  LDRB  R3,[R14,#-1]"); //get swi number into R3 as fourth operand

	switch (swi_number) //handle flash write/erase and ROM backdoor first
	{
	case 0: 
		//--------------------------------------------------------------------------------------
		// SWI ALIAS: erase_data_flash_segment()
		// 	Erases one segment of Data Flash and wait for erase to complete.
		//--------------------------------------------------------------------------------------
	case 1: 	
		//--------------------------------------------------------------------------------------
		// SWI ALIAS: erase_dflash_segment_no_delay()
		// 	Erase one segment of Data Flash and return without waiting for completion.
		//--------------------------------------------------------------------------------------
		{
			union DFLASHCTRL_REG dflashctrl_shadow;	// Shadow copy of control register

			if (arg1 >= DATA_FLASH_NUM_SEGMENTS)	
			{
				return;		// Invalid segment number
			}
			DecRegs.FLASHILOCK.all = 0x42DC157E; //unlock flash write;
			// Set the bits in the Data Flash Control Register to erase the indicated segment
			dflashctrl_shadow.all = DecRegs.DFLASHCTRL.all;	// Read the hardware register
			dflashctrl_shadow.bit.PAGE_ERASE = 1; 			// Erase one segment
			dflashctrl_shadow.bit.PAGE_SEL = arg1;		// Segment number
			DecRegs.DFLASHCTRL.all = dflashctrl_shadow.all;	// Write the hardware register
			if (swi_number == 1)	// 0= Wait for erase to complete, 1= return immediately
			{
				return;
			}
			while(DecRegs.DFLASHCTRL.bit.BUSY != 0)
			{
				; //do nothing while it programs
			}
			return;

		}
	case 3: //write word to data flash

		if(((arg1) < DATA_FLASH_START_ADDRESS) ||
			((arg1) > DATA_FLASH_END_ADDRESS))
		{//if out of data flash range
			return;
		}

		//this clears read only bit to permit writes to data flash.
		DecRegs.FLASHILOCK.all = 0x42DC157E; //unlock flash write

		DecRegs.MFBALR2.bit.BLOCK_SIZE =2;
		DecRegs.MFBALR2.bit.ADDRESS = 0x22;
		DecRegs.MFBALR2.bit.RONLY = 0;

		//put data in word.
		*(Uint32 *)(arg1 & 0xfffffffc) = arg2 ;

		DecRegs.MFBALR2.bit.RONLY = 1;

		while(DecRegs.DFLASHCTRL.bit.BUSY != 0)
		{
			; //do nothing while it programs
		}
		return;

		//handle interrupt enables/disables next
	case 4: //enable fiq
		asm(" MRS     r0, spsr "); //get saved psr
		asm(" BIC     r0, r0, #0x40 "); // clear fiq disable
		asm(" MSR     spsr, r0"); //restore saved psr
		return;
	case 5: //disable fiq
		asm(" MRS     r0, spsr "); //get saved psr
		asm(" ORR     r0, r0, #0x40 "); // set fiq disable
		asm(" MSR     spsr, r0"); //restore saved psr
		return;
	case 6: //enable irq
		asm(" MRS     r0, spsr "); //get saved psr
		asm(" BIC     r0, r0, #0x80 "); // clear irq disable
		asm(" MSR     spsr, r0"); //restore saved psr
		return;
	case 7: //disable irq
		asm(" MRS     r0, spsr "); //get saved psr
		asm(" ORR     r0, r0, #0x80 "); // set irq disable
		asm(" MSR     spsr, r0"); //restore saved psr
		return;
	case 8: //write to fiq/irq program_control_register
		CimRegs.FIRQPR.all = arg1;
		return;
	case 9: //write to fiq/irq program_control_register
		CimRegs.REQMASK.all = arg1;
		return;
	case 10: // switch to supervisor mode
		asm(" MRS     r0, spsr "); //get saved psr
		asm(" BIC	  r0, r0, #0x1F "); // clear 5 lsbs.
		asm(" ORR     r0, r0, #0x13 "); // set mode bits to 13.
		asm(" MSR     spsr, r0"); //restore saved psr
		return;
	case 11: // switch to user mode
		asm(" MRS     r0, spsr "); //get saved psr
		asm(" BIC	  r0, r0, #0x1F "); // clear 5 lsbs.
		asm(" ORR     r0, r0, #0x10 "); // set mode bits to 10.
		asm(" MSR     spsr, r0"); //restore saved psr
		return;
	case 12: // clear integrity word.
		{
			{
				register Uint32 * program_index = (Uint32 *) 0x19000; //store destination address for program
				register Uint32 * source_index = (Uint32 *)zero_out_integrity_word; //Set source address of PFLASH;

				register Uint32 counter;

				for(counter=0; counter < 500; counter++) //Copy program from PFLASH to RAM
				{
					*(program_index++)=*(source_index++);
				}
			}
			{
				register FUNC_PTR func_ptr;
				func_ptr=(FUNC_PTR)0x19000;     //Set function to 0x19000
				func_ptr(); 
			}        //execute erase checksum

			return;
		}
	case 14: // erase entire Program Flash
		{
			{
				register Uint32 * program_index = (Uint32 *) 0x19000; //store destination address for program
				register Uint32 * source_index = (Uint32 *)clear_program_flash; //Set source address of PFLASH;

				register Uint32 counter;

				for(counter=0; counter < 500; counter++) //Copy program from PFLASH to RAM
				{
					*(program_index++)=*(source_index++);
				}
			}
			{
				register FUNC_PTR func_ptr;
				func_ptr=(FUNC_PTR)0x19000;     //Set function to 0x19000
				func_ptr(); 
			}        //execute mass erase PFLASH

			return;
		}
	default:
		break;
	}
}

#pragma INTERRUPT(abort_prefetch_exception,PABT)
void abort_prefetch_exception(void)
{
}

#pragma INTERRUPT(abort_data_fetch_exception,DABT)
void abort_data_fetch_exception(void)
{
}

#pragma INTERRUPT(fast_interrupt,FIQ)
void fast_interrupt(void)
{
}



