#include <stdio.h>
#include <time.h>

#define IRQNOW *intAddress = 1
#define FIQNOW *intAddress = 2
#define CLEARIRQ *clearAddress = 1
#define CLEARFIQ *clearAddress = 2
#define IRQIN(cycles) *irqAddress = cycles
#define FIQIN(cycles) *fiqAddress = cycles

unsigned *intAddress = (unsigned*) 0x200000;
unsigned *irqAddress = (unsigned*) 0x200004;
unsigned *fiqAddress = (unsigned*) 0x200008;
unsigned *clearAddress = (unsigned*) 0x20000C;

/* function to enable IRQ and FIQ */
__inline void enable_IF(void);

extern int exit(int);

unsigned Install_Handler( unsigned routine, unsigned *vector )
{
	unsigned vec, oldvec;

	vec = ((routine - (unsigned)vector - 0x8) >> 2 );
	if( vec & 0xff000000 ) {
		printf( "Installation of exception handler failed\n" );
		exit(1);
	}

	vec = 0xea000000 | vec;
	oldvec = *vector;
	*vector = vec;
	return (oldvec);
}

__irq void IRQHandler(void)
{
	CLEARIRQ;
}

__irq void FIQHandler(void)
{
	CLEARFIQ;
}

int main()
{
	unsigned *irqvec = (unsigned*) 0x18;
	unsigned *fiqvec = (unsigned*) 0x1c;

	printf( "Installing handlers\n" );

	(void)Install_Handler( (unsigned)IRQHandler, irqvec );
	(void)Install_Handler( (unsigned)FIQHandler, fiqvec );
	
	enable_IF();

	printf( "Handlers installed\n" );

	printf( "Cause an irq\n" );
	IRQNOW;

	printf( "Cause an FIQ\n" );
	FIQNOW;

	printf( "Cause an irq\n" );
	IRQNOW;

	IRQIN(600);
	printf( "1\n2\n3\n4\n5\n6\n7\n8\n9\n" );
	FIQIN(1500);
	printf( "1\n2\n3\n4\n5\n6\n7\n8\n9\n" );

	IRQIN(1000);
	FIQIN(1000);
	
	printf( "1\n2\n3\n4\n5\n6\n7\n8\n9\n" );
}

__inline void enable_IF(void)
{
  int tmp;
  __asm
  {
    MRS tmp, CPSR
    BIC tmp, tmp, #0xC0
    MSR CPSR_c, tmp
  }
}