#include "efi.h"

#include "isr.h"







static void (*cHandlerFunc[255])() = {0};

static short vectorNum;



void interrupt_handler(void);

asm ("

	.TEXT

	.ALIGN 2 

	.GLOBL interrupt_handler



	/*

	** Note the 0x42 constant is the number of bytes into the stack to

	** obtain the vector offset. This constant is determined by the

	** number and size of the registers pushed onto the stack by the

	** MOVEM instruction.

	*/

interrupt_handler:

	MOVEM.L %D0-%D7/%A0-%A6,%SP@-   /* Save all regs */

	MOVE.W (0x42,%A7),%D0			/* Grab the vector offset from the stack frame */

	LSR.W #2,%D0					/* Shift to get the vector number */

	MOVE.W %D0,vectorNum			/* Store to named variable */

	JBSR cHandler					/* Jump to 'c' handler */

	MOVEM.L %SP@+,%D0-%D7/%A0-%A6	/* Restore regs */

	RTE								/* Return from exception */

");



void installHandler(int vecNum, void (*func)() )

{

	static char fp = 1;



	/*

	** On first pass, load the vector table with a common

	** assembly language handler. This will in turn call our

	** specified 'c' language handler...

	*/

	if( fp )

	{

		short i;

		long *vectorPtr = (long *)VECTORBASE;



		/*

		** Load all vectors, skipping the reset program counter

		** and stack pointer.

		*/

		for( i=2; i<256; i++ )

		{

			vectorPtr[i] = (long)interrupt_handler;

		}

		fp = 0;

	}



	/*

	** Now install our 'c' handler in the function table.

	*/

	cHandlerFunc[vecNum] = func;

}



void cHandler()

{

	int i;

	/*

	** Now that we've got the vector offset, it's a simple matter

	** to compute the vector number. We use this number as an index

	** into a table of 'c' function pointers that actually do

	** something useful...

	*/

	if( cHandlerFunc[vectorNum] )

		(*cHandlerFunc[vectorNum])();

    else

    	i++;



	return;

}