diff --git a/boards/OLIMEX_LPC_P2148/board.c b/boards/OLIMEX_LPC_P2148/board.c
index 26cce1d90..45c492cbf 100644
--- a/boards/OLIMEX_LPC_P2148/board.c
+++ b/boards/OLIMEX_LPC_P2148/board.c
@@ -54,25 +54,18 @@ static CH_IRQ_HANDLER(T0IrqHandler) {
 
 /*
  * Early initialization code.
- * This initialization is performed just after reset before BSS and DATA
- * segments initialization.
+ * This initialization must be performed just after stack setup and before
+ * any other initialization.
  */
-void hwinit0(void) {
+void __early_init(void) {
 
   lpc214x_clock_init();
 }
 
 /*
- * Late initialization code.
- * This initialization is performed after BSS and DATA segments initialization
- * and before invoking the main() function.
+ * Board-specific initialization code.
  */
-void hwinit1(void) {
-
-  /*
-   * HAL initialization.
-   */
-  halInit();
+void boardInit(void) {
 
   /*
    * System Timer initialization, 1ms intervals.
@@ -85,9 +78,4 @@ void hwinit1(void) {
   timer->TC_MCR = 3;    /* Interrupt and clear TC on match MR0. */
   timer->TC_TCR = 2;    /* Reset counter and prescaler. */
   timer->TC_TCR = 1;    /* Timer enabled. */
-
-  /*
-   * ChibiOS/RT initialization.
-   */
-  chSysInit();
 }
diff --git a/boards/OLIMEX_LPC_P2148/board.h b/boards/OLIMEX_LPC_P2148/board.h
index 48dc4f6bb..7a1ec7cf6 100644
--- a/boards/OLIMEX_LPC_P2148/board.h
+++ b/boards/OLIMEX_LPC_P2148/board.h
@@ -82,4 +82,14 @@
 #define PB_WP1          24
 #define PB_CP1          25
 
+#if !defined(_FROM_ASM_)
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void boardInit(void);
+#ifdef __cplusplus
+}
+#endif
+#endif /* _FROM_ASM_ */
+
 #endif /* _BOARD_H_ */
diff --git a/demos/ARM7-LPC214x-FATFS-GCC/main.c b/demos/ARM7-LPC214x-FATFS-GCC/main.c
index ffe50af6a..9edae1db7 100644
--- a/demos/ARM7-LPC214x-FATFS-GCC/main.c
+++ b/demos/ARM7-LPC214x-FATFS-GCC/main.c
@@ -221,8 +221,7 @@ static void RemoveHandler(eventid_t id) {
 }
 
 /*
- * Entry point, note, the main() function is already a thread in the system
- * on entry.
+ * Application entry point.
  */
 int main(int argc, char **argv) {
   static const evhandler_t evhndl[] = {
@@ -236,6 +235,16 @@ int main(int argc, char **argv) {
   (void)argc;
   (void)argv;
 
+  /*
+   * System initializations.
+   * - HAL initialization, this also initializes the configured device drivers
+   *   and performs the board-specific initializations.
+   * - Kernel initialization, the main() function becomes a thread and the
+   *   RTOS is active.
+   */
+  halInit();
+  chSysInit();
+
   /*
    * Activates the serial driver 2 using the driver default configuration.
    */
diff --git a/demos/ARM7-LPC214x-G++/main.cpp b/demos/ARM7-LPC214x-G++/main.cpp
index b6b7c83ba..8a9ca1c05 100644
--- a/demos/ARM7-LPC214x-G++/main.cpp
+++ b/demos/ARM7-LPC214x-G++/main.cpp
@@ -142,8 +142,7 @@ static void TimerHandler(eventid_t id) {
 }
 
 /*
- * Entry point, note, the main() function is already a thread in the system
- * on entry.
+ * Application entry point.
  */
 int main(int argc, char **argv) {
   static const evhandler_t evhndl[] = {
@@ -155,6 +154,16 @@ int main(int argc, char **argv) {
   (void)argc;
   (void)argv;
 
+  /*
+   * System initializations.
+   * - HAL initialization, this also initializes the configured device drivers
+   *   and performs the board-specific initializations.
+   * - Kernel initialization, the main() function becomes a thread and the
+   *   RTOS is active.
+   */
+  halInit();
+  System::Init();
+
   /*
    * Activates the serial driver 2 using the driver default configuration.
    */
diff --git a/demos/ARM7-LPC214x-GCC/main.c b/demos/ARM7-LPC214x-GCC/main.c
index 9ac1b7698..6cc8037f6 100644
--- a/demos/ARM7-LPC214x-GCC/main.c
+++ b/demos/ARM7-LPC214x-GCC/main.c
@@ -60,14 +60,23 @@ static msg_t Thread2(void *arg) {
 }
 
 /*
- * Entry point, note, the main() function is already a thread in the system
- * on entry.
+ * Application entry point.
  */
 int main(int argc, char **argv) {
 
   (void)argc;
   (void)argv;
 
+  /*
+   * System initializations.
+   * - HAL initialization, this also initializes the configured device drivers
+   *   and performs the board-specific initializations.
+   * - Kernel initialization, the main() function becomes a thread and the
+   *   RTOS is active.
+   */
+  halInit();
+  chSysInit();
+
   /*
    * Activates the serial driver 2 using the driver default configuration.
    */
diff --git a/docs/reports/LPC2148-48-ARM.txt b/docs/reports/LPC2148-48-ARM.txt
index 3a4ebc0aa..c179db3cf 100644
--- a/docs/reports/LPC2148-48-ARM.txt
+++ b/docs/reports/LPC2148-48-ARM.txt
@@ -5,7 +5,7 @@ Settings: CCLK=48, MAMCR=2, MAMTIM=3 (3 wait states)
 
 *** ChibiOS/RT test suite
 ***
-*** Kernel:       2.1.4unstable
+*** Kernel:       2.1.6unstable
 *** GCC Version:  4.5.1
 *** Architecture: ARM7
 *** Core Variant: ARM7TDMI
@@ -114,11 +114,11 @@ Settings: CCLK=48, MAMCR=2, MAMTIM=3 (3 wait states)
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.5 (Benchmark, threads, full cycle)
---- Score : 89745 threads/S
+--- Score : 88089 threads/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.6 (Benchmark, threads, create only)
---- Score : 128237 threads/S
+--- Score : 126542 threads/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.7 (Benchmark, mass reschedule, 5 threads)
@@ -126,7 +126,7 @@ Settings: CCLK=48, MAMCR=2, MAMTIM=3 (3 wait states)
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.8 (Benchmark, round robin context switching)
---- Score : 276080 ctxswc/S
+--- Score : 276084 ctxswc/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.9 (Benchmark, I/O Queues throughput)
diff --git a/docs/reports/LPC2148-48-THUMB.txt b/docs/reports/LPC2148-48-THUMB.txt
index 9fcee16af..199f592f4 100644
--- a/docs/reports/LPC2148-48-THUMB.txt
+++ b/docs/reports/LPC2148-48-THUMB.txt
@@ -5,7 +5,7 @@ Settings: CCLK=48, MAMCR=2, MAMTIM=3 (3 wait states)
 
 *** ChibiOS/RT test suite
 ***
-*** Kernel:       2.1.4unstable
+*** Kernel:       2.1.6unstable
 *** GCC Version:  4.5.1
 *** Architecture: ARM7
 *** Core Variant: ARM7TDMI
@@ -98,7 +98,7 @@ Settings: CCLK=48, MAMCR=2, MAMTIM=3 (3 wait states)
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.1 (Benchmark, messages #1)
---- Score : 106223 msgs/S, 212446 ctxswc/S
+--- Score : 106224 msgs/S, 212448 ctxswc/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.2 (Benchmark, messages #2)
@@ -114,11 +114,11 @@ Settings: CCLK=48, MAMCR=2, MAMTIM=3 (3 wait states)
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.5 (Benchmark, threads, full cycle)
---- Score : 71990 threads/S
+--- Score : 70297 threads/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.6 (Benchmark, threads, create only)
---- Score : 110394 threads/S
+--- Score : 109135 threads/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.7 (Benchmark, mass reschedule, 5 threads)
diff --git a/os/ports/GCC/ARM/crt0.s b/os/ports/GCC/ARM/crt0.s
index 1ce42ce57..33cbb5600 100644
--- a/os/ports/GCC/ARM/crt0.s
+++ b/os/ports/GCC/ARM/crt0.s
@@ -27,25 +27,25 @@
 
 #if !defined(__DOXYGEN__)
 
-.set    MODE_USR, 0x10
-.set    MODE_FIQ, 0x11
-.set    MODE_IRQ, 0x12
-.set    MODE_SVC, 0x13
-.set    MODE_ABT, 0x17
-.set    MODE_UND, 0x1B
-.set    MODE_SYS, 0x1F
+        .set    MODE_USR, 0x10
+        .set    MODE_FIQ, 0x11
+        .set    MODE_IRQ, 0x12
+        .set    MODE_SVC, 0x13
+        .set    MODE_ABT, 0x17
+        .set    MODE_UND, 0x1B
+        .set    MODE_SYS, 0x1F
 
-.equ    I_BIT, 0x80
-.equ    F_BIT, 0x40
+        .set    I_BIT, 0x80
+        .set    F_BIT, 0x40
 
-.text
-.code 32
-.balign 4
+        .text
+        .code   32
+        .balign 4
 
 /*
  * Reset handler.
  */
-.global ResetHandler
+        .global ResetHandler
 ResetHandler:
         /*
          * Stack pointers initialization.
@@ -85,15 +85,15 @@ ResetHandler:
          * Early initialization.
          */
 #ifndef THUMB_NO_INTERWORKING
-        bl      hwinit0
+        bl      __early_init
 #else
         add     r0, pc, #1
         bx      r0
-.code 16
-        bl      hwinit0
+        .code   16
+        bl      __early_init
         mov     r0, pc
         bx      r0
-.code 32
+        .code   32
 #endif
         /*
          * Data initialization.
@@ -119,21 +119,19 @@ bssloop:
         strlo   r0, [r1], #4
         blo     bssloop
         /*
-         * Late initialization.
+         * Main program invocation.
          */
 #ifdef THUMB_NO_INTERWORKING
         add     r0, pc, #1
         bx      r0
-.code 16
-        bl      hwinit1
+        .code   16
         mov     r0, #0
         mov     r1, r0
         bl      main
         ldr     r1, =MainExitHandler
         bx      r1
-.code 32
+        .code   32
 #else
-        bl      hwinit1
         mov     r0, #0
         mov     r1, r0
         bl      main
@@ -143,10 +141,9 @@ bssloop:
 /*
  * Default main function exit handler.
  */
-.weak MainExitHandler
-.globl MainExitHandler
+        .weak   MainExitHandler
+        .global MainExitHandler
 MainExitHandler:
-
 .loop:  b       .loop
 
 /*
@@ -156,29 +153,13 @@ MainExitHandler:
  * segments initialization.
  */
 #ifdef THUMB_NO_INTERWORKING
-.thumb_func
-.code 16
+        .thumb_func
+        .code   16
 #endif
-.weak hwinit0
+        .weak   __early_init
 hwinit0:
         bx      lr
-.code 32
-
-/*
- * Default late initialization code. It is declared weak in order to be
- * replaced by the real initialization code.
- * Late initialization is performed after BSS and DATA segments initialization
- * and before invoking the main() function.
- */
-#ifdef THUMB_NO_INTERWORKING
-.thumb_func
-.code 16
-#endif
-.weak hwinit1
-hwinit1:
-        bx      lr
-.code 32
-
+        .code   32
 #endif
 
 /** @} */
diff --git a/os/ports/GCC/ARMCMx/crt0_v6m.s b/os/ports/GCC/ARMCMx/crt0_v6m.s
index 8cfbb7977..fd7e4fca4 100644
--- a/os/ports/GCC/ARMCMx/crt0_v6m.s
+++ b/os/ports/GCC/ARMCMx/crt0_v6m.s
@@ -100,7 +100,7 @@ endbloop:
         msr     CONTROL, r0
         isb
         /*
-         * Main program invokation.
+         * Main program invocation.
          */
         movs    r0, #0
         mov     r1, r0
diff --git a/os/ports/GCC/ARMCMx/crt0_v7m.s b/os/ports/GCC/ARMCMx/crt0_v7m.s
index 25aacecc2..fb0d4a137 100644
--- a/os/ports/GCC/ARMCMx/crt0_v7m.s
+++ b/os/ports/GCC/ARMCMx/crt0_v7m.s
@@ -95,7 +95,7 @@ bloop:
         msr     CONTROL, r0
         isb
         /*
-         * Main program invokation.
+         * Main program invocation.
          */
         movs    r0, #0
         mov     r1, r0