From d97eac380ee6213dc4f4b4d3b07c97b561fdb936 Mon Sep 17 00:00:00 2001
From: Anup Patel <anup@brainfault.org>
Date: Wed, 6 Jul 2011 21:23:04 +0530
Subject: [PATCH] Converted context switching functions in terms of setjump and
 longjump primitives.

Signed-off-by: Anup Patel <anup@brainfault.org>
---
 ports/arm7a/arm_main.c     |  2 +-
 ports/arm7a/atomport-asm.s | 34 ++++++++++++++++------------------
 ports/arm7a/atomport.c     | 38 ++++++++++++--------------------------
 3 files changed, 29 insertions(+), 45 deletions(-)

diff --git a/ports/arm7a/arm_main.c b/ports/arm7a/arm_main.c
index d8ace0e..ec42025 100644
--- a/ports/arm7a/arm_main.c
+++ b/ports/arm7a/arm_main.c
@@ -171,7 +171,7 @@ int main ( void )
     {
 	arm_irq_setup();
 
-	arm_timer_init(1000, 1);
+	arm_timer_init((1000000 / SYSTEM_TICKS_PER_SEC), 1);
 
 	arm_uart_init();
 
diff --git a/ports/arm7a/atomport-asm.s b/ports/arm7a/atomport-asm.s
index fa09386..667dea9 100644
--- a/ports/arm7a/atomport-asm.s
+++ b/ports/arm7a/atomport-asm.s
@@ -40,34 +40,31 @@ archGetCPSR:
 	bx	lr
 
 /**
- * int archSetJumpLowLevel(pt_regs_t *regs)
+ * int archSetJump(pt_regs_t *regs)
  */
-	.globl archSetJumpLowLevel
-archSetJumpLowLevel:
+	.globl archSetJump
+archSetJump:
 	add	r0, r0, #(4 * 16)
 	str	lr, [r0]
 	sub	r0, r0, #(4 * 14)
 	stm	r0, {r1-r14}
 	mov	r0, r0 /* NOP */
-	sub	sp, sp, #4
-	str	r1, [sp]
-	mov	r1, #0
+	str	r2, [r1]
+	mov	r2, #0
 	sub	r0, r0, #4
-	str	r1, [r0]
-	mrs	r1, cpsr_all
-	add	r0, r0, #4
-	str	r1, [r0]
-	ldr	r1, [sp]
-	sub	sp, sp, #4
+	str	r2, [r0]
+	mrs	r2, cpsr_all
+	sub	r0, r0, #4
+	str	r2, [r0]
+	ldr	r2, [r1]
 	mov	r0, #1
 	bx	lr
 
 /**
- * void archLongJumpLowLevel(pt_regs_t *regs)
+ * void archLongJump(pt_regs_t *regs)
  */
-	.globl archLongJumpLowLevel
-archLongJumpLowLevel:
-	add	r0, r0, #(4 * 17)
+	.globl archLongJump
+archLongJump:
 	mrs	r1, cpsr_all
 	SET_CURRENT_MODE CPSR_MODE_UNDEFINED
 	mov	sp, r0
@@ -78,9 +75,10 @@ archLongJumpLowLevel:
 	SET_CURRENT_MODE CPSR_MODE_FIQ
 	mov	sp, r0
 	msr	cpsr_all, r1
-	sub	r0, r0, #(4 * 17)
 	ldr     r1, [r0], #4 /* Get CPSR from stack */
+	msr	spsr_all, r1
+	orr	r1, r1, #(CPSR_IRQ_DISABLED | CPSR_FIQ_DISABLED)
 	msr	cpsr_all, r1
-	ldm	r0, {r0-r15}
+	ldm	r0, {r0-r15}^
 	mov	r0, r0 /* NOP */
 
diff --git a/ports/arm7a/atomport.c b/ports/arm7a/atomport.c
index 96f2cc4..08bb591 100644
--- a/ports/arm7a/atomport.c
+++ b/ports/arm7a/atomport.c
@@ -63,13 +63,13 @@ void archThreadContextInit (ATOM_TCB *tcb_ptr, void *stack_top,
 	for (i = 1; i < 13; i++) {
 		regs->gpr[i] = 0x0;
 	}
-	regs->sp = (uint32_t)stack_top - sizeof(pt_regs_t) - 2048;
+	regs->sp = (uint32_t)stack_top - sizeof(pt_regs_t) - 1024;
 	regs->lr = (uint32_t)entry_point;
 	regs->pc = (uint32_t)entry_point;
 }
 
-extern int archSetJumpLowLevel(pt_regs_t *regs);
-extern void archLongJumpLowLevel(pt_regs_t *regs);
+extern int archSetJump(pt_regs_t *regs, uint32_t *tmp);
+extern void archLongJump(pt_regs_t *regs);
 extern uint32_t archGetCPSR(void);
 
 /**
@@ -86,10 +86,9 @@ extern uint32_t archGetCPSR(void);
  */
 void archFirstThreadRestore(ATOM_TCB *new_tcb)
 {
-	pt_regs_t *regs = NULL;
-	regs = (pt_regs_t *)((uint32_t)new_tcb->sp_save_ptr 
+	pt_regs_t *regs = (pt_regs_t *)((uint32_t)new_tcb->sp_save_ptr 
 							- sizeof(pt_regs_t));
-	archLongJumpLowLevel(regs);
+	archLongJump(regs);
 }
 
 /**
@@ -101,28 +100,15 @@ void archFirstThreadRestore(ATOM_TCB *new_tcb)
  */
 void archContextSwitch(ATOM_TCB *old_tcb, ATOM_TCB *new_tcb)
 {
-	uint32_t mode;
-	pt_regs_t tmp;
-	pt_regs_t *old_regs = NULL;
-	pt_regs_t *new_regs = NULL;
-	old_regs = (pt_regs_t *)((uint32_t)old_tcb->sp_save_ptr 
+	uint32_t tmp = 0x0, lr = 0x0;
+	pt_regs_t *old_regs = (pt_regs_t *)((uint32_t)old_tcb->sp_save_ptr
 							- sizeof(pt_regs_t));
-	new_regs = (pt_regs_t *)((uint32_t)new_tcb->sp_save_ptr 
+	pt_regs_t *new_regs = (pt_regs_t *)((uint32_t)new_tcb->sp_save_ptr
 							- sizeof(pt_regs_t));
-	mode = archGetCPSR() & CPSR_MODE_MASK;
-	if ((mode == CPSR_MODE_IRQ) || (mode == CPSR_MODE_FIQ)) {
-		/* Interrupt Context */
-		memcpy(&tmp, old_regs, sizeof(pt_regs_t));
-		if (archSetJumpLowLevel(old_regs)) {
-			archLongJumpLowLevel(new_regs);
-		} else {
-			memcpy(old_regs, &tmp, sizeof(pt_regs_t));
-		}
-	} else {
-		/* Thread Context */
-		if (archSetJumpLowLevel(old_regs)) {
-			archLongJumpLowLevel(new_regs);
-		}
+	asm volatile (" mov %0, lr\n\t" :"=r"(lr):);
+	if (archSetJump(old_regs, &tmp)) {
+		old_regs->lr = lr;
+		archLongJump(new_regs);
 	}
 }