diff --git a/docs/reports/STM32F407-168-IAR.txt b/docs/reports/STM32F407-168-IAR.txt
index c703d5f59..c80e1c981 100644
--- a/docs/reports/STM32F407-168-IAR.txt
+++ b/docs/reports/STM32F407-168-IAR.txt
@@ -7,7 +7,7 @@ Compiler: IAR C/C++ Compiler for ARM 6.30.3.33228
 *** ChibiOS/RT test suite
 ***
 *** Kernel:       2.3.5unstable
-*** Compiled:     Dec 27 2011 - 15:57:01
+*** Compiled:     Dec 28 2011 - 10:01:37
 *** Compiler:     IAR
 *** Architecture: ARMv7-ME
 *** Core Variant: Cortex-M4
diff --git a/os/ports/GCC/ARMCMx/chcore_v6m.c b/os/ports/GCC/ARMCMx/chcore_v6m.c
index fa5b6b8c3..933af5e16 100644
--- a/os/ports/GCC/ARMCMx/chcore_v6m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v6m.c
@@ -133,7 +133,9 @@ __attribute__((naked))
 #endif
 void _port_switch_from_isr(void) {
 
+  dbg_check_lock();
   chSchDoReschedule();
+  dbg_check_unlock();
   asm volatile ("_port_exit_from_isr:" : : : "memory");
 #if CORTEX_ALTERNATE_SWITCH
   SCB_ICSR = ICSR_PENDSVSET;
diff --git a/os/ports/IAR/ARMCMx/chcore.h b/os/ports/IAR/ARMCMx/chcore.h
index 9b3f466c0..e1fc20a83 100644
--- a/os/ports/IAR/ARMCMx/chcore.h
+++ b/os/ports/IAR/ARMCMx/chcore.h
@@ -206,6 +206,23 @@ struct intctx {};
 
 #endif /* defined(__DOXYGEN__) */
 
+/**
+ * @brief   Excludes the default @p chSchIsPreemptionRequired()implementation.
+ */
+#define PORT_OPTIMIZED_ISPREEMPTIONREQUIRED
+
+#if (CH_TIME_QUANTUM > 0) || defined(__DOXYGEN__)
+/**
+ * @brief   Inlineable version of this kernel function.
+ */
+#define chSchIsPreemptionRequired()                                         \
+  (rlist.r_preempt ? firstprio(&rlist.r_queue) > currp->p_prio :            \
+                     firstprio(&rlist.r_queue) >= currp->p_prio)
+#else /* CH_TIME_QUANTUM == 0 */
+#define chSchIsPreemptionRequired()                                         \
+  (firstprio(&rlist.r_queue) > currp->p_prio)
+#endif /* CH_TIME_QUANTUM == 0 */
+
 #endif /* _FROM_ASM_ */
 
 #endif /* _CHCORE_H_ */
diff --git a/os/ports/IAR/ARMCMx/chcore_v6m.c b/os/ports/IAR/ARMCMx/chcore_v6m.c
index 9df844baa..69edf60e8 100644
--- a/os/ports/IAR/ARMCMx/chcore_v6m.c
+++ b/os/ports/IAR/ARMCMx/chcore_v6m.c
@@ -103,8 +103,20 @@ void _port_irq_epilogue(regarm_t lr) {
     ctxp = (struct extctx *)__get_PSP();
     ctxp--;
     __set_PSP((unsigned long)ctxp);
-    ctxp->pc = (regarm_t)_port_switch_from_isr;
     ctxp->xpsr = (regarm_t)0x01000000;
+
+    /* The exit sequence is different depending on if a preemption is
+       required or not.*/
+    if (chSchIsPreemptionRequired()) {
+      /* Preemption is required we need to enforce a context switch.*/
+      ctxp->pc = (regarm_t)_port_switch_from_isr;
+    }
+    else {
+      /* Preemption not required, we just need to exit the exception
+         atomically.*/
+      ctxp->pc = (regarm_t)_port_exit_from_isr;
+    }
+
     /* Note, returning without unlocking is intentional, this is done in
        order to keep the rest of the context switching atomic.*/
   }
diff --git a/os/ports/IAR/ARMCMx/chcore_v6m.h b/os/ports/IAR/ARMCMx/chcore_v6m.h
index 75f6c84da..c4da68ed8 100644
--- a/os/ports/IAR/ARMCMx/chcore_v6m.h
+++ b/os/ports/IAR/ARMCMx/chcore_v6m.h
@@ -352,32 +352,14 @@ struct context {
 }
 #endif
 
-#if 0
-/**
- * @brief   Excludes the default @p chSchIsPreemptionRequired()implementation.
- */
-#define PORT_OPTIMIZED_ISPREEMPTIONREQUIRED
-
-#if (CH_TIME_QUANTUM > 0) || defined(__DOXYGEN__)
-/**
- * @brief   Inlineable version of this kernel function.
- */
-#define chSchIsPreemptionRequired()                                         \
-  (rlist.r_preempt ? firstprio(&rlist.r_queue) > currp->p_prio :            \
-                     firstprio(&rlist.r_queue) >= currp->p_prio)
-#else /* CH_TIME_QUANTUM == 0 */
-#define chSchIsPreemptionRequired()                                         \
-  (firstprio(&rlist.r_queue) > currp->p_prio)
-#endif /* CH_TIME_QUANTUM == 0 */
-#endif
-
 #ifdef __cplusplus
 extern "C" {
 #endif
   void port_halt(void);
-  void _port_switch(Thread *ntp, Thread *otp);
   void _port_irq_epilogue(regarm_t lr);
   void _port_switch_from_isr(void);
+  void _port_exit_from_isr(void);
+  void _port_switch(Thread *ntp, Thread *otp);
   void _port_thread_start(void);
 #ifdef __cplusplus
 }
diff --git a/os/ports/IAR/ARMCMx/chcore_v7m.c b/os/ports/IAR/ARMCMx/chcore_v7m.c
index ab227efa6..1de863c06 100644
--- a/os/ports/IAR/ARMCMx/chcore_v7m.c
+++ b/os/ports/IAR/ARMCMx/chcore_v7m.c
@@ -171,7 +171,6 @@ void _port_irq_epilogue(void) {
     else {
       /* Preemption not required, we just need to exit the exception
          atomically.*/
-      void _port_exit_from_isr(void);
       ctxp->pc = (regarm_t)_port_exit_from_isr;
     }
 
diff --git a/os/ports/IAR/ARMCMx/chcore_v7m.h b/os/ports/IAR/ARMCMx/chcore_v7m.h
index b6c44d4ea..a99b3a755 100644
--- a/os/ports/IAR/ARMCMx/chcore_v7m.h
+++ b/os/ports/IAR/ARMCMx/chcore_v7m.h
@@ -469,31 +469,15 @@ struct context {
 }
 #endif
 
-/**
- * @brief   Excludes the default @p chSchIsPreemptionRequired()implementation.
- */
-#define PORT_OPTIMIZED_ISPREEMPTIONREQUIRED
-
-#if (CH_TIME_QUANTUM > 0) || defined(__DOXYGEN__)
-/**
- * @brief   Inlineable version of this kernel function.
- */
-#define chSchIsPreemptionRequired()                                         \
-  (rlist.r_preempt ? firstprio(&rlist.r_queue) > currp->p_prio :            \
-                     firstprio(&rlist.r_queue) >= currp->p_prio)
-#else /* CH_TIME_QUANTUM == 0 */
-#define chSchIsPreemptionRequired()                                         \
-  (firstprio(&rlist.r_queue) > currp->p_prio)
-#endif /* CH_TIME_QUANTUM == 0 */
-
 #ifdef __cplusplus
 extern "C" {
 #endif
   void port_halt(void);
   void _port_init(void);
-  void _port_switch(Thread *ntp, Thread *otp);
   void _port_irq_epilogue(void);
   void _port_switch_from_isr(void);
+  void _port_exit_from_isr(void);
+  void _port_switch(Thread *ntp, Thread *otp);
   void _port_thread_start(void);
 #ifdef __cplusplus
 }
diff --git a/os/ports/IAR/ARMCMx/chcoreasm_v6m.s b/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
index fb1cb0fb7..38f0f0bc5 100644
--- a/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
+++ b/os/ports/IAR/ARMCMx/chcoreasm_v6m.s
@@ -37,7 +37,6 @@ SCB_ICSR        SET     0xE000ED04
         SECTION .text:CODE:NOROOT(2)
 
         EXTERN  chThdExit
-        EXTERN  chSchIsPreemptionRequired
         EXTERN  chSchDoReschedule
 #if CH_DBG_SYSTEM_STATE_CHECK
         EXTERN  dbg_check_unlock
@@ -87,18 +86,16 @@ _port_thread_start:
  * Exception handlers return here for context switching.
  */
         PUBLIC  _port_switch_from_isr
+        PUBLIC  _port_exit_from_isr
 _port_switch_from_isr:
 #if CH_DBG_SYSTEM_STATE_CHECK
         bl      dbg_check_lock
 #endif
-        bl      chSchIsPreemptionRequired
-        cmp     r0, #0
-        beq     noresch
         bl      chSchDoReschedule
-noresch:
 #if CH_DBG_SYSTEM_STATE_CHECK
         bl      dbg_check_unlock
 #endif
+_port_exit_from_isr:
         ldr     r2, =SCB_ICSR
         movs    r3, #128
 #if CORTEX_ALTERNATE_SWITCH