diff --git a/demos/STM32/RT-STM32F407-DISCOVERY-MEMS/.cproject b/demos/STM32/RT-STM32F407-DISCOVERY-MEMS/.cproject
index ed0bb07d9..0ccbe54e9 100644
--- a/demos/STM32/RT-STM32F407-DISCOVERY-MEMS/.cproject
+++ b/demos/STM32/RT-STM32F407-DISCOVERY-MEMS/.cproject
@@ -1,7 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?>
-
-<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
 	<storageModule moduleId="org.eclipse.cdt.core.settings">
 		<cconfiguration id="0.114656749">
 			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.114656749" moduleId="org.eclipse.cdt.core.settings" name="Default">
diff --git a/testhal/STM32/STM32F4xx/IRQ_STORM/.project b/testhal/STM32/STM32F4xx/IRQ_STORM/.project
index 122d48d9d..fc76b0256 100644
--- a/testhal/STM32/STM32F4xx/IRQ_STORM/.project
+++ b/testhal/STM32/STM32F4xx/IRQ_STORM/.project
@@ -29,6 +29,11 @@
 			<type>2</type>
 			<locationURI>CHIBIOS/os/hal/boards/ST_STM32F4_DISCOVERY</locationURI>
 		</link>
+		<link>
+			<name>common</name>
+			<type>2</type>
+			<locationURI>CHIBIOS/testhal/common</locationURI>
+		</link>
 		<link>
 			<name>os</name>
 			<type>2</type>
diff --git a/testhal/STM32/STM32F4xx/IRQ_STORM/Makefile b/testhal/STM32/STM32F4xx/IRQ_STORM/Makefile
index 3fec9da36..7e5004cd0 100644
--- a/testhal/STM32/STM32F4xx/IRQ_STORM/Makefile
+++ b/testhal/STM32/STM32F4xx/IRQ_STORM/Makefile
@@ -107,6 +107,9 @@ CSRC = $(STARTUPSRC) \
        $(PLATFORMSRC) \
        $(BOARDSRC) \
        $(TESTSRC) \
+       $(CHIBIOS)/os/hal/lib/streams/memstreams.c \
+       $(CHIBIOS)/os/hal/lib/streams/chprintf.c \
+       $(CHIBIOS)/testhal/common/irq_storm.c \
        main.c
 
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global
@@ -138,7 +141,9 @@ ASMSRC = $(STARTUPASM) $(PORTASM) $(OSALASM)
 
 INCDIR = $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \
          $(HALINC) $(PLATFORMINC) $(BOARDINC) $(TESTINC) \
-         $(CHIBIOS)/os/various
+         $(CHIBIOS)/os/hal/lib/streams \
+         $(CHIBIOS)/os/various \
+         $(CHIBIOS)/testhal/common
 
 #
 # Project, sources and paths
diff --git a/testhal/STM32/STM32F4xx/IRQ_STORM/main.c b/testhal/STM32/STM32F4xx/IRQ_STORM/main.c
index e863910c3..a676086db 100644
--- a/testhal/STM32/STM32F4xx/IRQ_STORM/main.c
+++ b/testhal/STM32/STM32F4xx/IRQ_STORM/main.c
@@ -14,140 +14,17 @@
     limitations under the License.
 */
 
-#include <stdlib.h>
-
 #include "ch.h"
 #include "hal.h"
 
-/*===========================================================================*/
-/* Configurable settings.                                                    */
-/*===========================================================================*/
-
-#ifndef RANDOMIZE
-#define RANDOMIZE       FALSE
-#endif
-
-#ifndef ITERATIONS
-#define ITERATIONS      100
-#endif
-
-#ifndef NUM_THREADS
-#define NUM_THREADS     4
-#endif
-
-#ifndef MAILBOX_SIZE
-#define MAILBOX_SIZE    4
-#endif
-
-/*===========================================================================*/
-/* Test related code.                                                        */
-/*===========================================================================*/
-
-#define MSG_SEND_LEFT   0
-#define MSG_SEND_RIGHT  1
-
-static bool saturated;
+#include "irq_storm.h"
 
 /*
- * Mailboxes and buffers.
- */
-static mailbox_t mb[NUM_THREADS];
-static msg_t b[NUM_THREADS][MAILBOX_SIZE];
-
-/*
- * Test worker threads.
- */
-static THD_WORKING_AREA(waWorkerThread[NUM_THREADS], 128);
-static THD_FUNCTION(WorkerThread, arg) {
-  static volatile unsigned x = 0;
-  static unsigned cnt = 0;
-  unsigned me = (unsigned)arg;
-  unsigned target;
-  unsigned r;
-  msg_t msg;
-
-  chRegSetThreadName("worker");
-
-  /* Work loop.*/
-  while (true) {
-    /* Waiting for a message.*/
-   chMBFetch(&mb[me], &msg, TIME_INFINITE);
-
-#if RANDOMIZE
-   /* Pseudo-random delay.*/
-   {
-     chSysLock();
-     r = rand() & 15;
-     chSysUnlock();
-     while (r--)
-       x++;
-   }
-#else
-   /* Fixed delay.*/
-   {
-     r = me >> 4;
-     while (r--)
-       x++;
-   }
-#endif
-
-    /* Deciding in which direction to re-send the message.*/
-    if (msg == MSG_SEND_LEFT)
-      target = me - 1;
-    else
-      target = me + 1;
-
-    if (target < NUM_THREADS) {
-      /* If this thread is not at the end of a chain re-sending the message,
-         note this check works because the variable target is unsigned.*/
-      msg = chMBPost(&mb[target], msg, TIME_IMMEDIATE);
-      if (msg != MSG_OK)
-        saturated = TRUE;
-    }
-    else {
-      /* Provides a visual feedback about the system.*/
-      if (++cnt >= 500) {
-        cnt = 0;
-        palTogglePad(GPIOD, GPIOD_LED5);
-      }
-    }
-  }
-}
-
-/*
- * GPT2 callback.
- */
-static void gpt4cb(GPTDriver *gptp) {
-  msg_t msg;
-
-  (void)gptp;
-  chSysLockFromISR();
-  msg = chMBPostI(&mb[0], MSG_SEND_RIGHT);
-  if (msg != MSG_OK)
-    saturated = TRUE;
-  chSysUnlockFromISR();
-}
-
-/*
- * GPT3 callback.
- */
-static void gpt3cb(GPTDriver *gptp) {
-  msg_t msg;
-
-  (void)gptp;
-  chSysLockFromISR();
-  msg = chMBPostI(&mb[NUM_THREADS - 1], MSG_SEND_LEFT);
-  if (msg != MSG_OK)
-    saturated = TRUE;
-  chSysUnlockFromISR();
-}
-
-/*
- * GPT2 configuration.
+ * GPT4 configuration.
  */
 static const GPTConfig gpt4cfg = {
-  1000000,  /* 1MHz timer clock.*/
-  gpt4cb,   /* Timer callback.*/
+  1000000,              /* 1MHz timer clock.*/
+  irq_storm_gpt1_cb,    /* Timer callback.*/
   0,
   0
 };
@@ -156,52 +33,30 @@ static const GPTConfig gpt4cfg = {
  * GPT3 configuration.
  */
 static const GPTConfig gpt3cfg = {
-  1000000,  /* 1MHz timer clock.*/
-  gpt3cb,   /* Timer callback.*/
+  1000000,              /* 1MHz timer clock.*/
+  irq_storm_gpt2_cb,    /* Timer callback.*/
   0,
   0
 };
 
-
-/*===========================================================================*/
-/* Generic demo code.                                                        */
-/*===========================================================================*/
-
-static void print(char *p) {
-
-  while (*p) {
-    chSequentialStreamPut(&SD2, *p++);
-  }
-}
-
-static void println(char *p) {
-
-  while (*p) {
-    chSequentialStreamPut(&SD2, *p++);
-  }
-  chSequentialStreamWrite(&SD2, (uint8_t *)"\r\n", 2);
-}
-
-static void printn(uint32_t n) {
-  char buf[16], *p;
-
-  if (!n)
-    chSequentialStreamPut(&SD2, '0');
-  else {
-    p = buf;
-    while (n)
-      *p++ = (n % 10) + '0', n /= 10;
-    while (p > buf)
-      chSequentialStreamPut(&SD2, *--p);
-  }
-}
+/*
+ * IRQ Storm configuration.
+ */
+static const irq_storm_config_t irq_storm_config = {
+  (BaseSequentialStream  *)&SD2,
+  GPIOD,
+  GPIOD_LED5,
+  &GPTD4,
+  &GPTD3,
+  &gpt4cfg,
+  &gpt3cfg,
+  STM32_SYSCLK
+};
 
 /*
  * Application entry point.
  */
 int main(void) {
-  unsigned i;
-  gptcnt_t interval, threshold, worst;
 
   /*
    * System initializations.
@@ -213,121 +68,15 @@ int main(void) {
   halInit();
   chSysInit();
 
-  /*
-   * Prepares the Serial driver 2 and GPT drivers 2 and 3.
-   */
+  /* Prepares the Serial driver 2.*/
   sdStart(&SD2, NULL);          /* Default is 38400-8-N-1.*/
   palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
   palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
-  gptStart(&GPTD4, &gpt4cfg);
-  gptStart(&GPTD3, &gpt3cfg);
 
-  /*
-   * Initializes the mailboxes and creates the worker threads.
-   */
-  for (i = 0; i < NUM_THREADS; i++) {
-    chMBObjectInit(&mb[i], b[i], MAILBOX_SIZE);
-    chThdCreateStatic(waWorkerThread[i], sizeof waWorkerThread[i],
-                      NORMALPRIO - 20, WorkerThread, (void *)i);
-  }
+  /* Running the test.*/
+  irq_storm_execute(&irq_storm_config);
 
-  /*
-   * Test procedure.
-   */
-  println("");
-  println("*** ChibiOS/RT IRQ-STORM long duration test");
-  println("***");
-  print("*** Kernel:       ");
-  println(CH_KERNEL_VERSION);
-  print("*** Compiled:     ");
-  println(__DATE__ " - " __TIME__);
-#ifdef PORT_COMPILER_NAME
-  print("*** Compiler:     ");
-  println(PORT_COMPILER_NAME);
-#endif
-  print("*** Architecture: ");
-  println(PORT_ARCHITECTURE_NAME);
-#ifdef PORT_CORE_VARIANT_NAME
-  print("*** Core Variant: ");
-  println(PORT_CORE_VARIANT_NAME);
-#endif
-#ifdef PORT_INFO
-  print("*** Port Info:    ");
-  println(PORT_INFO);
-#endif
-#ifdef PLATFORM_NAME
-  print("*** Platform:     ");
-  println(PLATFORM_NAME);
-#endif
-#ifdef BOARD_NAME
-  print("*** Test Board:   ");
-  println(BOARD_NAME);
-#endif
-  println("***");
-  print("*** System Clock: ");
-  printn(STM32_SYSCLK);
-  println("");
-  print("*** Iterations:   ");
-  printn(ITERATIONS);
-  println("");
-  print("*** Randomize:    ");
-  printn(RANDOMIZE);
-  println("");
-  print("*** Threads:      ");
-  printn(NUM_THREADS);
-  println("");
-  print("*** Mailbox size: ");
-  printn(MAILBOX_SIZE);
-  println("");
-
-  println("");
-  worst = 0;
-  for (i = 1; i <= ITERATIONS; i++){
-    print("Iteration ");
-    printn(i);
-    println("");
-    saturated = FALSE;
-    threshold = 0;
-    for (interval = 2000;
-        interval >= 2;
-        interval -= (interval + 9) / 10) {
-      gptStartContinuous(&GPTD4, interval - 1); /* Slightly out of phase.*/
-      gptStartContinuous(&GPTD3, interval + 1); /* Slightly out of phase.*/
-      chThdSleepMilliseconds(1000);
-      gptStopTimer(&GPTD4);
-      gptStopTimer(&GPTD3);
-      if (!saturated)
-        print(".");
-      else {
-        print("#");
-        if (threshold == 0)
-          threshold = interval;
-        break;
-      }
-    }
-    /* Gives the worker threads a chance to empty the mailboxes before next
-       cycle.*/
-    chThdSleepMilliseconds(20);
-    println("");
-    print("Saturated at ");
-    printn(threshold);
-    println(" uS");
-    println("");
-    if (threshold > worst)
-      worst = threshold;
-  }
-  gptStopTimer(&GPTD4);
-  gptStopTimer(&GPTD3);
-
-  print("Worst case at ");
-  printn(worst);
-  println(" uS");
-  println("");
-  println("Test Complete");
-
-  /*
-   * Normal main() thread activity, nothing in this test.
-   */
+  /* Normal main() thread activity, nothing in this test.*/
   while (true) {
     chThdSleepMilliseconds(5000);
   }
diff --git a/testhal/common/irq_storm.c b/testhal/common/irq_storm.c
index 8d07e3f19..5e0e4cc69 100644
--- a/testhal/common/irq_storm.c
+++ b/testhal/common/irq_storm.c
@@ -22,6 +22,8 @@
  * @{
  */
 
+#include <stdlib.h>
+
 #include "ch.h"
 #include "hal.h"