diff options
| author | Thomas White <taw@physics.org> | 2018-06-23 17:50:16 +0200 |
|---|---|---|
| committer | Thomas White <taw@physics.org> | 2018-06-23 17:50:16 +0200 |
| commit | c4cdeef09cb36f0e7e9d84571a023f59b33b7d1c (patch) | |
| tree | 82f0f5c10f631d895bcd29e26cd208b0365a9941 /src/main.s | |
| parent | 07ddaabc49362f672e0d4f52f57220aac7af39cf (diff) | |
More tidying up, proper SWI handler
Diffstat (limited to 'src/main.s')
| -rw-r--r-- | src/main.s | 107 |
1 files changed, 30 insertions, 77 deletions
@@ -18,8 +18,6 @@ * */ -@ vim:ft=armv5 - .include "swi_numbers.h" .section .init @@ -56,7 +54,7 @@ reset: ERET /* Set up supervisor mode stack */ - LDR R13, =stack_svc + LDR R13, =stack_svc_cpu0 /* Relocate vector table */ MOV R10, #0x8000 @@ -66,95 +64,50 @@ reset: LDMIA R10!, {R0-R7} STMIA R11!, {R0-R7} - /* Set another CPU doing something */ - LDR R0, =flash_status_led - MOV R1, #0x40000000 - STR R0,[R1, #0xbc] - SEV - + /* Configure GPIOs */ LDR R8, =0x3f200000 LDR R1, [R8, #0x04] @ GPFSEL1 ORR R1, R1, #1<<18 @ GPIO pin 16 is output BIC R1, R1, #3<<21 @ GPIO pin 17 is input ORR R1, R1, #1<<24 @ GPIO pin 18 is output + ORR R1, R1, #1<<29 @ GPIO pin 29 is output STR R1, [R8, #0x04] @ GPFSEL1 - @ Flash green LED - MRC P15, 0, R0, C0, C0, 5 - AND R0, R0, #0x0f - CMP R0, #0 - BNE skip_flash - - MOV R1, #1<<18 - STR R1, [R8, #0x1c] @ GPSET0 - BL pause - MOV R1, #1<<18 - STR R1, [R8, #0x28] @ GPCLR0 - BL pause - -skip_flash: + /* Set another CPU doing something */ + LDR R0, =init_cpu3 + MOV R1, #0x40000000 + STR R0,[R1, #0xbc] + SEV - @ Switch to USR mode and set up stack + /* Switch to USR mode and set up stack */ MRS R0, CPSR BIC R0, R0, #0x1f ORR R0, R0, #0x10 MSR CPSR_c, R0 - LDR R13, =stack_usr - -repeat: - MOV R0, #0 - SWI 0 - TST R0, #1<<17 - MOVNE R0, #1 - SWINE 0 - B repeat - - -swi_handler: - STMDB R13!, {R8, LR} - LDR R8, =0x3f200000 - CMP R0, #0 - BEQ get_gpio - CMP R0, #1 - BLEQ flash - LDMIA R13!, {R8, PC}^ - -get_gpio: - LDR R0, [R8, #0x34] @ GPLEV0 - LDMIA R13!, {R8, PC}^ - - -flash: - STMDB R13!, {R1, R2, R4, LR} - MOV R4, #5 + LDR R13, =stack_usr_cpu0 -1: - MOV R1, #1<<16 - STR R1, [R8, #0x28] @ GPCLR0 - MOV R1, #1<<18 - STR R1, [R8, #0x1c] @ GPSET0 + /* Start user mode task */ + B flash_button - BL pause - MOV R1, #1<<18 - STR R1, [R8, #0x28] @ GPCLR0 - MOV R1, #1<<16 - STR R1, [R8, #0x1c] @ GPSET0 - - BL pause - - MOV R1, #1<<18 - ORR R1, #1<<16 - STR R1, [R8, #0x28] @ GPCLR0 +init_cpu3: + /* Out of HYP mode */ + MRS R0, CPSR + BIC R0, R0, #0x1f + ORR R0, R0, #0x13 + MSR SPSR_cxsf, R0 + ADD R0, PC, #4 + MSR ELR_hyp,r0 + ERET - SUBS R4, R4, #1 - BNE 1b + /* Set up supervisor mode stack */ + LDR R13, =stack_svc_cpu3 - LDMIA R13!, {R1, R2, R4, PC} + /* Switch to USR mode and set up stack */ + MRS R0, CPSR + BIC R0, R0, #0x1f + ORR R0, R0, #0x10 + MSR CPSR_c, R0 + LDR R13, =stack_usr_cpu3 -pause: - MOV R2, #0x3f000 -1: - SUBS R2, R2, #1 - BNE 1b - MOV PC, LR + B flash_status_led |