micro() use DWT cyclecount if enabled else use rtos tick

DWT must be previously enabled by user sketch with dwt_enable()

Author: hathach

cores/nRF5/delay.c

@@ -30,11 +30,6 @@ uint32_t millis( void )
   return tick2ms(xTaskGetTickCount());
 }
 
-uint32_t micros( void )
-{
-  return tick2us(xTaskGetTickCount());
-}
-
 void delay( uint32_t ms )
 {
   uint32_t ticks = ms2tick(ms);

cores/nRF5/delay.h

@@ -26,6 +26,13 @@ extern "C" {
 #include <stdint.h>
 #include "nrfx.h"
 #include "variant.h"
+#include "rtos.h"
+
+static inline bool dwt_enabled(void) __attribute__((always_inline));
+static inline bool dwt_enabled(void)
+{
+  return (CoreDebug->DEMCR & CoreDebug_DEMCR_TRCENA_Msk) && (DWT->CTRL & DWT_CTRL_CYCCNTENA_Msk);
+}
 
 /**
  * \brief Returns the number of milliseconds since the Arduino board began running the current program.
@@ -46,7 +53,12 @@ extern uint32_t millis( void ) ;
  *
  * \note There are 1,000 microseconds in a millisecond and 1,000,000 microseconds in a second.
  */
-extern uint32_t micros( void ) ;
+static inline uint32_t micros( void ) __attribute__((always_inline));
+static inline uint32_t micros( void )
+{
+  // Use DWT cycle count if it is enabled, otherwise use rtos tick
+  return dwt_enabled() ? (DWT->CYCCNT / 64) : tick2us(xTaskGetTickCount());
+}
 
 /**
  * \brief Pauses the program for the amount of time (in miliseconds) specified as parameter.

cores/nRF5/freertos/portable/CMSIS/nrf52/port_cmsis_systick.c

@@ -230,6 +230,12 @@ void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime )
             if (diff > 0)
             {
                 vTaskStepTick(diff);
+
+                // If dwt cycle count is enable, adjust it as welll
+                if ( (CoreDebug->DEMCR & CoreDebug_DEMCR_TRCENA_Msk) && (DWT->CTRL & DWT_CTRL_CYCCNTENA_Msk) )
+                {
+                  DWT->CYCCNT += ((diff * 1000000) / configTICK_RATE_HZ) * 64;
+                }
             }
         }
     }