X-Git-Url: http://git.mutantstargoat.com/user/nuclear/?p=rpikern;a=blobdiff_plain;f=src%2Fstartup.s;h=84e7e94d0b4037cec5229b228900e0f530504cec;hp=7765ede46d9a56f27b3ae9b3436d6e654f5080fb;hb=38a008b8d1a3a20c401397b4e132fb578e5f0c10;hpb=e848811902a51b67dedb6f6325dda19a6f21e820

diff --git a/src/startup.s b/src/startup.s
index 7765ede..84e7e94 100644
--- a/src/startup.s
+++ b/src/startup.s
@@ -3,14 +3,37 @@
 	.section .startup
 	.code 32
 
+	.global startup
 startup:
+	cpsid if
+
 	@ stop all but one of the cores
 	mrc p15, 0, r0, c0, c0, 5
 	ands r0, r0, #0xff
 	bne exit
 
-	@ setup stack
-	ldr sp, =_stacktop
+	@ detect if we're running in hyp mode, and drop to svc
+	mrs r0, cpsr
+	and r1, r0, #0x1f
+	cmp r1, #0x1a
+	bne hypend
+
+	bic r0, #0x1f
+	orr r0, #0x13
+	@msr spsr_cxsf, r0
+	add r0, pc, #4
+	msr elr_hyp, r0
+	@eret
+	mov pc, r0
+hypend:
+
+	@ setup initial stacks, allow 4k stack for IRQs
+	@mov r0, #0x12	@ switch to IRQ mode
+	@msr cpsr, r0
+	@ldr sp, =_stacktop
+	@mov r0, #0x13	@ switch to supervisor mode
+	@msr cpsr, r0
+	ldr sp, =_stacktop - 4096
 
 	@ clear bss
 	ldr r0, =_bss_start
@@ -28,19 +51,4 @@ startup:
 exit:	wfe
 	b exit
 
-	.global dbgled
-dbgled:
-	ldr r3, =0x3f200000	@ gpio base
-	ldr r2, =0x9000		@ gpio 24 and 25 -> output
-	str r2, [r3, #8]	@ store to GPFSEL2
-	ldr r2, =0x01000000	@ bit 24
-	tst r0, #1
-	strne r2, [r3, #0x1c]	@ GPSET0
-	streq r2, [r3, #0x28]	@ GPCLR0
-	lsl r2, #1
-	tst r0, #2
-	strne r2, [r3, #0x1c]	@ GPSET0
-	streq r2, [r3, #0x28]	@ GPCLR0
-	bx lr
-
 @ vi:set filetype=armasm: