diff --git a/os/hal/boards/NONSTANDARD_STM32F4_BARTHESS2/board.h b/os/hal/boards/NONSTANDARD_STM32F4_BARTHESS2/board.h
index 8d60e82ca..de376b347 100644
--- a/os/hal/boards/NONSTANDARD_STM32F4_BARTHESS2/board.h
+++ b/os/hal/boards/NONSTANDARD_STM32F4_BARTHESS2/board.h
@@ -1178,6 +1178,10 @@
                                      PIN_AFIO_AF(GPIOI_PIN14, 0) |          \
                                      PIN_AFIO_AF(GPIOI_PIN15, 0))
 
+#define nand_wp_assert()    palClearPad(GPIOB, GPIOB_NAND_WP)
+#define nand_wp_release()   palSetPad(GPIOB, GPIOB_NAND_WP)
+#define red_led_on()        palSetPad(GPIOE, GPIOE_LED_R)
+#define red_led_off()       palClearPad(GPIOE, GPIOE_LED_R)
 
 #if !defined(_FROM_ASM_)
 #ifdef __cplusplus
diff --git a/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_adc.c b/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_adc.c
index dc245560c..4e37b757c 100644
--- a/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_adc.c
+++ b/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_adc.c
@@ -39,17 +39,13 @@
 #define ADC_AN33_1_OFFSET         (ADC_CHANNEL_IN14 - 10)
 #define ADC_AN33_2_OFFSET         (ADC_CHANNEL_IN15 - 10)
 
-
-
 static void adcerrorcallback(ADCDriver *adcp, adcerror_t err);
 static void adccallback(ADCDriver *adcp, adcsample_t *buffer, size_t n);
 
-
-
 static adcsample_t samples[ADC_NUM_CHANNELS * ADC_BUF_DEPTH];
 
-volatile uint32_t its = 0;
-volatile uint32_t errors = 0;
+static uint32_t ints = 0;
+static uint32_t errors = 0;
 
 static const ADCConversionGroup adccg = {
   TRUE,
@@ -80,23 +76,20 @@ static void adcerrorcallback(ADCDriver *adcp, adcerror_t err) {
   (void)err;
 
   osalSysHalt("");
-//  chSysLockFromIsr();
-//  adcStartConversionI(&ADCD1, &adccg, samples, ADC_BUF_DEPTH);
-//  chSysUnlockFromIsr();
 }
 
 static void adccallback(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
   (void)adcp;
   (void)buffer;
   (void)n;
-  its++;
+  ints++;
 }
 
 /*
  *
  */
 void dma_storm_adc_start(void){
-  its = 0;
+  ints = 0;
   errors = 0;
 
   /* Activates the ADC1 driver and the temperature sensor.*/
@@ -114,6 +107,6 @@ uint32_t dma_storm_adc_stop(void){
   adcStopConversion(&ADCD1);
   adcSTM32DisableTSVREFE();
   adcStop(&ADCD1);
-  return its;
+  return ints;
 }
 
diff --git a/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_spi.c b/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_spi.c
index a75701d74..742eeab2b 100644
--- a/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_spi.c
+++ b/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_spi.c
@@ -59,7 +59,7 @@ static const SPIConfig spicfg = {
     0, //SPI_CR1_BR_1 | SPI_CR1_BR_0
 };
 
-static uint32_t its;
+static uint32_t ints;
 static binary_semaphore_t sem;
 static bool stop = false;
 
@@ -72,7 +72,7 @@ static bool stop = false;
  */
 
 static void spi_end_cb(SPIDriver *spip){
-  its++;
+  ints++;
 
   if (stop){
     chSysLockFromISR();
@@ -94,7 +94,7 @@ static void spi_end_cb(SPIDriver *spip){
  */
 
 void dma_storm_spi_start(void){
-  its = 0;
+  ints = 0;
   stop = false;
   chBSemObjectInit(&sem, true);
   spiStart(&SPID1, &spicfg);
@@ -105,6 +105,6 @@ uint32_t dma_storm_spi_stop(void){
   stop = true;
   chBSemWait(&sem);
   spiStop(&SPID1);
-  return its;
+  return ints;
 }
 
diff --git a/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_uart.c b/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_uart.c
index 11c6f961c..e82e6838c 100644
--- a/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_uart.c
+++ b/testhal/STM32/STM32F4xx/FSMC_NAND/dma_storm_uart.c
@@ -69,7 +69,7 @@ static const UARTConfig uart_cfg = {
   0
 };
 
-static uint32_t its;
+static uint32_t ints;
 
 /*
  ******************************************************************************
@@ -84,7 +84,7 @@ static uint32_t its;
  */
 static void txend1(UARTDriver *uartp) {
 
-  its++;
+  ints++;
   chSysLockFromISR();
   uartStartSendI(uartp, STORM_BUF_LEN, txbuf);
   chSysUnlockFromISR();
@@ -148,7 +148,7 @@ void dma_storm_uart_start(void){
     rxbuf[i] = 0;
   }
 
-  its = 0;
+  ints = 0;
   uartStart(&UARTD6, &uart_cfg);
   uartStartReceive(&UARTD6, STORM_BUF_LEN, rxbuf);
   uartStartSend(&UARTD6, STORM_BUF_LEN, txbuf);
@@ -160,5 +160,5 @@ uint32_t dma_storm_uart_stop(void){
   uartStopReceive(&UARTD6);
   uartStop(&UARTD6);
 
-  return its;
+  return ints;
 }
diff --git a/testhal/STM32/STM32F4xx/FSMC_NAND/main.c b/testhal/STM32/STM32F4xx/FSMC_NAND/main.c
index bc3d33a07..ecfe589ca 100644
--- a/testhal/STM32/STM32F4xx/FSMC_NAND/main.c
+++ b/testhal/STM32/STM32F4xx/FSMC_NAND/main.c
@@ -21,7 +21,7 @@
 /*
  * Hardware notes.
  *
- * Use _external_ pullup on ready/busy pin of NAND IC.
+ * Use external pullup on ready/busy pin of NAND IC for a speed reason.
  *
  * Chose MCU with 140 (or more) pins package because 100 pins packages
  * has no dedicated interrupt pins for FSMC.
@@ -38,6 +38,13 @@
  * Yes, you have to realize it in sowftware yourself.
  */
 
+/*
+ * Software notes.
+ *
+ * For correct calculation of timing values you need AN2784 document
+ * from STMicro.
+ */
+
 #include "ch.h"
 #include "hal.h"
 
@@ -66,10 +73,12 @@
 #define NAND_ROW_WRITE_CYCLES     3
 #define NAND_COL_WRITE_CYCLES     2
 
-/* statuses returning by NAND IC on 0x70 command */
-#define NAND_STATUS_OP_FAILED     ((uint8_t)1 << 0)
-#define NAND_STATUS_READY         ((uint8_t)1 << 6)
-#define NAND_STATUS_NOT_RPOTECTED ((uint8_t)1 << 7)
+#define NANF_TEST_START_BLOCK     1100
+#define NAND_TEST_END_BLOCK       1150
+
+#if USE_KILL_BLOCK_TEST
+#define NAND_TEST_KILL_BLOCK      8000
+#endif
 
 /*
  ******************************************************************************
@@ -102,11 +111,11 @@ static uint8_t ref_buf[NAND_PAGE_SIZE];
 /*
  *
  */
-//static TimeMeasurement tmu_erase;
-//static TimeMeasurement tmu_write_data;
-//static TimeMeasurement tmu_write_spare;
-//static TimeMeasurement tmu_read_data;
-//static TimeMeasurement tmu_read_spare;
+static time_measurement_t tmu_erase;
+static time_measurement_t tmu_write_data;
+static time_measurement_t tmu_write_spare;
+static time_measurement_t tmu_read_data;
+static time_measurement_t tmu_read_spare;
 
 #if NAND_USE_BAD_MAP
 static uint32_t badblock_map[NAND_BLOCKS_COUNT / 32];
@@ -128,7 +137,7 @@ static const NANDConfig nandcfg = {
     NAND_COL_WRITE_CYCLES,
     /* stm32 specific fields */
     ((FSMCNAND_TIME_HIZ << 24) | (FSMCNAND_TIME_HOLD << 16) | \
-                                (FSMCNAND_TIME_WAIT << 8) | FSMCNAND_TIME_SET),
+                                 (FSMCNAND_TIME_WAIT << 8) | FSMCNAND_TIME_SET),
 #if !STM32_NAND_USE_FSMC_INT
     ready_isr_enable,
     ready_isr_disable
@@ -168,14 +177,10 @@ static const EXTConfig extcfg = {
 };
 #endif /* !STM32_NAND_USE_FSMC_INT */
 
-/*
- *
- */
-volatile uint32_t IdleCnt = 0;
-volatile systime_t T = 0;
+static uint32_t BackgroundThdCnt = 0;
 
 #if USE_KILL_BLOCK_TEST
-volatile uint32_t KillCycle = 0;
+static uint32_t KillCycle = 0;
 #endif
 
 /*
@@ -203,28 +208,15 @@ static void ready_isr_disable(void) {
 }
 #endif /* STM32_NAND_USE_FSMC_INT */
 
-static void nand_wp_assert(void) {
-  palClearPad(GPIOB, GPIOB_NAND_WP);
-}
-
-static void nand_wp_release(void) {
-  palSetPad(GPIOB, GPIOB_NAND_WP);
-}
-
-static void red_led_on(void) {
-  palSetPad(GPIOE, GPIOE_LED_R);
-}
-
-static void red_led_off(void) {
-  palClearPad(GPIOE, GPIOE_LED_R);
-}
-
-static THD_WORKING_AREA(fsmcIdleThreadWA, 128);
-static THD_FUNCTION(fsmcIdleThread, arg) {
+/**
+ *
+ */
+static THD_WORKING_AREA(BackgroundThreadWA, 128);
+static THD_FUNCTION(BackgroundThread, arg) {
   (void)arg;
 
   while(true){
-    IdleCnt++;
+    BackgroundThdCnt++;
   }
   return 0;
 }
@@ -248,17 +240,14 @@ static bool is_erased(NANDDriver *dp, size_t block){
   return true;
 }
 
+/*
+ *
+ */
 static void pattern_fill(void) {
 
   size_t i;
 
-
-
-
-  ///////////////////////// FIXME //////////////////////////////////
-  //srand(hal_lld_get_counter_value());
-  srand(0);
-
+  srand(chSysGetRealtimeCounterX());
 
   for(i=0; i<NAND_PAGE_SIZE; i++){
     ref_buf[i] = rand() & 0xFF;
@@ -273,6 +262,9 @@ static void pattern_fill(void) {
   osalDbgCheck(0 == memcmp(ref_buf, nand_buf, NAND_PAGE_SIZE));
 }
 
+/*
+ *
+ */
 #if USE_KILL_BLOCK_TEST
 static void kill_block(NANDDriver *nandp, uint32_t block){
 
@@ -280,7 +272,7 @@ static void kill_block(NANDDriver *nandp, uint32_t block){
   size_t page = 0;
   uint8_t op_status;
 
-  /* This test require good block.*/
+  /* This test requires good block.*/
   osalDbgCheck(!nandIsBad(nandp, block));
 
   while(true){
@@ -314,6 +306,9 @@ static void kill_block(NANDDriver *nandp, uint32_t block){
 }
 #endif /* USE_KILL_BLOCK_TEST */
 
+/*
+ *
+ */
 typedef enum {
   ECC_NO_ERROR = 0,
   ECC_CORRECTABLE_ERROR = 1,
@@ -321,8 +316,11 @@ typedef enum {
   ECC_CORRUPTED = 3,
 } ecc_result_t;
 
-static ecc_result_t parse_ecc(uint32_t ecclen, uint32_t ecc1, uint32_t ecc2,
-                                                          uint32_t *corrupted){
+/*
+ *
+ */
+static ecc_result_t parse_ecc(uint32_t ecclen,
+                    uint32_t ecc1, uint32_t ecc2, uint32_t *corrupted){
 
   size_t i = 0;
   uint32_t corr = 0;
@@ -357,6 +355,9 @@ static ecc_result_t parse_ecc(uint32_t ecclen, uint32_t ecc1, uint32_t ecc2,
   }
 }
 
+/*
+ *
+ */
 static void invert_bit(uint8_t *buf, uint32_t byte, uint32_t bit){
   osalDbgCheck((byte < NAND_PAGE_DATA_SIZE) && (bit < 8));
   buf[byte] ^= ((uint8_t)1) << bit;
@@ -456,12 +457,11 @@ static void general_test (NANDDriver *nandp, size_t first,
   red_led_on();
 
   /* initialize time measurement units */
-  ////////////////////////////// FIXME //////////////////////////////
-//  tmObjectInit(&tmu_erase);
-//  tmObjectInit(&tmu_write_data);
-//  tmObjectInit(&tmu_write_spare);
-//  tmObjectInit(&tmu_read_data);
-//  tmObjectInit(&tmu_read_spare);
+  chTMObjectInit(&tmu_erase);
+  chTMObjectInit(&tmu_write_data);
+  chTMObjectInit(&tmu_write_spare);
+  chTMObjectInit(&tmu_read_data);
+  chTMObjectInit(&tmu_read_spare);
 
   /* perform basic checks */
   for (block=first; block<last; block++){
@@ -479,43 +479,43 @@ static void general_test (NANDDriver *nandp, size_t first,
       for (page=0; page<nandp->config->pages_per_block; page++){
         pattern_fill();
 
-        //tmStartMeasurement(&tmu_write_data);
+        chTMStartMeasurementX(&tmu_write_data);
         op_status = nandWritePageData(nandp, block, page,
                       nand_buf, nandp->config->page_data_size, &wecc);
-        //tmStopMeasurement(&tmu_write_data);
+        chTMStopMeasurementX(&tmu_write_data);
         osalDbgCheck(0 == (op_status & 1)); /* operation failed */
 
-        //tmStartMeasurement(&tmu_write_spare);
+        chTMStartMeasurementX(&tmu_write_spare);
         op_status = nandWritePageSpare(nandp, block, page,
                       nand_buf + nandp->config->page_data_size,
                       nandp->config->page_spare_size);
-        //tmStopMeasurement(&tmu_write_spare);
+        chTMStopMeasurementX(&tmu_write_spare);
         osalDbgCheck(0 == (op_status & 1)); /* operation failed */
 
         /* read back and compare */
         for (round=0; round<read_rounds; round++){
           memset(nand_buf, 0, NAND_PAGE_SIZE);
 
-          //tmStartMeasurement(&tmu_read_data);
+          chTMStartMeasurementX(&tmu_read_data);
           nandReadPageData(nandp, block, page,
                       nand_buf, nandp->config->page_data_size, &recc);
-          //tmStopMeasurement(&tmu_read_data);
+          chTMStopMeasurementX(&tmu_read_data);
           osalDbgCheck(0 == (recc ^ wecc)); /* ECC error detected */
 
-          //tmStartMeasurement(&tmu_read_spare);
+          chTMStartMeasurementX(&tmu_read_spare);
           nandReadPageSpare(nandp, block, page,
                       nand_buf + nandp->config->page_data_size,
                       nandp->config->page_spare_size);
-          //tmStopMeasurement(&tmu_read_spare);
+          chTMStopMeasurementX(&tmu_read_spare);
 
           osalDbgCheck(0 == memcmp(ref_buf, nand_buf, NAND_PAGE_SIZE)); /* Read back failed */
         }
       }
 
       /* make clean */
-      //tmStartMeasurement(&tmu_erase);
+      chTMStartMeasurementX(&tmu_erase);
       op_status = nandErase(nandp, block);
-      //tmStopMeasurement(&tmu_erase);
+      chTMStopMeasurementX(&tmu_erase);
       osalDbgCheck(0 == (op_status & 1)); /* operation failed */
 
       status = is_erased(nandp, block);
@@ -537,19 +537,15 @@ static void general_test (NANDDriver *nandp, size_t first,
  */
 int main(void) {
 
-  size_t start = 1100;
-  size_t end = 1150;
-  volatile int32_t adc_its = 0;
-  volatile int32_t spi_its = 0;
-  volatile int32_t uart_its = 0;
-  volatile int32_t adc_its_idle = 0;
-  volatile int32_t spi_its_idle = 0;
-  volatile int32_t uart_its_idle = 0;
-  volatile uint32_t idle_thread_cnt = 0;
-
-  #if USE_KILL_BLOCK_TEST
-  size_t kill = 8000;
-  #endif
+  /* performance counters */
+  int32_t adc_ints = 0;
+  int32_t spi_ints = 0;
+  int32_t uart_ints = 0;
+  int32_t adc_idle_ints = 0;
+  int32_t spi_idle_ints = 0;
+  int32_t uart_idle_ints = 0;
+  uint32_t background_cnt = 0;
+  systime_t T = 0;
 
   /*
    * System initializations.
@@ -568,45 +564,57 @@ int main(void) {
 
   chThdSleepMilliseconds(4000);
 
-  chThdCreateStatic(fsmcIdleThreadWA,
-        sizeof(fsmcIdleThreadWA),
+  chThdCreateStatic(BackgroundThreadWA,
+        sizeof(BackgroundThreadWA),
         NORMALPRIO - 20,
-        fsmcIdleThread,
+        BackgroundThread,
         NULL);
 
   nand_wp_release();
 
+  /*
+   * run NAND test in parallel with DMA load and background thread
+   */
   dma_storm_adc_start();
   dma_storm_uart_start();
   dma_storm_spi_start();
   T = chVTGetSystemTimeX();
-  general_test(&NANDD1, start, end, 1);
+  general_test(&NANDD1, NANF_TEST_START_BLOCK, NAND_TEST_END_BLOCK, 1);
   T = chVTGetSystemTimeX() - T;
-  adc_its = dma_storm_adc_stop();
-  uart_its = dma_storm_uart_stop();
-  spi_its = dma_storm_spi_stop();
+  adc_ints = dma_storm_adc_stop();
+  uart_ints = dma_storm_uart_stop();
+  spi_ints = dma_storm_spi_stop();
   chSysLock();
-  idle_thread_cnt = IdleCnt;
-  IdleCnt = 0;
+  background_cnt = BackgroundThdCnt;
+  BackgroundThdCnt = 0;
   chSysUnlock();
 
+  /*
+   * run DMA load and background thread _without_ NAND test
+   */
   dma_storm_adc_start();
   dma_storm_uart_start();
   dma_storm_spi_start();
   chThdSleep(T);
-  adc_its_idle = dma_storm_adc_stop();
-  uart_its_idle = dma_storm_uart_stop();
-  spi_its_idle = dma_storm_spi_stop();
+  adc_idle_ints = dma_storm_adc_stop();
+  uart_idle_ints = dma_storm_uart_stop();
+  spi_idle_ints = dma_storm_spi_stop();
 
-  osalDbgCheck(idle_thread_cnt > (IdleCnt / 4));
-  osalDbgCheck(abs(adc_its - adc_its_idle)   < (adc_its_idle  / 20));
-  osalDbgCheck(abs(uart_its - uart_its_idle) < (uart_its_idle / 20));
-  osalDbgCheck(abs(spi_its - spi_its_idle)   < (spi_its_idle  / 10));
+  /*
+   * ensure that NAND code have negligible impact on other subsystems
+   */
+  osalDbgCheck(background_cnt > (BackgroundThdCnt / 4));
+  osalDbgCheck(abs(adc_ints - adc_idle_ints)   < (adc_idle_ints  / 20));
+  osalDbgCheck(abs(uart_ints - uart_idle_ints) < (uart_idle_ints / 20));
+  osalDbgCheck(abs(spi_ints - spi_idle_ints)   < (spi_idle_ints  / 10));
 
-  ecc_test(&NANDD1, end);
+  /*
+   * perform ECC calculation test
+   */
+  ecc_test(&NANDD1, NAND_TEST_END_BLOCK);
 
 #if USE_KILL_BLOCK_TEST
-  kill_block(&NANDD1, kill);
+  kill_block(&NANDD1, NAND_TEST_KILL_BLOCK);
 #endif
 
   nand_wp_assert();