| 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. |