Fixed IRQ bug

Updated handlers

Started filling svc calls

1 files changed, 67 insertions, 30 deletions

diff --git a/src/boot.S b/src/boot.S
index 6bb24ae..da2d602 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -24,9 +24,32 @@ reset:
 	orr r1, r1, #0x12
 	msr cpsr_c,r1
 	mov sp,#0x4000
+	// setup sp in FIQ mode.
+	bic r1, r0, #0x1f
+	orr r1, r1, #0x11
+	msr cpsr_c,r1
+	mov sp,#0x2000
+	// setup sp in UNDEF mode.
+	bic r1, r0, #0x1f
+	orr r1, r1, #0x1B
+	msr cpsr_c,r1
+	mov sp,#0x1000
+	// setup sp in ABORT mode.
+	bic r1, r0, #0x1f
+	orr r1, r1, #0x17
+	msr cpsr_c,r1
+	mov sp,#0x0800
+	// Setup sp in USR/SYS mode.
+	bic r1, r0, #0x1f
+	orr r1, r1, #0x1f
+	msr cpsr_c,r1
+	mov sp,#0x6000
 
-	// restore cpsr.
-	msr cpsr_c, r0
+	// restore to SVC (in the case of a reset)
+	bic r1, r0, #0x1f
+	orr r1, r1, #0x13
+	msr cpsr_c, r1
+	//msr cpsr_c, r0
 
 	// setup the stack in SVC mode.
 	mov sp, #0x8000
@@ -55,9 +78,7 @@ reset:
 
 irq:
 	push {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr}
-	cpsid i
 	bl c_irq_handler
-	cpsie i
 	pop  {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr}
 	subs pc, lr, #4
 
@@ -66,47 +87,63 @@ io_halt:
 	wfi
 	b io_halt
 
-io_halt_undefined:
+// TODO: SETUP OTHER STACKS
+undefined:
 	stmfd sp!, {r0-r12,lr}
-	ldr r0, [lr, #-8]
+	ldr r0, [lr, #-4]
 	push {r0}
 	ldr r0, =undefined_msg
 	bl uart_string
 	pop {r0}
+	bl uart_hex
+	ldr r0, =undefined_at
+	bl uart_string
+	// Output lr
+	mov r1, #0x1000
+	ldr r0, [r1, #-4]
 	bl uart_hexn
 	// Skip instruction for now
 	// In future, 
-	//  ldmfd ... pc->lr
-	//  subs pc, lr, #8
+	//  ldmfd sp!, {r0-r12,lr} // Note the lack of ^ since subs will handle it
+	//  subs pc, lr, #4
 	ldmfd sp!, {r0-r12,pc}^
-io_halt_swi:
+svc:
 	stmfd sp!, {r0-r12,lr}
 	ldr r0, [lr, #-4]
 	bic r0, #0xFF000000
 	push {r0}
-	ldr r0, =swi_msg
+	ldr r0, =svc_msg
 	bl uart_string
-	pop {r0}
+	ldr r0, [sp]
 	bl uart_hexn
+	pop {r0}
+	// SVC #0 returns to supervisor mode
+	// TODO: Make supervisor mode return to a specific location
+	//  (rather than to a user location) such as the kernel loop
+	cmp r0, #0
+	bne 1f
+	mrs r0, spsr
+	bic r0, #0x1f
+	// Return in supervisor mode
+	orr r0, #0x13
+	msr spsr, r0
+1:
 	ldmfd sp!, {r0-r12,pc}^
 io_halt_prefetch:
+	stmfd sp!, {r0-r12,lr}
 	ldr r0, =prefetch_msg
-	push {lr}
 	bl uart_string
-	pop {lr}
-	b io_halt
+	ldmfd sp!, {r0-r12,pc}^
 io_halt_data:
+	stmfd sp!, {r0-r12,lr}
 	ldr r0, =data_msg
-	push {lr}
-	bl uart_string
-	pop {lr}
-	b io_halt
-io_halt_unused:
-	ldr r0, =unused_msg
-	push {lr}
 	bl uart_string
-	pop {lr}
-	b io_halt
+	// Output return address
+	mov r1, #0x800
+	ldr r0, [r1, #-4]
+	sub r0, #8
+	bl uart_hexn
+	ldmfd sp!, {r0-r12,pc}^
 io_halt_fiq:
 	ldr r0, =fiq_msg
 	push {lr}
@@ -118,7 +155,7 @@ io_halt_fiq:
 vector:
 	ldr pc, reset_handler
 	ldr pc, undefined_handler
-	ldr pc, swi_handler
+	ldr pc, svc_handler
 	ldr pc, prefetch_handler
 	ldr pc, data_handler
 	ldr pc, unused_handler
@@ -126,18 +163,18 @@ vector:
 	ldr pc, fiq_handler
 
 reset_handler:      .word reset
-undefined_handler:  .word io_halt_undefined
-swi_handler:        .word io_halt_swi
+undefined_handler:  .word undefined
+svc_handler:        .word svc
 prefetch_handler:   .word io_halt_prefetch
 data_handler:       .word io_halt_data
-unused_handler:     .word io_halt_unused
+unused_handler:     .word io_halt
 irq_handler:        .word irq
 fiq_handler:        .word io_halt_fiq
 
 .section .data
-undefined_msg:	.asciz "Undefined Handler\n"
-swi_msg:	.asciz "SWI Handler\n"
+undefined_msg:	.asciz "Undefined Handler\nUNDEF "
+undefined_at:	.asciz " @ 0x"
+svc_msg:	.asciz "SVC Handler\nSVC #"
 prefetch_msg:	.asciz "Prefetch Handler\n"
 data_msg:	.asciz "Data Handler\n"
-unused_msg:	.asciz "Unused Handler\n"
 fiq_msg:	.asciz "FIQ\n"