diff options
Diffstat (limited to 'os/boot/puma/trap.c')
| -rw-r--r-- | os/boot/puma/trap.c | 190 |
1 files changed, 190 insertions, 0 deletions
diff --git a/os/boot/puma/trap.c b/os/boot/puma/trap.c new file mode 100644 index 00000000..6322e5c7 --- /dev/null +++ b/os/boot/puma/trap.c @@ -0,0 +1,190 @@ +#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); + +void +setvec(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 void +safeintr(Ureg*, void *a) +{ + int v = (int)a; +// print("spurious interrupt %d\n", v); + USED(v); +} + +void +trapinit(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(). + */ + +void +trap(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(); +} + +void +dumpregs(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());*/ +} |