move exception handlers in C files

This commit is contained in:
Olaf Rempel 2008-02-29 15:33:56 +01:00
parent 021be80589
commit e6155fb7ad
6 changed files with 162 additions and 115 deletions

View File

@ -22,7 +22,7 @@
.equ AT91C_BASE_AIC, (0xFFFFF000) .equ AT91C_BASE_AIC, (0xFFFFF000)
.equ IRQ_Stack_Size, (3 * 8 * 4) .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 ABT_Stack_Size, 192
.equ ARM_MODE_FIQ, 0x11 .equ ARM_MODE_FIQ, 0x11
@ -47,16 +47,19 @@ rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */
irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */ irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */
fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */ 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 */ /* 0x80000000 will result in Prefetch Abort */
.word InitReset .word InitReset
.word 0x80000000 .word 0x80000000
.word SWI_Handler .word SWI_Handler
.word ABT_Handler_Entry .word ABT_Handler
.word ABT_Handler_Entry .word ABT_Handler
.word 0x80000000 .word 0x80000000
.word IRQ_Handler_Entry .word IRQ_Handler
.word FIQ_Handler_Entry .word FIQ_Handler
.endfunc .endfunc
@ -123,45 +126,9 @@ LoopZI: cmp r1, r2
exit: b . exit: b .
.endfunc .endfunc
.global ABT_Handler_Entry .global FIQ_Handler
.func ABT_Handler_Entry .func FIQ_Handler
ABT_Handler_Entry: FIQ_Handler:
/* 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:
/* Save r0 to ARM_MODE_FIQ:r9 */ /* Save r0 to ARM_MODE_FIQ:r9 */
mov r9, r0 mov r9, r0
@ -186,58 +153,3 @@ FIQ_Handler_Entry:
/* restore PC using the LR_fiq directly */ /* restore PC using the LR_fiq directly */
subs pc, lr, #4 subs pc, lr, #4
.endfunc .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

View File

@ -79,3 +79,92 @@ void at91_init1(void)
} }
aic->AIC_SPU = (uint32_t)empty_isr; 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"
);
}

View File

@ -36,7 +36,7 @@ enum ctx_state {
struct context { struct context {
/* regs *MUST* be first in struct! */ /* regs *MUST* be first in struct! */
struct register_context regs; struct register_context regs;
uint32_t stacksize; void *stack;
uint8_t state; uint8_t state;
uint8_t priority; uint8_t priority;

4
main.c
View File

@ -57,10 +57,10 @@ int main(void)
at91_pio_init(); 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); 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); printf("test_ctx(2)=%p\n\r", text_ctx2);
sem_init(&sem, 0); sem_init(&sem, 0);

View File

@ -160,3 +160,45 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
} }
dbgu_putchar('\n'); 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"
);
}

View File

@ -9,7 +9,7 @@
#include "rtos/spinlock.h" #include "rtos/spinlock.h"
/* linked list of ready contexts, #1 is the running thread */ /* 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 */ /* pointer to the running thread */
struct context *current_context = NULL; struct context *current_context = NULL;
@ -47,6 +47,9 @@ __attribute__((naked)) void SWI_Handler(void)
current_context->state = CONTEXT_RUNNING; 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 */ /* restore register context from current_context */
asm volatile ( asm volatile (
/* get pointer to struct register_context */ /* 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) static void isr_context_ready(struct context *ctx)
{ {
struct context *q = run_queue; struct context *q = run_queue;
struct context **qprev = &run_queue; struct context *volatile *qprev = &run_queue;
while (q && (q->priority <= ctx->priority)) { while (q && (q->priority <= ctx->priority)) {
qprev = &q->run_queue; 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.r12 = 0x12121212;
ctx->regs.r14 = 0x14141414; ctx->regs.r14 = 0x14141414;
ctx->stacksize = stacksize; ctx->stack = stack;
ctx->priority = priority; ctx->priority = priority;
disable_irqs(); disable_irqs();
@ -291,6 +294,7 @@ void init_context(void)
current_context = create_ctx(4, 255, NULL, NULL); current_context = create_ctx(4, 255, NULL, NULL);
printf("idle_ctx=%p\n\r", current_context); printf("idle_ctx=%p\n\r", current_context);
/* For now, we're the only thread, so this simply safes our context */ /* For now, we're the only thread, so this simply safes our context */
disable_irqs(); disable_irqs();
isr_context_switch(); isr_context_switch();