diff --git a/demos/ARM7-LPC214x-GCC/chcore.c b/demos/ARM7-LPC214x-GCC/chcore.c
index 5313ecdb4..8b9b4d464 100644
--- a/demos/ARM7-LPC214x-GCC/chcore.c
+++ b/demos/ARM7-LPC214x-GCC/chcore.c
@@ -162,19 +162,11 @@ void chSysPuts(char *msg) {
 __attribute__((naked, weak))
 void IrqHandler(void) {
 
-  asm(".code 32                                 \n\t" \
-      "stmfd    sp!, {r0-r3, r12, lr}           \n\t");
-#ifdef THUMB
-  asm("add      r0, pc, #1                      \n\t" \
-      "bx       r0                              \n\t" \
-      ".code 16                                 \n\t");
-  VICVectAddr = 0;
-  asm("ldr      r0, =IrqCommon                  \n\t" \
-      "bx       r0                              \n\t");
-#else
-  VICVectAddr = 0;
-  asm("b        IrqCommon                       \n\t");
-#endif
+  chSysIRQEnterI();
+
+  /* nothing */
+
+  chSysIRQExitI();
 }
 
 /*
@@ -183,21 +175,148 @@ void IrqHandler(void) {
 __attribute__((naked, weak))
 void T0IrqHandler(void) {
 
-  asm(".code 32                                 \n\t" \
-      "stmfd    sp!, {r0-r3, r12, lr}           \n\t");
+  chSysIRQEnterI();
+
+  T0IR = 1;             /* Clear interrupt on match MR0. */
+  chSysTimerHandlerI();
+
+  chSysIRQExitI();
+}
+
+/*
+ * Common IRQ exit code, \p chSysIRQExitI() just jumps here.
+ *
+ * System stack frame structure after a context switch in the
+ * interrupt handler:
+ *
+ * High +------------+
+ *      |   LR_USR   | -+
+ *      |     R12    |  |
+ *      |     R3     |  |
+ *      |     R2     |  | External context: IRQ handler frame
+ *      |     R1     |  |
+ *      |     R0     |  |
+ *      |   LR_IRQ   |  |   (user code return address)
+ *      |    SPSR    | -+   (user code status)
+ *      |    ....    | <- chSchDoRescheduleI() stack frame, optimize it for space
+ *      |     LR     | -+   (system code return address)
+ *      |     R11    |  |
+ *      |     R10    |  |
+ *      |     R9     |  |
+ *      |     R8     |  | Internal context: chSysSwitchI() frame
+ *      |    (R7)    |  |   (optional, see CH_CURRP_REGISTER_CACHE)
+ *      |     R6     |  |
+ *      |     R5     |  |
+ * SP-> |     R4     | -+
+ * Low  +------------+
+ */
+__attribute__((naked, weak))
+void IrqCommon(void) {
+  register BOOL b asm("r0");
+
+  VICVectAddr = 0;
+  b = chSchRescRequiredI();
 #ifdef THUMB
-  asm("add      r0, pc, #1                      \n\t" \
-      "bx       r0                              \n\t" \
-      ".code 16                                 \n\t");
-  T0IR = 1;             /* Clear interrupt on match MR0. */
-  chSysTimerHandlerI();
-  VICVectAddr = 0;
-  asm("ldr      r0, =IrqCommon                  \n\t" \
-      "bx       r0                              \n\t");
+  asm(".p2align 2,,                                             \n\t" \
+      "mov      lr, pc                                          \n\t" \
+      "bx       lr                                              \n\t" \
+      ".code 32                                                 \n\t");
+#endif
+  /*
+   * If a reschedulation is not required then just returns from the IRQ.
+   */
+  asm("cmp      r0, #0                                          \n\t" \
+      "ldmeqfd  sp!, {r0-r3, r12, lr}                           \n\t" \
+      "subeqs   pc, lr, #4                                      \n\t");
+  /*
+   * Reschedulation required, saves the external context on the
+   * system/user stack and empties the IRQ stack.
+   */
+  asm(".set     MODE_IRQ, 0x12                                  \n\t" \
+      ".set     MODE_SYS, 0x1F                                  \n\t" \
+      ".set     I_BIT, 0x80                                     \n\t" \
+      "ldmfd    sp!, {r0-r3, r12, lr}                           \n\t" \
+      "msr      CPSR_c, #MODE_SYS | I_BIT                       \n\t" \
+      "stmfd    sp!, {r0-r3, r12, lr}                           \n\t" \
+      "msr      CPSR_c, #MODE_IRQ | I_BIT                       \n\t" \
+      "mrs      r0, SPSR                                        \n\t" \
+      "mov      r1, lr                                          \n\t" \
+      "msr      CPSR_c, #MODE_SYS | I_BIT                       \n\t" \
+      "stmfd    sp!, {r0, r1}                                   \n\t");
+
+#ifdef THUMB_NO_INTERWORKING
+  asm("add      r0, pc, #1                                      \n\t" \
+      "bx       r0                                              \n\t" \
+      ".code 16                                                 \n\t" \
+      "bl       chSchDoRescheduleI                              \n\t" \
+      ".p2align 2,,                                             \n\t" \
+      "mov      lr, pc                                          \n\t" \
+      "bx       lr                                              \n\t" \
+      ".code 32                                                 \n\t");
 #else
-  T0IR = 1;             /* Clear interrupt on match MR0. */
-  chSysTimerHandlerI();
-  VICVectAddr = 0;
-  asm("b        IrqCommon                       \n\t");
+  asm("bl       chSchDoRescheduleI                              \n\t");
+#endif
+
+  /*
+   * Restores the external context.
+   */
+  asm("ldmfd    sp!, {r0, r1}                                   \n\t" \
+      "msr      CPSR_c, #MODE_IRQ | I_BIT                       \n\t" \
+      "msr      SPSR_fsxc, r0                                   \n\t" \
+      "mov      lr, r1                                          \n\t" \
+      "msr      CPSR_c, #MODE_SYS | I_BIT                       \n\t" \
+      "ldmfd    sp!, {r0-r3, r12, lr}                           \n\t" \
+      "msr      CPSR_c, #MODE_IRQ | I_BIT                       \n\t" \
+      "subs     pc, lr, #4                                      \n\t");
+
+  /*
+   * Threads entry/exit code. It is declared weak so you can easily replace it.
+   * NOTE: It is always invoked in ARM mode, it does the mode switching.
+   * NOTE: It is included into IrqCommon to make sure the symbol refers to
+   *       32 bit code.
+   */
+  asm(".set    F_BIT, 0x40                                      \n\t" \
+      ".weak threadstart                                        \n\t" \
+      ".globl threadstart                                       \n\t" \
+      "threadstart:                                             \n\t" \
+      "msr      CPSR_c, #MODE_SYS                               \n\t");
+#ifndef THUMB_NO_INTERWORKING
+  asm("mov      r0, r5                                          \n\t" \
+      "mov      lr, pc                                          \n\t" \
+      "bx       r4                                              \n\t" \
+      "bl       chThdExit                                       \n\t");
+#else
+  asm("add      r0, pc, #1                                      \n\t" \
+      "bx       r0                                              \n\t" \
+      ".code 16                                                 \n\t" \
+      "mov      r0, r5                                          \n\t" \
+      "bl       jmpr4                                           \n\t" \
+      "bl       chThdExit                                       \n\t" \
+      "jmpr4:                                                   \n\t" \
+      "bx       r4                                              \n\t");
 #endif
 }
+
+/*
+ * System halt.
+ */
+__attribute__((naked, weak))
+void chSysHalt(void) {
+
+  asm(".set     F_BIT, 0x40                                     \n\t" \
+      ".set     I_BIT, 0x80                                     \n\t");
+#ifdef THUMB
+  asm(".p2align 2,,                                             \n\t" \
+      "mov      r0, pc                                          \n\t" \
+      "bx       r0                                              \n\t");
+#endif
+  asm(".code 32                                                 \n\t" \
+      ".weak _halt32                                            \n\t" \
+      ".globl _halt32                                           \n\t" \
+      "_halt32:                                                 \n\t" \
+      "mrs      r0, CPSR                                        \n\t" \
+      "orr      r0, #I_BIT | F_BIT                              \n\t" \
+      "msr      CPSR_c, r0                                      \n\t" \
+      ".loop:                                                   \n\t" \
+      "b        .loop                                           \n\t");
+}
diff --git a/demos/ARM7-LPC214x-GCC/chcore.h b/demos/ARM7-LPC214x-GCC/chcore.h
index 941036830..cc1113c18 100644
--- a/demos/ARM7-LPC214x-GCC/chcore.h
+++ b/demos/ARM7-LPC214x-GCC/chcore.h
@@ -92,7 +92,7 @@ extern void chSysUnlock(void);
 #define INT_REQUIRED_STACK 0x10
 #else /* !THUMB */
 #define INT_REQUIRED_STACK 0
-#endif /* THUMB */
+#endif /* !THUMB */
 #define StackAlign(n) ((((n) - 1) | 3) + 1)
 #define UserStackSize(n) StackAlign(sizeof(Thread) +                    \
                                     sizeof(struct intctx) +             \
@@ -101,16 +101,37 @@ extern void chSysUnlock(void);
                                     INT_REQUIRED_STACK)
 #define WorkingArea(s, n) ULONG32 s[UserStackSize(n) >> 2];
 
+#ifdef THUMB
+#define chSysIRQEnterI() {                                              \
+  asm(".code 32                                 \n\t"                   \
+      "stmfd    sp!, {r0-r3, r12, lr}           \n\t"                   \
+      "add      r0, pc, #1                      \n\t"                   \
+      "bx       r0                              \n\t"                   \
+      ".code 16                                 \n\t");                 \
+}
+
+#define chSysIRQExitI() {                                               \
+  VICVectAddr = 0;                                                      \
+  asm("ldr      r0, =IrqCommon                  \n\t"                   \
+      "bx       r0                              \n\t");                 \
+}
+#else /* !THUMB */
+#define chSysIRQEnterI() {                                              \
+  asm("stmfd    sp!, {r0-r3, r12, lr}           \n\t");                 \
+}
+
+#define chSysIRQExitI() {                                               \
+  asm("b        IrqCommon                       \n\t");                 \
+}
+#endif /* !THUMB */
+
 /* It requires zero bytes, but better be safe.*/
 #define IDLE_THREAD_STACK_SIZE 8
 void _IdleThread(void *p) __attribute__((noreturn));
 
-void chSysHalt(void) __attribute__((noreturn));
+void chSysHalt(void);
 void chSysSwitchI(Context *oldp, Context *newp);
 void chSysPuts(char *msg);
 void threadstart(void);
-void DefFiqHandler(void);
-void DefIrqHandler(void);
-void SpuriousHandler(void);
 
 #endif /* _CHCORE_H_ */
diff --git a/demos/ARM7-LPC214x-GCC/chcore2.s b/demos/ARM7-LPC214x-GCC/chcore2.s
index 7c9d1d0d7..35dd49597 100644
--- a/demos/ARM7-LPC214x-GCC/chcore2.s
+++ b/demos/ARM7-LPC214x-GCC/chcore2.s
@@ -34,73 +34,15 @@
 .code 32
 .balign 4
 
-.globl threadstart
-threadstart:
-        msr     CPSR_c, #MODE_SYS
-#ifndef THUMB_NO_INTERWORKING
-        mov     r0, r5
-        mov     lr, pc
-        bx      r4
-        bl      chThdExit
-#else
-        add     r0, pc, #1
-        bx      r0
-.code 16
-        mov     r0, r5
-        bl      jmpr4
-        bl      chThdExit
-jmpr4:  bx      r4
-.code 32
-#endif
-
-.weak UndHandler
-.globl UndHandler
-UndHandler:
-
-.weak SwiHandler
-.globl SwiHandler
-SwiHandler:
-
-.weak PrefetchHandler
-.globl PrefetchHandler
-PrefetchHandler:
-
-.weak AbortHandler
-.globl AbortHandler
-AbortHandler:
-
-.weak FiqHandler
-.globl FiqHandler
-FiqHandler:
-        b       halt32
-
-.weak chSysHalt
-#ifdef THUMB_NO_INTERWORKING
-.code 16
-.globl chSysHalt
-chSysHalt:
-        mov     r0, pc
-        bx      r0
-.code 32
-#else
-.globl chSysHalt
-chSysHalt:
-#endif
-halt32:
-        mrs     r0, CPSR
-        orr     r0, #I_BIT | F_BIT
-        msr     CPSR_c, r0
-.loop:  b       .loop
-
 #ifdef THUMB
 .globl chSysLock
 chSysLock:
-        msr     CPSR_c, #0x9F
+        msr     CPSR_c, #MODE_SYS | I_BIT
         bx      lr
 
 .globl chSysUnlock
 chSysUnlock:
-        msr     CPSR_c, #0x1F
+        msr     CPSR_c, #MODE_SYS
         bx      lr
 #endif
 
@@ -127,80 +69,3 @@ chSysSwitchI:
         ldmfd   sp!, {r4, r5, r6, r7, r8, r9, r10, r11, pc}
 #endif
 #endif /* CH_CURRP_REGISTER_CACHE */
-
-/*
- * Common exit point for all IRQ routines, it performs the rescheduling if
- * required.
- * System stack frame structure after a context switch in the
- * interrupt handler:
- *
- * High +------------+
- *      |   LR_USR   | -+
- *      |     R12    |  |
- *      |     R3     |  |
- *      |     R2     |  | External context: IRQ handler frame
- *      |     R1     |  |
- *      |     R0     |  |
- *      |   LR_IRQ   |  |   (user code return address)
- *      |    SPSR    | -+   (user code status)
- *      |    ....    | <- mk_DoRescheduleI() stack frame, optimize it for space
- *      |     LR     | -+   (system code return address)
- *      |     R11    |  |
- *      |     R10    |  |
- *      |     R9     |  |
- *      |     R8     |  | Internal context: mk_SwitchI() frame
- *      |    (R7)    |  |   (optional, see CH_CURRP_REGISTER_CACHE)
- *      |     R6     |  |
- *      |     R5     |  |
- * SP-> |     R4     | -+
- * Low  +------------+
- */
-#ifdef THUMB_NO_INTERWORKING
-.code 16
-.globl IrqCommon
-IrqCommon:
-        bl      chSchRescRequiredI
-        mov     lr, pc
-        bx      lr
-.code 32
-#else
-.globl IrqCommon
-IrqCommon:
-        bl      chSchRescRequiredI
-#endif
-        cmp     r0, #0                          // Simply returns if a
-        ldmeqfd sp!, {r0-r3, r12, lr}           // reschedule is not
-        subeqs  pc, lr, #4                      // required.
-
-        // Saves the IRQ mode registers in the system stack.
-        ldmfd   sp!, {r0-r3, r12, lr}           // IRQ stack now empty.
-        msr     CPSR_c, #MODE_SYS | I_BIT
-        stmfd   sp!, {r0-r3, r12, lr}           // Registers on System Stack.
-        msr     CPSR_c, #MODE_IRQ | I_BIT
-        mrs     r0, SPSR
-        mov     r1, lr
-        msr     CPSR_c, #MODE_SYS | I_BIT
-        stmfd   sp!, {r0, r1}                   // Push R0=SPSR, R1=LR_IRQ.
-
-        // Context switch.
-#ifdef THUMB_NO_INTERWORKING
-        add     r0, pc, #1
-        bx      r0
-.code 16
-        bl      chSchDoRescheduleI
-        mov     lr, pc
-        bx      lr
-.code 32
-#else
-        bl      chSchDoRescheduleI
-#endif
-
-        // Re-establish the IRQ conditions again.
-        ldmfd   sp!, {r0, r1}                   // Pop R0=SPSR, R1=LR_IRQ.
-        msr     CPSR_c, #MODE_IRQ | I_BIT
-        msr     SPSR_fsxc, r0
-        mov     lr, r1
-        msr     CPSR_c, #MODE_SYS | I_BIT
-        ldmfd   sp!, {r0-r3, r12, lr}
-        msr     CPSR_c, #MODE_IRQ | I_BIT
-        subs    pc, lr, #4
diff --git a/demos/ARM7-LPC214x-GCC/crt0.s b/demos/ARM7-LPC214x-GCC/crt0.s
index a7845036d..9032b0742 100644
--- a/demos/ARM7-LPC214x-GCC/crt0.s
+++ b/demos/ARM7-LPC214x-GCC/crt0.s
@@ -147,3 +147,24 @@ bssloop:
         bl      chSysHalt
 .code 32
 #endif
+
+.weak UndHandler
+.globl UndHandler
+UndHandler:
+
+.weak SwiHandler
+.globl SwiHandler
+SwiHandler:
+
+.weak PrefetchHandler
+.globl PrefetchHandler
+PrefetchHandler:
+
+.weak AbortHandler
+.globl AbortHandler
+AbortHandler:
+
+.weak FiqHandler
+.globl FiqHandler
+FiqHandler:
+        b       _halt32
diff --git a/demos/Win32-MSVS/chcore.h b/demos/Win32-MSVS/chcore.h
index 91fd7be9e..b9fbb0147 100644
--- a/demos/Win32-MSVS/chcore.h
+++ b/demos/Win32-MSVS/chcore.h
@@ -60,6 +60,8 @@ typedef struct {
 #define chSysLock()
 #define chSysUnlock()
 #define chSysPuts(msg) {}
+#define chSysIRQEnterI()
+#define chSysIRQExitI()
 
 #define INT_REQUIRED_STACK 0
 #define StackAlign(n) ((((n) - 1) | 3) + 1)
diff --git a/demos/Win32-MinGW/chcore.h b/demos/Win32-MinGW/chcore.h
index 2b90dd470..43265326b 100644
--- a/demos/Win32-MinGW/chcore.h
+++ b/demos/Win32-MinGW/chcore.h
@@ -60,6 +60,8 @@ typedef struct {
 #define chSysLock()
 #define chSysUnlock()
 #define chSysPuts(msg) {}
+#define chSysIRQEnterI()
+#define chSysIRQExitI()
 
 #define INT_REQUIRED_STACK 0
 #define StackAlign(n) ((((n) - 1) | 3) + 1)
diff --git a/ports/ARM7-LPC214x/GCC/lpc214x_serial.c b/ports/ARM7-LPC214x/GCC/lpc214x_serial.c
index cfd4f696e..43adab3b5 100644
--- a/ports/ARM7-LPC214x/GCC/lpc214x_serial.c
+++ b/ports/ARM7-LPC214x/GCC/lpc214x_serial.c
@@ -55,7 +55,6 @@ static void ServeInterrupt(UART *u, FullDuplexDriver *com) {
 
     switch (u->UART_IIR & IIR_SRC_MASK) {
     case IIR_SRC_NONE:
-      VICVectAddr = 0;
       return;
     case IIR_SRC_ERROR:
       SetError(u->UART_LSR, com);
@@ -98,37 +97,21 @@ static void ServeInterrupt(UART *u, FullDuplexDriver *com) {
 __attribute__((naked, weak))
 void UART0IrqHandler(void) {
 
-  asm(".code 32                                 \n\t" \
-      "stmfd    sp!, {r0-r3, r12, lr}           \n\t");
-#ifdef THUMB
-  asm("add      r0, pc, #1                      \n\t" \
-      "bx       r0                              \n\t" \
-      ".code 16                                 \n\t");
+  chSysIRQEnterI();
+
   ServeInterrupt(U0Base, &COM1);
-  asm("ldr      r0, =IrqCommon                  \n\t" \
-      "bx       r0                              \n\t");
-#else
-  ServeInterrupt(U0Base, &COM1);
-  asm("b        IrqCommon                       \n\t");
-#endif
+
+  chSysIRQExitI();
 }
 
 __attribute__((naked, weak))
 void UART1IrqHandler(void) {
 
-  asm(".code 32                                 \n\t" \
-      "stmfd    sp!, {r0-r3, r12, lr}           \n\t");
-#ifdef THUMB
-  asm("add      r0, pc, #1                      \n\t" \
-      "bx       r0                              \n\t" \
-      ".code 16                                 \n\t");
+  chSysIRQEnterI();
+
   ServeInterrupt(U1Base, &COM2);
-  asm("ldr      r0, =IrqCommon                  \n\t" \
-      "bx       r0                              \n\t");
-#else
-  ServeInterrupt(U1Base, &COM2);
-  asm("b        IrqCommon                       \n\t");
-#endif
+
+  chSysIRQExitI();
 }
 
 #ifdef FIFO_PRELOAD
diff --git a/readme.txt b/readme.txt
index 18ba71eb5..f6692c623 100644
--- a/readme.txt
+++ b/readme.txt
@@ -43,13 +43,20 @@ AVR-AT90CANx-GCC       - Port on AVR AT90CAN128, not complete yet.
 - NEW: Binary Mutexes, the new mechanism provides a complete implementation
   of the "priority inheritance" algorithm as a tool for work around the
   priority inversion problem.
-  The Mutexes are not meant to replace the Semaphores that are still the best
-  mechanism for synchronization between interrupt handlers and high level
-  code, something Mutexes cannot do.
+  The Mutexes are not meant to replace the Semaphores that still are the best
+  synchronization mechanism between interrupt handlers and high level
+  code, something that Mutexes cannot do.
   Soon an article will be added to the wiki describing pro and cons of the
   various mechanisms and the correct use cases.
 - RT Semaphores subsystem removed, the Mutexes implements a better solution
   for the same problem.
+- Modified the test suite in order to have each test case to have the same
+  alignment enforced on functions. This is done to reduce MAM/Cache alignment
+  effects on the measurement.
+- IRQ entry/exit code is now encapsulated into two new macros, see chcore.h
+  for details.
+- Most of the asm code previously in chcore2.s is now inline asm code in
+  chcore.c (ARM port).
 
 *** 0.4.5 ***
 - Moved the serial IRQ handlers and VIC vectors initialization inside the
diff --git a/src/chmtx.c b/src/chmtx.c
index 1c0055acd..c097f2f33 100644
--- a/src/chmtx.c
+++ b/src/chmtx.c
@@ -56,15 +56,15 @@ static void prio_enq(Thread *tp, ThreadsQueue *tqp) {
 }
 
 /*
- * 0 +++BA++------------------2+++--8++++++++++BR0----------------------------
- * 1 .......++AA++--2+++++++++BA....8..........++++++++BR8++++AR1-------------
- * 2 .............++AA..............................................----++AR++
- * 8 .............................++AA........................++++++AR++......
+ * 0 +++BL++------------------2+++--8++++++++++BU0----------------------------
+ * 1 .......++AL++--2+++++++++BL....8..........++++++++BU8++++AU1-------------
+ * 2 .............++AL..............................................----++AU++
+ * 8 .............................++AL........................++++++AU++......
  *
  *
- * 5 ++++++++++++++++++AA+....9++++++++++++++AR5------------------------------
+ * 5 ++++++++++++++++++AL+....9++++++++++++++AU5------------------------------
  * 7 .....................++--------------------------------------------------
- * 9 .......................++AA.............+++++++++AR++++++++++++++++++++++
+ * 9 .......................++AL.............+++++++++AU++++++++++++++++++++++
  */
 
 /**
diff --git a/src/templates/chcore.h b/src/templates/chcore.h
index c88748f8b..3f796e8ab 100644
--- a/src/templates/chcore.h
+++ b/src/templates/chcore.h
@@ -100,6 +100,21 @@ typedef struct {
  */
 #define chSysUnlock()
 
+/**
+ * IRQ handler enter code.
+ * @note Usually IRQ handlers function are also declared naked.
+ * @note On some architectures this macro can be empty.
+ */
+#define chSysIRQEnterI()
+
+/**
+ * IRQ handler exit code.
+ * @note Usually IRQ handlers function are also declared naked.
+ * @note This macro must perform the final reschedulation by using
+ *       \p chSchRescRequiredI() and \p chSchDoRescheduleI().
+ */
+#define chSysIRQExitI()
+
 void _IdleThread(void *p);
 void chSysHalt(void);
 void chSysSwitchI(Context *oldp, Context *newp);