working store & restore

This commit is contained in:
Olaf Rempel 2008-02-24 17:32:34 +01:00
parent 011d784923
commit 991b088b0d
2 changed files with 118 additions and 47 deletions

View File

@ -29,6 +29,7 @@
.equ ARM_MODE_IRQ, 0x12 .equ ARM_MODE_IRQ, 0x12
.equ ARM_MODE_SVC, 0x13 .equ ARM_MODE_SVC, 0x13
.equ ARM_MODE_ABT, 0x17 .equ ARM_MODE_ABT, 0x17
.equ ARM_MODE_SYS, 0x1F
.equ I_BIT, 0x80 .equ I_BIT, 0x80
.equ F_BIT, 0x40 .equ F_BIT, 0x40
@ -46,10 +47,11 @@ 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
/* 0x80000000 will result in Prefetch Abort */ /* 0x80000000 will result in Prefetch Abort */
.word InitReset .word InitReset
.word 0x80000000 .word 0x80000000
.word 0x80000000 .word SWI_Handler
.word ABT_Handler_Entry .word ABT_Handler_Entry
.word ABT_Handler_Entry .word ABT_Handler_Entry
.word 0x80000000 .word 0x80000000
@ -88,7 +90,7 @@ InitReset:
sub r0, r0, #ABT_Stack_Size sub r0, r0, #ABT_Stack_Size
/* Setup Supervisor Mode Stack (IRQ & NMI enabled) */ /* Setup Supervisor Mode Stack (IRQ & NMI enabled) */
msr CPSR_c, #ARM_MODE_SVC msr CPSR_c, #ARM_MODE_SYS
mov sp, r0 mov sp, r0
/* Relocate .data section */ /* Relocate .data section */
@ -167,7 +169,7 @@ FIQ_Handler_Entry:
ldr r0, [r8, #AIC_FVR] ldr r0, [r8, #AIC_FVR]
/* Switch to ARM_MODE_SVC and save registers there */ /* 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 } stmfd sp!, { r1-r3, r12, lr }
/* execute FIQ in SVC_MODE */ /* execute FIQ in SVC_MODE */
@ -208,7 +210,7 @@ IRQ_Handler_Entry:
str r14, [r14, #AIC_IVR] str r14, [r14, #AIC_IVR]
/* Enable Interrupt and Switch in Supervisor Mode */ /* 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 */ /* Save scratch/used registers and LR in User Stack */
stmfd sp!, { r1-r3, r12, r14 } stmfd sp!, { r1-r3, r12, r14 }

155
main.c
View File

@ -18,6 +18,7 @@
***************************************************************************/ ***************************************************************************/
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <string.h>
#include "AT91SAM7S256.h" #include "AT91SAM7S256.h"
#include "board.h" #include "board.h"
@ -25,65 +26,136 @@
#include "at91_dbgu.h" #include "at91_dbgu.h"
#include "memalloc.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) struct context *ctx = (struct context *)((uint8_t *)stack + stacksize);
*stack-- = 0x14141414; // r14 (lr) ctx->regs.r0 = (uint32_t)arg;
*stack-- = org_stack -4; // r13 (sp) ctx->regs.pc = (uint32_t)code;
*stack-- = 0x12121212; // r12 ctx->regs.cpsr = 0x0000001F;
*stack-- = 0x11111111; // r11 ctx->regs.sp = (uint32_t)ctx;
*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)
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 */ /* we're in the scheduler, SVC mode */
void restore_ctx(uint32_t *stack) void restore_ctx(void)
{ {
asm volatile ( asm volatile (
/* restore spsr */ // debug: assume we're in svc mode
"mov lr, r0 \n\t" "msr cpsr_c, 0x13 \n\r"
"ldmia lr!, {r0} \n\t"
"msr spsr, r0 \n\t"
/* restore all registers */ "ldr r0, =current_ctx \n\t"
"ldmia lr, {r0-r13} \n\t" "ldr r0, [r0] \n\t"
"nop \n\t"
"ldmia sp!, {r14-r15}^ \n\t" // get r0, r1 and pc(in lr)
"nop \n\t" "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"
); );
} }
__attribute__((naked)) void SWI_Handler(void)
void store_ctx(void)
{ {
asm volatile ( asm volatile (
/* push r0 on (task)stack */ "stmdb sp!, {r0-r1} \n\t"
"stmdb sp!, {r0} \n\r"
// 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 */ "mrs r1, spsr \n\t"
"stmdb r0!, {lr} \n\r"
"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) 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); while(1);
} }
@ -97,9 +169,6 @@ int main(void)
at91_dbgu_init(); at91_dbgu_init();
at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r"); at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");
uint32_t *blub = static_alloc(1024); current_ctx = create_ctx(1024, testfunc, 0);
printf("blub: %p\n\r", blub); restore_ctx();
uint32_t *tmp = init_ctx(blub + (1024/4), testfunc, 0);
restore_ctx(tmp);
} }