Basic 8052 context switching is working

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

14 files changed, 527 insertions, 222 deletions

diff --git a/nuttx/arch/pjrc-8051/sdcc-2.6.0.patch b/nuttx/arch/pjrc-8051/sdcc-2.6.0.patch
new file mode 100644
index 000000000..e518dab0a
--- /dev/null
+++ b/nuttx/arch/pjrc-8051/sdcc-2.6.0.patch
@@ -0,0 +1,11 @@
+--- sdcc/device/lib/Makefile.orig	2007-03-06 09:55:01.000000000 -0600
++++ sdcc/device/lib/Makefile	2007-03-06 09:58:32.000000000 -0600
+@@ -242,7 +242,7 @@
+ model-mcs51-stack-auto:
+ 	if [ "`grep mcs51 $(top_builddir)ports.build`" = mcs51 ]; then \
+ 	  for model in $(MODELS); do \
+-	    $(MAKE) MODELFLAGS="--model-$$model --stack-auto" PORT=$$model PORTDIR=$(BUILDDIR)/$$model-stack-auto PORTINCDIR=$(INCDIR)/mcs51 objects; \
++	    $(MAKE) MODELFLAGS="--model-$$model --stack-auto --int-long-reent --float-reent" PORT=$$model PORTDIR=$(BUILDDIR)/$$model-stack-auto PORTINCDIR=$(INCDIR)/mcs51 objects; \
+ 	  done \
+ 	fi
+ 
diff --git a/nuttx/arch/pjrc-8051/src/Makefile b/nuttx/arch/pjrc-8051/src/Makefile
index acd3746ec..b05d231f9 100644
--- a/nuttx/arch/pjrc-8051/src/Makefile
+++ b/nuttx/arch/pjrc-8051/src/Makefile
@@ -48,12 +48,12 @@ CSRCS		= up_initialize.c up_idle.c up_interruptcontext.c \
 		  up_releasepending.c up_reprioritizertr.c \
 		  up_exit.c up_assert.c up_allocateheap.c \
 		  up_irq.c up_savecontext.c up_restorecontext.c \
-		  up_timerisr.c up_putc.c
+		  up_timerisr.c up_putc.c up_debug.c
 COBJS		= $(CSRCS:.c=$(OBJEXT))
 SRCS		= $(SSRCS) $(CSRCS)
 OBJS		= $(AOBJS) $(COBJS)
 
-SDCCLIBDIR	= /usr/local/share/sdcc/lib/large
+SDCCLIBDIR	= /usr/local/share/sdcc/lib/large-stack-auto
 SDCCPATH	= -L$(SDCCLIBDIR)
 SDCCLIBS	= -llibfloat.lib -llibint.lib -lliblong.lib -llibmysdcc.lib -lmcs51.lib
 
@@ -66,7 +66,7 @@ LDLIBS		= $(addprefix -l,$(notdir $(LINKLIBS)))
 
 LDFLAGS		= --model-large --nostdlib \
 		  --data-loc 0x30 --iram-size 0x100 \
-		  --code-loc 0x20c0 --code-size 0x5f40 \
+		  --code-loc 0x2100 --code-size 0x5f40 \
 		  --xram-loc 0x0100 --xram-size 0x1f00
 
 DEPSRCS		= $(SRCS) $(LINKSSRCS)
diff --git a/nuttx/arch/pjrc-8051/src/up_assert.c b/nuttx/arch/pjrc-8051/src/up_assert.c
index 0234e9ecd..277494c42 100644
--- a/nuttx/arch/pjrc-8051/src/up_assert.c
+++ b/nuttx/arch/pjrc-8051/src/up_assert.c
@@ -42,6 +42,9 @@
 #include <stdlib.h>
 #include <assert.h>
 #include <debug.h>
+#include <8052.h>
+#include "up_internal.h"
+#include "up_mem.h"
 
 /************************************************************
  * Definitions
@@ -56,7 +59,7 @@
  ************************************************************/
 
 /************************************************************
- * Public Funtions
+ * Public Functions
  ************************************************************/
 
 /************************************************************
@@ -66,6 +69,7 @@
 void up_assert(void)
 {
   dbg("Assertion failed\n");
+  up_dumpstack();
   exit(EXIT_FAILURE);
 }
 
@@ -76,5 +80,6 @@ void up_assert(void)
 void up_assert_code(int errorcode)
 {
   dbg("Assertion failed with error code: %d\n", errorcode);
+  up_dumpstack();
   exit(errorcode);
 }
diff --git a/nuttx/arch/pjrc-8051/src/up_blocktask.c b/nuttx/arch/pjrc-8051/src/up_blocktask.c
index 8a74d0ef0..e8ea55523 100644
--- a/nuttx/arch/pjrc-8051/src/up_blocktask.c
+++ b/nuttx/arch/pjrc-8051/src/up_blocktask.c
@@ -131,7 +131,7 @@ void up_block_task(FAR _TCB *tcb, tstate_t task_state)
                * Just copy the current registers into the OLD rtcb.
                */
 
-               up_savestack(&tcb->xcp);
+               up_savestack(&tcb->xcp, g_irqtos);
 
               /* Restore the exception context of the rtcb at the (new) head 
                * of the g_readytorun task list.
@@ -140,9 +140,11 @@ void up_block_task(FAR _TCB *tcb, tstate_t task_state)
               rtcb = (FAR _TCB*)g_readytorun.head;
               dbg("New Active Task TCB=%p\n", rtcb);
 
-              /* Then switch contexts */
+              /* Then setup so that the context will be performed on exit
+               * from the interrupt.
+               */
 
-              up_restorestack(&tcb->xcp);
+              g_irqcontext = &rtcb->xcp;
             }
 
           /* Copy the user C context into the TCB at the (old) head of the
diff --git a/nuttx/arch/pjrc-8051/src/up_debug.c b/nuttx/arch/pjrc-8051/src/up_debug.c
new file mode 100644
index 000000000..6f59f488b
--- /dev/null
+++ b/nuttx/arch/pjrc-8051/src/up_debug.c
@@ -0,0 +1,162 @@
+/************************************************************
+ * up_assert.c
+ *
+ *   Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name Gregory Nutt nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <nuttx/config.h>
+#include <sys/types.h>
+#include <debug.h>
+#include <8052.h>
+#include <nuttx/arch.h>
+#include <arch/irq.h>
+#include "up_internal.h"
+#include "up_mem.h"
+
+/************************************************************
+ * Definitions
+ ************************************************************/
+
+/************************************************************
+ * Private Data
+ ************************************************************/
+
+/************************************************************
+ * Private Functions
+ ************************************************************/
+
+#ifdef CONFIG_FRAME_DUMP
+static void _up_puthex(ubyte hex) __naked
+{
+  hex; /* To avoid unreferenced argument warning */
+  _asm
+        mov     a, dpl
+        ljmp    PM2_ENTRY_PHEX
+  _endasm;
+}
+
+static void _up_putspace(void) __naked
+{
+  _asm
+        mov     a, #0x20
+        ljmp    PM2_ENTRY_COUT
+  _endasm;
+}
+
+static void _up_putcolon(void) __naked
+{
+  _asm
+        mov     a, #0x3a
+        lcall	PM2_ENTRY_COUT
+  _endasm;
+}
+
+static void _up_putnl(void) __naked
+{
+  _asm
+	ljmp	PM2_ENTRY_NEWLINE
+  _endasm;
+}
+
+static void _up_puts(__code char *ptr)
+{
+  for (; *ptr; ptr++)
+    {
+       up_putc(*ptr);
+    }
+}
+
+static void _up_dump16(__code char *ptr, ubyte msb, ubyte lsb)
+{
+  _up_puts(ptr);
+  _up_puthex(msb);
+  _up_puthex(lsb);
+  _up_putnl();
+}
+
+static void _up_dump8(__code char *ptr, ubyte b)
+{
+  _up_puts(ptr);
+  _up_puthex(b);
+  _up_putnl();
+}
+#endif
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: up_dumpstack
+ ************************************************************/
+
+#ifdef CONFIG_FRAME_DUMP
+void up_dumpstack(void)
+{
+  NEAR ubyte *start = (NEAR ubyte *)(UP_STACK_BASE & 0xf0);
+  NEAR ubyte *end   = (NEAR ubyte *)SP;
+  ubyte i;
+
+  while (start < end)
+    {
+      _up_puthex((ubyte)start);
+      _up_putcolon();
+
+      for (i = 0; i < 8; i++)
+        {
+          _up_putspace();
+          _up_puthex(*start);
+          start++;
+        }
+      _up_putnl();
+    }
+}
+#endif
+
+/************************************************************
+ * Name: up_dumpframe
+ ************************************************************/
+
+#ifdef CONFIG_FRAME_DUMP
+void up_dumpframe(FAR struct xcptcontext *context)
+{
+  FAR ubyte *start = &context->stack[context->nbytes - FRAME_SIZE];
+  _up_dump16(" RET  ", start[FRAME_RETMS], start[FRAME_RETLS]);
+  _up_dump8(" IE   ", start[FRAME_IE]);
+  _up_dump16(" DPTR ", start[FRAME_DPH], start[FRAME_DPL]);
+  _up_dump8(" PSW  ", start[FRAME_PSW]);
+}
+#endif
diff --git a/nuttx/arch/pjrc-8051/src/up_head.S b/nuttx/arch/pjrc-8051/src/up_head.S
index 662b6b18e..35acea2db 100644
--- a/nuttx/arch/pjrc-8051/src/up_head.S
+++ b/nuttx/arch/pjrc-8051/src/up_head.S
@@ -55,6 +55,7 @@
  ************************************************************/
 
 	.globl	_g_irqtos
+	.globl	_g_irqcontext
 
 /************************************************************
  * Public Functions
@@ -167,18 +168,36 @@ _up_interrupt:
 	clr	psw
 	push	_bp
 
-	/* Mark that we are in an interrupt */
+	/* Save the IRQ number in r3 */
+
+	mov	r2, a
+
+	/* Mark that we are in an interrupt and provide the top
+	 * of stack pointer to the context switching logic.
+	 */
 
 	mov	dptr, #_g_irqtos
+	mov	a, sp
         movx    @dptr, a
 
+	/* Nullify the context pointer.  If a context switch is
+	 * needed, this will be set to the address of the context
+	 * structure.
+	 */
+
+	mov	dptr,#_g_irqcontext
+	clr	a
+	movx	@dptr,a
+	inc	dptr
+	movx	@dptr,a
+
 	/* Now call void irq_dispatch(int irq, FAR void *context)
          *
 	 * First, create the first argument as (int)irqno
 	 */
 
-        mov     dpl, a
-	clr	dph
+        mov     dpl, r2
+	mov	dph, #0
 
         /* Create the second argument (void *context) on the stack */
 
@@ -186,30 +205,120 @@ _up_interrupt:
 	clr	a
 	push	acc
 
-	/* Then dispatch the IRQ.  On return, the stack is off by
-	 * by two because the above two pushes, but we fix that by
-	 * reloading sp from g_irqtos below.
-	 */
+	/* Then dispatch the IRQ. */
 
 	lcall	_irq_dispatch
+	pop	acc
+	pop	acc
 
-	/* Get the stackpointer.  This might be the stack pointer that
-	 * we increment above or it might be the stack pointer of a
-	 * new task if the a context switch happened during the
-	 * interrupt handling.
-	 */
+	/* Indicate that we are no longer in an interrupt */
 
 	mov	dptr, #_g_irqtos
+	clr	a
+        movx    @dptr, a
+
+	/* Check if a context switch is pending */
+
+	mov	dptr,#_g_irqcontext
 	movx	a, @dptr
-	mov	sp, a
+	mov     r2, a
+	inc	dptr
+	movx	a, @dptr
+	mov	r3, a
 
-	/* Indicate that we are no longer in an interrupt */
+        orl     a, r2
+        jz      00003$
+
+	/* A conext switch is pending Clear g_irqcontext */
 
+	mov	dpl, r2
+	mov	dph, r3
 	clr	a
 	movx	@dptr, a
+	inc	dptr
+	movx	@dptr, a
+
+#ifdef CONFIG_INTERRUPT_FRAME_DUMP
+	mov	dpl, r2
+	mov	dph, r3
+	push	ar2
+	push	ar3
+	lcall	_up_dumpframe
+	pop	ar3
+	pop	ar2
+#endif
+
+	/* Register usage in the following:
+	 *
+	 * R0   - Holds working the 8-bit IRAM pointer
+	 * R1   - Not used
+	 * R2-3 - Holds the working 16-bit XRAM pointer
+	 * R4   - Holds the working byte count
+	 * R5   - Holds the new stack pointer
+	 * R6-7 - Not used
+	 */
 
-	/* Then return from the interrupt */
+	/* Fetch r4 = context->nbytes */
 
+	mov	dpl, r2
+	mov	dph, r3
+	movx	a, @dptr
+	mov	r4, a
+
+	/* Save the new stack pointer in r5 */
+
+	add	a, #(STACK_BASE-1)
+	mov	r5, a
+
+	/* Save r2-3 = &context->stack */
+
+	inc	dptr
+	mov	r2, dpl
+	mov	r3, dph
+
+	/* Set r0 = stack base address */
+
+	mov	r0, #STACK_BASE
+
+	/* Top of the copy loop */
+00001$:
+	dec	r4
+	jz	00002$
+
+	/* Fetch the next byte from context->stack */
+
+	mov	dpl, r2
+	mov	dph, r3
+	movx	a,@dptr
+
+	/* Increment the XRAM pointer */
+
+	inc	dptr
+	mov	r2, dpl
+	mov	r3, dph
+
+	/* Save the next byte into IRAM */
+
+	mov	@r0, a
+
+	/* Increment the IRAM pointer */
+
+	inc	r0
+	sjmp	00001$
+
+	/* Set the new stack pointer */
+
+00002$:
+	mov	sp, r5
+
+#ifdef CONFIG_INTERRUPT_FRAME_DUMP
+	lcall	_up_dumpstack
+#endif
+	/* Then restore the context from the stack and return
+	 * from the interrupt
+	 */
+
+00003$:
 	pop	_bp 
 	pop	psw
 	pop	ar1 
@@ -223,18 +332,6 @@ _up_interrupt:
 	pop	b
 	pop	dph
 	pop	dpl
-
-	/* Restore the interrupt state per the stored IE value */
-
+	pop	ie
 	pop	acc
-	jb	acc.7, 00001$
-	clr	ie.7
-	sjmp	00002$
-  00001$:
-	setb	ie.7
-  00002$:
-
-	/* Finally pop off the ACC, which was the first register saved. */
-
-	pop acc
 	reti
diff --git a/nuttx/arch/pjrc-8051/src/up_initialize.c b/nuttx/arch/pjrc-8051/src/up_initialize.c
index 5388882da..c8f2ef01d 100644
--- a/nuttx/arch/pjrc-8051/src/up_initialize.c
+++ b/nuttx/arch/pjrc-8051/src/up_initialize.c
@@ -51,14 +51,24 @@
  * Private Data
  ************************************************************/
 
-/* This is the top of the stack containing the interrupt stack frame.  It
- * is set when processing an interrupt.  It is also cleared when the
- * interrupt returns so this can also be used like a boolean indication that
- * we are in an interrupt.
+/* This is the top of the stack containing the interrupt
+ * stack frame.  It is set when processing an interrupt.  It
+ * is also cleared when the interrupt returns so this can
+ * also be used like a boolean indication that we are in an
+ * interrupt.
  */
 
 ubyte g_irqtos;
 
+/* If during execution of an interrup handler, a context
+ * switch must be performed, the follwing will be set to
+ * to that address of the relevant context structure.  The
+ * actual switch will be deferred until the time that the
+ * the interrupt exits.
+ */
+
+FAR struct xcptcontext *g_irqcontext;
+
 /************************************************************
  * Private Functions
  ************************************************************/
@@ -105,7 +115,9 @@ void up_initialize(void)
 
   /* Initialize the system timer interrupt */
 
+#ifndef CONFIG_SUPPRESS_INTERRUPTS
   up_timerinit();
+#endif
 
   /* Initialize the serial console support */
 }
diff --git a/nuttx/arch/pjrc-8051/src/up_internal.h b/nuttx/arch/pjrc-8051/src/up_internal.h
index 25fa06243..8f27179cb 100644
--- a/nuttx/arch/pjrc-8051/src/up_internal.h
+++ b/nuttx/arch/pjrc-8051/src/up_internal.h
@@ -44,6 +44,13 @@
  * Public Definitions
  **************************************************************************/
 
+/* Bring-up debug configurations */
+
+#define CONFIG_FRAME_DUMP 1             /* Enabled stack/frame dumping logic */
+#define CONFIG_SUPPRESS_INTERRUPTS 1    /* Do not enable interrupts */
+#define CONFIG_SWITCH_FRAME_DUMP 1      /* Dump frames from normal switches */
+#undef  CONFIG_INTERRUPT_FRAME_DUMP     /* Dump frames from interrupt switches */
+
 /* Memory Map
  *
  * BEGIN  END     DESCRIPTION
@@ -161,14 +168,24 @@ sfr at 0xc9 T2MOD ;
 
 #ifndef __ASSEMBLY__
 
-/* This is the top of the stack containing the interrupt stack frame.  It
- * is set when processing an interrupt.  It is also cleared when the
- * interrupt returns so this can also be used like a boolean indication that
- * we are in an interrupt.
+/* This is the top of the stack containing the interrupt
+ * stack frame.  It is set when processing an interrupt.  It
+ * is also cleared when the interrupt returns so this can
+ * also be used like a boolean indication that we are in an
+ * interrupt.
  */
 
 extern ubyte g_irqtos;
 
+/* If during execution of an interrup handler, a context
+ * switch must be performed, the follwing will be set to
+ * to that address of the relevant context structure.  The
+ * actual switch will be deferred until the time that the
+ * the interrupt exits.
+ */
+
+extern FAR struct xcptcontext *g_irqcontext;
+
 #endif /* __ASSEMBLY */
 
 /**************************************************************************
@@ -181,11 +198,18 @@ extern ubyte g_irqtos;
 extern void  up_addregion(void);
 #endif
 extern void  up_irqinitialize(void);
-extern void  up_restorecontext(FAR struct xcptcontext *context);
-extern void  up_restorestack(FAR struct xcptcontext *context);
-extern ubyte up_savecontext(FAR struct xcptcontext *context);
-extern void  up_savestack(FAR struct xcptcontext *context);
+extern void  up_restorecontext(FAR struct xcptcontext *context) _naked;
+extern ubyte up_savecontext(FAR struct xcptcontext *context) __naked;
+extern void  up_savestack(FAR struct xcptcontext *context, ubyte tos);
 extern void  up_timerinit(void);
 
+#ifdef CONFIG_FRAME_DUMP
+extern void up_dumpstack(void);
+extern void up_dumpframe(FAR struct xcptcontext *context);
+#else
+# define up_dumpstack()
+# define up_dumpframe(x)
+#endif
+
 #endif /* __ASSEMBLY */
 #endif /* __ARCH_UP_INTERNAL_H */
diff --git a/nuttx/arch/pjrc-8051/src/up_irq.c b/nuttx/arch/pjrc-8051/src/up_irq.c
index e9f2c2a13..8d024ca3a 100644
--- a/nuttx/arch/pjrc-8051/src/up_irq.c
+++ b/nuttx/arch/pjrc-8051/src/up_irq.c
@@ -69,7 +69,9 @@
 
 void up_irqinitialize(void)
 {
-#if 1 /* remove me */
+#ifndef CONFIG_SUPPRESS_INTERRUPTS
+  /* Disable all interrupts */
+
   IE = 0;
 #else
   /* Enable interrupts globally, but disable all interrupt
diff --git a/nuttx/arch/pjrc-8051/src/up_releasepending.c b/nuttx/arch/pjrc-8051/src/up_releasepending.c
index 64650bc89..dfab2ecb5 100644
--- a/nuttx/arch/pjrc-8051/src/up_releasepending.c
+++ b/nuttx/arch/pjrc-8051/src/up_releasepending.c
@@ -94,7 +94,7 @@ void up_release_pending(void)
            * Just copy the current registers into the OLD rtcb.
            */
 
-           up_savestack(&rtcb->xcp);
+           up_savestack(&rtcb->xcp, g_irqtos);
 
           /* Restore the exception context of the rtcb at the (new) head 
            * of the g_readytorun task list.
@@ -103,9 +103,11 @@ void up_release_pending(void)
           rtcb = (FAR _TCB*)g_readytorun.head;
           dbg("New Active Task TCB=%p\n", rtcb);
 
-          /* Then switch contexts */
+          /* Then setup so that the context will be performed on exit
+           * from the interrupt.
+           */
 
-          up_restorestack(&rtcb->xcp);
+          g_irqcontext = &rtcb->xcp;
         }
 
       /* Copy the exception context into the TCB of the task that
diff --git a/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c b/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c
index 39c1cf508..6aa1da68a 100644
--- a/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c
+++ b/nuttx/arch/pjrc-8051/src/up_reprioritizertr.c
@@ -142,7 +142,7 @@ void up_reprioritize_rtr(FAR _TCB *tcb, ubyte priority)
                * Just copy the current registers into the OLD rtcb.
                */
 
-               up_savestack(&tcb->xcp);
+               up_savestack(&tcb->xcp, g_irqtos);
 
               /* Restore the exception context of the rtcb at the (new) head 
                * of the g_readytorun task list.
@@ -151,9 +151,11 @@ void up_reprioritize_rtr(FAR _TCB *tcb, ubyte priority)
               rtcb = (FAR _TCB*)g_readytorun.head;
               dbg("New Active Task TCB=%p\n", rtcb);
 
-              /* Then switch contexts */
+              /* Then setup so that the context will be performed on exit
+               * from the interrupt.
+               */
 
-              up_restorestack(&tcb->xcp);
+              g_irqcontext = &tcb->xcp;
             }
 
           /* Copy the exception context into the TCB at the (old) head of the
diff --git a/nuttx/arch/pjrc-8051/src/up_restorecontext.c b/nuttx/arch/pjrc-8051/src/up_restorecontext.c
index 98b4d0bb4..ce5735db9 100644
--- a/nuttx/arch/pjrc-8051/src/up_restorecontext.c
+++ b/nuttx/arch/pjrc-8051/src/up_restorecontext.c
@@ -65,25 +65,116 @@
  **************************************************************************/
 
 /**************************************************************************
- * Name: up_popcontext
+ * Public Functions
+ **************************************************************************/
+
+/**************************************************************************
+ * Name: up_restorecontext
  *
  * Description:
- *   Pop the current execution context from the stack and return to the
- *   execution context.  Similar operations are executed from the interrupt
- *   state restore
+ *   Restore the stack specified in the context structure and return to
+ *   that context
  *
  * Inputs:
- *   None
+ *   context - Holds the stack content of the context to return to
  *
  * Return:
- *   This function does not return
+ *   This function does not return.
  *
  **************************************************************************/
 
-static void up_popcontext(ubyte newsp) __naked
-
+void up_restorecontext(FAR struct xcptcontext *context) __naked
 {
-  _asm 
+  _asm
+	ar2 = 0x02
+	ar3 = 0x03
+	ar4 = 0x04
+	ar5 = 0x05
+	ar6 = 0x06
+	ar7 = 0x07
+	ar0 = 0x00
+	ar1 = 0x01
+
+#ifdef CONFIG_SWITCH_FRAME_DUMP
+	push	dpl
+	push	dph
+	lcall	_up_dumpframe
+	pop	dph
+	pop	dpl
+#endif
+
+	/* Interrupts should be disabled for the following.  up_popcontext() will
+	 * set the new interrupt state correctly.
+	 */
+
+	clr	ea
+
+	/* Register usage in the following:
+	 *
+	 * R0   - Holds working the 8-bit IRAM pointer
+	 * R1   - Not used
+	 * R2-3 - Holds the working 16-bit XRAM pointer
+	 * R4   - Holds the working byte count
+	 * R5   - Holds the new stack pointer
+	 * R6-7 - Not used
+	 */
+
+	/* Fetch r4 = context->nbytes */
+
+	movx	a, @dptr
+	mov	r4, a
+
+	/* Save the new stack pointer in r5 */
+
+	add	a, #(STACK_BASE-1)
+	mov	r5, a
+
+	/* Save r2-3 = &context->stack */
+
+	inc	dptr
+	mov	r2, dpl
+	mov	r3, dph
+
+	/* Set r0 = stack base address */
+
+	mov	r0, #STACK_BASE
+
+	/* Top of the copy loop */
+00001$:
+	dec	r4
+	jz	00002$
+
+	/* Fetch the next byte from context->stack */
+
+	mov	dpl, r2
+	mov	dph, r3
+	movx	a,@dptr
+
+	/* Increment the XRAM pointer */
+
+	inc	dptr
+	mov	r2, dpl
+	mov	r3, dph
+
+	/* Save the next byte into IRAM */
+
+	mov	@r0, a
+
+	/* Increment the IRAM pointer */
+
+	inc	r0
+	sjmp	00001$
+00002$:
+
+	/* Set the new stack pointer */
+
+	mov	sp, r5
+
+#ifdef CONFIG_SWITCH_FRAME_DUMP
+	lcall	_up_dumpstack
+#endif
+	/* Then restore the context from the stack */
+
 	pop	_bp 
 	pop	psw
 	pop	ar1 
@@ -101,98 +192,16 @@ static void up_popcontext(ubyte newsp) __naked
 	/* Restore the interrupt state per the stored IE value */
 
 	pop	acc
-	jb	acc.7,00001$
+	jb	acc.7,00003$
 	clr	ie.7
-	sjmp	00002$
-  00001$:
+	sjmp	00004$
+  00003$:
 	setb	ie.7
-  00002$:
+  00004$:
 
 	pop acc
 	ret
   _endasm;
 }
 
-/**************************************************************************
- * Public Functions
- **************************************************************************/
-
-/**************************************************************************
- * Name: up_restorecontext
- *
- * Description:
- *   Restore the stack specified in the context structure and return to
- *   that context
- *
- * Inputs:
- *   context - Holds the stack content of the context to return to
- *
- * Return:
- *   This function does not return.
- *
- **************************************************************************/
-
-void up_restorecontext(FAR struct xcptcontext *context)
-{
-  int nbytes       = context->nbytes;
-  FAR  ubyte *src  = context->stack;
-  NEAR ubyte *dest = (NEAR ubyte*)STACK_BASE;
-
-  /* Interrupts should be disabled for the following.  up_popcontext() will
-   * set the new interrupt state correctly.
-   */
-
-  (void)irqsave();
-
-  while (nbytes--)
-    {
-      *src++ = *dest++;
-    }
-
-  /* Then return to the restored context (probably restoring interrupts) */
-
-  up_popcontext(context->nbytes + (STACK_BASE-1));
-}
-
-/**************************************************************************
- * Name: up_restorestack
- *
- * Description:
- *   Restore the entire interrupt stack contents in the provided context
- *   structure.
- *
- * Inputs:
- *   context - the context structure from  which to restore the stack info
- *
- * Return:
- *   None
- *
- * Assumptions:
- *   - We are in an interrupt handler with g_irqtos set
- *   - Interrupts are disabled
- *
- **************************************************************************/
-
-void up_restorestack(FAR struct xcptcontext *context)
-{
-  /* Now copy the current stack frame (including the saved execution
-   * context) from internal RAM to XRAM.
-   */
-
-  ubyte nbytes      = context->nbytes;
-  FAR  ubyte *src   = context->stack;
-  NEAR ubyte *dest  = (NEAR ubyte*)STACK_BASE;
-
-  while (nbytes--)
-    {
-      *dest++ = *src++;
-    }
-
-  /* We are still in the interrupt context, but the size of the interrupt
-   * stack has changed.
-   */
-
-  g_irqtos = context->nbytes + (STACK_BASE-1);
-}
-
 
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;
 
diff --git a/nuttx/arch/pjrc-8051/src/up_unblocktask.c b/nuttx/arch/pjrc-8051/src/up_unblocktask.c
index 1db56f3b7..3f09d7ce5 100644
--- a/nuttx/arch/pjrc-8051/src/up_unblocktask.c
+++ b/nuttx/arch/pjrc-8051/src/up_unblocktask.c
@@ -122,7 +122,7 @@ void up_unblock_task(FAR _TCB *tcb)
                * Just copy the current stack into the OLD rtcb.
                */
 
-               up_savestack(&rtcb->xcp);
+               up_savestack(&rtcb->xcp, g_irqtos);
 
               /* Restore the exception context of the rtcb at the (new) head 
                * of the g_readytorun task list.
@@ -131,9 +131,11 @@ void up_unblock_task(FAR _TCB *tcb)
               rtcb = (FAR _TCB*)g_readytorun.head;
               dbg("New Active Task TCB=%p\n", rtcb);
 
-              /* Then switch contexts */
+              /* Then setup so that the context will be performed on exit
+               * from the interrupt.
+               */
 
-              up_restorestack(&rtcb->xcp);
+              g_irqcontext = &rtcb->xcp;
             }
 
          /* We are not in an interrupt andler.  Copy the user C context