| 63,58 → 63,92 |
|---|
| * Temporary exception stack is used to save a few registers |
| * before stack switch takes place. |
| * |
| * The stack fram created by the function looks like: |
| * |
| * |_________________| |
| * | | |
| * | SPSR | |
| * | | |
| * |_________________| |
| * | Stack Pointer | |
| * | of | |
| * | Previous Mode | |
| * |_________________| |
| * | Return address | |
| * | of | |
| * | Previous Mode | |
| * |_________________| |
| * | R0 - R12 | |
| * | of | |
| * | Previous Mode | |
| * |_________________| |
| * | Return address | |
| * | from | |
| * |Exception Handler| |
| * |_________________| |
| * | | |
| * |
| */ |
| inline static void setup_stack_and_save_regs() |
| { |
| asm volatile ( |
| "ldr r13, =exc_stack\n" |
| "stmfd r13!, {r0}\n" |
| "mrs r0, spsr\n" |
| "and r0, r0, #0x1f\n" |
| "stmfd r13!, {r0-r3}\n" |
| "mrs r1, cpsr\n" |
| "bic r1, r1, #0x1f\n" |
| "mrs r2, spsr\n" |
| "and r0, r2, #0x1f\n" |
| "cmp r0, #0x10\n" |
| "bne 1f\n" |
| |
| /* prev mode was usermode */ |
| "ldmfd r13!, {r0}\n" |
| "mov r0, sp\n" |
| "mov r3, lr\n" |
| |
| /* Switch to supervisor mode */ |
| "orr r1, r1, #0x13\n" |
| "msr cpsr_c, r1\n" |
| |
| /* Load sp with [supervisor_sp] */ |
| "ldr r13, =supervisor_sp\n" |
| "ldr r13, [r13]\n" |
| |
| /* Populate the stack frame */ |
| "msr spsr, r2\n" |
| "mov lr, r3\n" |
| "stmfd r13!, {lr}\n" |
| "stmfd r13!, {r0-r12}\n" |
| "stmfd r13!, {r4-r12}\n" |
| "ldmfd r0!, {r4-r7}\n" |
| "stmfd r13!, {r4-r7}\n" |
| "stmfd r13!, {r13, lr}^\n" |
| "mrs r0, spsr\n" |
| "stmfd r13!, {r0}\n" |
| "stmfd r13!, {r2}\n" |
| "b 2f\n" |
| |
| |
| |
| /* mode was not usermode */ |
| "1:\n" |
| "stmfd r13!, {r1, r2, r3}\n" |
| "mrs r1, cpsr\n" |
| "mov r2, lr\n" |
| "bic r1, r1, #0x1f\n" |
| /* Switch to previous mode which is undoubtedly the supervisor mode */ |
| "orr r1, r1, r0\n" |
| "mrs r0, cpsr\n" |
| "mov r0, lr\n" |
| "mov r3, sp\n" |
| "msr cpsr_c, r1\n" |
| |
| "mov r3, r13\n" |
| "stmfd r13!, {r2}\n" |
| "mov r2, lr\n" |
| |
| /* Populate the stack frame */ |
| "mov r1, sp\n" |
| "stmfd r13!, {r0}\n" |
| "stmfd r13!, {r4-r12}\n" |
| "mov r1, r13\n" |
| |
| /* the following two lines are for debugging */ |
| "mov sp, #0\n" |
| "mov lr, #0\n" |
| "msr cpsr_c, r0\n" |
| |
| "ldmfd r13!, {r4, r5, r6, r7}\n" |
| "stmfd r1!, {r4, r5, r6}\n" |
| "stmfd r1!, {r7}\n" |
| "stmfd r1!, {r2}\n" |
| "stmfd r1!, {r3}\n" |
| "mrs r0, spsr\n" |
| "stmfd r1!, {r0}\n" |
| "mov r13, r1\n" |
| |
| /* Store r0-r3 in r4-r7 and then push it on to stack */ |
| "ldmfd r3!, {r4-r7}\n" |
| "stmfd r13!, {r4-r7}\n" |
| |
| /* Push return address and stack pointer on to stack */ |
| "stmfd r13!, {lr}\n" |
| "stmfd r13!, {r1}\n" |
| "mov lr, r0\n" |
| "msr spsr, r2\n" |
| "stmfd r13!, {r2}\n" |
| |
| "2:\n" |
| ); |
| } |
| 146,7 → 180,6 |
|---|
| "mrs r0, cpsr \n" |
| "msr cpsr_c, r3 \n" |
| |
| "mov r13, r1 \n" |
| "mov lr, r2 \n" |
| "msr cpsr_c, r0 \n" |
| |
| 157,39 → 190,6 |
|---|
| } |
| |
| |
| /** Switch CPU to mode in which interrupts are serviced (currently it |
| * is Undefined mode). |
| * |
| * The default mode for interrupt servicing (Interrupt Mode) |
| * can not be used because of nested interrupts (which can occur |
| * because interrupts are enabled in higher levels of interrupt handler). |
| */ |
| inline static void switch_to_irq_servicing_mode() |
| { |
| /* switch to Undefined mode */ |
| asm volatile( |
| /* save regs used during switching */ |
| "stmfd sp!, {r0-r3} \n" |
| |
| /* save stack pointer and link register to r1, r2 */ |
| "mov r1, sp \n" |
| "mov r2, lr \n" |
| |
| /* mode switch */ |
| "mrs r0, cpsr \n" |
| "bic r0, r0, #0x1f \n" |
| "orr r0, r0, #0x1b \n" |
| "msr cpsr_c, r0 \n" |
| |
| /* restore saved sp and lr */ |
| "mov sp, r1 \n" |
| "mov lr, r2 \n" |
| |
| /* restore original regs */ |
| "ldmfd sp!, {r0-r3} \n" |
| ); |
| } |
| |
| /** Calls exception dispatch routine. */ |
| #define CALL_EXC_DISPATCH(exception) \ |
| asm volatile ( \ |
| 288,13 → 288,7 |
|---|
| "sub lr, lr, #4" |
| ); |
| |
| setup_stack_and_save_regs(); |
| |
| switch_to_irq_servicing_mode(); |
| |
| CALL_EXC_DISPATCH(EXC_IRQ) |
| |
| load_regs(); |
| PROCESS_EXCEPTION(EXC_IRQ) |
| } |
| |
| /** Software Interrupt handler. |