working store & restore
This commit is contained in:
parent
011d784923
commit
991b088b0d
10
at91_init0.s
10
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 }
|
||||
|
|
|
|||
155
main.c
155
main.c
|
|
@ -18,6 +18,7 @@
|
|||
***************************************************************************/
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#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();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue