From 9ca41f57fbbc1bc69a3d8751218cd85ef93680d6 Mon Sep 17 00:00:00 2001
From: Thomas White <taw@physics.org>
Date: Tue, 19 Jun 2018 22:43:10 +0200
Subject: Do something with multiple CPUs

---
 src/main.s | 49 ++++++++++++++++++++++++++++++++++++++++++-------
 1 file changed, 42 insertions(+), 7 deletions(-)

diff --git a/src/main.s b/src/main.s
index e45da64..a9f7af3 100644
--- a/src/main.s
+++ b/src/main.s
@@ -25,13 +25,6 @@ hang:
 	B	hang
 
 reset:
-
-	@ Hang all except one of the processors
-	MRC	P15, 0, R0, C0, C0, 5
-	AND	R0, R0, #3
-	CMP	R0, #0
-	BNE	hang
-
 	@ Get out of HYP mode
 	MRS	R0, CPSR
 	BIC	R0, R0, #0x1f
@@ -51,6 +44,11 @@ reset:
 	LDMIA	R10!, {R0-R7}
 	STMIA	R11!, {R0-R7}
 
+	LDR R0, =start_cpu1
+	MOV R1, #0x40000000
+	STR R0,[R1, #0xac]
+	SEV
+
 	LDR	R8, =0x3f200000
 	LDR	R1, [R8, #0x04]		@ GPFSEL1
 	ORR	R1, R1, #1<<18		@ GPIO pin 16 is output
@@ -59,6 +57,11 @@ reset:
 	STR	R1, [R8, #0x04]		@ GPFSEL1
 
 	@ Flash green LED
+	MRC	P15, 0, R0, C0, C0, 5
+	AND	R0, R0, #0x0f
+	CMP	R0, #0
+	BNE	skip_flash
+
 	MOV	R1, #1<<18
 	STR	R1, [R8, #0x1c]		@ GPSET0
 	BL	pause
@@ -66,6 +69,8 @@ reset:
 	STR	R1, [R8, #0x28]		@ GPCLR0
 	BL	pause
 
+skip_flash:
+
 	@ Switch to USR mode and set up stack
 	MRS	R0, CPSR
 	BIC	R0, R0, #0x1f
@@ -82,6 +87,32 @@ repeat:
 	B	repeat
 
 
+start_cpu1:
+	LDR	R8, =0x3f200000
+	LDR	R1, [R8, #0x04]		@ GPFSEL1
+	ORR	R1, R1, #1<<18		@ GPIO pin 16 is output
+	BIC	R1, R1, #3<<21		@ GPIO pin 17 is input
+	ORR	R1, R1, #1<<24		@ GPIO pin 18 is output
+	STR	R1, [R8, #0x04]		@ GPFSEL1
+
+forever:
+	MRC	P15, 0, R0, C0, C0, 5
+	AND	R0, R0, #0x0f
+flashred:
+	MOV	R1, #1<<29
+	STR	R1, [R8, #0x1c]		@ GPSET0
+	BL	longpause
+	MOV	R1, #1<<29
+	STR	R1, [R8, #0x28]		@ GPCLR0
+	BL	longpause
+	SUBS	R0, R0, #1
+	BNE	flashred
+
+	BL	longpause
+	BL	longpause
+	BL	longpause
+	B	forever
+
 swi_handler:
 	STMDB	R13!, {R8, LR}
 	LDR	R8, =0x3f200000
@@ -130,3 +161,7 @@ wait:
 	SUBS	R2, R2, #1
 	BNE	wait
 	MOV	PC, LR
+
+longpause:
+	MOV	R2, #0x3f0000
+	B	wait
-- 
cgit v1.2.3