From 991b088b0d2ec802dbef8218c36f680f7254e0ad Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Sun, 24 Feb 2008 17:32:34 +0100 Subject: [PATCH] working store & restore --- at91_init0.s | 10 ++-- main.c | 155 +++++++++++++++++++++++++++++++++++++-------------- 2 files changed, 118 insertions(+), 47 deletions(-) diff --git a/at91_init0.s b/at91_init0.s index b2ced47..8dd1835 100644 --- a/at91_init0.s +++ b/at91_init0.s @@ -29,6 +29,7 @@ .equ ARM_MODE_IRQ, 0x12 .equ ARM_MODE_SVC, 0x13 .equ ARM_MODE_ABT, 0x17 + .equ ARM_MODE_SYS, 0x1F .equ I_BIT, 0x80 .equ F_BIT, 0x40 @@ -46,10 +47,11 @@ rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */ irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */ fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */ + .extern SWI_Handler /* 0x80000000 will result in Prefetch Abort */ .word InitReset .word 0x80000000 - .word 0x80000000 + .word SWI_Handler .word ABT_Handler_Entry .word ABT_Handler_Entry .word 0x80000000 @@ -88,7 +90,7 @@ InitReset: sub r0, r0, #ABT_Stack_Size /* Setup Supervisor Mode Stack (IRQ & NMI enabled) */ - msr CPSR_c, #ARM_MODE_SVC + msr CPSR_c, #ARM_MODE_SYS mov sp, r0 /* Relocate .data section */ @@ -167,7 +169,7 @@ FIQ_Handler_Entry: ldr r0, [r8, #AIC_FVR] /* Switch to ARM_MODE_SVC and save registers there */ - msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT + msr CPSR_c, #ARM_MODE_SYS | I_BIT | F_BIT stmfd sp!, { r1-r3, r12, lr } /* execute FIQ in SVC_MODE */ @@ -208,7 +210,7 @@ IRQ_Handler_Entry: str r14, [r14, #AIC_IVR] /* Enable Interrupt and Switch in Supervisor Mode */ - msr CPSR_c, #ARM_MODE_SVC + msr CPSR_c, #ARM_MODE_SYS /* Save scratch/used registers and LR in User Stack */ stmfd sp!, { r1-r3, r12, r14 } diff --git a/main.c b/main.c index 582660e..813f6f3 100644 --- a/main.c +++ b/main.c @@ -18,6 +18,7 @@ ***************************************************************************/ #include #include +#include #include "AT91SAM7S256.h" #include "board.h" @@ -25,65 +26,136 @@ #include "at91_dbgu.h" #include "memalloc.h" -uint32_t * init_ctx(uint32_t *stack, void *code, void *arg) +struct register_context { + uint32_t r0; + uint32_t r1; + uint32_t pc; + uint32_t cpsr; + uint32_t r2; + uint32_t r3; + uint32_t r4; + uint32_t r5; + uint32_t r6; + uint32_t r7; + uint32_t r8; + uint32_t r9; + uint32_t r10; + uint32_t r11; + uint32_t r12; + uint32_t sp; + uint32_t r14; +}; + +struct context { + struct register_context regs; + uint32_t stacksize; +}; + +struct context * create_ctx(uint32_t stacksize, void *code, void *arg) { - uint32_t org_stack = (uint32_t)stack; + void *stack = static_alloc(sizeof(struct context) + stacksize); + memset(stack, 0, stacksize); - *stack-- = (uint32_t) code; // r15 (pc) - *stack-- = 0x14141414; // r14 (lr) - *stack-- = org_stack -4; // r13 (sp) - *stack-- = 0x12121212; // r12 - *stack-- = 0x11111111; // r11 - *stack-- = 0x10101010; // r10 - *stack-- = 0x09090909; // r9 - *stack-- = 0x08080808; // r8 - *stack-- = 0x07070707; // r7 - *stack-- = 0x06060606; // r6 - *stack-- = 0x05050505; // r5 - *stack-- = 0x04040404; // r4 - *stack-- = 0x03030303; // r3 - *stack-- = 0x02020202; // r2 - *stack-- = 0x01010101; // r1 - *stack-- = (uint32_t) arg; // r0 (function parameter) - *stack = 0xF0000013; // SPSR (SVC, ARM, IRQ & FIQ enabled) + struct context *ctx = (struct context *)((uint8_t *)stack + stacksize); + ctx->regs.r0 = (uint32_t)arg; + ctx->regs.pc = (uint32_t)code; + ctx->regs.cpsr = 0x0000001F; + ctx->regs.sp = (uint32_t)ctx; - return stack; + ctx->regs.r1 = 0x01010101; + ctx->regs.r2 = 0x02020202; + ctx->regs.r3 = 0x03030303; + ctx->regs.r4 = 0x04040404; + ctx->regs.r5 = 0x05050505; + ctx->regs.r6 = 0x06060606; + ctx->regs.r7 = 0x07070707; + ctx->regs.r8 = 0x08080808; + ctx->regs.r9 = 0x09090909; + ctx->regs.r10 = 0x10101010; + ctx->regs.r11 = 0x11111111; + ctx->regs.r12 = 0x12121212; + ctx->regs.r14 = 0x14141414; + + return ctx; } +struct context *current_ctx; + /* we're in the scheduler, SVC mode */ -void restore_ctx(uint32_t *stack) +void restore_ctx(void) { asm volatile ( - /* restore spsr */ - "mov lr, r0 \n\t" - "ldmia lr!, {r0} \n\t" - "msr spsr, r0 \n\t" + // debug: assume we're in svc mode + "msr cpsr_c, 0x13 \n\r" - /* restore all registers */ - "ldmia lr, {r0-r13} \n\t" - "nop \n\t" - "ldmia sp!, {r14-r15}^ \n\t" - "nop \n\t" + "ldr r0, =current_ctx \n\t" + "ldr r0, [r0] \n\t" + + // get r0, r1 and pc(in lr) + "ldmia r0!, {r1-r2,r14} \n\t" + + // put r0, r1 on (svc)stack + "stmdb sp!, {r1-r2} \n\t" + + // get spsr(in r1), r2-r14 (usermode!) + "ldmia r0, {r1-r14}^ \n\t" + "nop \n\t" + + // store spsr + "msr spsr, r1 \n\t" + + // get r0 & r1 from (svc)stack + "ldmia sp!, {r0, r1} \n\t" + + // jump & restore cpsr from spsr + "movs pc, lr \n\t" ); } - -void store_ctx(void) +__attribute__((naked)) void SWI_Handler(void) { asm volatile ( - /* push r0 on (task)stack */ - "stmdb sp!, {r0} \n\r" + "stmdb sp!, {r0-r1} \n\t" - // TODO: fill r0 with address TCB + "ldr r0, =current_ctx \n\t" + "ldr r0, [r0] \n\t" + "add r0, r0, #68 \n\t" - /* return address */ - "stmdb r0!, {lr} \n\r" + "mrs r1, spsr \n\t" + + "stmdb r0, {r1-r14}^ \n\t" + "nop \n\t" + "sub r0, r0, #56 \n\t" + + "ldmia sp!, {r1, r2} \n\t" + "stmdb r0!, {r1, r2, r14} \n\t" + ); + + printf("swi: ok\n\r"); + + asm volatile ( + "ldr r0, =current_ctx \n\t" + "ldr r0, [r0] \n\t" + + "ldmia r0!, {r1-r2,r14} \n\t" + "stmdb sp!, {r1-r2} \n\t" + + "ldmia r0, {r1-r14}^ \n\t" + "nop \n\t" + + "msr spsr, r1 \n\t" + "ldmia sp!, {r0, r1} \n\t" + "movs pc, lr \n\t" ); } void testfunc(void *p) { - printf("bla: %p\n\r", p); + printf("testfunc: %p\n\r", p); + + asm volatile ("swi"); + + printf("testfunc: %p\n\r", p); while(1); } @@ -97,9 +169,6 @@ int main(void) at91_dbgu_init(); at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r"); - uint32_t *blub = static_alloc(1024); - printf("blub: %p\n\r", blub); - - uint32_t *tmp = init_ctx(blub + (1024/4), testfunc, 0); - restore_ctx(tmp); + current_ctx = create_ctx(1024, testfunc, 0); + restore_ctx(); }