SpinLocks

2 files changed, 142 insertions, 45 deletions

diff --git a/src/_arch/arm/cpu/boot.rs b/src/_arch/arm/cpu/boot.rs
index b81a16a..1b4ed74 100644
--- a/src/_arch/arm/cpu/boot.rs
+++ b/src/_arch/arm/cpu/boot.rs
@@ -18,3 +18,54 @@ global_asm!(include_str!("boot.s"));
 pub unsafe fn _start_rust() -> ! {
     crate::kernel_init()
 }
+
+/// # Rust entry for other cores of the `kernel` binary.
+///
+/// This function is unmangled so that the
+/// ASM boot code can switch to Rust safely.
+#[no_mangle]
+pub extern "C" fn _start_other_core(_core: u32) -> ! {
+    loop {
+        use crate::INITIALIZED_BOOL;
+        use core::sync::atomic::Ordering;
+        if let Ok(true) =
+            INITIALIZED_BOOL.compare_exchange(true, false, Ordering::Acquire, Ordering::Relaxed)
+        {
+            crate::serial_println!("Ran core {}!", _core);
+            let u = crate::Box::<u32>::new(42);
+            crate::serial_println!("{}", u);
+            INITIALIZED_BOOL.store(true, Ordering::Release);
+            break;
+        }
+    }
+    #[allow(unreachable_code)]
+    loop {}
+}
+
+/// # Prefetch
+#[no_mangle]
+pub extern "C" fn prefetch() -> ! {
+    crate::serial_println!("Prefetch handler");
+    loop {}
+}
+
+/// # Data
+#[no_mangle]
+pub extern "C" fn data() -> ! {
+    crate::serial_println!("Data handler");
+    loop {}
+}
+
+/// # IRQ
+#[no_mangle]
+pub extern "C" fn irq() -> ! {
+    crate::serial_println!("IRQ handler");
+    loop {}
+}
+
+/// # FIQ
+#[no_mangle]
+pub extern "C" fn fiq() -> ! {
+    crate::serial_println!("FIQ handler");
+    loop {}
+}
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