40,6 → 40,7 |
#include <interrupt.h> |
#include <arch/drivers/gxemul.h> |
#include <arch/mm/page_fault.h> |
#include <print.h> |
|
#define PREFETCH_OFFSET 0x8 |
#define BRANCH_OPCODE 0xea000000 |
48,33 → 49,127 |
#define EXC_VECTORS_SIZE 0x20 |
#define EXC_VECTORS 0x8 |
|
extern uintptr_t supervisor_sp; |
extern uintptr_t exc_stack; |
|
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\ |
cmp r0, #0x10 \n\ |
bne 1f \n\ |
\n\ |
@prev mode was usermode \n\ |
ldmfd r13!, {r0} \n\ |
ldr r13, =supervisor_sp \n\ |
stmfd r13!, {r0-r12, r13, lr} \n\ |
stmfd r13!, {r13, lr}^ \n\ |
mrs r0, spsr \n\ |
stmfd r13!, {r0} \n\ |
b 2f \n\ |
\n\ |
@prev mode was not usermode \n\ |
1: \n\ |
stmfd r13!, {r1, r2, r3} \n\ |
mrs r1, cpsr \n\ |
mov r2, lr \n\ |
bic r1, r1, #0x1f \n\ |
orr r1, r1, r0 \n\ |
mrs r0, cpsr \n\ |
msr cpsr_c, r1 \n\ |
\n\ |
mov r3, r13 \n\ |
stmfd r13!, {r2} \n\ |
stmfd r13!, {r3} \n\ |
stmfd r13!, {r4-r12} \n\ |
mov r2, lr \n\ |
mov r1, r13 \n\ |
msr cpsr_c, r0 \n\ |
\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:" |
); |
} |
|
|
inline static void load_regs() |
{ |
asm volatile( "ldmfd r13!, {r0} \n\ |
msr spsr, r0 \n\ |
and r0, r0, #0x1f \n\ |
cmp r0, #0x10 \n\ |
bne 3f \n\ |
\n\ |
@return to user mode \n\ |
ldmfd r13!, {r13, lr}^ \n\ |
b 4f \n\ |
\n\ |
@return to non-user mode \n\ |
3: \n\ |
ldmfd r13!, {r1, r2} \n\ |
mrs r3, cpsr \n\ |
bic r3, r3, #0x1f \n\ |
orr r3, r3, r0 \n\ |
mrs r0, cpsr \n\ |
msr cpsr_c, r3 \n\ |
\n\ |
mov r13, r1 \n\ |
mov lr, r2 \n\ |
msr cpsr_c, r0 \n\ |
\n\ |
@actual return \n\ |
4: \n\ |
ldmfd r13!, {r0-r12, r13, pc}^" |
); |
} |
|
|
#define SAVE_REGS_TO_STACK \ |
asm("stmfd sp!, {r0-r12, sp, lr}"); \ |
asm("stmfd r13!, {r0-r12, r13, lr}"); \ |
asm("mrs r14, spsr"); \ |
asm("stmfd sp!, {r14}"); |
asm("stmfd r13!, {r14}"); |
|
|
|
#define CALL_EXC_DISPATCH(exception) \ |
asm("mov r0, %0" : : "i" (exception)); \ |
asm("mov r1, sp"); \ |
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 sp!, {r14}"); \ |
asm("ldmfd r13!, {r14}"); \ |
asm("msr spsr, r14"); \ |
asm("ldmfd sp!, {r0-r12, sp, pc}^"); |
asm("ldmfd r13!, {r0-r12, r13, pc}^"); |
|
|
|
/** General exception handler. |
* Stores registers, dispatches the exception, |
* and finally restores registers and returns from exception processing. |
*/ |
|
#define PROCESS_EXCEPTION(exception) \ |
setup_stack_and_save_regs(); \ |
CALL_EXC_DISPATCH(exception) \ |
load_regs(); |
|
/* #define PROCESS_EXCEPTION(exception) \ |
SAVE_REGS_TO_STACK \ |
CALL_EXC_DISPATCH(exception) \ |
LOAD_REGS_FROM_STACK |
LOAD_REGS_FROM_STACK*/ |
|
/** Updates specified exception vector to jump to given handler. |
* Addresses of handlers are stored in memory following exception vectors. |
91,8 → 186,10 |
|
/* store handler's address */ |
*(vector + EXC_VECTORS) = handler_addr; |
|
} |
|
|
static void reset_exception_entry() |
{ |
PROCESS_EXCEPTION(EXC_RESET); |
135,10 → 232,27 |
static void irq_exception_entry() |
{ |
asm("sub lr, lr, #4"); |
// SAVE_REGS_TO_STACK |
// CALL_EXC_DISPATCH(EXC_IRQ) |
// LOAD_REGS_FROM_STACK; |
PROCESS_EXCEPTION(EXC_IRQ); |
} |
|
// static void prefetch_abort_exception(int exc_no, istate_t* istate) |
// { |
// dputs("(PREFETCH|DATA) ABORT exception caught, not processed.\n"); |
// } |
|
static void swi_exception(int exc_no, istate_t* istate) |
{ |
dprintf("\nIstate dump:\n"); |
dprintf(" r0:%X r1:%X r2:%X r3:%X\n", istate->r0, istate->r1, istate->r2, istate->r3); |
dprintf(" r4:%X r5:%X r6:%X r7:%X\n", istate->r4, istate->r5, istate->r6, istate->r7); |
dprintf(" r8:%X r9:%X r10:%X r11:%X\n", istate->r8, istate->r9, istate->r10, istate->r11); |
dprintf(" r12:%X r13:%X lr:%X spsr:%X\n", istate->r12, istate->sp, istate->lr, istate->spsr); |
dprintf(" prev_lr:%X prev_sp:%X\n", istate->prev_lr, istate->prev_sp); |
} |
|
/** Interrupt Exception handler. |
* Determines the sources of interrupt, and calls their handlers. |
*/ |
213,6 → 327,7 |
(unsigned*)EXC_FIQ_VEC); |
} |
|
#ifdef HIGH_EXCEPTION_VECTORS |
/** Activates using high exception vectors addresses. */ |
static void high_vectors() |
{ |
225,8 → 340,8 |
|
asm volatile( "mcr p15, 0, %0, c1, c1" : : "r" (control_reg)); |
} |
#endif |
|
|
/** Initializes exception handling. |
* |
* Installs low-level exception handlers and then registers |
237,19 → 352,22 |
#ifdef HIGH_EXCEPTION_VECTORS |
high_vectors(); |
#endif |
|
install_exception_handlers(); |
|
exc_register(EXC_IRQ, "interrupt", (iroutine) irq_exception); |
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 */ |
} |
|
/* TODO change soon, temporary hack. */ |
/** Sets stack pointers in all supported exception modes. |
* |
* @param stack_ptr stack pointer |
*/ |
void setup_exception_stacks() |
{ |
/* switch to particular mode and set "sp" there */ |
/* switch to particular mode and set "r13" there */ |
|
uint32_t cspr = current_status_reg_read(); |
|
257,13 → 375,13 |
current_status_reg_control_write( |
(cspr & ~STATUS_REG_MODE_MASK) | IRQ_MODE |
); |
asm("ldr sp, =irq_stack"); |
asm("ldr r13, =exc_stack"); |
|
/* abort stack */ |
current_status_reg_control_write( |
(cspr & ~STATUS_REG_MODE_MASK) | ABORT_MODE |
); |
asm("ldr sp, =abort_stack"); |
asm("ldr r13, =exc_stack"); |
|
/* TODO if you want to test other exceptions than IRQ, |
make stack analogous to irq_stack (in start.S), |