update traps for pi2 (from richard)

Reference: /n/atom/patch/applied/pitrapupd
Date: Sun Jan  3 08:43:32 CET 2016
Signed-off-by: quanstro@quanstro.net

--- /sys/src/9/bcm/dma.c	Sun Jan  3 08:43:18 2016
+++ /sys/src/9/bcm/dma.c	Sun Jan  3 08:43:19 2016
@@ -168,7 +168,7 @@
 	ti = 0;
 	switch(dir){
 	case DmaD2M:
-		cachedwbinvse(dst, len);
+		cachedinvse(dst, len);
 		ti = Srcdreq | Destinc;
 		cb->sourcead = dmaioaddr(src);
 		cb->destad = dmaaddr(dst);
@@ -181,7 +181,7 @@
 		break;
 	case DmaM2M:
 		cachedwbse(src, len);
-		cachedwbinvse(dst, len);
+		cachedinvse(dst, len);
 		ti = Srcinc | Destinc;
 		cb->sourcead = dmaaddr(src);
 		cb->destad = dmaaddr(dst);
--- /sys/src/9/bcm/trap.c	Sun Jan  3 08:43:21 2016
+++ /sys/src/9/bcm/trap.c	Sun Jan  3 08:43:23 2016
@@ -13,13 +13,20 @@
 #include "arm.h"
 
 #define INTREGS		(VIRTIO+0xB200)
+#define	LOCALREGS	(VIRTIO+IOSIZE)
 
 typedef struct Intregs Intregs;
 typedef struct Vctl Vctl;
 
 enum {
+	Debug = 0,
+
 	Nvec = 8,		/* # of vectors at start of lexception.s */
 	Fiqenable = 1<<7,
+
+	Localtimerint	= 0x40,
+	Localmboxint	= 0x50,
+	Localintpending	= 0x60,
 };
 
 /*
@@ -45,15 +52,16 @@
 
 struct Vctl {
 	Vctl	*next;
-	int		irq;
+	int	irq;
+	int	cpu;
 	u32int	*reg;
 	u32int	mask;
 	void	(*f)(Ureg*, void*);
 	void	*a;
 };
 
-static Vctl *vctl;
-static Vctl *vfiq;
+static Lock vctllock;
+static Vctl *vctl, *vfiq;
 
 static char *trapnames[PsrMask+1] = {
 	[ PsrMusr ] "user mode",
@@ -76,14 +84,16 @@
 {
 	Vpage0 *vpage0;
 
-	/* disable everything */
-	intrsoff();
-
-	/* set up the exception vectors */
-	vpage0 = (Vpage0*)HVECTORS;
-	memmove(vpage0->vectors, vectors, sizeof(vpage0->vectors));
-	memmove(vpage0->vtable, vtable, sizeof(vpage0->vtable));
-	cacheuwbinv();
+	if (m->machno == 0) {
+		/* disable everything */
+		intrsoff();
+		/* set up the exception vectors */
+		vpage0 = (Vpage0*)HVECTORS;
+		memmove(vpage0->vectors, vectors, sizeof(vpage0->vectors));
+		memmove(vpage0->vtable, vtable, sizeof(vpage0->vtable));
+		cacheuwbinv();
+		l2cacheuwbinv();
+	}
 
 	/* set up the stacks for the interrupt modes */
 	setr13(PsrMfiq, (u32int*)(FIQSTKTOP));
@@ -96,6 +106,21 @@
 }
 
 void
+intrcpushutdown(void)
+{
+	u32int *enable;
+
+	if(soc.armlocal == 0)
+		return;
+	enable = (u32int*)(LOCALREGS + Localtimerint) + m->machno;
+	*enable = 0;
+	if(m->machno){
+		enable = (u32int*)(LOCALREGS + Localmboxint) + m->machno;
+		*enable = 1;
+	}
+}
+
+void
 intrsoff(void)
 {
 	Intregs *ip;
@@ -109,6 +134,30 @@
 	ip->FIQctl = 0;
 }
 
+/* called from cpu0 after other cpus are shutdown */
+void
+intrshutdown(void)
+{
+	intrsoff();
+	intrcpushutdown();
+}
+
+static void
+intrtime(void)
+{
+	ulong diff;
+	ulong x;
+
+	x = perfticks();
+	diff = x - m->perf.intrts;
+	m->perf.intrts = x;
+
+	m->perf.inintr += diff;
+	if(up == nil && m->perf.inidle > diff)
+		m->perf.inidle -= diff;
+}
+
+
 /*
  *  called by trap to handle irq interrupts.
  *  returns true iff a clock interrupt, thus maybe reschedule.
@@ -118,16 +167,23 @@
 {
 	Vctl *v;
 	int clockintr;
+	int found;
 
+	m->perf.intrts = perfticks();
 	clockintr = 0;
+	found = 0;
 	for(v = vctl; v; v = v->next)
-		if(*v->reg & v->mask){
+		if(v->cpu == m->machno && (*v->reg & v->mask) != 0){
+			found = 1;
 			coherence();
 			v->f(ureg, v->a);
 			coherence();
-			if(v->irq == IRQclock)
+			if(v->irq == IRQclock || v->irq == IRQcntps || v->irq == IRQcntpns)
 				clockintr = 1;
 		}
+	if(!found)
+		m->spuriousintr++;
+	intrtime();
 	return clockintr;
 }
 
@@ -139,14 +195,16 @@
 {
 	Vctl *v;
 
+	m->perf.intrts = perfticks();
 	v = vfiq;
 	if(v == nil)
-		panic("unexpected item in bagging area");
+		panic("cpu%d: unexpected item in bagging area", m->machno);
 	m->intr++;
 	ureg->pc -= 4;
 	coherence();
 	v->f(ureg, v->a);
 	coherence();
+	intrtime();
 }
 
 void
@@ -161,17 +219,24 @@
 	if(v == nil)
 		panic("irqenable: no mem");
 	v->irq = irq;
-	if(irq >= IRQbasic){
+	v->cpu = 0;
+	if(irq >= IRQlocal){
+		enable = (u32int*)(LOCALREGS + Localtimerint) + m->machno;
+		v->reg = (u32int*)(LOCALREGS + Localintpending) + m->machno;
+		v->mask = 1 << (irq - IRQlocal);
+		v->cpu = m->machno;
+	}else if(irq >= IRQbasic){
 		enable = &ip->ARMenable;
 		v->reg = &ip->ARMpending;
-		v->mask = 1<<(irq-IRQbasic);
+		v->mask = 1 << (irq - IRQbasic);
 	}else{
 		enable = &ip->GPUenable[irq/32];
 		v->reg = &ip->GPUpending[irq/32];
-		v->mask = 1<<(irq%32);
+		v->mask = 1 << (irq % 32);
 	}
 	v->f = f;
 	v->a = a;
+	lock(&vctllock);
 	if(irq == IRQfiq){
 		assert((ip->FIQctl & Fiqenable) == 0);
 		assert((*enable & v->mask) == 0);
@@ -180,8 +245,12 @@
 	}else{
 		v->next = vctl;
 		vctl = v;
-		*enable = v->mask;
+		if(irq >= IRQlocal)
+			*enable |= 1 << (irq - IRQlocal);
+		else
+			*enable = v->mask;
 	}
+	unlock(&vctllock);
 }
 
 static char *
@@ -195,35 +264,43 @@
 	return s;
 }
 
-/*
- *  called by trap to handle access faults
- */
+/* this is quite helpful during mmu and cache debugging */
 static void
-faultarm(Ureg *ureg, uintptr va, int user, int read)
+ckfaultstuck(uintptr va)
 {
-	int n, insyscall;
-	char buf[ERRMAX];
 	static int cnt, lastpid;
-	static ulong lastva;
+	static uintptr lastva;
 
-	if(up == nil) {
-		dumpregs(ureg);
-		panic("fault: nil up in faultarm, accessing %#p", va);
-	}
-	insyscall = up->insyscall;
-	up->insyscall = 1;
-	/* this is quite helpful during mmu and cache debugging */
-	if(va == lastva && up->pid == lastpid) {
+	if (va == lastva && up->pid == lastpid) {
 		++cnt;
 		if (cnt >= 2)
 			/* fault() isn't fixing the underlying cause */
-			panic("fault: %d consecutive faults for va %#lux",
+			panic("fault: %d consecutive faults for va %#p",
 				cnt+1, va);
 	} else {
 		cnt = 0;
 		lastva = va;
 		lastpid = up->pid;
 	}
+}
+
+/*
+ *  called by trap to handle access faults
+ */
+static void
+faultarm(Ureg *ureg, uintptr va, int user, int read)
+{
+	int n, insyscall;
+	char buf[ERRMAX];
+
+	if(up == nil) {
+		//dumpregs(ureg);
+		panic("fault: nil up in faultarm, pc %#p accessing %#p", ureg->pc, va);
+	}
+	insyscall = up->insyscall;
+	up->insyscall = 1;
+	if (Debug)
+		ckfaultstuck(va);
 
 	n = fault(va, read);
 	if(n < 0){
@@ -299,8 +376,8 @@
 	clockintr = 0;		/* if set, may call sched() before return */
 	switch(ureg->type){
 	default:
-		panic("unknown trap; type %#lux, psr mode %#lux", ureg->type,
-			ureg->psr & PsrMask);
+		panic("unknown trap; type %#lux, psr mode %#lux pc %lux", ureg->type,
+			ureg->psr & PsrMask, ureg->pc);
 		break;
 	case PsrMirq:
 		clockintr = irq(ureg);
@@ -515,13 +592,21 @@
  * Fill in enough of Ureg to get a stack trace, and call a function.
  * Used by debugging interface rdb.
  */
+
+static void
+getpcsp(ulong *pc, ulong *sp)
+{
+	*pc = getcallerpc(&pc);
+	*sp = (ulong)&pc-4;
+}
+
 void
 callwithureg(void (*fn)(Ureg*))
 {
 	Ureg ureg;
 
-	ureg.pc = getcallerpc(&fn);
-	ureg.sp = PTR2UINT(&fn);
+	getpcsp((ulong*)&ureg.pc, (ulong*)&ureg.sp);
+	ureg.r14 = getcallerpc(&fn);
 	fn(&ureg);
 }