diff --git a/nuttx/arch/c5471/include/irq.h b/nuttx/arch/c5471/include/irq.h
index c3bb57c29..1f9f262ff 100644
--- a/nuttx/arch/c5471/include/irq.h
+++ b/nuttx/arch/c5471/include/irq.h
@@ -60,50 +60,45 @@
  * xcp.regs array:
  */
 
-#define JB_R0                (0)
-#define JB_R1                (1)
-#define JB_R2                (2)
-#define JB_R3                (3)
-#define JB_R12               (4)
+#define REG_R0                   (0)
+#define REG_R1                   (1)
+#define REG_R2                   (2)
+#define REG_R3                   (3)
+#define REG_R4                   (4)
+#define REG_R5                   (5
+#define REG_R6                   (6)
+#define REG_R7                   (7)
+#define REG_R8                   (8)
+#define REG_R9                   (9)
+#define REG_R10                  (10)
+#define REG_R11                  (11)
+#define REG_R12                  (12)
+#define REG_R13                  (13)
+#define REG_R14                  (14)
+#define REG_R15                  (15)
+#define REG_CPSR                 (16)
 
-#define XCPTCONTEXT_IRQ_REGS (5)
-#define XCPTCONTEXT_UOFFSET  (4 * XCPTCONTEXT_IRQ_REGS)
+#define XCPTCONTEST_REGS         (17)
+#define XCPTCONTEXT_SIZE         (4 * XCPTCONTEST_REGS)
 
-#define JB_CPSR              (0 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R4                (1 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R5                (2 + XCPTCONTEXT_IRQ_REGS
-#define JB_R6                (3 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R7                (4 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R8                (5 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R9                (6 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R10               (7 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R11               (8 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R13               (9 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R14               (10 + XCPTCONTEXT_IRQ_REGS)
-#define JB_R15               /* Not saved */
-
-#define XCPTCONTEXT_USER_REG (11)
-#define XCPTCONTEST_REGS     (XCPTCONTEXT_USER_REG+XCPTCONTEXT_IRQ_REGS)
-#define XCPTCONTEXT_SIZE     (4 * XCPTCONTEST_REGS)
-
-#define JB_A1                JB_R0
-#define JB_A2                JB_R1
-#define JB_A3                JB_R2
-#define JB_A4                JB_R3
-#define JB_V1                JB_R4
-#define JB_V2                JB_R5
-#define JB_V3                JB_R6
-#define JB_V4                JB_R7
-#define JB_V5                JB_R8
-#define JB_V6                JB_R9
-#define JB_V7                JB_R10
-#define JB_SB                JB_R9
-#define JB_SL                JB_R10
-#define JB_FP                JB_R11
-#define JB_IP                JB_R12
-#define JB_SP                JB_R13
-#define JB_LR                JB_R14
-#define JB_PC                JB_R15
+#define REG_A1                   REG_R0
+#define REG_A2                   REG_R1
+#define REG_A3                   REG_R2
+#define REG_A4                   REG_R3
+#define REG_V1                   REG_R4
+#define REG_V2                   REG_R5
+#define REG_V3                   REG_R6
+#define REG_V4                   REG_R7
+#define REG_V5                   REG_R8
+#define REG_V6                   REG_R9
+#define REG_V7                   REG_R10
+#define REG_SB                   REG_R9
+#define REG_SL                   REG_R10
+#define REG_FP                   REG_R11
+#define REG_IP                   REG_R12
+#define REG_SP                   REG_R13
+#define REG_LR                   REG_R14
+#define REG_PC                   REG_R15
 
 /* C5471 Interrupts */
 
@@ -165,7 +160,7 @@ struct xcptcontext
    * signal processing.
    */
 
-  uint32 saved_lr;
+  uint32 saved_pc;
   uint32 saved_cpsr;
 
   /* Register save area */
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 all registers except for r0, r1, R15, and CPSR */
+
+	add	r1, r0, #(4*REG_R2)	/* Offset to REG_R2 storage */
+	ldmia	r1, {r2-r14}		/* Recover registers */
+
+	/* Create a stack frame to hold the PC */
+
+	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 */
+
+	/* 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.
+         */
+
+	ldr	r1, [r0, #(4*REG_CPSR)]	/* Fetch the stored CPSR value */
+	msr	cpsr, r1		/* Set the CPSR */
+
+	/* Now recover r0 and r1 */
+
+	ldr	r0, [sp, #8]
+	ldr	r1, [sp, #4]
+	add	sp, sp, #(2*4)
+
+	/* Then return to the address at the stop of the stack,
+	 * destroying the stack frame
 	 */
 
-	/* Recover the user context (we will then have a new stack pointer) */
-
-	add	r1, r0, #XCPTCONTEXT_UOFFSET
-	ldmia	r1, {r3-r11, r13-r14}
-
-	/* Save the CSPR value and one scratch register on the stack */
-
-	sub	sp, sp, #2*4		/* Create a frame to hold two regs */
-	stmia	sp, {r3, r4}		/* Save the CPSR (r3) and scratch (r4) */
-
-	/* Then recover the remaining registers */
-
-	ldmia	r0, {r0-r3, r12}	/* Recover volatile regs */
-
-	/* Now we can restore the CPSR (probably re-enabling interrupts) */
-
-	ldr	r4, [sp]
-	msr	cpsr, r4
-
-	/* Then recover the correct r4 value */
-
-	ldr	r4, [sp, #4]
-
-	/* Destroy the temporary stack frame and return */
-
-	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 */
-
-	add	r0, r0, #XCPTCONTEXT_UOFFSET
-
-	/* Get the current cpsr as well */
-
-	mrs	r3, cpsr	/* R3 = CPSR value */
-
-	/* 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.
+	/* Save the volatile registers (plus r12 which really
+	 * doesn't need to be saved)
 	 */
 
-	stmia	r0, {r3-r11, r13-r14}
+	add	r1, r0, #(4*REG_R4)
+	stmia	r1, {r4-r14}
+
+	/* Save the current cpsr */
+
+	mrs	r2, cpsr		/* R3 = CPSR value */
+	add	r1, r0, #(4*REG_CPSR)
+	str	r2, [r1]
+
+	/* Finally save the return address as the PC so that we
+	 * return to the exit from this function.
+	 */
+
+        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 */
+	/* 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 */
 
 .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 */
+	/* 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 */
 
 .Lundeftmp:
 	.word	up_undeftmp
diff --git a/nuttx/mm/mm_free.c b/nuttx/mm/mm_free.c
index 3363a46b0..7fb2a061e 100644
--- a/nuttx/mm/mm_free.c
+++ b/nuttx/mm/mm_free.c
@@ -115,7 +115,7 @@ void free(void *mem)
 
       node->size          += next->size;
       andbeyond->preceding =  node->size | (andbeyond->preceding & MM_ALLOC_BIT);
-      next                 = andbeyond;
+      next                 = (struct mm_freenode_s *)andbeyond;
     }
 
   /* Check if the preceding node is also free and, if so, merge
diff --git a/nuttx/sched/sem_internal.h b/nuttx/sched/sem_internal.h
index 460b358fc..dd15ae6df 100644
--- a/nuttx/sched/sem_internal.h
+++ b/nuttx/sched/sem_internal.h
@@ -87,7 +87,7 @@ extern "C" {
 #endif
 
 EXTERN void weak_function sem_initialize(void);
-EXTERN void               sem_waitirq(_TCB *wtcb);
+EXTERN void weak_function sem_waitirq(_TCB *wtcb);
 EXTERN nsem_t            *sem_findnamed(const char *name);
 
 #undef EXTERN