From e6155fb7ade7b9aaab16c24c1da82e125b73ee54 Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Fri, 29 Feb 2008 15:33:56 +0100 Subject: [PATCH] move exception handlers in C files --- at91_init0.s | 112 +++++------------------------------------ at91_init1.c | 107 +++++++++++++++++++++++++++++++++++---- include/rtos/context.h | 2 +- main.c | 4 +- src/at91_exceptions.c | 42 ++++++++++++++++ src/rtos/context.c | 10 ++-- 6 files changed, 162 insertions(+), 115 deletions(-) diff --git a/at91_init0.s b/at91_init0.s index 8dd1835..dfa5398 100644 --- a/at91_init0.s +++ b/at91_init0.s @@ -22,7 +22,7 @@ .equ AT91C_BASE_AIC, (0xFFFFF000) .equ IRQ_Stack_Size, (3 * 8 * 4) - .equ FIQ_Stack_Size, (3 * 8 * 4) + .equ FIQ_Stack_Size, (0 * 8 * 4) .equ ABT_Stack_Size, 192 .equ ARM_MODE_FIQ, 0x11 @@ -47,16 +47,19 @@ rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */ irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */ fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */ - .extern SWI_Handler + .extern SWI_Handler + .extern ABT_Handler + .extern IRQ_Handler + /* 0x80000000 will result in Prefetch Abort */ .word InitReset .word 0x80000000 .word SWI_Handler - .word ABT_Handler_Entry - .word ABT_Handler_Entry + .word ABT_Handler + .word ABT_Handler .word 0x80000000 - .word IRQ_Handler_Entry - .word FIQ_Handler_Entry + .word IRQ_Handler + .word FIQ_Handler .endfunc @@ -123,45 +126,9 @@ LoopZI: cmp r1, r2 exit: b . .endfunc - .global ABT_Handler_Entry - .func ABT_Handler_Entry -ABT_Handler_Entry: - /* disable interrupts (F_BIT not set on entry) */ - msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT - - /* store all registers */ - stmfd sp!, { r0-r12 } - - /* saved cpsr (from aborted mode) */ - mrs r0, SPSR - - /* address of abort (pc) */ - mov r3, lr - - /* enter previous mode and get lr(r14), sp(r13) */ - /* TODO: interrupts might be enabled? */ - /* TODO: thumb mode enabled? */ - msr CPSR_c, r0 - mov r1, sp - mov r2, lr - - /* return to abort mode */ - msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT - - /* store remaining registers (r1-r3 == r13-r15) */ - stmfd sp!, { r1-r3 } - mov r1, sp - - /* execute C Handler (cpsr, registers) */ - ldr r5, =at91_abt_handler - mov lr, pc - bx r5 - b . - .endfunc - - .global FIQ_Handler_Entry - .func FIQ_Handler_Entry -FIQ_Handler_Entry: + .global FIQ_Handler + .func FIQ_Handler +FIQ_Handler: /* Save r0 to ARM_MODE_FIQ:r9 */ mov r9, r0 @@ -186,58 +153,3 @@ FIQ_Handler_Entry: /* restore PC using the LR_fiq directly */ subs pc, lr, #4 .endfunc - - .global IRQ_Handler_Entry - .func IRQ_Handler_Entry -IRQ_Handler_Entry: - /* Manage Exception Entry */ - /* Adjust and save LR_irq in IRQ stack */ - sub lr, lr, #4 - stmfd sp!, { lr } - - /* Save SPSR need to be saved for nested interrupt */ - mrs r14, SPSR - stmfd sp!, { r14 } - - /* Save and r0 in IRQ stack */ - stmfd sp!, { r0 } - - /* Write in the IVR to support Protect Mode */ - /* No effect in Normal Mode */ - /* De-assert the NIRQ and clear the source in Protect Mode */ - ldr r14, =AT91C_BASE_AIC - ldr r0, [r14, #AIC_IVR] - str r14, [r14, #AIC_IVR] - - /* Enable Interrupt and Switch in Supervisor Mode */ - msr CPSR_c, #ARM_MODE_SYS - - /* Save scratch/used registers and LR in User Stack */ - stmfd sp!, { r1-r3, r12, r14 } - - /* Branch to the routine pointed by the AIC_IVR */ - mov r14, pc - bx r0 - - /* Restore scratch/used registers and LR from User Stack */ - ldmia sp!, { r1-r3, r12, r14 } - - /* Disable Interrupt and switch back in IRQ mode */ - msr CPSR_c, #ARM_MODE_IRQ | I_BIT - - /* Mark the End of Interrupt on the AIC */ - ldr r14, =AT91C_BASE_AIC - str r14, [r14, #AIC_EOICR] - - /* Restore SPSR_irq and r0 from IRQ stack */ - ldmia sp!, { r0 } - - /* Restore SPSR_irq and r0 from IRQ stack */ - ldmia sp!, { r14 } - msr SPSR_cxsf, r14 - - /* Restore adjusted LR_irq from IRQ stack directly in the PC */ - ldmia sp!, { pc }^ - - .endfunc - .end diff --git a/at91_init1.c b/at91_init1.c index 32e5f72..26abfc8 100644 --- a/at91_init1.c +++ b/at91_init1.c @@ -31,33 +31,33 @@ void at91_init1(void) { /* disable watchdog */ *AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS; - + /* enable user reset */ *AT91C_RSTC_RMR = (AT91C_RSTC_KEY & 0xA5 << 24) | AT91C_RSTC_URSTEN; /* Set Flash Waitstates */ *AT91C_MC_FMR = AT91C_MC_FWS_1FWS; - - /* + + /* * Enable main oscillator (MAINCK) * startup time: 8*6/32768 -> 1.46ms */ AT91S_PMC *pmc = AT91C_BASE_PMC; - pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) | + pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) | AT91C_CKGR_MOSCEN; while (!(pmc->PMC_SR & AT91C_PMC_MOSCS)); - /* + /* * PLLCK = 18.432MHz / 24 * 125 = 96MHz -> div:24, mul:124 * startup time: 32/32768 -> 976us - */ - pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) | - (AT91C_CKGR_MUL & (124<<16)) | + */ + pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) | + (AT91C_CKGR_MUL & (124<<16)) | (AT91C_CKGR_PLLCOUNT & (32<<8)) ; while (!(pmc->PMC_SR & AT91C_PMC_LOCK)); /* MCK = PLLCK / 2 = 48MHz */ - pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | + pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | AT91C_PMC_PRES_CLK_2; while (!(pmc->PMC_SR & AT91C_PMC_MCKRDY)); @@ -79,3 +79,92 @@ void at91_init1(void) } aic->AIC_SPU = (uint32_t)empty_isr; } + +/* TODO: make it static */ +uint32_t nested_count = 0; + +__attribute__((naked)) void IRQ_Handler(void) +{ + asm volatile ( + ".equ AIC_IVR_OFF, (256) \n\t" + ".equ AIC_EOICR_OFF, (304) \n\t" + ".equ AT91C_BASE_AIC, (0xFFFFF000) \n\t" + ".equ ARM_MODE_IRQ, 0x12 \n\t" + ".equ ARM_MODE_SYS, 0x1F \n\t" + ".equ I_BIT, 0x80 \n\t" + ".equ F_BIT, 0x40 \n\t" + + /* Manage Exception Entry */ + /* Save LR_irq to IRQ stack */ + "sub lr, lr, #4 \n\t" + "stmdb sp!, { lr } \n\t" + + /* Save r0 and SPSR to IRQ stack */ + "mrs r14, SPSR \n\t" + "stmdb sp!, { r0, r14 } \n\t" + + /* count nested interrupts */ + "ldr r14, =nested_count \n\t" + "ldr r0, [r14] \n\t" + "add r0, r0, #1 \n\t" + "str r0, [r14] \n\t" + + /* De-assert the NIRQ and clear the source in Protect Mode */ + "ldr r14, =AT91C_BASE_AIC \n\t" + "ldr r0, [r14, #AIC_IVR_OFF] \n\t" + "str r14, [r14, #AIC_IVR_OFF] \n\t" + + /* Enable Interrupt and Switch in Supervisor Mode */ + "msr CPSR_c, #ARM_MODE_SYS \n\t" + + /* Save scratch/used registers and LR in User Stack */ + "stmdb sp!, { r1-r3, r12, r14 } \n\t" + + /* Branch to the routine pointed by the AIC_IVR */ + "mov r14, pc \n\t" + "bx r0 \n\t" + + /* Restore scratch/used registers and LR from User Stack */ + "ldmia sp!, { r1-r3, r12, r14 } \n\t" + + /* Disable Interrupt and switch back in IRQ mode */ + "msr CPSR_c, #ARM_MODE_IRQ | I_BIT \n\t" + + /* Mark the End of Interrupt on the AIC */ + "ldr r14, =AT91C_BASE_AIC \n\t" + "str r14, [r14, #AIC_EOICR_OFF] \n\t" + + /* count nested interrupts */ + "ldr r14, =nested_count \n\t" + "ldr r0, [r14] \n\t" + "subs r0, r0, #1 \n\t" + "str r0, [r14] \n\t" + "beq irq_ctx_save \n\t" + + /* Restore r0 and SPSR_irq from IRQ stack */ + "ldmia sp!, { r0, r14 } \n\t" + "msr SPSR_cxsf, r14 \n\t" + + /* Restore adjusted LR_irq from IRQ stack */ + "ldmia sp!, { pc }^ \n\t" + + /* save the userspace context to current_context */ + "irq_ctx_save: \n\t" + + /* get top of struct register_context */ + "ldr r0, =current_context \n\t" + "ldr r0, [r0] \n\t" + "add r0, r0, #68 \n\t" + + /* save usermode cpsr & r2-14 */ + "mrs r1, spsr \n\t" + "stmdb r0, {r1-r14}^ \n\t" + "nop \n\t" + "sub r0, r0, #56 \n\t" + + /* save r0-1 and svc_lr (= pc) */ + "ldmia sp!, {r1, r2} \n\t" + "stmdb r0!, {r1, r2, r14} \n\t" + + ); +} diff --git a/include/rtos/context.h b/include/rtos/context.h index 8140b2f..8b1de54 100644 --- a/include/rtos/context.h +++ b/include/rtos/context.h @@ -36,7 +36,7 @@ enum ctx_state { struct context { /* regs *MUST* be first in struct! */ struct register_context regs; - uint32_t stacksize; + void *stack; uint8_t state; uint8_t priority; diff --git a/main.c b/main.c index 233c50a..d5f5f85 100644 --- a/main.c +++ b/main.c @@ -57,10 +57,10 @@ int main(void) at91_pio_init(); - struct context *text_ctx1 = create_ctx(256, 0x80, testfunc, (void *)1); + struct context *text_ctx1 = create_ctx(512, 0x80, testfunc, (void *)1); printf("test_ctx(1)=%p\n\r", text_ctx1); - struct context *text_ctx2 = create_ctx(256, 0x80, testfunc, (void *)2); + struct context *text_ctx2 = create_ctx(512, 0x80, testfunc, (void *)2); printf("test_ctx(2)=%p\n\r", text_ctx2); sem_init(&sem, 0); diff --git a/src/at91_exceptions.c b/src/at91_exceptions.c index 73c497c..616c646 100644 --- a/src/at91_exceptions.c +++ b/src/at91_exceptions.c @@ -160,3 +160,45 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers) } dbgu_putchar('\n'); } + + +__attribute__((naked)) void ABT_Handler(void) +{ + asm volatile ( + ".equ ARM_MODE_ABT, 0x17 \n\t" + ".equ I_BIT, 0x80 \n\t" + ".equ F_BIT, 0x40 \n\t" + + /* disable interrupts (F_BIT not set on entry) */ + "msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT \n\t" + + /* store all registers */ + "stmfd sp!, { r0-r12 } \n\t" + + /* saved cpsr (from aborted mode) */ + "mrs r0, SPSR \n\t" + + /* address of abort (pc) */ + "mov r3, lr \n\t" + + /* enter previous mode and get lr(r14), sp(r13) */ + /* TODO: interrupts might be enabled? */ + /* TODO: thumb mode enabled? */ + "msr CPSR_c, r0 \n\t" + "mov r1, sp \n\t" + "mov r2, lr \n\t" + + /* return to abort mode */ + "msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT \n\t" + + /* store remaining registers (r1-r3 == r13-r15) */ + "stmfd sp!, { r1-r3 } \n\t" + "mov r1, sp \n\t" + + /* execute C Handler (cpsr, registers) */ + "ldr r5, =at91_abt_handler \n\t" + "mov lr, pc \n\t" + "bx r5 \n\t" + "b . \n\t" + ); +} diff --git a/src/rtos/context.c b/src/rtos/context.c index a04edcc..0d326a0 100644 --- a/src/rtos/context.c +++ b/src/rtos/context.c @@ -9,7 +9,7 @@ #include "rtos/spinlock.h" /* linked list of ready contexts, #1 is the running thread */ -struct context *run_queue = NULL; +struct context *volatile run_queue = NULL; /* pointer to the running thread */ struct context *current_context = NULL; @@ -47,6 +47,9 @@ __attribute__((naked)) void SWI_Handler(void) current_context->state = CONTEXT_RUNNING; + if (((uint32_t *)current_context->stack)[0] != 0xdeadbeef) + printf("<- task stack corrupt (%p)\n\r", current_context); + /* restore register context from current_context */ asm volatile ( /* get pointer to struct register_context */ @@ -80,7 +83,7 @@ static uint8_t isr_context_switch(void) static void isr_context_ready(struct context *ctx) { struct context *q = run_queue; - struct context **qprev = &run_queue; + struct context *volatile *qprev = &run_queue; while (q && (q->priority <= ctx->priority)) { qprev = &q->run_queue; @@ -272,7 +275,7 @@ struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)( ctx->regs.r12 = 0x12121212; ctx->regs.r14 = 0x14141414; - ctx->stacksize = stacksize; + ctx->stack = stack; ctx->priority = priority; disable_irqs(); @@ -291,6 +294,7 @@ void init_context(void) current_context = create_ctx(4, 255, NULL, NULL); printf("idle_ctx=%p\n\r", current_context); + /* For now, we're the only thread, so this simply safes our context */ disable_irqs(); isr_context_switch();