diff --git a/demos/ARMCM4-STM32F303-DISCOVERY/chconf.h b/demos/ARMCM4-STM32F303-DISCOVERY/chconf.h
index 727f9de89..31fcff51c 100644
--- a/demos/ARMCM4-STM32F303-DISCOVERY/chconf.h
+++ b/demos/ARMCM4-STM32F303-DISCOVERY/chconf.h
@@ -41,7 +41,7 @@
  *          setting also defines the system tick time unit.
  */
 #if !defined(CH_CFG_FREQUENCY) || defined(__DOXYGEN__)
-#define CH_CFG_FREQUENCY                    10000
+#define CH_CFG_FREQUENCY                    1000
 #endif
 
 /**
@@ -53,7 +53,7 @@
  *          this value.
  */
 #if !defined(CH_CFG_TIMEDELTA) || defined(__DOXYGEN__)
-#define CH_CFG_TIMEDELTA                    2
+#define CH_CFG_TIMEDELTA                    0
 #endif
 
 /**
@@ -359,7 +359,7 @@
  * @note    The default is @p FALSE.
  */
 #if !defined(CH_DBG_SYSTEM_STATE_CHECK) || defined(__DOXYGEN__)
-#define CH_DBG_SYSTEM_STATE_CHECK           FALSE
+#define CH_DBG_SYSTEM_STATE_CHECK           TRUE
 #endif
 
 /**
@@ -370,7 +370,7 @@
  * @note    The default is @p FALSE.
  */
 #if !defined(CH_DBG_ENABLE_CHECKS) || defined(__DOXYGEN__)
-#define CH_DBG_ENABLE_CHECKS                FALSE
+#define CH_DBG_ENABLE_CHECKS                TRUE
 #endif
 
 /**
@@ -382,7 +382,7 @@
  * @note    The default is @p FALSE.
  */
 #if !defined(CH_DBG_ENABLE_ASSERTS) || defined(__DOXYGEN__)
-#define CH_DBG_ENABLE_ASSERTS               FALSE
+#define CH_DBG_ENABLE_ASSERTS               TRUE
 #endif
 
 /**
@@ -393,7 +393,7 @@
  * @note    The default is @p FALSE.
  */
 #if !defined(CH_DBG_ENABLE_TRACE) || defined(__DOXYGEN__)
-#define CH_DBG_ENABLE_TRACE                 FALSE
+#define CH_DBG_ENABLE_TRACE                 TRUE
 #endif
 
 /**
@@ -407,7 +407,7 @@
  *          @p panic_msg variable set to @p NULL.
  */
 #if !defined(CH_DBG_ENABLE_STACK_CHECK) || defined(__DOXYGEN__)
-#define CH_DBG_ENABLE_STACK_CHECK           FALSE
+#define CH_DBG_ENABLE_STACK_CHECK           TRUE
 #endif
 
 /**
@@ -419,7 +419,7 @@
  * @note    The default is @p FALSE.
  */
 #if !defined(CH_DBG_FILL_THREADS) || defined(__DOXYGEN__)
-#define CH_DBG_FILL_THREADS                 FALSE
+#define CH_DBG_FILL_THREADS                 TRUE
 #endif
 
 /**
diff --git a/os/kernel/include/chstats.h b/os/kernel/include/chstats.h
index a62eba0cc..d40117882 100644
--- a/os/kernel/include/chstats.h
+++ b/os/kernel/include/chstats.h
@@ -62,60 +62,12 @@ typedef struct {
                                                 critical zones duration.    */
   time_measurement_t    m_crit_isr; /**< @brief Measurement of ISRs critical
                                                 zones duration.             */
-  time_measurement_t    m_isr;      /**< @brief Measurement of ISRs total
-                                                duration.                   */
 } kernel_stats_t;
 
 /*===========================================================================*/
 /* Module macros.                                                            */
 /*===========================================================================*/
 
-/**
- * @brief   Increases the IRQ counter.
- */
-#define _stats_increase_irq() kernel_stats.n_irq++
-
-/**
- * @brief   Increases the context switch counter.
- */
-#define _stats_increase_ctxswc() kernel_stats.n_ctxswc++
-
-/**
- * @brief   Starts the measurement of a thread critical zone.
- */
-#define _stats_start_measure_crit_thd()                                     \
-  chTMStartMeasurementX(&kernel_stats.m_crit_thd)
-
-/**
- * @brief   Stops the measurement of a thread critical zone.
- */
-#define _stats_stop_measure_crit_thd()                                      \
-  chTMStopMeasurementX(&kernel_stats.m_crit_thd)
-
-/**
- * @brief   Starts the measurement of an ISR critical zone.
- */
-#define _stats_start_measure_crit_isr()                                     \
-  chTMStartMeasurementX(&kernel_stats.m_crit_isr)
-
-/**
- * @brief   Stops the measurement of an ISR critical zone.
- */
-#define _stats_stop_measure_crit_isr()                                      \
-  chTMStopMeasurementX(&kernel_stats.m_crit_isr)
-
-/**
- * @brief   Starts the measurement of an ISR duration.
- */
-#define _stats_start_measure_isr()                                          \
-  chTMStartMeasurementX(&kernel_stats.m_crit_isr)
-
-/**
- * @brief   Stops the measurement of an ISR duration.
- */
-#define _stats_stop_measure_isr()                                           \
-  chTMStopMeasurementX(&kernel_stats.m_crit_isr)
-
 /*===========================================================================*/
 /* External declarations.                                                    */
 /*===========================================================================*/
@@ -128,6 +80,12 @@ extern kernel_stats_t kernel_stats;
 extern "C" {
 #endif
   void _stats_init(void);
+  void _stats_increase_irq(void);
+  void _stats_increase_ctxswc(void);
+  void _stats_start_measure_crit_thd(void);
+  void _stats_stop_measure_crit_thd(void);
+  void _stats_start_measure_crit_isr(void);
+  void _stats_stop_measure_crit_isr(void);
 #ifdef __cplusplus
 }
 #endif
@@ -145,8 +103,6 @@ extern "C" {
 #define _stats_stop_measure_crit_thd()
 #define _stats_start_measure_crit_isr()
 #define _stats_stop_measure_crit_isr()
-#define _stats_start_measure_isr()
-#define _stats_stop_measure_isr()
 
 #endif /* !CH_DBG_STATISTICS */
 
diff --git a/os/kernel/include/chsys.h b/os/kernel/include/chsys.h
index 9663e85ac..bfb521d65 100644
--- a/os/kernel/include/chsys.h
+++ b/os/kernel/include/chsys.h
@@ -61,7 +61,6 @@
  */
 #define CH_IRQ_PROLOGUE()                                                   \
   PORT_IRQ_PROLOGUE();                                                      \
-  _stats_start_measure_isr();                                               \
   _stats_increase_irq();                                                    \
   dbg_check_enter_isr()
 
@@ -75,7 +74,6 @@
  */
 #define CH_IRQ_EPILOGUE()                                                   \
   dbg_check_leave_isr();                                                    \
-  _stats_stop_measure_isr();                                                \
   PORT_IRQ_EPILOGUE()
 
 /**
@@ -196,6 +194,24 @@
 #define chSysGetRealtimeCounterX() (rtcnt_t)port_rt_get_counter_value()
 #endif
 
+/**
+ * @brief   Performs a context switch.
+ * @note    Not a user function, it is meant to be invoked by the scheduler
+ *          itself or from within the port layer.
+ *
+ * @param[in] ntp       the thread to be switched in
+ * @param[in] otp       the thread to be switched out
+ *
+ * @special
+ */
+#define chSysSwitch(ntp, otp) {                                             \
+                                                                            \
+  dbg_trace(otp);                                                           \
+  _stats_increase_ctxswc();                                                 \
+  CH_CFG_CONTEXT_SWITCH_HOOK(ntp, otp);                                     \
+  port_switch(ntp, otp);                                                    \
+}
+
 /*===========================================================================*/
 /* External declarations.                                                    */
 /*===========================================================================*/
@@ -220,24 +236,6 @@ extern "C" {
 /* Module inline functions.                                                  */
 /*===========================================================================*/
 
-/**
- * @brief   Performs a context switch.
- * @note    Not a user function, it is meant to be invoked by the scheduler
- *          itself or from within the port layer.
- *
- * @param[in] ntp       the thread to be switched in
- * @param[in] otp       the thread to be switched out
- *
- * @special
- */
-static inline void chSysSwitch(thread_t *ntp, thread_t *otp) {
-
-  dbg_trace(otp);
-  _stats_increase_ctxswc();
-  CH_CFG_CONTEXT_SWITCH_HOOK(ntp, otp);
-  port_switch(ntp, otp);
-}
-
 /**
  * @brief   Raises the system interrupt priority mask to the maximum level.
  * @details All the maskable interrupt sources are disabled regardless their
diff --git a/os/kernel/include/chtm.h b/os/kernel/include/chtm.h
index 8d8d021fd..b72bff8e6 100644
--- a/os/kernel/include/chtm.h
+++ b/os/kernel/include/chtm.h
@@ -31,8 +31,6 @@
 
 #if CH_CFG_USE_TM || defined(__DOXYGEN__)
 
-#define port_rt_get_counter_value() 0
-
 /*===========================================================================*/
 /* Module constants.                                                         */
 /*===========================================================================*/
diff --git a/os/kernel/src/chdebug.c b/os/kernel/src/chdebug.c
index 44058dff8..ba9c469b9 100644
--- a/os/kernel/src/chdebug.c
+++ b/os/kernel/src/chdebug.c
@@ -234,7 +234,7 @@ void _trace_init(void) {
  */
 void dbg_trace(thread_t *otp) {
 
-  dbg_trace_buffer.tb_ptr->se_time   = chTimeNow();
+  dbg_trace_buffer.tb_ptr->se_time   = chVTGetSystemTimeX();
   dbg_trace_buffer.tb_ptr->se_tp     = currp;
   dbg_trace_buffer.tb_ptr->se_wtobjp = otp->p_u.wtobjp;
   dbg_trace_buffer.tb_ptr->se_state  = (uint8_t)otp->p_state;
diff --git a/os/kernel/src/chstats.c b/os/kernel/src/chstats.c
index eb2023c93..93739da17 100644
--- a/os/kernel/src/chstats.c
+++ b/os/kernel/src/chstats.c
@@ -69,11 +69,59 @@ void _stats_init(void) {
 
   kernel_stats.n_irq = 0;
   kernel_stats.n_ctxswc = 0;
-  chTMObjectInit(&kernel_stats.m_isr);
   chTMObjectInit(&kernel_stats.m_crit_thd);
   chTMObjectInit(&kernel_stats.m_crit_isr);
 }
 
+
+/**
+ * @brief   Increases the IRQ counter.
+ */
+void _stats_increase_irq(void) {
+
+  kernel_stats.n_irq++;
+}
+
+/**
+ * @brief   Increases the context switch counter.
+ */
+void _stats_increase_ctxswc(void) {
+
+  kernel_stats.n_ctxswc++;
+}
+
+/**
+ * @brief   Starts the measurement of a thread critical zone.
+ */
+void _stats_start_measure_crit_thd(void) {
+
+  chTMStartMeasurementX(&kernel_stats.m_crit_thd);
+}
+
+/**
+ * @brief   Stops the measurement of a thread critical zone.
+ */
+void _stats_stop_measure_crit_thd(void) {
+
+  chTMStopMeasurementX(&kernel_stats.m_crit_thd);
+}
+
+/**
+ * @brief   Starts the measurement of an ISR critical zone.
+ */
+void _stats_start_measure_crit_isr(void) {
+
+  chTMStartMeasurementX(&kernel_stats.m_crit_isr);
+}
+
+/**
+ * @brief   Stops the measurement of an ISR critical zone.
+ */
+void _stats_stop_measure_crit_isr(void) {
+
+  chTMStopMeasurementX(&kernel_stats.m_crit_isr);
+}
+
 #endif /* CH_DBG_STATISTICS */
 
 /** @} */
diff --git a/os/kernel/src/chsys.c b/os/kernel/src/chsys.c
index 168bd1376..7057a7e01 100644
--- a/os/kernel/src/chsys.c
+++ b/os/kernel/src/chsys.c
@@ -163,7 +163,7 @@ void chSysInit(void) {
  */
 void chSysHalt(void) {
 
-  chSysDisable();
+  port_disable();
 
 #if defined(CH_CFG_SYSTEM_HALT_HOOK) || defined(__DOXYGEN__)
   CH_CFG_SYSTEM_HALT_HOOK();
diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.c b/os/ports/GCC/ARMCMx/chcore_v7m.c
index bb099e50c..821f2403e 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.c
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.c
@@ -62,9 +62,9 @@ CH_IRQ_HANDLER(SysTickVector) {
 
   CH_IRQ_PROLOGUE();
 
-  chSysLockFromIsr();
+  chSysLockFromISR();
   chSysTimerHandlerI();
-  chSysUnlockFromIsr();
+  chSysUnlockFromISR();
 
   CH_IRQ_EPILOGUE();
 }
@@ -191,9 +191,11 @@ __attribute__((naked))
 #endif
 void _port_switch_from_isr(void) {
 
+  _stats_start_measure_crit_thd();
   dbg_check_lock();
   chSchDoReschedule();
   dbg_check_unlock();
+  _stats_stop_measure_crit_thd();
   asm volatile ("_port_exit_from_isr:" : : : "memory");
 #if !CORTEX_SIMPLIFIED_PRIORITY || defined(__DOXYGEN__)
   asm volatile ("svc     #0");
diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.h b/os/ports/GCC/ARMCMx/chcore_v7m.h
index 6cd461408..b9b30dad2 100644
--- a/os/ports/GCC/ARMCMx/chcore_v7m.h
+++ b/os/ports/GCC/ARMCMx/chcore_v7m.h
@@ -421,6 +421,10 @@ static inline void port_init(void) {
   SCB_VTOR = CORTEX_VTOR_INIT;
   SCB_AIRCR = AIRCR_VECTKEY | AIRCR_PRIGROUP(CORTEX_PRIGROUP_INIT);
 
+  /* DWT cycle counter enable.*/
+  SCS_DEMCR |= SCS_DEMCR_TRCENA;
+  DWT_CTRL  |= DWT_CTRL_CYCCNTENA;
+
   /* Initialization of the system vectors used by the port.*/
   nvicSetSystemHandlerPriority(HANDLER_SVCALL,
     CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SVCALL));
@@ -595,6 +599,12 @@ static inline void port_wait_for_interrupt(void) {
 #endif
 }
 
+
+static inline rtcnt_t port_rt_get_counter_value(void) {
+
+  return DWT_CYCCNT;
+}
+
 #endif /* _FROM_ASM_ */
 
 #endif /* _CHCORE_V7M_H_ */
diff --git a/test/testbmk.c b/test/testbmk.c
index 293dbd607..c04239817 100644
--- a/test/testbmk.c
+++ b/test/testbmk.c
@@ -218,7 +218,8 @@ static void bmk4_execute(void) {
   thread_t *tp;
   uint32_t n;
 
-  tp = threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriority()+1, thread4, NULL);
+  tp = threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriority()+1,
+                                      thread4, NULL);
   n = 0;
   test_wait_tick();
   test_start_timer(1000);
diff --git a/test/testmbox.c b/test/testmbox.c
index 0ba52aa09..1ca34dadc 100644
--- a/test/testmbox.c
+++ b/test/testmbox.c
@@ -80,7 +80,7 @@ static void mbox1_execute(void) {
   /*
    * Testing initial space.
    */
-  test_assert(1, chMBGetFreeCountI(&mb1) == MB_SIZE, "wrong size");
+  test_assert_lock(1, chMBGetFreeCountI(&mb1) == MB_SIZE, "wrong size");
 
   /*
    * Testing enqueuing and backward circularity.
diff --git a/test/testsem.c b/test/testsem.c
index d73a5e11b..0810300ea 100644
--- a/test/testsem.c
+++ b/test/testsem.c
@@ -104,7 +104,7 @@ static void sem1_execute(void) {
   chSemAddCounterI(&sem1, 2);
   chSysUnlock();
   test_wait_threads();
-  test_assert(2, chSemGetCounterI(&sem1) == 1, "invalid counter");
+  test_assert_lock(2, chSemGetCounterI(&sem1) == 1, "invalid counter");
 }
 
 ROMCONST struct testcase testsem1 = {
@@ -252,7 +252,7 @@ static void sem4_execute(void) {
   /* Creates a taken binary semaphore.*/
   chBSemObjectInit(&bsem, TRUE);
   chBSemReset(&bsem, TRUE);
-  test_assert(1, chBSemGetStateI(&bsem) == TRUE, "not taken");
+  test_assert_lock(1, chBSemGetStateI(&bsem) == TRUE, "not taken");
 
   /* Starts a signaler thread at a lower priority.*/
   threads[0] = chThdCreateStatic(wa[0], WA_SIZE,
@@ -262,18 +262,18 @@ static void sem4_execute(void) {
   chBSemWait(&bsem);
   
   /* The binary semaphore is expected to be taken.*/
-  test_assert(2, chBSemGetStateI(&bsem) == TRUE, "not taken");
-  
+  test_assert_lock(2, chBSemGetStateI(&bsem) == TRUE, "not taken");
+
   /* Releasing it, check both the binary semaphore state and the underlying
      counter semaphore state..*/
   chBSemSignal(&bsem);
-  test_assert(3, chBSemGetStateI(&bsem) == FALSE, "still taken");
-  test_assert(4, chSemGetCounterI(&bsem.bs_sem) == 1, "unexpected counter");
-  
+  test_assert_lock(3, chBSemGetStateI(&bsem) == FALSE, "still taken");
+  test_assert_lock(4, chSemGetCounterI(&bsem.bs_sem) == 1, "unexpected counter");
+
   /* Checking signaling overflow, the counter must not go beyond 1.*/
   chBSemSignal(&bsem);
-  test_assert(3, chBSemGetStateI(&bsem) == FALSE, "taken");
-  test_assert(5, chSemGetCounterI(&bsem.bs_sem) == 1, "unexpected counter");
+  test_assert_lock(3, chBSemGetStateI(&bsem) == FALSE, "taken");
+  test_assert_lock(5, chSemGetCounterI(&bsem.bs_sem) == 1, "unexpected counter");
 }
 
 ROMCONST struct testcase testsem4 = {