/* 
 * 2013 by Fabian Vogt
 */

#include <stdint.h>

#define VIC_REG(x) (*(uint32_t*)(0xDC000000 + x))

void enable_irq(uint8_t irq)
{
	VIC_REG(0x10) = 1 << irq;
}

void disable_irq(uint8_t irq)
{
	VIC_REG(0x14) = 1 << irq;
}

void set_as_fiq(uint8_t irq)
{
	disable_irq(irq);
	VIC_REG(0x0C) = 1 << irq;
}

void activate_ints()
{
	asm(	"mrs r0, cpsr;"
		"bic r0, r0, #0xC0;"
		"msr cpsr_c, r0;" ::: "r0");
}
void deactivate_ints()
{
	asm(	"mrs r0, cpsr;"
		"orr r0, r0, #0xC0;"
		"msr cpsr_c, r0;" ::: "r0");
}

static uint32_t old_irq, old_fiq, old_mask;
static uint32_t* fiq = (uint32_t*)0x3C;
static uint32_t* irq = (uint32_t*)0x38;

// fiq_handler must be a jump instruction (either ldr pc, something or b something)
// irq_handler must either be coded in asm or in c with __irq prefix (__irq void handler(... )
void init_interrupts(void (*fiq_handler)(void), void (*irq_handler)(void))
{
	deactivate_ints();
	old_mask = VIC_REG(0x10);
	
	VIC_REG(0x14) = ~0;	//Disable all IRQs
	
	old_fiq = *fiq;
	old_irq = *irq;
	
	*fiq = (uint32_t)fiq_handler;
	*irq = (uint32_t)irq_handler;
	
	activate_ints();
}

void unregister()
{
	deactivate_ints();
	
	VIC_REG(0x10) = old_mask;
	VIC_REG(0x0C) = 0; //Disable FIQ
	
	*fiq = old_fiq;
	*irq = old_irq;
	
	activate_ints();
}