working store & restore

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 }

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)
+	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 */
-void restore_ctx(uint32_t *stack)
+void restore_ctx(void)
 {
 	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)
+		"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 (
-		/* 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)
 {
-	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);
+	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);
 }