From 2b61ad947c64d76875311e6af08c87a5592055b3 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Fri, 26 Aug 2022 19:49:13 -0700
Subject: SpinLocks

---
 src/_arch/arm/cpu/boot.s | 136 +++++++++++++++++++++++++++++++----------------
 1 file changed, 91 insertions(+), 45 deletions(-)

(limited to 'src/_arch/arm/cpu/boot.s')

diff --git a/src/_arch/arm/cpu/boot.s b/src/_arch/arm/cpu/boot.s
index fd2a652..0151386 100644
--- a/src/_arch/arm/cpu/boot.s
+++ b/src/_arch/arm/cpu/boot.s
@@ -1,84 +1,130 @@
 .equ _core_id_mask, 0b11
 .section .text.boot
 
+.macro init_core coreid
+	// set vector address.
+	ldr r0, =vector
+	mcr p15, 0, r0, c12, c0, 0
+	cps #0x12 // Setup sp in IRQ mode.
+	ldr sp, =irq_stack_core\coreid
+	cps #0x11 // Setup sp in FIQ mode.
+	ldr sp, =fiq_stack_core\coreid
+	cps #0x1B // Setup sp in UNDEF mode.
+	ldr sp, =undefined_stack_core\coreid
+	cps #0x17 // Setup sp in ABORT mode.
+	ldr sp, =data_stack_core\coreid
+	cps #0x1f // Setup sp in USR/SYS mode.
+	ldr sp, =sys_stack_core\coreid
+	cps #0x13 // Setup sp in SVC mode.
+	ldr sp, =svc_stack_core\coreid
+.endm
+
+.macro core_stacks coreid
+	.space 4096
+undefined_stack_core\coreid:
+	.space 4096
+svc_stack_core\coreid:
+	.space 4096
+data_stack_core\coreid:
+	.space 4096
+irq_stack_core\coreid:
+	.space 4096
+fiq_stack_core\coreid:
+	.space 4096
+sys_stack_core\coreid:
+.endm
+
 .global _start
 _start:
 reset:
 	cpsid aif
+
+	// Exit Hypervisor Mode
+	mrs r0, cpsr
+	and r1, r0, #0x1F
+	cmp r1, #0x1A
+	bne 1f
+	bic r0, r0, #0x1f
+	orr r0, r0, #0x13
+	msr spsr_cxsf, r0
+	add r0, pc, #4
+	msr ELR_hyp, r0
+	eret
+
+1:
+	// disable core0,1,2.
 	mrc p15, #0, r1, c0, c0, #5
-	and r1, r1, #_core_id_mask
+	and r1, r1, #3
 	cmp r1, #1
-	beq core1run
+	beq runcore1
 	cmp r1, #2
-	beq core2run
+	beq runcore2
 	cmp r1, #3
-	beq core3run
+	bge runcore3
 
-	ldr r0, =vector
-	mcr p15, 0, r0, c12, c0, 0
-	cps #0x12
-	ldr sp, =core0_irq_stack
-	cps #0x11
-	ldr sp, =core0_fiq_stack
-	cps #0x1B
-	ldr sp, =core0_undefined_stack
-	cps #0x17
-	ldr sp, =core0_data_stack
-	cps #0x1f
-	ldr sp, =core0_sys_stack
-	cps #0x13
-	ldr sp, =core0_svc_stack
+	init_core 0
 
+	// Clear out bss.
 	ldr r4, =__bss_start
 	ldr r9, =__bss_end
 	mov r5, #0
 	mov r6, #0
 	mov r7, #0
 	mov r8, #0
-	b 2f
-1:
+	b       2f
+
+1:	// store multiple at r4.
 	stmia r4!, {{r5-r8}}
-2:
+
+2:	// If we are still below bss_end, loop.
 	cmp r4, r9
 	blo 1b
 
 	ldr r3, =_start_rust
 	blx r3
 
-core1run:
-core2run:
-core3run:
-.global io_halt
+runcore1:
+	init_core 1
+	mov r0, #1
+	b _start_other_core
+runcore2:
+	init_core 2
+	mov r0, #2
+	b _start_other_core
+runcore3:
+	init_core 3
+	mov r0, #3
+	b _start_other_core
 undefined:
+.global io_halt
 io_halt:
 	wfi
 	b io_halt
 
+
 .align 5
 vector:
 	ldr pc, reset_handler
 	ldr pc, undefined_handler
-	ldr pc, undefined_handler
-	ldr pc, undefined_handler
-	ldr pc, undefined_handler
-	ldr pc, undefined_handler
-	ldr pc, undefined_handler
-	ldr pc, undefined_handler
+	ldr pc, svc_handler
+	ldr pc, prefetch_handler
+	ldr pc, data_handler
+	ldr pc, unused_handler
+	ldr pc, irq_handler
+	ldr pc, fiq_handler
 
-reset_handler: .word reset
-undefined_handler: .word undefined
+reset_handler:      .word reset
+undefined_handler:  .word undefined
+svc_handler:        .word svc
+prefetch_handler:   .word prefetch
+data_handler:       .word data
+unused_handler:     .word io_halt
+irq_handler:        .word irq
+fiq_handler:        .word fiq
 
 .section .bss.sysstacks
 .align 4
-	.space 4096
-core0_undefined_stack:
-	.space 4096
-core0_svc_stack:
-	.space 4096
-core0_data_stack:
-	.space 4096
-core0_irq_stack:
-	.space 4096
-core0_fiq_stack:
-	.space 4096
-core0_sys_stack:
+core_stacks 0
+core_stacks 1
+core_stacks 2
+core_stacks 3
-- 
cgit v1.2.1