#include "boot.h" typedef struct IrqEntry {		void	(*r)(Ureg*, void*);		void 	*a;				} IrqEntry; IrqEntry Irq[V_MAXNUM+1]; static void dumpstk(ulong *);void dumpregs(Ureg* ureg); voidsetvec(int v, void (*f)(Ureg*, void*), void* a){	if(v < 0 || v >= V_MAXNUM)		panic("setvec: interrupt source %d out of range\n", v);	Irq[v].r = f;	Irq[v].a = a;} ulong irqstack[64];ulong fiqstack[64];ulong abtstack[64];ulong undstack[64]; static voidsafeintr(Ureg*, void *a){	int v = (int)a;//	print("spurious interrupt %d\n", v);	USED(v);} voidtrapinit(void){	int offset;	ulong op;	int v;	int s; 	s = splhi(); 	/* set up stacks for various exceptions */ 	setr13(PsrMirq, irqstack+nelem(irqstack)-1);	setr13(PsrMfiq, fiqstack+nelem(fiqstack)-1);	setr13(PsrMabt, abtstack+nelem(abtstack)-1);	setr13(PsrMund, undstack+nelem(undstack)-1); 	for(v = 0; v <= V_MAXNUM; v++) {		Irq[v].r = safeintr;		Irq[v].a = (void *)v;	} 	/* Reset Exception */	offset = ((((ulong) _vsvccall) - 0x0)-8) >> 2;	op = ( 0xea << 24 ) | offset;	*((ulong *) 0x0) = op;  	/* Undefined Instruction Exception */	offset = ((((ulong) _vundcall) - 0x4)-8) >> 2;	op = ( 0xea << 24 ) | offset;	*((ulong *) 0x4) = op; 	/* SWI Exception */	offset = ((((ulong) _vsvccall) - 0x8)-8) >> 2;	op = ( 0xea << 24 ) | offset;	*((ulong *) 0x8) = op; 	/* Prefetch Abort Exception */	offset = ((((ulong) _vpabcall) - 0xc)-8) >> 2;	op = ( 0xea << 24 ) | offset;	*((ulong *) 0xc) = op; 	/* Data Abort Exception */	offset = ((((ulong) _vdabcall) - 0x10)-8) >> 2;	op = ( 0xea << 24 ) | offset;	*((ulong *) 0x10) = op; 	/* IRQ Exception */ 	offset = ((((ulong) _virqcall) - 0x18)-8) >> 2;	op = ( 0xea << 24 ) | offset;	*((ulong *) 0x18) = op; 	/* FIQ Exception */ 	offset = ((((ulong) _vfiqcall) - 0x1c)-8) >> 2;	op = ( 0xea << 24 ) | offset;	*((ulong *) 0x1c) = op;  	flushIcache();	writeBackDC();	flushDcache();	flushIcache();	drainWBuffer(); 	splx(s);} /* *  trap is called splhi(). */ voidtrap(Ureg* ureg){	ushort mask;	IrqEntry *ip; 	/*	 * All interrupts/exceptions should be resumed at ureg->pc-4,	 * except for Data Abort which resumes at ureg->pc-8.	 */	ureg->pc -= 4; 	switch(ureg->type) {	case PsrMirq:				/* Interrupt Request */		mask = *(uchar*)HARI1 | ((*(uchar*)HARI2) << 8);		ip = Irq;		while (mask != 0) {			if(mask&1)				ip->r(ureg, ip->a);			ip++;			mask >>= 1;		}		break; 	case PsrMfiq:					/* FIQ */		mask = *(uchar*)HARI1 & HARI1_FIQ_MASK;		ip = Irq;		while (mask != 0) {			if(mask&1)				ip->r(ureg, ip->a);			ip++;			mask >>= 1;		}		break; 	case PsrMund:			/* Undefined instruction */		dumpregs(ureg);		panic("Undefined Instruction Exception\n");		break; 	case PsrMsvc:				/* Jump through 0 or SWI  */		dumpregs(ureg);		panic("SVC/SWI Exception\n");		break; 	case PsrMabt:					/* Prefetch abort */		ureg->pc -= 4;		/* FALLTHROUGH */ 	case PsrMabt+1:	{				/* Data abort */		uint far =0;		uint fsr =0; 		USED(far,fsr);		fsr = 0;	/*mmuregr(CpFSR);*/		far = 0;	/*mmuregr(CpFAR);	*/		if (ureg->type == PsrMabt)			print("Prefetch Abort/");		print("Data Abort\n");				print("Data Abort: FSR %8.8uX FAR %8.8uX\n", fsr, far);		/* FALLTHROUGH */	}	default:		dumpregs(ureg);		panic("exception %uX\n", ureg->type);		break;	} 	splhi();} voiddumpregs(Ureg* ureg){	print("PSR %8.8uX type %2.2uX PC %8.8uX LINK %8.8uX\n",		ureg->psr, ureg->type, ureg->pc, ureg->link);	print("R14 %8.8uX R13 %8.8uX R12 %8.8uX R11 %8.8uX R10 %8.8uX\n",		ureg->r14, ureg->r13, ureg->r12, ureg->r11, ureg->r10);	print("R9  %8.8uX R8  %8.8uX R7  %8.8uX R6  %8.8uX R5  %8.8uX\n",		ureg->r9, ureg->r8, ureg->r7, ureg->r6, ureg->r5);	print("R4  %8.8uX R3  %8.8uX R2  %8.8uX R1  %8.8uX R0  %8.8uX\n",		ureg->r4, ureg->r3, ureg->r2, ureg->r1, ureg->r0);	print("Stack is at: %8.8uX\n", ureg);/*	print("CPSR %8.8uX SPSR %8.8uX\n", cpsrr(), spsrr());*/}