.equ _core_id_mask, 0b11
.section .text.boot

.global _start
_start:
reset:
	cpsid aif
	mrc p15, #0, r1, c0, c0, #5
	and r1, r1, #_core_id_mask
	cmp r1, #1
	beq core1run
	cmp r1, #2
	beq core2run
	cmp r1, #3
	beq core3run

	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

	ldr r4, =__bss_start
	ldr r9, =__bss_end
	mov r5, #0
	mov r6, #0
	mov r7, #0
	mov r8, #0
	b 2f
1:
	stmia r4!, {{r5-r8}}
2:
	cmp r4, r9
	blo 1b

	ldr r3, =_start_rust
	blx r3

core1run:
core2run:
core3run:
.global io_halt
undefined:
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

reset_handler: .word reset
undefined_handler: .word undefined

.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: