1 files changed, 45 insertions, 70 deletions

diff --git a/nuttx/arch/pjrc-8051/src/up_savecontext.c b/nuttx/arch/pjrc-8051/src/up_savecontext.c
index 1eb24b704..9631c2a11 100644
--- a/nuttx/arch/pjrc-8051/src/up_savecontext.c
+++ b/nuttx/arch/pjrc-8051/src/up_savecontext.c
@@ -66,32 +66,40 @@
  **************************************************************************/
 
 /**************************************************************************
- * Name: up_pushcontext
+ * Public Functions
+ **************************************************************************/
+
+/**************************************************************************
+ * Name: up_savecontext
  *
  * Description:
- *   Push the current execution context onto the before the stack.  Similar
- *   operations are executed from the interrupt state save.
+ *   Push the current execution context onto the stack, then save the
+ *   entire stack contents in the provided context structure.
  *
  * Inputs:
- *   None
+ *   context - the context structure in which to save the stack info
  *
  * Return:
- *   Returns the stack pointer (always non-zero).  However, when
- *   up_popcontext() executes, it will appear as a return from this
- *   function with return value == 0
+ *   0 = Normal state save return
+ *   1 = This is the matching return from up_restorecontext()
  *
  **************************************************************************/
 
-static ubyte up_pushcontext(void) __naked
+ubyte up_savecontext(FAR struct xcptcontext *context) _naked
 {
-  /* Push the current execution context onto the stack. */
-
-  _asm
-	push	acc
+ _asm
+	/* Create the stack frame that we want when it is time to restore
+	 * this* context.  The return address will be the return address
+	 * of this function, the return value will be zero.
+	 */
+
+	clr	a
+	push	acc	/* ACC = 0 */
 	push	ie
-	mov	dptr, #0 /* Save return value (dpl) = 0 */
-	push	dpl
-	push	dph
+	mov	a, #1
+	push	acc	/* DPL = 1 */
+	clr	a
+	push	acc	/* DPH = 0 */
 	push	b
 	push	ar2
 	push	ar3
@@ -105,65 +113,33 @@ static ubyte up_pushcontext(void) __naked
 	clr	psw
 	push	_bp
 
-	/* And return the current stack pointer value in dpl */
-
-	mov	dpl, sp
-	ret
-  _endasm;
-}
+	/* Disable interrupts while we create a snapshot of the stack */
 
-/**************************************************************************
- * Public Functions
- **************************************************************************/
-
-/**************************************************************************
- * Name: up_savecontext
- *
- * Description:
- *   Push the current execution context onto the stack, then save the
- *   entire stack contents in the provided context structure.
- *
- * Inputs:
- *   context - the context structure in which to save the stack info
- *
- * Return:
- *   0 = Normal state save return
- *   1 = This is the matching return from up_restorecontext()
- *
- **************************************************************************/
-
-ubyte up_savecontext(FAR struct xcptcontext *context)
-{
-  irqstate_t flags = irqsave();
-  ubyte sp = up_pushcontext();
-  if (sp)
-    {
-      /* Now copy the current stack frame (including the saved execution
-       * context) from internal RAM to XRAM.
-       */
-
-      ubyte nbytes     = sp - (STACK_BASE-1);
-      NEAR ubyte *src  = (NEAR ubyte*)STACK_BASE;
-      FAR  ubyte *dest = context->stack;
+	push	ie
+	mov	ea, 0
 
-      /* Then copy the stack info into the context structure */
+	/* Now copy the current stack frame (including the saved execution
+	 * context) from internal RAM to XRAM.
+	 */
 
-      context->nbytes = nbytes;
-      while (nbytes--)
-        {
-          *dest++ = *src++;
-        }
+	push	sp
+	lcall	_up_savestack
+	pop	acc
 
-      /* Return zero so that the return behavior is similar to setjmp */
+	/* Restore the interrupt state */
 
-      irqrestore(flags);
-      return 0;
-    }
+	pop	ie
 
-  /* Return one so that the return behavior is similar to setjmp */
+	/* Now that we have a snapshot of the desired stack frame saved,
+	 * restore the correct stackpointer.
+	 */
 
-  irqrestore(flags);
-  return 1;
+	mov	a, sp
+	subb	a, #15
+	mov	sp, a
+	mov	dpl,#0
+	ret
+  _endasm;
 }
 
 /**************************************************************************
@@ -180,18 +156,17 @@ ubyte up_savecontext(FAR struct xcptcontext *context)
  *   None
  *
  * Assumptions:
- *   - We are in an interrupt handler with g_irqtos set
  *   - Interrupts are disabled
  *
  **************************************************************************/
 
-void up_savestack(FAR struct xcptcontext *context)
+void up_savestack(FAR struct xcptcontext *context, ubyte tos)
 {
   /* Now copy the current stack frame (including the saved execution
    * context) from internal RAM to XRAM.
    */
 
-  ubyte nbytes     = g_irqtos - (STACK_BASE-1);
+  ubyte nbytes     = tos - (STACK_BASE-1);
   NEAR ubyte *src  = (NEAR ubyte*)STACK_BASE;
   FAR  ubyte *dest = context->stack;