from richard: update the pi reboot code for pi2

Reference: /n/atom/patch/applied/pireboot
Date: Sun Jan  3 20:33:37 CET 2016
Signed-off-by: quanstro@quanstro.net

--- /sys/src/9/bcm/rebootcode.s	Sun Jan  3 20:33:18 2016
+++ /sys/src/9/bcm/rebootcode.s	Sun Jan  3 20:33:19 2016
@@ -1,8 +1,13 @@
 /*
- * armv6 reboot code
+ * armv6/armv7 reboot code
  */
 #include "arm.s"
 
+#define PTEDRAM		(Dom0|L1AP(Krw)|Section)
+
+#define WFI	WORD	$0xe320f003	/* wait for interrupt */
+#define WFE	WORD	$0xe320f002	/* wait for event */
+
 /*
  * Turn off MMU, then copy the new kernel to its correct location
  * in physical memory.  Then jump to the start of the kernel.
@@ -15,7 +20,7 @@
 	/* copy in arguments before stack gets unmapped */
 	MOVW	R0, R8			/* entry point */
 	MOVW	p2+4(FP), R9		/* source */
-	MOVW	n+8(FP), R10		/* byte count */
+	MOVW	n+8(FP), R6		/* byte count */
 
 	/* SVC mode, interrupts disabled */
 	MOVW	$(PsrDirq|PsrDfiq|PsrMsvc), R1
@@ -29,6 +34,28 @@
 	BIC	$CpCmmu, R1
 	MCR	CpSC, 0, R1, C(CpCONTROL), C(0), CpMainctl
 
+	/* continue with reboot only on cpu0 */
+	CPUID(R2)
+	BEQ	bootcpu
+
+	/* other cpus wait for inter processor interrupt from cpu0 */
+	/* turn icache back on */
+	MRC	CpSC, 0, R1, C(CpCONTROL), C(0), CpMainctl
+	ORR	$(CpCicache), R1
+	MCR	CpSC, 0, R1, C(CpCONTROL), C(0), CpMainctl
+	BARRIERS
+dowfi:
+	WFI
+	MOVW	$0x40000060, R1
+	ADD		R2<<2, R1
+	MOVW	0(R1), R0
+	AND		$0x10, R0
+	BEQ		dowfi
+	MOVW	$0x8000, R1
+	BL		(R1)
+	B		dowfi
+
+bootcpu:
 	/* set up a tiny stack for local vars and memmove args */
 	MOVW	R8, SP			/* stack top just before kernel dest */
 	SUB	$20, SP			/* allocate stack frame */
@@ -37,11 +64,12 @@
 	MOVW	R8, 16(SP)		/* save dest (entry point) */
 	MOVW	R8, R0			/* first arg is dest */
 	MOVW	R9, 8(SP)		/* push src */
-	MOVW	R10, 12(SP)		/* push size */
+	MOVW	R6, 12(SP)		/* push size */
 	BL	memmove(SB)
 	MOVW	16(SP), R8		/* restore entry point */
 
 	/* jump to kernel physical entry point */
+	ORR	R8,R8
 	B	(R8)
 	B	0(PC)
 
@@ -51,43 +79,38 @@
  * clobbers R0-R2, and returns with SP invalid.
  */
 TEXT cachesoff(SB), 1, $-4
+	MOVM.DB.W [R14,R1-R10], (R13)		/* save regs on stack */
 
-	/* write back and invalidate caches */
-	BARRIERS
-	MOVW	$0, R0
-	MCR	CpSC, 0, R0, C(CpCACHE), C(CpCACHEwbi), CpCACHEall
-	MCR	CpSC, 0, R0, C(CpCACHE), C(CpCACHEinvi), CpCACHEall
-
-	/* turn caches off */
+	/* turn caches off, invalidate icache */
 	MRC	CpSC, 0, R1, C(CpCONTROL), C(0), CpMainctl
 	BIC	$(CpCdcache|CpCicache|CpCpredict), R1
 	MCR	CpSC, 0, R1, C(CpCONTROL), C(0), CpMainctl
+	MOVW	$0, R0
+	MCR	CpSC, 0, R0, C(CpCACHE), C(CpCACHEinvi), CpCACHEall
 
 	/* invalidate stale TLBs before changing them */
 	BARRIERS
-	MOVW	$KZERO, R0			/* some valid virtual address */
+	MOVW	$0, R0
 	MCR	CpSC, 0, R0, C(CpTLB), C(CpTLBinvu), CpTLBinv
 	BARRIERS
 
-	/* from here on, R0 is base of physical memory */
-	MOVW	$PHYSDRAM, R0
-
 	/* redo double map of first MiB PHYSDRAM = KZERO */
-	MOVW	$(L1+L1X(PHYSDRAM)), R2		/* address of PHYSDRAM's PTE */
+	MOVW	12(R(MACH)), R2		/* m->mmul1 (virtual addr) */
 	MOVW	$PTEDRAM, R1			/* PTE bits */
-	ORR	R0, R1				/* dram base */
 	MOVW	R1, (R2)
 
 	/* invalidate stale TLBs again */
 	BARRIERS
+	MOVW	$0, R0
 	MCR	CpSC, 0, R0, C(CpTLB), C(CpTLBinvu), CpTLBinv
 	BARRIERS
 
 	/* relocate SB and return address to PHYSDRAM addressing */
 	MOVW	$KSEGM, R1		/* clear segment bits */
 	BIC	R1, R12			/* adjust SB */
-	ORR	R0, R12
+	MOVM.IA.W (R13), [R14,R1-R10]		/* restore regs from stack */
+
+	MOVW	$KSEGM, R1		/* clear segment bits */
 	BIC	R1, R14			/* adjust return address */
-	ORR	R0, R14
 
 	RET
--- /sys/src/9/bcm/arm.s	Sun Jan  3 20:33:21 2016
+++ /sys/src/9/bcm/arm.s	Sun Jan  3 20:33:22 2016
@@ -11,8 +11,6 @@
 
 #define L1X(va)		(((((va))>>20) & 0x0fff)<<2)
 
-#define PTEDRAM		(Dom0|L1AP(Krw)|Section|Cached|Buffered)
-
 /*
  * new instructions
  */
--- /sys/src/9/bcm/fns.h	Sun Jan  3 20:33:24 2016
+++ /sys/src/9/bcm/fns.h	Sun Jan  3 20:33:25 2016
@@ -6,7 +6,9 @@
 extern void archreboot(void);
 extern void archreset(void);
 extern void armtimerset(int);
+extern void cachedwb(void);
 extern void cachedwbinv(void);
+extern void cachedinvse(void*, int);
 extern void cachedwbse(void*, int);
 extern void cachedwbinvse(void*, int);
 extern void cacheiinv(void);
@@ -45,6 +47,7 @@
 extern char* getconf(char*);
 extern char *getethermac(void);
 extern uint getfirmware(void);
+extern int getncpus(void);
 extern int getpower(int);
 extern void getramsize(Confmem*);
 extern uint gettemp(int);
@@ -53,7 +56,7 @@
 #define intrenable(i, f, a, b, n) irqenable((i), (f), (a))
 extern void intrsoff(void);
 extern int l2ap(int);
-extern void l2cachewbinv(void);
+extern void l2cacheuwbinv(void);
 extern void links(void);
 extern void mmuinit(void*);
 extern void mmuinit1(void*);
@@ -66,12 +69,15 @@
 extern void procsetup(Proc*);
 extern void screeninit(void);
 extern void setpower(int, int);
+extern void setclkrate(int, ulong);
 extern void setr13(int, u32int*);
 extern int splfhi(void);
 extern int splflo(void);
 extern void swcursorinit(void);
 extern void syscallfmt(int syscallno, ulong pc, va_list list);
 extern int tas(void *);
+extern int startcpus(uint);
+extern void stopcpu(uint);
 extern void touser(uintptr);
 extern void trapinit(void);
 extern void uartconsinit(void);