From c4cdeef09cb36f0e7e9d84571a023f59b33b7d1c Mon Sep 17 00:00:00 2001
From: Thomas White <taw@physics.org>
Date: Sat, 23 Jun 2018 17:50:16 +0200
Subject: More tidying up, proper SWI handler

---
 src/main.s | 107 +++++++++++++++++--------------------------------------------
 1 file changed, 30 insertions(+), 77 deletions(-)

(limited to 'src/main.s')

diff --git a/src/main.s b/src/main.s
index b981b2a..40f3ff0 100644
--- a/src/main.s
+++ b/src/main.s
@@ -18,8 +18,6 @@
  *
  */
 
-@ vim:ft=armv5
-
 .include "swi_numbers.h"
 
 .section .init
@@ -56,7 +54,7 @@ reset:
 	ERET
 
 	/* Set up supervisor mode stack */
-	LDR	R13, =stack_svc
+	LDR	R13, =stack_svc_cpu0
 
 	/* Relocate vector table */
 	MOV	R10, #0x8000
@@ -66,95 +64,50 @@ reset:
 	LDMIA	R10!, {R0-R7}
 	STMIA	R11!, {R0-R7}
 
-	/* Set another CPU doing something */
-	LDR R0, =flash_status_led
-	MOV R1, #0x40000000
-	STR R0,[R1, #0xbc]
-	SEV
-
+	/* Configure GPIOs */
 	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
+	ORR	R1, R1, #1<<29		@ GPIO pin 29 is output
 	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
-	MOV	R1, #1<<18
-	STR	R1, [R8, #0x28]		@ GPCLR0
-	BL	pause
-
-skip_flash:
+	/* Set another CPU doing something */
+	LDR	R0, =init_cpu3
+	MOV	R1, #0x40000000
+	STR	R0,[R1, #0xbc]
+	SEV
 
-	@ Switch to USR mode and set up stack
+	/* Switch to USR mode and set up stack */
 	MRS	R0, CPSR
 	BIC	R0, R0, #0x1f
 	ORR	R0, R0, #0x10
 	MSR	CPSR_c, R0
-	LDR	R13, =stack_usr
-
-repeat:
-	MOV	R0, #0
-	SWI	0
-	TST	R0, #1<<17
-	MOVNE	R0, #1
-	SWINE	0
-	B	repeat
-
-
-swi_handler:
-	STMDB	R13!, {R8, LR}
-	LDR	R8, =0x3f200000
-	CMP	R0, #0
-	BEQ	get_gpio
-	CMP	R0, #1
-	BLEQ	flash
-	LDMIA	R13!, {R8, PC}^
-
-get_gpio:
-	LDR	R0, [R8, #0x34]		@ GPLEV0
-	LDMIA	R13!, {R8, PC}^
-
-
-flash:
-	STMDB	R13!, {R1, R2, R4, LR}
-	MOV	R4, #5
+	LDR	R13, =stack_usr_cpu0
 
-1:
-	MOV	R1, #1<<16
-	STR	R1, [R8, #0x28]		@ GPCLR0
-	MOV	R1, #1<<18
-	STR	R1, [R8, #0x1c]		@ GPSET0
+	/* Start user mode task */
+	B	flash_button
 
-	BL	pause
 
-	MOV	R1, #1<<18
-	STR	R1, [R8, #0x28]		@ GPCLR0
-	MOV	R1, #1<<16
-	STR	R1, [R8, #0x1c]		@ GPSET0
-
-	BL	pause
-
-	MOV	R1, #1<<18
-	ORR	R1, #1<<16
-	STR	R1, [R8, #0x28]		@ GPCLR0
+init_cpu3:
+	/* Out of HYP mode */
+	MRS	R0, CPSR
+	BIC	R0, R0, #0x1f
+	ORR	R0, R0, #0x13
+	MSR	SPSR_cxsf, R0
+	ADD	R0, PC, #4
+	MSR	ELR_hyp,r0
+	ERET
 
-	SUBS	R4, R4, #1
-	BNE	1b
+	/* Set up supervisor mode stack */
+	LDR	R13, =stack_svc_cpu3
 
-	LDMIA	R13!, {R1, R2, R4, PC}
+	/* Switch to USR mode and set up stack */
+	MRS	R0, CPSR
+	BIC	R0, R0, #0x1f
+	ORR	R0, R0, #0x10
+	MSR	CPSR_c, R0
+	LDR	R13, =stack_usr_cpu3
 
-pause:
-	MOV	R2, #0x3f000
-1:
-	SUBS	R2, R2, #1
-	BNE	1b
-	MOV	PC, LR
+	B	flash_status_led
-- 
cgit v1.2.3