diff --git a/core/cortex-m/init.S b/core/cortex-m/init.S
index ac82530509..bd8d19a638 100644
--- a/core/cortex-m/init.S
+++ b/core/cortex-m/init.S
@@ -34,7 +34,7 @@ vector usage_fault    		  @ Usage fault handler
 .long 0               		  @ reserved
 .long 0               		  @ reserved
 .long 0               		  @ reserved
-vector svc            		  @ SWI
+.long svc_helper_handler	  @ SWI
 vector debug          		  @ Debug handler
 .long 0               		  @ reserved
 vector pendsv         		  @ PendSV handler
@@ -384,6 +384,30 @@ fini_loop:
 default_handler:
 	b exception_panic
 
+/*
+ * SVC handler helper
+ *
+ * Work around issue where a late exception can corrupt r0 to r3,
+ * see section 2.7 (svc) of Cortex-M3 Application Note 179:
+ * http://infocenter.arm.com/help/topic/com.arm.doc.dai0179b/AppsNote179.pdf
+ *
+ * This approach differs slightly from the one in the document,
+ * it only loads r0 (desched) and r1 (resched) for svc_handler.
+ */
+.thumb_func
+svc_helper_handler:
+	tst lr, #4		/* see if called from supervisor mode */
+	mrs r2, msp		/* get the correct stack pointer into r2 */
+	it ne
+	mrsne r2, psp
+	ldr r1, [r2, #4]	/* get regs from stack frame */
+	ldr r0, [r2]
+	b svc_handler		/* call svc_handler */
+
+/* Call default_handler if svc_handler is not found (task.c is not built) */
+.weak svc_handler
+.set svc_handler, default_handler
+
 .align 2
 _bss_start:
 .long __bss_start
diff --git a/core/cortex-m/task.c b/core/cortex-m/task.c
index 510e4cba32..a4417f4fff 100644
--- a/core/cortex-m/task.c
+++ b/core/cortex-m/task.c
@@ -263,6 +263,7 @@ void svc_handler(int desched, task_id_t resched)
 		 */
 		tasks_ready &= ~(1 << (current - tasks));
 	}
+	ASSERT(resched <= TASK_ID_COUNT);
 	tasks_ready |= 1 << resched;
 
 	ASSERT(tasks_ready);