diff options
| -rw-r--r-- | src/boot.S | 97 |
1 files changed, 67 insertions, 30 deletions
@@ -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" |