63,92 → 63,58 |
* 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-r3}\n" |
"mrs r1, cpsr\n" |
"bic r1, r1, #0x1f\n" |
"mrs r2, spsr\n" |
"and r0, r2, #0x1f\n" |
"stmfd r13!, {r0}\n" |
"mrs r0, spsr\n" |
"and r0, r0, #0x1f\n" |
"cmp r0, #0x10\n" |
"bne 1f\n" |
|
/* prev mode was usermode */ |
"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] */ |
"ldmfd r13!, {r0}\n" |
"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!, {r4-r12}\n" |
"ldmfd r0!, {r4-r7}\n" |
"stmfd r13!, {r4-r7}\n" |
"stmfd r13!, {r0-r12}\n" |
"stmfd r13!, {r13, lr}^\n" |
"stmfd r13!, {r2}\n" |
"mrs r0, spsr\n" |
"stmfd r13!, {r0}\n" |
"b 2f\n" |
|
|
|
/* mode was not usermode */ |
"1:\n" |
/* Switch to previous mode which is undoubtedly the supervisor mode */ |
"stmfd r13!, {r1, r2, r3}\n" |
"mrs r1, cpsr\n" |
"mov r2, lr\n" |
"bic r1, r1, #0x1f\n" |
"orr r1, r1, r0\n" |
"mov r0, lr\n" |
"mov r3, sp\n" |
"mrs r0, cpsr\n" |
"msr cpsr_c, r1\n" |
|
/* Populate the stack frame */ |
"mov r1, sp\n" |
"stmfd r13!, {r0}\n" |
|
"mov r3, r13\n" |
"stmfd r13!, {r2}\n" |
"mov r2, lr\n" |
"stmfd r13!, {r4-r12}\n" |
"mov r1, r13\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" |
|
/* 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" |
|
"2:\n" |
); |
} |
180,6 → 146,7 |
"mrs r0, cpsr \n" |
"msr cpsr_c, r3 \n" |
|
"mov r13, r1 \n" |
"mov lr, r2 \n" |
"msr cpsr_c, r0 \n" |
|
190,6 → 157,39 |
} |
|
|
/** 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,7 → 288,13 |
"sub lr, lr, #4" |
); |
|
PROCESS_EXCEPTION(EXC_IRQ) |
setup_stack_and_save_regs(); |
|
switch_to_irq_servicing_mode(); |
|
CALL_EXC_DISPATCH(EXC_IRQ) |
|
load_regs(); |
} |
|
/** Software Interrupt handler. |