56,28 → 56,13 |
extern uintptr_t supervisor_sp; |
extern uintptr_t exc_stack; |
|
|
/** Switches to kernel stack and saves all registers there. |
* |
* Temporary exception stack is used to save a few registers |
* before stack switch takes place. |
*/ |
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\ |
128,7 → 113,11 |
); |
} |
|
|
/** Returns from exception mode. |
* |
* Previously saved state of registers (including control register) |
* is restored from the stack. |
*/ |
inline static void load_regs() |
{ |
asm volatile( "ldmfd r13!, {r0} \n\ |
159,32 → 148,15 |
); |
} |
|
|
|
/*#define SAVE_REGS_TO_STACK \ |
asm("stmfd r13!, {r0-r12, r13, lr}"); \ |
asm("mrs r14, spsr"); \ |
asm("stmfd r13!, {r14}"); |
*/ |
|
|
/** Calls exception dispatch routine. */ |
#define CALL_EXC_DISPATCH(exception) \ |
asm("mov r0, %0" : : "i" (exception)); \ |
asm("mov r1, r13"); \ |
asm("bl exc_dispatch"); |
|
|
/**Loads registers from the stack and resets SPSR before exitting exception |
* handler. |
|
#define LOAD_REGS_FROM_STACK \ |
asm("ldmfd r13!, {r14}"); \ |
asm("msr spsr, r14"); \ |
asm("ldmfd r13!, {r0-r12, r13, pc}^"); |
*/ |
|
|
/** General exception handler. |
* |
* Stores registers, dispatches the exception, |
* and finally restores registers and returns from exception processing. |
* |
195,13 → 167,9 |
CALL_EXC_DISPATCH(exception) \ |
load_regs(); |
|
/* #define PROCESS_EXCEPTION(exception) \ |
SAVE_REGS_TO_STACK \ |
CALL_EXC_DISPATCH(exception) \ |
LOAD_REGS_FROM_STACK*/ |
|
|
/** Updates specified exception vector to jump to given handler. |
* |
* Addresses of handlers are stored in memory following exception vectors. |
*/ |
static void install_handler (unsigned handler_addr, unsigned* vector) |
264,7 → 232,12 |
} |
|
|
/** Low-level Interrupt Exception handler. */ |
/** Low-level Interrupt Exception handler. |
* |
* CPU is switched to Undefined mode before further interrupt processing |
* because of possible occurence of nested interrupt exception, which |
* would overwrite (and thus spoil) stack pointer. |
*/ |
static void irq_exception_entry() |
{ |
asm("sub lr, lr, #4"); |
274,7 → 247,6 |
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"); |
281,7 → 253,6 |
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) |
378,37 → 349,9 |
exc_register(EXC_PREFETCH_ABORT, "prefetch abort", (iroutine) prefetch_abort); |
exc_register(EXC_DATA_ABORT, "data abort", (iroutine) data_abort); |
exc_register(EXC_SWI, "software interrupt", (iroutine) swi_exception); |
/* TODO add next */ |
} |
|
|
/** Sets stack pointers in all supported exception modes. */ |
void setup_exception_stacks() |
{ |
/* switch to particular mode and set "r13" there */ |
|
uint32_t cspr = current_status_reg_read(); |
|
/* IRQ stack */ |
current_status_reg_control_write( |
(cspr & ~STATUS_REG_MODE_MASK) | IRQ_MODE |
); |
asm("ldr r13, =exc_stack"); |
|
/* abort stack */ |
current_status_reg_control_write( |
(cspr & ~STATUS_REG_MODE_MASK) | ABORT_MODE |
); |
asm("ldr r13, =exc_stack"); |
|
/* TODO if you want to test other exceptions than IRQ, |
make stack analogous to irq_stack (in start.S), |
and then set stack pointer here */ |
|
current_status_reg_control_write(cspr); |
} |
|
|
/** Prints #istate_t structure content. |
* |
* @param istate Structure to be printed. |