Fix problems in state restore logic

git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@5 42af7a65-404d-4744-a932-0658087f49c3

8 files changed, 104 insertions, 148 deletions

diff --git a/nuttx/arch/c5471/src/up_fullcontextrestore.S b/nuttx/arch/c5471/src/up_fullcontextrestore.S
index c4b230316..572d3c20c 100644
--- a/nuttx/arch/c5471/src/up_fullcontextrestore.S
+++ b/nuttx/arch/c5471/src/up_fullcontextrestore.S
@@ -78,38 +78,40 @@ up_fullcontextrestore:
 
 	/* On entry, a1 (r0) holds address of the register save area */
 
-	/* Restore the volatile registers.  This is not necessary for
-	 * normally task-to-task context switches (where the context
-	 * was saved by up_saveusercontext()), but is necesary when
-	 * the full context was saved through interrupt handling.
-	 */
-
-	/* Recover the user context (we will then have a new stack pointer) */
-
-	add	r1, r0, #XCPTCONTEXT_UOFFSET
-	ldmia	r1, {r3-r11, r13-r14}
+	/* Recover all registers except for r0, r1, R15, and CPSR */
 
-	/* Save the CSPR value and one scratch register on the stack */
+	add	r1, r0, #(4*REG_R2)	/* Offset to REG_R2 storage */
+	ldmia	r1, {r2-r14}		/* Recover registers */
 
-	sub	sp, sp, #2*4		/* Create a frame to hold two regs */
-	stmia	sp, {r3, r4}		/* Save the CPSR (r3) and scratch (r4) */
+	/* Create a stack frame to hold the PC */
 
-	/* Then recover the remaining registers */
+	sub	sp, sp, #(3*4)		/* Frame for three registers */
+	ldr	r1, [r0, #(4*REG_R0)]	/* Fetch the stored r0 value */
+	str	r2, [sp, #8]		/* Save it at the top of the stack */
+	ldr	r1, [r0, #(4*REG_R1)]	/* Fetch the stored r1 value */
+	str	r2, [sp, #4]		/* Save it in the stack */
+	ldr	r1, [r0, #(4*REG_PC)]	/* Fetch the stored pc value */
+	str	r2, [sp]		/* Save it at the bottom of the stack */
 
-	ldmia	r0, {r0-r3, r12}	/* Recover volatile regs */
+	/* Now we can restore the CPSR.  We wait until we are completely
+	 * finished with the context save data to do this. Restore the CPSR
+	 * may re-enable and interrupts and we couldt be in a context
+	 * where save structure is only protected by interrupts being disabled.
+         */
 
-	/* Now we can restore the CPSR (probably re-enabling interrupts) */
+	ldr	r1, [r0, #(4*REG_CPSR)]	/* Fetch the stored CPSR value */
+	msr	cpsr, r1		/* Set the CPSR */
 
-	ldr	r4, [sp]
-	msr	cpsr, r4
+	/* Now recover r0 and r1 */
 
-	/* Then recover the correct r4 value */
+	ldr	r0, [sp, #8]
+	ldr	r1, [sp, #4]
+	add	sp, sp, #(2*4)
 
-	ldr	r4, [sp, #4]
-
-	/* Destroy the temporary stack frame and return */
+	/* Then return to the address at the stop of the stack,
+	 * destroying the stack frame
+	 */
 
-	add	sp, sp, #2*4
-	movs	pc, lr
+	ldr	pc, [sp], #4
 	.size up_fullcontextrestore, . - up_fullcontextrestore
 
diff --git a/nuttx/arch/c5471/src/up_initialize.c b/nuttx/arch/c5471/src/up_initialize.c
index 660cf44d7..62fdfd387 100644
--- a/nuttx/arch/c5471/src/up_initialize.c
+++ b/nuttx/arch/c5471/src/up_initialize.c
@@ -41,6 +41,7 @@
 #include <sys/types.h>
 #include <debug.h>
 #include <nuttx/arch.h>
+#include <nuttx/fs.h>
 #include "up_internal.h"
 
 /************************************************************
@@ -90,4 +91,8 @@ void up_initialize(void)
   up_disable_irq(C5471_IRQ_SYSTIMER);
   irq_attach(C5471_IRQ_SYSTIMER, (xcpt_t)up_timerisr);
   up_enable_irq(C5471_IRQ_SYSTIMER);
+
+  /* Register devices */
+
+  devnull_register();   /* Standard /dev/null */
 }
diff --git a/nuttx/arch/c5471/src/up_initialstate.c b/nuttx/arch/c5471/src/up_initialstate.c
index 3a26c45f1..0309daab0 100644
--- a/nuttx/arch/c5471/src/up_initialstate.c
+++ b/nuttx/arch/c5471/src/up_initialstate.c
@@ -42,6 +42,7 @@
 #include <string.h>
 #include <nuttx/arch.h>
 #include "up_internal.h"
+#include "c5471.h"
 
 /************************************************************
  * Private Definitions
@@ -80,6 +81,7 @@ void up_initial_state(_TCB *tcb)
   /* Initialize the initial exception register context structure */
 
   memset(xcp, 0, sizeof(struct xcptcontext));
-  xcp->regs[JB_SP] = (uint32)tcb->adj_stack_ptr;
-  xcp->regs[JB_LR] = (uint32)tcb->start;
+  xcp->regs[REG_SP]   = (uint32)tcb->adj_stack_ptr;
+  xcp->regs[REG_PC]   = (uint32)tcb->start;
+  xcp->regs[REG_CPSR] = SVC_MODE | F_BIT;
 }
diff --git a/nuttx/arch/c5471/src/up_saveusercontext.S b/nuttx/arch/c5471/src/up_saveusercontext.S
index b9a9c2501..142f3660f 100644
--- a/nuttx/arch/c5471/src/up_saveusercontext.S
+++ b/nuttx/arch/c5471/src/up_saveusercontext.S
@@ -89,29 +89,27 @@ up_saveusercontext:
 	 */
 
         mov	ip, #1
-	str	ip, [r0, #JB_R0]
+	str	ip, [r0, #(4*REG_R0)]
 
-	/* Get the offset to the user save area */
+	/* Save the volatile registers (plus r12 which really
+	 * doesn't need to be saved)
+	 */
 
-	add	r0, r0, #XCPTCONTEXT_UOFFSET
+	add	r1, r0, #(4*REG_R4)
+	stmia	r1, {r4-r14}
 
-	/* Get the current cpsr as well */
+	/* Save the current cpsr */
 
-	mrs	r3, cpsr	/* R3 = CPSR value */
+	mrs	r2, cpsr		/* R3 = CPSR value */
+	add	r1, r0, #(4*REG_CPSR)
+	str	r2, [r1]
 
-	/* We need to save:
-	 *
-	 * Volatile register:	r3 (holds the cpsr value)
-	 * Static registers:	v1-v7 (aka r4-r10)
-	 * Frame pointer:	fp (aka r11)
-	 * Stack pointer:	sp (aka r13)
-	 * Return address:	lr (aka r14)
-	 *
-	 * These have to be save in the same order as is done
-	 * by the interrupt handling logic.
+	/* Finally save the return address as the PC so that we
+	 * return to the exit from this function.
 	 */
 
-	stmia	r0, {r3-r11, r13-r14}
+        add	r1, r0, #(4*REG_PC)
+	str	lr, [r1]
 
 	/* Return 0 */
 
diff --git a/nuttx/arch/c5471/src/up_schedulesigaction.c b/nuttx/arch/c5471/src/up_schedulesigaction.c
index 7fa92e49f..4ea39bec2 100644
--- a/nuttx/arch/c5471/src/up_schedulesigaction.c
+++ b/nuttx/arch/c5471/src/up_schedulesigaction.c
@@ -138,15 +138,15 @@ void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver)
                */
 
               tcb->xcp.sigdeliver    = sigdeliver;
-              tcb->xcp.saved_lr      = current_regs[JB_LR];
-              tcb->xcp.saved_cpsr    = current_regs[JB_CPSR];
+              tcb->xcp.saved_pc      = current_regs[REG_PC];
+              tcb->xcp.saved_cpsr    = current_regs[REG_CPSR];
 
               /* Then set up to vector to the trampoline with interrupts
                * disabled
                */
 
-              current_regs[JB_LR]    = (uint32)up_sigdeliver;
-              current_regs[JB_CPSR]  = SVC_MODE | I_BIT | F_BIT;
+              current_regs[REG_PC]    = (uint32)up_sigdeliver;
+              current_regs[REG_CPSR]  = SVC_MODE | I_BIT | F_BIT;
 
               /* And make sure that the saved context in the TCB
                * is the same as the interrupt return context.
@@ -170,15 +170,15 @@ void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver)
            */
 
           tcb->xcp.sigdeliver    = sigdeliver;
-          tcb->xcp.saved_lr      = tcb->xcp.regs[JB_LR];
-          tcb->xcp.saved_cpsr    = tcb->xcp.regs[JB_CPSR];
+          tcb->xcp.saved_pc      = tcb->xcp.regs[REG_PC];
+          tcb->xcp.saved_cpsr    = tcb->xcp.regs[REG_CPSR];
 
           /* Then set up to vector to the trampoline with interrupts
            * disabled
            */
 
-          tcb->xcp.regs[JB_LR]   = (uint32)up_sigdeliver;
-          tcb->xcp.regs[JB_CPSR] = SVC_MODE | I_BIT | F_BIT;
+          tcb->xcp.regs[REG_PC]   = (uint32)up_sigdeliver;
+          tcb->xcp.regs[REG_CPSR] = SVC_MODE | I_BIT | F_BIT;
         }
 
       irqrestore(flags);
diff --git a/nuttx/arch/c5471/src/up_sigdeliver.c b/nuttx/arch/c5471/src/up_sigdeliver.c
index 00b38a9dd..1ef77c182 100644
--- a/nuttx/arch/c5471/src/up_sigdeliver.c
+++ b/nuttx/arch/c5471/src/up_sigdeliver.c
@@ -85,8 +85,8 @@ void up_sigdeliver(void)
   /* Save the real return state on the stack. */
 
   up_copystate(regs, rtcb->xcp.regs);
-  regs[JB_LR]   = rtcb->xcp.saved_lr;
-  regs[JB_CPSR] = rtcb->xcp.saved_cpsr;
+  regs[REG_PC]   = rtcb->xcp.saved_pc;
+  regs[REG_CPSR] = rtcb->xcp.saved_cpsr;
 
   /* Get a local copy of the sigdeliver function pointer.
    * we do this so that we can nullify the sigdeliver
@@ -95,7 +95,7 @@ void up_sigdeliver(void)
    * signals.
    */
 
-  sigdeliver = rtcb->xcp.sigdeliver;
+  sigdeliver           = rtcb->xcp.sigdeliver;
   rtcb->xcp.sigdeliver = NULL;
 
   /* Then enable interrupts.  We should still be safe from
diff --git a/nuttx/arch/c5471/src/up_undefinedinsn.c b/nuttx/arch/c5471/src/up_undefinedinsn.c
index 3c213eb31..25c46b133 100644
--- a/nuttx/arch/c5471/src/up_undefinedinsn.c
+++ b/nuttx/arch/c5471/src/up_undefinedinsn.c
@@ -55,7 +55,7 @@
  ************************************************************/
 
 /************************************************************
- * Public Funtions
+ * Public Functions
  ************************************************************/
 
 /************************************************************
diff --git a/nuttx/arch/c5471/src/up_vectors.S b/nuttx/arch/c5471/src/up_vectors.S
index f1c846750..b9a4e6d6c 100644
--- a/nuttx/arch/c5471/src/up_vectors.S
+++ b/nuttx/arch/c5471/src/up_vectors.S
@@ -107,12 +107,11 @@ up_vectorirq:
 	/* Create a context structure */
 
 	sub	sp, sp, #XCPTCONTEXT_SIZE
-	stmia	sp, {r0-r3, r12}	/* Save volatile regs */
+	stmia	sp, {r0-r14}		/* Save the SVC mode regs */
 	ldr	r0, .Lirqtmp		/* Points to temp storage */
-	ldr	lr, [r0]		/* Recover lr */
-	ldr	r3, [r0, $4]		/* Recover SPSR */
-	add	r1, sp, #XCPTCONTEXT_UOFFSET
-	stmia	r1, {r3-r11, r13-r14}	/* Save SPSR+r4-r11+lr+sp */
+	ldmia	r0, {r1, r2}		/* Recover r1=lr_IRQ, r2=spsr_IRQ */
+	add	r0, sp, #(4*REG_PC)	/* Offset to pc, cpsr storage */
+	stmia	r0, {r1, r2}
 
 	/* Now decode the interrupt */
 
@@ -150,22 +149,11 @@ up_vectorirq:
 	mov	r1, sp			/* Get r1=xcp */
 	bl	up_prefetchabort	/* Call the handler */
 
-	/* Recover the SVC_MODE registers */
+	/* Restore the CPSR, SVC modr registers and return */
 .Lnoirqset:
-	add	r0, sp, #XCPTCONTEXT_UOFFSET
-	ldmia	r0, {r3-r11, r13-r14}
-	msr	spsr, r3
-	ldmia	sp, {r0-r3, r12}	/* recover volatile regs */
-	add	sp, sp, #XCPTCONTEXT_SIZE
-	movs	pc, lr			/* return & move spsr into cpsr */
-
-	@
-	@ now branch to the relevent MODE handling routine
-	@
-
-	and	lr, lr, #15
-	ldr	lr, [pc, lr, lsl #2]
-	movs	pc, lr				@ Changes mode and branches
+	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
+	msr	spsr, r0
+	ldmia	sp, {r0-r15}^		/* Return */
 
 .Lirqtmp:
 	.word	up_irqtmp
@@ -179,7 +167,6 @@ up_vectorirq:
  *   SWI interrupt. We enter the SWI in SVC mode
  ************************************************************/
 
-	.align	5
 	.global	up_vectorswi
 	.type	up_vectorswi, %function
 up_vectorswi:
@@ -195,10 +182,11 @@ up_vectorswi:
 	/* Create a context structure */
 
 	sub	sp, sp, #XCPTCONTEXT_SIZE
-	stmia	sp, {r0-r3, r12}	/* Save volatile regs */
-	mrs	r3, spsr		/* Get r3=interrupted CPSR */
-	add	r0, sp, #XCPTCONTEXT_UOFFSET
-	stmia	r0, {r3-r11, r13-r14}	/* Save CPSR+r4-r11+lr+sp */
+	stmia	sp, {r0-r14}		/* Save the SVC mode regs */
+	mrs	r2, spsr		/* Get the saved CPSR */
+	mov	r1, r14			/* Save r14 as the PC */
+	add	r0, sp, #(4*REG_PC)	/* Offset to pc, cpsr storage */
+	stmia	r0, {r1, r2}
 
 	/* Then call the SWI handler with interrupt disabled.
 	 * void up_syscall(struct xcptcontext *xcp)
@@ -208,15 +196,13 @@ up_vectorswi:
 	mov	r0, sp			/* Get r0=xcp */
 	bl	up_syscall		/* Call the handler */
 
-.LignoreSWI:
-	/* Recover the SVC_MODE registers */
+	/* Restore the CPSR, SVC modr registers and return */
 
-	add	r0, sp, #XCPTCONTEXT_UOFFSET
-	ldmia	r0, {r3-r11, r13-r14}
-	msr	spsr, r3
-	ldmia	sp, {r0-r3, r12}	/* recover volatile regs */
-	add	sp, sp, #XCPTCONTEXT_SIZE
-	movs	pc, lr			/* return & move spsr into cpsr */
+	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
+	msr	spsr, r0
+	ldmia	sp, {r0-r15}^		/* Return */
+
+	.align	5
 
 /************************************************************
  * Name: up_vectordata
@@ -228,7 +214,6 @@ up_vectorswi:
  *
  ************************************************************/
 	
-	.text
 	.global	up_vectordata
 	.type	up_vectordata, %function
 up_vectordata:
@@ -251,12 +236,11 @@ up_vectordata:
 	/* Create a context structure */
 
 	sub	sp, sp, #XCPTCONTEXT_SIZE
-	stmia	sp, {r0-r3, r12}	/* Save volatile regs */
+	stmia	sp, {r0-r14}		/* Save the SVC mode regs */
 	ldr	r0, .Ldaborttmp		/* Points to temp storage */
-	ldr	lr, [r0]		/* Recover lr */
-	ldr	r3, [r0, $4]		/* Recover SPSR */
-	add	r1, sp, #XCPTCONTEXT_UOFFSET
-	stmia	r1, {r3-r11, r13-r14}	/* Save SPSR+r4-r11+lr+sp */
+	ldmia	r0, {r1, r2}		/* Recover r1=lr_IRQ, r2=spsr_IRQ */
+	add	r0, sp, #(4*REG_PC)	/* Offset to pc, cpsr storage */
+	stmia	r0, {r1, r2}
 
 	/* Then call the data abort handler with interrupt disabled.
 	 * void up_dataabort(struct xcptcontext *xcp)
@@ -266,22 +250,11 @@ up_vectordata:
 	mov	r0, sp			/* Get r0=xcp */
 	bl	up_dataabort		/* Call the handler */
 
-	/* Recover the SVC_MODE registers */
+	/* Restore the CPSR, SVC modr registers and return */
 
-	add	r0, sp, #XCPTCONTEXT_UOFFSET
-	ldmia	r0, {r3-r11, r13-r14}
-	msr	spsr, r3
-	ldmia	sp, {r0-r3, r12}	/* recover volatile regs */
-	add	sp, sp, #XCPTCONTEXT_SIZE
-	movs	pc, lr			/* return & move spsr into cpsr */
-
-	@
-	@ now branch to the relevent MODE handling routine
-	@
-
-	and	lr, lr, #15
-	ldr	lr, [pc, lr, lsl #2]
-	movs	pc, lr				@ Changes mode and branches
+	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
+	msr	spsr, r0
+	ldmia	sp, {r0-r15}^		/* Return */
 
 .Ldaborttmp:
 	.word	up_aborttmp
@@ -318,12 +291,11 @@ up_vectorprefetch:
 	/* Create a context structure */
 
 	sub	sp, sp, #XCPTCONTEXT_SIZE
-	stmia	sp, {r0-r3, r12}	/* Save volatile regs */
+	stmia	sp, {r0-r14}		/* Save the SVC mode regs */
 	ldr	r0, .Lpaborttmp		/* Points to temp storage */
-	ldr	lr, [r0]		/* Recover lr */
-	ldr	r3, [r0, $4]		/* Recover SPSR */
-	add	r1, sp, #XCPTCONTEXT_UOFFSET
-	stmia	r1, {r3-r11, r13-r14}	/* Save SPSR+r4-r11+lr+sp */
+	ldmia	r0, {r1, r2}		/* Recover r1=lr_IRQ, r2=spsr_IRQ */
+	add	r0, sp, #(4*REG_PC)	/* Offset to pc, cpsr storage */
+	stmia	r0, {r1, r2}
 
 	/* Then call the data abort handler with interrupt disabled.
 	 * void up_prefetchabort(struct xcptcontext *xcp)
@@ -333,22 +305,11 @@ up_vectorprefetch:
 	mov	r0, sp			/* Get r0=xcp */
 	bl	up_prefetchabort	/* Call the handler */
 
-	/* Recover the SVC_MODE registers */
-
-	add	r0, sp, #XCPTCONTEXT_UOFFSET
-	ldmia	r0, {r3-r11, r13-r14}
-	msr	spsr, r3
-	ldmia	sp, {r0-r3, r12}	/* recover volatile regs */
-	add	sp, sp, #XCPTCONTEXT_SIZE
-	movs	pc, lr			/* return & move spsr into cpsr */
+	/* Restore the CPSR, SVC modr registers and return */
 
-	@
-	@ now branch to the relevent MODE handling routine
-	@
-
-	and	lr, lr, #15
-	ldr	lr, [pc, lr, lsl #2]
-	movs	pc, lr				@ Changes mode and branches
+	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
+	msr	spsr, r0
+	ldmia	sp, {r0-r15}^		/* Return */
 
 .Lpaborttmp:
 	.word	up_aborttmp
@@ -385,12 +346,11 @@ up_vectorundefinsn:
 	/* Create a context structure */
 
 	sub	sp, sp, #XCPTCONTEXT_SIZE
-	stmia	sp, {r0-r3, r12}	/* Save volatile regs */
+	stmia	sp, {r0-r14}		/* Save the SVC mode regs */
 	ldr	r0, .Lundeftmp		/* Points to temp storage */
-	ldr	lr, [r0]		/* Recover lr */
-	ldr	r3, [r0, $4]		/* Recover SPSR */
-	add	r1, sp, #XCPTCONTEXT_UOFFSET
-	stmia	r1, {r3-r11, r13-r14}	/* Save SPSR+r4-r11+lr+sp */
+	ldmia	r0, {r1, r2}		/* Recover r1=lr_IRQ, r2=spsr_IRQ */
+	add	r0, sp, #(4*REG_PC)	/* Offset to pc, cpsr storage */
+	stmia	r0, {r1, r2}
 
 	/* Then call the data abort handler with interrupt disabled.
 	 * void up_undefinedinsn(struct xcptcontext *xcp)
@@ -400,22 +360,11 @@ up_vectorundefinsn:
 	mov	r0, sp			/* Get r0=xcp */
 	bl	up_undefinedinsn	/* Call the handler */
 
-	/* Recover the SVC_MODE registers */
-
-	add	r0, sp, #XCPTCONTEXT_UOFFSET
-	ldmia	r0, {r3-r11, r13-r14}
-	msr	spsr, r3
-	ldmia	sp, {r0-r3, r12}	/* recover volatile regs */
-	add	sp, sp, #XCPTCONTEXT_SIZE
-	movs	pc, lr			/* return & move spsr into cpsr */
-
-	@
-	@ now branch to the relevent MODE handling routine
-	@
+	/* Restore the CPSR, SVC modr registers and return */
 
-	and	lr, lr, #15
-	ldr	lr, [pc, lr, lsl #2]
-	movs	pc, lr				@ Changes mode and branches
+	ldr	r0, [sp, #(4*REG_CPSR)]	/* Setup the SVC mode SPSR */
+	msr	spsr, r0
+	ldmia	sp, {r0-r15}^		/* Return */
 
 .Lundeftmp:
 	.word	up_undeftmp