diff --git a/boards/OLIMEX_SAM7_EX256/board.c b/boards/OLIMEX_SAM7_EX256/board.c
index 38c84fc1d..e13a05420 100644
--- a/boards/OLIMEX_SAM7_EX256/board.c
+++ b/boards/OLIMEX_SAM7_EX256/board.c
@@ -60,10 +60,10 @@ static CH_IRQ_HANDLER(SYSIrqHandler) {
 
 /*
  * 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) {
   
   /* Watchdog disabled.*/
   AT91C_BASE_WDTC->WDTC_WDMR = AT91C_WDTC_WDDIS;
@@ -72,16 +72,9 @@ void hwinit0(void) {
 }
 
 /*
- * 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) {
 
   /*
    * LCD pins setup.
@@ -124,9 +117,4 @@ void hwinit1(void) {
   AT91C_BASE_PIOA->PIO_PDR   = AT91C_PA3_RTS0 | AT91C_PA4_CTS0;
   AT91C_BASE_PIOA->PIO_ASR   = AT91C_PIO_PA3 | AT91C_PIO_PA4;
   AT91C_BASE_PIOA->PIO_PPUDR = AT91C_PIO_PA3 | AT91C_PIO_PA4;
-
-  /*
-   * ChibiOS/RT initialization.
-   */
-  chSysInit();
 }
diff --git a/boards/OLIMEX_SAM7_EX256/board.h b/boards/OLIMEX_SAM7_EX256/board.h
index 22add597e..961fda9c0 100644
--- a/boards/OLIMEX_SAM7_EX256/board.h
+++ b/boards/OLIMEX_SAM7_EX256/board.h
@@ -91,4 +91,14 @@
 #define PIOB_PHY_IRQ            26
 #define PIOB_PHY_IRQ_MASK       (1 << PIOB_PHY_IRQ)
 
+#if !defined(_FROM_ASM_)
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void boardInit(void);
+#ifdef __cplusplus
+}
+#endif
+#endif /* _FROM_ASM_ */
+
 #endif /* _BOARD_H_ */
diff --git a/boards/OLIMEX_SAM7_P256/board.c b/boards/OLIMEX_SAM7_P256/board.c
index 12b221930..cd9a43873 100644
--- a/boards/OLIMEX_SAM7_P256/board.c
+++ b/boards/OLIMEX_SAM7_P256/board.c
@@ -54,10 +54,10 @@ static CH_IRQ_HANDLER(SYSIrqHandler) {
 
 /*
  * 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) {
   
   /* Watchdog disabled.*/
   AT91C_BASE_WDTC->WDTC_WDMR = AT91C_WDTC_WDDIS;
@@ -66,16 +66,9 @@ void hwinit0(void) {
 }
 
 /*
- * 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) {
 
   /*
    * LED pins setup.
@@ -106,9 +99,4 @@ void hwinit1(void) {
   AIC_EnableIT(AT91C_ID_SYS);
   AT91C_BASE_PITC->PITC_PIMR = (MCK / 16 / CH_FREQUENCY) - 1;
   AT91C_BASE_PITC->PITC_PIMR |= AT91C_PITC_PITEN | AT91C_PITC_PITIEN;
-
-  /*
-   * ChibiOS/RT initialization.
-   */
-  chSysInit();
 }
diff --git a/boards/OLIMEX_SAM7_P256/board.h b/boards/OLIMEX_SAM7_P256/board.h
index 0e2a3d783..65877652d 100644
--- a/boards/OLIMEX_SAM7_P256/board.h
+++ b/boards/OLIMEX_SAM7_P256/board.h
@@ -71,4 +71,14 @@
 #define PIOA_MMC_NPCS0          11
 #define PIOA_MMC_NPCS0_MASK     (1 << PIOA_MMC_NPCS0_MASK)
 
+#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-AT91SAM7S-FATFS-GCC/main.c b/demos/ARM7-AT91SAM7S-FATFS-GCC/main.c
index 5ebd0b9ff..95c5c94a3 100644
--- a/demos/ARM7-AT91SAM7S-FATFS-GCC/main.c
+++ b/demos/ARM7-AT91SAM7S-FATFS-GCC/main.c
@@ -291,8 +291,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[] = {
@@ -305,6 +304,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 1 using the driver default configuration.
    */
diff --git a/demos/ARM7-AT91SAM7S-GCC/main.c b/demos/ARM7-AT91SAM7S-GCC/main.c
index 82e1ee5f2..49ce3df70 100644
--- a/demos/ARM7-AT91SAM7S-GCC/main.c
+++ b/demos/ARM7-AT91SAM7S-GCC/main.c
@@ -35,13 +35,23 @@ static msg_t Thread1(void *p) {
 }
 
 /*
- * 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 1 using the driver default configuration.
    */
diff --git a/demos/ARM7-AT91SAM7X-FATFS-GCC/main.c b/demos/ARM7-AT91SAM7X-FATFS-GCC/main.c
index 3ed2ab0e6..5708cb3b3 100644
--- a/demos/ARM7-AT91SAM7X-FATFS-GCC/main.c
+++ b/demos/ARM7-AT91SAM7X-FATFS-GCC/main.c
@@ -274,8 +274,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[] = {
@@ -288,6 +287,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 1 using the driver default configuration.
    */
diff --git a/demos/ARM7-AT91SAM7X-GCC/main.c b/demos/ARM7-AT91SAM7X-GCC/main.c
index f5514b41a..a4af5dadb 100644
--- a/demos/ARM7-AT91SAM7X-GCC/main.c
+++ b/demos/ARM7-AT91SAM7X-GCC/main.c
@@ -35,13 +35,22 @@ static msg_t Thread1(void *p) {
 }
 
 /*
- * 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 1 using the driver default configuration.
    */
diff --git a/demos/ARM7-AT91SAM7X-LWIP-GCC/main.c b/demos/ARM7-AT91SAM7X-LWIP-GCC/main.c
index 895766132..bee5b36c9 100644
--- a/demos/ARM7-AT91SAM7X-LWIP-GCC/main.c
+++ b/demos/ARM7-AT91SAM7X-LWIP-GCC/main.c
@@ -38,14 +38,23 @@ static msg_t Thread1(void *p) {
 }
 
 /*
- * 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 1 using the driver default configuration.
    */
diff --git a/demos/ARM7-AT91SAM7X-UIP-GCC/main.c b/demos/ARM7-AT91SAM7X-UIP-GCC/main.c
index 3a3f5eeb6..1fd5fcf52 100644
--- a/demos/ARM7-AT91SAM7X-UIP-GCC/main.c
+++ b/demos/ARM7-AT91SAM7X-UIP-GCC/main.c
@@ -39,14 +39,23 @@ static msg_t Thread1(void *p) {
 }
 
 /*
- * 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/AT91SAM7X-48-ARM.txt b/docs/reports/AT91SAM7X-48-ARM.txt
index b4085be19..9a177cde4 100644
--- a/docs/reports/AT91SAM7X-48-ARM.txt
+++ b/docs/reports/AT91SAM7X-48-ARM.txt
@@ -5,7 +5,7 @@ Settings: MCK=48.054857, MC_FMR = AT91C_MC_FWS_1FWS (1 wait state)
 
 *** 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: MCK=48.054857, MC_FMR = AT91C_MC_FWS_1FWS (1 wait state)
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.5 (Benchmark, threads, full cycle)
---- Score : 67349 threads/S
+--- Score : 65956 threads/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.6 (Benchmark, threads, create only)
---- Score : 96994 threads/S
+--- Score : 95636 threads/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.7 (Benchmark, mass reschedule, 5 threads)
@@ -126,7 +126,7 @@ Settings: MCK=48.054857, MC_FMR = AT91C_MC_FWS_1FWS (1 wait state)
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.8 (Benchmark, round robin context switching)
---- Score : 211344 ctxswc/S
+--- Score : 211348 ctxswc/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.9 (Benchmark, I/O Queues throughput)
diff --git a/docs/reports/AT91SAM7X-48-THUMB.txt b/docs/reports/AT91SAM7X-48-THUMB.txt
index d3fdc3c18..b283f46d5 100644
--- a/docs/reports/AT91SAM7X-48-THUMB.txt
+++ b/docs/reports/AT91SAM7X-48-THUMB.txt
@@ -5,7 +5,7 @@ Settings: MCK=48.054857, MC_FMR = AT91C_MC_FWS_1FWS (1 wait state)
 
 *** 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: MCK=48.054857, MC_FMR = AT91C_MC_FWS_1FWS (1 wait state)
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.1 (Benchmark, messages #1)
---- Score : 106526 msgs/S, 213052 ctxswc/S
+--- Score : 106525 msgs/S, 213050 ctxswc/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.2 (Benchmark, messages #2)
@@ -114,11 +114,11 @@ Settings: MCK=48.054857, MC_FMR = AT91C_MC_FWS_1FWS (1 wait state)
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.5 (Benchmark, threads, full cycle)
---- Score : 70339 threads/S
+--- Score : 68821 threads/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.6 (Benchmark, threads, create only)
---- Score : 105584 threads/S
+--- Score : 104432 threads/S
 --- Result: SUCCESS
 ----------------------------------------------------------------------------
 --- Test Case 11.7 (Benchmark, mass reschedule, 5 threads)