59,6 → 59,25 |
|
inline static void setup_stack_and_save_regs() |
{ |
/* |
|
str r0, =exc_stack |
str r1, =exc_stack + 4 |
mrs r0, cpsr |
mrs r1, spsr |
and r0, r0, #0x1f |
and r1, r1, #0x1f |
cmp r0, r1 |
be 3f |
@different exception mode => preserve sp |
ldr r1, [=exc_stack + 4] |
ldr r0, [=exc_stack] |
|
3: |
mrs r0, spsr |
stmfd sp!, {r0, sp, lr} |
*/ |
|
asm volatile( "ldr r13, =exc_stack \n\ |
stmfd r13!, {r0} \n\ |
mrs r0, spsr \n\ |
249,7 → 268,25 |
static void irq_exception_entry() |
{ |
asm("sub lr, lr, #4"); |
PROCESS_EXCEPTION(EXC_IRQ); |
setup_stack_and_save_regs(); |
|
/* switch to Undefined mode */ |
asm("stmfd sp!, {r0-r3}"); |
asm("mov r1, sp"); |
asm("mov r2, lr"); |
// asm("mrs r3, spsr"); |
asm("mrs r0, cpsr"); |
asm("bic r0, r0, #0x1f"); |
asm("orr r0, r0, #0x1b"); |
asm("msr cpsr_c, r0"); |
asm("mov sp, r1"); |
asm("mov lr, r2"); |
// asm("msr spsr, r3"); |
asm("ldmfd sp!, {r0-r3}"); |
|
CALL_EXC_DISPATCH(EXC_IRQ) |
|
load_regs(); |
} |
|
|