interrupts.S (3837B)
1 #include <p_modes.h> 2 #include <errno.h> 3 4 .section .text 5 .global vector_table_init 6 vector_table_init: 7 mrc p15, 0, r0, c1, c0, 0 @ read CP15 SCTRL register 8 bic r0, r0, #(1 << 13) @ set V flag to 0 (disable high vectors) 9 mcr p15, 0, r0, c1, c0, 0 @ write CP15 SCTRL register 10 11 ldr r0, =vector_table 12 mcr p15, 0, r0, c12, c0, 0 @ set vector base address (VBAR) 13 14 bx lr 15 16 .balign 32 @ the 5 least-significant bits of VBAR are reserved (i.e. 32 bytes alignment) 17 vector_table: 18 ldr pc, =reset 19 ldr pc, =undefined_insn 20 ldr pc, =swi_ex 21 ldr pc, =prefetch_abort 22 ldr pc, =data_abort 23 b . @ not assigned 24 ldr pc, =irq_ex 25 ldr pc, =fiq_ex 26 .ltorg 27 28 @@ save all the registers 29 .macro SAVE_ALL_EX 30 sub sp, sp, #4 @ make space for cpsr variable (cpsr of the previous mode) 31 stmfd sp!, { lr } @ save lr (pc (r15) of the previous mode) 32 sub sp, sp, #8 @ make space for sp (r13) and lr (r14) registers 33 stmfd sp!, { r0 - r12 } @ save r0 until r12 34 mrs r0, spsr @ read spsr (cpsr of the previous mode) 35 str r0, [sp, #(4 * 16)] @ save cpsr variable 36 bic r0, r0, #0x60 @ clear F, T flags 37 orr r0, r0, #(1 << 7) @ set I flag 38 mrs r2, cpsr @ backup current cpsr 39 msr cpsr_c, r0 @ switch to previous mode 40 mov r0, sp 41 mov r1, lr 42 msr cpsr, r2 @ swith back to the current mode 43 str r0, [sp, #(4 * 13)] @ save sp (r13) 44 str r1, [sp, #(4 * 14)] @ save lr (r14) 45 @@ restore destroyed registers 46 ldr r0, [sp] 47 ldr r1, [sp, #4] 48 ldr r2, [sp, #8] 49 .endm 50 51 @@ restore registers 52 .macro RESTORE_ALL_EX 53 ldr r0, [sp, #(4 * 13)] @ load sp (r13) 54 ldr r1, [sp, #(4 * 14)] @ load lr (r14) 55 ldr r2, [sp, #(4 * 16)] @ load cpsr variable 56 msr spsr, r2 @ restore spsr 57 bic r2, r2, #0x60 @ clear F, T flags 58 orr r2, r2, #(1 << 7) @ set I flag 59 mrs r3, cpsr @ backup current cpsr 60 msr cpsr_c, r2 @ switch to previous mode 61 mov sp, r0 62 mov lr, r1 63 msr cpsr, r3 @ swith back to the current mode 64 ldmfd sp!, { r0 - r12 } 65 add sp, sp, #8 @ release the space we had for sp (r13) and lr (r14) 66 ldr lr, [sp], #4 @ load lr (pc (r15) of the previous mode) 67 add sp, sp, #4 @ release the space we had for cpsr variable 68 .endm 69 70 reset: 71 b . 72 73 74 undefined_insn: 75 movs pc, lr 76 77 @@ SWI/SVC handler use Linux ARM EABI calling convention: 78 @@ r0 - r6 registers are used for the syscall arguments, it can take up to 7 arguments. 79 @@ r7 register is used for the syscall number. 80 swi_ex: 81 @@ save r8 - r12, lr registers 82 stmfd sp!, { r8 - r12, lr } 83 @@ save spsr register 84 mrs r8, spsr 85 str r8, [sp, #-4]! 86 87 @@ calculate the number of system calls 88 ldr r8, =syscall_table_end 89 ldr r8, [r8] 90 lsr r8, r8, #2 91 92 @@ if syscall number is out of syscall table boundaries then return -ENOSYS 93 cmp r7, r8 94 movcs r0, #-ENOSYS 95 bcs 2f 96 97 @@ prepare arguments and call the system call 98 stmfd sp!, { r4 - r6 } 99 ldr lr, =1f 100 ldr r8, =syscall_table 101 ldr pc, [r8, r7, lsl #2] 102 103 @@ return from SWI/SVC exception handler 104 1: 105 add sp, sp, #(4 * 3) 106 2: 107 ldr r8, [sp], #4 108 msr spsr, r8 109 ldmfd sp!, { r8 - r12, pc }^ 110 111 prefetch_abort: 112 @@ save registers 113 SAVE_ALL_EX 114 @@ set led d1 on 115 mov r0, #1 116 bl set_led_d1 117 @@ call handler 118 mov r0, sp @ pass a pointer to the registers 119 mov r1, #0 @ 0 = prefetch 120 bl abort_handler @ call handler 121 122 b . @ freeze 123 @subs pc, lr, #4 124 125 data_abort: 126 @@ save registers 127 SAVE_ALL_EX 128 @@ set led d2 on 129 mov r0, #1 130 bl set_led_d2 131 @@ call handler 132 mov r0, sp @ pass a pointer to the registers 133 mov r1, #1 @ 1 = data 134 bl abort_handler @ call handler 135 136 b . @ freeze 137 @subs pc, lr, #8 138 139 irq_ex: 140 @@ save registers 141 SAVE_ALL_EX 142 @@ call handler 143 mov r0, sp @ pass a pointer to the registers 144 bl irq_handler @ call irq_handler 145 @@ restore registers 146 RESTORE_ALL_EX 147 @@ exit exception 148 subs pc, lr, #4 149 150 fiq_ex: 151 sub lr, lr, #4 152 str lr, [sp, #-4]! 153 mov r0, #1 154 bl set_led_d2 155 ldmfd sp!, { pc }^